finding a transformation from one coordinate system to another so that all features that appear in both data sets are aligned with each other

three_point_registration.cpp

//PointsAとPointsBはそれぞれ対応が取れている3点の三次元座標が入っている。
//このPointsAからPointsBまでの座標変換を行う行列を計算する。
// OpenCV 2.4.x, VS2012Proで動作確認済み
// 参考資料　http://www1.cs.columbia.edu/~allen/PHOTOPAPERS/hmwk1.pdf 等
void GetTransformMatrix(const std::vector<cv::Point3d> PointsA,const std::vector<cv::Point3d> PointsB,cv::Mat &modelview_matrix ){
	std::cout<<"Get Modelview Matrix from 3point "<<std::endl;

	if(PointsA.size() != 3 || PointsB.size()!=3)
	{
		throw  std::length_error("vector length not match exception");
		return ;
	}

	auto length_of_vector=std::min(PointsA.size(),PointsB.size());

	//中心点座標を求める
	cv::Point3d center_a = std::accumulate(PointsA.begin(), PointsA.end(), cv::Point3d(0.0,0.0,0.0))*(double)(1.00/length_of_vector);
	cv::Point3d center_b = std::accumulate(PointsB.begin(), PointsB.end(), cv::Point3d(0.0,0.0,0.0))*(double)(1.00/length_of_vector);
	
	auto PointsA_copy=PointsA;
	auto PointsB_copy=PointsB;
	for( auto &itr_a : PointsA_copy ){
		itr_a -= center_a;
	}
	for( auto &itr_b : PointsB_copy ){
		itr_b -= center_b;
	}
	
	cv::Mat MA=cv::Mat(3,1 ,CV_64F);
	cv::Mat MB=cv::Mat(1,3 ,CV_64F);
	cv::Mat H=cv::Mat::zeros(3,3 ,CV_64F);
	
	for(unsigned int i=0; i < length_of_vector; i++){
		MA.at<double>(0,0)=PointsA_copy[i].x;
		MA.at<double>(1,0)=PointsA_copy[i].y;
		MA.at<double>(2,0)=PointsA_copy[i].z;
	
		MB.at<double>(0,0)=PointsB_copy[i].x;
		MB.at<double>(0,1)=PointsB_copy[i].y;
		MB.at<double>(0,2)=PointsB_copy[i].z;

		auto MC=MA*MB;
		H+=MC;
	}
	
	//for svd Vt,U,Sはすべて3x3
	cv::Mat Vt,U,S;//3x3 double
	
	//cv::SVDecompで出てくるV項は転置した値であることに注意
	cv::SVDecomp(H,S,U,Vt,0);
	//転置を元に戻す
	cv::Mat V= Vt.t();//V=cv::Mat(3,3 ,CV_64F)

	cv::Mat R= V*U.t();
	//cout<<"R:"<<R<<endl;
	if(cv::determinant(R)<0){
		cout<<endl<<"***Refrection***"<<endl<<endl;
	Vt.at<double>(2,0)=-1*Vt.at<double>(2,0);
	Vt.at<double>(2,1)=-1*Vt.at<double>(2,1);
	Vt.at<double>(2,2)=-1*Vt.at<double>(2,2);
	
	R=Vt.t()*U.t();
	
	}
	cv::Mat translate = cv::Mat::eye(4,4,CV_64F);
	cv::Mat to_origin = cv::Mat::eye(4,4,CV_64F);
	cv::Mat rotation = cv::Mat::eye(4,4,CV_64F);

	for(int y=0;y<3;y++){
		for(int x=0;x<3;x++){
			rotation.at<double>(y,x)=R.at<double>(y,x);
		}
	}
	
	to_origin.at<double>(0,3)=-center_a.x;
	to_origin.at<double>(1,3)=-center_a.y;
	to_origin.at<double>(2,3)=-center_a.z;

	translate.at<double>(0,3)=center_b.x;
	translate.at<double>(1,3)=center_b.y;
	translate.at<double>(2,3)=center_b.z;
	
	cv::Mat rot_global= rotation* to_origin;

	modelview_matrix=translate*(rot_global);

	cout<<"modelview"<<endl<<modelview_matrix<<endl;
	
	return;
}