class SLAM_Localization(LandmarkLocalization, EKF_SLAM):
alpha = 3.0 # factor in estimating covariance.
def __init__(self, nodeName="slam_ekf"):
super(SLAM_Localization, self).__init__(nodeName)
self.icp = SubICP()
self.extraction = Extraction()
self.isFirstScan = True
self.laser_count = 0
# interval
self.laser_interval = 5
# State Vector [x y yaw].T, column vector.
# self.xOdom = np.zeros((STATE_SIZE,1))
self.xEst = np.zeros((STATE_SIZE, 1))
# Covariance. Initial is very certain.
self.PEst = np.zeros((STATE_SIZE, STATE_SIZE))
# landMark Estimation. Like former self.tar_pc
self.lEst = np.zeros((LM_SIZE, 0)) # lEst should be of 2*N size
# ros topic
self.laser_sub = rospy.Subscriber('/course_agv/laser/scan', LaserScan,
self.laserCallback)
# self.location_pub = rospy.Publisher('ekf_location',Odometry,queue_size=3)
## localization parameters
# minimum landmark matches to update.
self.min_match = int(rospy.get_param('/slam/min_match', 2))
# minimum number of points for a landmark cluster
self.extraction.landMark_min_pt = int(
rospy.get_param('/slam/landMark_min_pt', 1))
# maximum radius to be identified as landmark
self.extraction.radius_max_th = float(
rospy.get_param('/slam/radius_max_th', 0.4))
OBSTACLE_RADIUS = 0.35
# feed icp landmark instead of laser.
def laserCallback(self, msg):
print('------seq: ', msg.header.seq)
if self.isFirstScan:
# feed in landmarks.
# self.icp.tar_pc=self.extraction.process(msg)
self.icp.tar_pc = self.icp.laserToNumpy(msg)
self.isFirstScan = False
return
# Update once every 5 laser scan because the we cannot distinguish rotation if interval is too small.
self.laser_count += 1
if self.laser_count < self.laser_interval:
return
# Updating process
self.laser_count = 0
# z is the landmarks in self frame as a 2*n array.
z = self.extraction.process(msg, True)
self.publishLandMark(z, "b")
# relative displacement
u = self.calc_odometry(msg)
# xEst,lEst,PEst is both predicted and updated in the ekf.
self.xEst, self.lEst, self.PEst = self.estimate(
self.xEst, self.lEst, self.PEst, z, u)
self.publishResult()
return
def calc_odometry(self, msg):
'''
Let icp handle the odometry, returns a relative odometry u.
'''
# laser callback is manually fed by its owner class.
return self.icp.laserCallback(msg)
# EKF virtual function.
def observation_model(self, xEst, lEst):
'''
returns an 2*n column vector list.
[z1,z2,....zn]
'''
rotation = tf.transformations.euler_matrix(0, 0, xEst[2, 0])[0:2, 0:2]
z = np.dot(rotation.T, lEst - xEst[0:2])
return z
def estimate(self, xEst, lEst, PEst, z, u):
G, Fx = self.jacob_motion(xEst, lEst, u)
# FIXME: the G and Fx transpose is confused. Thanks god they are all symmetric ones...
covariance = np.dot(G, np.dot(PEst, G.T)) + np.dot(
Fx, np.dot(Cx, Fx.T))
# Predict
xPredict = self.odom_model(xEst, u)
zEst = self.observation_model(xPredict, lEst)
self.publishLandMark(zEst,
color="r",
namespace="estimate",
frame=self.nodeName)
neighbour = self.icp.findNearest(z, zEst)
# Predicted
zPredict = zEst[:, neighbour.tar_indices]
self.publishLandMark(zPredict,
color="g",
namespace="paired",
frame=self.nodeName)
lSize = np.size(lEst, 1)
zSize = np.size(z, 1)
# how many observed landmarks are missed, how many new ones will be added.
missedIndices = sorted(
list(set(range(zSize)) - set(neighbour.src_indices)))
paired = len(neighbour.src_indices)
missed = len(missedIndices)
neighbour.src_indices += missedIndices
# the newly-added landmarks
neighbour.tar_indices += range(lSize, missed + lSize)
zObserved = z[:, neighbour.src_indices]
# add new landmarks to newL (new lEst)
# newL=np.repeat(xPredict[0:2],missed,axis=1)
# delicately select the spawning position...
newL = np.dot(
tf.transformations.euler_matrix(0, 0, xPredict[2, 0])[0:2, 0:2],
z[:, missedIndices]) + xPredict[0:2]
newL = np.hstack((lEst, newL))
zEst = self.observation_model(xPredict, newL)
zPredict = zEst[:, neighbour.tar_indices]
# the newly-added landmarks' uncertainty is very large
# block is so powerful!
covariance = scilinalg.block_diag(covariance,
np.diag([INF] * LM_SIZE * missed))
yPredict = self.XLtoY(xPredict, newL)
variance = self.alpha / (zSize + self.alpha) * OBSTACLE_RADIUS
print("\n\npaired: {} variance: {} missed: {} landmarks: {}".format(
paired, variance, missed, lSize + missed))
if zSize < self.min_match:
print("Observed landmarks are too little to execute update.")
# only update according to the prediction stage.
return xPredict, newL, covariance
m = self.jacob_h(newL, neighbour, xPredict)
# z (2*n)array->(2n*1) array
zPredict = np.vstack(np.hsplit(zPredict, np.size(zPredict, 1)))
zObserved = np.vstack(np.hsplit(zObserved, np.size(zObserved, 1)))
# print("delta z: \n{}\n".format(zObserved-zPredict))
# print("covariance:\n{}\n".format(covariance))
# Karman factor. Universal formula.
K = np.dot(
np.dot(covariance, m.T),
np.linalg.inv(
np.dot(m, np.dot(covariance, m.T)) +
np.diag([variance**2] * 2 * zSize)))
# Update
fix = np.dot(K, zObserved - zPredict)
# print("fix: \n{}\n\n".format(fix))
yEst = yPredict + fix
PEst = covariance - np.dot(K, np.dot(m, covariance))
xEst, lEst = self.YtoXL(yEst)
return xEst, lEst, PEst
def XLtoY(self, x, l):
# split y (2*N) to 2N*1
y = np.vstack(np.hsplit(l, np.size(l, 1)))
y = np.vstack((x, y))
return y
def YtoXL(self, y):
x, l = np.vsplit(y, [STATE_SIZE])
l = np.hstack(np.vsplit(l, np.size(l) / LM_SIZE))
return x, l
class Localization():
def __init__(self):
self.icp = ICP()
self.ekf = EKF()
self.extraction = Extraction()
# odom robot init states
self.robot_x = rospy.get_param('/icp/robot_x',0)
self.robot_y = rospy.get_param('/icp/robot_y',0)
self.robot_theta = rospy.get_param('/icp/robot_theta',0)
self.sensor_sta = [self.robot_x,self.robot_y,self.robot_theta]
self.isFirstScan = True
self.src_pc = []
self.tar_pc = []
# State Vector [x y yaw]
self.xOdom = np.zeros((3,1))
self.xEst = np.zeros((3,1))
self.PEst = np.eye(3)
# map observation
self.obstacle = []
# radius
self.obstacle_r = 10
# init map
self.updateMap()
# ros topic
self.laser_sub = rospy.Subscriber('/course_agv/laser/scan',LaserScan,self.laserCallback)
self.location_pub = rospy.Publisher('ekf_location',Odometry,queue_size=3)
self.odom_pub = rospy.Publisher('icp_odom',Odometry,queue_size=3)
self.odom_broadcaster = tf.TransformBroadcaster()
self.landMark_pub = rospy.Publisher('/landMarks',MarkerArray,queue_size=1)
def updateMap(self):
print("debug: try update map obstacle")
rospy.wait_for_service('/static_map')
try:
getMap = rospy.ServiceProxy('/static_map',GetMap)
self.map = getMap().map
# print(self.map)
except:
e = sys.exc_info()[0]
print('Service call failed: %s'%e)
# Update for planning algorithm
map_data = np.array(self.map.data).reshape((-1,self.map.info.height)).transpose()
tx,ty = np.nonzero((map_data > 20)|(map_data < -0.5))
ox = (tx*self.map.info.resolution+self.map.info.origin.position.x)*1.0
oy = (ty*self.map.info.resolution+self.map.info.origin.position.y)*1.0
self.obstacle = np.vstack((ox,oy)).transpose()
self.obstacle_r = self.map.info.resolution
print("debug: update map obstacle success! ")
def laserCallback(self,msg):
u = self.calc_odometry(msg)
z = self.ekf.odom_model(self.xEst,self.calc_map_observation(self.laserToNumpy(msg)))
self.xEst,self.PEst = self.ekf.estimate(self.xEst,self.PEst,z,u)
self.publishResult()
pass
def laserEstimation(self):
# TODO
return pc
def calc_map_observation(self,msg):
landmarks_src = self.extraction.process(msg,True)
landmarks_tar = self.extraction.process(self.laserEstimation(),True)
print("lm : ",len(landmarks_src.id),len(landmarks_tar.id))
self.publishLandMark(landmarks_src,landmarks_tar)
src_pc = self.lm2pc(landmarks_src)
tar_pc = self.lm2pc(landmarks_tar)
transform_acc = self.icp.process(tar_pc,src_pc)
return self.T2u(transform_acc)
def calc_odometry(self,msg):
if self.isFirstScan:
self.tar_pc = self.laserToNumpy(msg)
self.isFirstScan = False
return np.array([[0.0,0.0,0.0]]).T
self.src_pc = self.laserToNumpy(msg)
transform_acc = self.icp.process(self.tar_pc,self.src_pc)
self.tar_pc = self.laserToNumpy(msg)
return self.T2u(transform_acc)
def lm2pc(self,lm):
total_num = len(lm.id)
dy = lm.position_y
dx = lm.position_x
range_l = np.hypot(dy,dx)
angle_l = np.arctan2(dy,dx)
pc = np.ones((3,total_num))
pc[0:2,:] = np.vstack((np.multiply(np.cos(angle_l),range_l),np.multiply(np.sin(angle_l),range_l)))
print("mdbg ",total_num)
return pc
def laserToNumpy(self,msg):
total_num = len(msg.ranges)
pc = np.ones([3,total_num])
range_l = np.array(msg.ranges)
angle_l = np.linspace(msg.angle_min,msg.angle_max,total_num)
pc[0:2,:] = np.vstack((np.multiply(np.cos(angle_l),range_l),np.multiply(np.sin(angle_l),range_l)))
return pc
def T2u(self,t):
dw = math.atan2(t[1,0],t[0,0])
u = np.array([[t[0,2],t[1,2],dw]])
return u.T
def publishResult(self):
# tf
s = self.xEst.reshape(-1)
q = tf.transformations.quaternion_from_euler(0,0,s[2])
self.odom_broadcaster.sendTransform((s[0],s[1],0.001),(q[0],q[1],q[2],q[3]),
rospy.Time.now(),"ekf_location","world_base")
# odom
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "world_base"
odom.pose.pose.position.x = s[0]
odom.pose.pose.position.y = s[1]
odom.pose.pose.position.z = 0.001
odom.pose.pose.orientation.x = q[0]
odom.pose.pose.orientation.y = q[1]
odom.pose.pose.orientation.z = q[2]
odom.pose.pose.orientation.w = q[3]
self.location_pub.publish(odom)
s = self.xOdom
q = tf.transformations.quaternion_from_euler(0,0,s[2])
# odom
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "world_base"
odom.pose.pose.position.x = s[0]
odom.pose.pose.position.y = s[1]
odom.pose.pose.position.z = 0.001
odom.pose.pose.orientation.x = q[0]
odom.pose.pose.orientation.y = q[1]
odom.pose.pose.orientation.z = q[2]
odom.pose.pose.orientation.w = q[3]
self.odom_pub.publish(odom)
# self.laser_pub.publish(self.target_laser)
pass
def publishLandMark(self,msg,msg2):
# msg = LandMarkSet()
if len(msg.id) <= 0:
return
landMark_array_msg = MarkerArray()
for i in range(len(msg.id)):
marker = Marker()
marker.header.frame_id = "course_agv__hokuyo__link"
marker.header.stamp = rospy.Time(0)
marker.ns = "lm"
marker.id = i
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = msg.position_x[i]
marker.pose.position.y = msg.position_y[i]
marker.pose.position.z = 0 # 2D
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 0.5 # Don't forget to set the alpha
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
landMark_array_msg.markers.append(marker)
for i in range(len(msg2.id)):
marker = Marker()
marker.header.frame_id = "course_agv__hokuyo__link"
marker.header.stamp = rospy.Time(0)
marker.ns = "lm"
marker.id = i+len(msg.id)
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = msg2.position_x[i]
marker.pose.position.y = msg2.position_y[i]
marker.pose.position.z = 0 # 2D
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 0.5 # Don't forget to set the alpha
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
landMark_array_msg.markers.append(marker)
self.landMark_pub.publish(landMark_array_msg)
class SLAM_EKF():
def __init__(self):
# ros param
self.robot_x = rospy.get_param('/slam/robot_x',0)
self.robot_y = rospy.get_param('/slam/robot_y',0)
self.robot_theta = rospy.get_param('/slam/robot_theta',0)
## ros param of mapping
self.map_x_width = rospy.get_param('/slam/map_width')
self.map_y_width = rospy.get_param('/slam/map_height')
self.map_reso = rospy.get_param('/slam/map_resolution')
self.map_cellx_width = int(round(self.map_x_width/self.map_reso))
self.map_celly_width = int(round(self.map_y_width/self.map_reso))
self.icp = ICP()
self.ekf = EKF()
self.extraction = Extraction()
self.mapping = Mapping(self.map_cellx_width,self.map_celly_width,self.map_reso)
# odom robot init states
self.sensor_sta = [self.robot_x,self.robot_y,self.robot_theta]
self.isFirstScan = True
self.src_pc = []
self.tar_pc = []
# State Vector [x y yaw]
self.xOdom = np.zeros((STATE_SIZE, 1))
self.xEst = np.zeros((STATE_SIZE, 1))
self.PEst = np.eye(STATE_SIZE)
# map observation
self.obstacle = []
# radius
self.obstacle_r = 10
# ros topic
self.laser_sub = rospy.Subscriber('/course_agv/laser/scan',LaserScan,self.laserCallback)
self.location_pub = rospy.Publisher('ekf_location',Odometry,queue_size=3)
self.odom_pub = rospy.Publisher('icp_odom',Odometry,queue_size=3)
self.odom_broadcaster = tf.TransformBroadcaster()
self.landMark_pub = rospy.Publisher('/landMarks',MarkerArray,queue_size=1)
self.map_pub = rospy.Publisher('/slam_map',OccupancyGrid,queue_size=1)
def laserCallback(self,msg):
np_msg = self.laserToNumpy(msg)
lm = self.extraction.process(np_msg)
# u = self.calc_odometry(self.lm2pc(lm))
u = self.calc_odometry(np_msg)
z = self.observation(lm)
self.xEst,self.PEst = self.ekf.estimate(self.xEst,self.PEst,z,u)
# TODO
# obs = self.u2T(self.xEst[0:3]).dot(np_msg)
# pmap = self.mapping.update(obs[0], obs[1], self.xEst[0], self.xEst[1])
# self.publishMap(pmap)
self.publishLandMark(lm)
self.publishResult()
pass
def observation(self,lm):
landmarks = lm
z = np.zeros((0, 3))
for i in range(len(landmarks.id)):
dx = landmarks.position_x[i]
dy = landmarks.position_y[i]
d = math.hypot(dx, dy)
angle = self.ekf.pi_2_pi(math.atan2(dy, dx))
zi = np.array([d, angle, i])
z = np.vstack((z, zi))
return z
def calc_odometry(self,np_msg):
if self.isFirstScan:
self.tar_pc = np_msg
self.isFirstScan = False
return np.array([[0.0,0.0,0.0]]).T
self.src_pc = np_msg
transform_acc = self.icp.process(self.tar_pc,self.src_pc)
self.tar_pc = np_msg
return self.T2u(transform_acc)
def laserToNumpy(self,msg):
total_num = len(msg.ranges)
pc = np.ones([3,total_num])
range_l = np.array(msg.ranges)
range_l[range_l == np.inf] = MAX_LASER_RANGE
angle_l = np.linspace(msg.angle_min,msg.angle_max,total_num)
pc[0:2,:] = np.vstack((np.multiply(np.cos(angle_l),range_l),np.multiply(np.sin(angle_l),range_l)))
return pc
def T2u(self,t):
dw = math.atan2(t[1,0],t[0,0])
u = np.array([[t[0,2],t[1,2],dw]])
return u.T
def u2T(self,u):
w = u[2]
dx = u[0]
dy = u[1]
return np.array([
[ math.cos(w),-math.sin(w), dx],
[ math.sin(w), math.cos(w), dy],
[0,0,1]
])
def lm2pc(self,lm):
total_num = len(lm.id)
dy = lm.position_y
dx = lm.position_x
range_l = np.hypot(dy,dx)
angle_l = np.arctan2(dy,dx)
pc = np.ones((3,total_num))
pc[0:2,:] = np.vstack((np.multiply(np.cos(angle_l),range_l),np.multiply(np.sin(angle_l),range_l)))
print("mdbg ",total_num)
return pc
def publishResult(self):
# tf
s = self.xEst.reshape(-1)
q = tf.transformations.quaternion_from_euler(0,0,s[2])
self.odom_broadcaster.sendTransform((s[0],s[1],0.001),(q[0],q[1],q[2],q[3]),
rospy.Time.now(),"ekf_location","world_base")
# odom
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "world_base"
odom.pose.pose.position.x = s[0]
odom.pose.pose.position.y = s[1]
odom.pose.pose.position.z = 0.001
odom.pose.pose.orientation.x = q[0]
odom.pose.pose.orientation.y = q[1]
odom.pose.pose.orientation.z = q[2]
odom.pose.pose.orientation.w = q[3]
self.location_pub.publish(odom)
s = self.xOdom
q = tf.transformations.quaternion_from_euler(0,0,s[2])
# odom
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "world_base"
odom.pose.pose.position.x = s[0]
odom.pose.pose.position.y = s[1]
odom.pose.pose.position.z = 0.001
odom.pose.pose.orientation.x = q[0]
odom.pose.pose.orientation.y = q[1]
odom.pose.pose.orientation.z = q[2]
odom.pose.pose.orientation.w = q[3]
self.odom_pub.publish(odom)
print("mdbg ",self.xEst.shape)
# self.laser_pub.publish(self.target_laser)
pass
def publishLandMark(self,msg):
# msg = LandMarkSet()
if len(msg.id) <= 0:
return
landMark_array_msg = MarkerArray()
for i in range(len(msg.id)):
marker = Marker()
marker.header.frame_id = "course_agv__hokuyo__link"
marker.header.stamp = rospy.Time(0)
marker.ns = "lm"
marker.id = i
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = msg.position_x[i]
marker.pose.position.y = msg.position_y[i]
marker.pose.position.z = 0 # 2D
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 0.5 # Don't forget to set the alpha
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
landMark_array_msg.markers.append(marker)
for i in range(((self.xEst.shape[0]-STATE_SIZE)/2)):
marker = Marker()
marker.header.frame_id = "world_base"
marker.header.stamp = rospy.Time(0)
marker.ns = "lm"
marker.id = i+len(msg.id)
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = self.xEst[STATE_SIZE+i*2,0]
marker.pose.position.y = self.xEst[STATE_SIZE+i*2+1,0]
marker.pose.position.z = 0 # 2D
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 0.5 # Don't forget to set the alpha
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
landMark_array_msg.markers.append(marker)
self.landMark_pub.publish(landMark_array_msg)
def publishMap(self,pmap):
map_msg = OccupancyGrid()
map_msg.header.seq = 1
map_msg.header.stamp = rospy.Time().now()
map_msg.header.frame_id = "map"
map_msg.info.map_load_time = rospy.Time().now()
map_msg.info.resolution = self.map_reso
map_msg.info.width = self.map_cellx_width
map_msg.info.height = self.map_celly_width
map_msg.info.origin.position.x = -self.map_cellx_width*self.map_reso/2.0
map_msg.info.origin.position.y = -self.map_celly_width*self.map_reso/2.0
map_msg.info.origin.position.z = 0
map_msg.info.origin.orientation.x = 0
map_msg.info.origin.orientation.y = 0
map_msg.info.origin.orientation.z = 0
map_msg.info.origin.orientation.w = 1.0
map_msg.data = list(pmap.T.reshape(-1)*100)
self.map_pub.publish(map_msg)
class EKF_Landmark_Localization(LandmarkLocalization, EKF_Landmark):
alpha = 3.0 # factor in estimating covariance.
def __init__(self, nodeName="ekf_icp"):
super(EKF_Landmark_Localization, self).__init__(nodeName)
self.icp = SubICP()
self.extraction = Extraction()
self.src_pc = None
self.isFirstScan = True
self.laser_count = 0
# interval
self.laser_interval = 5
# State Vector [x y yaw].T, column vector.
self.xEst = np.zeros((STATE_SIZE, 1))
# Covariance. Initial state is certain.
# self.PEst=np.eye(STATE_SIZE)
self.PEst = np.zeros((STATE_SIZE, STATE_SIZE))
# init map
# map observation
self.tar_pc = None
self.updateMap()
# ros topic
self.laser_sub = rospy.Subscriber('/course_agv/laser/scan', LaserScan,
self.laserCallback)
# self.location_pub = rospy.Publisher('ekf_location',Odometry,queue_size=3)
# parameters from launch file.
# minimum landmark matches to update. Actually even 1 point is acceptable.
self.min_match = int(rospy.get_param('/localization/min_match', 1))
# minimum number of points for a landmark cluster
self.extraction.landMark_min_pt = int(
rospy.get_param('/localization/landMark_min_pt', 2))
# maximum radius to be identified as landmark
self.extraction.radius_max_th = float(
rospy.get_param('/localization/radius_max_th', 0.4))
def updateMap(self):
'''
get all the isolated pixels in the map as landmarks,
then extract the positions of landmarks into self.tar_pc
'''
print("debug: try update map obstacle")
rospy.wait_for_service('/static_map')
try:
getMap = rospy.ServiceProxy('/static_map', GetMap)
self.map = getMap().map
# print(self.map)
except:
e = sys.exc_info()[0]
print('Service call failed: %s' % e)
# transposed (row,column)-> (x,y)
self.map.data = np.array(self.map.data).reshape(
(-1, self.map.info.height)).transpose()
tx, ty = np.nonzero((self.map.data > 20) | (self.map.data < -0.5))
landmarkList = []
for (x, y) in zip(tx, ty):
if self.isolated((x, y)):
landmarkList.append((x, y))
originPos = np.array(
[self.map.info.origin.position.x, self.map.info.origin.position.y])
# numpy's broadcasting feature
self.tar_pc = np.transpose(
np.array(landmarkList) * self.map.info.resolution + originPos)
print("landmark list:\n{}".format(self.tar_pc))
print("debug: update map obstacle success! ")
def isolated(self, pair):
# direction: up, down, left, right
directions = np.array([(0, 1), (0, -1), (-1, 0), (1, 0)])
pair = np.array(pair)
for dr in directions:
newPair = pair + dr
# detects if the obstacle is on the boundary
if newPair[0] < 0 or newPair[0] >= self.map.info.width or newPair[
1] < 0 or newPair[1] >= self.map.info.height:
continue
if self.map.data[tuple(newPair)] < -0.5 or self.map.data[tuple(
newPair)] > 20:
return False
return True
# feed icp landmark instead of laser.
def laserCallback(self, msg):
print('------seq: ', msg.header.seq)
if self.isFirstScan:
# feed in landmarks.
# self.icp.tar_pc=self.extraction.process(msg)
self.icp.tar_pc = self.icp.laserToNumpy(msg)
self.isFirstScan = False
return
# Update once every 5 laser scan because the we cannot distinguish rotation if interval is too small.
self.laser_count += 1
if self.laser_count < self.laser_interval:
return
# Updating process
self.laser_count = 0
landmarks = self.extraction.process(msg, True)
self.publishLandMark(landmarks, "b")
u = self.calc_odometry(msg)
# z is the landmarks as a 2*n array.
z = landmarks
# xEst is both predicted and updated in the ekf.
self.xEst, self.PEst = self.estimate(self.xEst, self.PEst, z, u)
self.publishResult()
return
def calc_odometry(self, msg):
'''
Let icp handle the odometry, returns a relative odometry u.
'''
# laser callback is manually fed by its owner class.
return self.icp.laserCallback(msg)
# EKF virtual function.
def observation_model(self, xEst):
'''
returns an 2*n column vector list.
[z1,z2,....zn]
'''
rotation = tf.transformations.euler_matrix(0, 0, xEst[2, 0])[0:2, 0:2]
z = np.dot(rotation.T, self.tar_pc - xEst[0:2, 0].reshape(2, 1))
return z
def estimate(self, xEst, PEst, z, u):
G, Fx = self.jacob_motion(xEst, u)
# FIXME: the G and Fx transpose is confused. Thanks god they are all symmetric matrices...
covariance = np.dot(G, np.dot(PEst, G.T)) + np.dot(
Fx, np.dot(Cx, Fx.T))
# Predict
xPredict = self.odom_model(xEst, u)
zEst = self.observation_model(xPredict)
self.publishLandMark(zEst,
color="r",
namespace="estimate",
frame="ekf_icp")
neighbour = self.icp.findNearest(z, zEst)
zPredict = zEst[:, neighbour.tar_indices]
zPrime = z[:, neighbour.src_indices]
length = len(neighbour.src_indices)
variance = self.alpha / (length + self.alpha) * OBSTACLE_RADIUS
print("\n\nlength: {} variance: {}".format(length, variance))
# turns out no matter how little the information is, it is of value somewhat. min_match can be set to 1.
if length < self.min_match:
print("Matching points are too little to execute update.")
# only update according to the prediction stage.
return xPredict, covariance
self.publishLandMark(zPredict,
color="g",
namespace="paired",
frame="ekf_icp")
m = self.jacob_h(self.tar_pc, neighbour, xPredict)
# z (2*n)array->(2n*1) array
zPredict = np.vstack(np.hsplit(zPredict, np.size(zPredict, 1)))
zPrime = np.vstack(np.hsplit(zPrime, np.size(zPrime, 1)))
print("delta z: \n{}\n\n".format(zPrime - zPredict))
# Karman factor. Universal formula.
K = np.dot(
np.dot(covariance, m.T),
np.linalg.inv(
np.dot(m, np.dot(covariance, m.T)) +
np.diag([variance] * 2 * length)))
# Update
xEst = xPredict + np.dot(K, zPrime - zPredict)
PEst = covariance - np.dot(K, np.dot(m, covariance))
return xEst, PEst
