save PCD not working
sai2602 opened this issue · 4 comments
Hi @JokerJohn,
Firstly, Thank you so much for the work. I am currently working with LIO-SAM for a use case where my robot is indoors (where there is no GNSS data) and I want to save the estimated pose in TUM format which then goes as an input to one of my 3D object detection network. In the datasaver, I see that this is already an option to save the optimized pose to text file in TUM format. The mapOptimizationGps.cpp instantiates the dataSaver and creates the subdirectory to save the text file, but, even after my run ends (I have a bag file), the subdirectory did not have the text file. I then saw that the "saveMapService" where the actual saving happens wasn't called anywhere and so I instantiated that too. I still don't see the text file being saved. Could you please help m with this?
After you play your rosbag, just use the command rosservice call /lio_sam_6axis/save_map which will call the saveMapService.
Hi @JokerJohn,
Firstly, Thank you so much for the work. I am currently working with LIO-SAM for a use case where my robot is indoors (where there is no GNSS data) and I want to save the estimated pose in TUM format which then goes as an input to one of my 3D object detection network. In the datasaver, I see that this is already an option to save the optimized pose to text file in TUM format. The mapOptimizationGps.cpp instantiates the dataSaver and creates the subdirectory to save the text file, but, even after my run ends (I have a bag file), the subdirectory did not have the text file. I then saw that the "saveMapService" where the actual saving happens wasn't called anywhere and so I instantiated that too. I still don't see the text file being saved. Could you please help m with this?
After you play your rosbag, just use the command rosservice call /lio_sam_6axis/save_map which will call the saveMapService.
Hi @JokerJohn ,
Thanks for the update. It still doesn't write the poses to the file. There is just one entry in the file and the statement "Consistent running" doesn't get printed. So, I guess the service is getting terminated after the first entry.
Just an update. I have now created a standalone API that always writes the odom (extracted from dataSaver.cpp). This is working for me now, but it would be great to know why the rosservice doesn't work. Getting this up and running would be useful for me since I also would at a later stage need the other options like saving the graph, map etc.
Just an update. I have now created a standalone API that always writes the odom (extracted from dataSaver.cpp). This is working for me now, but it would be great to know why the rosservice doesn't work. Getting this up and running would be useful for me since I also would at a later stage need the other options like saving the graph, map etc.
Ensure that you execute the rosservice command after playing the bag package. If the save operation still fails, I speculate it might be due to the keyframe lock not successfully locking the corresponding code segment (mtx.lock()). As saving the point cloud is very time-consuming, I suggest minimizing the code segment between mtx.lock() and mtx.unlock(). You can prioritize saving the trajectory and save the point cloud map at the end. The updated code can be as follows:
mtx.lock();
for (int i = 0; i < cloudKeyPoses6D->size(); ++i) {
PointTypePose p = cloudKeyPoses6D->at(i);
// keyframeTimes.push_back(p.time);
// Eigen::Translation3d tf_trans(p.x, p.y, p.z);
// Eigen::AngleAxisd rot_x(p.roll, Eigen::Vector3d::UnitX());
// Eigen::AngleAxisd rot_y(p.pitch, Eigen::Vector3d::UnitY());
// Eigen::AngleAxisd rot_z(p.yaw, Eigen::Vector3d::UnitZ());
// Eigen::Matrix4d trans = (tf_trans * rot_z * rot_y *
// rot_x).matrix(); keyframePosestrans.push_back(trans);
nav_msgs::Odometry laserOdometryROS;
laserOdometryROS.header.stamp = ros::Time().fromSec(p.time);
laserOdometryROS.header.frame_id = odometryFrame;
laserOdometryROS.child_frame_id = lidarFrame;
laserOdometryROS.pose.pose.position.x = p.x;
laserOdometryROS.pose.pose.position.y = p.y;
laserOdometryROS.pose.pose.position.z = p.z;
laserOdometryROS.pose.pose.orientation =
tf::createQuaternionMsgFromRollPitchYaw(p.roll, p.pitch, p.yaw);
keyframePosesOdom.push_back(laserOdometryROS);
// temp_ptr->clear();
// *temp_ptr += *surfCloudKeyFrames.at(i);
// *temp_ptr += *cornerCloudKeyFrames.at(i);
// keyframePc.push_back(temp_ptr);
geometry_msgs::TransformStamped transform_msg;
transform_msg.header.stamp = ros::Time().fromSec(p.time);
transform_msg.header.frame_id = odometryFrame;
transform_msg.child_frame_id = lidarFrame;
transform_msg.transform.translation.x = p.x;
transform_msg.transform.translation.y = p.y;
transform_msg.transform.translation.z = p.z;
transform_msg.transform.rotation =
tf::createQuaternionMsgFromRollPitchYaw(p.roll, p.pitch, p.yaw);
transform_vec.push_back(transform_msg);
if (useGPS) {
// we save optimized origin gps point, maybe the altitude value need to
// be fixes
Eigen::Vector3d first_point(p.x, p.y, p.z);
Eigen::Vector3d lla_point, ecef_point;
ecef_point = gpsTools.ENU2ECEF(first_point);
lla_point = gpsTools.ECEF2LLA(ecef_point);
lla_vec.push_back(lla_point);
}
}
mtx.unlock();
dataSaverPtr->saveTimes(keyframeTimes);
dataSaverPtr->saveGraphGtsam(gtSAMgraph, isam, isamCurrentEstimate);
dataSaverPtr->saveOptimizedVerticesTUM(isamCurrentEstimate);
//dataSaverPtr->saveOptimizedVerticesKITTI(isamCurrentEstimate);
dataSaverPtr->saveOdometryVerticesTUM(keyframeRawOdom);
// dataSaverPtr->saveResultBag(keyframePosesOdom, keyframeCloudDeskewed, transform_vec);
if (useGPS) dataSaverPtr->saveKMLTrajectory(lla_vec);
std::cout << "Times, isam, raw_odom, pose_odom, pose3D, pose6D: "
<< keyframeTimes.size() << " " << isamCurrentEstimate.size()
<< ", " << keyframeRawOdom.size() << " "
<< keyframePosesOdom.size() << " " << cloudKeyPoses3D->size()
<< " " << cloudKeyPoses6D->size() << std::endl;
std::cout << "key_cloud, surf, corner, raw_frame size: "
<< keyframePc.size() << " " << surfCloudKeyFrames.size() << " "
<< cornerCloudKeyFrames.size() << " "
<< laserCloudRawKeyFrames.size() << std::endl;
pcl::PointCloud<PointType>::Ptr globalCornerCloud(
new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalCornerCloudDS(
new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalSurfCloud(
new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalSurfCloudDS(
new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalRawCloud(
new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalRawCloudDS(
new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr globalMapCloud(
new pcl::PointCloud<PointType>());
for (int i = 0; i < (int) cloudKeyPoses3D->size(); i++) {
*globalCornerCloud += *transformPointCloud(cornerCloudKeyFrames[i],
&cloudKeyPoses6D->points[i]);
*globalSurfCloud += *transformPointCloud(surfCloudKeyFrames[i],
&cloudKeyPoses6D->points[i]);
// *globalRawCloud += *transformPointCloud(laserCloudRawKeyFrames[i],
// &cloudKeyPoses6D->points[i]);
cout << "\r" << std::flush << "Processing feature cloud " << i << " of "
<< cloudKeyPoses6D->size() << " ..." << std::endl;
}
downSizeFilterCorner.setInputCloud(globalCornerCloud);
downSizeFilterCorner.setLeafSize(globalMapLeafSize, globalMapLeafSize,
globalMapLeafSize);
downSizeFilterCorner.filter(*globalCornerCloudDS);
// down-sample and save surf cloud
downSizeFilterSurf.setInputCloud(globalSurfCloud);
downSizeFilterSurf.setLeafSize(globalMapLeafSize, globalMapLeafSize,
globalMapLeafSize);
downSizeFilterSurf.filter(*globalSurfCloudDS);
// save global point cloud map
*globalMapCloud += *globalCornerCloudDS;
*globalMapCloud += *globalSurfCloudDS;
// *globalMapCloud += *globalRawCloud;
// downSizeFilterSurf.setInputCloud(globalRawCloud);
// downSizeFilterSurf.setLeafSize(globalMapLeafSize, globalMapLeafSize,
// globalMapLeafSize);
// downSizeFilterSurf.filter(*globalRawCloudDS);
std::cout << "global map size: " << globalMapCloud->size() << " " << globalRawCloudDS->size() << std::endl;
dataSaverPtr->savePointCloudMap(*globalMapCloud);
// dataSaverPtr->savePointCloudMap(keyframePosesOdom,
// laserCloudRawKeyFrames);
cout << "****************************************************" << endl;
cout << "Saving map to pcd files completed: " << endl;
Unfortunately, I currently do not have sufficient time to update this repo. I will fix the bug as soon as I get a chance.