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 step(self):
scan, odom = self.raw_data[self.idx]
rot, trans = getRtFromM(self.scan_base)
scan = transpose(rot, trans, scan)
try:
if not self.checkupdate(matrix_to_pose(self.last_odom),matrix_to_pose(odom)):
return False
last_odom = self.last_odom
last_scan = self.last_scan
self.last_odom = odom
self.last_scan = scan
except AttributeError:
self.last_odom = odom
self.last_scan = scan
return False
print check_loop_candidate(scan)
delta_odom_init = getDeltaM(last_odom, odom)
delta_rot_init, delta_trans_init = getRtFromM(delta_odom_init)
#start_time = time.time()
icp = ICP(last_scan, scan)
#delta_rot, delta_trans, cost = icp.calculate(20, delta_rot_init, delta_trans_init, 0.01)
delta_rot = delta_rot_init
delta_trans = delta_trans_init
delta_odom = getMFromRt(delta_rot, delta_trans)
#print 'init yaw:',matrix_to_pose(delta_odom_init)[2]
#print 'icp yaw:',matrix_to_pose(delta_odom)[2]
self.pose_matrix = np.dot(self.pose_matrix, delta_odom)
self.graph_base.append((scan, delta_odom, self.pose_matrix))
#print self.pose_matrix
rot, trans = getRtFromM(self.pose_matrix)
scan_t = transpose(rot, trans, scan)
self.gui.setpcd(scan_t)
self.gui.setrobot(matrix_to_pose(self.pose_matrix))
return True
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 __init__(self):
self.gui_ = ScanGUI()
# map params
self.map_w = 20.0 #5x5 physical map
self.map_h = 20.0
# self.map_ = SparseMap(res = 0.02)
# self.map_ = DenseMap(w=self.map_w, h=self.map_h, res = 0.1)
self.map_ = ProbMap(w=self.map_w, h=self.map_h, res=0.1)
# raycast params
self.dist_res = .01
self.ang_res = np.deg2rad(1.0)
# query params
self.sensor_radius = 5.0
#self.seen_thresh = 2
self.seen_thresh = 0.68 # probability! TODO: tune
#Robot properities
self.linVector = Vector3(x=0.0, y=0.0, z=0.0)
self.angVector = Vector3(x=0.0, y=0.0, z=0.0)
self.lidar_range = 5.0
# 2d pose
self.x = 0
self.y = 0
self.theta = 0
self.path = np.empty(shape=(0,3), dtype=np.float32)
# sensor data cache
self.ranges = [] #From stable scan
self.old_ranges = []
self.points = [] #From projected stable scan
self.old_points = []
self.img = []
#Offset based on ICP, (x,y,theta)
self.map_to_odom = np.zeros(shape=3)
self.bridge = CvBridge()
#ICP
self.icp = ICP()
#Ben
#self.data_path = "/home/bziemann/data/"
#Jaime
self.data_path = "/tmp/"
#ROS2/.02
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=2)
self.vizPub = rospy.Publisher('/visualization_marker', Marker, queue_size=10)
self.tfl_ = tf.TransformListener()
self.tfb_ = tf.TransformBroadcaster()
rospy.Subscriber("/stable_scan", LaserScan, self.checkLaser)
rospy.Subscriber("/projected_stable_scan", PointCloud, self.checkPoints)
#rospy.Subscriber("/odom", Odometry, self.setLocation)
rospy.Subscriber('/camera/image_raw', Image, self.setImage)
def creategraph(self, data):
nodes = []
edges = []
# Generate basic nodes and edges for our pose graph
for i in range(len(data)):
scan, pose = data[i]
nodes.append(pose)
self.pg.nodes.append(pose)
if i == 0:
continue
infm = np.array([[weight_trans_, 0, 0], [0, weight_trans_, 0],
[0, 0, weight_rot_]])
edge = [i - 1, i, get_motion_vector(nodes[i], nodes[i - 1]), infm]
edges.append(edge)
self.pg.edges.append(PoseEdge(*edge))
self.pg.nodes = np.array(self.pg.nodes)
# Detect the loop-closing
loop_count = 0
self.loop_candidates = []
for i in range(len(data)):
if i + min_loop_dist_ >= len(data):
continue
scan_i, pose_i = data[i]
if not check_loop_candidate(scan_i, hough_weight_):
continue
self.loop_candidates.append(i)
for j in range(i + min_loop_dist_, len(data)):
scan_j, pose_j = data[j]
dst = distance.euclidean(pose_i[0:2], pose_j[0:2])
if dst > max_dist_:
continue
#if not check_loop_candidate(scan_j, hough_weight_):
# continue
loop_count += 1
delta_odom_init = getDeltaM(v2t(pose_i), v2t(pose_j))
delta_rot_init, delta_trans_init = getRtFromM(delta_odom_init)
#start_time = time.time()
icp = ICP(scan_i, scan_j)
delta_rot, delta_trans, cost = icp.calculate(
200, delta_rot_init, delta_trans_init, max_dist_)
if (cost > first_cost_):
#print 'old cost:',cost
delta_rot, delta_trans, cost = icp.calculate(
200, delta_rot, delta_trans, 0.5)
#print 'new cost:',cost
#print '-------------'
if (use_cost_threshold_ and (cost > second_cost_)):
continue
#cost = cost/5
if icp_debug_:
new_scan_j = transpose(delta_rot, delta_trans.ravel(),
scan_j)
scan_j = transpose(delta_rot_init,
delta_trans_init.ravel(), scan_j)
show_icp_matching(
scan_i, new_scan_j, scan_j,
str(i) + '-->' + str(j) + 'cost:' + str(cost))
#plt.show()
infm = np.array([[weight_trans_ / cost**2, 0, 0],
[0, weight_trans_ / cost**2, 0],
[0, 0, weight_rot_ / cost**2]])
edge = [i, j, t2v(getMFromRt(delta_rot, delta_trans)), infm]
edges.append(edge)
self.pg.edges.append(PoseEdge(*edge))
print('{0} loop have been detected!'.format(loop_count))
'--number',
dest='pointNum',
help='number of points in the point cloud',
type='int',
default=256)
parser.add_option('-p',
'--plot',
dest='plot',
action='store_true',
help='visualize icp result',
default=False)
parser.add_option('-c',
'--core',
dest='coreNum',
help='number of threads used in the cuda',
type='int',
default=0)
(options, args) = parser.parse_args()
pc1 = PointCloud()
pc1.initFromRand(options.pointNum, 0, 1, 10, 20, 0, 1)
pc2 = PointCloud(pc1)
pc2.applyTransformation(None, None)
icpSolver = ICP(pc1, pc2, plot=options.plot)
icpSolver.solve()
print
icpParallelSolver = ICPParallel(pc1,
pc2,
numCore=options.coreNum,
plot=options.plot)
icpParallelSolver.solve()