SLAM14讲程序错误整理——自第六讲

1、Ceres库文件链接target_link_libraries

使用如下LIBS方式链接库文件会放生错误

target_link_libraries(cerescurvefitting ${OpenCV_LIBS} ${CERES_LIBS})

需要全名LIBRARIES

target_link_libraries(cerescurvefitting ${OpenCV_LIBS} ${CERES_LIBRARIES})

OpenCV_LIBS，其中CV都要大写

add_executable(cerescurefitting2 cerescurefitting2.cpp)
target_link_libraries(cerescurefitting2 ${OpenCV_LIBS} ${CERES_LIBRARIES})

1.1 PCL库报C++编译错误

Error:Specified unknown feature "cxx_std_14" for target "pointCloud2Mesh".

解决，添加如下cmkae代码

ADD_COMPILE_OPTIONS(-std=c++11 )
ADD_COMPILE_OPTIONS(-std=c++14 )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

1.2 G20库编译报错

/home/roman/code/slambook_trying/ch7/src/pose_estimation.cpp:91: undefined reference to `g2o::SparseOptimizer::SparseOptimizer()'
/home/roman/code/slambook_trying/ch7/src/pose_estimation.cpp:92: undefined reference to `g2o::SparseOptimizer::setAlgorithm(g2o::OptimizationAlgorithm*)'
/home/roman/code/slambook_trying/ch7/src/pose_estimation.cpp:93: undefined reference to `g2o::SparseOptimizer::setVerbose(bool)'
/home/roman/code/slambook_trying/ch7/src/pose_estimation.cpp:99: undefined reference to `g2o::VertexSBAPointXYZ::VertexSBAPointXYZ()'

在CMakeLists.txt 中的 target_link_libraries()添加

g2o_core g2o_stuff g2o_types_sba g2o_csparse_extension

第八章光流法

由于安装了双版本opencv（/home/s/opencv_4_install），CmakeLists.txt查询opencv包的方式要改变

set(CMAKE_PREFIX_PATH "/home/s/opencv_4_install")

在这里还改动了opncv3.2，要修改回来需要
gedit ~/.bashrc ，讲最后的路径换乘3.2的
编译出现error: ‘CV_GRAY2BGR’ was not declared in this scope的错误，加入：

#include 

2、读取图像

//
// Created by roman on 20-7-3.
//

#include 
#include 
#include 

using namespace std;

int main(){
//%06 为用6个0占为
    boost::format fim("./%06d.png");

    for (int i = 1; i < 6; i++){
        cv::Mat img = cv::imread((fim % i).str(),0);
        cv::imshow("img",img);
        cv::waitKey(0);
    }

    /*string image_file = "./000001.png";
    cv::Mat img = cv::imread(image_file);
    cv::imshow("img",img);
    cv::waitKey(0);
     */
}

3、aligned_allocator

https://www.cnblogs.com/Glucklichste/p/10912257.html
在使用Eigen的时候，如果STL容器中的元素是Eigen数据库结构，比如下面用vector容器存储Eigen::Matrix4f类型或用map存储Eigen::Vector4f数据类型时：

vector<Eigen::Matrix4d>;
std::map<int, Eigen::Vector4f>

这么使用编译能通过，当运行时会报段错误。
这个分配器所在头文件为：

#include 

下面才是正确的定义方法

std::vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d>>

	
std::map<int, Eigen::Vector4f, Eigen::aligned_allocator<std::pair<const int, Eigen::Vector4f>>
 

4、ceres::SizedCostFunction

链接https://www.jianshu.com/p/efe0d10197ba
这是个可变参数的模板，第一个参数为误差的维度，由于误差是SE(3)的李代数se(3)，所以是6维，后面的参数都是你要传入的参数的维度，这里要优化两个位姿，所以需要传入两个参数，每个位姿的维度为6，所以设置两个6，因此在程序中继承它的时候，传入了模板参数<6,6,6>，之后核心在于重载函数

virtual bool Evaluate(double const* const* parameters,
                      double* residuals,
                      double** jacobians) const;

注意这里是的const，一个不能少，我来介绍一下这个函数怎么写。
由于你有两个传入参数，所以parameters和jacobians这个二级指针里面，含有两个一级指针，就是说parameters[0]和parameters1,以及jacobians[0]和jacobians1这四个指针是有内存的，当然residuals也是有内存的
之后，由于param的维度都是6，所以parameters[0]和parameters1的空间分配量都为6,residuals自然也是6，注意，jacobians[0]和jacobians1两个雅可比指针的维度都是36,因为误差是6维,参数是6维,求导得到的矩阵为6*6，在这里把这个矩阵存成向量，行优先存储。你需要在这个函数里面把残差已经雅可比全部计算好，然后返回一个true。

