很久没有更新 有些懈怠 争取这个月把这本书啃完
ICP主要用在3d-3d点的相机位姿估计问题上 主要分为SVD与非线性优化两种方法
SVD：
整体思路是在获取了两张图片匹配点的，带有深度信息的点后，对其进行一定的处理，通过化简目标函数，最后通过求解一个矩阵的SVD来求的R和t。具体的推导在书中有 不再赘述。
在编写代码的时候我基本没有看示例代码，按照数学推导自己编写的代码，不是很难。
svd的计算用到了
Eigen:: JacobiSVD<Eigen::Matrix<double,3,3>>(W, Eigen::ComputeFullU|Eigen::ComputeFullV)
函数，模板参数是要分解的矩阵类型。后面的参数表示分别计算方针U和V
参数还有ComputeThinU,ComputeThinV，区别是矩阵维数是否为动态的：
Thin unitaries are only available if your matrix type has a Dynamic number of columns (for example MatrixXf).
通过svd.matrixU()和svd.matrixU()即可得到U和V。
这里需要注意如何将常用的Eigen库Eigen::Martix3d转化为OpenCV的图像格式cv::Mat，这里用的是R = ( cv::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 ));
借助了cv::Mat_是Mat的子类，它可以直接通过重载<<运算符构造出匿名对象，很方便。Eigen库可以通过重载的()运算符直接访问具体的元素。
BA：
仍在是借助了图优化的内容，但是复杂了一些。重点是雅可比矩阵的计算。
先简单复习一下g2o的优化过程，我的其他文章已经写过了
首先需要定义线性方程求解器，然后定义矩阵块求解器，选择算法，然后创建优化对象。
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,3>> Block;// Implementation of a solver operating on the blocks of the Hessian 矩阵块求解器
Block::LinearSolverType* LinearSovler = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); //线性方程求解器 //需要在cmake里链接g2o_csparse_extension库
Block* BlockSolverPtr = new Block(std::unique_ptr<Block::LinearSolverType> (LinearSovler)); //构造函数创建矩阵块求解器对象（指针）
g2o::OptimizationAlgorithmLevenberg* Algorithm = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<g2o::Solver>(BlockSolverPtr));//BlockSolver是g2o::Solver的子类 共有继承下来的
g2o::SparseOptimizer Optimizer;
Optimizer.setAlgorithm(Algorithm);对于这个问题，目标函数是
相对于李代数最小化它的误差e(x)，需要对李代数进行求导，也就是李代数的扰动模型。
后文再细说为什么是这个形式
这里的顶点用的是自带的g2o::VertexSE3Expmap，注意需要在CMakeLists中添加g2o_types_sba，因为VertexSE3Expmap是在这个库中定义的，需要进行链接。
//添加顶点
g2o::VertexSE3Expmap* VertexPose = new g2o::VertexSE3Expmap(); //自带类 se3类型 包含R和t
VertexPose->setId(0);
Eigen::Matrix3d R_;
Eigen::Matrix3d _R = Eigen::Matrix3d::Identity() ;
R_ << R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ), R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ), R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
Eigen::Vector3d t_;
t_ << t.at<double> (0,0), t.at<double> (1,0), t.at<double> (2,0);
g2o::SE3Quat et(R_, t_);
VertexPose->setEstimate(et);
Optimizer.addVertex(VertexPose);这个类在进行setEstimate时可要先将R和t转化为四元数的形式。
接下来是边的定义，需要自己重写一个
class Edge3d : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>{//误差值的维度 类型 顶点类型 这里误差是p-exp(e)p' 三维向量
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//宏函数 重写内存分配函数
Edge3d(const Eigen::Vector3d& point) : _point(point){}; //传进来的是p'
virtual void computeError(){ //虚函数 指针交给父类管理 写入目标函数
const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap* >(_vertices[0]); //把顶点转换为g2o::VertexSE3Expmap类型 同时把内容传进来
_error = _measurement - pose->estimate().map(_point);//pose->estimate().map(_point)就是把p'坐标进行坐标变换 measurement是p
}
//这里只需要误差项关于相机位姿（李代数）的导数 也就是Xi雅可比
//https://blog.csdn.net/zhubaohua_bupt/article/details/74011005?ops_request_misc=&request_id=&biz_id=102&utm_term=%E7%9B%B8%E6%9C%BA%E4%BD%8D%E5%A7%BF%E4%BC%98%E5%8C%96%20%E9%9B%85%E5%8F%AF%E6%AF%94&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-0-74011005.142^v70^js_top,201^v4^add_ask&spm=1018.2226.3001.4187
virtual void linearizeOplus(){ //线性化 利用一阶泰勒展开近似 需要定义雅可比矩阵j
const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap* >(_vertices[0]); //把顶点转换为g2o::VertexSE3Expmap类型 同时把内容传进来
Eigen::Vector3d PointTrans = pose->estimate().map(_point);
double x = PointTrans[0];
double y = PointTrans[1];
double z = PointTrans[2];
_jacobianOplusXi(0,0) = 0;
_jacobianOplusXi(0,1) = -z;
_jacobianOplusXi(0,2) = y;
_jacobianOplusXi(0,3) = -1;
_jacobianOplusXi(0,4) = 0;
_jacobianOplusXi(0,5) = 0;
_jacobianOplusXi(1,0) = z;
_jacobianOplusXi(1,1) = 0;
_jacobianOplusXi(1,2) = -x;
_jacobianOplusXi(1,3) = 0;
_jacobianOplusXi(1,4) = -1;
_jacobianOplusXi(1,5) = 0;
_jacobianOplusXi(2,0) = -y;
_jacobianOplusXi(2,1) = x;
_jacobianOplusXi(2,2) = 0;
_jacobianOplusXi(2,3) = 0;
_jacobianOplusXi(2,4) = 0;
_jacobianOplusXi(2,5) = -1;
}
bool read( std::istream& in){}
bool write( std::ostream& out )const{}
protected:
Eigen::Vector3d _point;
};EIGEN_MAKE_ALIGNED_OPERATOR_NEW是Eigen中的宏函数，用于进行内存分配的。
computeError就是目标函数measurement是观测值，后者是预测值。
static_cast是一种类型转换 把储存的顶点类型转换为我们需要的VertexSE3Expmap
pose->estimate()是我们要的估计值，pose->estimate().map()其实就是对某个点进行坐标变换，pose->estimate().map(_point)就是对第二张图的点，利用迭代出的相继位姿进行变换，也就是
这里书中没有再给出雅可比矩阵的推导过程，具体可以参考这篇文章：
SLAM优化位姿时，误差函数的雅可比矩阵的推导。_zhubaohua_bupt的博客-CSDN博客_slam 雅可比矩阵
根据文章的推导，可以看出原本的雅可比矩阵分为三部分（书中进行了合并，分为了两部分）
第一部分是误差关于投影点的导数，对应BaseUnaryEdge中的_jacobianOplusXj，这部分不涉及复杂的扰动模型，归结到底就是相机归一化平面的模型。
第二部分是误差关于相机位姿的导数，对应BaseUnaryEdge中的_jacobianOplusXi，这里用到了扰动模型
回想3d-2d最小化重投影误差的时候，由于相机位姿和观测点是一起考虑的，所以分别计算了两部分雅可比，最终是2*6的矩阵。
但在3d-3d的优化过程中，其实是不必考虑相机内参部分的，所以ICP也可以用于激光SLAM领域。
由于不需要考虑关于投影点的雅可比，所以只需要在写代码时重写_jacobianOplusXi就可以了，是一个3*6的矩阵。这里需要将前三列与后三列调换位置，主要是g2o的规则不太相同。
在边的类写完之后，只需要将之前匹配好的点导入至各个边中就可以了，主要分为
setid, setvertex, setInformation, setMeasurement几个步骤
最后一个 setMeasurement 就会把值保存在computeError中的_ measurement中，一切就说通了。
优化完成后，通过访问顶点VertexPose->estimate就可以得到优化后的位姿，但他仍在是四元数的形式，可以通过类中的提供的方法to_homogeneous_matrix()变成李代数的形式。
最后附上计算结果：
********************************SVD********************************
[0.9984466389557263, 0.05557944836736908, 0.003903085598995937;
-0.05512774034557463, 0.9956276426496312, -0.07540908059596663;
-0.008077215015402317, 0.07507677477833613, 0.9971450428530838]
[0.0487365597401111;
0.0487365597401111;
0.0487365597401111]
iteration= 0 chi2= 0.500594 time= 4.0599e-05 cumTime= 4.0599e-05 edges= 65 schur= 0 lambda= 0.000596 levenbergIter= 1
iteration= 1 chi2= 0.500594 time= 1.0461e-05 cumTime= 5.106e-05 edges= 65 schur= 0 lambda= 0.000397 levenbergIter= 1
iteration= 2 chi2= 0.500594 time= 9.11e-06 cumTime= 6.017e-05 edges= 65 schur= 0 lambda= 0.000265 levenbergIter= 1
iteration= 3 chi2= 0.500594 time= 8.93e-06 cumTime= 6.91e-05 edges= 65 schur= 0 lambda= 0.000177 levenbergIter= 1
iteration= 4 chi2= 0.500594 time= 8.95e-06 cumTime= 7.805e-05 edges= 65 schur= 0 lambda= 0.000118 levenbergIter= 1
iteration= 5 chi2= 0.500594 time= 3.0891e-05 cumTime= 0.000108941 edges= 65 schur= 0 lambda= 4241247602600.487305 levenbergIter= 10
********************************BA********************************
[0.9984466389541541, 0.05557944839565528, 0.003903085598318479;
-0.05512774037632337, 0.9956276426712256, -0.07540908028837792;
-0.008077214999849563, 0.07507677447102423, 0.9971450428763476]
[0.04873655974105028;
0.08215334050889304;
-0.02880827951251929]这里仍在是用SVD的结果作为BA的输入，我试着用单位阵作为R的初始迭代值，基本没有偏差，依然很准确。
最后附上源码：
#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/imgcodecs.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <g2o/core/block_solver.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/types/slam3d/vertex_pointxyz.h>
#include <g2o/types/sba/vertex_se3_expmap.h>
#include <g2o/core/base_unary_edge.h>
cv::Point2d pixel2cam ( const cv::Point2d& , const cv::Mat& );
void PoseEstimationSVD(const std::vector<cv::Point3d>& p1, const std::vector<cv::Point3d>& p2, cv::Mat& R, cv::Mat& t);
void PoseEstimation3d3dBA(cv::Mat& , cv::Mat& , const std::vector<cv::Point3d>& , const std::vector<cv::Point3d>& );
class Edge3d : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>{//误差值的维度 类型 顶点类型 这里误差是p-exp(e)p' 三维向量
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//宏函数 重写内存分配函数
Edge3d(const Eigen::Vector3d& point) : _point(point){}; //传进来的是p'
virtual void computeError(){ //虚函数 指针交给父类管理 写入目标函数
const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap* >(_vertices[0]); //把顶点转换为g2o::VertexSE3Expmap类型 同时把内容传进来
_error = _measurement - pose->estimate().map(_point);//pose->estimate().map(_point)就是把p'坐标进行坐标变换 measurement是p
}
//这里只需要误差项关于相机位姿（李代数）的导数 也就是Xi雅可比
//https://blog.csdn.net/zhubaohua_bupt/article/details/74011005?ops_request_misc=&request_id=&biz_id=102&utm_term=%E7%9B%B8%E6%9C%BA%E4%BD%8D%E5%A7%BF%E4%BC%98%E5%8C%96%20%E9%9B%85%E5%8F%AF%E6%AF%94&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-0-74011005.142^v70^js_top,201^v4^add_ask&spm=1018.2226.3001.4187
virtual void linearizeOplus(){ //线性化 利用一阶泰勒展开近似 需要定义雅可比矩阵j
const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap* >(_vertices[0]); //把顶点转换为g2o::VertexSE3Expmap类型 同时把内容传进来
Eigen::Vector3d PointTrans = pose->estimate().map(_point);
double x = PointTrans[0];
double y = PointTrans[1];
double z = PointTrans[2];
_jacobianOplusXi(0,0) = 0;
_jacobianOplusXi(0,1) = -z;
_jacobianOplusXi(0,2) = y;
_jacobianOplusXi(0,3) = -1;
_jacobianOplusXi(0,4) = 0;
_jacobianOplusXi(0,5) = 0;
_jacobianOplusXi(1,0) = z;
_jacobianOplusXi(1,1) = 0;
_jacobianOplusXi(1,2) = -x;
_jacobianOplusXi(1,3) = 0;
_jacobianOplusXi(1,4) = -1;
_jacobianOplusXi(1,5) = 0;
_jacobianOplusXi(2,0) = -y;
_jacobianOplusXi(2,1) = x;
_jacobianOplusXi(2,2) = 0;
_jacobianOplusXi(2,3) = 0;
_jacobianOplusXi(2,4) = 0;
_jacobianOplusXi(2,5) = -1;
}
bool read( std::istream& in){}
bool write( std::ostream& out )const{}
protected:
Eigen::Vector3d _point;
};
int main(){
cv::Mat img1 = cv::imread("../1.png",1);
cv::Mat img2 = cv::imread("../2.png",1);
cv::Mat depth1 = cv::imread("../1_depth.png");
cv::Mat depth2 = cv::imread("../2_depth.png");
std::vector<cv::KeyPoint> KeyPoint1,KeyPoint2;
cv::Mat descriptors1,descriptors2;
cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();//represented as vectors in a multidimensional space
cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();//初始化描述子 descriptor为指针
cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
detector->detect(img1,KeyPoint1);
detector->detect(img2,KeyPoint2);
descriptor->compute(img1,KeyPoint1,descriptors1);
descriptor->compute(img2,KeyPoint2,descriptors2);
std::vector<cv::DMatch> matches;
matcher->match(descriptors1,descriptors2,matches);
double min = 10000,max = 0;
min = min_element( matches.begin(), matches.end(), [](const cv::DMatch& m1, const cv::DMatch& m2) {return m1.distance<m2.distance;} )->distance;
max = max_element( matches.begin(), matches.end(), [](const cv::DMatch& m1, const cv::DMatch& m2) {return m1.distance<m2.distance;} )->distance;
std::vector<cv::DMatch> GoodMatches;
for(auto i = matches.begin(); i < matches.end(); i++){
if(i->distance <= std::max( 2*min, 30.0 )){
GoodMatches.push_back(*i);
}
}
std::vector<cv::Point3d> depthpoint1;
std::vector<cv::Point3d> depthpoint2;
cv::Mat K = (cv::Mat_<double>(3,3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);//相机内参 fx fy cx cy
for(auto i:GoodMatches){
unsigned short d1 = depth1.ptr<unsigned short>(int(KeyPoint1[i.queryIdx].pt.y))[int(KeyPoint1[i.queryIdx].pt.x)];
unsigned short d2 = depth2.ptr<unsigned short>(int(KeyPoint2[i.trainIdx].pt.y))[int(KeyPoint2[i.trainIdx].pt.x)];
if(d1 == 0 || d2 == 0) continue;
cv::Point2d pixel1(KeyPoint1[i.queryIdx].pt.x,KeyPoint1[i.queryIdx].pt.y);
cv::Point2d pixel2(KeyPoint2[i.trainIdx].pt.x,KeyPoint2[i.trainIdx].pt.y);
cv::Point3d temp1(pixel2cam(pixel1,K).x*d1/5000.0, pixel2cam(pixel1,K).y*d1/5000.0, d1/5000.0);
cv::Point3d temp2(pixel2cam(pixel2,K).x*d2/5000.0, pixel2cam(pixel2,K).y*d2/5000.0, d2/5000.0);
depthpoint1.push_back(temp1);
depthpoint2.push_back(temp2);
}
//std::cout << GoodMatches.size() << std::endl;
cv::Mat R,t;
PoseEstimationSVD(depthpoint1,depthpoint2,R,t);
std::cout << "********************************SVD********************************" << std::endl;
std::cout << R << std::endl;
std::cout << t << std::endl;
PoseEstimation3d3dBA(R,t,depthpoint1, depthpoint2);
std::cout << "********************************BA********************************" << std::endl;
std::cout << R << std::endl;
std::cout << t << std::endl;
return 0;
}
//像素平面到归一化平面变换 相差相机内参
cv::Point2d pixel2cam ( const cv::Point2d& p, const cv::Mat& K )
{
return cv::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 PoseEstimationSVD(const std::vector<cv::Point3d>& p1, const std::vector<cv::Point3d>& p2, cv::Mat& R, cv::Mat& t){
//计算质心
cv::Point3d weight1, weight2;
auto j = p2.begin();
int count = p1.size();
for(auto i = p1.begin(); i < p1.end() ; i++ ,j++){
weight1.x += i->x;
weight1.y += i->y;
weight1.z += i->z;
weight2.x += j->x;
weight2.y += j->y;
weight2.z += j->z;
}
weight1.x = weight1.x / count;
weight1.y = weight1.y / count;
weight1.z = weight1.z / count;
weight2.x = weight2.x / count;
weight2.y = weight2.y / count;
weight2.z = weight2.z / count;
//计算去质心坐标
std::vector<Eigen::Vector3d> q1, q2;
j = p2.begin();
for(auto i = p1.begin(); i < p1.end() ; i++ ,j++){
Eigen::Vector3d temp1(i->x - weight1.x, i->y - weight1.y, i->z - weight1.z);
Eigen::Vector3d temp2(j->x - weight2.x, j->y - weight2.y, j->z - weight2.z);
q1.push_back(temp1);
q2.push_back(temp2);
}
//计算W矩阵
Eigen::Matrix<double,3,3> W = Eigen::Matrix3d::Zero();
auto ii = q2.begin();
for(auto i = q1.begin(); i < q1.end() ; i++ ,ii++){
//W += Eigen::Vector3d(*i) * Eigen::Vector3d(*ii).transpose();
W += (*i) * (ii->transpose());
}
//std::cout << "W" << W <<std::endl;
//对M 进行SVD分解 得到UV
Eigen:: JacobiSVD<Eigen::Matrix<double,3,3>> svd(W, Eigen::ComputeFullU|Eigen::ComputeFullV);
auto U = svd.matrixU();
auto V = svd.matrixV();
//std::cout << "U" << U <<std::endl;
//std::cout << "V" << V <<std::endl;
Eigen::Matrix<double,3,3> R_ = U * V.transpose();
R = ( cv::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 t_ = Eigen::Vector3d(weight1.x, weight1.y, weight1.z) - R_ * Eigen::Vector3d(weight2.x, weight2.y, weight2.z);
t = (cv::Mat_<double> (3,1) << t_(0,0), t_(0,1), t_(0,2));
}
void PoseEstimation3d3dBA(cv::Mat& R, cv::Mat& t, const std::vector<cv::Point3d>& point1, const std::vector<cv::Point3d>& point2){
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,3>> Block;// Implementation of a solver operating on the blocks of the Hessian 矩阵块求解器
Block::LinearSolverType* LinearSovler = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); //线性方程求解器 //需要在cmake里链接g2o_csparse_extension库
Block* BlockSolverPtr = new Block(std::unique_ptr<Block::LinearSolverType> (LinearSovler)); //构造函数创建矩阵块求解器对象（指针）
g2o::OptimizationAlgorithmLevenberg* Algorithm = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<g2o::Solver>(BlockSolverPtr));//BlockSolver是g2o::Solver的子类 共有继承下来的
g2o::SparseOptimizer Optimizer;
Optimizer.setAlgorithm(Algorithm);
//添加顶点
g2o::VertexSE3Expmap* VertexPose = new g2o::VertexSE3Expmap(); //自带类 se3类型 包含R和t
VertexPose->setId(0);
Eigen::Matrix3d R_;
Eigen::Matrix3d _R = Eigen::Matrix3d::Identity() ;
R_ << R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ), R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ), R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
Eigen::Vector3d t_;
t_ << t.at<double> (0,0), t.at<double> (1,0), t.at<double> (2,0);
g2o::SE3Quat et(_R, t_);
VertexPose->setEstimate(et);
Optimizer.addVertex(VertexPose);
int index = 0;
auto j = point2.begin();
for(auto i = point1.begin(); i < point1.end(); i++, j++, index++){
Edge3d* Edge = new Edge3d(Eigen::Vector3d(j->x, j->y, j->z));
Edge->setId(index);
Edge->setVertex(0, Optimizer.vertex(0));
Edge->setInformation(Eigen::Matrix3d::Identity());
Edge->setMeasurement(Eigen::Vector3d(i->x, i->y, i->z));
Optimizer.addEdge(Edge);
}
Optimizer.setVerbose(1);
Optimizer.initializeOptimization();
Optimizer.optimize(10);
Eigen::Matrix4d T_ = VertexPose->estimate().to_homogeneous_matrix();
R = ( cv::Mat_<double> ( 3,3 ) << T_ ( 0,0 ), T_ ( 0,1 ), T_ ( 0,2 ), T_ ( 1,0 ), T_ ( 1,1 ), T_ ( 1,2 ), T_ ( 2,0 ), T_ ( 2,1 ), T_ ( 2,2 ));
t = (cv::Mat_<double>(3,1) << T_(0,3), T_(1,3), T_(2,3));
}
548
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
