Loop Closure And Map Saving

[Omar Nour]

You have done a really amazing job regarding map quality!

But I would like to know if there is a loop closure module as from my tests, it seems that there is no loop closure.

For the map, is there a way to save a generated map in pcd? Also, can i extract the path to be used with evo evaluation tool?

[nlog2]

Thanks for your attention. 😄

But I would like to know if there is a loop closure module as from my tests, it seems that there is no loop closure.

Traj-LO is a state estimator without a loop closure module. Since we maintain a local map with a fixed radius, it can be regarded as having implicit loop constraints. Of course, drift in long-term estimation is inevitable; we may plan to add a pose graph module in the future.

For the map, is there a way to save a generated map in pcd? Also, can i extract the path to be used with evo evaluation tool?

Yes, you can save the map. The map saving process is similar to transferring points to the visualizer, so you can use the following code as a reference. It's important to note that measure->points are the raw points, while visData->data are the undistorted points, with the reference coordinates being visData->T_w = pp.first. Just concatenate all undistorted points from each scan to generate a global map.

Traj-LO/src/core/odometry.cpp

Lines 131 to 142 in dc7d2c8

 // map & trajectory visualization 
 if (vis_data_queue && 
     ((frame_id / config_.seg_num) % (config_.frame_num) == 0)) { 
   ScanVisData::Ptr visData(new ScanVisData); 
  
   posePair pp{frame_poses_[tp.first].getPose(), 
               frame_poses_[tp.second].getPose()}; 
   UndistortRawPoints(measure->points, visData->data, pp); 
  
   visData->T_w = pp.first; 
   vis_data_queue->push(visData);  // may block the thread 
 } 

As for the estimated trajectory, it is already stored in trajectory_, which is a pair of timestamp and its corresponding pose. You can save them to a TXT file when the odometry is finished.

Traj-LO/src/core/odometry.cpp

Lines 150 to 158 in dc7d2c8

 // save pose in window 
 for (const auto& kv : frame_poses_) { 
   trajectory_.emplace_back(kv.first, kv.second.getPose()); 
 } 
  
 // Here, you can save the trajectory for comparision 
  
 isFinish = true; 
 std::cout << "Finisher LiDAR Odometry " << std::endl; 

We will update these functions in a future commit.