【slam十四讲第二版】【课后习题】【第十讲~后端2】

企业开发 2023-04-09 01:40:35 阅读次数: 0

【slam十四讲第二版】【课后习题】【第十讲~后端2】

0 前言

1 课后题

1.1 将位姿图中的误差交换位置，推导按照此定义的左乘扰动雅可比矩阵

1.2 参照g2o程序，在Ceres中实现对“球”位姿图的优化

1.2.1 pose_graph_ceres_SE3.cpp

#include <iostream>
#include <fstream>

#include <ceres/ceres.h>
#include <ceres/rotation.h>


#include <Eigen/Core>
#include <Eigen/Dense>

#include "sophus/so3.h"
#include "sophus/se3.h"

// 需要设置新的参数域，因此需要继承LocalParameterization
// 具体可以参考http://www.ceres-solver.org/nnls_modeling.html?highlight=localparameterization#localparameterization
class SE3Parameterization : public ceres::LocalParameterization
{
    
    
public:
    SE3Parameterization() {
    
    }
    virtual ~SE3Parameterization() {
    
    }

    // 如果更新，此处使用李代数更新，而不是简单的加减
    virtual bool Plus(const double* x,
                      const double* delta,
                      double* x_plus_delta) const
    {
    
    
        Eigen::Map<const Eigen::Matrix<double, 6, 1>> lie(x);
        Eigen::Map<const Eigen::Matrix<double, 6, 1>> delta_lie(delta);

        Sophus::SE3 T = Sophus::SE3::exp(lie);
        Sophus::SE3 delta_T = Sophus::SE3::exp(delta_lie);

        // 李代数左乘更新
        Eigen::Matrix<double, 6, 1> x_plus_delta_lie = (delta_T * T).log();

        for(int i = 0; i < 6; ++i)
            x_plus_delta[i] = x_plus_delta_lie(i, 0);

        return true;
    }

    // 如果计算Jacobian，此处大概是局部求导；不过由于我已经在残差函数中仔细计算了导数，所以这里单位矩阵就好
    virtual bool ComputeJacobian(const double* x,
                                 double* jacobian) const
    {
    
    
        ceres::MatrixRef(jacobian, 6, 6) = ceres::Matrix::Identity(6, 6);
        return true;
    }
    virtual int GlobalSize() const {
    
     return 6; }
    virtual int LocalSize() const {
    
     return 6; }
};

// 自己求导数的类，需要重构SizedCostFunction
class PosegraphBA: public ceres::SizedCostFunction<6,6,6>
{
    
    
public:
    Sophus::SE3 deltaSE3Inv;

    PosegraphBA(double x,double y,double z,
                double s,double vx,double vy,double vz)
    {
    
    
        Eigen::Quaterniond q( s,vx, vy, vz );
        q.normalize();
        deltaSE3Inv = Sophus::SE3( q,Eigen::Vector3d( x, y, z ) ).inverse();
    }

