Registering point clouds rigidly with scale using PCL [w/code]

Hello
Sorry for the bombardment of posts, but I want to share some stuff I’ve been working on lately, so when I find time I just shoot the posts out.
So this time I’ll talk shortly about how to get an estimation of a rigid transformation between two clouds, that potentially are also of different scale. You will end up with a rigid transformation (Rotation Translation) and a scale factor, son in fact it will be a Similarity Transformation. We will first find the right scale, and then find the right transformation, given there is one (but we will find the best transformation there is).

Plain vanilla rigid transformation

This part is easy, since it’s just using PCL’s pcl::SampleConsensusModelRegistration. They really did create a beautiful API for PCL, highly interchangeable algorithms and approaches.

int RANSACRegister(const pcl::PointCloud<pcl::PointNormal>::Ptr& cloudA,
			const pcl::PointCloud<pcl::PointNormal>::Ptr& cloudB,
			Eigen::Matrix4f& Tresult)
{
	pcl::SampleConsensusModelRegistration<pcl::PointNormal>::Ptr sac_model(new pcl::SampleConsensusModelRegistration<pcl::PointNormal>(cloudA));
	sac_model->setInputTarget(cloudB);
	pcl::RandomSampleConsensus<pcl::PointNormal> ransac(sac_model);
	//pcl::LeastMedianSquares<pcl::PointNormal> ransac(sac_model); //might as well try these out too!
	//pcl::ProgressiveSampleConsensus<pcl::PointNormal> ransac(sac_model);
	ransac.setDistanceThreshold(0.1);
	//upping the verbosity level to see some info
	pcl::console::VERBOSITY_LEVEL vblvl = pcl::console::getVerbosityLevel();
	pcl::console::setVerbosityLevel(pcl::console::VERBOSITY_LEVEL::L_DEBUG);
	ransac.computeModel(1);
	pcl::console::setVerbosityLevel(vblvl);
	Eigen::VectorXf coeffs;
	ransac.getModelCoefficients(coeffs);
	assert(coeffs.size() == 16);
	Tresult = Eigen::Map<Eigen::Matrix4f>(coeffs.data(),4,4);
	vector<int> inliers; ransac.getInliers(inliers);
	return inliers.size();
}

Here we can see how simple it is to get a rigid transform between two clouds using RANSAC in PCL. There’s even a lot of garnish you can get rid of…
You can also not use pcl::PointNormalpcl::PointXYZ or pcl::PointXYZRGB. Heck you can even write this function as a template which is very common in PCL.

Let’s iterate on scale

So next step is performing the above function for variable scale. Since our two point clouds may not be of the same scale, we can look around a neighborhood of scales (in steps) and find the best fit in terms of inliers.

void ScaleRANSACRegisterEx(const pcl::PointCloud<pcl::PointNormal>::Ptr& cloudA,
					const pcl::PointCloud<pcl::PointNormal>::Ptr& cloudB,
					Eigen::Matrix4f& Tresult,
					double& in_out_s,
					int num_iterations,
					double iteration_scale_step)
{
	double s = in_out_s;
	int max_inliers = 0; Eigen::Matrix4f max_T; double max_s = s;
	for(int i=-(num_iterations/2);i<=(num_iterations/2);i++)
	//int i=0;
	{
		double _s = (s + (double)i*(s*iteration_scale_step));
		cout << "scale synth to " << _s << endl;
		Eigen::Matrix4f T = Eigen::Matrix4f(Eigen::Matrix4f::Identity());
		T.topLeftCorner(3,3) *= Eigen::Matrix3f::Identity() * _s;
		cout << "apply scale"<<endl<<T<<endl;
		pcl::PointCloud<pcl::PointNormal> cloudA_trans;
		pcl::transformPointCloud<pcl::PointNormal>(*cloudA, cloudA_trans, T);
		int inliers_num = RANSACRegister(cloudA_trans.makeShared(),cloudB,Tresult);
		cout << "RANSAC rigid transform:"<<endl<<Tresult.transpose()<<endl;
		cout << "RANSAC inliers:"<<inliers_num<<endl;
		cout << "------------------------------------------------------------------------" << endl;
		if(inliers_num>max_inliers) {
			max_inliers = inliers_num;
			max_T = Tresult;
			max_s = _s;
		}
	}
	Tresult = max_T;
	in_out_s = max_s;
}

Here I’m simply iterating over different scales with regular steps. It’s a very common practice in many methods, when trying to find the parameters that maximize the support (I’m looking at the maximum support from the RANSAC process).

Getting a prior estimate of the scale

Now all that’s left is a place to start from. If we think about it, the two clouds can be of completely different scales: one is 1000 meters wide and the other 0.1 mm wide… we need a good guess. Wise Yoda says PCA can help here, and the intuition is that if the two clouds are roughly similar their major axes will also align in direction and scale. Rotation we are taking care of inside the RANSAC for ridig transform, but the scale we can definitely use as initial guess. So taking the largest eigenvalue of the PCA process over both clouds will give us the scale.

void ScaleRANSACRegister(const pcl::PointCloud<pcl::PointNormal>::Ptr& cloudA,
					const pcl::PointCloud<pcl::PointNormal>::Ptr& cloudB,
					Eigen::Matrix4f& Tresult,
					double& max_s,
					int num_iterations,
					double iteration_scale_step)
{
	pcl::PCA<pcl::PointNormal> pca;
	pca.setInputCloud(cloudA);
	Eigen::Vector4f v_A_mu = pca.getMean();
	Eigen::Vector3f ev_A = pca.getEigenValues();
	pca.setInputCloud(cloudB);
	Eigen::Vector4f v_B_mu = pca.getMean();
	Eigen::Vector3f ev_B = pca.getEigenValues();
	double s = sqrt(ev_B[0])/sqrt(ev_A[0]);
	//rough
	ScaleRANSACRegisterEx(cloudA,cloudB,Tresult,s,num_iterations,iteration_scale_step);
	max_s = s;
	//fine
	ScaleRANSACRegisterEx(cloudA,cloudB,Tresult,max_s,num_iterations,iteration_scale_step/10.0);
}

Note that I take the square root of the eigenvalue as it is actually the rectified variance of the data in the major axes (sigma squared).
That pretty much wraps it up..
Enjoy!
Roy.