TEST (PCL, IterativeClosestPoint)
{
IterativeClosestPoint<PointXYZ, PointXYZ> reg;
reg.setInputCloud (cloud_source.makeShared ());
reg.setInputTarget (cloud_target.makeShared ());
reg.setMaximumIterations (50);
reg.setTransformationEpsilon (1e-8);
reg.setMaxCorrespondenceDistance (0.05);
// Register
reg.align (cloud_reg);
EXPECT_EQ (int (cloud_reg.points.size ()), int (cloud_source.points.size ()));
Eigen::Matrix4f transformation = reg.getFinalTransformation ();
EXPECT_NEAR (transformation (0, 0), 0.8806, 1e-3);
EXPECT_NEAR (transformation (0, 1), 0.036481287330389023, 1e-2);
EXPECT_NEAR (transformation (0, 2), -0.4724, 1e-3);
EXPECT_NEAR (transformation (0, 3), 0.03453, 1e-3);
EXPECT_NEAR (transformation (1, 0), -0.02354, 1e-3);
EXPECT_NEAR (transformation (1, 1), 0.9992, 1e-3);
EXPECT_NEAR (transformation (1, 2), 0.03326, 1e-3);
EXPECT_NEAR (transformation (1, 3), -0.001519, 1e-3);
EXPECT_NEAR (transformation (2, 0), 0.4732, 1e-3);
EXPECT_NEAR (transformation (2, 1), -0.01817, 1e-3);
EXPECT_NEAR (transformation (2, 2), 0.8808, 1e-3);
EXPECT_NEAR (transformation (2, 3), 0.04116, 1e-3);
EXPECT_EQ (transformation (3, 0), 0);
EXPECT_EQ (transformation (3, 1), 0);
EXPECT_EQ (transformation (3, 2), 0);
EXPECT_EQ (transformation (3, 3), 1);
}
TEST (PCL, IterativeClosestPointWithRejectors)
{
IterativeClosestPoint<PointXYZ, PointXYZ> reg;
reg.setMaximumIterations (50);
reg.setTransformationEpsilon (1e-8);
reg.setMaxCorrespondenceDistance (0.15);
// Add a median distance rejector
pcl::registration::CorrespondenceRejectorMedianDistance::Ptr rej_med (new pcl::registration::CorrespondenceRejectorMedianDistance);
rej_med->setMedianFactor (4.0);
reg.addCorrespondenceRejector (rej_med);
// Also add a SaC rejector
pcl::registration::CorrespondenceRejectorSampleConsensus<PointXYZ>::Ptr rej_samp (new pcl::registration::CorrespondenceRejectorSampleConsensus<PointXYZ>);
reg.addCorrespondenceRejector (rej_samp);
size_t ntransforms = 10;
for (size_t t = 0; t < ntransforms; t++)
{
// Sample a fixed offset between cloud pairs
Eigen::Affine3f delta_transform;
sampleRandomTransform (delta_transform, 0., 0.05);
// Sample random global transform for each pair, to make sure we aren't biased around the origin
Eigen::Affine3f net_transform;
sampleRandomTransform (net_transform, 2*M_PI, 10.);
PointCloud<PointXYZ>::ConstPtr source (cloud_source.makeShared ());
PointCloud<PointXYZ>::Ptr source_trans (new PointCloud<PointXYZ>);
PointCloud<PointXYZ>::Ptr target_trans (new PointCloud<PointXYZ>);
pcl::transformPointCloud (*source, *source_trans, delta_transform.inverse () * net_transform);
pcl::transformPointCloud (*source, *target_trans, net_transform);
reg.setInputSource (source_trans);
reg.setInputTarget (target_trans);
// Register
reg.align (cloud_reg);
Eigen::Matrix4f trans_final = reg.getFinalTransformation ();
// Translation should be within 1cm
for (int y = 0; y < 4; y++)
EXPECT_NEAR (trans_final (y, 3), delta_transform (y, 3), 1E-2);
// Rotation within .1
for (int y = 0; y < 4; y++)
for (int x = 0; x < 3; x++)
EXPECT_NEAR (trans_final (y, x), delta_transform (y, x), 1E-1);
}
}
bool
IncrementalPoseEstimatorFromRgbFeatures::optimizeWithICP(
pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud_source,
Pose3D& depth_pose,
int closest_view_index)
{
pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud_target = m_image_data[closest_view_index].sampled_cloud;
pcl::PointCloud<pcl::PointXYZ> cloud_reg;
Pose3D new_depth_pose = depth_pose;
ntk_dbg_print(cloud_source->points.size(), 1);
ntk_dbg_print(cloud_target->points.size(), 1);
IterativeClosestPoint<PointXYZ, PointXYZ> reg;
reg.setInputCloud (cloud_target);
reg.setInputTarget (cloud_source);
reg.setMaximumIterations (50);
reg.setTransformationEpsilon (1e-5);
reg.setMaxCorrespondenceDistance (0.5);
reg.align (cloud_reg);
if (!reg.hasConverged())
{
ntk_dbg(1) << "ICP did not converge, ignoring.";
return false;
}
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);
Pose3D icp_pose;
icp_pose.setCameraTransform(T);
new_depth_pose.applyTransformAfter(icp_pose);
depth_pose = new_depth_pose;
return true;
}
void
compute (const pcl::PCLPointCloud2::ConstPtr &source,
const pcl::PCLPointCloud2::ConstPtr &target,
pcl::PCLPointCloud2 &transformed_source)
{
// Convert data to PointCloud<T>
PointCloud<PointNormal>::Ptr src (new PointCloud<PointNormal>);
PointCloud<PointNormal>::Ptr tgt (new PointCloud<PointNormal>);
fromPCLPointCloud2 (*source, *src);
fromPCLPointCloud2 (*target, *tgt);
// Estimate
TicToc tt;
tt.tic ();
print_highlight (stderr, "Computing ");
#define Scalar double
//#define Scalar float
TransformationEstimationLM<PointNormal, PointNormal, Scalar>::Ptr te (new TransformationEstimationLM<PointNormal, PointNormal, Scalar>);
//TransformationEstimationSVD<PointNormal, PointNormal, Scalar>::Ptr te (new TransformationEstimationSVD<PointNormal, PointNormal, Scalar>);
CorrespondenceEstimation<PointNormal, PointNormal, double>::Ptr cens (new CorrespondenceEstimation<PointNormal, PointNormal, double>);
//CorrespondenceEstimationNormalShooting<PointNormal, PointNormal, PointNormal>::Ptr cens (new CorrespondenceEstimationNormalShooting<PointNormal, PointNormal, PointNormal>);
//CorrespondenceEstimationNormalShooting<PointNormal, PointNormal, PointNormal, double>::Ptr cens (new CorrespondenceEstimationNormalShooting<PointNormal, PointNormal, PointNormal, double>);
cens->setInputSource (src);
cens->setInputTarget (tgt);
//cens->setSourceNormals (src);
CorrespondenceRejectorOneToOne::Ptr cor_rej_o2o (new CorrespondenceRejectorOneToOne);
CorrespondenceRejectorMedianDistance::Ptr cor_rej_med (new CorrespondenceRejectorMedianDistance);
cor_rej_med->setInputSource<PointNormal> (src);
cor_rej_med->setInputTarget<PointNormal> (tgt);
CorrespondenceRejectorSampleConsensus<PointNormal>::Ptr cor_rej_sac (new CorrespondenceRejectorSampleConsensus<PointNormal>);
cor_rej_sac->setInputSource (src);
cor_rej_sac->setInputTarget (tgt);
cor_rej_sac->setInlierThreshold (0.005);
cor_rej_sac->setMaximumIterations (10000);
CorrespondenceRejectorVarTrimmed::Ptr cor_rej_var (new CorrespondenceRejectorVarTrimmed);
cor_rej_var->setInputSource<PointNormal> (src);
cor_rej_var->setInputTarget<PointNormal> (tgt);
CorrespondenceRejectorTrimmed::Ptr cor_rej_tri (new CorrespondenceRejectorTrimmed);
IterativeClosestPoint<PointNormal, PointNormal, Scalar> icp;
icp.setCorrespondenceEstimation (cens);
icp.setTransformationEstimation (te);
icp.addCorrespondenceRejector (cor_rej_o2o);
//icp.addCorrespondenceRejector (cor_rej_var);
//icp.addCorrespondenceRejector (cor_rej_med);
//icp.addCorrespondenceRejector (cor_rej_tri);
//icp.addCorrespondenceRejector (cor_rej_sac);
icp.setInputSource (src);
icp.setInputTarget (tgt);
icp.setMaximumIterations (1000);
icp.setTransformationEpsilon (1e-10);
PointCloud<PointNormal> output;
icp.align (output);
print_info ("[done, ");
print_value ("%g", tt.toc ());
print_info (" ms : ");
print_value ("%d", output.width * output.height);
print_info (" points], has converged: ");
print_value ("%d", icp.hasConverged ());
print_info (" with score: %f\n", icp.getFitnessScore ());
Eigen::Matrix4d transformation = icp.getFinalTransformation ();
//Eigen::Matrix4f transformation = icp.getFinalTransformation ();
PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n",
transformation (0, 0), transformation (0, 1), transformation (0, 2), transformation (0, 3),
transformation (1, 0), transformation (1, 1), transformation (1, 2), transformation (1, 3),
transformation (2, 0), transformation (2, 1), transformation (2, 2), transformation (2, 3),
transformation (3, 0), transformation (3, 1), transformation (3, 2), transformation (3, 3));
// Convert data back
pcl::PCLPointCloud2 output_source;
toPCLPointCloud2 (output, output_source);
concatenateFields (*source, output_source, transformed_source);
}
bool RelativePoseEstimatorICP :: estimateNewPose(const RGBDImage& image)
{
if (m_ref_cloud.points.size() < 1)
{
ntk_dbg(1) << "Reference cloud was empty";
return false;
}
PointCloud<PointXYZ>::Ptr target = m_ref_cloud.makeShared();
PointCloud<PointXYZ>::Ptr source (new PointCloud<PointXYZ>());
rgbdImageToPointCloud(*source, image);
PointCloud<PointXYZ>::Ptr filtered_source (new PointCloud<PointXYZ>());
PointCloud<PointXYZ>::Ptr filtered_target (new PointCloud<PointXYZ>());
pcl::VoxelGrid<pcl::PointXYZ> grid;
grid.setLeafSize (m_voxel_leaf_size, m_voxel_leaf_size, m_voxel_leaf_size);
grid.setInputCloud(source);
grid.filter(*filtered_source);
grid.setInputCloud(target);
grid.filter(*filtered_target);
PointCloud<PointXYZ> cloud_reg;
IterativeClosestPoint<PointXYZ, PointXYZ> reg;
reg.setMaximumIterations (m_max_iterations);
reg.setTransformationEpsilon (1e-5);
reg.setMaxCorrespondenceDistance (m_distance_threshold);
reg.setInputCloud (filtered_source);
reg.setInputTarget (filtered_target);
reg.align (cloud_reg);
if (!reg.hasConverged())
{
ntk_dbg(1) << "ICP did not converge, ignoring.";
return false;
}
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);
Pose3D icp_pose;
icp_pose.setCameraTransform(T);
ntk_dbg_print(icp_pose.cvTranslation(), 1);
// Pose3D stores the transformation from 3D space to image.
// So we have to invert everything so that unprojecting from
// image to 3D gives the right transformation.
// H2' = ICP * H1'
m_current_pose.resetCameraTransform();
m_current_pose = *image.calibration()->depth_pose;
m_current_pose.invert();
m_current_pose.applyTransformAfter(icp_pose);
m_current_pose.invert();
return true;
}