    //  Evaluate是很重要的一个函数
    virtual bool Evaluate(double const* const* pose,
                          double *residual,
                          double **jacobians) const
    {
    
    
        // Get Pose A
        Eigen::Map<const Eigen::Matrix<double,6,1>> poseAVec6d(pose[0]);
        Sophus::SE3 poseASE3 = Sophus::SE3::exp(poseAVec6d);

        // Get Pose B
        Eigen::Map<const Eigen::Matrix<double,6,1>> poseBVec6d(pose[1]);
        Sophus::SE3 poseBSE3 = Sophus::SE3::exp(poseBVec6d);

        // Compute Error
        Sophus::SE3 errorSE3 = deltaSE3Inv*poseASE3.inverse()*poseBSE3;
        Eigen::Matrix<double,6,1> errorVec6d = errorSE3.log();

        // 残差项
        residual[0] = errorVec6d(0);
        residual[1] = errorVec6d(1);
        residual[2] = errorVec6d(2);
        residual[3] = errorVec6d(3);
        residual[4] = errorVec6d(4);
        residual[5] = errorVec6d(5);

        if( !jacobians )
            return true;

        if( !jacobians[0] && !jacobians[1] )
            return true;

        // 以下都是jacobian的公式，具体也对应了公式
        // 可以参考http://www.ceres-solver.org/nnls_modeling.html?highlight=evaluate#_CPPv2N5ceres12CostFunction8EvaluateEPPCdPdPPd
        {
    
    
            // 公式11.10 J_r^{-1}
            Eigen::Matrix<double,6,6> J;
            J.block(0,0,3,3) = Sophus::SO3::hat(errorSE3.so3().log());
            J.block(0,3,3,3) = Sophus::SO3::hat(errorSE3.translation());
            J.block(3,0,3,3) = Eigen::Matrix3d::Zero(3,3);
            J.block(3,3,3,3) = Sophus::SO3::hat(errorSE3.so3().log());
            J = J*0.5 + Eigen::Matrix<double,6,6>::Identity();

            // 公式11.8
            // row correspond with error
            // col correspond with parameterA
            Eigen::Matrix<double,6,6> jacA = - J * poseBSE3.inverse().Adj();

            jacobians[0][ 0] = jacA(0,0); jacobians[0][ 1] = jacA(1,0); jacobians[0][ 2] = jacA(2,0); jacobians[0][ 3] = jacA(3,0); jacobians[0][ 4] = jacA(4,0); jacobians[0][ 5] = jacA(5,0);
            jacobians[0][ 6] = jacA(0,1); jacobians[0][ 7] = jacA(1,1); jacobians[0][ 8] = jacA(2,1); jacobians[0][ 9] = jacA(3,1); jacobians[0][10] = jacA(4,1); jacobians[0][11] = jacA(5,1);
            jacobians[0][12] = jacA(0,2); jacobians[0][13] = jacA(1,2); jacobians[0][14] = jacA(2,2); jacobians[0][15] = jacA(3,2); jacobians[0][16] = jacA(4,2); jacobians[0][17] = jacA(5,2);
            jacobians[0][18] = jacA(0,3); jacobians[0][19] = jacA(1,3); jacobians[0][20] = jacA(2,3); jacobians[0][21] = jacA(3,3); jacobians[0][22] = jacA(4,3); jacobians[0][23] = jacA(5,3);
            jacobians[0][24] = jacA(0,4); jacobians[0][25] = jacA(1,4); jacobians[0][26] = jacA(2,4); jacobians[0][27] = jacA(3,4); jacobians[0][28] = jacA(4,4); jacobians[0][29] = jacA(5,4);
            jacobians[0][30] = jacA(0,5); jacobians[0][31] = jacA(1,5); jacobians[0][32] = jacA(2,5); jacobians[0][33] = jacA(3,5); jacobians[0][34] = jacA(4,5); jacobians[0][35] = jacA(5,5);

            // 公式11.9
            Eigen::Matrix<double,6,6> jacB =   J * poseBSE3.inverse().Adj();

            jacobians[1][ 0] = jacB(0,0); jacobians[1][ 1] = jacB(1,0); jacobians[1][ 2] = jacB(2,0); jacobians[1][ 3] = jacB(3,0); jacobians[1][ 4] = jacB(4,0); jacobians[1][ 5] = jacB(5,0);
            jacobians[1][ 6] = jacB(0,1); jacobians[1][ 7] = jacB(1,1); jacobians[1][ 8] = jacB(2,1); jacobians[1][ 9] = jacB(3,1); jacobians[1][10] = jacB(4,1); jacobians[1][11] = jacB(5,1);
            jacobians[1][12] = jacB(0,2); jacobians[1][13] = jacB(1,2); jacobians[1][14] = jacB(2,2); jacobians[1][15] = jacB(3,2); jacobians[1][16] = jacB(4,2); jacobians[1][17] = jacB(5,2);
            jacobians[1][18] = jacB(0,3); jacobians[1][19] = jacB(1,3); jacobians[1][20] = jacB(2,3); jacobians[1][21] = jacB(3,3); jacobians[1][22] = jacB(4,3); jacobians[1][23] = jacB(5,3);
            jacobians[1][24] = jacB(0,4); jacobians[1][25] = jacB(1,4); jacobians[1][26] = jacB(2,4); jacobians[1][27] = jacB(3,4); jacobians[1][28] = jacB(4,4); jacobians[1][29] = jacB(5,4);
            jacobians[1][30] = jacB(0,5); jacobians[1][31] = jacB(1,5); jacobians[1][32] = jacB(2,5); jacobians[1][33] = jacB(3,5); jacobians[1][34] = jacB(4,5); jacobians[1][35] = jacB(5,5);

        }

        return true;
    }
};



