《SLAM 14讲》第十讲：后端优化
因为《SLAM 14讲》上第十讲代码有点复杂，所以按照书上把解析参数的那个类去掉了，写了一个简单的版本。
最新的 g2o 和《SLAM 14讲》里面用的有点不同，有些转化函数输入的不再是普通的指针而是unique_ptr，需要在配置options的函数改一下写法。详见 3.求解BA问题的主程序：
g2o_bunddle_adjustment.cpp中的 setSolverOptions 函数 。
和书上一样用了这个数据集：http://grail.cs.washington.edu/projects/bal/
1. 一些工具函数
这部分直接从书上的代码copy的。。
tools.h
#ifndef TOOLS_H
#define TOOLS_H
#include <algorithm>
#include <cmath>
#include <limits>
template<typename T>
inline T DotProduct(const T x[3], const T y[3]) {
return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]);
}
template<typename T>
inline void CrossProduct(const T x[3], const T y[3], T result[3]){
result[0] = x[1] * y[2] - x[2] * y[1];
result[1] = x[2] * y[0] - x[0] * y[2];
result[2] = x[0] * y[1] - x[1] * y[0];
}
template<typename T>
inline void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) {
const T theta2 = DotProduct(angle_axis, angle_axis);
if (theta2 > T(std::numeric_limits<double>::epsilon())) {
const T theta = sqrt(theta2);
const T costheta = cos(theta);
const T sintheta = sin(theta);
const T theta_inverse = 1.0 / theta;
const T w[3] = { angle_axis[0] * theta_inverse,
angle_axis[1] * theta_inverse,
angle_axis[2] * theta_inverse };
T w_cross_pt[3];
CrossProduct(w, pt, w_cross_pt);
const T tmp = DotProduct(w, pt) * (T(1.0) - costheta);
result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp;
result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;
result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;
} else {
T w_cross_pt[3];
CrossProduct(angle_axis, pt, w_cross_pt);
result[0] = pt[0] + w_cross_pt[0];
result[1] = pt[1] + w_cross_pt[1];
result[2] = pt[2] + w_cross_pt[2];
}
}
template<typename T>
inline bool CamProjectionWithDistortion(const T* camera, const T* point, T* predictions){
T p[3];
AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] are the translation
p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];
// Compute the center fo distortion
T xp = -p[0]/p[2];
T yp = -p[1]/p[2];
// Apply second and fourth order radial distortion
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;
}
#endif
2. g2o的顶点和边
这部分计算Jacobians矩阵也用了ceres的自动求导，其实留空也可以，留空就是直接用g2o的数值求导但，是时间上会慢一点。
g2o_ba_type.h
#ifndef G2O_BA_TYPE_H
#define G2O_BA_TYPE_H
#include <iostream>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include "ceres/ceres.h"
typedef Eigen::Matrix<double, 9, 1> Vector9d;
using Eigen::Vector3d;
using Eigen::Vector2d;
#include "tools.h"
class VertexCameraBAL : public g2o::BaseVertex<9, Vector9d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexCameraBAL() {}
virtual bool read ( istream& is ) {return false;}
virtual bool write ( ostream& os ) const {return false;}
virtual void setToOriginImpl() {}
virtual void oplusImpl ( const double* update ) {
Vector9d::ConstMapType v ( update );
_estimate += v;
}
};
class VertexPointBAL : public g2o::BaseVertex<3, Vector3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexPointBAL() {}
virtual bool read ( istream& is ) {return false;}
virtual bool write ( ostream& os ) const {return false;}
virtual void setToOriginImpl() {}
virtual void oplusImpl ( const double* update ) {
Vector3d::ConstMapType v ( update );
_estimate += v;
}
};
class EdgeObservationBAL : public g2o::BaseBinaryEdge<2, Vector2d, VertexCameraBAL, VertexPointBAL> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeObservationBAL() {}
virtual bool read ( istream& is ) {return false;}
virtual bool write ( ostream& os ) const {return false;}
virtual void computeError() override {
const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex( 0 ) );
const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex( 1 ) );
( *this ) ( cam->estimate().data(), point->estimate().data(), _error.data() );
}
template<typename T>
bool operator() ( const T* camera, const T* point, T* residuals ) const {
T predictions[2];
CamProjectionWithDistortion ( camera, point, predictions );
residuals[0] = predictions[0] - T ( measurement() ( 0 ) );
residuals[1] = predictions[1] - T ( measurement() ( 1 ) );
return true;
}
virtual void linearizeOplus() override {
const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );
typedef ceres::internal::AutoDiff<EdgeObservationBAL, double, VertexCameraBAL::Dimension, VertexPointBAL::Dimension> BalAutoDiff;
Eigen::Matrix<double, Dimension, VertexCameraBAL::Dimension, Eigen::RowMajor> dError_dCamera;
Eigen::Matrix<double, Dimension, VertexPointBAL::Dimension, Eigen::RowMajor> dError_dPoint;
double *parameters[] = { const_cast<double*> ( cam->estimate().data() ), const_cast<double*> ( point->estimate().data() ) };
double *jacobians[] = { dError_dCamera.data(), dError_dPoint.data() };
double value[Dimension];
bool diffState = BalAutoDiff::Differentiate ( *this, parameters, Dimension, value, jacobians );
// copy over the Jacobians (convert row-major -> column-major)
if ( diffState )
{
_jacobianOplusXi = dError_dCamera;
_jacobianOplusXj = dError_dPoint;
}
else
{
assert ( 0 && "Error while differentiating" );
_jacobianOplusXi.setZero();
_jacobianOplusXj.setZero();
}
}
};
#endif
3. 求解BA问题
读取数据集的类：
BALProblem.h
#ifndef BALPROBLEM_H
#define BALPROBLEM_H
#include <iostream>
#include <cstdio>
#include <string>
#include <fstream>
#include "g2o_ba_type.h"
#include "tools.h"
using namespace std;
class BALProblem {
public:
BALProblem(const string& filename);
~BALProblem() {
delete [] point_index_;
delete [] camera_index_;
delete [] observations_;
delete [] parameters_;
}
void WriteToPLYFile(const string& filename) const;
void WriteToFile(const string& filename) const;
int camera_block_size() const {return 9;}
int point_block_size() const {return 3;}
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_;}
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_;}
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();
}
double* mutable_cameras() {return parameters_;}
double* mutable_points() {return parameters_ + camera_block_size() * num_cameras_;}
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();
}
void Normalize();
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_;
string filename_;
int* point_index_;
int* camera_index_;
double* observations_;
double* parameters_;
};
#endif
BALProblem.cpp
#include "BALProblem.h"
#include "tools.h"
double Median(std::vector<double>* data){
int n = data->size();
std::vector<double>::iterator mid_point = data->begin() + n/2;
std::nth_element(data->begin(),mid_point,data->end());
return *mid_point;
}
BALProblem::BALProblem(const string& filename) {
filename_ = filename;
ifstream fin;
fin.open(filename_);
if (!fin) {
cout << "Error opening the file!\n";
return;
}
fin >> num_cameras_;
fin >> num_points_;
fin >> num_observations_;
cout << "Header: he number of cameras is: " << num_cameras_ << ", ";
cout << "the number of points is: " << num_points_ << ", ";
cout << "the number of observations is: " << num_observations_ << ".\n";
point_index_ = new int[num_observations_];
camera_index_ = new int[num_observations_];
observations_ = new double[2 * num_observations_];
num_parameters_ = camera_block_size()*num_cameras_ + point_block_size()*num_points_;
parameters_ = new double[num_parameters_];
for (int i=0; i<num_observations_; i++) {
fin >> camera_index_[i];
fin >> point_index_[i];
fin >> observations_[2*i];
fin >> observations_[2*i + 1];
}
for (int j=0; j<num_parameters_; j++) {
fin >> parameters_[j];
}
fin.close();
}
// Write the problem to a PLY file for inspection in Meshlab or CloudCompare
void BALProblem::WriteToPLYFile(const std::string& filename)const{
std::ofstream of(filename.c_str());
of<< "ply"
<< '\n' << "format ascii 1.0"
<< '\n' << "element vertex " << num_cameras_ + num_points_
<< '\n' << "property float x"
<< '\n' << "property float y"
<< '\n' << "property float z"
<< '\n' << "property uchar red"
<< '\n' << "property uchar green"
<< '\n' << "property uchar blue"
<< '\n' << "end_header" << std::endl;
// Export extrinsic data (i.e. camera centers) as green points.
double angle_axis[3];
double center[3];
for(int i = 0; i < num_cameras(); ++i){
const double* camera = cameras() + camera_block_size() * i;
CameraToAngelAxisAndCenter(camera, angle_axis, center);
of << center[0] << ' ' << center[1] << ' ' << center[2]
<< "0 255 0" << '\n';
}
// Export the structure (i.e. 3D Points) as white points.
const double* points = parameters_ + camera_block_size() * num_cameras_;
for(int i = 0; i < num_points(); ++i){
const double* point = points + i * point_block_size();
for(int j = 0; j < point_block_size(); ++j){
of << point[j] << ' ';
}
of << "255 255 255\n";
}
of.close();
}
void BALProblem::WriteToFile(const string& filename) const {
ofstream fout;
fout.open(filename);
fout << num_cameras_ << " " << num_points_ << " " << num_observations_;
fout << endl;
for (int i=0; i<num_observations_; i++) {
fout << camera_index_[i] << " " << point_index_[i] << " " << observations_[2*i] << " " << observations_[2*i+1] << endl;
}
for (int i=0; i<num_parameters_; i++) {
fout << parameters_[i];
}
fout.close();
}
void BALProblem::CameraToAngelAxisAndCenter(const double* camera,
double* angle_axis,
double* center) const{
Eigen::Map<Vector3d> angle_axis_ref(angle_axis);
angle_axis_ref = Eigen::Map<const Vector3d>(camera);
// c = -R't
Eigen::Vector3d inverse_rotation = -angle_axis_ref;
AngleAxisRotatePoint(inverse_rotation.data(),
camera + camera_block_size() - 6,
center);
Eigen::Map<Vector3d>(center) *= -1.0;
}
void BALProblem::AngleAxisAndCenterToCamera(const double* angle_axis,
const double* center,
double* camera) const{
Eigen::Map<const Vector3d> angle_axis_ref(angle_axis);
Eigen::Map<Vector3d> cam(camera); cam = angle_axis_ref;
// t = -R * c
AngleAxisRotatePoint(angle_axis,center,camera + camera_block_size() - 6);
Eigen::Map<Vector3d>(camera + camera_block_size() - 6) *= -1.0;
}
void BALProblem::Normalize(){
// Compute the marginal median of the geometry
std::vector<double> tmp(num_points_);
Eigen::Vector3d median;
double* points = mutable_points();
for(int i = 0; i < 3; ++i){
for(int j = 0; j < num_points_; ++j){
tmp[j] = points[3 * j + i];
}
median(i) = Median(&tmp);
}
for(int i = 0; i < num_points_; ++i){
Eigen::Map<Vector3d> point(points + 3 * i, 3);
tmp[i] = (point - median).lpNorm<1>();
}
const double median_absolute_deviation = Median(&tmp);
// Scale so that the median absolute deviation of the resulting
// reconstruction is 100
const double scale = 100.0 / median_absolute_deviation;
// X = scale * (X - median)
for(int i = 0; i < num_points_; ++i){
Eigen::Map<Vector3d> point(points + 3 * i, 3);
point = scale * (point - median);
}
double* cameras = mutable_cameras();
double angle_axis[3];
double center[3];
for(int i = 0; i < num_cameras_ ; ++i){
double* camera = cameras + camera_block_size() * i;
CameraToAngelAxisAndCenter(camera, angle_axis, center);
// center = scale * (center - median)
Eigen::Map<Vector3d> cen(center);
cen = scale * (cen - median);
AngleAxisAndCenterToCamera(angle_axis, center,camera);
}
}
求解BA问题的主程序：
g2o_bunddle_adjustment.cpp
#include "BALProblem.h"
#include "g2o_ba_type.h"
#include "tools.h"
#include <iostream>
#include "g2o/stuff/sampler.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/core/batch_stats.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/optimization_algorithm_dogleg.h"
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/solvers/eigen/linear_solver_eigen.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/types/sba/types_six_dof_expmap.h"
#include "g2o/solvers/structure_only/structure_only_solver.h"
typedef Eigen::Map<const Vector3d> ConstVecRef;
typedef Eigen::Map<Vector3d> VecRef;
typedef g2o::BlockSolver<g2o::BlockSolverTraits<9,3> > BalBlockSolver;
void BuildProblem(const BALProblem* bal_problem, g2o::SparseOptimizer* optimizer) {
const int num_cameras = bal_problem->num_cameras();
const int num_points = bal_problem->num_points();
const int num_observations = bal_problem->num_observations();
const int camera_block_size = bal_problem->camera_block_size();
const int point_block_size = bal_problem->point_block_size();
const double* cameras = bal_problem->cameras();
for (int i=0; i<num_cameras; i++) {
Eigen::Map<const Eigen::Matrix<double, 9, 1>> tempcam(cameras + i*camera_block_size);
VertexCameraBAL* pcam = new VertexCameraBAL();
pcam->setId(i);
pcam->setEstimate(tempcam);
optimizer->addVertex(pcam);
}
const double* points = bal_problem->points();
for (int i=0; i<num_points; i++) {
ConstVecRef temppoi(points + i*point_block_size);
VertexPointBAL* ppoint = new VertexPointBAL();
ppoint->setId(num_cameras + i);
ppoint->setEstimate(temppoi);
ppoint->setMarginalized(true);
optimizer->addVertex(ppoint);
}
const double* observations = bal_problem->observations();
for (int i=0; i<num_observations; i++) {
const int cameraid = bal_problem->camera_index()[i];
const int pointid = bal_problem->point_index()[i] + num_cameras;
EdgeObservationBAL* pedge = new EdgeObservationBAL();
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
rk->setDelta(1.0);
pedge->setRobustKernel(rk);
pedge->setVertex(0, dynamic_cast<VertexCameraBAL*>(optimizer->vertex(cameraid)));
pedge->setVertex(1, dynamic_cast<VertexPointBAL*>(optimizer->vertex(pointid)));
pedge->setInformation(Eigen::Matrix2d::Identity());
pedge->setMeasurement(Eigen::Vector2d(observations[2*i],observations[2*i+1]));
optimizer->addEdge(pedge);
}
}
void WriteToBALProblem(BALProblem* bal_problem, g2o::SparseOptimizer* optimizer) {
const int num_cameras = bal_problem->num_cameras();
const int num_points = bal_problem->num_points();
const int num_observations = bal_problem->num_observations();
const int camera_block_size = bal_problem->camera_block_size();
const int point_block_size = bal_problem->point_block_size();
double* cameras = bal_problem->mutable_cameras();
for (int i=0; i<num_cameras; i++) {
VertexCameraBAL* pcam = dynamic_cast<VertexCameraBAL*> (optimizer->vertex(i));
Vector9d newcam = pcam->estimate();
memcpy(cameras + i*camera_block_size, newcam.data(), sizeof(double)*camera_block_size);
}
double* points = bal_problem->mutable_points();
for (int i=0; i<num_points; i++) {
VertexPointBAL* ppoint = dynamic_cast<VertexPointBAL*> (optimizer->vertex(i+num_cameras));
Vector3d newpoint = ppoint->estimate();
memcpy(points + i*point_block_size, newpoint.data(), sizeof(double)*point_block_size);
}
}
// 最新版的g2o的写法在注释中
void setSolverOptions(g2o::SparseOptimizer* optimizer) {
BalBlockSolver* solver_ptr;
g2o::LinearSolver<BalBlockSolver::PoseMatrixType>* linearSolver = new g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>();
dynamic_cast<g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>*>(linearSolver)->setBlockOrdering(true);
solver_ptr = new BalBlockSolver(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
/*
std::unique+ptr<g2o::LinearSolver<BalBlockSolver::PoseMatrixType>> linearSolver(new g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>());
dynamic_cast<g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>*>(linearSolver.get())->setBlockOrdering(true);
std::unique_ptr<BalBlockSolver> solver_ptr( new BalBlockSolver(std::move(linearSolver)) );
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr));
*/
optimizer->setAlgorithm(solver);
}
void SolveBALProblem(const string& filename) {
BALProblem bal_problem(filename);
// show some information here ...
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;
bal_problem.Normalize();
std::cout << "Normalization complete..." << std::endl;
bal_problem.WriteToPLYFile("initial.ply");
g2o::SparseOptimizer optimizer;
setSolverOptions(&optimizer);
BuildProblem(&bal_problem, &optimizer);
std::cout << "begin optimizaiton .."<< std::endl;
// perform the optimizaiton
optimizer.initializeOptimization();
optimizer.setVerbose(true);
optimizer.optimize(20);
std::cout << "optimization complete.. "<< std::endl;
// write the optimized data into BALProblem class
WriteToBALProblem(&bal_problem, &optimizer);
bal_problem.WriteToPLYFile("final.ply");
bal_problem.WriteToFile("final_data.txt");
}
int main(int argc, char** argv) {
if (argc != 2) {
cout << "usage: bundle_adjustment <path for datasest>";
return 1;
}
string filename = argv[1];
SolveBALProblem(filename);
return 0;
}
6. CMakeLists.txt
其中cmake_modules这个文件就是直接从《SLAM 14讲》这将的文件里copy的。
cmake_minimum_required(VERSION 2.8)
project(g2o_customBundle)
set(CMAKE_BUILD_TYPE "Release")
set( CMAKE_CXX_FLAGS "-O3" )
LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
find_package(Ceres REQUIRED)
Find_Package(G2O REQUIRED)
Find_Package(Eigen3 REQUIRED)
Find_Package(Cholmod REQUIRED)
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
SET(G2O_LIBS g2o_cli g2o_ext_freeglut_minimal g2o_simulator g2o_solver_slam2d_linear
g2o_types_icp g2o_types_slam2d g2o_types_sba g2o_types_slam3d g2o_core g2o_interface
g2o_solver_csparse g2o_solver_structure_only g2o_csparse_extension g2o_opengl_helper g2o_solver_dense
g2o_stuff g2o_types_sclam2d g2o_parser g2o_solver_pcg g2o_types_data g2o_types_sim3 cxsparse )
include_directories(${EIGEN3_INCLUDE_DIR} ${CHOLMOD_INCLUDE_DIR} ${CERES_INCLUDE_DIRS} )
message("find g2o libraries in: " ${G2O_INCLUDE_DIR})
message("find g2o lib in " ${G2O_LIBS})
message("find cholmod in " ${CHOLMOD_INCLUDE_DIR})
message("find ceres in " ${CERES_INCLUDE_DIR})
add_executable(${PROJECT_NAME} g2o_bunddle_adjustment.cpp)
add_library(BALPro BALProblem.cpp)
target_link_libraries(${PROJECT_NAME} ${G2O_LIBS} ${CHOLMOD_LIBRARIES} BALPro ${CERES_LIBRARIES})
5. 结果
用g2o数值求导的结果：
用ceres自动求导的结果：
优化前后的比较：
1513
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
