_old_code_.txt

	def predictionAndUpdateOneParticle(self, X, dt, dt2, keypoints, step, P):
		
		weight = 0.0	
		weights = []
		count_of_known_keypoints = 0
		x_diff_sum = np.array([0.0, 0.0, 0.0])
		x_diffs = []
		
		# 姿勢予測 prediction of position
		X_ = Particle()
		X_.landmarks = X.landmarks
		X_.x = X.x + dt*X.v + dt2*X.a
		X_.v = X.v + dt*X.a
		X_.a = X.a
		X_.o = X.o
		
		for keypoint in keypoints:
			#############################
			start_time_ = time.clock() 
			#############################
			# previous landmark id
			prevLandmarkId = (step-1)*10000 + keypoint.prevIndex
			# new landmark id
			landmarkId = step*10000 + keypoint.index
			# The landmark is already observed or not?
			if(X.landmarks.has_key(prevLandmarkId) == False):
				# Fisrt observation
				# Initialize landmark and append to particle
				landmark = Landmark()
				landmark.init(X, keypoint, P, self.focus)
				X.landmarks[landmarkId] = landmark
			else:
				# Already observed
				count_of_known_keypoints += 1
				X.landmarks[landmarkId] = X.landmarks[prevLandmarkId]
				del X.landmarks[prevLandmarkId]
				
				# Actual observation z				
				z = np.array([keypoint.x, keypoint.y])
				
				# 計測予測 prediction of observation
				#  Calc h(x), Hx, Hm (Jacobian matrix of h with respect to X and Landmark)
				z__, Hx, Hm = X.landmarks[landmarkId].calcObservation(X_, self.focus)
				
				# 姿勢のカルマンフィルタ Kalman filter of position
				S = Hm.dot(X.landmarks[landmarkId].sigma.dot(Hm.T)) + self.R
				L = S + Hx.dot(self.Q.dot(Hx.T))
				Sinv = np.linalg.inv(S)
				Linv = np.linalg.inv(L)
				sigmax = np.linalg.inv( Hx.T.dot(Sinv.dot(Hx)) + np.linalg.inv(self.Q) )
				mux = sigmax.dot(Hx.T.dot(Sinv.dot(z - z__)))
				
				# 姿勢のサンプリング sampling of position
				x_diff = np.random.multivariate_normal(mux, sigmax)
				x_diffs.append(x_diff)
				x_diff_sum += x_diff
				
				# 計測再予測 reprediction of observation
				z_ = X.landmarks[landmarkId].h(X_.x + x_diff, X.o, self.focus)
				
				# ランドマークの予測 prediction of landmark
				K = X.landmarks[landmarkId].sigma.dot(Hm.T.dot(Sinv))
				X.landmarks[landmarkId].mu += K.dot(z - z_)
				X.landmarks[landmarkId].sigma = X.landmarks[landmarkId].sigma - K.dot(Hm.dot(X.landmarks[landmarkId].sigma))
				
				# 重み計算 calc weight
				w = (1.0 / (math.sqrt( np.linalg.det(2.0 * math.pi * L) ))) * np.exp( -0.5 * ( (z-z_).T.dot(Linv.dot(z-z_)) ) )
				weights.append(w)
				
				###############################
				end_time_ = time.clock()
				if(self.count == 0):
					#print(x_diff)
					#print ""+str(landmarkId)+" update time = %f" %(end_time_-start_time_)
					pass
				###############################
					
		if(count_of_known_keypoints > 0):
			x_diff = x_diff_sum/float(count_of_known_keypoints)
			X_.x += x_diff
			X_.v += (2.0*x_diff)/dt
			
			weight /= float(count_of_known_keypoints)
			weight *= 1000
		
		###############################
		print("weight "+str(weight))
		#if(self.count == 0):
			#print("weight "+str(weight))
		###########################
		self.count+=1
		###########################
		
		return X_, weight







				###############################
				end_time_ = time.clock()
				if(self.count == 0):
					print("z "),
					print(z)
					print("z_ "),
					print(z_)
					print("z__ "),
					print(z__)
					#print ""+str(landmarkId)+" update time = %f" %(end_time_-start_time_)
					pass
				###############################






		
		
	def calcObservation(self, X, focus):
		"""
		Calc h and H (Jacobian matrix of h)
		
		Observation function
			z = h(x) + v
			
			h(x) = [h1(x), h2(x)].T
			h1(x) = f*hx/hz - cx
			h2(x) = f*hy/hz - cy
		"""
		
		# often used variables
		# xi, yi, zi, xt, yt, zt, p (Inverse depth)
		xi = self.mu[0]
		yi = self.mu[1]
		zi = self.mu[2]
		xt = X.x[0]
		yt = X.x[1]
		zt = X.x[2]
		p = self.mu[5]
		# sin, cos
		sinTheta = sin(self.mu[3])
		cosTheta = cos(self.mu[3])
		sinPhi = sin(self.mu[4])
		cosPhi = cos(self.mu[4])
		
		# Rotation matrix (Global coordinates -> Local coordinates)
		rotXinv = Util.rotationMatrixX(-X.o[0])
		rotYinv = Util.rotationMatrixY(-X.o[1])
		rotZinv = Util.rotationMatrixZ(-X.o[2])
		R = np.dot(rotXinv, np.dot(rotYinv, rotZinv))
		
		# hG = [hx, hy, hz].T in the global coordinates
		hG = np.array([p * (xi - xt) + cosPhi * sinTheta,
					p * (yi - yt) - sinPhi,
					p * (zi - zt) + cosPhi * cosTheta])
		
		# hL = h Local = [hx, hy, hz].T in the local coordinates
		hL = np.dot(R, hG)
		hx = hL[0]
		hy = hL[1]
		hz = hL[2]
		
		# h1 = - f*hx/hz, h2 = - f*hy/hz , and Device coordinates -> Camera coordinates
		h1 = - (focus * hx / hz)
		h2 = focus * hy / hz
		
		# derivative
		R11 = R[0][0]
		R12 = R[0][1]
		R13 = R[0][2]
		R21 = R[1][0]
		R22 = R[1][1]
		R23 = R[1][2]
		R31 = R[2][0]
		R32 = R[2][1]
		R33 = R[2][2]
		
		dhxxi = p * R11
		dhyxi = p * R21
		dhzxi = p * R31
		
		dhxyi = p * R12
		dhyyi = p * R22
		dhzyi = p * R32
		
		dhxzi = p * R13
		dhyzi = p * R23
		dhzzi = p * R33
		
		dhxTheta = R11 * cosPhi * cosTheta - R13 * cosPhi * sinTheta
		dhyTheta = R21 * cosPhi * cosTheta - R23 * cosPhi * sinTheta
		dhzTheta = R31 * cosPhi * cosTheta - R33 * cosPhi * sinTheta
		
		dhxPhi = - R11 * sinTheta * sinPhi - R12 * cosPhi - R13 * cosTheta * sinPhi
		dhyPhi = - R21 * sinTheta * sinPhi - R22 * cosPhi - R23 * cosTheta * sinPhi
		dhzPhi = - R31 * sinTheta * sinPhi - R32 * cosPhi - R33 * cosTheta * sinPhi
		
		dhxp = R11 * (xi - xt) + R12 * (yi - yt) + R13 * (zi - zt)
		dhyp = R21 * (xi - xt) + R22 * (yi - yt) + R23 * (zi - zt)
		dhzp = R31 * (xi - xt) + R32 * (yi - yt) + R33 * (zi - zt)
		
		# Jacobian
		f_hz2 = focus / (hz * hz) # focus / (hz)^2
		
		dh1xi = - f_hz2 * (dhxxi * hz - hx * dhzxi)
		dh1yi = - f_hz2 * (dhxyi * hz - hx * dhzyi)
		dh1zi = - f_hz2 * (dhxzi * hz - hx * dhzzi)
		dh1Theta = - f_hz2 * (dhxTheta * hz - hx * dhzTheta)
		dh1Phi = - f_hz2 * (dhxPhi * hz - hx * dhzPhi)
		dh1p = - f_hz2 * (dhxp * hz - hx * dhzp)
		
		dh2xi = f_hz2 * (dhyxi * hz - hy * dhzxi)
		dh2yi = f_hz2 * (dhyyi * hz - hy * dhzyi)
		dh2zi = f_hz2 * (dhyzi * hz - hy * dhzzi)
		dh2Theta = f_hz2 * (dhyTheta * hz - hy * dhzTheta)
		dh2Phi = f_hz2 * (dhyPhi * hz - hy * dhzPhi)
		dh2p = f_hz2 * (dhyp * hz - hy * dhzp)
		
		H = np.array([[dh1xi, dh1yi, dh1zi, dh1Theta, dh1Phi, dh1p],
					[dh2xi, dh2yi, dh2zi, dh2Theta, dh2Phi, dh2p]])
		
		return np.array([h1,h2]), H






	def pf_step_camera(self, X, dt, keypoints, step, P, M):
		""" One Step of Sampling Importance Resampling for Particle Filter
			for IMU sensor
		Parameters
		----------
		X : 状態 List of state set
		dt : 時刻の差分 delta of time
		keypoints : 特徴点 keypoints
		step : 現在のステップ数 current step
		P : デバイス位置の分散共分散行列 Variance-covariance matrix of position
		M : パーティクルの数 num of particles
		Returns
		-------
		X_resampled : 次の状態 List updated state
		"""

		# 初期化 init
		X_predicted = range(M)
		weight = range(M)
		X_resampled = range(M)

		
		#############################
		start_time_ = time.clock() 
		#############################
		# 推定と更新 prediction and update
		X_predicted, weight = self.predictionAndUpdate(X, dt, keypoints, step, P)
		###############################
		end_time_ = time.clock()
		#print "update   time = %f" %(end_time_-start_time_) 
		###############################
		
		#############################
		start_time_ = time.clock() 
		#############################
		# 正規化とリサンプリング normalization and resampling
		X_resampled = self.normalizationAndResampling(X_predicted, weight, M)
		###############################
		end_time_ = time.clock()
		#print "resample time = %f" %(end_time_-start_time_) 
		###############################

		return X_resampled







	def likelihood(self, keypoints, step, P, X):
		""" Likelihood function
		- 尤度関数
			p(y|x) ~ exp(-1/2 * (|y-h(x)|.t * sigma * |y-h(x)|)
		- 観測モデル
			z = h(x) + v
			v ~ N(0, sigma)
		Parameters
		----------
		keypoints : 観測 Observation 特徴点 keypoints
		step : 現在のステップ数 current step
		x : 予測　Predicted particle
		Returns
		-------
		likelihood : 尤度 Likelihood
		"""
		
		rss = 0.0 # Residual sum of squares
		likelihood = 0.0 # Likelihood
		
		for keypoint in keypoints:
			# previous landmark id
			prevLandmarkId = (step-1)*10000 + keypoint.prevIndex
			# new landmark id
			landmarkId = step*10000 + keypoint.index
			# The landmark is already observed or not?
			#############################
			start_time_ = time.clock() 
			#############################
			if(X.landmarks.has_key(prevLandmarkId) == False):
				# Fisrt observation
				# Initialize landmark and append to particle
				landmark = Landmark()
				landmark.init(X, keypoint, P, self.focus)
				X.landmarks[landmarkId] = landmark
				###############################
				end_time_ = time.clock() #####################
				if(self.count == 0): ###############################
					pass
					#print ""+str(landmarkId)+" init   time = %f" %(end_time_-start_time_) #####################
				###############################
			else:
				# Already observed
				X.landmarks[landmarkId] = X.landmarks[prevLandmarkId]
				del X.landmarks[prevLandmarkId]
				# Observation z				
				z = numpy.array([keypoint.x, keypoint.y])
				# Calc h and H (Jacobian matrix of h)
				h, H = X.landmarks[landmarkId].calcObservation(X, self.focus)
				# Kalman filter (Landmark update)
				X.landmarks[landmarkId].mu, X.landmarks[landmarkId].sigma = KF.execEKF1Update(z, h, X.landmarks[landmarkId].mu, X.landmarks[landmarkId].sigma, H, self.R)
				# Calc residual sum of squares
				rss += (z-h).T.dot(z-h)
				###############################
				end_time_ = time.clock() #####################
				if(self.count == 0): ###############################
					pass
					#print ""+str(landmarkId)+" update time = %f" %(end_time_-start_time_) #####################
				###############################

		likelihood = numpy.exp( (-0.5*rss) / (self.noise_camera*len(keypoints)) )

		###############################
		if(self.count == 0):
			print("rss "+str(rss))
		###########################
		self.count+=1
		###########################
		return likelihood






				###############################
				if(self.count == 0): ###############################
					print("z "),
					print(z)
					print("h "),
					print(h)
					print("Hx"),
					print(H.dot(X.landmarks[landmarkId].mu))
				###############################





