Bundle Adjustment
问题1:文献阅读
为何说 Bundle Adjustment is slow 是不对的?
可以利用H矩阵的稀疏性质对BA整个过程进行加速。
BA 中有哪些需要注意参数化的地方?Pose 和Point 各有哪些参数化方式?有何优缺点。
- 参数化的地方:相机的内参外参,观测点的三维坐标,观测点的像素坐标
- Pose参数化方法:欧拉角(万向锁,表示方法不唯一) 四元数(存在单位向量的约束) 旋转矩阵 旋转向量
- Point参数化方法:齐次坐标
问题2:BAL-dataset
程序思路
- 数据读取部分,使用ifstream库按照文件数据格式直接读取即可
- G2O节点和边的类的构造可以直接按照书中第九讲的样例程序编写即可
- 将读取的3D点坐标和相机内外参数放入到对应的节点对象中,同时将他的指针保存在一个数组中,方便后续查找
- 将节点和彼此构成约束的边加入到优化器中开始优化即可
主要代码
#include <iostream>
#include <fstream>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/types/sba/types_sba.h>
#include <Eigen/Core>
#include <sophus/se3.hpp>
#include <g2o/core/block_solver.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <iostream>
#include <pangolin/pangolin.h>
#include <boost/format.hpp>
using namespace std;
using namespace Eigen;
using namespace Sophus;
typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> VecSE3;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVec3d;
struct Camera
{
Camera(){}
explicit Camera(double *data_addr)
{
R = SO3d::exp(Vector3d(data_addr[0], data_addr[1], data_addr[2]));
t = Vector3d(data_addr[3], data_addr[4], data_addr[5]);
f = data_addr[6];
k1 = data_addr[7];
k2 = data_addr[8];
}
/// 将估计值放入内存
void set_to(double *data_addr) {
auto r = R.log();
for (int i = 0; i < 3; ++i) data_addr[i] = r[i];
for (int i = 0; i < 3; ++i) data_addr[i + 3] = t[i];
data_addr[6] = f;
data_addr[7] = k1;
data_addr[8] = k2;
}
Sophus::SO3d R;
Vector3d t;
double f = 0;
double k1 = 0, k2 = 0;
};
// g2o vertex that use sophus::SE3 as pose
class VertexCamera : public g2o::BaseVertex<9, Camera> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
VertexCamera() {}
~VertexCamera() {}
bool read(std::istream &is) {return false;}
bool write(std::ostream &os) const {return false;}
virtual void setToOriginImpl()
{
_estimate.R = Sophus::SO3d();
_estimate.t = Vector3d(0, 0, 0);
_estimate.f = 0;
_estimate.k1 = 0;
_estimate.k2 = 0;
}
virtual void oplusImpl(const double *update_)
{
_estimate.R = SO3d::exp(Vector3d(update_[0], update_[1], update_[2])) * _estimate.R;
_estimate.t += Vector3d(update_[3], update_[4], update_[5]);
_estimate.f += update_[6];
_estimate.k1 += update_[7];
_estimate.k2 += update_[8];
}
// 根据估计值投影一个点
Vector2d project(const Vector3d &point) {
Vector3d pc = _estimate.R * point + _estimate.t;
pc = -pc / pc[2];
double r2 = pc.squaredNorm();
double distortion = 1.0 + r2 * (_estimate.k1 + _estimate.k2 * r2);
return Vector2d(_estimate.f * distortion * pc[0],
_estimate.f * distortion * pc[1]);
}
};
class VertexPoint : public g2o::BaseVertex<3, Vector3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexPoint() {}
virtual void setToOriginImpl() override {
_estimate = Vector3d(0, 0, 0);
}
virtual void oplusImpl(const double *update) override {
_estimate += Vector3d(update[0], update[1], update[2]);
}
virtual bool read(istream &in) {}
virtual bool write(ostream &out) const {}
};
class EdgeReproject : public g2o::BaseBinaryEdge<2, Vector2d, VertexPoint, VertexCamera> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeReproject() {}
~EdgeReproject() {}
virtual void computeError() override {
// TODO START YOUR CODE HERE
auto v0 = (VertexPoint * ) _vertices[0];
auto v1 = (VertexCamera * ) _vertices[1];
Vector2d p = v1->project(v0->estimate());
_error = p - _measurement;
// compute projection error ...
// END YOUR CODE HERE
}
// Let g2o compute jacobian for you
virtual bool read(istream &in) {return false;}
virtual bool write(ostream &out) const {return false;}
private:
};
struct edgeMsg
{
int cameraIndex;
int pointIndex;
Vector2d measure;
};
void WriteToPLYFile(const std::string &filename, VecVec3d points, vector<Camera> cameras);
string file_path = "/home/r213a/workspace/vslamlesson/L7/project/data/problem-52-64053-pre.txt";
int main(int argc, char **argv) {
// 读取数据
ifstream fin(file_path);
int cameraNum, pointNum, obNum;
if(!fin.is_open())
{
cout << "文件打开失败" << endl;
return 1;
}
fin >> cameraNum >> pointNum >> obNum;
vector<edgeMsg> edges;
VecVec3d points;
vector<Camera> cameras;
for(int i = 0; i < obNum; i++)
{
edgeMsg theEdge;
fin >> theEdge.cameraIndex >> theEdge.pointIndex >> theEdge.measure[0] >> theEdge.measure[1];
edges.push_back(theEdge);
}
for(int i = 0; i < cameraNum; i++)
{
double data[9];
fin >> data[0] >> data[1] >> data[2] >> data[3] >> data[4] >> data[5] >> data[6] >> data[7] >> data[8];
Camera cam(data);
cameras.push_back(cam);
}
for(int i = 0; i < pointNum; i++)
{
Vector3d point;
fin >> point[0] >> point[1] >> point[2];
points.push_back(point);
}
cout << "读取完成,共读取观测点数量:" << edges.size() << ", 相机数量:" << cameras.size() << ", 三维点数量:" << points.size() << endl;
WriteToPLYFile("origin.ply", points, cameras);
// 构建优化器
typedef g2o::BlockSolver<g2o::BlockSolverTraits<9, 3>> BlockSolverType;
//typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;
auto solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
vector<VertexPoint*> VertexPoints;
vector<VertexCamera*> VertexCameras;
//添加节点和边
//添加相机
for(int i = 0; i < cameras.size(); i++)
{
VertexCamera* p = new VertexCamera();
p->setEstimate(cameras[i]);
p->setId(i);
optimizer.addVertex(p);
VertexCameras.push_back(p);
}
//添加三维点
for(int i = 0; i < points.size(); i++)
{
VertexPoint* p = new VertexPoint();
p->setEstimate(points[i]);
p->setId(i + cameras.size());
p->setMarginalized(true);
optimizer.addVertex(p);
VertexPoints.push_back(p);
}
//添加边
for(int i = 0; i < edges.size(); i++)
{
EdgeReproject *edge = new EdgeReproject();
edge->setVertex(0, VertexPoints[edges[i].pointIndex]);
edge->setVertex(1, VertexCameras[edges[i].cameraIndex]);
edge->setMeasurement(edges[i].measure);
edge->setInformation(Matrix2d::Identity());
edge->setRobustKernel(new g2o::RobustKernelHuber()); //设置鲁棒核函数 此处为Huber核
optimizer.addEdge(edge);
}
cout << "开始优化" << endl;
optimizer.initializeOptimization(0);
optimizer.optimize(5);
for(int i = 0; i < points.size(); i++)
{
points[i] = VertexPoints[i]->estimate();
}
for(int i = 0; i < cameras.size(); i++)
{
cameras[i] = VertexCameras[i]->estimate();
}
WriteToPLYFile("final.ply", points, cameras);
return 0;
}
void WriteToPLYFile(const std::string &filename, VecVec3d points, vector<Camera> cameras){
std::ofstream of(filename.c_str());
of << "ply"
<< '\n' << "format ascii 1.0"
<< '\n' << "element vertex " << cameras.size() + points.size()
<< '\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 < cameras.size(); ++i) {
of << cameras[i].t[0] << ' ' << cameras[i].t[1] << ' ' << cameras[i].t[2]
<< " 0 255 0" << '\n';
}
// Export the structure (i.e. 3D Points) as white points.
for (int i = 0; i < points.size(); ++i) {
of << points[i][0] << ' ';
of << points[i][1] << ' ';
of << points[i][2] << ' ';
of << " 255 255 255\n";
}
of.close();
}
CMAKE
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIR})
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})
Find_Package(CSparse REQUIRED)
include_directories("/usr/include/suitesparse")
SET(G2O_LIBS g2o_csparse_extension g2o_stuff g2o_core cxsparse)
add_executable(directBA directBA.cpp)
target_link_libraries(directBA ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_TYPES_DATA} ${G2O_TYPES_SBA} )
target_link_libraries(directBA ${Pangolin_LIBRARIES})
target_link_libraries(directBA ${OpenCV_LIBRARIES})
add_executable(BAL BAL.cpp)
target_link_libraries(BAL ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_TYPES_DATA} ${G2O_TYPES_SBA} ${G2O_SOLVER_CSPARSE} ${G2O_LIBS})
target_link_libraries(BAL ${Pangolin_LIBRARIES})
运行结果
各个变量的雅可比是什么
问题2:实现
能否不以 [ x , y , z ] T [x, y, z]^T [x,y,z]T的形式参数化每个点?
可以只用深度参数化每个点
取 4x4 的 patch 好吗?取更大的 patch 好还是取小一点的 patch 好?
patch过大影响计算速度,过小会降低鲁棒性
从本题中,你看到直接法与特征点法在 BA 阶段有何不同?
特征点法的BA优化的目标是冲投影误差,而直接法由于没有特征点的配对关系,优化的目标是灰度值,相比于特征点法,直接法在链式法则求导的过程中多了一层灰度对像素坐标的偏导数
由于图像的差异,你可能需要鲁棒核函数,例如 Huber,此时 Huber 的阈值如何选取?
Huber 阈值的设定可从BA过程中的平均误差中确定量纲,在通过不断的调试确定阈值。
代码实现主要思想
- 边的误差计算:根据旋转矩阵和三维空间点P,将观测点投影到像素坐标系下,确定没有越界后,使用灰度值的差作为误差
- 插入节点和边:将读取到的三维位置和初始姿态分别放到对应的节点对象中,加入到优化器的同时还要将其指针存到数组中,确保能够获取到优化后的结果
- 优化结果的获取:将存到数组中的数据取出放到结果数组中
主要代码
// 计算误差
virtual void computeError() override {
// TODO START YOUR CODE HERE
auto v0 = (g2o::VertexSBAPointXYZ * ) _vertices[0];
auto v1 = (VertexSophus * ) _vertices[1];
Eigen::Vector3d p;
p = v1->estimate()*v0->estimate();
p[0] /= p[2];
p[1] /= p[2];
double px = p[0]*fx+cx;
double py = p[1]*fy+cy;
if(px-2 < 0 || py - 2 < 0 || px + 1 > targetImg.cols - 1 || py + 1 > targetImg.rows - 1)
{
this->setLevel(1);
for(int i = 0; i < 16; i++)
_error[i] = 0;
return;
}
for(int i = 0; i < 4; i++)
{
for(int j = 0; j < 4; j++)
{
double _px = px - 2 + i;
double _py = py - 2 + j;
_error[i+j*4] = origColor[i+j*4] - GetPixelValue(targetImg, _px, _py);
}
}
}
//加入节点和边
// 加入优化节点
for(int i = 0; i < points.size(); i++)
{
g2o::VertexSBAPointXYZ* p = new g2o::VertexSBAPointXYZ();
p->setId(i + poses.size());
p->setEstimate(points[i]);
p->setMarginalized(true);
optimizer.addVertex(p);
vertex_points.push_back(p);
}
for(int i = 0; i < poses.size(); i++)
{
VertexSophus *p = new VertexSophus();
p->setId(i);
p->setEstimate(poses[i]);
optimizer.addVertex(p);
vertex_sophus.push_back(p);
}
//加入优化的边
for(int i = 0; i < points.size(); i++)
{
for(int j = 0; j < poses.size(); j++)
{
EdgeDirectProjection *edge = new EdgeDirectProjection(color[i], images[j]);
edge->setVertex(0, vertex_points[i]);
edge->setVertex(1, vertex_sophus[j]);
edge->setInformation(Eigen::MatrixXd::Identity(16, 16));
edge->setRobustKernel(new g2o::RobustKernelHuber());
optimizer.addEdge(edge);
}
}
// TODO fetch data from the optimizer
// START YOUR CODE HERE
for(int i = 0; i < points.size(); i++)
{
points[i] = vertex_points[i]->estimate();
}
for(int i = 0; i < poses.size(); i++)
{
poses[i] = vertex_sophus[i]->estimate();
}
// END YOUR CODE HERE
CMAKE
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIR})
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
find_package(G2O REQUIRED)
include_directories(${G2O_INCLUDE_DIRS})
add_executable(directBA directBA.cpp)
target_link_libraries(directBA ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY}
target_link_libraries(directBA ${Pangolin_LIBRARIES})
target_link_libraries(directBA ${OpenCV_LIBRARIES})