Problem with map alignment by exporting pose estimations

[ASajwan]

Hi,

First, thanks for making this code public !

I run the code through a log and I see a crisp map being generated in rviz.

Now, for the same log, I am trying to export the pose estimation into a txt file to then offline use a C++ program outside of ROS to stitch together all the point clouds. But, I see that my point clouds don't align well when I stitch them after transforming them by the motion estimates.

I am exporting transformAftMapped (to get EulerAngles) from publishTF() in mapOptimization.cpp using: odomTimeFile << odomAftMapped.header.stamp << "\n"; odomFile << odomAftMapped.pose.pose.position.z << "," // x pose << odomAftMapped.pose.pose.position.x << "," // y pose << odomAftMapped.pose.pose.position.y << "," // z pose << transformAftMapped[2] << "," // roll << transformAftMapped[0] << "," //pitch << transformAftMapped[1] << "\n"; // yaw since I saw that the incoming cloud in adjustDistortion() in featureAssociation.cpp is also like the following in the code: point.x = segmentedCloud->points[i].y; point.y = segmentedCloud->points[i].z; point.z = segmentedCloud->points[i].x; I am saving all point clouds in pcd files by adding the following lines in copyPointCloud() in imageProjection.cpp: pcl::io::savePCDFileASCII(, *laserCloudIn); pcdTimeFile << cloudHeader.stamp << "\n";

So, in total I have 3 things:

1. pcd folder : with all point cloud files in .pcd
2. odomFile : with nx6 matrix of poses in CSV
3. odomTimeFile and pcdTimeFile : containing the respective timestamps to find the correspondences between transforms/pose and point clouds. I use PCL transform function as well as transformPointCloud() provided in mapOptimization.cpp in my patch-the-point-clouds code.

I am aware that there could be two problems here:

1. Coordinate systems - I am finding them confusing here.
2. Timestamps but, I don't know where exactly the problem is.

Can you please help me out here? Any help is much appreciated !

[ASajwan]

Figured it out. Thank you :)

[TixiaoShan]

Glad you figured out! For those who might need this, line 754 to 759 in mapOptmization should be the right place for exporting the point cloud.

[ASajwan]

Sure Tixiao. Since I needed intensity information too, I used raw point clouds and exported just the laser odometry estimation from LeGO-LOAM using laserOdometry2 (lines 202-210 in transformFusion.cpp). It works.