SLAM后端:优化空间点、相机位姿与参数 - 《SLAM 14讲》第十讲
最近研究了一下SLAM后端优化的过程,主要是用BA模型进行优化,顺便也熟悉了如何用g2o实现非线性最小二乘的求解。本文主要介绍一下《SLAM 14讲》第十讲用g2o做BA优化的代码。
1. 开始的开始
物理和数学原理:
我们要优化的量为所有测得的路标在世界坐标系下的坐标,记为 ;所有相机的位姿,记为 ,其中 为旋转向量, 为平移向量;相机的内参,记为 ,按照书中我们把图像的中心定位像素坐标系的原点,故 不用考虑。
我们通过重投影误差最小来优化各个量:
其中 为第 个路标在第 个相机位姿处的投影坐标(以图像中心为原点); 为投影函数; 为第 个相机要优化的参数,是一个9维向量,前三位是旋转向量,中间三维是平移向量,后三维分别是 ; 为第 个路标世界坐标系下的坐标。
实现原理: 利用矩阵 的稀疏性,用g2o求解。
实现步骤:
- 设置顶点和边。顶点有两个,相机参数和路标位置;每条边连接相机参数和路标位置。
- 配置线性求解器。
- 设置迭代方式。
- 构造 Bundle Adjustment 问题,即往
g2o::SparseOptimizer*
中添加顶点和边。 - 开始求解。
自己简化了一下代码,见这篇文章:《SLAM 14讲》第十讲:后端优化。
2. 投影函数
projection.h
// camera : 9维向量
// [0-2] : 旋转向量
// [3-5] : 平移向量
// [6-8] : 相机参数, [6] 焦距, [7-8] k_1, k_2
// point : 3D 位置
// predictions : 重投影位置,图像中心为原点
template<typename T>
inline bool CamProjectionWithDistortion(const T* camera, const T* point, T* predictions){
// 这里是把 point 转化到相机坐标系下:P_c = R*P_w + t
T p[3];
// tools/rotation.h 里的通过旋转向量旋转点的函数
AngleAxisRotatePoint(camera, point, p);
p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];
// 计算畸变中心
// 这里就是计算 point 在归一化平面的坐标,取负数可能是因为书中生成的图像为倒立的。
// 感觉我们一般测得的数据这里应该不用取负数。不知道对不对。。
T xp = -p[0]/p[2];
T yp = -p[1]/p[2];
// 取得相机畸变系数 k_1, k_2
const T& l1 = camera[7];
const T& l2 = camera[8];
T r2 = xp*xp + yp*yp;
T distortion = T(1.0) + r2 * (l1 + l2 * r2);
const T& focal = camera[6];
predictions[0] = focal * distortion * xp;
predictions[1] = focal * distortion * yp;
return true;
}
3. 设置顶点
Eigen::Map
的用法,参见官网。
g2o_bal_class.h
// 这里的参数分别为顶点的维度、顶点的数据类型
class VertexCameraBAL : public g2o::BaseVertex<9,Eigen::VectorXd> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexCameraBAL() {}
// 读取、存储、设置初始值都直接留空就好了,下面也用不到这些
virtual bool read ( std::istream& is ) {return false;}
virtual bool write ( std::ostream& os ) const {return false;}
virtual void setToOriginImpl() {}
// 更新相机参数
virtual void oplusImpl ( const double* update ) {
// 把 double* 映射到 VectorXd,VertexCameraBAL::Dimension 即为输入的9
// 相当于 Eigen::Map<const VectorXd> ( update, VertexCameraBAL::Dimension )
Eigen::VectorXd::ConstMapType v ( update, VertexCameraBAL::Dimension );
_estimate += v;
}
};
class VertexPointBAL : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexPointBAL() {}
// 读取、存储、设置初始值都直接留空
virtual bool read ( std::istream& ) {return false;}
virtual bool write ( std::ostream& ) const {return false;}
virtual void setToOriginImpl() {}
// 更新路标位置
virtual void oplusImpl ( const double* update ) {
// 把 double* 映射到 Vector3d
Eigen::Vector3d::ConstMapType v ( update );
_estimate += v;
}
};
4. 设置边
stactic_cast, dynamic_cast, const_cast
:请参见static_cast、dynamic_cast、 const_cast-详解。
Eigen::Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols >
:其中 _Options
主要参数有 RowMajor, ColMajor
,区别存储时(体现为用data()
映射时) RowMajor
映射次序为一行一行的存储,ColMajor
为一列一列的存储。
ceres::internal::AutoDiff
: 在ceres
的autodiff.h
里有介绍。
g2o_bal_class.h
// 参数为误差的维度、测量值的类型、顶点0、顶点1
class EdgeObservationBAL : public g2o::BaseBinaryEdge<2, Eigen::Vector2d,VertexCameraBAL, VertexPointBAL> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeObservationBAL() {}
// 读取、存储直接留空
virtual bool read ( std::istream& /*is*/ ) {return false;}
virtual bool write ( std::ostream& /*os*/ ) const { return false;}
// override 为保留字,表示当前函数重写了基类的虚函数
virtual void computeError() override {
// static_cast 就是把 vertex ( i ) 强制转化为 const Vertex*
const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );
// 这里用到了下面的 operator()
// cam->estimate() 返回 Eigen::VectorXd,data() 就是把 VectorXd 映射到 double*
( *this ) ( cam->estimate().data(), point->estimate().data(), _error.data() );
}
// 计算残差 (residual) 即重投影误差,也为了下面能使用 ceres::AutoDiff
template<typename T>
bool operator() ( const T* camera, const T* point, T* residuals ) const {
T predictions[2];
// projection.h 里的函数
CamProjectionWithDistortion ( camera, point, predictions );
// measurement() 返回的是测量值 (_measurement) 类型为 Vector2d
residuals[0] = predictions[0] - T ( measurement() ( 0 ) );
residuals[1] = predictions[1] - T ( measurement() ( 1 ) );
return true;
}
virtual void linearizeOplus() override {
// 用 ceres 的 autodiff 直接求导,否则系统将直接用 g2o 里面的数值求导求 Jacobians
// 或者自己算一下 Jacobians ,但因为涉及12个优化量,所以式子会很复杂
// 获取两个顶点的指针
const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );
// 配置 autodiff,模板参数为:Functor(包含 operator() 的类或函数)、所有用到的数据类型、要求导的优化量的维度
// 这块找不到资料,所以就照着源代码解释了
typedef ceres::internal::AutoDiff<EdgeObservationBAL, double, VertexCameraBAL::Dimension, VertexPointBAL::Dimension> BalAutoDiff;
// 误差关于相机参数的导数和关于路标位置的 Jacobians
// 这里为 RowMajor,为了之后映射的时候顺序对应
Eigen::Matrix<double, Dimension, VertexCameraBAL::Dimension, Eigen::RowMajor> dError_dCamera;
Eigen::Matrix<double, Dimension, VertexPointBAL::Dimension, Eigen::RowMajor> dError_dPoint;
// 两个待求导变量的数据 放到 *parameters[] 里
double *parameters[] = { const_cast<double*> ( cam->estimate().data() ), const_cast<double*> ( point->estimate().data() ) };
// 两个 Jacobians的指针 放到 *jacobians[],感觉顺序和上面都是一一对应的
double *jacobians[] = { dError_dCamera.data(), dError_dPoint.data() };
// 用来存误差的数据,Dimension 即为在模板里设置的2
double value[Dimension];
// 开始自动求导,参数为:Functor,待求导变量数据,误差维度,误差数据,输出jacobians
bool diffState = BalAutoDiff::Differentiate ( *this, parameters, Dimension, value, jacobians );
// 复制 Jacobians 这里的 _jacobianOplusXi 是ColMajor的
if ( diffState ) {
_jacobianOplusXi = dError_dCamera;
_jacobianOplusXj = dError_dPoint;
}
else {
// 报错
assert ( 0 && "Error while differentiating" );
_jacobianOplusXi.setZero();
_jacobianOplusXj.setZero();
}
}
};
5. 构造 Bundle Adjustment 问题
鲁棒核函数Huber
:
g2o_bal_class.h
// BALPromblem储存所有的数据,比如相机位姿、路标位置...
// BundleParams储存所有配置参数,比如线性求解器类型、鲁棒函数...
void BuildProblem(const BALProblem* bal_problem, g2o::SparseOptimizer* optimizer, const BundleParams& params)
{
// 从BALProblem中读取一系列的数据
const int num_points = bal_problem->num_points();
const int num_cameras = bal_problem->num_cameras();
const int camera_block_size = bal_problem->camera_block_size();
const int point_block_size = bal_problem->point_block_size();
// 从BALProblem中读取相机的初始数据
const double* raw_cameras = bal_problem->cameras();
for(int i = 0; i < num_cameras; ++i) {
// ConstVectorRef是之前typedef的一个Eigen映射,把第i个相机的数据映射到VectorXd
ConstVectorRef temVecCamera(raw_cameras + camera_block_size * i,camera_block_size);
// 建立第i个相机顶点,添加初始估计值和index
VertexCameraBAL* pCamera = new VertexCameraBAL();
pCamera->setEstimate(temVecCamera);
pCamera->setId(i);
// 记得把顶点加到优化器里面
optimizer->addVertex(pCamera);
}
// 从BALProblem中读取路标的初始数据
const double* raw_points = bal_problem->points();
for(int j = 0; j < num_points; ++j) {
// 建立第j个路标顶点,添加初始估计值和index
// 每个顶点的index都是唯一的,故从相机顶点最后开始添加
ConstVectorRef temVecPoint(raw_points + point_block_size * j, point_block_size);
VertexPointBAL* pPoint = new VertexPointBAL();
pPoint->setEstimate(temVecPoint);
pPoint->setId(j + num_cameras);
// 记得把顶点加到优化器里面
// 这里要把点的稀疏性设为true
pPoint->setMarginalized(true);
optimizer->addVertex(pPoint);
}
// 设置边,从BALPromblem中读取观测点数据
const int num_observations = bal_problem->num_observations();
const double* observations = bal_problem->observations(); // 指向第一个观测点的指针
for(int i = 0; i < num_observations; ++i) {
// 建立从第i个观测点对应的相机与路标的边
EdgeObservationBAL* bal_edge = new EdgeObservationBAL();
// 从BALProblem里获取与第i个观测点对应的相机id和路标id
const int camera_id = bal_problem->camera_index()[i]; // 获得相机id
const int point_id = bal_problem->point_index()[i] + num_cameras; // 获得路标id
// 如果设置鲁棒性为true,则边中加入鲁棒函数Huber
if(params.robustify) {
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
rk->setDelta(1.0);
bal_edge->setRobustKernel(rk);
}
// 设置第i条边连接的相机顶点和路标顶点,即第i个观测点对应的相机id和路标id
// 路标id前面已经加过相机数了,故直接作为路标顶点id
bal_edge->setVertex(0,dynamic_cast<VertexCameraBAL*>(optimizer->vertex(camera_id)));
bal_edge->setVertex(1,dynamic_cast<VertexPointBAL*>(optimizer->vertex(point_id)));
// 设置信息矩阵,不太懂这是什么。
bal_edge->setInformation(Eigen::Matrix2d::Identity());
// 设置测量值
bal_edge->setMeasurement(Eigen::Vector2d(observations[2*i+0],observations[2*i + 1]));
// 把边加入到优化器中
optimizer->addEdge(bal_edge) ;
}
}
6. 配置线性求解器、设置迭代方式
g2o_bal_class.h
void SetSolverOptionsFromFlags(BALProblem* bal_problem, const BundleParams& params, g2o::SparseOptimizer* optimizer) {
BalBlockSolver* solver_ptr;
// 配置线性求解器
g2o::LinearSolver<BalBlockSolver::PoseMatrixType>* linearSolver = 0;
// 选择用那种线性方程求解器
if(params.linear_solver == "dense_schur" ) {
linearSolver = new g2o::LinearSolverDense<BalBlockSolver::PoseMatrixType>();
}
else if(params.linear_solver == "sparse_schur") {
linearSolver = new g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>();
dynamic_cast<g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>* >(linearSolver)->setBlockOrdering(true); // AMD ordering , only needed for sparse cholesky solver
}
solver_ptr = new BalBlockSolver(linearSolver);
// 设置迭代方式
g2o::OptimizationAlgorithmWithHessian* solver;
if(params.trust_region_strategy == "levenberg_marquardt") {
solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
}
else if(params.trust_region_strategy == "dogleg") {
solver = new g2o::OptimizationAlgorithmDogleg(solver_ptr);
}
else {
std::cout << "Please check your trust_region_strategy parameter again.."<< std::endl;
exit(EXIT_FAILURE);
}
optimizer->setAlgorithm(solver);
}
7. 开始求解
g2o_bal_class.h
void SolveProblem(const char* filename, const BundleParams& params) {
// 从文件读取BALProblem数据
BALProblem bal_problem(filename);
// 输出一些信息
std::cout << "bal problem file loaded..." << std::endl;
std::cout << "bal problem have " << bal_problem.num_cameras() << " cameras and "
<< bal_problem.num_points() << " points. " << std::endl;
std::cout << "Forming " << bal_problem.num_observations() << " observatoins. " << std::endl;
std::cout << "beginning problem..." << std::endl;
// 原来有一个添加噪声的代码,感觉这里其实不用也行
// 把数据归一化,是为了控制场景的大小,归一化函数在BALProblem.cpp中
bal_problem.Normalize();
// 存储原始的路标点和相机位姿,函数也在BALProblem.cpp中
if(!params.initial_ply.empty()){
bal_problem.WriteToPLYFile(params.initial_ply);
}
std::cout << "Normalization complete..." << std::endl;
// 配置线性求解器、迭代方式,构建问题
g2o::SparseOptimizer optimizer;
SetSolverOptionsFromFlags(&bal_problem, params, &optimizer);
BuildProblem(&bal_problem, &optimizer, params);
std::cout << "begin optimizaiton .."<< std::endl;
// 开始优化
optimizer.initializeOptimization();
optimizer.setVerbose(true);
optimizer.optimize(params.num_iterations); // 设置最大迭代次数
std::cout << "optimization complete.. "<< std::endl;
// 把优化后的数据写回bal_problem里面
WriteToBALProblem(&bal_problem, &optimizer);
// 把结果写进ply文件
if(!params.final_ply.empty()) {
bal_problem.WriteToPLYFile(params.final_ply);
}
}
8. 最后的最后
最后介绍一下一些参数、数据读取的类:
BundleParams.h
struct BundleParams {
public:
BundleParams(int argc, char** argv);
virtual ~BundleParams(){};
public:
string input;
string trust_region_strategy;
string linear_solver;
bool robustify; // 是否使用鲁棒函数
int num_iterations;
// 制造噪声用的
int random_seed;
double rotation_sigma;
double translation_sigma;
double point_sigma;
// 点云文件名
string initial_ply;
string final_ply;
CommandArgs arg;
};
BundleParams::BundleParams(int argc, char** argv)
{
arg.param("input", input, "", "file which will be processed");
arg.param("trust_region_strategy", trust_region_strategy, "levenberg_marquardt",
"Options are: levenberg_marquardt, dogleg.");
arg.param("linear_solver", linear_solver, "dense_schur", // iterative schur and cgnr(pcg) leave behind...
"Options are: sparse_schur, dense_schur");
arg.param("robustify", robustify, false, "Use a robust loss function");
arg.param("num_iterations", num_iterations,20, "Number of iterations.");
// 制造噪声数据
arg.param("rotation_sigma", rotation_sigma, 0.0, "Standard deviation of camera rotation "
"perturbation.");
arg.param("translation_sigma", translation_sigma,0.0, "translation perturbation.");
arg.param("point_sigma",point_sigma,0.0,"Standard deviation of the point "
"perturbation.");
arg.param("random_seed", random_seed, 38401,"Random seed used to set the state ");
// 点云文件名
arg.param("initial_ply", initial_ply,"initial.ply","Export the BAL file data as a PLY file.");
arg.param("final_ply", final_ply, "final.ply", "Export the refined BAL file data as a PLY");
arg.parseArgs(argc, argv);
}
BALProblem.h
class BALProblem {
public:
// 从文件读取数据
explicit BALProblem(const std::string& filename, bool use_quaternions = false);
~BALProblem(){
delete[] point_index_;
delete[] camera_index_;
delete[] observations_;
delete[] parameters_;
}
// 把数据写入文件
void WriteToFile(const std::string& filename)const;
// 把数据写入点云文件
void WriteToPLYFile(const std::string& filename)const;
// 归一化函数
void Normalize();
// 添加噪声函数
void Perturb(const double rotation_sigma,
const double translation_sigma,
const double point_sigma);
// 返回相机和路标优化维度
int camera_block_size() const{ return use_quaternions_? 10 : 9; }
int point_block_size() const{ return 3; }
// 返回相机个数、路标个数、观测点个数、数据个数
// 数据结构为:相机个数*相机维度、路标个数*路标维度
// 相机前三维是旋转向量、中间三维是平移向量、后三维是f,k_1,k_2
int num_cameras() const{ return num_cameras_; }
int num_points() const{ return num_points_; }
int num_observations() const{ return num_observations_; }
int num_parameters() const{ return num_parameters_; }
// 返回第一个路标id的指针、第一个相机id的指针、第一个观测点数据的指针
const int* point_index() const{ return point_index_; }
const int* camera_index() const{ return camera_index_; }
const double* observations() const{ return observations_; }
// 返回第一个数据(相机数据+路标数据)的指针
const double* parameters() const{ return parameters_; }
// 返回相机数据的指针、路标数据的指针
const double* cameras() const{ return parameters_; }
const double* points() const{ return parameters_ + camera_block_size() * num_cameras_; }
// 返回可改变的相机数据、路标数据的指针
// 这里只是为了和之前不能改变的数据区别开来,更加的清晰
double* mutable_cameras() { return parameters_; }
double* mutable_points() { return parameters_ + camera_block_size() * num_cameras_; }
// 返回第i个相机、路标数据,可变或不可变的指针
double* mutable_camera_for_observation(int i) {
return mutable_cameras() + camera_index_[i] * camera_block_size();
}
double* mutable_point_for_observation(int i) {
return mutable_points() + point_index_[i] * point_block_size();
}
const double* camera_for_observation(int i)const {
return cameras() + camera_index_[i] * camera_block_size();
}
const double* point_for_observation(int i)const {
return points() + point_index_[i] * point_block_size();
}
private:
void CameraToAngelAxisAndCenter(const double* camera,
double* angle_axis,
double* center)const;
void AngleAxisAndCenterToCamera(const double* angle_axis,
const double* center,
double* camera)const;
int num_cameras_;
int num_points_;
int num_observations_;
int num_parameters_;
bool use_quaternions_;
// 路标id、相机id、观测点数据、所有数据
int* point_index_;
int* camera_index_;
double* observations_;
double* parameters_;
};
还有一个command_args.h
里面的类就不介绍了,和问题没有太大的关系,这个类的主要作用是解析输入的参数。
翻到一篇很好的文章:《视觉SLAM十四讲》中第十章代码中雅克比的具体形式推导。