virtual bool Evaluate(double const* const* parameters,

                      double* residuals,

                      double** jacobians) const {
        Eigen::Map<const Eigen::Matrix<double, 6, 1>> lie_i(*parameters);
        Eigen::Map<const Eigen::Matrix<double, 6, 1>> lie_j(*(parameters + 1));
        Eigen::Matrix<double, 6, 6> Jac_i;
        Eigen::Matrix<double, 6, 6> Jac_j;
        Sophus::SE3d T_i = Sophus::SE3d::exp(lie_i);
        Sophus::SE3d T_j = Sophus::SE3d::exp(lie_j);
        Sophus::SE3d Tij_estimate = T_i * T_j.inverse();

        Sophus::SE3d err = Tij_estimate * T_ij_.inverse();

        Eigen::Matrix<double, 6, 6> Jl;

        Jl.block(3, 3, 3, 3) = Jl.block(0, 0, 3, 3) = Sophus::SO3d::hat(err.so3().log());
        Jl.block(0, 3, 3, 3) = Sophus::SO3d::hat(err.translation());
        Jl.block(3, 0, 3, 3) = Eigen::Matrix3d::Zero();
        Eigen::Matrix<double, 6, 6> I = Eigen::Matrix<double, 6, 6>::Identity();
        Jl.noalias() = sqrt_information_ * (I - 0.5 * Jl);
        Jac_i = Jl;
        
        Eigen::Matrix<double, 6, 1> err_ = sqrt_information_ * err.log();
        
        const Eigen::Matrix<double, 3, 3>& R = Tij_estimate.rotationMatrix();
        Eigen::Matrix<double, 6, 6> adj;
        adj.block<3, 3>(3, 3) = adj.block<3, 3>(0, 0) = R;
        adj.block<3, 3>(0, 3) = Sophus::SO3d::hat(Tij_estimate.translation()) * R;
        adj.block<3, 3>(3, 0) = Eigen::Matrix<double, 3, 3>::Zero(3, 3);

        Jac_j = -Jac_i * adj;
        
        int k = 0;
        for(int i = 0; i < 6; i++) {
            residuals[i] = err_(i);
            if(jacobians) {
                for (int j = 0; j < 6; ++j) {
                    if (jacobians[0])
                        jacobians[0][k] = Jac_i(i, j);
                    if (jacobians[1])
                        jacobians[1][k] = Jac_j(i, j);
                    k++;
                }
            }
        }
        return true;
    }

5、Eigen::Vector3d 严重错误

Vector3d的引用是只有一个（）或者[ ]，不能用两个括号，错误一直提示SIGAV，找了一天的错误，还是要去熟悉以下EIGEN啊！！

Eigen::Vector3d P2 = T_SE3 * P1;
double u = fx_ * P2[0] / P2[2] + cx_;
double v = fy_ * P2[1] / P2[2] + cy_;

6、改换Opencv4.3后ORB_SLAM2编译错误

https://blog.csdn.net/K4762/article/details/105270600

#include
#include 
//需要改换的两个头文件名称

7、ICP求出的R是相机1相对于2还是2相对于1问题

注意！
书本中p是第二帧图像，p’是第一帧图像，估计的R是R21，乘以p1恰好变换到p2

8、 自己定义位姿（Eigen::Isometry3d &pose21）和g2o顶点（VertexSE3Expmap）不一样，要注意返回当前估计位姿（ pose21 = v->estimate();），不然一直为初始状态E，错误两天才排查出来

void SemiDirectPoseEstimation(Mat &img1, Mat &img2,
                              vector<Eigen::Vector3d> &vP1,
                              vector<double> &vMeasurePixelValue,
                              vector<Eigen::Vector2d> &vChoosePoints,
                              Eigen::Isometry3d &pose21, Eigen::Matrix3d &K)
{

    // 初始化g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> BlockSolverType;  // 求解的向量是6＊1的
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
            g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N
    // g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );
    optimizer.setVerbose( true );

    //相机顶点

    g2o::VertexSE3Expmap *v = new g2o::VertexSE3Expmap();
    v->setId(0);
    v->setEstimate(g2o::SE3Quat(pose21.rotation(),pose21.translation()));
    optimizer.addVertex(v);

    //添加边
    for (int i = 0; i < vChoosePoints.size(); ++i) {
        SparseXYZ2Pixel *edge = new SparseXYZ2Pixel(img2,vP1[i],K);
        edge->setId(i+1);
        edge->setVertex(0, v);
        edge->setMeasurement(vMeasurePixelValue[i]);
        edge->setParameterId(0,0);
        edge->setInformation(Eigen::Matrix<double,1,1>::Identity());
        optimizer.addEdge(edge);
    }

    optimizer.setVerbose(true);
    cout<<"edges in graph: "<< optimizer.edges().size() <<endl;
    optimizer.initializeOptimization();
    optimizer.optimize(30);
    
    //自己定义位姿和g2o顶点不一样的要注意返回当前估计位姿，不然一直为初始状态E，错误才排查出来
    pose21 = v->estimate();

    //cout << "image "  << "-----------------------\n" << "T21 = \n " << pose21.matrix() << endl;
}