深蓝学院-视觉SLAM理论与实践-第十二期-第7章作业

最新推荐文章于 2022-03-28 22:30:03 发布

小宋今天要早睡

最新推荐文章于 2022-03-28 22:30:03 发布

阅读量653

点赞数

分类专栏： SLAM 文章标签：算法

本文链接：https://blog.csdn.net/weixin_40918019/article/details/123662033

版权

SLAM 专栏收录该内容

11 篇文章 10 订阅

订阅专栏

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})

运行结果

控制台输出

原始点云

优化点云 ## 直接法的BA ### 问题1：数学模型 #### 如何描述任意一点投影在任意一图像中形成的 error? 可以通过灰度值的差进行描述 #### 每个err关联几个优化变量？每个err关联个优化变量，其中六个坐标转化矩阵对应的李代数，另外一个为投影点的深度信息

各个变量的雅可比是什么

问题2：实现

能否不以 $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})

运行结果

控制台输出

点云图

参考

G2O优化细节

小宋今天要早睡

关注

0
点赞
踩
10

收藏

觉得还不错? 一键收藏
0
评论
深蓝学院-视觉SLAM理论与实践-第十二期-第7章作业

Bundle Adjustment问题1：文献阅读为何说 Bundle Adjustment is slow 是不对的?可以利用H矩阵的稀疏性质对BA整个过程进行加速。BA 中有哪些需要注意参数化的地方？Pose 和Point 各有哪些参数化方式？有何优缺点。参数化的地方：相机的内参外参，观测点的三维坐标，观测点的像素坐标Pose参数化方法：欧拉角(万向锁，表示方法不唯一) 四元数(存在单位向量的约束) 旋转矩阵旋转向量Point参数化方法：齐次坐标问题2：BAL-dataset程序
复制链接

扫一扫