1.ICP

假设有一组配对好的3D点, \(P={P_{1}, ..., P_{N}}\) , \(P^{'}={P_{1}^{'}, ..., P_{N}^{'}}\)。

有一个欧式变换R,t,使得: \(p_{i} = Rp^{'}_{i} + t\)

该问题可以用迭代最近点(ICP)来求解。注意考虑两组3D点的变换时,和相机没有关系。

ICP求解线性代数的求解(SVD)和非线性优化方式求解(类似于BA)

2.SVD求解:

定义误差项: \(e_{i} = p_{i} - ( Rp^{'}_{i} + t )\)

构建最小二乘问题,使误差平方和达到极小的R,t

定义两组点的质心:

\(p = \frac{1}{n} \sum^{n}_{i=1} (p_{i}), p^{'} = \frac{1}{n} \sum^{n}_{i=1} (p_{i}^{'}),\)

步骤:

  • 计算两组点的质心位置 \(p,p^{'}\),然后再计算每个点的去质心坐标:

    \(q_{i} = p_{i} - p, q_{i}^{'} = p_{i}^{'} - p^{'}\)

  • 计算 \(R^{*} = argmin \frac{1}{2} \sum^{n}_{i=1} || q_{i} - Rq_{i}^{'} ||^{2}\)

  • 将上式展开,优化函数变为求解 \(-tr( R \sum^{n}_{i=1} q_{i}^{'} q_{i}^{T} )\)

    定义 \(W=\sum^{n}_{i=1} q_{i}^{'} q_{i}^{T}\),对W进行SVD分解,得到\(W=U \Sigma V^{T}\)

  • \Sigma 为奇异值组成的对角矩阵,对角线元素从大到小排列,而U和V为正交矩阵。当W满秩时,\(R=UV^{T}\)

  • 根据求出的R,计算t: \(t^{*} = p - Rp^{'}\)

3.代码:

void pose_estimation_3d3d(const vector<Point3f> &pts1,
const vector<Point3f> &pts2,
Mat &R, Mat &t) {
Point3f p1, p2; // center of mass
//求质心
int N = pts1.size();
for (int i = 0; i < N; i++) {
p1 += pts1[i];
p2 += pts2[i];
} p1 = Point3f(Vec3f(p1) / N);
p2 = Point3f(Vec3f(p2) / N);
vector<Point3f> q1(N), q2(N); // remove the center
//去质心
for (int i = 0; i < N; i++) {
q1[i] = pts1[i] - p1;
q2[i] = pts2[i] - p2;
} // compute q1*q2^T
Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
for (int i = 0; i < N; i++) {
W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
}
cout << "W=" << W << endl; // SVD on W
Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV(); cout << "U=" << U << endl;
cout << "V=" << V << endl; Eigen::Matrix3d R_ = U * (V.transpose());
if (R_.determinant() < 0) {
R_ = -R_;
}
Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z); // convert to cv::Mat
//推导是按第二张图到第一张图的变化,
//此处进行逆变换,即为第一张图到第二张图的变化
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)
);
t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

4.非线性优化方法:

/// 节点,优化变量维度和数据类型
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; //初始化
virtual void setToOriginImpl() override {
_estimate = Sophus::SE3d();
} //更新估计值
/// left multiplication on SE3
virtual void oplusImpl(const double *update) override { Eigen::Matrix<double, 6, 1> update_eigen;
update_eigen << update[0], update[1], update[2], update[3], update[4], update[5]; _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
} virtual bool read(istream &in) override {} virtual bool write(ostream &out) const override {}
}; /// 边,误差模型 观测维度,观测数据类型, 链接节点类型
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPose> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d &point) : _point(point) {} virtual void computeError() override {
//获取姿态估计值
const VertexPose *pose = static_cast<const VertexPose *> ( _vertices[0] );
//计算误差,测量值-转换值
_error = _measurement - pose->estimate() * _point;
} //计算雅可比矩阵
//雅可比矩阵为[I,-P'^]
virtual void linearizeOplus() override {
VertexPose *pose = static_cast<VertexPose *>(_vertices[0]);
Sophus::SE3d T = pose->estimate();
Eigen::Vector3d xyz_trans = T * _point;
//单位矩阵
_jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
//向量到反对称矩阵
_jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(xyz_trans);
} bool read(istream &in) {} bool write(ostream &out) const {} protected:
Eigen::Vector3d _point;
}; //将顶点和边加入g2o
oid bundleAdjustment(
const vector<Point3f> &pts1,
const vector<Point3f> &pts2,
Mat &R, Mat &t) {
// 构建图优化,先设定g2o
typedef g2o::BlockSolverX BlockSolverType;
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
// 梯度下降方法,可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出 // vertex
VertexPose *pose = new VertexPose(); // camera pose
pose->setId(0);
pose->setEstimate(Sophus::SE3d());
optimizer.addVertex(pose); // edges
for (size_t i = 0; i < pts1.size(); i++) {
EdgeProjectXYZRGBDPoseOnly *edge = new EdgeProjectXYZRGBDPoseOnly(
Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z));
edge->setVertex(0, pose);
edge->setMeasurement(Eigen::Vector3d(
pts1[i].x, pts1[i].y, pts1[i].z));
edge->setInformation(Eigen::Matrix3d::Identity());
optimizer.addEdge(edge);
} chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(10);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "optimization costs time: " << time_used.count() << " seconds." << endl; cout << endl << "after optimization:" << endl;
cout << "T=\n" << pose->estimate().matrix() << endl; // convert to cv::Mat
Eigen::Matrix3d R_ = pose->estimate().rotationMatrix();
Eigen::Vector3d t_ = pose->estimate().translation();
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)
);
t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

视觉十四讲:第七讲_3D-3D:ICP估计姿态的更多相关文章

  1. 视觉slam十四讲第七章课后习题6

    版权声明:本文为博主原创文章,转载请注明出处: http://www.cnblogs.com/newneul/p/8545450.html 6.在PnP优化中,将第一个相机的观测也考虑进来,程序应如何 ...

  2. 视觉slam十四讲第七章课后习题7

    版权声明:本文为博主原创文章,转载请注明出处:http://www.cnblogs.com/newneul/p/8544369.html  7.题目要求:在ICP程序中,将空间点也作为优化变量考虑进来 ...

  3. 视觉slam学习之路(一)看高翔十四讲所遇到的问题

      目前实验室做机器人,主要分三个方向,定位导航,建图,图像识别,之前做的也是做了下Qt上位机,后面又弄红外识别,因为这学期上课也没怎么花时间在项目,然后导师让我们确定一个方向来,便于以后发论文什么. ...

  4. 浅读《视觉SLAM十四讲:从理论到实践》--操作1--初识SLAM

    下载<视觉SLAM十四讲:从理论到实践>源码:https://github.com/gaoxiang12/slambook 第二讲:初识SLAM 2.4.2 Hello SLAM(书本P2 ...

  5. 高翔《视觉SLAM十四讲》从理论到实践

    目录 第1讲 前言:本书讲什么:如何使用本书: 第2讲 初始SLAM:引子-小萝卜的例子:经典视觉SLAM框架:SLAM问题的数学表述:实践-编程基础: 第3讲 三维空间刚体运动 旋转矩阵:实践-Ei ...

  6. 高博-《视觉SLAM十四讲》

    0 讲座 (1)SLAM定义 对比雷达传感器和视觉传感器的优缺点(主要介绍视觉SLAM) 单目:不知道尺度信息 双目:知道尺度信息,但测量范围根据预定的基线相关 RGBD:知道深度信息,但是深度信息对 ...

  7. 《视觉SLAM十四讲》第2讲

    目录 一 视觉SLAM中的传感器 二 经典视觉SLAM框架 三 SLAM问题的数学表述 注:原创不易,转载请务必注明原作者和出处,感谢支持! 本讲主要内容: (1) 视觉SLAM中的传感器 (2) 经 ...

  8. 《视觉SLAM十四讲》第1讲

    目录 一 视觉SLAM 注:原创不易,转载请务必注明原作者和出处,感谢支持! 一 视觉SLAM 什么是视觉SLAM? SLAM是Simultaneous Localization and Mappin ...

  9. 视觉SLAM十四讲:从理论到实践 两版 PDF和源码

    视觉SLAM十四讲:从理论到实践 第一版电子版PDF 链接:https://pan.baidu.com/s/1SuuSpavo_fj7xqTYtgHBfw提取码:lr4t 源码github链接:htt ...

  10. 《SLAM十四讲》个人学习知识点梳理

    0.引言 从六月末到八月初大概一个月时间一直在啃SLAM十四讲[1]这本书,这本书把SLAM中涉及的基本知识点都涵盖了,所以在这里做一个复习,对这本书自己学到的东西做一个梳理. 书本地址:http:/ ...

随机推荐

  1. shell文件报错syntax error near unexpected token '$'\r''

    本来跑的好好得一个文件,在windows下修改了,然后移植到linux就报错了. 找了一圈以下是解决方案: 这种情况发生的原因是因为你所处理的文件换行符是dos格式的"\r\n" ...

  2. yum的$releaser与$basearch

    最近配置centos 的yum 源时,想要配置一个通配的yum源,注意到发行的网络yum源的url地址中通常有两个变量 https://vault.centos.org/$releaser/cloud ...

  3. 重学c#系列——逆变和协变[二十四]

    前言 简单整理一下逆变和协变. 正文 什么是逆变和协变呢? 首先逆变和协变都是术语. 协变表示能够使用比原始指定的派生类型的派生程度更大的类型. 逆变表示能够使用比原始指定的派生类型的派生程度更小的类 ...

  4. Visual Studio高版本 ArcObject for .Net 低版本

    在基于ArcGIS的开发中,经常会存在Visual Studio版本高,ArcObject for .Net 版本低的问题.例如Visual Studio 2015的环境下,安装ArcObject f ...

  5. 【Java EE】Day12 XML、约束(DTD、Schema)、解析方式、Jsoup、选择器(Selector、XPath)

    一.XML介绍 1.概述 Extensible Markup Language--可扩展标记语言 标记语言 :标签构成 可扩展:可以自定义标签 2.功能 存储数据 作为配置文件使用 作为数据载体在网络 ...

  6. USB口3A限流保护芯片。带短路保护

    一般说明 PW1503是超低RDS(ON)开关,具有可编程的电流限制,以保护电源源于过电流和短路情况.它具有超温保护以及反向闭锁功能. PW1503采用薄型(1毫米)5针薄型SOT封装,提供可调版本. ...

  7. [seaborn] seaborn学习笔记8-避免过度绘图Avoid Overplotting

    8 避免过度绘图Avoid Overplotting(代码下载) 过度绘图是散点图及几天常见图表中最常见的问题之一.如下图所示当数据集很大时,散点图的点会重叠,使得图形变得不可读.在这篇文章中,提出了 ...

  8. Hadoop详解(09) - Hadoop新特性

    Hadoop详解(09) - Hadoop新特性 Hadoop2.x新特性 远程主机之间的文件复制 scp实现两个远程主机之间的文件复制 推 push:scp -r hello.txt root@ha ...

  9. 真正“搞”懂HTTP协议07之body的玩法(实践篇)

    我真没想到这篇文章竟然写了将近一个月,一方面我在写这篇文章的时候阳了,所以将近有两周没干活,另外一方面,我发现在写基于Node的HTTP的demo的时候,我不会Node,所以我又要一边学学Node,一 ...

  10. 从开发属于你自己的第一个 Python 库,做一名真正的程序员「双语版」

    你好,我是悦创.之前我在 CSDN 编写了一篇开发 Python 库的教程,有人加我提问到的一些问题,我来更新一下这篇文章:https://blog.csdn.net/qq_33254766/arti ...