相機標定(二)——圖像座標與世界座標轉換

一、座標關係

相機中有四個座標系，分別爲world，camera，image，pixel

world爲世界座標系，可以任意指定 $x_w$ 軸和 $y_w$ 軸，爲上圖P點所在座標系。
camera爲相機座標系，原點位於小孔，z軸與光軸重合， $x_w$ 軸和 $y_w$ 軸平行投影面，爲上圖座標系 $X_cY_cZ_c$ 。
image爲圖像座標系，原點位於光軸和投影面的交點， $x_w$ 軸和 $y_w$ 軸平行投影面，爲上圖座標系 $XYZ$ 。
pixel爲像素座標系，從小孔向投影面方向看，投影面的左上角爲原點， $uv$ 軸和投影面兩邊重合，該座標系與圖像座標系處在同一平面，但原點不同。

二、座標變換

下式爲像素座標pixel與世界座標world的變換公式，右側第一個矩陣爲相機內參數矩陣，第二個矩陣爲相機外參數矩陣。
$s\begin{bmatrix} u\\v\\1 \end{bmatrix} =\begin{bmatrix} f_x&0&c_x\\0&f_y&c_y\\0&0&1 \end{bmatrix} \begin{bmatrix} r_{11} & r_{12}&r_{13}&t_1 \\ r_{21} & r_{22}&r_{23}&t_2 \\ r_{31} & r_{32}&r_{33}&t_3 \end{bmatrix} \begin{bmatrix} x\\y\\z\\1 \end{bmatrix}$

2.1 變換流程

$P_{uv} = KTP_w$

該方程右側隱含了一次齊次座標到非齊次座標的轉換

$K$ 內參 $T_{camera}^{pixel}$ ：像素座標系相對於相機座標系的變換（與相機和鏡頭有關）
$T$ 外參 $T_{world}^{camera}$ ：相機座標系相對於世界座標系的變換

順序變換

從pixel到camera，使用內參變換

$P_{camera}(3 \times 1) = {T_{camera}^{pixel}}^{-1}(3 \times 3)*P_{pixel}(3 \times 1)*depth$

從camera到world，使用外參變換

$P_{world} (4 \times 1)= {T_{world}^{camera}}^{-1}(4 \times 4)* P_{camera}(4 \times 1)$

注意：兩個變換之間的矩陣大小不同，需要分開計算，從pixel到camera獲得的相機座標爲非齊次，需轉換爲齊次座標再進行下一步變換。而在進行從camera到world時，需將外參矩陣轉換爲齊次再進行計算。

直接變換
$\begin{bmatrix} X\\Y\\Z \end{bmatrix}= R^{-1}(M^{-1}*s*\begin{bmatrix} u\\v\\1 \end{bmatrix} - t)$
注意：直接變換是直接根據變換公式獲得，實際上包含pixel到camera和camera到world，實際上和順序變換一樣，通過順序變換可以更清晰瞭解變換過程。

2.2 參數計算

內參

通過張正友標定獲得

外參

通過PNP估計獲得

深度s

深度s爲目標點在相機座標系Z方向的值

2.3 外參計算

solvePnP函數

Perspective-n-Point是通過n組給定點的世界座標與像素座標估計相機位置的方法。OpenCV內部提供的函數爲solvePnP()，函數介紹如下：

bool solvePnP(InputArray objectPoints, 
	      	  InputArray imagePoints, 
	      	  InputArray cameraMatrix, 
	      	  InputArray distCoeffs, 
              OutputArray rvec, 
              OutputArray tvec, 
              bool useExtrinsicGuess=false, 
              int flags=ITERATIVE )

objectPoints，輸入世界座標系中點的座標；
imagePoints，輸入對應圖像座標系中點的座標；
cameraMatrix, 相機內參數矩陣；
distCoeffs, 畸變係數；
rvec, 旋轉向量，需輸入一個非空Mat，需要通過cv::Rodrigues轉換爲旋轉矩陣；
tvec, 平移向量，需輸入一個非空Mat；
useExtrinsicGuess, 默認爲false，如果設置爲true則輸出輸入的旋轉矩陣和平移矩陣；
flags，選擇採用的算法；
- CV_ITERATIVE Iterative method is based on Levenberg-Marquardt optimization. In this case the function finds such a pose that minimizes reprojection error, that is the sum of squared distances between the observed projections imagePoints and the projected (using projectPoints() ) objectPoints .
- CV_P3P Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang “Complete Solution Classification for the Perspective-Three-Point Problem”. In this case the function requires exactly four object and image points.
- CV_EPNP Method has been introduced by F.Moreno-Noguer, V.Lepetit and P.Fua in the paper “EPnP: Efficient Perspective-n-Point Camera Pose Estimation”.

