手撕 视觉slam14讲 ch13 代码（3）相机类、数据集类、参数配置类

我们首先从数据集读取入手，包括读取数据集的参数文件来获得相机的内参和外参,以及读取图片，因此，我们还需要抽象出相机类、数据集类、参数配置类。

相机类最简单，我们先来实现它

1.相机类

camera.h:

Camera类存储相机的内参和外参,并完成相机坐标系、像素坐标系、和世界坐标系之间的坐标变换。在抽象的过程中，我们分为参数和函数的确定，首先是参数：

• 智能指针
• 定义内参
• 基线
• 外参，双目到单目位姿变换
• 外参的逆，即单目到双目位姿变换

之后是构造函数和功能函数：

• 构造函数（无参）
• 构造函数（有参，初始化参数量）
• 取出位姿
• 读取内参矩阵K
• 世界to相机：T_c_w变换矩阵 乘 世界坐标系的点位置
• 相机to世界：T_c_w变换矩阵的逆 乘 相机坐标系的点位置
• 相机to像素
• 像素to相机
• 像素to世界
• 世界to像素

// Camera类存储相机的内参和外参,并完成相机坐标系、像素坐标系、和世界坐标系之间的坐标变换。

#pragma once

#ifndef MYSLAM_CAMERA_H
#define MYSLAM_CAMERA_H

#include"MYSLAM/common_include.h"

namespace MYSLAM{

class Camera{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//内存对齐
    typedef std::shared_ptr<Camera>Ptr;//智能指针
    double fx_=0,fy_=0,cx_=0, cy_=0,//定义内参
    baseline_=0;//基线
    SE3 pose_;//外参，双目到单目位姿变换 
    SE3 pose_inv_;

    // 构造函数（无参）
    Camera();
    // 构造函数（有参，初始化参数量）
    Camera(double fx, double fy, double cx, double cy, double baseline,
           const SE3 &pose  ) : fx_(fx), fy_(fy), cx_(cx), cy_(cy), baseline_(baseline), pose_(pose) {
            pose_inv_ = pose_.inverse();
           };

    // 取出位姿
    SE3 pose()const{
        return pose_;
    }

    //读取内参矩阵K
    Mat33 K()const{
        Mat33 k;
        k<<fx_,0,cx_,0,fy_,cy_,0,0,1;
        return k;
    }

    //世界to相机：T_c_w变换矩阵 乘 世界坐标系的点位置
    Vec3 world2camera(const Vec3 &p_w, const SE3 &T_c_w);
    
    //相机to世界：T_c_w变换矩阵的逆 乘 相机坐标系的点位置
    Vec3 camera2world(const Vec3 &p_c, const SE3 &T_c_w);

    //相机to像素
    Vec3 camera2pixel(const Vec3 &p_c);

    //像素to相机
    Vec3 pixel2camera(const Vec2 &p_p, double depth=1 );

    //像素to世界
    Vec3 pixel2world(const Vec2 &p_p,const SE3 &T_c_w, double depth=1);

    //世界to像素
    Vec3 world2pixel(const Vec3 &p_w,const SE3 &T_c_w);

};

}//namespace MYSLAM

#endif  // MYSLAM_CAMERA_H

camera.cpp:

#include"MYSLAM/camera.h"

namespace MYSLAM{

Camera::Camera(){};

//世界to相机：T_c_w变换矩阵 乘 世界坐标系的点位置
Vec3  Camera::world2camera(const Vec3 &p_w, const SE3 &T_c_w){
    return pose_*T_c_w*p_w;
}

//相机to世界：T_c_w变换矩阵的逆 乘 相机坐标系的点位置
Vec3 Camera::camera2world(const Vec3 &p_c, const SE3 &T_c_w){
    return T_c_w.inverse()*pose_inv_*p_c;
}

//相机to像素
Vec3 Camera::camera2pixel(const Vec3 &p_c){
    return Vec2(
        fx_*p_c(0,0)/p_c(2,0)+cx_,      // u = fx * X / Z+ cx
        fy_*p_c(1,0)/p_c(2,0)+cy_       // v = fy * Y / Z+ cy
    );
}

//像素to相机
Vec3 Camera::pixel2camera(const Vec2 &p_p, double depth ){
    return Vec3(
        (p_p(0,0)-cx_)*depth / fx_,    //  X=(u-cx)*z / fx
        (p_p(1,0)-cy_)*depth / fy_,     // Y=(v-cy)*z/ fy
        depth
    );
}

// 像素to世界
Vec3 Camera::pixel2world(const Vec2 &p_p,const SE3 &T_c_w, double depth){
    return camera2world(pixel2camera(p_p , depth),T_c_w);//像素to相机，相机to世界
}

//世界to像素
Vec3 Camera::world2pixel(const Vec3 &p_w,const SE3 &T_c_w){
    return camera2pixel(world2camera(p_w , T_c_w)); //世界to相机，相机to像素
}


}//namespace MYSL