int main( int argc, char *argv[] )
{
    
    
    google::InitGoogleLogging(argv[0]);

    if(argc<2)
    {
    
    
        std::cerr<<"./pose_graph_ceres_SE3 sphere.g2o"<<std::endl;
        return -1;
    }

    std::cout<<"Input g2o file: "<<argv[1]<<std::endl;


    std::ifstream g2oFile( argv[1] );
    if ( !g2oFile )
    {
    
    
        std::cout<<"file "<<argv[1]<<" does not exist."<<std::endl;
        return -1;
    }
    // Count Pose and Edge
    int poseCount = 0;
    int edgeCount = 0;
    std::string fileLine;
    while( std::getline(g2oFile,fileLine) )
    {
    
    
        if(fileLine[0]=='V')
        {
    
    
            poseCount++;
        }
        if(fileLine[0]=='E')
        {
    
    
            edgeCount++;
        }
    }
    g2oFile.clear();
    g2oFile.seekg(std::ios::beg);


    std::cout<<poseCount<<std::endl;
    std::cout<<edgeCount<<std::endl;


    // Init Ceres
    ceres::Problem problem;

    // Load Data
    double *poseData = new double[poseCount*6];
    for( int i=0; i<poseCount; i++ )
    {
    
    
        std::string flag;
        int id;
        double x,y,z,s,vx,vy,vz;
        g2oFile>>flag>>id>>x>>y>>z>>vx>>vy>>vz>>s;
        Eigen::Quaterniond q( s,vx, vy, vz );
        q.normalize();
        Eigen::Matrix<double,6,1> poseVec6d = Sophus::SE3( q,Eigen::Vector3d( x, y, z ) ).log();

        poseData[6*i+0] = poseVec6d(0);
        poseData[6*i+1] = poseVec6d(1);
        poseData[6*i+2] = poseVec6d(2);
        poseData[6*i+3] = poseVec6d(3);
        poseData[6*i+4] = poseVec6d(4);
        poseData[6*i+5] = poseVec6d(5);
    }

    ceres::LocalParameterization *local_parameterization = new SE3Parameterization();

    // Add Residual
    for( int i=0; i<edgeCount; i++ )
    {
    
    
        std::string flag;
        int idA,idB;
        double x,y,z,s,vx,vy,vz;
        double temp;
        g2oFile>>flag>>idA>>idB>>x>>y>>z>>vx>>vy>>vz>>s;
        // I dont't know how to use info
        for( int j=0; j<21; j++ )
        {
    
    
            g2oFile>>temp;
        }

        ceres::CostFunction *costFunction = new PosegraphBA(x,y,z,s,vx,vy,vz);

        problem.AddResidualBlock (  costFunction,
                                    nullptr,
                                    poseData+6*idA,
                                    poseData+6*idB
        );

        problem.SetParameterization(poseData+6*idA,local_parameterization);
        problem.SetParameterization(poseData+6*idB,local_parameterization);

    }
    g2oFile.close();


    // Set Ceres
    ceres::Solver::Options options;
    // options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
    options.minimizer_progress_to_stdout = true;

    // Solve
    ceres::Solver::Summary summary;
    ceres::Solve ( options, &problem, &summary );

    // Report
    std::cout<<summary.FullReport() <<std::endl;
    std::ofstream txt("1.txt");
    for( int i=0; i<poseCount; i++ )
    {
    
    
        Eigen::Map<const Eigen::Matrix<double,6,1>> poseAVec6d( poseData+6*i );
        Sophus::SE3 poseSE3 = Sophus::SE3::exp(poseAVec6d);

        txt<<poseSE3.translation().transpose()<<std::endl;
    }

    txt.close();


    // Release Data
    delete []poseData;

    return 0;
}

1.2.2 CMakeLists.txt

cmake_minimum_required(VERSION 2.8)

project(pose_graph_ceres_SE3)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_STANDARD 14)

include_directories("/usr/include/eigen3")
find_package(Sophus REQUIRED)
Find_Package(Ceres REQUIRED)

set(Sophus_LIBRARIES "/usr/local/lib/libSophus.so")

include_directories(${
    
    Sophus_INCLUDE_DIRS} )

add_executable(pose_graph_ceres_SE3 src/pose_graph_ceres_SE3.cpp)

target_link_libraries(pose_graph_ceres_SE3 ${
    
    CERES_LIBRARIES} ${
    
    Sophus_LIBRARIES} fmt)

1.2.3 rotation.h

#ifndef ROTATION_H
#define ROTATION_H

#include <algorithm>
#include <cmath>
#include <limits>

//
// math functions needed for rotation conversion. 

// dot and cross production 

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];
}


//


