/*
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 );
}


}