reduce_measurement_g2o::reduce_measurement_g2o(reduce_measurement_g2o& rb,
tbb::split) :
size(rb.size), frames(rb.frames) {
init_feature_detector();
icp.setMaxCorrespondenceDistance(0.5);
boost::shared_ptr<PointToPlane> point_to_plane(new PointToPlane);
icp.setTransformationEstimation(point_to_plane);
}
reduce_measurement_g2o::reduce_measurement_g2o(
const tbb::concurrent_vector<color_keyframe::Ptr> & frames, int size) :
size(size), frames(frames) {
init_feature_detector();
icp.setMaxCorrespondenceDistance(0.5);
boost::shared_ptr<PointToPlane> point_to_plane(new PointToPlane);
icp.setTransformationEstimation(point_to_plane);
}
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;
}
bool RelativePoseEstimatorICPWithNormals<PointT> ::
computeRegistration(Pose3D& relative_pose,
PointCloudConstPtr source_cloud,
PointCloudConstPtr target_cloud,
PointCloudType& aligned_cloud)
{
#ifndef HAVE_PCL_GREATER_THAN_1_2_0
return super::computeRegistration(relative_pose, source_cloud, target_cloud, aligned_cloud);
#else
pcl::IterativeClosestPoint<PointT, PointT> reg;
typedef pcl::registration::TransformationEstimationPointToPlaneLLS<PointT, PointT> PointToPlane;
boost::shared_ptr<PointToPlane> point_to_plane (new PointToPlane);
reg.setTransformationEstimation (point_to_plane);
// typedef TransformationEstimationRGBD<PointT, PointT> TransformRGBD;
// boost::shared_ptr<TransformRGBD> transform_rgbd (new TransformRGBD);
// reg.setTransformationEstimation (transform_rgbd);
#ifdef HAVE_PCL_GREATER_THAN_1_6_0 // rejectors are not well supported before 1.7
boost::shared_ptr<pcl::registration::CorrespondenceRejectorDistance> rejector_distance (new pcl::registration::CorrespondenceRejectorDistance);
rejector_distance->setInputSource<PointT>(source_cloud);
rejector_distance->setInputTarget<PointT>(target_cloud);
rejector_distance->setMaximumDistance(m_distance_threshold);
reg.addCorrespondenceRejector(rejector_distance);
boost::shared_ptr<pcl::registration::CorrespondenceRejectorSurfaceNormal> rejector_normal (new pcl::registration::CorrespondenceRejectorSurfaceNormal);
rejector_normal->setThreshold(cos(M_PI/4.f));
rejector_normal->initializeDataContainer<PointT, PointT>();
rejector_normal->setInputSource<PointT>(source_cloud);
rejector_normal->setInputTarget<PointT>(target_cloud);
rejector_normal->setInputNormals<PointT, PointT>(source_cloud);
rejector_normal->setTargetNormals<PointT, PointT>(target_cloud);
reg.addCorrespondenceRejector(rejector_normal);
boost::shared_ptr<pcl::registration::CorrespondenceRejectorOneToOne> rejector_one_to_one (new pcl::registration::CorrespondenceRejectorOneToOne);
reg.addCorrespondenceRejector(rejector_one_to_one);
typedef pcl::registration::CorrespondenceRejectorSampleConsensus<PointT> RejectorConsensusT;
boost::shared_ptr<RejectorConsensusT> rejector_ransac (new RejectorConsensusT());
rejector_ransac->setInputSource(source_cloud);
rejector_ransac->setInputTarget(target_cloud);
rejector_ransac->setInlierThreshold(m_ransac_outlier_threshold);
rejector_ransac->setMaxIterations(100);
reg.addCorrespondenceRejector(rejector_ransac);
boost::shared_ptr<pcl::registration::CorrespondenceRejectorVarTrimmed> rejector_var_trimmed (new pcl::registration::CorrespondenceRejectorVarTrimmed());
rejector_var_trimmed->setInputSource<PointT>(source_cloud);
rejector_var_trimmed->setInputTarget<PointT>(target_cloud);
rejector_var_trimmed->setMinRatio(0.1f);
rejector_var_trimmed->setMaxRatio(0.75f);
reg.addCorrespondenceRejector(rejector_var_trimmed);
boost::shared_ptr<pcl::registration::CorrespondenceRejectorTrimmed> rejector_trimmed (new pcl::registration::CorrespondenceRejectorTrimmed());
rejector_trimmed->setMinCorrespondences(static_cast<int>(0.1f * source_cloud->size()));
rejector_trimmed->setOverlapRatio(0.5f);
reg.addCorrespondenceRejector(rejector_trimmed);
#endif
#if 0
ntk::Mesh target_mesh;
pointCloudToMesh(target_mesh, *target_cloud);
target_mesh.saveToPlyFile("debug_target.ply");
ntk::Mesh source_mesh;
pointCloudToMesh(source_mesh, *source_cloud);
source_mesh.saveToPlyFile("debug_source.ply");
#endif
reg.setMaximumIterations (m_max_iterations);
reg.setTransformationEpsilon (1e-10);
reg.setMaxCorrespondenceDistance (m_distance_threshold);
reg.setRANSACOutlierRejectionThreshold(m_ransac_outlier_threshold);
#ifdef HAVE_PCL_GREATER_THAN_1_6_0
reg.setInputSource (source_cloud);
#else
reg.setInputCloud (source_cloud);
#endif
reg.setInputTarget (target_cloud);
reg.align (aligned_cloud);
if (!reg.hasConverged())
{
ntk_dbg(1) << "ICP did not converge, ignoring.";
return false;
}
if (0)
{
ntk::Mesh mesh1, mesh2;
pointCloudToMesh(mesh1, aligned_cloud);
pointCloudToMesh(mesh2, *target_cloud);
mesh1.saveToPlyFile("debug_icp_1.ply");
mesh2.saveToPlyFile("debug_icp_2.ply");
}
#if 0
ntk::Mesh debug_mesh;
pointCloudToMesh(debug_mesh, aligned_cloud);
debug_mesh.saveToPlyFile("debug_aligned.ply");
#endif
ntk_dbg_print(reg.getFitnessScore(), 1);
Eigen::Matrix4f t = reg.getFinalTransformation ();
cv::Mat1f T(4,4);
//toOpencv(t,T);
for (int r = 0; r < 4; ++r)
for (int c = 0; c < 4; ++c)
T(r,c) = t(r,c);
relative_pose.setCameraTransform(T);
#endif
return true;
}
