1.从CMake文件了解整体结构
(1)前置工作
0)文件结构
.
├── app
│ ├── CMakeLists.txt
│ └── run_kitti_stereo.cpp
├── CMakeLists.txt
├── cmake_modules
│ ├── FindCSparse.cmake
│ ├── FindG2O.cmake
│ └── FindGlog.cmake
├── config
│ └── default.yaml
├── include
│ └── myslam
│ ├── algorithm.h
│ ├── backend.h
│ ├── camera.h
│ ├── common_include.h
│ ├── config.h
│ ├── dataset.h
│ ├── feature.h
│ ├── frame.h
│ ├── frontend.h
│ ├── g2o_types.h
│ ├── map.h
│ ├── mappoint.h
│ ├── viewer.h
│ └── visual_odometry.h
├── src
│ ├── backend.cpp
│ ├── camera.cpp
│ ├── CMakeLists.txt
│ ├── config.cpp
│ ├── dataset.cpp
│ ├── feature.cpp
│ ├── frame.cpp
│ ├── frontend.cpp
│ ├── map.cpp
│ ├── mappoint.cpp
│ ├── viewer.cpp
│ └── visual_odometry.cpp
└── test
├── CMakeLists.txt
└── test_triangulation.cpp
1)系统变量
PROJECT_BINARY_DIR:编译发生的当前目录
PROJECT_SOURCE_DIR:工程所在目录
2)SET指令
显式定义变量
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
3)ADD_EXECUTABLE指令
4)ADD_SUBDIRECTORY指令
1 ADD_SUBDIRECTOR(src bin)
:源文件夹src编译的文件将保存在/build/bin下
2 ADD_SUBDIRECTOR(src)
:源文件夹src编译的文件将保存在/build/src下
5)LIST指令
告诉cmake
在哪儿去找的.cmake
文件
6)INCLUDE_DIRECTORIES指令
寻找库的头文件
指定路径:
include_directories("/usr/local/include/eigen3")
若使用FIND_PACKAGE:
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
7)TARGET_LINK_LIBRARIES指令
添加需要链接的共享库
第一个参数:源文件
第二个参数:所依赖的库
target_link_libraries(myslam ${THIRD_PARTY_LIBS})
8)ADD_LIBRARY指令
添加名为…的库,库的源文件可指定
第一个参数:库的名字
第二个参数:库的源文件
add_library(myslam
frame.cpp
mappoint.cpp
)
9)ADD_TEST指令
需要与ENABLE_TESTRING()配合使用
10)FOREACH——ENDFOREACH
FOREACH (test_src ${TEST_SOURCES})
ADD_EXECUTABLE(${test_src} ${test_src}.cpp)
TARGET_LINK_LIBRARIES(${test_src} ${THIRD_PARTY_LIBS} myslam)
ADD_TEST(${test_src} ${test_src})
ENDFOREACH (test_src)
(2)分析
1)最外层CMakeLists.txt
1 把所需要的库添加进来
2 自己编写程序的头文件添加进来
3 编译的中间位文件做好分组
4 将添加进来的库打个包,为内层调用作准备
2)/src/CMakeLists.txt
1 src文件夹下存放源代码文件
2 将源码文件与库链接起来
3 这些文件本身也可以看成库
3)/test/CMakeLists.txt
测试文件
4)app/CMakeLists.txt
1 主程序
2 调用编写的文件以及库文件
2.前置知识
(1)智能指针
shared_ptr
基于引用计数的共享内存解决方案
1)基本用法
# include <iostream>
# include <new>
# include <memory>
int main ( )
{
std:: shared_ptr< int > x ( new int ( 3 ) ) ;
std:: cout << x. use_count ( ) << std:: endl;
std:: shared_ptr< int > y = x;
std:: cout << y. use_count ( ) << std:: endl;
std:: cout << x. use_count ( ) << std:: endl;
}
# include <iostream>
# include <new>
# include <memory>
std:: shared_ptr< int > fun ( )
{
std:: shared_ptr< int > res ( new int ( 100 ) ) ;
return res;
}
int main ( )
{
auto y = fun ( ) ;
}
weak_ptr
防止循环引用而引入的智能指针
循环引用
# include <iostream>
# include <new>
# include <memory>
struct Str
{
std:: shared_ptr< Str> m_nei;
~ Str ( )
{
std:: cout << "~Str() is called!\n" ;
}
} ;
int main ( )
{
std:: shared_ptr< Str> x ( new Str{ } ) ;
std:: shared_ptr< Str> y ( new Str{ } ) ;
x-> m_nei = y;
y-> m_nei = x;
}
(1)基于 shared_ptr 构造
# include <iostream>
# include <new>
# include <memory>
struct Str
{
std:: weak_ptr< Str> m_nei;
~ Str ( )
{
std:: cout << "~Str() is called!\n" ;
}
} ;
int main ( )
{
std:: shared_ptr< Str> x ( new Str{ } ) ;
std:: shared_ptr< Str> y ( new Str{ } ) ;
x-> m_nei = y;
y-> m_nei = x;
}
(2)lock方法
# include <iostream>
# include <new>
# include <memory>
struct Str
{
std:: weak_ptr< Str> m_nei;
~ Str ( )
{
std:: cout << "~Str() is called!\n" ;
}
} ;
int main ( )
{
std:: shared_ptr< Str> x ( new Str{ } ) ;
std:: shared_ptr< Str> y ( new Str{ } ) ;
x-> m_nei = y;
y-> m_nei = x;
if ( auto ptr = x-> m_nei. lock ( ) ; ptr)
{
std:: cout << "Can access pointer\n" ;
}
else
{
std:: cout << "Cannot access pointer\n" ;
}
}
unordered_map
与map类似,与 set / map 相比查找性能更好,但插入操作一些情况下会慢
1树中的每个结点是一个 std::pair
# include <iostream>
# include <map>
int main ( )
{
std:: map< char , int > m{ { 'a' , 3 } , { 'b' , 4 } } ;
for ( auto ptr = m. begin ( ) ; ptr != m. end ( ) ; ptr++ )
{
auto p = * ptr;
std:: cout << p. first << ' ' << p. second << std:: endl;
}
for ( auto p : m)
{
std:: cout << p. first << ' ' << p. second << std:: endl;
}
for ( auto [ k, v] : m)
{
std:: cout << k << ' ' << v << std:: endl;
}
}
2键 (pair.first) 需要支持使用 < 比较大小,或者采用自定义的比较函数来引入大小关系
3访问元素: find / contains / [] / at
# include <iostream>
# include <map>
int main ( )
{
std:: map< int , bool > m;
m. insert ( std:: pair < const int , bool > ( 3 , true ) ) ;
auto ptr = m. find ( 3 ) ;
std:: cout << ptr-> first << '' << ptr-> second;
std:: cout << m[ 3 ] << std:: endl;
}
(2)多线程及线程锁
1.多线程
(1)构造方式
假如直接执行run()函数,那么由于run函数内部是一个无限循环(A),则主函数中的无限循环(B)永远无法被执行;通过新开一个线程,A与 B同时执行了。
# include <iostream>
# include <thread>
class A
{
public :
void run ( )
{
while ( true )
{
std:: cout << "AAAA" << std:: endl;
std:: this_thread:: sleep_for ( std:: chrono:: milliseconds ( 50 ) ) ;
}
}
} ;
int main ( )
{
A aa;
std:: thread thr ( & A:: run, & aa) ;
while ( true )
{
std:: cout << "BBBB" << std:: endl;
std:: this_thread:: sleep_for ( std:: chrono:: milliseconds ( 50 ) ) ;
}
return 0 ;
}
(2)join()方法
阻塞当前线程直至 执行完毕再执行下面代码。
# include <iostream>
# include <thread>
class A
{
public :
void run ( )
{
while ( true )
{
std:: cout << "AAAA" << std:: endl;
std:: this_thread:: sleep_for ( std:: chrono:: milliseconds ( 50 ) ) ;
}
}
} ;
int main ( )
{
A * aa = new A;
std:: thread thr ( & A:: run, aa) ;
thr. join ( ) ;
return 0 ;
}
2.互斥
(1)unique_lock
3.从相机数据开始
(1)KITTI Odometry数据集
传感器安装位置
Kitti传感器主要包含相机(Cam0-4)、GPS/IMU、激光雷达(Velodyne Laserscanner)。本课题中只用到了相机,所以只讨论相机。
在calib.txt文件中P0到P4分别对应四个相机数据,其中,P0的原点为车辆坐标系原点,P0, P1, P2, P3 分别代表对应的相机内参矩阵, 大小为 3x4:
P
r
e
c
t
i
=
[
f
u
i
0
c
u
i
−
f
u
i
b
x
i
0
f
v
i
c
v
i
0
0
0
1
0
]
(3)
P^{i}_{rect}= \left[ \begin{matrix} f^i_u & 0 & c^i_u & -f^i_ub^i_x \\ 0 & f^i_v & c^i_v & 0 \\ 0 & 0 & 1 & 0 \end{matrix} \right] \tag{3}
P rec t i = ⎣
⎡ f u i 0 0 0 f v i 0 c u i c v i 1 − f u i b x i 0 0 ⎦
⎤ ( 3 ) 第四列数据
−
f
u
i
b
x
i
-f^i_ub^i_x
− f u i b x i 中的
b
x
i
b^i_x
b x i 表示此相机相对与车辆坐标系原点的距离,只有平移没有旋转。
通过 b ,将四个相机的相对关系描述出来。
(2)相机模型
此部分为复习,复习课本知识并思考如何融入项目之中。
1 相机内参数
由KITTI数据集给出
2 相机外参数
对于现实世界的空间点P,我们需要将其转换到相机坐标系:
P
c
=
T
c
w
P
w
P_c = T_{cw}P_w
P c = T c w P w Tips:世界坐标系不变
3 具体方法
1)世界坐标系中的点P转换到相机坐标系
2)相机坐标系中的点
P
c
P_c
P c 转换到像素坐标系
4 Camera类的构造
相机内参数以及双目相机基线长度由数据集给出,唯一一个不确定的参数便是相机的位姿。
通过数据集对相机位姿初始化:由数据集可以得到,Cam0 为世界坐标系的原点,
b
x
i
b_x^i
b x i 为 第 i 个相机与Cam0的平移关系,旋转关系由于安装原因没有旋转。
4.基本数据结构
需要处理的数据:
1 图像:双目视觉中的一对图像,可以称为一帧。
2 提取特征
3 通过特征计算3D位置,即路标。
(1)图像(帧)
1 构造函数
通过构造函数,我认为能够迅速了解一个类。
1 对于一系列的图像,可以为其编号
2 该帧出现的时间
3 关键帧
4 相机的位姿
5 读入的左右图像
Frame :: Frame ( long id, double time_stamp, const SE3 & pose, const Mat & left, const Mat & right)
: id_ ( id) , time_stamp_ ( time_stamp) , pose_ ( pose) , left_img_ ( left) , right_img_ ( right) { }
typedef std:: shared_ptr< Frame> Ptr;
unsigned long id_ = 0 ;
unsigned long keyframe_id_ = 0 ;
bool is_keyframe_ = false ;
double time_stamp_;
SE3 pose_;
std:: mutex pose_mutex_;
cv:: Mat left_img_, right_img_;
2 创建帧
分配帧id以及关键帧id
Frame:: Ptr Frame :: CreateFrame ( ) {
static long factory_id = 0 ;
Frame:: Ptr new_frame ( new Frame) ;
new_frame-> id_ = factory_id++ ;
return new_frame;
}
void Frame :: SetKeyFrame ( ) {
static long keyframe_factory_id = 0 ;
is_keyframe_ = true ;
keyframe_id_ = keyframe_factory_id++ ;
}
3 工厂模式
Frame:: Ptr Frame :: CreateFrame ( ) {
static long factory_id = 0 ;
Frame:: Ptr new_frame ( new Frame) ;
new_frame-> id_ = factory_id++ ;
return new_frame;
}
(2)从帧中提取特征
特征充当了一个中介,连接起3维世界中的点与图像中的点。
那么最主要的信息便是从帧图像中提取到的2D位置,哪一个帧有此特征以及对应的3D路标也是需要考虑的。
typedef std:: shared_ptr< Feature> Ptr;
std:: weak_ptr< Frame> frame_;
cv:: KeyPoint position_;
std:: weak_ptr< MapPoint> map_point_;
bool is_outlier_ = false ;
bool is_on_left_image_ = true ;
Feature ( std:: shared_ptr< Frame> frame, const cv:: KeyPoint & kp)
: frame_ ( frame) , position_ ( kp) { }
(3)路标点
路标点最重要的信息是它的3D位置,其次是被哪些特征所观察。
1 构造函数
ID,3D位置
typedef std:: shared_ptr< MapPoint> Ptr;
unsigned long id_ = 0 ;
bool is_outlier_ = false ;
Vec3 pos_ = Vec3 :: Zero ( ) ;
std:: mutex data_mutex_;
int observed_times_ = 0 ;
std:: list< std:: weak_ptr< Feature>> observations_;
MapPoint :: MapPoint ( long id, Vec3 position) : id_ ( id) , pos_ ( position) { }
2 创建路标点
工厂模式创建路标点,可以看出与创建帧有相似性
MapPoint:: Ptr MapPoint :: CreateNewMappoint ( ) {
static long factory_id = 0 ;
MapPoint:: Ptr new_mappoint ( new MapPoint) ;
new_mappoint-> id_ = factory_id++ ;
return new_mappoint;
}
Vec3 Pos ( ) {
std:: unique_lock< std:: mutex> lck ( data_mutex_) ;
return pos_;
}
void SetPos ( const Vec3 & pos) {
std:: unique_lock< std:: mutex> lck ( data_mutex_) ;
pos_ = pos;
} ;
3 观察点
void AddObservation ( std:: shared_ptr< Feature> feature) {
std:: unique_lock< std:: mutex> lck ( data_mutex_) ;
observations_. push_back ( feature) ;
observed_times_++ ;
}
void MapPoint :: RemoveObservation ( std:: shared_ptr< Feature> feat) {
std:: unique_lock< std:: mutex> lck ( data_mutex_) ;
for ( auto iter = observations_. begin ( ) ; iter != observations_. end ( ) ;
iter++ ) {
if ( iter-> lock ( ) == feat) {
observations_. erase ( iter) ;
feat-> map_point_. reset ( ) ;
observed_times_-- ;
break ;
}
}
}
(4)地图类
为了实际持有Frame和MapPoint对象,定义地图类。
1 成员变量
typedef std:: shared_ptr< Map> Ptr;
typedef std:: unordered_map< unsigned long , MapPoint:: Ptr> LandmarksType;
typedef std:: unordered_map< unsigned long , Frame:: Ptr> KeyframesType;
std:: mutex data_mutex_;
LandmarksType landmarks_;
LandmarksType active_landmarks_;
KeyframesType keyframes_;
KeyframesType active_keyframes_;
Frame:: Ptr current_frame_ = nullptr ;
int num_active_keyframes_ = 7 ;
2 实现功能
void InsertKeyFrame ( Frame:: Ptr frame) ;
void InsertMapPoint ( MapPoint:: Ptr map_point) ;
LandmarksType GetAllMapPoints ( ) {
std:: unique_lock< std:: mutex> lck ( data_mutex_) ;
return landmarks_;
}
KeyframesType GetAllKeyFrames ( ) {
std:: unique_lock< std:: mutex> lck ( data_mutex_) ;
return keyframes_;
}
LandmarksType GetActiveMapPoints ( ) {
std:: unique_lock< std:: mutex> lck ( data_mutex_) ;
return active_landmarks_;
}
KeyframesType GetActiveKeyFrames ( ) {
std:: unique_lock< std:: mutex> lck ( data_mutex_) ;
return active_keyframes_;
}
void Map :: InsertKeyFrame ( Frame:: Ptr frame) {
current_frame_ = frame;
if ( keyframes_. find ( frame-> keyframe_id_) == keyframes_. end ( ) ) {
keyframes_. insert ( make_pair ( frame-> keyframe_id_, frame) ) ;
active_keyframes_. insert ( make_pair ( frame-> keyframe_id_, frame) ) ;
} else {
keyframes_[ frame-> keyframe_id_] = frame;
active_keyframes_[ frame-> keyframe_id_] = frame;
}
if ( active_keyframes_. size ( ) > num_active_keyframes_) {
RemoveOldKeyframe ( ) ;
}
}
void Map :: InsertMapPoint ( MapPoint:: Ptr map_point)
{
if ( landmarks_. find ( map_point-> id_) == landmarks_. end ( ) ) {
landmarks_. insert ( make_pair ( map_point-> id_, map_point) ) ;
active_landmarks_. insert ( make_pair ( map_point-> id_, map_point) ) ;
} else {
landmarks_[ map_point-> id_] = map_point;
active_landmarks_[ map_point-> id_] = map_point;
}
}
void Map :: RemoveOldKeyframe ( ) {
if ( current_frame_ == nullptr ) return ;
double max_dis = 0 , min_dis = 9999 ;
double max_kf_id = 0 , min_kf_id = 0 ;
auto Twc = current_frame_-> Pose ( ) . inverse ( ) ;
for ( auto & kf : active_keyframes_) {
if ( kf. second == current_frame_) continue ;
auto dis = ( kf. second-> Pose ( ) * Twc) . log ( ) . norm ( ) ;
if ( dis > max_dis) {
max_dis = dis;
max_kf_id = kf. first;
}
if ( dis < min_dis) {
min_dis = dis;
min_kf_id = kf. first;
}
}
const double min_dis_th = 0.2 ;
Frame:: Ptr frame_to_remove = nullptr ;
if ( min_dis < min_dis_th) {
frame_to_remove = keyframes_. at ( min_kf_id) ;
} else {
frame_to_remove = keyframes_. at ( max_kf_id) ;
}
LOG ( INFO) << "remove keyframe " << frame_to_remove-> keyframe_id_;
active_keyframes_. erase ( frame_to_remove-> keyframe_id_) ;
for ( auto feat : frame_to_remove-> features_left_) {
auto mp = feat-> map_point_. lock ( ) ;
if ( mp) {
mp-> RemoveObservation ( feat) ;
}
}
for ( auto feat : frame_to_remove-> features_right_) {
if ( feat == nullptr ) continue ;
auto mp = feat-> map_point_. lock ( ) ;
if ( mp) {
mp-> RemoveObservation ( feat) ;
}
}
CleanMap ( ) ;
}
void Map :: CleanMap ( ) {
int cnt_landmark_removed = 0 ;
for ( auto iter = active_landmarks_. begin ( ) ;
iter != active_landmarks_. end ( ) ; ) {
if ( iter-> second-> observed_times_ == 0 ) {
iter = active_landmarks_. erase ( iter) ;
cnt_landmark_removed++ ;
} else {
++ iter;
}
}
LOG ( INFO) << "Removed " << cnt_landmark_removed << " active landmarks" ;
}
感受:代码读下来只能说需要时间去理解,脑子里隐约有了个关于框架,基本的参数以及参数的功能有了一定的了解,但是对于一些辅助参数比如各种 ID,时间戳等在自己编写程序的时候能够思考加上,以及选用什么样的容器也是需要思考的地方。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)