注意：solvePnP()的參數rvec和tvec應該都是double類型的

程序實現

//輸入參數
Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 攝像機內參數矩陣 */
Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0)); /* 攝像機的5個畸變係數：k1,k2,p1,p2,k3 */
double zConst = 0;//實際座標系的距離,若工作平面與相機距離固定可設置爲0
	
//計算參數
double s;
Mat rotationMatrix = Mat (3, 3, DataType<double>::type);
Mat tvec = Mat (3, 1, DataType<double>::type);
void calcParameters(vector<cv::Point2f> imagePoints, vector<cv::Point3f> objectPoints)
{
	//計算旋轉和平移
	Mat rvec(3, 1, cv::DataType<double>::type);
	cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
	cv::Rodrigues(rvec, rotationMatrix);
}

2.4 深度計算

理想情況下，相機與目標平面平行（只有繞Z軸的旋轉），但實際上相機與目標平面不會完全平行，存在繞X和Y軸的旋轉，此時深度s並不是固定值 $t_3$ ，計算深度值爲：
$s = t_3 + r_{31} * x + r_{32} * y + r_{33} * z$
若使用固定值進行變換會導致較大誤差。解決方案如下：

計算多個點的深度值，擬合一個最優值
通過外參計算不同位置的深度（此處採用該方案）

注意：此處環境爲固定單目與固定工作平面，不同情況下獲得深度方法不同。

像素座標pixel與世界座標world轉換公式可簡化爲
$s\begin{bmatrix} u\\v\\1 \end{bmatrix} =M(R\begin{bmatrix} X\\Y\\Z_{const} \end{bmatrix}+t)$
$M$ 爲相機內參數矩陣， $R$ 爲旋轉矩陣， $t$ 爲平移矩陣， $z_{const}$ 爲目標點在世界座標Z方向的值，此處爲0。

變換可得
$R^{-1}M^{-1}s\begin{bmatrix} u\\v\\1 \end{bmatrix} =\begin{bmatrix} X\\Y\\Z_{const} \end{bmatrix}+R^{-1}t$
當相機內外參已知可計算獲得 $s$

三、程序實現

3.1 Matlab

clc;
clear;

% 內參
syms fx cx fy cy;
M = [fx,0,cx;
    0,fy,cy;
    0,0,1];
            
% 外參
%旋轉矩陣
syms r11 r12 r13 r21 r22 r23 r31 r32 r33;
R = [r11,r12,r13;
  	r21,r22,r23;
  	r31,r32,r33];

%平移矩陣
syms t1 t2 t3;
t = [t1;
    t2;
    t3];
%外參矩陣 
T = [R,t;
    0,0,0,1];
 
% 圖像座標 
syms u v;
imagePoint = [u;v;1];   
 
% 計算深度
syms zConst;
rightMatrix = inv(R)*inv(M)*imagePoint;
leftMatrix = inv(R)*t;
s = (zConst + leftTemp(3))/rightTemp(3);

% 轉換世界座標方式一
worldPoint1 = inv(R) * (s*inv(M) * imagePoint - t)

% 轉換世界座標方式二
cameraPoint = inv(M)* imagePoint * s;% image->camrea
worldPoint2 = inv(T)* [cameraPoint;1];% camrea->world
worldPoint2 = [worldPoint2(1);worldPoint2(2);worldPoint2(3)]

3.2 C++

該程序參考《視覺SLAM十四講》第九講實踐章：設計前端代碼部分進行修改獲得，去掉了李羣庫Sopuhus依賴，因該庫在windows上調用較爲麻煩，若在Linux建議採用Sopuhus。

camera.h

#ifndef CAMERA_H
#define CAMERA_H

#include <Eigen/Core>
#include <Eigen/Geometry>
using Eigen::Vector4d;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Quaterniond;
using Eigen::Matrix;

class Camera
{
public:
    Camera();

