在理论部分已经总结了使用SVD求解的步骤(视觉SLAM理论入门——(9)视觉里程计之特征点法—ICP_floatinglong的博客-CSDN博客):
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;
}