2.参数类

config.h:

Config 类负责参数文件的读取,并在程序任意地方都可随时提供参数的值

同样参数：

• 智能指针
• 文件

函数：

• 构造函数：私有构造函数生成单例模式
• 析构函数：关闭文件，删除相关buff
• 打开一个新的文件
• 根据键值获取参数值

//  * 配置类，使用SetParameterFile确定配置文件
//  * 然后用Get得到对应值
//  * 单例模式
// Config 类负责参数文件的读取,并在程序任意地方都可随时提供参数的值。所以我们把 Config 写成单例模式(Singleton)。
// 它只有一个全局对象,当我们设置参数文件时,创建该对象并读取参数文件,随后就可以在任意地方访问参数值,最后在程序结束时自动销毁

#pragma once
#ifndef MYSLAM_CONFIG_H
#define MYSLAM_CONFIG_H

#include"MYSLAM/common_include.h"

namespace MYSLAM{

//配置文件类
class Config{

private:
static std::shared_ptr<Config>config_;//智能指针
cv::FileStorage file_;//文件
Config(){};//私有构造函数生成单例模式

public:
~Config(){};//析构函数：关闭文件，删除相关buff
//打开一个新的文件
static bool SetParameterFile(std::string &filename);

//函数模板， 模板是实现代码重用机制的一种工具，它可以实现类型参数化，即把类型定义为参数功能要求： 我们需要对int、char、string、double等类型的数据做交换操作，
//（ 有了模板功能，则只需要编写一个函数即可，编译器可以通过输入参数的类型，推断出形参的类型。）
template <typename T>

// 根据键值获取参数值
static T Get(const std::string &key) {
        return T(Config::config_->file_[key]);
    }
};

}//namespace MYSLAM

#endif  // MYSLAM_CONFIG_H

config..cpp: 

#include"MYSLAM/config.h"

namespace MYSLAM{

// 打开文件
bool Config::SetParameterFile(std::string &filename){
    //没有config则构建
    if (config_ == nullptr ){
        config_=std::shared_ptr<Config>(new Config);
    }
    // config_->file_就定义为cv::FileStorage形式，变量是filename.c_str(),参数是cv::Filestorage::READ.是只读模式，不修改。
    config_->file_=cv::FileStorage(filename.c_str(),cv::FileStorage::READ);
    // 如果文档打不开，就用std::cerr输出错误信息
    if (config_->file_.isOpened()==false)
    {
        LOG(ERROR) << "parameter file " << filename << " does not exist.";
        config_->file_.release();//关闭文件并删除buff
        return false;
    }
    return true;//如果打开，返回ture
}

//析构函数：关闭文件，删除相关buff
Config:: ~Config(){
    if (config_->file_.isOpened())
    {
       config_->file_.release();
    }
}

std::shared_ptr<Config> Config::config_ = nullptr;//智能全局指针

}//namespace MYSLAM

3.数据集类

dataset.h

Dataset类用于数据集读取，在构造时传入配置文件路径，初始化之后可获得相机和下一帧图像

参数：

• 智能指针
• 数据集路径
• 当前图像id
• 每一帧对应的相机参数

函数：

• 构造函数
• 初始化，返回是否成功
• 创建并返回下一帧
• 根据id返回相机参数

// 数据集读取
//  构造时传入配置文件路径，配置文件的dataset_dir为数据集路径
//  Init之后可获得相机和下一帧图像

#ifndef MYSLAM_DATASET_H
#define MYSLAM_DATASET_H

#include "MYSLAM/camera.h"
#include "MYSLAM/common_include.h"
#include "MYSLAM/frame.h"

namespace MYSLAM{

// 数据集读取类
class Dataset{

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef std::shared_ptr<Dataset>Ptr;//智能指针