    // coordinate transform: world, camera, pixel
    Vector3d world2camera( const Vector3d& p_w);
    Vector3d camera2world( const Vector3d& p_c);
    Vector2d camera2pixel( const Vector3d& p_c);
    Vector3d pixel2camera( const Vector2d& p_p); 
    Vector3d pixel2world ( const Vector2d& p_p);
    Vector2d world2pixel ( const Vector3d& p_w);

	// set params
	void setInternalParams(double fx, double cx, double fy, double cy);
	void setExternalParams(Quaterniond Q, Vector3d t);
	void setExternalParams(Matrix<double, 3, 3>  R, Vector3d t);

	// cal depth
	double calDepth(const Vector2d& p_p);

private:
    // 內參
	double fx_, fy_, cx_, cy_, depth_scale_;
	Matrix<double, 3, 3> inMatrix_;

    // 外參
    Quaterniond Q_;
	Matrix<double, 3, 3>  R_;
    Vector3d t_; 
	Matrix<double, 4, 4> exMatrix_;
};

#endif // CAMERA_H

camera.cpp

#include "camera.h"

Camera::Camera(){}

Vector3d Camera::world2camera ( const Vector3d& p_w)
{
	Vector4d p_w_q{ p_w(0,0),p_w(1,0),p_w(2,0),1};
	Vector4d p_c_q = exMatrix_ * p_w_q;
	return Vector3d{p_c_q(0,0),p_c_q(1,0),p_c_q(2,0)};
}

Vector3d Camera::camera2world ( const Vector3d& p_c)
{
	Vector4d p_c_q{ p_c(0,0),p_c(1,0),p_c(2,0),1 };
	Vector4d p_w_q = exMatrix_.inverse() * p_c_q;
    return Vector3d{ p_w_q(0,0),p_w_q(1,0),p_w_q(2,0) };
}

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 = calDepth(p_p);
    return Vector3d (
               ( p_p ( 0,0 )-cx_ ) *depth/fx_,
               ( p_p ( 1,0 )-cy_ ) *depth/fy_,
               depth
           );
}

Vector2d Camera::world2pixel ( const Vector3d& p_w)
{
    return camera2pixel ( world2camera(p_w) );
}

Vector3d Camera::pixel2world ( const Vector2d& p_p)
{
    return camera2world ( pixel2camera ( p_p ));
}

double Camera::calDepth(const Vector2d& p_p)
{
	Vector3d p_p_q{ p_p(0,0),p_p(1,0),1 };
	Vector3d rightMatrix = R_.inverse() * inMatrix_.inverse() * p_p_q;
	Vector3d leftMatrix = R_.inverse() * t_;
	return leftMatrix(2,0)/rightMatrix(2,0);
}

void Camera::setInternalParams(double fx, double cx, double fy, double cy)
{
	fx_ = fx;
	cx_ = cx;
	fy_ = fy;
	cy_ = cy;

	inMatrix_ << fx, 0, cx,
				0, fy, cy,
				0, 0, 1;
}

void Camera::setExternalParams(Quaterniond Q, Vector3d t)
{
	Q_ = Q;
	R_ = Q.normalized().toRotationMatrix();
	setExternalParams(R_,t);
}

void Camera::setExternalParams(Matrix<double, 3, 3>  R, Vector3d t)
{
	t_ = t;
	R_ = R;

	exMatrix_ << R_(0, 0), R_(0, 1), R_(0, 2), t(0,0),
		R_(1, 0), R_(1, 1), R_(1, 2), t(1,0),
		R_(2, 0), R_(2, 1), R_(2, 2), t(2,0),
		0, 0, 0, 1;
}

參考

image coordinate to world coordinate opencv

Computing x,y coordinate (3D) from image point

單應矩陣

camera_calibration_and_3d

《視覺SLAM十四講》—相機與圖像+實踐章：設計前端

相機標定(二)——圖像座標與世界座標轉換

一、座標關係

二、座標變換

2.1 變換流程

2.2 參數計算

2.3 外參計算

2.4 深度計算

三、程序實現

3.1 Matlab

3.2 C++

參考

痞子衡嵌入式：恩智浦i.MX RT1xxx系列MCU啓動那些事（12.A）- uSDHC eMMC啓動時間(RT1170)

Ubuntu安裝ROS2

SLAM學習——卡爾曼濾波（KF/EKF）

運動規劃——多姿態插值

機械臂動力學——二軸機械臂動力學參數識別

SLAM學習——使用ARUCO_marker進行AR投影

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結