undistort points

[narutojxl]

Hi doctor jiao @gogojjh,
Thanks for your excellent work and sharing to the community ! I download the smaller bag RHD to test.

• Q1: When we undistort points, is it a typo pose_undist[IDX_REF], it seems that pose_undist[n] ?
  In config yaml file, IDX_REF default value is 0. So the pose_undist[0] is laser0 from k frame to k+1 frame delta transform, pose_undist[1] is the laser1 sensor.
  

  M-LOAM/estimator/src/estimator/estimator.cpp

  Lines 403 to 405 in 2276310

  	for (PointI &point : cur_feature_.second[n]["corner_points_less_sharp"]) TransformToEnd(point, point, pose_undist[IDX_REF], true, SCAN_PERIOD);
  	for (PointI &point : cur_feature_.second[n]["surf_points_less_flat"]) TransformToEnd(point, point, pose_undist[IDX_REF], true, SCAN_PERIOD);
  	for (PointI &point : cur_feature_.second[n]["laser_cloud"]) TransformToEnd(point, point, pose_undist[IDX_REF], true, SCAN_PERIOD);

• Q2: When we assign cur_feature_ to the prev_feature_, where cur_feature_ is the raw current feature points, not be deskewed. I found in the Estimator::undistortMeasurements() function, we evently convert current feature points into current end instant according to the estimated relative transform from k to k+1 frame. If we after this moment assign cur_feature_ to the prev_feature_, the prev_feature_ is current feature points in current end instant. Then for next frame loop, we convert current feature points into current start instant using the linearly and slerp interpolation of last estimated delta transform, accordint to every point's time ratio,

feature_extract.hpp#L154 && #292: TransformToStart(cloud_data.points[i], point_sel, pose_local, false, SCAN_PERIOD);

it seems make sense as prev_feature kd-tree points and the cur_feature_ points in the same frame. I'm not sure i correctly understand code totally.

M-LOAM/estimator/src/estimator/estimator.cpp

Lines 532 to 543 in 2276310

	// pass cur_feature to prev_feature
	prev_time_ = cur_time_;
	prev_feature_.first = prev_time_;
	prev_feature_.second.clear();
	prev_feature_.second.resize(NUM_OF_LASER);
	for (size_t n = 0; n < NUM_OF_LASER; n++)
	{
	prev_feature_.second[n].insert(make_pair("corner_points_less_sharp",
	cur_feature_.second[n].find("corner_points_less_sharp")->second));
	prev_feature_.second[n].insert(make_pair("surf_points_less_flat",
	cur_feature_.second[n].find("surf_points_less_flat")->second));
	}

M-LOAM/estimator/src/lidarTracker/lidar_tracker.cpp

Lines 58 to 59 in 2276310

	f_extract_.matchCornerFromScan(kdtree_corner_last, *corner_points_last, *corner_points_sharp, pose_local, corner_scan_features);
	f_extract_.matchSurfFromScan(kdtree_surf_last, *surf_points_last, *surf_points_flat, pose_local, surf_scan_features);

• Q3: In the above, TransformToStart(cloud_data.points[i], point_sel, pose_local, false, SCAN_PERIOD); feature_extract.hpp#L292, why we call TransformToStart() with b_distortion = false. According to my understanding, i think we should call with b_distortion = true, because current feature points is raw points, not be deskewed. I don't understand this code.

• BTW: should we comment this line?
  

  M-LOAM/estimator/src/rosNodeRHD.cpp

  Line 213 in 2276310

  for (size_t i = NUM_OF_LASER; i < 2; i++) sub_lidar[i] = new LidarSubType(nh, CLOUD_TOPIC[0], 1);

If i don't describle my problem clearly, please tell me, i will add more info. Look forward to discuss with you, thanks for your help and time in advance!

Best regards
jxl

[gogojjh]

Q1: should be n, I have fixed this issue, but not tested...

Q2: In our design, the deskewed points are only used in lidar mapping, and not fed to the odometry.

Q3: setting b_distortion=true may not get good results .... so we set b_distortion=false. May two reasons make the results worse: 1. since the timestamp is given to each point according to its relative horizontal angle to the starting angle, this assumption may be not always correct; 2. the undistortMeasurements() may exist some bugs that we haven't figured out ...

Q4: you cannot comment this line. When NUM_OF_LASER=1, this line can give a valid pointer to sublidar[1]. This can prevent the segmentation fault of line 216.

[narutojxl]

Hi doctro jiao, @gogojjh
Thanks for your detailed and kind reply, my problems are solved, thanks again!