# 高翔Slambook第七讲代码解读（3d-2d位姿估计）

↑两张平面图

↑一张平面图+一张深度图 与一张平面图

```#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>

using namespace std;
using namespace cv;

void find_feature_matches (
const Mat& img_1, const Mat& img_2,
std::vector<KeyPoint>& keypoints_1,
std::vector<KeyPoint>& keypoints_2,
std::vector< DMatch >& matches );

// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );

const vector<Point3f> points_3d,
const vector<Point2f> points_2d,
const Mat& K,
Mat& R, Mat& t
);

int main ( int argc, char** argv )
{
if ( argc != 5 )
{
cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
return 1;
}
//-- 读取图像

vector<KeyPoint> keypoints_1, keypoints_2;
vector<DMatch> matches;
find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

// 建立3D点
Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
vector<Point3f> pts_3d;
vector<Point2f> pts_2d;
for ( DMatch m:matches )
{
ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
if ( d == 0 )   // bad depth
continue;
float dd = d/5000.0;
Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
}

cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;

Mat r, t;
solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
Mat R;
cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵

cout<<"R="<<endl<<R<<endl;
cout<<"t="<<endl<<t<<endl;

bundleAdjustment ( pts_3d, pts_2d, K, R, t );
return 0;
}```

```if ( argc != 5 )

{
cout<<"usage:pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
return 1;
}```

`Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );`

```// 建立3D点
vector<Point3f> pts_3d;
vector<Point2f> pts_2d;
for ( DMatch m:matches )
{
ushortd = d1.ptr<unsigned short>(int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
if ( d == 0 )  // bad depth

continue;

float dd = d/5000.0;
Point2dp1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
pts_3d.push_back ( Point3f ( p1.x*dd,p1.y*dd, dd ) );
pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
}```

`ushort d = d1.ptr<unsignedshort> (int( keypoints_1[m.queryIdx].pt.y )) [ int (keypoints_1[m.queryIdx].pt.x ) ];`

`ushort d = d1.ptr<unsignedshort> ( row )[ cloumn ];`

`ushort d = d1.ptr<unsignedshort> (int( keypoints_1[m.queryIdx].pt.y )) [ int (keypoints_1[m.queryIdx].pt.x ) ];`

```float dd = d/5000.0;
Point2dp1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
pts_2d.push_back ( keypoints_2[m.trainIdx].pt );```

```  // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
Mat r, t;
solvePnP (pts_3d, pts_2d, K, Mat(), r, t, false );
Mat R;
cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵```

```void bundleAdjustment (
const vector< Point3f > points_3d,
const vector< Point2f > points_2d,
const Mat& K,
Mat& R, Mat& t )
{
// 初始化g2o
typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;
// pose 维度为 6, landmark 维度为 3
Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();
// 线性方程求解器

//     Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
//     g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );

Block* solver_ptr = new Block( std::unique_ptr<Block::LinearSolverType>(linearSolver) );
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr) );

g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );

// vertex
g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
Eigen::Matrix3d R_mat;
R_mat <<
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 );
pose->setId ( 0 );
pose->setEstimate ( g2o::SE3Quat (
R_mat,
Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
) );

int index = 1;
for ( const Point3f p:points_3d )   // landmarks
{
g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
point->setId ( index++ );
point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
}

// parameter: camera intrinsics
g2o::CameraParameters* camera = new g2o::CameraParameters (
K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
);
camera->setId ( 0 );

// edges
index = 1;
for ( const Point2f p:points_2d )
{
g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
edge->setId ( index );
edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
edge->setVertex ( 1, pose );
edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
edge->setParameterId ( 0,0 );
edge->setInformation ( Eigen::Matrix2d::Identity() );
index++;
}

optimizer.setVerbose ( true );
optimizer.initializeOptimization();
optimizer.optimize ( 100 );
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="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
}```

Ubuntu 16.04安装ROS教程

Windows&Ubuntu16.04 双系统图文教程

0 条评论

• ### 【OpenCV教程】core 模块 - Mat - 基本图像容器

从真实世界中获取数字图像有很多方法，比如数码相机、扫描仪、CT或者磁共振成像。无论哪种方法，我们（人类）看到的是图像，而让数字设备来“看“的时候，都是在记录图像...

• ### 人眼疲倦检测开源算法

这次小白为小伙伴们带来了和生活相关性很大的一个系统。司机疲劳驾驶一直都是交通事故高发的主要原因。因此本项目的开发者计划通过视觉观测人的眼睛来判断被观察者是否存在...

• ### 三维重建新应用：精确还原凶杀现场细节！

SLAM和三维重建的落地应用一直走的比较艰难，之前华为mate30 发布三维重建应用时，还是很亮眼的，计算机视觉life 当时也第一时间进行了解读（看这里：华为...

• ### tkinter -- Event(1)

分别测试鼠标的 Relase 事件，只有当鼠标被 Relase 后移动才回产生 Relase 事件

• ### 币圈用户请小心！WebInjects可以清空你的加密货币账户

网络犯罪分子正在尝试各种各样的方法来将他们的“罪恶之手”伸向广大用户的加密货币钱包。研究人员发现，攻击者正在利用能够劫持浏览器（中间人攻击的一种）的恶意软件来劫...

• ### 1.7编程基础之字符串03:基因相关性

#include<iostream> #include<cstring> #include<cstdio> #include<cstdlib> using na...

• ### RPA应用于IT服务的十大场景

大量企业的实践表明，在IT项目的生命周期中，大约80%的时间与IT项目运营维护有关,而该阶段的投资仅占整个IT投资的20%，由此形成了典型的“轻服务、重技术”现...

• ### 《数据可视化基础》第七章：分布可视化：经验累积分布函数和q-q图

以下部分是基于《Fundamentals of Data Visualization》学习笔记，要是有兴趣的话，可以直接看原版书籍：https://serial...