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;
}
