【三维重建】相机模型以及坐标转换

理论内容：立体视觉入门指南（1）：坐标系与相机参数
代码出处

练习作业

1. 世界坐标系到相机坐标系的转换 W2C
2. 相机坐标系到世界坐标系的转换 C2W
3. 相机坐标系到图像坐标系的转换 C2I
4. 世界坐标系到图像坐标系的转换 W2I
5. 有深度作为参数，图像坐标系到相机坐标系的转换I2C
6. 有深度作为参数，图像坐标系到世界坐标系的转换I2W

照着代码抄了一下，其中比如RT_转置矩阵，相机坐标系转换到世界坐标系感觉原代码不对，自己按照自己的理解做了更正。

#ifndef SV3D_CAMERA_H
#define SV3D_CAMERA_H

#include "eigen_defs.h"

namespace sv3d
{
    class Camera
    {
    public:
        Mat3 K_;  // 内参矩阵
        Mat3 R_;  // 旋转矩阵
        Vec3 t_;  // 平移矩阵
        Mat34 P_; // 投影矩阵
        Mat3 KI_; // 内参矩阵的逆矩阵
        Mat3 RT_; // 旋转矩阵的转置矩阵

    public:
        inline Camera() = default;
        inline ~Camera() = default;

        // 通过K、R、t来构造相机，注意用了移动语义避免复制操作，传入的参数在执行构造函数后内存销毁
        inline Camera(Mat3 K, Mat3 R, Vec3 t) : K_(std::move(K)), R_(std::move(R)), t_(std::move(t))
        {
            KI_ = K_.inverse();
            RT_ = R_.transpose();
            P_ << K_ * R_, K_ * t_;
        }

        // 相机坐标系转换到世界坐标系
        inline Vec3 TransformPointC2W(const Vec3 &X) const
        {
            return R_.inverse() * (X - t_);
        }

        // 世界坐标系转换到相机坐标系
        inline Vec3 TransformPointW2C(const Vec3 &X) const
        {
            return R_ * X + t_;
        }

        // 相机坐标系转到影像坐标系
        inline Vec2 TransformPointC2I(const Vec3 &X) const
        {
            auto I = Vec3(K_ * X);
            return Vec2(I[0] / I[2], I[1] / I[2]); // 归一化
        }

        // 世界坐标系到影像坐标系
        inline Vec2 TransformPointW2I(const Vec3 &X) const
        {
            return TransformPointC2I(TransformPointW2C(X));
        }

        // 影像坐标系转换到相机坐标系(深度已知 X[2])
        inline Vec3 TransformPointI2C(const Vec3 &X) const
        {
            auto Xt = X;
            Xt[0] *= Xt[2];
            Xt[1] *= Xt[2];
            return KI_ * Xt;
        }

        // 影像坐标系转换到世界坐标系(深度已知：X[2]) 
        inline Vec3 TransformPointI2W(const Vec3 &X) const
        {
            return TransformPointC2W(TransformPointI2C(X));
        }
    }
}

#endif

eigen_defs.h

/* -*-c++-*- StereoV3D - Copyright (C) 2021.
* Author	: Ethan Li<[email protected]>
* https://github.com/ethan-li-coding/StereoV3DCode
*/

#ifndef SV3D_EIGEN_DEFS_H
#define SV3D_EIGEN_DEFS_H

#include <Eigen/Core>
#include <Eigen/Dense>
using namespace Eigen;

namespace sv3d
{
	// 3d 矢量 (double类型)
	using Vec2 = Eigen::Vector2d;
		
	/// 2d 矢量 (float类型)
	using Vec2f = Eigen::Vector2f;
	
	// 3d 矢量 (double类型)
	using Vec3 = Eigen::Vector3d;

	// 3d 矢量 (float类型)
	using Vec3f = Eigen::Vector3f;

	/// 4d 矢量 
	using Vec4 = Eigen::Vector4d;
	
	// 9d 矢量
	using Vec9 = Eigen::Matrix<double, 9, 1>;
	
	// 3x3 矩阵 (double类型)
	using Mat3 = Eigen::Matrix<double, 3, 3>;
		
	// 3x4 矩阵 (double类型)
	using Mat34 = Eigen::Matrix<double, 3, 4>;
	
	// 3x3 矩阵 (double类型)（行主序）
	using RMat3 = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>;

	// 3x4 矩阵 (double类型)
	using RMat34 = Eigen::Matrix<double, 3, 4, Eigen::RowMajor>;

	// 4x4 矩阵 (double类型)
	using Mat4 = Eigen::Matrix<double, 4, 4>;
	
	// 2xN 矩阵 (double类型)
	using Mat2X = Eigen::Matrix<double, 2, Eigen::Dynamic>;
		
	// 3xN 矩阵 (double类型)
	using Mat3X = Eigen::Matrix<double, 3, Eigen::Dynamic>;

	// Nx9 矩阵 (double类型)
	using MatX9 = Eigen::Matrix<double, Eigen::Dynamic, 9>;

	// Nx9 矩阵 (double类型)（行主序）
	using RMatX9 = Eigen::Matrix<double, Eigen::Dynamic, 9, Eigen::RowMajor>;

	// NxM 矩阵 (double类型)
	using MatXX = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;

	// NxM 矩阵 (double类型)（行主序）
	using RMatXX = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
}

#endif