/**
* Register cloud to the octomap
*/
bool srs_env_model::CPcToOcRegistration::registerCloud( tPointCloudPtr & cloud, const SMapWithParameters & map )
{
if( !m_registration.isRegistering() )
{
ROS_ERROR( "No registration method selected." );
return false;
}
// std::cerr << "Reg start" << std::endl;
// Get patch
m_patchMaker.setCloudFrameId( map.frameId );
m_patchMaker.computeCloud( map, cloud->header.stamp );
if( m_patchMaker.getCloud().size() == 0 )
return false;
// Resample input cloud
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2 ());
pcl::toROSMsg( *cloud, *cloudMsg);
// std::cerr << "Patch size: " << m_patchMaker.getCloud().size() << ", " << cloudMsg->data.size() << std::endl;
m_gridFilter.setInputCloud( cloudMsg );
m_gridFilter.setLeafSize( map.resolution, map.resolution, map.resolution );
m_gridFilter.filter( *m_resampledCloud );
// std::cerr << "Voxel grid size: " << m_resampledCloud->data.size() << std::endl;
// Try to register cloud
tPointCloudPtr cloudSource( new tPointCloud() );
tPointCloudPtr cloudBuffer( new tPointCloud() );
tPointCloudPtr cloudTarget( new tPointCloud() );
pcl::fromROSMsg( *m_resampledCloud, *cloudSource );
pcl::copyPointCloud( m_patchMaker.getCloud(), *cloudTarget );
// std::cerr << "Calling registration: " << cloudSource->size() << ", " << cloudTarget->size() << std::endl;
bool rv( m_registration.process( cloudSource, cloudTarget, cloudBuffer ) );
if( ! rv )
std::cerr << "Registration failed" << std::endl;
return rv;
}
int main(int argc, char** argv) {
if (argc != 3) {
std::cerr << "please provide 2 point clouds as arguments: <source>.pcd <target>.pcd)" << std::endl;
exit(0);
}
PointCloud::Ptr cloudSource(new PointCloud);
PointCloud::Ptr cloudOut(new PointCloud);
PointCloud::Ptr cloudTarget(new PointCloud);
PointCloud::Ptr cloudSourceFiltered(new PointCloud);
PointCloud::Ptr cloudTargetFiltered(new PointCloud);
PointCloudNormal::Ptr cloudSourceNormal(new PointCloudNormal);
PointCloudNormal::Ptr cloudTargetNormal(new PointCloudNormal);
PointCloudNormal::Ptr cloudHandlesSource(new PointCloudNormal);
PointCloudNormal::Ptr cloudHandlesTarget(new PointCloudNormal);
PointCloudNormal::Ptr cloudSourceNormalNoNaNs(new PointCloudNormal);
PointCloudNormal::Ptr cloudTargetNormalNoNaNs(new PointCloudNormal);
pcl::PCDWriter writer;
std::vector<int> indicesSource, indicesTarget;
//Fill in the cloud data
ROS_INFO("Reading files....");
pcl::PCDReader reader;
reader.read(argv[1], *cloudSource);
reader.read(argv[2], *cloudTarget);
//pcl::copyPointCloud (*cloudSourceNormal, *cloudSource);
//pcl::copyPointCloud (*cloudTargetNormal, *cloudTarget);
normalEstimation(cloudSource, cloudSourceNormal, 0.03);
normalEstimation(cloudTarget, cloudTargetNormal, 0.03);
std::vector<int> sourceHandleClusters;
std::vector<int> targetHandleClusters;
ROS_INFO("Extracting handles....");
extractHandles(cloudSource, cloudSourceFiltered, cloudSourceNormal, sourceHandleClusters);
extractHandles(cloudTarget, cloudTargetFiltered, cloudTargetNormal, targetHandleClusters);
std::vector<int> removedPoints;
removeNaNs(cloudSourceNormal, cloudSourceNormalNoNaNs, removedPoints);
adjustIndicesFromRemovedPoints(sourceHandleClusters, removedPoints);
removeNaNs(cloudTargetNormal, cloudTargetNormalNoNaNs, removedPoints);
adjustIndicesFromRemovedPoints(targetHandleClusters, removedPoints);
writer.write("handlesSource.pcd", *cloudSourceNormalNoNaNs, sourceHandleClusters, true);
writer.write("handlesTarget.pcd", *cloudTargetNormalNoNaNs, targetHandleClusters, true);
ROS_INFO("Calculating inital transformation from ICP SVD on handles....");
Eigen::Matrix4f guess;
guess << 1, 0, 0, 0.0,
0, 1, 0, 0.5,
0, 0, 1, 0.33,
0, 0, 0, 1;
pcl::IterativeClosestPoint<PointNormal, PointNormal> icp;
// set source and target clouds from indices of pointclouds
pcl::ExtractIndices<PointNormal> handlefilter;
pcl::PointIndices::Ptr sourceHandleIndices (new pcl::PointIndices);
sourceHandleIndices->indices = sourceHandleClusters;
handlefilter.setIndices(sourceHandleIndices);
handlefilter.setInputCloud(cloudSourceNormalNoNaNs);
handlefilter.filter(*cloudHandlesSource);
icp.setInputCloud(cloudHandlesSource);
pcl::PointIndices::Ptr targetHandleIndices (new pcl::PointIndices);
targetHandleIndices->indices = targetHandleClusters;
handlefilter.setIndices(targetHandleIndices);
handlefilter.setInputCloud(cloudTargetNormalNoNaNs);
handlefilter.filter(*cloudHandlesTarget);
icp.setInputTarget(cloudHandlesTarget);
PointCloudNormal Final;
icp.align(Final, guess);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
ROS_INFO("Initialize transformation estimation object....");
boost::shared_ptr< TransformationEstimationJointOptimize<PointNormal, PointNormal > >
transformationEstimation_(new TransformationEstimationJointOptimize<PointNormal, PointNormal>());
float denseCloudWeight = 1.0;
float visualFeatureWeight = 0.0;
float handleFeatureWeight = 0.25;
transformationEstimation_->setWeights(denseCloudWeight, visualFeatureWeight, handleFeatureWeight);
transformationEstimation_->setCorrespondecesDFP(indicesSource, indicesTarget);
// Eigen::Matrix4f guess;
// guess << 0.999203, 0.0337772, -0.0213298, 0.0110106,
// -0.0349403, 0.99778, -0.0567293, -0.00746282,
// 0.0193665, 0.0574294, 0.998162, 0.0141431,
// 0, 0, 0, 1;
// guess << 1, 0.2, 0.3, -0.3,
// -0.2, 1, -0.2, 0.9,
// -0.3, 0.2, 1, 0.4,
// 0, 0, 0, 1;
// guess << 1, 0, 0, 0.0,
// 0, 1, 0, 0.5,
// 0, 0, 1, 0.33,
// 0, 0, 0, 1;
// icp svd for handles node_1/node_91
// guess << 0.993523, 0.0152363, -0.112636, 0.138385,
// -0.0264756, 0.994739, -0.0989777, 0.615225,
// 0.110535, 0.101318, 0.988696, 0.347863,
// 0, 0, 0, 1;
// custom icp
ROS_INFO("Initialize icp object....");
pcl::IterativeClosestPointJointOptimize<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> icpJointOptimize; //JointOptimize
icpJointOptimize.setMaximumIterations (20);
icpJointOptimize.setTransformationEpsilon (0);
icpJointOptimize.setMaxCorrespondenceDistance(0.1);
icpJointOptimize.setRANSACOutlierRejectionThreshold(0.03);
icpJointOptimize.setEuclideanFitnessEpsilon (0);
icpJointOptimize.setTransformationEstimation (transformationEstimation_);
icpJointOptimize.setHandleSourceIndices(sourceHandleClusters);
icpJointOptimize.setHandleTargetIndices(targetHandleClusters);
icpJointOptimize.setInputCloud(cloudSourceNormalNoNaNs);
icpJointOptimize.setInputTarget(cloudTargetNormalNoNaNs);
ROS_INFO("Running ICP....");
PointCloudNormal::Ptr cloud_transformed( new PointCloudNormal);
icpJointOptimize.align ( *cloud_transformed, icp.getFinalTransformation()); //init_tr );
std::cout << "[SIIMCloudMatch::runICPMatch] Has converged? = " << icpJointOptimize.hasConverged() << std::endl <<
" fitness score (SSD): " << icpJointOptimize.getFitnessScore (1000) << std::endl
<< icpJointOptimize.getFinalTransformation () << "\n";
transformPointCloud (*cloudSource, *cloudOut, icpJointOptimize.getFinalTransformation());
ROS_INFO("Writing output....");
writer.write("converged.pcd", *cloudOut, true);
}