class Landmark:

	def __init__(self, id_, step_, index_):
		self.id = id_
		self.step = step_
		self.index = index_
		self.mu = np.array([0.0,0.0,0.0,0.0,0.0,0.0])
		self.sigma = np.zeros([])




		
	
	def findLandmark(self, step_, index_):
		if(step_ == -1):
			return "none"
			
		if(len(self.landmarks) == 0):
			return "none"
		
		for landmark in reversed(self.landmarks):
			if(landmark.step < step_):
				break
			if(landmark.step == step_ and landmark.index == index_):
				return landmark
		return "none"






	def f_IMU(self, X, dt, dt2, accel, ori, w_a):
		""" Transition model
		- 状態方程式
			x_t = f(x_t-1, u) + w
			w ~ N(0, sigma)
		"""
	
		#X_new = copy.deepcopy(X)
		X_new = Particle()
		X_new.landmarks = X.landmarks
	
		# Transition with noise (only x,v)
		#w_mean = numpy.zeros(3) # mean of noise
		#w_cov_a = numpy.eye(3) * self.noise_a_sys # covariance matrix of noise (accel)
		#w_a = numpy.random.multivariate_normal(w_mean, w_cov_a) # generate random
		
		X_new.x = X.x + dt*X.v + dt2*X.a + dt2*w_a
		X_new.v = X.v + dt*X.a + dt*w_a
		X_new.a = accel
		X_new.o = ori
	
		return X_new


	def f_camera(self, dt, X):
		""" Transition model
		- 状態方程式
			x_t = f(x_t-1, u) + w
			w ~ N(0, sigma)
		"""

		#X_new = copy.deepcopy(X)
		X_new = Particle()
		X_new.landmarks = X.landmarks

		dt2 = 0.5 * dt * dt
		
		# Transition with noise (only x,v)
		w_mean = numpy.zeros(3) # mean of noise
		w_cov_a = numpy.eye(3) * self.noise_a_sys # covariance matrix of noise (accel)
		w_a = numpy.random.multivariate_normal(w_mean, w_cov_a) # generate random
		
		X_new.x = X.x + dt*X.v + dt2*X.a + dt2*w_a
		X_new.v = X.v + dt*X.a + dt*w_a
		X_new.a = X.a
		X_new.o = X.o

		return X_new







	def predictionAndUpdateIMU(self, X, dt, accel, ori, M, noise_a_sys):
		print("start")
		queue = multiprocessing.Queue()
		jobs = []
		for Xlist in list(more_itertools.chunked(X, int(M/4))):
			job = multiprocessing.Process(target=worker_IMU, args=(queue, Xlist, dt, accel, ori, noise_a_sys))
			jobs.append(job)
			job.start()
		[j.join() for j in jobs]
		return [queue.get() for i in xrange(M)]




	def predictionAndUpdateIMU(self, X, X_predicted, dt, accel, ori, M, noise_a_sys):
		pool = mp.Pool(mp.cpu_count())
		results = [pool.apply_async(f_IMU, args=(Xi, dt, accel, ori, noise_a_sys)) for Xi in X]    
		output = [p.get() for p in results]
		pool.close()
		pool.join()
		return output






		start_time_ = time.clock() #####################
		end_time_ = time.clock() #####################
		print "time = %f" %(end_time_-start_time_) #####################




	#Set new data and Execute all functions
	def processData(self,data):

		#if nomatch then nothing to do
		if(data[0] == "nomatch"):
			#print("nomatch"),
			return

		keypoints = []
		for d in data:
			if(d != ''):
				keypoints.append(KeyPoint(d))




		##############################
		print("----------------")
		for k in keypointPairs:
			print(k.x1),
			print(k.y1),
			print(" -> "),
			print(k.x2),
			print(k.y2)
		##############################




		##################################
		# print variance of x
		print(self.sigma[0][0]),
		print(self.sigma[1][1]),
		print(self.sigma[2][2])
		dt2 = 0.5 * 0.02 * 0.02
		w_mean = np.zeros(3) # mean of noise
		w_cov_a = np.eye(3) * self.PFnoise_a_sys # covariance matrix of noise (accel)
		w_a = np.random.multivariate_normal(w_mean, w_cov_a) # generate random
		print(dt2*w_a)
		##################################



		# ----- Set parameters here! ----- #
		self.M = 100 # total number of particles パーティクルの数
		self.f = 1575.54144 # focus length of camera [px] カメラの焦点距離 [px]
		# Kalman Filter
		self.noise_a_sys = 0.01 # system noise of acceleration　加速度のシステムノイズ
		self.noise_g_sys = 0.01 # system noise of gyro　ジャイロのシステムノイズ
		self.noise_a_obs = 0.00000001 # observation noise of acceleration　加速度の観測ノイズ
		self.noise_g_obs = 0.00000001 # observation noise of gyro　ジャイロの観測ノイズ
		# Particle Filter
		self.PFnoise_a_sys = 5.0 # system noise of acceleration　加速度のシステムノイズ
		self.PFnoise_g_sys = 5.0 # system noise of gyro　ジャイロのシステムノイズ
		self.PFnoise_a_obs = 0.00000001 # observation noise of acceleration　加速度の観測ノイズ
		self.PFnoise_g_obs = 0.00000001 # observation noise of gyro　ジャイロの観測ノイズ
		self.PFnoise_coplanarity_obs = 0.1 # observation noise of coplanarity 共面条件の観測ノイズ
		# ----- Set parameters here! ----- #





		# count
		self.count += 1
		print(self.count),



		coplanarity_matrix = range(M) ########################

		############################
		print(coplanarity_matrix[0][0]),
		print(coplanarity_matrix[0][1]),
		print(coplanarity_matrix[0][2])
		############################







			##################################
			# print variance of x
			print("IMU"),
			print(self.sigma[0][0]),
			print(self.sigma[1][1]),
			print(self.sigma[2][2])
			# print variance of a
			#print("IMU"),
			#print(self.sigma[6][6]),
			#print(self.sigma[7][7]),
			#print(self.sigma[8][8])
			##################################

		##################################
		# print variance of x
		print("Camera"),
		print(self.sigma[0][0]),
		print(self.sigma[1][1]),
		print(self.sigma[2][2])
		# print variance of a
		#print("Camera"),
		#print(self.sigma[6][6]),
		#print(self.sigma[7][7]),
		#print(self.sigma[8][8])
		##################################





		###################
		print("====================")
		for X_ in self.X:
			print(X_.x[0]),
			print(X_.x[1]),
			print(X_.x[2])
		print("====================")
		###################

		###################
		print("------------------------")
		for X_ in self.X:
			print(X_.x[0]),
			print(X_.x[1]),
			print(X_.x[2])
		print("------------------------")
		###################