视觉SLAM实践入门——（14）高斯牛顿法求解PnP

使用高斯牛顿法求解PnP的步骤：

1、定义向量类型（使用Eigen库可以方便地进行矩阵运算，但是涉及内存对齐的问题。将Eigen的数据类型作为vector模板参数，需要指定对应的内存分配器，关于内存对齐的细节可以参考https://zhuanlan.zhihu.com/p/93824687）

2、提取ORB特征并匹配点对

3、计算雅可比矩阵 J

4、利用 J ，增量方程的 H、b

5、求解 Hx = b

6、更新优化变量

7、如果目标函数下降，继续迭代，直至达到局部极小值或者迭代次数到达上限

 

#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 <chrono>

#include <sophus/se3.hpp>

//提取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));
}

//G-N法求解PnP问题
void solvePnPByGN(const vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> &ps3d, 
	const vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> &ps2d, 
	const Mat &k,
	Sophus::SE3d &pose)
{
	const int iterations = 10;	//迭代次数
	double cost = 0, lastCost = 0;	//目标函数
	double fx = k.at<double>(0, 0);
	double cx = k.at<double>(0, 2);
	double fy = k.at<double>(1, 1);
	double cy = k.at<double>(1, 2);
			
	//开始迭代求解
	for(int i = 0; i < iterations; i++)
	{
		Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
		Eigen::Matrix<double, 6, 1> b = Eigen::Matrix<double, 6, 1>::Zero();

		//计算J、H、b
		cost = 0;
		for(int j = 0; j < (int)ps3d.size(); j++)
		{		
			Eigen::Vector3d tp3d = pose * ps3d[j];	//旋转后的空间坐标
			Eigen::Vector2d tpNor2d(tp3d[0]/tp3d[2], tp3d[1]/tp3d[2]);	//旋转后点的归一化坐标
			Eigen::Vector2d tp2d(fx*tpNor2d[0] + cx, fy*tpNor2d[1] + cy);	//旋转后点的像素坐标

			Eigen::Vector2d e = ps2d[j] - tp2d;	//误差		
			cost += e.squaredNorm();
			
			Eigen::Matrix<double, 2, 6> J;
			double invZ = 1.0 / tp3d[2];
			double invZ2 = invZ * invZ;
			//J是2x6矩阵
			J << -fx * invZ, 0, fx * tp3d[0] * invZ2,
				fx * tp3d[0] * tp3d[1] * invZ2,	-fx - fx * tp3d[0] * tp3d[0] * invZ2, fx * tp3d[1] * invZ,
				0, -fy * invZ, fy * tp3d[1] * invZ2,
				fy + fy * tp3d[1] * tp3d[1] * invZ2, -fy * tp3d[0] * tp3d[1] * invZ2, -fy * tp3d[0] * invZ;

			H += J.transpose()*J;
			b += -J.transpose()*e;
		}

		//求解 Hx = b
		Eigen::Matrix<double, 6, 1> dx;
		dx = H.ldlt().solve(b);
		
		if(isnan(dx[0]))
		{
			cout << "isnan" << endl;
			break;
		}
		if(i > 0 && cost >= lastCost)
		{
			cout << "cost and last cost: " << cost << " " << lastCost << endl;
			break;
		}
		
		//更新优化变量和目标函数
		pose = Sophus::SE3d::exp(dx)*pose;
		lastCost = cost;
		
		cout << i << "\tcost: " << cost << endl;
		
		//dx足够小
		if(dx.norm() <= 1e-6)
		{
			cout << "converge" << endl;
			break;
		}
	}
}

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

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

	vector<KeyPoint> kp1, kp2;
	vector<DMatch> matches;
	findAndMatchOrbFeature(img1, img2, kp1, kp2, matches);
	
		
	//读取深度图，获得图1的3d点，每个像素占据16字节（单通道）
	//最好在这个循环同步获取图2的2d点。因为3d点与2d是匹配点对，循环中会根据深度值剔除一些点，如果图2的2d点在其他地方获取，也必须根据深度进行筛选
	Mat k = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
	Mat img3 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);
	vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> ps2d;
	vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> ps3d;	
	for(auto m : matches)
	{
		Point2d p2d = kp1[m.queryIdx].pt;
		int x = p2d.x, y = p2d.y;
		unsigned short pixel = img3.ptr<unsigned short>(y)[x];
		
		if(pixel == 0)	continue;
		
		double dPixel = pixel / 5000.0;
		Point2d pNor2d = pixelToNor(p2d, k);
		ps3d.push_back(Eigen::Vector3d(pNor2d.x*dPixel, pNor2d.y*dPixel, dPixel));
		
		p2d = kp2[m.trainIdx].pt;
		x = p2d.x;
		y = p2d.y;
		ps2d.push_back(Eigen::Vector2d(x, y));
	}
	
	Sophus::SE3d pose;
	solvePnPByGN(ps3d, ps2d, k, pose);

	cout << "T：" << endl << pose.matrix() << endl;
	
	return 0;
}