07 手写 BA 优化
原理见笔记 《后端 1》
世界坐标系 --> 相机坐标系(外参)–> 归一化坐标 --> 去畸变 --> 像素坐标系(内参)
由此得到的估计值与实际观测到的像素坐标作差,得到二维误差项。
7.1 误差及雅可比矩阵
7.2 Ceres BA 优化
注意残差块的维度:待优化变量分成两块,相机内外参和空间点坐标;残差为像素理论值与实际值之差,为2维。
/****** 计算重投影误差项* 定义误差项
*/#ifndef SNAVELYREPROJECTIONERROR_H
#define SNAVELYREPROJECTIONERROR_H#include <iostream>
#include <ceres/ceres.h>
#include "common/projection.h"
#include "common/tools/rotation.h"// 误差项,仿函数
// observation_x, observation_y 实际值
class SnavelyReprojectionError
{
public:SnavelyReprojectionError(double observation_x, double observation_y) : observed_x(observation_x), observed_y(observation_y) {}template<typename T>// 待优化变量分为两块:camera 内外参 9 维; point 空间点 3 维// 残差为 2 维bool operator()(const T* const camera, const T* const point, T* residuals){T predictions[2];CamProjectionWithDistortion(camera, point, predictions); // 计算理论值residuals[0] = predictions[0] - observed_x;residuals[1] = predictions[1] - observed_y;return true;}static ceres::CostFunction* Create(const double observed_x, const double observed_y){// 使用自动求导,模板参数:误差类型,输出维度2,输入维度 9 + 3return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>(new SnavelyReprojectionError(observed_x, observed_y)));}private:double observed_x;double observed_y;
};#endif
7.3 g2o BA 优化
两个顶点,分别表示相机内外参(9维)、空间点(3维),边为二元边,连接两个顶点。
// 相机顶点 9 维
class VertexCameraBAL : public g2o::BaseVertex<9, Eigen::VectorXd>
{};// 空间点 3 维
class VertexPointBAL : public g2o::BaseVertex<3, Eigen::Vector3d>
{};// 定义二元边
// 误差维度2, 误差类型Eigen::Vector2d, 连接两种顶点
class EdgeObservationBAL : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, VertexCameraBAL, VertexPointBAL>
{};