第九章0.1解读

2017年10月16日 23:00:48

阅读数：351

首先上一张框架图，0.1版本主要是对各文件进行归类和整体框架分布。
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 model
class 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了，去寻找depth
double 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_H

namespace 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 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] );
    }
};
}

#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 第九章0.1解读

第九章0.1解读

猜你喜欢