def predictionAndUpdateOneParticle_firsttime(self, X, dt, dt2, keypoints, step, P): """ FastSLAM1.0 """ weight = 0.0 # weight (return value) weights = [] # 姿勢予測 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: # 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: print("Error on pf_step_camera_firsttime") self.count+=1 return X_, weight
def predictionAndUpdateOneParticle_firsttime(self, X, dt, dt2, keypoints, step, P): """ FastSLAM1.0 """ weight = 0.0 # weight (return value) weights = [] # 姿勢予測 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: # 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: print("Error on pf_step_camera_firsttime") self.count += 1 return X_, weight
def predictionAndUpdateOneParticle(self, X, dt, dt2, noise, keypoints, step, P): """ FastSLAM1.0 """ weight = 0.0 # weight (return value) weights = [] count_of_known_keypoints = 0 # 姿勢予測 prediction of position X_ = Particle() X_.landmarks = X.landmarks X_.x = X.x + dt * X.v + dt2 * X.a + self.dt2_camera * noise X_.v = X.v + dt * X.a + self.dt_camera * noise X_.a = X.a X_.o = X.o 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? 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] # Observation z z = np.array([keypoint.x, keypoint.y]) # Calc h and H (Jacobian matrix of h) h, Hx, H = X_.landmarks[landmarkId].calcObservation( X_, self.focus) # Kalman filter (Landmark update) X_.landmarks[landmarkId].mu, X_.landmarks[ landmarkId].sigma, S, Sinv = KF.execEKF1Update( z, h, X_.landmarks[landmarkId].mu, X_.landmarks[landmarkId].sigma, H, self.R) # Calc weight w = 0.0 try: w = (1.0 / (math.sqrt(np.linalg.det(2.0 * math.pi * S))) ) * np.exp(-0.5 * ((z - h).T.dot(Sinv.dot(z - h)))) except: print("Error on calc weight: ") weights.append(w) ############################### if (self.count == 0): #print("z "), #print(z) #print("h "), #print(h) #print((math.sqrt( np.linalg.det(2.0 * math.pi * S) ))) pass ############################### for w in weights: weight += w """ if(count_of_known_keypoints > 0): for i in xrange(len(weights)): if(i==0): weight = (100*w) else: weight *= (100*w) """ ############################### #print("weight "+str(weight)) if (self.count == 0): #print("weight "+str(weight)) pass ########################### self.count += 1 ########################### return X_, weight
def predictionAndUpdateOneParticle2(self, X, dt, dt2, keypoints, step, P): """ FastSLAM2.0 """ RSS = [] # residual sum of squares weight = 0.0 # weight (return value) 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 """ mu_ = copy.deepcopy(X_.landmarks[landmarkId].mu) sigma_ = copy.deepcopy(X_.landmarks[landmarkId].sigma) S = Hm.dot(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)) """ 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_, XYZ = 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 rss_ = (z - z_).T.dot(Linv.dot(z - z_)) RSS.append(rss_) w = (1.0 / (math.sqrt(np.linalg.det(2.0 * math.pi * L)))) * np.exp( -0.5 * (rss_)) weights.append(w) ############################### end_time_ = time.clock() if (self.count == 0): #print(XYZ) #print(x_diff) #print ""+str(landmarkId)+" update time = %f" %(end_time_-start_time_) pass ############################### if (count_of_known_keypoints > 0): # 残差が最小のときの重みと位置を採用する max_index = RSS.index(min(RSS)) weight = weights[max_index] weight *= 1000 X_.x += x_diffs[max_index] X_.v += (2.0 * x_diffs[max_index]) / self.dt_camera ############################### #print("weight "+str(weight)) #if(self.count == 0): #print("weight "+str(weight)) ########################### self.count += 1 ########################### return X_, weight