视觉SLAM实践入门——（16）使用SVD求解ICP

在理论部分已经总结了使用SVD求解的步骤（视觉SLAM理论入门——（9）视觉里程计之特征点法—ICP_floatinglong的博客-优快云博客）：

 

1、求点对的去质心坐标

2、估计旋转矩阵 R

3、计算平移向量 t

书中的问题

这部分书上源码有点不好理解（实际上书中并没有指明参考系，假定以相机1为参考系）：

1、求得的 R 不是相机1到相机2的旋转矩阵， 而是它的逆（）

2、求得的 t 不是相机1到相机2的平移向量，而是它的相反数（ -t ）

结论 — 按照书中的源码，求解时应该是以相机2作为参考系，求相机2到相机1的位姿（前面2d-2d、3d-2d均是以相机1作为参考），本文所附代码以相机1为参考，求解相机1到相机2的位姿

以相机1为参考进行运动估计：

1、对于旋转矩阵，有两种解释

① 按照书中对 w 的定义以及 对 R 的求解公式（认为这是正确的），要以相机1为参考进行求解，需要将下面代码点对的位置交换

W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();

② 按照比较自然的定义方法， w 取书本定义的 w 的转置，那么求解 R 的时候，也应该取书本中公式的转置，那么按照上面代码对 w 的定义，求解的便是相机1到相机2的旋转矩阵

2、对于平移向量，同样是点对的问题，如果要求相机1到相机2的平移向量，下面代码的计算方法应该改为 

  Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);

下面贴上我的代码，定义方式与源码有点不一样，大致过程是一样的

#include <iostream>
using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
using namespace cv;

#include <Eigen/Core>
#include <Eigen/SVD>
#include <Eigen/Dense>


//提取ORB特征并匹配
void findAndMatchOrbFeature(const Mat &img1, const Mat &img2, 
	vector<KeyPoint> &kp1, vector<KeyPoint> &kp2, 
	vector<DMatch> &matches)
{	
	Ptr<FeatureDetector> detector = ORB::create();
	Ptr<DescriptorExtractor> desc = ORB::create();
	Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
	
	//检测角点
	detector->detect(img1, kp1);
	detector->detect(img2, kp2);
	
	//计算描述子
	Mat desc1, desc2;	
	desc->compute(img1, kp1, desc1);
	desc->compute(img2, kp2, desc2);
	
	//匹配点对
	vector<DMatch> allMatches;
	matcher->match(desc1, desc2, allMatches);
	
	//检测误匹配
	double minDist = 10000;
	for(auto m : allMatches)
		if(m.distance < minDist)	minDist = m.distance;
	
	double useDist = max(2*minDist, 30.0);
	for(auto m : allMatches)	
		if(m.distance <= useDist)
			matches.push_back(m);	
}

//将像素坐标转化为归一化坐标
Point2d pixelToNor(const Point2d &p, const Mat & k)
{
	//像素坐标 = k * 归一化坐标
	//xp = fx * xn + cx
	return Point2d((p.x - k.at<double>(0, 2)) / k.at<double>(0, 0), (p.y - k.at<double>(1, 2)) / k.at<double>(1, 1));
}

void poseEstimate3d3d(const vector<Point3d> &p1s3d, const vector<Point3d> &p2s3d, 
	Mat &R, Mat &t)
{
/***********************
    1、求去质心坐标
	2、SVD分解求R
	3、求t
************************/

	Point3d p1(0, 0, 0), p2(0, 0, 0);
	//求质心p1、p2
	for(int i = 0; i < (int)p1s3d.size(); i++)
	{
		p1 += p1s3d[i];
		p2 += p2s3d[i];
	}
	p1 = p1 / (int)(p1s3d.size());
	p2 = p2 / (int)(p2s3d.size());
	
	//使用去质心坐标，求 w
	Eigen::Matrix3d w = Eigen::Matrix3d::Zero();
	for(int i = 0; i < (int)p1s3d.size(); i++)
	{		
		Point3d p_3d = p1s3d[i] - p1;	
		Point3d p3d = p2s3d[i] - p2;
		
		Eigen::Vector3d p_V3d(p_3d.x, p_3d.y, p_3d.z), pV3d(p3d.x, p3d.y, p3d.z);
		w += p_V3d*pV3d.transpose();
	}
	
	//SVD分解求 R ，按照这里 w 的定义方式，R = VU^T
	JacobiSVD<Eigen::Matrix3d> svd(w, ComputeFullU | ComputeFullV );
	Eigen::Matrix3d U = svd.matrixU();
	Eigen::Matrix3d V = svd.matrixV();
	
	Eigen::Matrix3d R_ = V*U.transpose();
		  
	if(R_.determinant() < 0)	//保证det > 0
		R_ = -R_;

	//根据 R ，求t
	t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));	
		
	//重构Mat格式的 R，t
	R = (Mat_<double>(3, 3) << R_(0, 0), R_(0, 1), R_(0, 2),
								R_(1, 0), R_(1, 1), R_(1, 2),
								R_(2, 0), R_(2, 1), R_(2, 2));
	
	Eigen::Vector3d p_(p1.x, p1.y, p1.z),  p(p2.x, p2.y, p2.z);
	Eigen::Vector3d t_ = p - R_*p_;
}

int main(int argc, char **argv)
{
	if(argc != 5)
	{
		cout << "error!" << endl;
		return 0;
	}

	Mat img1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
	Mat img2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);

	//提取ORB特征、匹配点对
	vector<KeyPoint> kp1, kp2;
	vector<DMatch> matches;
	findAndMatchOrbFeature(img1, img2, kp1, kp2, matches);
	
		
	//读取深度图，获得3d点对，每个像素占据16字节（单通道）
	Mat img3 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);
	Mat img4 = imread(argv[4], CV_LOAD_IMAGE_UNCHANGED);	
	Mat k = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
	vector<Point3d> p1s3d, p2s3d;
	for(auto m : matches)
	{
		Point2d p12d = kp1[m.queryIdx].pt;
		unsigned short pixel1 = img3.ptr<unsigned short>((int)p12d.y)[(int)p12d.x];
		Point2d p22d = kp2[m.trainIdx].pt;
		unsigned short pixel2 = img4.ptr<unsigned short>((int)p22d.y)[(int)p22d.x];
		
		if(pixel1 == 0 || pixel2 == 0)	continue;
		
		double depth = pixel1 / 5000.0;		
		Point2d pNor2d = pixelToNor(p12d, k);		
		Point3d p3d(pNor2d.x*depth, pNor2d.y*depth, depth);
		p1s3d.push_back(p3d);
		
		depth = pixel2 / 5000.0;
		pNor2d = pixelToNor(p22d, k);		
		p3d = Point3d(pNor2d.x*depth, pNor2d.y*depth, depth);
		p2s3d.push_back(p3d);		
	}
	
	//ICP估计
	Mat R, t;
	poseEstimate3d3d(p1s3d, p2s3d, R, t);
	cout << "R :\n" << R << endl << "t : " << t.t() << endl;;
	
	return 0;
}