来自《slam十四讲》
头文件都放在一个myslam的命名空间内
common_include.h包含共同使用的头文件，减少多个类中引用头文件的复杂性
#ifndef COMMON_INCLUDE_H #define COMMON_INCLUDE_H // define the commonly included file to avoid a long include list // for Eigen #includecamera.h:#include using Eigen::Vector2d; using Eigen::Vector3d; // for Sophus #include using Sophus::SE3; // for cv #include using cv::Mat; // std #include #include #include
#include #include #include #include #include
camera类存储相机的内参和外参，完成相机坐标系，像素坐标系，世界坐标系之间坐标变换。这里世界坐标系需要相机变动的外参与参数方式传入。
#ifndef CAMERA_H
#define CAMERA_H
#include "myslam/common_include.h"
namespace myslam
{
// Pinhole RGBD camera model
class Camera
{
public:
//采用智能指针shared_ptr的方式指向自身，可以避免内存泄露和空指针等问题
//定义方式：Camera::Ptr camera_=New(Camera);
typedef std::shared_ptr Ptr;
float fx_, fy_, cx_, cy_, depth_scale_; // Camera intrinsics
//构造函数需要先读取数据，depth_scale初值为0
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 外参;p_w point_world;p_c point_camera;p_p point_pixel;depth 深度信息
//凡是涉及到世界坐标系的都需要外参T_c_w
// 2 就是 to
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
#include "myslam/camera.h"
//在myslam命名空间下
namespace myslam
{
Camera::Camera()
{
}
//世界to相机：T_c_w变换矩阵 乘 世界坐标系的点位置
Vector3d Camera::world2camera ( const Vector3d& p_w, const SE3& T_c_w )
{
return T_c_w * p_w;
}
//相机to世界：T_c_w变换矩阵的逆 乘 相机坐标系的点位置
Vector3d Camera::camera2world ( const Vector3d& p_c, const SE3& T_c_w )
{
return T_c_w.inverse() *p_c;
}
//相机to坐标系：虽然p_c是3*1矩阵，但输出没问题，和p_c(n)一样
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_
);
}
//坐标系to相机：上式倒求
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
);
}
//世界to像素：world->camera->pixel
Vector2d Camera::world2pixel ( const Vector3d& p_w, const SE3& T_c_w )
{
return camera2pixel ( world2camera ( p_w, T_c_w ) );
}
//像素->世界：pixel->camera->wprld
Vector3d Camera::pixel2world ( const Vector2d& p_p, const SE3& T_c_w, double depth )
{
return camera2world ( pixel2camera ( p_p, depth ), T_c_w );
}
}
frame.h
由于 Frame 类是基本数据单元,在许多地方会用到它,但现在初期设计阶段,我们还不清楚以后可能新加的内容。所以这里的 Frame 类只提供基本的数据存储和接口。如果之后有新增的内容,我们就继续往里添加。
#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 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
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 创建帧
static Frame::Ptr createFrame();
// find the depth in depth map
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++) );
}
double Frame::findDepth ( const cv::KeyPoint& kp )
{
int x = cvRound(kp.pt.x);
int y = cvRound(kp.pt.y);
//.ptr模板函数定位像素值的方法
ushort d = depth_.ptr(y)[x];
if ( d!=0 )
{
return double(d)/camera_->depth_scale_;//除比例尺
}
else //某个点没采集到深度值(过远)
{
// check the nearby points
int dx[4] = {-1,0,1,0};
int dy[4] = {0,-1,0,1};
for ( int i=0; i<4; i++ )
{
d = depth_.ptr( y+dy[i] )[x+dx[i]];
if ( d!=0 )
{
return double(d)/camera_->depth_scale_;
}
}
}
return -1.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_ );
//取得p_cam为相机和物体实际距离，小于0 则false
if ( p_cam(2,0)<0 )
return false;
Vector2d pixel = camera_->world2pixel( pt_world, T_c_w_ );
//xy值都大于0并且小于color图的行、列，则为真
return pixel(0,0)>0 && pixel(1,0)>0
&& pixel(0,0)
mappoint.h
MapPoint 表示路标点。我们将估计它的世界坐标,并且我们会拿当前帧提取到的特征点与地图中的路标点匹配,来估计相机的运动,因此还需要存储它对应的描述子。此外,我们会记录一个点被观测到的次数和被匹配到的次数,作为评价它的好坏程度的指标。
#ifndef MAPPOINT_H
#define MAPPOINT_H
namespace myslam
{
class Frame;
class MapPoint
{
public:
typedef shared_ptr 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)
{
}
//初始化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.h
Map 类管理着所有的路标点,并负责添加新路标、删除不好的路标等工作。VO 的匹配过程只需要和 Map 打交道即可。当然 Map 也会有很多操作,但现阶段我们只定义主要的数据结构。
#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.cpp
#include "myslam/map.h"
namespace myslam
{
//插入关键帧
void Map::insertKeyFrame ( Frame::Ptr frame )
{
cout<<"Key frame size = "<id_) == keyframes_.end() )//关键帧里没有，插入
{
//以pair形式插入该帧，该pair包括id和frame
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.h
Config 类负责参数文件的读取,并在程序任意地方都可随时提供参数的值。所以我们把 Config 写成单件模式(Singleton)。它只有一个全局对象,当我们设置参数文件时,创建该对象并读取参数文件,随后就可以在任意地方访问参数值,最后在程序结束时自动销毁。
单件模式,也叫单例模式,可以说是设计模式中最简单的一种。顾名思义,就是创造独一无二的唯一的一个实例化的对象。为了防止多个对象的干涉，只需要一个对象就够了。
#ifndef CONFIG_H
#define CONFIG_H
#include "myslam/common_include.h"
namespace myslam
{
class Config
{
private:
static std::shared_ptr 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
config.cpp
#include "myslam/config.h"
namespace myslam
{
void Config::setParameterFile( const std::string& filename )
{
if ( config_ == nullptr )
config_ = shared_ptr(new Config);
config_->file_ = cv::FileStorage( filename.c_str(), cv::FileStorage::READ );
if ( config_->file_.isOpened() == false )//文件无法打开则报错
{
std::cerr<<"parameter file "<file_.release();
return;
}
}
Config::~Config()
{
if ( file_.isOpened() )
file_.release();
}
shared_ptr Config::config_ = nullptr;//智能全局指针
}
参考：更加多注释地学习视觉SLAM第九讲程序[1] - 古月居
