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. 【RPC和Protobuf】之RPC入门

    一,概念 RPC:Remote procedure call(远程过程调用),分布式系统中不同节点之间流行的通信方式 服务端: 注: 1.执行下面的代码之后,会相应的启动一个tcp进程 C:\User ...

  2. 再见CMS

    观察网站最下方,根据备案号搜到这是个齐博CMS,然后百度就可以搜到齐博CMS漏洞了 然后开始利用 首先尝试了在用户信息修改处进行注入,发现好像想不通了,就在博客界面进行注入 Payload: 获取版本 ...

  3. Springboot 2.3.1配置拦截器遇到的坑

    1.多个配置类继承WebConfigureSupport或实现WebConfigure接口导致拦截器失效 2.拦截器中的bean无法正常注入,SpringBoot项目的Bean装配默认规则是根据App ...

  4. js-day02-综合案例ATM存款书写

     <script>                 // 1. 不断的弹出对话框         // 3. 金额的变量         let money = 100         w ...

  5. JavaEE Day09 JavaScript基础

    之前学了html.css两种静态资源 JavaScript是另一种静态资源,今日内容[重点]:JavaScript(是一门编程语言,2days)基础 一.JavaScript简介 1.概念 JavaS ...

  6. 【HBase】简介、结构、数据模型、快速入门部署、shell操作、架构原理、读写数据流程、数据刷写、压缩、分割、Phoenix、表的映射、与hive集成、优化

    一.简介 1.定义 分布式.可扩展.支持海量数据存储的NoSQL数据库 2.数据模型 2.1逻辑结构 2.2物理存储结构 2.3数据模型介绍 Name Space:相当于数据库,包含很多张表 Regi ...

  7. Gepetto:使用chatGPT来对函数功能进行分析并重命名变量的IDA插件

    最近OpenAI的chatGPT很火,chatGPT是一个大型的语言模型,能够生成人类语言的文本,主要用于对话式的问答和聊天,以及模拟人类的对话行为 有关chatGPT的介绍就不多赘述了,相关内容很多 ...

  8. JAVA学到方法写了一个四则运算计算器,请教一下有什么需要改进的

    package method; /* * 四则运算计算器 * */ import java.util.Scanner; public class Demo07 { public static void ...

  9. 前端知识之JS(javascirpt)

    目录 JS简介 JS基础 1.注释语法 2.引入JS的多种方式 3.结束符号 变量与常量 基本数据类型 1.数字类型(Number) 2.字符类型(string) 3.布尔类型(Boolean) 4. ...

  10. java中的动态绑定机制

    本文主要讲述java中的动态绑定机制. 老韩ppt关于动态绑定机制: 示例代码如下: public class DynamicBinding { public static void main(Str ...