看到这里忘了之前的 看完这里理解了之前的(第六讲g2o的时候) 记录一下(只截取g2o部分)
(代码是书上的 完整代码去github拿 我只是做注释解析)
/// vertex and edges used in g2o ba
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将double数组update转化成矩阵
update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
_estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
// 为什么要这样更新 第四讲中的位姿李代数扰动的更新是用左乘
// 这里的exp不是数学的exp函数 而是sophus的exp表示将矩阵转化为李群格式(即能与上面重置的_estimate能够相乘的格式)可以看一下下面的图复习一下
}
virtual bool read(istream &in) override {} //不更新
virtual bool write(ostream &out) const override {}
};
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}
//边:每个3d点在第二个相机中的投影 点的输入数据:pos 相机:K
virtual void computeError() override { //计算误差的函数 误差e = 预测值 - 观测值
const VertexPose *v = static_cast<VertexPose *> (_vertices[0]); //创建一个顶点对象
Sophus::SE3d T = v->estimate(); //返回当前的估计值 返回格式即_estimate的格式
Eigen::Vector3d pos_pixel = _K * (T * _pos3d); //3d点位置转为第二个相机的像素位置(未归一化)
pos_pixel /= pos_pixel[2]; //归一化处理
_error = _measurement - pos_pixel.head<2>(); //预测值-观测值(实际值)_measurement为这一次的预测值 这里这个<2>有点吓人 我去输出了一下(看后面的图)发现其实就是取前两个值作为矩阵 计算出来的error为一个2维向量 保存的是像素坐标的误差
}
virtual void linearizeOplus() override { //边的雅可比计算函数 计算每条边相对于顶点的雅可比
const VertexPose *v = static_cast<VertexPose *> (_vertices[0]); //创建一个顶点对象
Sophus::SE3d T = v->estimate(); //同上
Eigen::Vector3d pos_cam = T * _pos3d; //第二个相机坐标系下空间点的坐标即x'y'z'
double fx = _K(0, 0); //从相机内参矩阵中读各参数
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; // pose is 6, landmark is 3
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
// 梯度下降方法,可以从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(); // camera 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); //这个数据我有改过 其实就是迭代的次数
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(); //这就是真实的位姿了
}
![](https://i-blog.csdnimg.cn/blog_migrate/382f7529c930e10f57d2c50c571ca542.png)
图源:https://blog.csdn.net/HHB791829200/article/details/123738022
![](https://i-blog.csdnimg.cn/blog_migrate/153ececa7997fc1fccf95fcf1c4e5a50.png)