编译run_vo.cpp报错
发现是未添加对fmt库的依赖,但添加后依然报错,如下:
对src/CMakeLists.txt更改,更改前:
cmake_minimum_required(VERSION 2.8)
project(vo_practice)
add_library(vo_practice_lib SHARED
frame.cpp
mappoint.cpp
map.cpp
camera.cpp
config.cpp
visual_odometry.cpp
)
target_link_libraries(vo_practice_lib ${
THIRD_PARTY_LIBS})
更改后:
cmake_minimum_required(VERSION 2.8)
project(vo_practice)
add_library(vo_practice_lib STATIC
frame.cpp
mappoint.cpp
map.cpp
camera.cpp
config.cpp
visual_odometry.cpp
)
target_link_libraries(vo_practice_lib ${
THIRD_PARTY_LIBS})
可以看到将动态库更改为静态库,编译成功
运行run_vo.cpp错误
问题1:内点数始终为0(PnP inliers:0)
检查源程序,在run_vo.cpp文件中,在新建Camera::Ptr时,直接使用的是默认拷贝构造函数,没有提供相机的内参,利用Config类的get函数初始化相机内参
vo_practice::Camera::Ptr camera ( new vo_practice::Camera );
camera->cx_=vo_practice::Config::get<float>("camera.cx");
camera->cy_=vo_practice::Config::get<float>("camera.cy");
camera->fx_=vo_practice::Config::get<float>("camera.fx");
camera->fy_=vo_practice::Config::get<float>("camera.fy");
camera->depth_scale_=vo_practice::Config::get<float>("camera.depth_scale");
问题2:坐标轴没有变化
调试后发现Tcw矩阵和Aff_mat矩阵并不一致,如下所示:
原代码:
// show the map and the camera pose
cv::Affine3d::Mat3 Aff_mat;
for(int i=0;i<3;++i)
{
for(int j=0;j<3;++j)
{
Aff_mat<<Tcw.rotationMatrix()(i,j);
}
}
cout<<"Tcw:\n"<<Tcw.rotationMatrix()<<endl<<endl;
cout<<"Aff_mat:\n"<<Aff_mat<<endl<<endl;
cv::Affine3d M(
// cv::Affine3d::Mat3(
// Tcw.rotation_matrix()(0,0), Tcw.rotation_matrix()(0,1), Tcw.rotation_matrix()(0,2),
// Tcw.rotation_matrix()(1,0), Tcw.rotation_matrix()(1,1), Tcw.rotation_matrix()(1,2),
// Tcw.rotation_matrix()(2,0), Tcw.rotation_matrix()(2,1), Tcw.rotation_matrix()(2,2)
// ),
Aff_mat,
// Tcw.rotationMatrix(),
cv::Affine3d::Vec3(
Tcw.translation()(0,0), Tcw.translation()(1,0), Tcw.translation()(2,0)
)
);
修改代码如下:
// show the map and the camera pose
cv::Affine3d::Mat3 Aff_mat;
for(int i=0;i<3;++i)
{
for(int j=0;j<3;++j)
{
Aff_mat(i,j)=Tcw.rotationMatrix()(i,j);
}
}
// cout<<"Tcw:\n"<<Tcw.rotationMatrix()<<endl<<endl;
// cout<<"Aff_mat:\n"<<Aff_mat<<endl<<endl;
cv::Affine3d M(
// cv::Affine3d::Mat3(
// Tcw.rotation_matrix()(0,0), Tcw.rotation_matrix()(0,1), Tcw.rotation_matrix()(0,2),
// Tcw.rotation_matrix()(1,0), Tcw.rotation_matrix()(1,1), Tcw.rotation_matrix()(1,2),
// Tcw.rotation_matrix()(2,0), Tcw.rotation_matrix()(2,1), Tcw.rotation_matrix()(2,2)
// ),
Aff_mat,
// Tcw.rotationMatrix(),
cv::Affine3d::Vec3(
Tcw.translation()(0,0), Tcw.translation()(1,0), Tcw.translation()(2,0)
)
);
成功解决问题
不同版本的g2o
问题1:g2o::VertexSBAPointXYZ不是g2o的成员
新版本没有,旧版本有
问题2:g2o的构建有差别
// using bundle adjustment to optimize the pose
//构建图优化,设定g2o
//旧版的g2o用下面的方法
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block;
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
Block* solver_ptr = new Block( linearSolver );
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
//新版的g2o用下面的方法
// typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 2>> BlockSolverType; // 求解的向量是6*1的
// typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
// // 梯度下降方法,可以从GN, LM, DogLeg 中选
// auto solver = new g2o::OptimizationAlgorithmLevenberg(
// g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );
新版的g2o也可以使用unique_ptr< >指针进行构造,如下:
// 构建图优化,先设定g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock; // 求解的向量是6*1的
DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense<DirectBlock::PoseMatrixType> ();
// DirectBlock* solver_ptr = new DirectBlock(linearSolver);
DirectBlock* solver_ptr = new DirectBlock(unique_ptr<DirectBlock::LinearSolverType>(linearSolver));
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr); // G-N
g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(unique_ptr<DirectBlock>(solver_ptr)); // G-N
// g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver); // L-M