SLAM14讲ch9前段设计

最新推荐文章于 2024-09-15 16:56:37 发布

weixin_41586393

最新推荐文章于 2024-09-15 16:56:37 发布

阅读量427

点赞数

分类专栏：视觉SLAM十四讲

本文链接：https://blog.csdn.net/weixin_41586393/article/details/90814920

版权

本文详细介绍了SLAM系统中的关键组件，包括相机类的实现，帧类的设计，路标点类的管理以及地图的构建。同时，还涵盖了配置文件的处理和常用库的使用。

摘要由CSDN通过智能技术生成

一、Camera:相机类
camera.h

#ifndef CAMERA_H
#define CAMERA_H
#include "myslam/common_include.h"
namespace myslam
{
class Camera
{
public:
    //定义指向camera的指针
    typedef std::shared_ptr<Camera> Ptr;
    //depth_scale_与单位有关，这里depth_scale_=1000
    float   fx_, fy_, cx_, cy_, 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 )
    {}
    // 像素坐标系、相机坐标系、世界坐标系之间的坐标转换
    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.cpp

#include "myslam/camera.h"
#include <myslam/config.h>
namespace myslam
{ 
Camera::Camera()
{
    //相机的几个参数从config文件中获取
    fx_ = Config::get<float>("camera.fx");
    fy_ = Config::get<float>("camera.fy");
    cx_ = Config::get<float>("camera.cx");
    cy_ = Config::get<float>("camera.cy");
    depth_scale_ = Config::get<float>("camera.depth_scale");
}
//下面的几个坐标转换也很好理解
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, c