def SCM_test():
# SCM parms
num_rings = args.num_rings
num_sectors = args.num_sectors
num_candidates = args.num_candidates
loop_threshold = args.loop_threshold
# data
sequence_dir = os.path.join(args.data_dir, args.sequence_idx, 'velodyne')
sequence_manager = Ptutils.KittiScanDirManager(sequence_dir)
scan_paths = sequence_manager.scan_fullpaths
SCM = ScanContextManager([num_rings, num_sectors], num_candidates,
loop_threshold)
num_downsample_points = 10000
dists = []
yaw_diff_degs = []
print("starting...")
for i, scan_path in tqdm(enumerate(scan_paths), total=len(scan_paths)):
# print('Running:{}...'.format(i))
curr_scan_pts = Ptutils.readScan(scan_path)
curr_scan_down_pts = Ptutils.random_sampling(curr_scan_pts,
num_downsample_points)
# add current node
SCM.addNode(i, curr_scan_down_pts)
if i > 30:
loop_idx, loop_dist, yaw_diff_deg = SCM.detectLoop()
#print("loop_dist:", loop_dist)
dists.append(loop_dist)
yaw_diff_degs.append(yaw_diff_deg)
if loop_idx == None:
pass
else:
print("Loop detected at frame : ", loop_idx)
print("mean dis:", np.mean(dists))
print("std dis:", np.std(dists))
print("mean yaw:", np.mean(yaw_diff_degs))
print("std yaw:", np.std(yaw_diff_degs))
parser.add_argument('--try_gap_loop_detection', type=int, default=10) # same as the original paper
parser.add_argument('--loop_threshold', type=float, default=0.11) # 0.11 is usually safe (for avoiding false loop closure)
parser.add_argument('--data_base_dir', type=str,
default='/your/path/.../data_odometry_velodyne/dataset/sequences')
parser.add_argument('--sequence_idx', type=str, default='00')
parser.add_argument('--save_gap', type=int, default=300)
args = parser.parse_args()
# dataset
sequence_dir = os.path.join(args.data_base_dir, args.sequence_idx, 'velodyne')
sequence_manager = Ptutils.KittiScanDirManager(sequence_dir)
scan_paths = sequence_manager.scan_fullpaths
num_frames = len(scan_paths)
# Pose Graph Manager (for back-end optimization) initialization
PGM = PoseGraphManager()
PGM.addPriorFactor()
# Result saver
save_dir = "result/" + args.sequence_idx
if not os.path.exists(save_dir): os.makedirs(save_dir)
ResultSaver = PoseGraphResultSaver(init_pose=PGM.curr_se3,
save_gap=args.save_gap,
num_frames=num_frames,
seq_idx=args.sequence_idx,
save_dir=save_dir)
default=180) # same as the original paper
parser.add_argument('--num_candidates', type=int, default=10) # must be int
parser.add_argument('--try_gap_loop_detection', type=int,
default=20) # same as the original paper
parser.add_argument(
'--loop_threshold', type=float,
default=0.02) # 0.11 is usually safe (for avoiding false loop closure)
parser.add_argument('--sequence_idx', type=str, default='00')
parser.add_argument('--save_gap', type=int, default=300)
args = parser.parse_args()
resolution = [0.01, 0.01, np.deg2rad(1)]
data_path = "./Laser_Data/140106/laser_data"
data = Ptutils.read_symphony_scan_np(data_path)
num_frames = data.shape[0]
gps = load_gps()
# Pose Graph Manager (for back-end optimization) initialization
PGM = PoseGraphManager()
PGM.addPriorFactor()
# Result saver
save_dir = "result/" + args.sequence_idx
if not os.path.exists(save_dir): os.makedirs(save_dir)
ResultSaver = PoseGraphResultSaver(init_pose=PGM.curr_se2,
save_gap=args.save_gap,
num_frames=num_frames,
seq_idx=args.sequence_idx,
save_dir=save_dir)
def read_LiDAR(self, data):
# sequence_dir = os.path.join(self.data_base_dir, self.sequence_idx, 'velodyne')
# sequence_manager = Ptutils.KittiScanDirManager(sequence_dir)
# scan_paths = sequence_manager.scan_fullpaths
tic = time.time()
rate = rospy.Rate(20)
self.num_frames = 6000
# print('jhloi')
# Pose Graph Manager (for back-end optimization) initialization
# Result saver
self.save_dir = "result/" + self.sequence_idx
if not os.path.exists(self.save_dir): os.makedirs(self.save_dir)
ResultSaver = PoseGraphResultSaver(init_pose=self.curr_se3,
save_gap=self.save_gap,
num_frames=self.num_frames,
seq_idx=self.sequence_idx,
save_dir=self.save_dir)
# Scan Context Manager (for loop detection) initialization
# for save the results as a video
fig_idx = 1
num_frames_to_skip_to_show = 5
# num_frames_to_save = np.floor(num_frames/num_frames_to_skip_to_show)
# with writer.saving(fig, video_name, num_frames_to_save): # this video saving part is optional
# @@@ MAIN @@@: data stream
# for for_idx, scan_path in tqdm(enumerate(scan_paths), total=num_frames, mininterval=5.0):
# get current information
self.curr_scan_pts = ptstoxyz.pointcloud2_to_xyz_array(
data, remove_nans=True)
# print (len(self.curr_scan_pts), 'nubetotal')
# plane1 = pyrsc.Circle()
# best_eq, best_inliers = plane1.fit(self.curr_scan_pts, 0.01)
# print(best_inliers)
print(len(self.curr_scan_pts), 'origin')
model_robust, inliers = ransac(self.curr_scan_pts,
LineModelND,
min_samples=10,
residual_threshold=1,
max_trials=2)
# print(type(inliers))
pos_True = inliers == False
# print(len(inliers[pos_false]) , 'outliers')
# print(self.curr_scan_pts.shape)
# curr_scan_pts = Ptutils.readScan(scan_paths)
var = []
for i in range(0, len(inliers)):
if inliers[i] == False:
var.append(i)
self.curr_scan_pts = self.curr_scan_pts[var[:], :]
print(len(self.curr_scan_pts), 'ransac')
# save current node
curr_node_idx = self.for_idx # make start with 0
self.curr_scan_down_pts = Ptutils.random_sampling(
self.curr_scan_pts, num_points=self.num_icp_points)
print(len(self.curr_scan_down_pts), 'random')
if (curr_node_idx == 0):
self.prev_node_idx = curr_node_idx
self.prev_scan_pts = copy.deepcopy(self.curr_scan_pts)
self.icp_initial = np.eye(4)
# continue
# calc odometry
# print (self.prev_scan_down_pts)
self.prev_scan_down_pts = Ptutils.random_sampling(
self.prev_scan_pts, num_points=self.num_icp_points)
self.odom_transform, _, _ = ICP.icp(self.curr_scan_down_pts,
self.prev_scan_down_pts,
init_pose=self.icp_initial,
max_iterations=100)
# print (self.odom_transform)
# update the current (moved) pose
self.curr_se3 = np.matmul(self.curr_se3, self.odom_transform)
self.icp_initial = self.odom_transform # assumption: constant velocity model (for better next ICP converges)
# print(self.odom_transform)
# add the odometry factor to the graph
# self.for_idx= self.for_idx+1
# renewal the prev information
self.prev_node_idx = curr_node_idx
self.prev_scan_pts = copy.deepcopy(self.curr_scan_pts)
# loop detection and optimize the graph
# if(PGM.curr_node_idx > 1 and PGM.curr_node_idx % self.try_gap_loop_detection == 0):
# # 1/ loop detection
# loop_idx, loop_dist, yaw_diff_deg = SCM.detectLoop()
# if(loop_idx == None): # NOT FOUND
# pass
# else:
# print("Loop event detected: ", PGM.curr_node_idx, loop_idx, loop_dist)
# # 2-1/ add the loop factor
# loop_scan_down_pts = SCM.getPtcloud(loop_idx)
# loop_transform, _, _ = ICP.icp(curr_scan_down_pts, loop_scan_down_pts, init_pose=yawdeg2se3(yaw_diff_deg), max_iterations=20)
# PGM.addLoopFactor(loop_transform, loop_idx)
#
# # 2-2/ graph optimization
# PGM.optimizePoseGraph()
#
# # 2-2/ save optimized poses
# ResultSaver.saveOptimizedPoseGraphResult(PGM.curr_node_idx, PGM.graph_optimized)
# save the ICP odometry pose result (no loop closure)
# ResultSaver.saveUnoptimizedPoseGraphResult(self.curr_se3, curr_node_idx)
self.pose_list = np.vstack(
(self.pose_list, np.reshape(self.curr_se3, (-1, 16))))
if (curr_node_idx % self.save_gap == 0
or curr_node_idx == self.num_frames):
# save odometry-only poses
filename = "pose" + self.sequence_idx + "unoptimized_" + str(
int(time.time())) + ".csv"
filename = os.path.join(self.save_dir, filename)
np.savetxt(filename, self.pose_list, delimiter=",")
# if(self.for_idx % num_frames_to_skip_to_show == 0):
# self.x = self.pose_list[:,3]
# self.y = self.pose_list[:,7]
## z = self.pose_list[:,11]
#
# fig = plt.figure(fig_idx)
# plt.clf()
# plt.plot(-self.y, self.x, color='blue') # kitti camera coord for clarity
# plt.axis('equal')
# plt.xlabel('x', labelpad=10)
# plt.ylabel('y', labelpad=10)
# plt.draw()
# plt.pause(0.01) #is necessary for the plot to update for some reason
# ResultSaver.vizCurrentTrajectory(fig_idx=fig_idx)
self.for_idx = self.for_idx + 1
# writer.grab_frame()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = " UTM_COORDINATE "
msg.pose.pose.position.x = -self.y[curr_node_idx - 4]
msg.pose.pose.position.y = self.x[curr_node_idx - 4]
self.pub.publish(msg)
rate.sleep()
toc = time.time()
def read_LiDAR(self, data):
rate = rospy.Rate(20)
self.num_frames = 6000
# Result saver
self.save_dir = "result/" + self.sequence_idx
if not os.path.exists(self.save_dir): os.makedirs(self.save_dir)
fig_idx = 1
num_frames_to_skip_to_show = 5
# get current information
self.curr_scan_pts = ptstoxyz.pointcloud2_to_xyz_array(
data, remove_nans=True)
print(self.curr_scan_pts.shape)
self.curr_scan_down_pts = Ptutils.random_sampling(
self.curr_scan_pts, num_points=self.num_icp_points)
# save current node
curr_node_idx = self.for_idx # make start with 0
if (curr_node_idx == 0):
self.prev_node_idx = curr_node_idx
self.prev_scan_pts = copy.deepcopy(self.curr_scan_pts)
self.icp_initial = np.eye(4)
# calc odometry
self.prev_scan_down_pts = Ptutils.random_sampling(
self.prev_scan_pts, num_points=self.num_icp_points)
self.odom_transform, _, _ = ICP.icp(self.curr_scan_down_pts,
self.prev_scan_down_pts,
init_pose=self.icp_initial,
max_iterations=20)
self.curr_se3 = np.matmul(self.curr_se3, self.odom_transform)
self.icp_initial = self.odom_transform # assumption: constant velocity model (for better next ICP converges)
# renewal the prev information
self.prev_node_idx = curr_node_idx
self.prev_scan_pts = copy.deepcopy(self.curr_scan_pts)
# save the ICP odometry pose result
self.pose_list = np.vstack(
(self.pose_list, np.reshape(self.curr_se3, (-1, 16))))
if (curr_node_idx % self.save_gap == 0
or curr_node_idx == self.num_frames):
# save odometry-only poses
filename = "pose" + self.sequence_idx + "unoptimized_" + str(
int(time.time())) + ".csv"
filename = os.path.join(self.save_dir, filename)
np.savetxt(filename, self.pose_list, delimiter=",")
if (self.for_idx % num_frames_to_skip_to_show == 0):
self.x = self.pose_list[:, 3]
self.y = self.pose_list[:, 7]
#print graph
fig = plt.figure(fig_idx)
plt.clf()
plt.plot(-self.y, self.x, color='blue')
plt.axis('equal')
plt.xlabel('x', labelpad=10)
plt.ylabel('y', labelpad=10)
plt.draw()
plt.pause(
0.01) #is necessary for the plot to update for some reason
self.for_idx = self.for_idx + 1
#odometry publisher
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = " UTM_COORDINATE "
msg.pose.pose.position.x = -self.y[curr_node_idx - 4]
msg.pose.pose.position.y = self.x[curr_node_idx - 4]
self.pub.publish(msg)
rate.sleep()
def read_LiDAR(self, data):
# sequence_dir = os.path.join(self.data_base_dir, self.sequence_idx, 'velodyne')
# sequence_manager = Ptutils.KittiScanDirManager(sequence_dir)
# scan_paths = sequence_manager.scan_fullpaths
print(self.for_idx)
num_frames = 6000
print('jhloi')
# Pose Graph Manager (for back-end optimization) initialization
PGM = PoseGraphManager()
PGM.addPriorFactor()
# Result saver
save_dir = "result/" + self.sequence_idx
if not os.path.exists(save_dir): os.makedirs(save_dir)
ResultSaver = PoseGraphResultSaver(init_pose=PGM.curr_se3,
save_gap=self.save_gap,
num_frames=num_frames,
seq_idx=self.sequence_idx,
save_dir=save_dir)
# Scan Context Manager (for loop detection) initialization
SCM = ScanContextManager(shape=[self.num_rings, self.num_sectors],
num_candidates=self.num_candidates,
threshold=self.loop_threshold)
# for save the results as a video
fig_idx = 1
fig = plt.figure(fig_idx)
# writer = FFMpegWriter(fps=15)
# video_name = self.sequence_idx + "_" + str(self.num_icp_points) + ".mp4"
num_frames_to_skip_to_show = 5
# num_frames_to_save = np.floor(num_frames/num_frames_to_skip_to_show)
# with writer.saving(fig, video_name, num_frames_to_save): # this video saving part is optional
# @@@ MAIN @@@: data stream
# for for_idx, scan_path in tqdm(enumerate(scan_paths), total=num_frames, mininterval=5.0):
# get current information
self.curr_scan_pts = ptstoxyz.pointcloud2_to_xyz_array(
data, remove_nans=True)
# curr_scan_pts = Ptutils.readScan(scan_paths)
self.curr_scan_down_pts = Ptutils.random_sampling(
self.curr_scan_pts, num_points=self.num_icp_points)
# save current node
PGM.curr_node_idx = self.for_idx # make start with 0
SCM.addNode(node_idx=PGM.curr_node_idx,
ptcloud=self.curr_scan_down_pts)
if (PGM.curr_node_idx == 0):
PGM.prev_node_idx = PGM.curr_node_idx
self.prev_scan_pts = copy.deepcopy(self.curr_scan_pts)
self.icp_initial = np.eye(4)
# continue
# calc odometry
print(self.prev_scan_down_pts)
self.prev_scan_down_pts = Ptutils.random_sampling(
self.prev_scan_pts, num_points=self.num_icp_points)
self.odom_transform, _, _ = ICP.icp(self.curr_scan_down_pts,
self.prev_scan_down_pts,
init_pose=self.icp_initial,
max_iterations=20)
print(self.odom_transform)
# update the current (moved) pose
PGM.curr_se3 = np.matmul(PGM.curr_se3, self.odom_transform)
print(PGM.curr_se3)
self.icp_initial = self.odom_transform # assumption: constant velocity model (for better next ICP converges)
# add the odometry factor to the graph
PGM.addOdometryFactor(self.odom_transform)
self.for_idx = self.for_idx + 1
# renewal the prev information
PGM.prev_node_idx = PGM.curr_node_idx
self.prev_scan_pts = copy.deepcopy(self.curr_scan_pts)
# loop detection and optimize the graph
# if(PGM.curr_node_idx > 1 and PGM.curr_node_idx % self.try_gap_loop_detection == 0):
# # 1/ loop detection
# loop_idx, loop_dist, yaw_diff_deg = SCM.detectLoop()
# if(loop_idx == None): # NOT FOUND
# pass
# else:
# print("Loop event detected: ", PGM.curr_node_idx, loop_idx, loop_dist)
# # 2-1/ add the loop factor
# loop_scan_down_pts = SCM.getPtcloud(loop_idx)
# loop_transform, _, _ = ICP.icp(curr_scan_down_pts, loop_scan_down_pts, init_pose=yawdeg2se3(yaw_diff_deg), max_iterations=20)
# PGM.addLoopFactor(loop_transform, loop_idx)
#
# # 2-2/ graph optimization
# PGM.optimizePoseGraph()
#
# # 2-2/ save optimized poses
# ResultSaver.saveOptimizedPoseGraphResult(PGM.curr_node_idx, PGM.graph_optimized)
# save the ICP odometry pose result (no loop closure)
ResultSaver.saveUnoptimizedPoseGraphResult(PGM.curr_se3,
PGM.curr_node_idx)
if (self.for_idx % num_frames_to_skip_to_show == 0):
ResultSaver.vizCurrentTrajectory(fig_idx=fig_idx)
self.for_idx = self.for_idx + 1