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
