void getTestDataFromBag(PointCloudPtr cloud_source, PointCloudPtr cloud_target, PointCloudPtr featureCloudSource, std::vector<int> &indicesSource, PointCloudPtr featureCloudTarget, std::vector<int> &indicesTarget, Eigen::Matrix4f &initialTransform, int rosMessageNumber) { sensor_msgs::PointCloud2::Ptr cloud_source_filtered (new sensor_msgs::PointCloud2 ()); sensor_msgs::PointCloud2::Ptr cloud_target_filtered (new sensor_msgs::PointCloud2 ()); PointCloudPtr cloud_ransac_estimation (new PointCloud); pcl::PCDWriter writer; std::stringstream filename; rosbag::Bag bag; bag.open("/media/burg/data/bagfiles/bench1-ManySweeps4-lowfeatureThresNode2.bag", rosbag::bagmode::Read); std::vector<std::string> topics; topics.push_back(std::string("/feature_match_out_topic")); rosbag::View view(bag, rosbag::TopicQuery(topics)); int i = 1; BOOST_FOREACH(rosbag::MessageInstance const m, view) { if(( i == rosMessageNumber) || (rosMessageNumber==0)) { pcl_tutorial::featureMatch::ConstPtr fm = m.instantiate<pcl_tutorial::featureMatch>(); ROS_INFO("Converting point cloud message to local pcl clouds"); sensor_msgs::PointCloud2::Ptr cloud_source_temp_Ptr (new sensor_msgs::PointCloud2 (fm->sourcePointcloud)); sensor_msgs::PointCloud2::Ptr cloud_target_temp_Ptr (new sensor_msgs::PointCloud2 (fm->targetPointcloud)); voxFilterPointCloud(cloud_source_temp_Ptr, cloud_source_filtered); voxFilterPointCloud(cloud_target_temp_Ptr, cloud_target_filtered); ROS_INFO("Converting dense clouds"); pcl::fromROSMsg(*cloud_source_filtered, *cloud_source); pcl::fromROSMsg(*cloud_target_filtered, *cloud_target); ROS_INFO("Converting sparse clouds"); pcl::fromROSMsg(fm->sourceFeatureLocations, *featureCloudSource); pcl::fromROSMsg(fm->targetFeatureLocations, *featureCloudTarget); ROS_INFO("Converting geometry message to eigen4f"); tf::Transform trans; tf::transformMsgToTF(fm->featureTransform,trans); initialTransform = EigenfromTf(trans); ROS_INFO_STREAM("transform from ransac: " << "\n" << initialTransform << "\n"); ROS_INFO("Extracting corresponding indices"); //int j = 1; indicesSource.clear(); indicesTarget.clear(); for(std::vector<pcl_tutorial::match>::const_iterator iterator_ = fm->matches.begin(); iterator_ != fm->matches.end(); ++iterator_) { indicesSource.push_back(iterator_->trainId); indicesTarget.push_back(iterator_->queryId); // ROS_INFO_STREAM("source point " << j << ": " << featureCloudSource->points[iterator_->queryId].x << ", " << featureCloudSource->points[iterator_->queryId].y << ", " << featureCloudSource->points[iterator_->queryId].z); // ROS_INFO_STREAM("target point " << j++ << ": " << featureCloudTarget->points[iterator_->trainId].x << ", " << featureCloudTarget->points[iterator_->trainId].y << ", " << featureCloudTarget->points[iterator_->trainId].z); // ROS_INFO("qidx: %d tidx: %d iidx: %d dist: %f", iterator_->queryId, iterator_->trainId, iterator_->imgId, iterator_->distance); } /*for (size_t cloudId = 0; cloudId < featureCloudSource->points.size (); ++cloudId) { ROS_INFO_STREAM("feature cloud: " << cloudId << ": " << featureCloudSource->points[cloudId].x << "; " << featureCloudSource->points[cloudId].y << "; " << featureCloudSource->points[cloudId].z); }*/ Eigen::Matrix4f ransacInverse = initialTransform.inverse(); transformPointCloud (*cloud_source, *cloud_ransac_estimation, ransacInverse); ROS_INFO("Writing point clouds"); filename << "cloud" << i << "DenseSource.pcd"; writer.write (filename.str(), *cloud_source, false); filename.str(""); filename << "cloud" << i << "DenseTarget.pcd"; writer.write (filename.str(), *cloud_target, true); filename.str(""); filename << "cloud" << i << "RANSAC-" << (int)indicesSource.size() << "-inliers.pcd"; writer.write (filename.str(), *cloud_ransac_estimation, true); filename.str(""); filename << "cloud" << i << "SparseSource.pcd"; writer.write (filename.str(), *featureCloudSource, false); filename.str(""); filename << "cloud" << i << "SparseTarget.pcd"; writer.write (filename.str(), *featureCloudTarget, false); filename.str(""); i++; // for(std::vector<int>::iterator iterator_ = indicesSource.begin(); iterator_ != indicesSource.end(); ++iterator_) { // ROS_INFO("source indice: %d", *iterator_); // } } else i++; } bag.close(); }
int main( int argc, char** argv ) { ros::init(argc, argv, "KITTI"); int offset = 0; int maxId = 4000; KITTI kitti(0,maxId,offset); loam_wrapper loam; std::vector<Eigen::Matrix4d> T_result; std::vector<Eigen::Matrix4d> T_result_delta; T_result.push_back(Eigen::Matrix4d::Identity()); Eigen::Matrix4d T_offset = kitti.getGroundTruthByID(offset); Eigen::Matrix4d T_all_od = kitti.getGroundTruthByID(offset); Eigen::Matrix4d T_all_map = kitti.getGroundTruthByID(offset); Eigen::Matrix4d T_diff_od_map; Eigen::Matrix4d T_Velo_to_Cam = kitti.getVelo_to_cam_T(); for (int i=offset; i<maxId-1;i++) { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_filtered (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target_filtered (new pcl::PointCloud<pcl::PointXYZ>); // Get Input Clouds pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source(new pcl::PointCloud<pcl::PointXYZ>()); kitti.getOneVel(cloud_source_filtered,i); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>()); kitti.getOneVel(cloud_target_filtered,i+1); pcl::VoxelGrid<pcl::PointXYZ> downSizeFilter; downSizeFilter.setInputCloud(cloud_source_filtered); downSizeFilter.setLeafSize(0.1f, 0.1f, 0.1f); downSizeFilter.filter(*cloud_source); pcl::VoxelGrid<pcl::PointXYZ> downSizeFilter2; downSizeFilter2.setInputCloud(cloud_target_filtered); downSizeFilter2.setLeafSize(0.1f, 0.1f, 0.1f); downSizeFilter2.filter(*cloud_target); // Transform into Cam coordinate system pcl::transformPointCloud(*cloud_source, *cloud_source, T_Velo_to_Cam); pcl::transformPointCloud(*cloud_target, *cloud_target, T_Velo_to_Cam); // transform according to ground truth Eigen::Matrix4d T_1 = kitti.getGroundTruthByID(i); Eigen::Matrix4d T_2 = kitti.getGroundTruthByID(i+1); Eigen::Matrix4d T_diff = T_1.inverse() * T_2; pcl::transformPointCloud (*cloud_target, *cloud_target, T_diff); loam.publishFirst(cloud_source); loam.publishSecond(cloud_target); pcl::PointCloud<pcl::PointNormal>::Ptr src(new pcl::PointCloud<pcl::PointNormal>); pcl::copyPointCloud(*cloud_source, *src); pcl::PointCloud<pcl::PointNormal>::Ptr tgt(new pcl::PointCloud<pcl::PointNormal>); pcl::copyPointCloud(*cloud_target, *tgt); pcl::NormalEstimation<pcl::PointNormal, pcl::PointNormal> norm_est; norm_est.setSearchMethod (pcl::search::KdTree<pcl::PointNormal>::Ptr (new pcl::search::KdTree<pcl::PointNormal>)); norm_est.setKSearch (10); norm_est.setInputCloud (tgt); norm_est.compute (*tgt); pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp; typedef pcl::registration::TransformationEstimationPointToPlane<pcl::PointNormal, pcl::PointNormal> PointToPlane; boost::shared_ptr<PointToPlane> point_to_plane(new PointToPlane); icp.setTransformationEstimation(point_to_plane); icp.setInputCloud(src); icp.setInputTarget(tgt); icp.setRANSACOutlierRejectionThreshold(0.05); icp.setRANSACIterations(100); icp.setMaximumIterations(1000); icp.setTransformationEpsilon(1e-3); pcl::PointCloud<pcl::PointNormal> output; icp.align(output); // Obtain the transformation that aligned cloud_source to cloud_source_registered Eigen::Matrix4d T_back= icp.getFinalTransformation().cast<double>(); T_all_map = T_back * T_all_map * T_diff; Eigen::Matrix4d T_gt = kitti.getGroundTruthDeltaByID(i); T_result_delta.push_back(T_back); T_all_od = T_offset*loam.T_total_od; std::cout << "[INFO]: i=" << i << std::endl; std::cout << "T_back:" << T_back << std::endl; std::cout << "T_all_od:" << T_all_od << std::endl; std::cout << "T_all_map:" << T_all_map << std::endl; std::cout << "T_gt:" << kitti.getGroundTruthByID(i) << std::endl; std::cout << "T_diff_od_map:" << T_diff_od_map << std::endl; T_result.push_back(T_all_map); loam.publishFirst(cloud_source); pcl::transformPointCloud (*cloud_target, *cloud_target, T_back); loam.publishSecond(cloud_target); } kitti.plotDeltaPoses(T_result,0); kitti.eval(T_result); return 0; }