// Converts from a angle anxis to quaternion : 
template<typename T>
inline void AngleAxisToQuaternion(const T *angle_axis, T *quaternion) {
    
    
    const T &a0 = angle_axis[0];
    const T &a1 = angle_axis[1];
    const T &a2 = angle_axis[2];
    const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;

    if (theta_squared > T(std::numeric_limits<double>::epsilon())) {
    
    
        const T theta = sqrt(theta_squared);
        const T half_theta = theta * T(0.5);
        const T k = sin(half_theta) / theta;
        quaternion[0] = cos(half_theta);
        quaternion[1] = a0 * k;
        quaternion[2] = a1 * k;
        quaternion[3] = a2 * k;
    } else {
    
     // in case if theta_squared is zero
        const T k(0.5);
        quaternion[0] = T(1.0);
        quaternion[1] = a0 * k;
        quaternion[2] = a1 * k;
        quaternion[3] = a2 * k;
    }
}

template<typename T>
inline void QuaternionToAngleAxis(const T *quaternion, T *angle_axis) {
    
    
    const T &q1 = quaternion[1];
    const T &q2 = quaternion[2];
    const T &q3 = quaternion[3];
    const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3;

    // For quaternions representing non-zero rotation, the conversion
    // is numercially stable
    if (sin_squared_theta > T(std::numeric_limits<double>::epsilon())) {
    
    
        const T sin_theta = sqrt(sin_squared_theta);
        const T &cos_theta = quaternion[0];

        // If cos_theta is negative, theta is greater than pi/2, which
        // means that angle for the angle_axis vector which is 2 * theta
        // would be greater than pi...

        const T two_theta = T(2.0) * ((cos_theta < 0.0)
                                      ? atan2(-sin_theta, -cos_theta)
                                      : atan2(sin_theta, cos_theta));
        const T k = two_theta / sin_theta;

        angle_axis[0] = q1 * k;
        angle_axis[1] = q2 * k;
        angle_axis[2] = q3 * k;
    } else {
    
    
        // For zero rotation, sqrt() will produce NaN in derivative since
        // the argument is zero. By approximating with a Taylor series,
        // and truncating at one term, the value and first derivatives will be
        // computed correctly when Jets are used..
        const T k(2.0);
        angle_axis[0] = q1 * k;
        angle_axis[1] = q2 * k;
        angle_axis[2] = q3 * k;
    }

}

template<typename T>
inline void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) {
    
    //内联函数作用是是pt经过轴角angle_axis旋转后得到的点result
    const T theta2 = DotProduct(angle_axis, angle_axis);
    if (theta2 > T(std::numeric_limits<double>::epsilon())) {
    
    
        // Away from zero, use the rodriguez formula 远离零，使用罗德里格斯公式
        //
        //   result = pt costheta +
        //            (w x pt) * sintheta +
        //            w (w . pt) (1 - costheta)
        //
        // We want to be careful to only evaluate the square root if the
        // norm of the angle_axis vector is greater than zero. Otherwise
        // we get a division by zero.
        // 我们要注意的是，仅当轴角向量的范数大于零时才计算平方根。否则我们得到的除法是零。
        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};//这个表示的单位长度的向量

        // Explicitly inlined evaluation of the cross product for
        // performance reasons.
        // 出于性能原因，对交叉积进行了明确的内联评估。
        /*const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1],
                                  w[2] * pt[0] - w[0] * pt[2],
                                  w[0] * pt[1] - w[1] * pt[0] };*/
        T w_cross_pt[3];
        CrossProduct(w, pt, w_cross_pt);

        const T tmp = DotProduct(w, pt) * (T(1.0) - costheta);
        //    (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (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 {
    
    
        // Near zero, the first order Taylor approximation of the rotation
        // matrix R corresponding to a vector w and angle w is
        //
        //   R = I + hat(w) * sin(theta)
        //
        // But sintheta ~ theta and theta * w = angle_axis, which gives us
        //
        //  R = I + hat(w)
        //
        // and actually performing multiplication with the point pt, gives us
        // R * pt = pt + w x pt.
        //
        // Switching to the Taylor expansion near zero provides meaningful
        // derivatives when evaluated using Jets.
        //
        // Explicitly inlined evaluation of the cross product for
        // performance reasons.
        /*const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1],
                                  angle_axis[2] * pt[0] - angle_axis[0] * pt[2],
                                  angle_axis[0] * pt[1] - angle_axis[1] * pt[0] };*/
        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];
    }
}

#endif // rotation.h

1.2.4 输出

