SLAM中“camera类中坐标转换"方法解读

/*
p_p : position_pexil  ;  p_c：position_camera  ; p_w : position_world ; T_c_w : 欧式矩阵(包含R,t信息) from camera to world 

cpp功能：实现关于pexil,camera,world的任意两者间的坐标转换。

pixel2world的实现是能过两步实现的： pixel -> camera ,进而 camera -> world ，反之， world2pixel 也一样。
*/
#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; // T_c_w即欧式矩阵，包含R,t的信息，公式为 T * P_world
}

Vector3d Camera::camera2world ( const Vector3d& p_c, const SE3& T_c_w )
{
    return T_c_w.inverse() *p_c;
}

//为什么p_c用两个值来索引，它不是一维向量吗?
//the convenience Vector3f is a (column) vector of 3 floats. It is defined as follows by Eigen:   Matrix<float, 3, 1> Vector3f;
//We also offer convenience typedefs for row-vectors, for example: Matrix<int, 1, 2> RowVector2i;
//so Vector3d is a  column vector ,they have 3 rows;
Vector2d Camera::camera2pixel ( const Vector3d& p_c )
{
    return Vector2d (
	    //参照书本5.5的公式
        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 (
	    //参照书本5.5的公式，depth 即为公式的Z。
        ( 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 );
}


}