视觉SLAM十四讲（第一版）第九讲VO Project笔记
一、工程编译存在的问题
1.Project0.1
1.1.编译工程时Sophus报错
fatal error: sophus/se3.h: 没有那个文件或目录
这个博客中写的非常好。首先分析原因是Sophus库的版本不同，有的是模板类实现，有的不是。另外博客中详细写了如何安装两个版本的Sophus，对其他库也适用，很有参考价值。
但是存在的另一个没有解决的问题是，之前自己安装的是模板类的Sophus库，现在安装非模板类的库，它的安装目录下没有share这个文件夹，所以找不到.cmake的文件，设置find_package的目录就没法设置。但是好在非模板类的库比较简单，可以直接在安装目录下找到头文件和库文件的目录，然后在CMakeLists.txt中手动设置这个目录。
# Sophus 这里用的是非模板类的Sophus，但是使用find_package找不到，很奇怪，只能手动加入这些路径
#set(Sophus_DIR "/usr/local/sophus-no-template/")
#find_package( Sophus REQUIRED )
#include_directories( ${Sophus_INCLUDE_DIRS} )
include_directories("/usr/local/sophus-no-template/include")
set(Sophus_LIBRARIES "/usr/local/sophus-no-template/lib/libSophus.so")
1.2.编译非模板类的Sophus时报错
Sophus/sophus/so2.cpp:33:26:error: lvalue required as left operand of assignment
unit_complex_.real() = 1.;
解决：关于SLAM十四讲中 非模板类Sophus库的安装问题
2.Project 0.3
报错：double free or corruption (out)，需要更改CMakelists.txt：
set( CMAKE_CXX_FLAGS “-std=c++11 -march=native -O3” )
改成
set( CMAKE_CXX_FLAGS “-std=c++11 -O3” )
另外：要注意这里的-O3优化其实是比较重要的，可以显著地提高程序运行的速度。因为我开始为了测试下面的Debug，就把编译类型改成了Debug，并且把上面的-O3优化去掉了，结果一帧VO运行的时间达到了150ms左右，画面变慢的现象很明显。后来把-O3加上之后，一帧VO的时间在23ms左右，比原来没有使用g2o的时候多了3ms左右的时间，正常。
注意：
- 这可真是个大坑，困扰了我很长时间，所以这里还是需要单独去看看到底这个命令代表什么？
- 另一个比较奇怪的地方是程序是运行到g2o的时候报这个错误，这是内存错误，但是是运行到g2o这里才报这个错误，这是为什么？
- 自己想Debug看一下问题在哪里，但是发现打了断点始终进不去，直接跳过了。其实这是因为CMakeLists.txt文件中设置了编译类型为Release，而非Debug，这样就导致无法Debug。
- 但是把编译模式设置成Debug之后，编译报错如下：
../../lib/libmyslam.so：对‘google::base::CheckOpMessageBuilder::CheckOpMessageBuilder(char const*)’未定义的引用
../../lib/libmyslam.so：对‘google::base::CheckOpMessageBuilder::~CheckOpMessageBuilder()’未定义的引用
../../lib/libmyslam.so：对‘google::base::CheckOpMessageBuilder::ForVar2()’未定义的引用
../../lib/libmyslam.so：对‘google::LogMessage::stream()’未定义的引用
../../lib/libmyslam.so：对‘google::LogMessageFatal::LogMessageFatal(char const*, int, google::CheckOpString const&)’未定义的引用
../../lib/libmyslam.so：对‘google::LogMessageFatal::~LogMessageFatal()’未定义的引用
../../lib/libmyslam.so：对‘google::base::CheckOpMessageBuilder::NewString[abi:cxx11]()’未定义的引用
这个错误是没有连接到glog的库上。但是很奇怪，使用Release模式怎么就不报这个错误？因为其他地方都没改。最后无奈只能在第三方库这里把glog的库也安装上，改成如下：
set( THIRD_PARTY_LIBS
${OpenCV_LIBS}
${Sophus_LIBRARIES}
g2o_core g2o_stuff g2o_types_sba glog
)
注意这里前面不添加find_package( glog REQUIRED )也可以（实际上是我find_package找不到，具体没有解决），因为经过查找可以知道glog这个库在lib/x86_64-linux-gnu这个目录下，应该是系统找库的时候的默认目录，所以不用找库直接链接也可以。
总结：
很多时候的程序是没法进行Debug模式进行调试的，原因对于SLAM来说很多程序都依赖于第三方的库，而第三方的库为了提高性能往往都是编译出Release模式的库，而没有编译出Debug模式的库。此时如果自己的工程使用Debug模式，那么也是链接不到Debug的相关库的，此时链接库的时候就相当于Debug模式的工程链接到了Release模式的库，很容易出现很多问题。
所以日常开发更常见的方法是自己的工程就使用Release版本，这样连接到的库也都是Release版本，不会出现链接库上的问题。在调试的时候，使用glog打印到日志文件中，看问题出现在哪里。或者使用printf命令行打印，比如在程序中的各个地方打印12345等，看程序停在哪里不动。
二、代码知识点和疑问整理
1.Project0.1
1.1.Camera类
1.1.1.类中对智能指针的类型进行重定义
一定注意，这里是对数据类型进行typedef的数据类型重定义，不是变量的定义。进行下面的定义之后，由于这个typedef是在类的内部进行声明的，所以如果此时Camera::Ptr就相当于一个数据类型，也就是Camera类中定义的一个数据类型Ptr，这个数据类型的实际类型是std::shared_ptr<Camera>。
之所以这样写，就是为了使用智能指针内存管理的方便，而使用typedef对智能指针的类型进行变量类型重定义，是为了后面使用这个智能指针的时候程序更加简洁。
typedef std::shared_ptr<Camera> Ptr;
1.1.2.成员函数的形参有默认值
在类的头文件中，给形参赋了默认值，但是在cpp文件中成员函数的实现中，并没有给相应的形参赋默认值，如下所示。
注意这是形参有默认值的固定写法。也就是如果函数同时有声明和实现，那么形参默认值写在声明里，而在函数的实现的地方形参是不写默认值的，否则此时会报重复赋值默认值的错误。而如果函数只有实现，而没有声明的话，那么只能把形参的默认值写在函数实现的地方了。
// camera.h
Vector3d pixel2camera( const Vector2d& p_p, double depth=1 );
// camrea.cpp
Vector3d Camera::pixel2camera ( const Vector2d& p_p, double depth )
{
return Vector3d (
( p_p ( 0,0 )-cx_ ) *depth/fx_,
( p_p ( 1,0 )-cy_ ) *depth/fy_,
depth
);
}
1.2.Frame类
1.2.1.forward declare 前向声明
如下的程序，在Frame类的定义之前进行了class MapPoint类的前向声明，但是实际上在Frame类中并没有用到MapPoint类，那这里为什么还要进行前向声明呢？
namespace myslam
{
// forward declare
class MapPoint; // ??? 这里定义这个干什么的？
class Frame
{
public:
..........
}
}
所谓前向声明，简单的来说就是之前编写程序的时候使用的外部变量或函数的声明，比如如下最简单的例子，使用前向声明（或者简单理解为之前使用的普通的函数声明），可以不包含头文件而编译一样可以通过。
// add.cpp:
int add(int x, int y)
{
return x + y;
}
// main.cpp:
#include <iostream>
int add(int x, int y); // 前向声明
int main()
{
std::cout << "The sum of 3 and 4 is " << add(3, 4) << std::endl;
return 0;
}
前向声明 前置声明 Forward Declare
总结：前向声明可以简单的理解为当前文件中使用了外部文件中定义的变量或者函数，但是仅仅使用了这一个或者几个，不需要把所有的头文件都包含进来，这样会让编译速度变慢。因此使用前向声明，可以通过编译，同时还能加快编译的速度。
详细解读：
前向声明 前置声明 Forward Declare
[C++] 前向声明(forward declaration)
1.2.2.Frame类中包含Camera类的指针
为了内存管理的方便性和安全性，这里要使用智能指针，因此在camera类中进行了智能指针的类型重定义。
1.2.3.static成员函数——工厂函数
使用这种类型的写法，这是一个静态成员函数，只能通过类来调用，不能通过具体的对象来调用。每次通过类来调用都生成一个对象的指针。
// frame.h
// factory function 工厂函数，这个需要注意
static Frame::Ptr createFrame();
// frame.cpp
Frame::Ptr Frame::createFrame()
{
static long factory_id = 0;
return Frame::Ptr( new Frame(factory_id++) ); // 只传入一个id就够了，其他的参数有默认值
}
问题：这种写法有什么好处？暂时记住这种通用的写法，就是为了生成一个类的指针，不是在程序中手动的new一个指针，而是在这个类中定义一个static函数，这个函数中new出这个类的指针，然后返回。
1.2.4.访问Mat中某个像素点的数据的方法
1.3.MapPoint类
这里面的相关知识点和Frame类中的相关知识点差不多，没有新的内容。
1.4.Map类
1.4.1.unordered_map标准库STL中的散列Hash
unordered_map，它是一个关联容器，内部采用的是hash表结构，拥有快速检索的功能。
这个数据类型具有以下特征：
关联性：通过key去检索value，而不是通过绝对地址（和顺序容器不同）
无序性：使用hash表存储，内部无序
Map : 每个值对应一个键值
键唯一性：不存在两个元素的键一样
动态内存管理：使用内存管理模型来动态管理所需要的内存空间
总结：这个容器的使用方法和普通的map基本相同，书中给出的使用这个容器的原因是：map类中存储了各个关键帧和路标点，既需要随机访问，又需要随时插入和删除，所以使用散列(Hash)来存储它们。
namespace myslam
{
class Map
{
public:
typedef shared_ptr<Map> Ptr;
unordered_map<unsigned long, MapPoint::Ptr > map_points_; // all landmarks
unordered_map<unsigned long, Frame::Ptr > keyframes_; // all key-frames
Map() {}
void insertKeyFrame( Frame::Ptr frame );
void insertMapPoint( MapPoint::Ptr map_point );
};
}
1.5.Config类
1.5.1.单例模式(Singleton)
由于Config类负责参数文件的读取，一次程序运行中肯定只能有一套参数，因此这里使用单例模式。它只有一个全局对象，当设置参数文件的时候，创建这个对象并读取参数文件，然后就可以在任意地方访问参数的值，最后在程序结束的时候自动销毁。
// ****************** config.h
namespace myslam
{
class Config
{
private:
static std::shared_ptr<Config> config_; // 注意这个是静态成员变量
cv::FileStorage file_;
Config () {} // private constructor makes a singleton 私有构造函数，这样会变成一个单例类
public:
~Config(); // close the file when deconstructing
// set a new config file
static void setParameterFile( const std::string& filename );
// access the parameter values
template< typename T >
static T get( const std::string& key )
{
return T( Config::config_->file_[key] ); // static成员变量，只能通过类来访问，对象访问无意义
}
};
}
// *************************** config.cpp
namespace myslam
{
void Config::setParameterFile( const std::string& filename )
{
if ( config_ == nullptr )
config_ = shared_ptr<Config>(new Config);
config_->file_ = cv::FileStorage( filename.c_str(), cv::FileStorage::READ );
if ( config_->file_.isOpened() == false )
{
std::cerr<<"parameter file "<<filename<<" does not exist."<<std::endl;
config_->file_.release();
return;
}
}
Config::~Config()
{
if ( file_.isOpened() )
file_.release();
}
shared_ptr<Config> Config::config_ = nullptr; // 静态成员变量要进行全局初始化
}
2.Project0.2
2.1.VisualOdometry视觉里程计类
2.1.1.setRef3DPoints()构造参考帧的3D点
这个函数比较关键，因为他和整个VO的算法思想有关。由于这里使用的是RGBD相机，也就是同时有彩色图和深度图，那么匹配特征点之后进行求解的时候就可以直接使用PnP了。所以这个函数就是构建PnP中3D点的关键。
这里VO的思路如下：对于参考帧，需要生成它的3D点，然后和当前帧的2D特征点进行匹配并求解PnP得到位姿。3D点需要构造，就是使用setRef3DPoints()这个函数。具体就是遍历参考帧的所有2D特征点，其对应位置的深度图如果深度值合法，那么就从像素坐标系反投影到相机坐标系得到3D点，同时把这个2D特征点对应的描述子存储起来，作为得到的3D点的描述子，用于后面和当前帧的特征点进行描述子的匹配。也就是参考帧的描述子是和参考帧的3D点一一对应的！
2.1.2.poseEstimationPnP()计算PnP的函数
这个函数中有很多知识点值得注意，看下面的注释.
void VisualOdometry::poseEstimationPnP()
{
// construct the 3d 2d observations
vector<cv::Point3f> pts3d;
vector<cv::Point2f> pts2d;
for ( cv::DMatch m:feature_matches_ )
{
// !!注意参考帧的描述子和3D点是一一对应的，所以特征点(描述子匹配) 得到的也是当前帧的2D特征点的描述子和参考帧的3D点的描述子的匹配序号关系
// !!具体是queryIdx还是trainIdx对应与参考帧中的索引，和前面进行描述子匹配的时候传递给描述子匹配的函数的形参的顺序有关
pts3d.push_back( pts_3d_ref_[m.queryIdx] ); // queryIdx是匹配的特征点对中，参考帧的索引
pts2d.push_back( keypoints_curr_[m.trainIdx].pt ); // trainIdx是匹配的特征点对中，当前帧的索引
}
// !!注意这种构造矩阵的方法，暂且记住吧
Mat K = ( cv::Mat_<double>(3,3)<<
ref_->camera_->fx_, 0, ref_->camera_->cx_,
0, ref_->camera_->fy_, ref_->camera_->cy_,
0,0,1
);
Mat rvec, tvec, inliers; // pnp求解得到的旋转向量，平移向量，内点
// 使用Ransac求解PnP问题，这是一个迭代求解的方法，有迭代次数的设置，求解过程中会标记内点
cv::solvePnPRansac( pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers );
num_inliers_ = inliers.rows; // !!在迭代求解的过程中会把内点存储到inliers这个矩阵的每一行中
cout<<"pnp inliers: "<<num_inliers_<<endl;
// 之前求解得到的是3D到2D的变换，就是从参考帧到当前帧的变换
T_c_r_estimated_ = SE3(
// !!注意这种访问Mat中某个位置的元素的值的方法，关键还在模板中的double参数这里
SO3(rvec.at<double>(0,0), rvec.at<double>(1,0), rvec.at<double>(2,0)),
Vector3d( tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0))
);
}
2.1.3.cv::solvePnPRansac()函数还要仔细看
TODO：这个函数感觉还是比较重要的，还是需要去看看内部的实现！
2.2.对于两两帧间的VO的讨论（很值得思考）
书上写的讨论非常好，这些存在的缺点都是优秀的slam框架中解决的问题，所以这里的讨论可以很好的解释很多slam框架中的一些工程trick的原因，即为什么要这样做。
3.Project0.3
3.1.问题：cv::solvePnPRansac()的迭代法和代码中手动使用的g2o迭代有关吗？
书中写的是即使使用Ransac求耳机PnP，但是仍然容易收到噪声的影响。但是OpenCV中的求解PnP的问题，不论是否使用了Ransac，都有一个迭代法求解的方式（还有其他很多方式，比如EPnP等），这里的迭代是怎么回事呢？
在ORBSLAM中有关于这方面的求解，所以还需要去看ORB的源码才能很好的理解。
3.2.问题：使用g2o迭代后估计的运动明显稳定了许多？
这个怎么能看出来估计的运动稳定了呢？直接看视频应该是看不出来的吧？
4.Project0.4
4.1.optimizeMap()优化地图的很多方法比较重要
这个函数是优化地图，实际上内部的处理就是如何管理局部地图，让它的规模不是很大，并且有助于特征匹配。内部的函数实现如下：
// *************************************** 地图的管理函数实现
void VisualOdometry::optimizeMap()
{
// remove the hardly seen and no visible points
for ( auto iter = map_->map_points_.begin(); iter != map_->map_points_.end(); )
{
// 当前帧没看到的地图点就擦除，是不是太严格了？
if ( !curr_->isInFrame(iter->second->pos_) ) // 如果这个地图点投影后不在当前帧中，那么就删除这个地图点
{
iter = map_->map_points_.erase(iter);
continue;
}
float match_ratio = float(iter->second->matched_times_)/iter->second->visible_times_;
// cout << match_ratio << ", ";
if ( match_ratio < map_point_erase_ratio_ ) // 如果匹配上的次数/观测到的次数 < 匹配擦除比例，那么就把这个地图点删除
{
iter = map_->map_points_.erase(iter);
continue;
}
double angle = getViewAngle( curr_, iter->second );
if ( angle > M_PI/6. ) // 当前帧观测地图点的方向和地图点第一次被观测的方向角度超过30度，那么擦除地图点
{
iter = map_->map_points_.erase(iter);
continue;
}
if ( iter->second->good_ == false ) // 如果这个地图点不是一个好点？？
{
// TODO try triangulate this map point 三角化这个地图点？？？就是使用两帧都观测到了这个地图点进行三角化，从而修正RGBD得到的深度的错误
}
iter++;
}
if ( match_2dkp_index_.size()<100 ) // 如果在当前帧匹配的特征点数目<100，那么就在地图中增加地图点
addMapPoints();
if ( map_->map_points_.size() > 1000 ) // 地图点数量太大了，那么就增加擦除地图点的比例
{
// TODO map is too large, remove some one
map_point_erase_ratio_ += 0.05;
}
else
map_point_erase_ratio_ = 0.1;
cout<<"map points: "<<map_->map_points_.size()<<endl;
}
// ************************ VO主函数中对地图管理函数的调用
case OK:
{
curr_ = frame;
curr_->T_c_w_ = ref_->T_c_w_; // 先把当前帧的位姿设置成和参考帧的位姿相同，这个需要用于特征匹配的时候初步判断哪些地图点在当前帧视野中
extractKeyPoints();
computeDescriptors();
featureMatching();
poseEstimationPnP();
if ( checkEstimatedPose() == true ) // a good estimation
{
curr_->T_c_w_ = T_c_w_estimated_; // 更新当前帧的位子估计值
optimizeMap(); // 优化地图
num_lost_ = 0;
if ( checkKeyFrame() == true ) // is a key-frame
{
addKeyFrame();
}
}
else // bad estimation due to various reasons
{
num_lost_++;
if ( num_lost_ > max_num_lost_ )
{
state_ = LOST;
}
return false;
}
break;
}
地图点的管理策略：
- 地图点不在当前帧的视野范围内，则删除地图点
- 地图点被成功特征匹配上的次数和被观测到的次数之比小于阈值，说明这个地图点的匹配特性不好，则删除地图点：被观测到的次数是在进行特征匹配的时候，首先判断地图点是否在当前相机视野内，如果在那么被观测的次数+1。被成功特征匹配上的次数是在求解PnP的时候，使用
solvePnPRansac保留了正确匹配的地图点，此时地图点成功匹配的次数+1。 - 当前相机光心指向地图点的方向向量，和地图点被加入到地图时观测到它的那帧图像的相机光心指向地图点的方向向量，如果二者角度差大于30度，说明此时观测地图点和首次观测到这个地图点角度差比较大了，删除地图点。为什么？比较大了不应该是更准确吗？
- 如果当前帧匹配到的2D特征点数目太少，那么就把当前帧特征点全部反投影到世界坐标系中，生成新的地图点，并添加到地图中。
- 如果当前地图中的地图点数量太多了，那么就提高第二条擦除的比例，就是成功匹配的次数/观测的次数比例要大，这样才是好的地图点。
- 如果这个地图点不是一个好点，比如这个地图点的深度值非常不准确，那么可以根据两次观测到的特征点的匹配进行三角化得到一个更加准确的深度值，从而修正RGB-D的深度值。
这个地图点的管理策略在后面的博客SLAM十四讲—前端0.4----VO+局部地图中有人也整理了（这个博主写的这几篇博客都非常好，值得一看），内容如下：
三、相关博客整理
SLAM十四讲—前端0.1—相机/帧/地图点/地图/配置类的定义
聚焦RDK与具身智能，汇集机器人开发教程、开源案例和技术交流
更多推荐
1
0- 0
已为社区贡献16条内容
所有评论(0)