slam十四讲之第七讲pose_estimation_3d2d代码讲解，巨细
slam十四讲之第七讲pose_estimation_3d2d代码讲解，巨细
以下均在ubuntu20.04下进行，略微差别均已改正，希望大家一起学习和批评指正。
文章的顺序 1. 定义模板 2.调用定义的函数 3.实现函数功能 高斯牛顿 和 图优化g2o
建议对这个书公式一起看
巨详细的特征点匹配部分代码讲解在这里：https://blog.csdn.net/weixin_51326570/article/details/112839378
本文主要是 高斯牛顿 和 图优化g2o 部分
#include detector = FeatureDetector::create ( "ORB" );
// Ptr descriptor = DescriptorExtractor::create ( "ORB" );
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
//-- 第一步:检测 Oriented FAST 角点位置
detector->detect(img_1, keypoints_1);
detector->detect(img_2, keypoints_2);
//-- 第二步:根据角点位置计算 BRIEF 描述子
descriptor->compute(img_1, keypoints_1, descriptors_1);
descriptor->compute(img_2, keypoints_2, descriptors_2);
//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
vector<DMatch> match;
// BFMatcher matcher ( NORM_HAMMING );
matcher->match(descriptors_1, descriptors_2, match);
//-- 第四步:匹配点对筛选
double min_dist = 10000, max_dist = 0;
//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
for (int i = 0; i < descriptors_1.rows; i++) {
double dist = match[i].distance;
if (dist < min_dist) min_dist = dist;
if (dist > max_dist) max_dist = dist;
}
printf("-- Max dist : %f \n", max_dist);
printf("-- Min dist : %f \n", min_dist);
//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
for (int i = 0; i < descriptors_1.rows; i++) {
if (match[i].distance <= max(2 * min_dist, 30.0)) {
matches.push_back(match[i]);
}
}
}
Point2d pixel2cam(const Point2d &p, const Mat &K) {
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 bundleAdjustmentGaussNewton(
const VecVector3d &points_3d,
const VecVector2d &points_2d,
const Mat &K,
Sophus::SE3d &pose) {
typedef Eigen::Matrix<double, 6, 1> Vector6d;
const int iterations = 10;
double cost = 0, lastCost = 0;
double fx = K.at<double>(0, 0);
double fy = K.at<double>(1, 1);
double cx = K.at<double>(0, 2);
double cy = K.at<double>(1, 2);
for (int iter = 0; iter < iterations; iter++) {
Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
Vector6d b = Vector6d::Zero();
cost = 0;//整体误差
// compute cost
for (int i = 0; i < points_3d.size(); i++) {
Eigen::Vector3d pc = pose * points_3d[i];
double inv_z = 1.0 / pc[2];//z的倒数，也是逆运算
double inv_z2 = inv_z * inv_z;
Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);//su = KP(P为相机坐标) 这个就算把矩阵展开用最后一行消去s
Eigen::Vector2d e = points_2d[i] - proj;//计算观测与预测值的误差
cost += e.squaredNorm();//误差求和
Eigen::Matrix<double, 2, 6> J;// 写出最终求导之后的的(2*6)的矩阵 我不太理解，既然能直接写出来，前面花里胡哨的干嘛
J << -fx * inv_z,
0,
fx * pc[0] * inv_z2,
fx * pc[0] * pc[1] * inv_z2,
-fx - fx * pc[0] * pc[0] * inv_z2,
fx * pc[1] * inv_z,
0,
-fy * inv_z,
fy * pc[1] * inv_z,
fy + fy * pc[1] * pc[1] * inv_z2,
-fy * pc[0] * pc[1] * inv_z2,
-fy * pc[0] * inv_z;
H += J.transpose() * J; //求H
b += -J.transpose() * e; //求b
}
Vector6d dx;
dx = H.ldlt().solve(b);//Hx=b求解x ldlt求逆运算
if (isnan(dx[0])) { //判断结果存不存在
cout << "result is nan!" << endl;
break;
}
if (iter > 0 && cost >= lastCost) { //若误差增大，直接输出，不再迭代
cout << "cost: " << cost << ", last cost: " << lastCost << endl;
break;
}
pose = Sophus::SE3d::exp(dx) * pose;//不断更新位姿，用李群也就是旋转矩阵形式
lastCost = cost;
cout << "iteration " << iter << " cost=" << cout.precision(12) << cost << endl;
if (dx.norm() < 1e-6) { //设置一个迭代终止条件
// converge
break;
}
}
cout << "pose by g-n: \n" << pose.matrix() << endl; //就是 T
}
/// vertex and edges used in g2o ba 接下来是用G2o求解
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 EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> { //e是2维的，u一个，v一个
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}//进行初始化
virtual void computeError() override {
const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
Sophus::SE3d T = v->estimate();
Eigen::Vector3d pos_pixel = _K * (T * _pos3d);//p(像素左边)=k*T*Pc(相机坐标)
pos_pixel /= pos_pixel[2];//除以深度距离，可以认为是把z变成1
_error = _measurement - pos_pixel.head<2>();//观测值 - 计算值
}
virtual void linearizeOplus() override {
const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);//使顶点都是顶点类型的，上边定义过
Sophus::SE3d T = v->estimate();//带估计量
Eigen::Vector3d pos_cam = T * _pos3d;//世界坐标转相机坐标
//定义那个(2*6)的雅可比矩阵，这可能就是灵魂吧
double fx = _K(0, 0);//把K的第0行，第0列取出来给fx，我是真的细
double fy = _K(1, 1);
double cx = _K(0, 2);
double cy = _K(1, 2);
double X = pos_cam[0];
double Y = pos_cam[1];
double Z = pos_cam[2];
double Z2 = Z * Z;
_jacobianOplusXi//有公式之间写
<< -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
}
virtual bool read(istream &in) override {}
virtual bool write(ostream &out) const override {}
private:
Eigen::Vector3d _pos3d;//定义一个向量，后面用来存放东西
Eigen::Matrix3d _K;
};
void bundleAdjustmentG2O(
const VecVector3d &points_3d,
const VecVector2d &points_2d,
const Mat &K,
Sophus::SE3d &pose) {
// 构建图优化，先设定g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType; // 位姿最小维度 6， 观测最小维度 3
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型，区域求解器求出海塞矩阵在给线性求解器 HX=b 求x。进行更新估计值
// 梯度下降方法，可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));//给区域求解选个方法，也就是解决海塞矩阵的方法
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
// vertex
VertexPose *vertex_pose = new VertexPose(); // 相机下vertex_pose
vertex_pose->setId(0);//给个编号
vertex_pose->setEstimate(Sophus::SE3d());//给个估计类型
optimizer.addVertex(vertex_pose);//开始工作
// 定义一下K
Eigen::Matrix3d K_eigen;
K_eigen <<
K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);
// edges
int index = 1;
for (size_t i = 0; i < points_2d.size(); ++i) {
auto p2d = points_2d[i];//把点的信息装起来
auto p3d = points_3d[i];
EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);//输入的边
edge->setId(index);
edge->setVertex(0, vertex_pose);//连接的点
edge->setMeasurement(p2d);//测量值
edge->setInformation(Eigen::Matrix2d::Identity());//初始化 协方差矩阵的逆运算
optimizer.addEdge(edge);//把边放入优化器
index++;
}
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//计时
optimizer.setVerbose(true);
optimizer.initializeOptimization();
optimizer.optimize(10);//迭代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 << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl;//输出估计值
pose = vertex_pose->estimate();
}