/home/bupo/my_study/slam14/slam14_my/cap10/pose_graph_ceres_SE3/cmake-build-debug/pose_graph_ceres_SE3 ./src/sphere.g2o
Input g2o file: ./src/sphere.g2o
2500
9799
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  4.913445e+05    0.00e+00    3.71e+02   0.00e+00   0.00e+00  1.00e+04        0    1.41e-02    1.82e-02
   1  1.707888e+08   -1.70e+08    3.71e+02   6.61e+03  -3.07e+03  5.00e+03        1    2.01e-01    2.20e-01
   2  1.672780e+08   -1.67e+08    3.71e+02   6.61e+03  -3.57e+03  1.25e+03        1    1.91e-01    4.11e-01
   3  1.474954e+08   -1.47e+08    3.71e+02   6.43e+03  -5.09e+03  1.56e+02        1    1.89e-01    6.00e-01
   4  1.225161e+08   -1.22e+08    3.71e+02   6.63e+03  -1.12e+04  9.77e+00        1    1.87e-01    7.87e-01
   5  9.940300e+06   -9.45e+06    3.71e+02   1.79e+03  -5.70e+03  3.05e-01        1    1.86e-01    9.73e-01
   6  4.906778e+05    6.67e+02    3.52e+02   4.37e+02   6.15e+00  9.16e-01        1    1.94e-01    1.17e+00
   7  6.610989e+05   -1.70e+05    3.52e+02   1.05e+02  -7.05e+02  4.58e-01        1    1.84e-01    1.35e+00
   8  5.614216e+05   -7.07e+04    3.52e+02   5.93e+01  -4.94e+02  1.14e-01        1    1.84e-01    1.54e+00
   9  5.033791e+05   -1.27e+04    3.52e+02   1.90e+01  -2.61e+02  1.43e-02        1    1.84e-01    1.72e+00
  10  4.917091e+05   -1.03e+03    3.52e+02   2.79e+00  -1.40e+02  8.94e-04        1    1.89e-01    1.91e+00
  11  4.907347e+05   -5.70e+01    3.52e+02   1.79e-01  -1.20e+02  2.79e-05        1    1.90e-01    2.10e+00
  12  4.906795e+05   -1.76e+00    3.52e+02   5.62e-03  -1.19e+02  4.37e-07        1    1.84e-01    2.28e+00

Solver Summary (v 2.0.0-eigen-(3.3.9)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-eigensparse-no_openmp)

                                     Original                  Reduced
Parameter blocks                         2500                     2500
Parameters                              15000                    15000
Residual blocks                          9799                     9799
Residuals                               58794                    58794

Minimizer                        TRUST_REGION

Sparse linear algebra library    SUITE_SPARSE
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
Threads                                     1                        1
Linear solver ordering              AUTOMATIC                     2500

Cost:
Initial                          4.913445e+05
Final                            4.906778e+05
Change                           6.667215e+02

Minimizer iterations                       13
Successful steps                            2
Unsuccessful steps                         11

Time (in seconds):
Preprocessor                         0.004068

  Residual only evaluation           0.053879 (13)
  Jacobian & residual evaluation     0.020577 (2)
  Linear solver                      2.403726 (13)
Minimizer                            2.507487

Postprocessor                        0.000336
Total                                2.511891

Termination:                      CONVERGENCE (Function tolerance reached. |cost_change|/cost: 5.615779e-08 <= 1.000000e-06)


进程已结束,退出代码0

猜你喜欢

转载自blog.csdn.net/qq_45954434/article/details/126047980
今日推荐
NetBSD 禁止提交由 AI 生成的代码
Apache Doris 2.0.10 版本正式发布！
开源日报 | 大模型开战；大模型独角兽被曝卖身；周鸿祎建议谷歌开源所有产品；最大开源AI社区提供1000万美元共享GPU
开源日报 | Chrome内置Gemini的意义不在于Gemini；中国AI追随之路的五大误区；ECharts创始人“下海”养鱼；谷歌I/O开发者大会什么都有，只是没有惊喜
微软回应中国区AI团队“打包赴美”传闻
基于大语言模型的开源知识库问答系统 MaxKB GitHub Star 数量突破 5,000 个！
周排行
static方法和非static方法的区别（java）
如何查找计算机专业paper
java.lang.ClassFormatError: Incompatible magic value 0 in class file com/sitecha
跳跃游戏II
stm32_之【建立工程】
TeaWeb v0.0.9 发布，统计底层优化、主机监控功能改进
事件分发 -----控制字体大小
JavaScript DOM练习（动态表格添加） December 25，2019
JSF Scope & CDI
实现从零搭建一个登录注册页面（附源代码）
每日归档
更多
2024-05-19(0)
2024-05-18(4)
2024-05-17(34)
2024-05-16(6)
2024-05-15(24)
2024-05-14(0)
2024-05-13(18)
2024-05-12(0)
2024-05-11(38)
2024-05-10(38)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022