    // 构造函数
    Dataset(const std::string &dataset_path){};

    // 初始化，返回是否成功
    bool Init();

    //创建并返回下一帧
    Frame::Ptr NextFrame();

    //根据id返回相机参数
    Camera::Ptr GetCamera(int camera_id) const { 
        return cameras_.at(camera_id);
    };

private:
// 参数
    std::string dataset_path_; //数据集路径
    int current_image_index_ =0 ;//当前图像id
    std::vector<Camera::Ptr>cameras_;//每一帧对应的相机参数（应该都是一样的
};
}//MYSLAM
#endif

 dataset.cpp：

#include"MYSLAM/dataset.h"
#include "MYSLAM/frame.h"

#include <boost/format.hpp>
#include <fstream>
#include <opencv2/opencv.hpp>

using namespace std;
namespace MYSLAM{

//构造函数
Dataset::Dataset (const std::string &dataset_path) : dataset_path_(dataset_path){}

// 初始化，返回是否成功
bool Dataset::Init(){
    // 从calib.txt中获取相机内外参
    // 00序列calib参数文件中一共包含了4个相机矩阵：其中P0代表0号左边灰度相机、P1代表1号右边灰度相机、P2代表2号左边彩色相机、P3代表3号右边彩色相机.每个相机12个参数组成的内参矩阵
    std::ifstream fin(dataset_path_ + "/calib.txt");//读取calib.txt
    if (!fin)
    {
        LOG(ERROR)<<"cannot find " << dataset_path_ << "/calib.txt!";
        return false;
    }
    // 遍历文件中的四个相机(P0,P1,P2,P3)
    for (int i = 0; i < 4; ++i)
    {
        //前三个字符是P0：所以这里定义了一个长度为3的字符数组，读完这三个字符后就遇到了第一个空格，fin将会跳过这个空格，读取参数
        char camera_name[3];
        for (int k = 0; k < 3; ++k)
        {
            fin >> camera_name[i];
        }
        double projection_data[12];
        for (int k = 0; i < 12; ++k)
        {
            fin >> projection_data[k];
        }
        //将相机后面对应的12个参数读入到projection_data[12]中
        Mat33  K;//内参矩阵,从相机坐标到像素坐标
        K << projection_data[0],projection_data[1],projection_data[2],
                  projection_data[4],projection_data[5],projection_data[6],
                  projection_data[8],projection_data[9],projection_data[10];
        Vec3 t;
        t << projection_data[4], projection_data[7], projection_data[11];

        t=K.inverse()*t;
        K=0.5*K;
        
        Camera::Ptr new_camera (new Camera (K(0,0),K(1,1),K(0,2),K(1,2), t.norm(),  SE3(SO3() , t)));
        cameras_.push_back(new_camera);
         LOG(INFO) << "Camera " << i << " extrinsics: " << t.transpose();
    }
    fin.close();
    current_image_index_=0;
    return true;
}

//创建并返回下一帧
Frame::Ptr Dataset::NextFrame(){
    boost::format fmt("%s/image_%d/%06d.png");//数据格式化
    // 1.读图
    cv::Mat image_left , image_right;
    image_left = cv::imread((fmt  % dataset_path_  % 0  % current_image_index_).str(), cv::IMREAD_GRAYSCALE);
    image_right = cv::imread((fmt  % dataset_path_  % 1  % current_image_index_).str(), cv::IMREAD_GRAYSCALE);

    // 如果左图或右图为空，则返回空指针
    if (image_left.data == nullptr || image_right.data == nullptr)
    {
        LOG(WARNING) << "cannot find images at index " << current_image_index_;       
        return nullptr;
    }
        
    // 2.改图大小
    cv::Mat image_left_resized , image_right_resized;
    cv::resize(image_left,image_left_resized, cv::Size(),0.5,0.5 ,cv::INTER_NEAREST);
    cv::resize(image_right,image_right_resized, cv::Size(),0.5,0.5 ,cv::INTER_NEAREST);

    // 工厂模式建立新帧
    auto new_frame =  Frame::CreateFrame();
    // 将改过尺寸的左右目图片赋给新帧
    new_frame ->left_img_ =image_left_resized;
    new_frame ->right_img_=image_right_resized;
    current_image_index_++;
    return new_frame;
}

}//namespace MYSLAM