第九章0.1解读
来源:互联网 发布:mac下载os x yosemite 编辑:程序博客网 时间:2024/05/01 14:16
首先上一张框架图,0.1版本主要是对各文件进行归类和整体框架分布。
看一下文件夹组织:
1、bin用来存放可执行的二进制文件。当然0.1版本还没有此文件夹,因为0.1版本主要是搭建SLAM库框架
2、include/myslam存放SLAM模块的头文件,主要是.h文件。当把包含目录设为include,引用自己的头文件时,用include”myslam/xxx.h”,这样不会跟其他库混淆。看图发现就是我们自己定义的一些类的头文件
3、src存放源代码文件,主要是.cpp文件。看图发现是自定义类的实现.cpp文件
4、test文件是测试SLAM库的文件,也是.cpp文件。也就是最后跑的实现程序,在这个程序中我们调用了自己写的SLAM库,所以叫test。当然这里也只有一个CMakelists.txt,因为还没有开始写
5、lib存放编译好的库文件
6、config存放配置文件,也就是需要经常修改的运行参数。这里存放在default.yaml中
7、cmake_modules第三方库的cmake文件,在使用g2o之类的库时会用到。
上一张类结构图:
这里依次说一下这些类:
Camera类,存储相机的内参和外参,有三坐标系(世界坐标系、相机坐标系、像素坐标系)下点的坐标转换函数。在有世界坐标系的情况下需要相机外参,这里当做外部参数传入。
上代码
camera.h:
#ifndef CAMERA_H#define CAMERA_H#include "myslam/common_include.h"namespace myslam{// Pinhole RGBD camera modelclass Camera{public: //这里定义指向自身类的指针,然后构造的时候这样用: //Camera::Ptr camera_ = New ( Camera ); typedef std::shared_ptr<Camera> Ptr; //float类型的相机内参 float fx_, fy_, cx_, cy_, depth_scale_; // Camera intrinsics //构造函数将数据读进来,注意看depth_scale是有默认值的。 Camera(); Camera ( float fx, float fy, float cx, float cy, float depth_scale=0 ) : fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), depth_scale_ ( depth_scale ) {} // coordinate transform: world, camera, pixel //世界、相机、像素三坐标系变换,涉及到世界坐标的都有外参T_c_w,涉及到由像素坐标向外转换都有深度值depth,因为像素坐标少一个深度维度,需要外参传入 Vector3d world2camera( const Vector3d& p_w, const SE3& T_c_w ); Vector3d camera2world( const Vector3d& p_c, const SE3& T_c_w ); Vector2d camera2pixel( const Vector3d& p_c ); Vector3d pixel2camera( const Vector2d& p_p, double depth=1 ); Vector3d pixel2world ( const Vector2d& p_p, const SE3& T_c_w, double depth=1 ); Vector2d world2pixel ( const Vector3d& p_w, const SE3& T_c_w );};}#endif // CAMERA_H
camera.cpp:
cpp没啥说的,主要就是6个坐标变换函数的实现,很简单,原理就是相机投影过程。方法上就是多利用已经定义好的函数,一是不容出错,二是比较简单明了。比如像素坐标和世界坐标准换时,就利用中间步骤定义的函数。
#include "myslam/camera.h"namespace myslam{Camera::Camera(){}Vector3d Camera::world2camera ( const Vector3d& p_w, const SE3& T_c_w ){ return T_c_w*p_w;}Vector3d Camera::camera2world ( const Vector3d& p_c, const SE3& T_c_w ){ return T_c_w.inverse() *p_c;}Vector2d Camera::camera2pixel ( const Vector3d& p_c ){ return Vector2d ( fx_ * p_c ( 0,0 ) / p_c ( 2,0 ) + cx_, fy_ * p_c ( 1,0 ) / p_c ( 2,0 ) + cy_ );}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 );}Vector2d Camera::world2pixel ( const Vector3d& p_w, const SE3& T_c_w ){ return camera2pixel ( world2camera ( p_w, T_c_w ) );}Vector3d Camera::pixel2world ( const Vector2d& p_p, const SE3& T_c_w, double depth ){ return camera2world ( pixel2camera ( p_p, depth ), T_c_w );}}
Frame类,帧,基本数据单元,也就是相机采集到的一张张图像,由于这里是RGBD相机,所以是一对图像(color图和depth图)。
frame.h
#ifndef FRAME_H#define FRAME_H#include "myslam/common_include.h"#include "myslam/camera.h"namespace myslam {// forward declare class MapPoint;class Frame{public: typedef std::shared_ptr<Frame> Ptr; unsigned long id_; // id of this frame double time_stamp_; // when it is recorded SE3 T_c_w_; // transform from world to camera Camera::Ptr camera_; // Pinhole RGBD Camera model 这就是前面定义的camera类型的对象了。 Mat color_, depth_; // color and depth image public: // data members Frame(); Frame( long id, double time_stamp=0, SE3 T_c_w=SE3(), Camera::Ptr camera=nullptr, Mat color=Mat(), Mat depth=Mat() ); ~Frame(); // factory function 创建函数?从实现来看就是调用了默认构造函数创建了个空白帧对象,唯一赋值的参数就是给id_赋了个值。 static Frame::Ptr createFrame(); // find the depth in depth map功能函数,由color图找到对应像素的深度值 double findDepth( const cv::KeyPoint& kp ); // Get Camera Center功能函数,求取相机光心 Vector3d getCamCenter() const; // check if a point is in this frame 功能函数,判断某点是否在相机内 bool isInFrame( const Vector3d& pt_world );};}#endif // FRAME_H
frame.cpp
#include "myslam/frame.h"namespace myslam{Frame::Frame(): id_(-1), time_stamp_(-1), camera_(nullptr){}Frame::Frame ( long id, double time_stamp, SE3 T_c_w, Camera::Ptr camera, Mat color, Mat depth ): id_(id), time_stamp_(time_stamp), T_c_w_(T_c_w), camera_(camera), color_(color), depth_(depth){}Frame::~Frame(){}Frame::Ptr Frame::createFrame(){ static long factory_id = 0; //这里注意下,由factory_id++一个数去构造Frame对象时,调用的是默认构造函数,由于默认构造函数全都有默认值,所以就是按坑填,先填第一个id_, //所以也就是相当于创建了一个只有ID号的空白帧。 return Frame::Ptr( new Frame(factory_id++) );}//注意看这个函数参数类型是cv::KeyPoint&,所以也就是color图已经detect出keypoint了,去寻找depthdouble Frame::findDepth ( const cv::KeyPoint& kp ){ int x = cvRound(kp.pt.x); int y = cvRound(kp.pt.y); ushort d = depth_.ptr<ushort>(y)[x];//这个还是.ptr模板函数定位像素值的方法,记住用法 if ( d!=0 ) { return double(d)/camera_->depth_scale_;//除以比例尺之后return } else //为0的情况呢?这些应该都是实际中有可能遇到的坑,需要总结。因为rgbd相机极有可能某个点没采集到深度值 { // check the nearby points 此像素为零的话访问下上下左右的像素,然后return值 int dx[4] = {-1,0,1,0}; int dy[4] = {0,-1,0,1}; for ( int i=0; i<4; i++ ) { d = depth_.ptr<ushort>( y+dy[i] )[x+dx[i]]; if ( d!=0 ) { return double(d)/camera_->depth_scale_; } } } return -1.0;//如果还没有,就返回-1.0,表示访问失败}//取相机光心的话,这里瞪大眼看!.translation()是取平移部分!不是取转置!!!!!!!!!T_c_w_.inverse()求出来的平移部分就是R^(-1)*(-t),也就是相机坐标系下的(0,0,0)在世界坐标系下的坐标,也就是相机光心的世界坐标!!!!!!Vector3d Frame::getCamCenter() const{ return T_c_w_.inverse().translation();}bool Frame::isInFrame ( const Vector3d& pt_world ){ Vector3d p_cam = camera_->world2camera( pt_world, T_c_w_ ); if ( p_cam(2,0)<0 ) //这步是取得Z值,小于0直接return false return false; Vector2d pixel = camera_->world2pixel( pt_world, T_c_w_ ); return pixel(0,0)>0 && pixel(1,0)>0 //xy值都大于0并且小于color图的行列 && pixel(0,0)<color_.cols && pixel(1,0)<color_.rows;}}
MapPoint类,为路标点,也就是Map类的单位成员,许多mappoint构成了一个Map。norm_不知道是干啥用的?~~
MapPoint.h
#ifndef MAPPOINT_H#define MAPPOINT_Hnamespace myslam{class Frame;class MapPoint{public: typedef shared_ptr<MapPoint> Ptr; unsigned long id_; // ID Vector3d pos_; // Position in world Vector3d norm_; // Normal of viewing direction Mat descriptor_; // Descriptor for matching int observed_times_; // being observed by feature matching algo. int correct_times_; // being an inliner in pose estimation被匹配的次数 MapPoint(); MapPoint( long id, Vector3d position, Vector3d norm ); // factory function static MapPoint::Ptr createMapPoint();};}#endif // MAPPOINT_H
MapPoint.cpp
#include "myslam/common_include.h"#include "myslam/mappoint.h"namespace myslam{MapPoint::MapPoint(): id_(-1), pos_(Vector3d(0,0,0)), norm_(Vector3d(0,0,0)), observed_times_(0), correct_times_(0){}MapPoint::MapPoint ( long id, Vector3d position, Vector3d norm ): id_(id), pos_(position), norm_(norm), observed_times_(0), correct_times_(0){}//这里的方法函数,还是跟默认构造函数类似,创建了一个零点MapPoint::Ptr MapPoint::createMapPoint(){ static long factory_id = 0; return MapPoint::Ptr( new MapPoint( factory_id++, Vector3d(0,0,0), Vector3d(0,0,0) ) );}}
Map类,一个Map就是包括很多mappoint的集合。Map类管理所有的路标点,并负责添加新路标、删除不好路标等操作,VO的匹配只要和Map交互就好了。由于存储了各个关键帧和路标点,所以需要随机访问并随机插入和删除,所以这里用散列(Hash)来存储,也就是键值对。程序中就是这一句:
unordered_map<unsigned long, MapPoint::Ptr > map_points_;
map.h
#ifndef MAP_H#define MAP_H#include "myslam/common_include.h"#include "myslam/frame.h"#include "myslam/mappoint.h"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 );};}#endif // MAP_H
map.cpp
#include "myslam/map.h"namespace myslam{void Map::insertKeyFrame ( Frame::Ptr frame ){ cout<<"Key frame size = "<<keyframes_.size()<<endl; if ( keyframes_.find(frame->id_) == keyframes_.end() ) { keyframes_.insert( make_pair(frame->id_, frame) ); } else { keyframes_[ frame->id_ ] = frame; }}void Map::insertMapPoint ( MapPoint::Ptr map_point ){ if ( map_points_.find(map_point->id_) == map_points_.end() ) { map_points_.insert( make_pair(map_point->id_, map_point) ); } else { map_points_[map_point->id_] = map_point; }}}
config类,
头文件中,构造函数声明为私有,这样就只能由类成员函数去调用构造函数创建对象,也就是setParameterFile()这个函数,这个函数的实现中有一句new Config ,调用了私有的默认构造函数,创建了一个指向config的智能指针。
文件读取使用OpenCV提供的FileStorage类,它可以读取YAML文件,通过模板函数get来获取任意类型参数值。
.h
#ifndef CONFIG_H#define CONFIG_H#include "myslam/common_include.h" namespace myslam {class Config{private: static std::shared_ptr<Config> config_; cv::FileStorage file_; Config () {} // private constructor makes a singletonpublic: ~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] ); }};}#endif // CONFIG_H
.cpp。单例模式的全局指针,就在此源文件中。单例模式可以保证整个程序中只有一个全局对象,设置参数文件时被创建,程序结束时被销毁。
这里的setParameterFile()就是将文件打开链接上,用于读取数据。
#include "myslam/config.h"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 );//cv::FileStorage()用于读取文件 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;//这里就是全局指针。}
读取数据时也很简单:
myslam::Config::setParameterFile("parameter.yaml");double fx = myslam::Config::get<double>("Camera.fx");
- 第九章0.1解读
- F28027第九课---SPI操作解读
- Bootstrap源码解读(第九弹:进度条)
- 第九章
- 第九章
- 第九章
- 第九章
- 第九章
- 第九章
- 第九章
- 第九章
- 第九章
- 第九章
- 第九章
- 第九章
- 第九章
- 第九章
- 第九章
- Codeforces-242C-King's Path(bfs)
- virtualbox centos7/ubuntu 网络配置
- ProjectForge研究第二天:工程及模块
- Java三方库记录
- 编程检验ASCII码值与字符对应关系 P11
- 第九章0.1解读
- Codeforces Round #441 (Div.2)
- 算数运算符和算术表达式
- Cygwin 和MinGW 的区别与联系是怎样的?
- Codeforces Round #149 (Div. 2) (bfs+STL)
- Fault Injection——持续更新
- 游戏‘微信打飞机 第一课
- 关于win10下搭建kafka0.10的一些坑
- 阿里巴巴Java开发规约插件安装IDEA和Eclipse教程