张正友相机标定原理
一、相机成像原理
1、相机成像系统中,共包含四个坐标系:世界坐标系、相机坐标系、图像坐标系、像素坐标系。
2、这四个坐标系之间的转化关系为:
- 其中,
$(U,V,W)$ 为在世界坐标系下一点的物理坐标,$(u,v)$ 为该点对应的在像素坐标系下的像素坐标, Z 为尺度因子。
3、内参矩阵和外参矩阵
- 我们将这样的矩阵称为内参矩阵,内参矩阵取决于相机的内部参数。其中,$f$为焦距,$dX$,$dY$分别表示$X$,$Y$方向上的一个像素在相机感光板上的物理长度(即一个像素在感光板上是多少毫米,$u_0$,$v_0$分别表示相机感光板中心在像素坐标系下的坐标,$\theta$表示感光板的横边和纵边之间的角度。
- 我们将这样的矩阵称为相机的外参矩阵,外参矩阵取决于相机坐标系和世界坐标系的相对位置,$R$ 表示旋转矩阵,$T$ 表示平移矢量。
4、畸变与畸变矫正
-
其中,$(x,y)$,$(\hat{x},\hat{y})$分别为理想的无畸变的归一化的图像坐标、畸变后的归一化图像坐标,
$r^2=x^2+y^2$ 为图像像素点到图像中心点的距离。
二、张正友相机标定原理
1、相机标定的目的是什么?
- 为什么要进行相机标定呢?比如,当我们拿到一张图片,进行识别之后,得到的两部分之间的距离为多少多少像素,但是这多少多少像素究竟对应实际世界中的多少米呢?这就需要利用相机标定的结果来将像素坐标转换到物理坐标来计算距离(当然这里值得说明,仅仅利用单目相机标定的结果,是无法直接从像素坐标转化到物理坐标的,因为透视投影丢失了一个维度的坐标,所以测距其实需要双目相机)。
-
相机标定的目的其实很简单,我们要想对一个成像系统建模,进而进行相应的计算,所必须的参数就是相机的内参矩阵和相机的外参矩阵,因此,相机标定的第一个目的就是获得相机的内参矩阵和外参矩阵。
- 相机标定的第二个目的就是获得相机的畸变参数,进而对拍摄的图片进行去畸变处理。
2、张正友标定法简介
-
张正友标定法用如下图所示的棋盘格标定板,在得到一张标定板的图像之后,可以利用相应的图像检测算法得到每一个角点的像素坐标
$(u,v)$ 。 -
张正友标定法将世界坐标系固定于棋盘格上,则棋盘格上任一点的物理坐标
$W=0$ ,由于标定板的世界坐标系是人为事先定义好的,标定板上每一个格子的大小是已知的,我们可以计算得到每一个角点在世界坐标系下的物理坐标$(U,V,W = 0)$ 。 -
我们将利用这些信息:每一个角点的像素坐标
$(u,v)$ 、每一个角点在世界坐标系下的物理坐标$(U,V,W = 0)$ ,来进行相机的标定,获得相机的内外参矩阵、畸变参数。
3、标定相机的内参矩阵和外参矩阵
张正友标定法标定相机的内外参数的思路如下:
- (1)、求解内参矩阵与外参矩阵的积;
- (2)、求解内参矩阵;
- (3)、求解外参矩阵。
- (4)、标定相机的畸变参数
- (5)、L-M 算法参数优化
(1)、求解内参矩阵与外参矩阵的积
将世界坐标系固定于棋盘格上,则棋盘格上任一点的物理坐标
我们对于上式做一定的说明。对于不同的图片,内参矩阵
我们将
利用上式,消去尺度因子
此时,尺度因子
由这里的
(2)、求解内参矩阵
我们已知了矩阵
则由
代入可得:
另外,我们发现,上述两个约束方程中均存在矩阵
则:
则用矩阵
注意:由于
因此,为了求解矩阵
上述方程看起来有点复杂,但是其实不然,我们可以记:
则上述方程化为:
其中,矩阵 $v= \begin{bmatrix}
v^t_{12} \
v^T_{11}-v^T_{22}\
\end{bmatrix}$
由于矩阵
此时,我们只要求解出向量
根据矩阵
(3)、求解外参矩阵
这里再次强调一下,对于同一个相机,相机的内参矩阵取决于相机的内部参数,无论标定板和相机的位置关系是怎么样的,相机的内参矩阵不变。这也正是在第 2 部分“求解内参矩阵”中,我们可以利用不同的图片(标定板和相机位置关系不同)获取的矩阵
但是,外参矩阵反映的是标定板和相机的位置关系。对于不同的图片,标定板和相机的位置关系已经改变,此时每一张图片对应的外参矩阵都是不同的。
在关系:
注意,这里值得指出,完整的外参矩阵为 $\begin{bmatrix}R&T \ 0&1\ \end{bmatrix}$ 。但是,由于张正友标定板将世界坐标系的原点选取在棋盘格上,则棋盘格上任一点的物理坐标
此时,相机的内参矩阵和外参矩阵均已得到。
注:以上推导都是假设不存在畸变参数的情况下成立的。但是事实上,相机是存在畸变参数的,因此,张正友标定法还需要通过 L-M 算法对于参数进行迭代优化。
(4)、标定相机的畸变参数
张正友标定法仅仅考虑了畸变模型中影响较大的径向畸变。 径向畸变公式(2 阶)如下:
其中,$(x,y),(\hat{x},\hat{y})$ 分别为理想的无畸变的归一化的图像坐标、畸变后的归一化图像坐标,$r$ 为图像像素点到图像中心点的距离,即
图像坐标和像素坐标的转化关系为:
其中,$(u,v)$ 为理想的无畸变的像素坐标。由于
同理可得畸变后的像素坐标
代入径向畸变公式(2 阶)则有:
可化简得:
每一个角点,只要知道畸变后的像素坐标
此时,相机的畸变矫正参数已经标定好。
那么,如何获得畸变后的像素坐标
利用上式,消去尺度因子
即可得到理想的、无畸变的像素坐标
需要指出,上述公式推导的时候以 2 阶径向畸变为例,但实际上更高阶的径向畸变同理,只是需要的约束方程个数更多而已。
(5)、L-M 算法参数优化
从上述推导过程就可以看出,张正友标定法是有很多近似的,所以仅仅利用上述的过程进行标定误差肯定是很大的。所以张正友标定法还利用 L-M(Levenberg-Marquardt)算法对参数进行了优化。
5.1、最小二乘法
阻尼参数
- 对与
$\mu>0$ ,$(H+\mu I)$ 正定,保证了$h$ 是梯度下降的方向。 - 当
$\mu$ 较大时:,其实就是梯度、最速下降法,当离最终结果较远的时候,很好。 - 当
$\mu$ 较小时,方法接近与高斯牛顿,当离最终结果很近时,可以获得二次收敛速度,很好。 看来,$\mu$ 的选取很重要。初始时,取
迭代终止条件
1.一阶导数为0: ,使用,
$\varepsilon_1$ 是设定的终止条件; 2.$x$变化步距离足够小,; 3.超过最大迭代次数。
5.2、LM算法的步骤
5.3、算法实现
问题:(高斯牛顿同款问题)非线性方程: ,给定
#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <eigen3/Eigen/Cholesky>
#include <chrono>
/* 计时类 */
class Runtimer{
public:
inline void start()
{
t_s_ = std::chrono::steady_clock::now();
}
inline void stop()
{
t_e_ = std::chrono::steady_clock::now();
}
inline double duration()
{
return std::chrono::duration_cast<std::chrono::duration<double>>(t_e_ - t_s_).count() * 1000.0;
}
private:
std::chrono::steady_clock::time_point t_s_; //start time ponit
std::chrono::steady_clock::time_point t_e_; //stop time point
};
/* 优化方程 */
class LevenbergMarquardt{
public:
LevenbergMarquardt(double* a, double* b, double* c):
a_(a), b_(b), c_(c)
{
epsilon_1_ = 1e-6;
epsilon_2_ = 1e-6;
max_iter_ = 50;
is_out_ = true;
}
void setParameters(double epsilon_1, double epsilon_2, int max_iter, bool is_out)
{
epsilon_1_ = epsilon_1;
epsilon_2_ = epsilon_2;
max_iter_ = max_iter;
is_out_ = is_out;
}
void addObservation(const double& x, const double& y)
{
obs_.push_back(Eigen::Vector2d(x, y));
}
void calcJ_fx()
{
J_ .resize(obs_.size(), 3);
fx_.resize(obs_.size(), 1);
for ( size_t i = 0; i < obs_.size(); i ++)
{
const Eigen::Vector2d& ob = obs_.at(i);
const double& x = ob(0);
const double& y = ob(1);
double j1 = -x*x*exp(*a_ * x*x + *b_*x + *c_);
double j2 = -x*exp(*a_ * x*x + *b_*x + *c_);
double j3 = -exp(*a_ * x*x + *b_*x + *c_);
J_(i, 0 ) = j1;
J_(i, 1) = j2;
J_(i, 2) = j3;
fx_(i, 0) = y - exp( *a_ *x*x + *b_*x +*c_);
}
}
void calcH_g()
{
H_ = J_.transpose() * J_;
g_ = -J_.transpose() * fx_;
}
double getCost()
{
Eigen::MatrixXd cost= fx_.transpose() * fx_;
return cost(0,0);
}
double F(double a, double b, double c)
{
Eigen::MatrixXd fx;
fx.resize(obs_.size(), 1);
for ( size_t i = 0; i < obs_.size(); i ++)
{
const Eigen::Vector2d& ob = obs_.at(i);
const double& x = ob(0);
const double& y = ob(1);
fx(i, 0) = y - exp( a *x*x + b*x +c);
}
Eigen::MatrixXd F = 0.5 * fx.transpose() * fx;
return F(0,0);
}
double L0_L( Eigen::Vector3d& h)
{
Eigen::MatrixXd L = -h.transpose() * J_.transpose() * fx_ - 0.5 * h.transpose() * J_.transpose() * J_ * h;
return L(0,0);
}
void solve()
{
int k = 0;
double nu = 2.0;
calcJ_fx();
calcH_g();
bool found = ( g_.lpNorm<Eigen::Infinity>() < epsilon_1_ );
std::vector<double> A;
A.push_back( H_(0, 0) );
A.push_back( H_(1, 1) );
A.push_back( H_(2,2) );
auto max_p = std::max_element(A.begin(), A.end());
double mu = *max_p;
double sumt =0;
while ( !found && k < max_iter_)
{
Runtimer t;
t.start();
k = k +1;
Eigen::Matrix3d G = H_ + mu * Eigen::Matrix3d::Identity();
Eigen::Vector3d h = G.ldlt().solve(g_);
if( h.norm() <= epsilon_2_ * ( sqrt(*a_**a_ + *b_**b_ + *c_**c_ ) +epsilon_2_ ) )
found = true;
else
{
double na = *a_ + h(0);
double nb = *b_ + h(1);
double nc = *c_ + h(2);
double rho =( F(*a_, *b_, *c_) - F(na, nb, nc) ) / L0_L(h);
if( rho > 0)
{
*a_ = na;
*b_ = nb;
*c_ = nc;
calcJ_fx();
calcH_g();
found = ( g_.lpNorm<Eigen::Infinity>() < epsilon_1_ );
mu = mu * std::max<double>(0.33, 1 - std::pow(2*rho -1, 3));
nu = 2.0;
}
else
{
mu = mu * nu;
nu = 2*nu;
}// if rho > 0
}// if step is too small
t.stop();
if( is_out_ )
{
std::cout << "Iter: " << std::left <<std::setw(3) << k << " Result: "<< std::left <<std::setw(10) << *a_ << " " << std::left <<std::setw(10) << *b_ << " " << std::left <<std::setw(10) << *c_ <<
" step: " << std::left <<std::setw(14) << h.norm() << " cost: "<< std::left <<std::setw(14) << getCost() << " time: " << std::left <<std::setw(14) << t.duration() <<
" total_time: "<< std::left <<std::setw(14) << (sumt += t.duration()) << std::endl;
}
} // while
if( found == true)
std::cout << "\nConverged\n\n";
else
std::cout << "\nDiverged\n\n";
}//function
Eigen::MatrixXd fx_;
Eigen::MatrixXd J_; // 雅克比矩阵
Eigen::Matrix3d H_; // H矩阵
Eigen::Vector3d g_;
std::vector< Eigen::Vector2d> obs_; // 观测
/* 要求的三个参数 */
double* a_, *b_, *c_;
/* parameters */
double epsilon_1_, epsilon_2_;
int max_iter_;
bool is_out_;
};//class LevenbergMarquardt
int main(int argc, char **argv) {
const double aa = 0.1, bb = 0.5, cc = 2; // 实际方程的参数
double a =0.0, b=0.0, c=0.0; // 初值
/* 构造问题 */
LevenbergMarquardt lm(&a, &b, &c);
lm.setParameters(1e-10, 1e-10, 100, true);
/* 制造数据 */
const size_t N = 100; //数据个数
cv::RNG rng(cv::getTickCount());
for( size_t i = 0; i < N; i ++)
{
/* 生产带有高斯噪声的数据 */
double x = rng.uniform(0.0, 1.0) ;
double y = exp(aa*x*x + bb*x + cc) + rng.gaussian(0.05);
/* 添加到观测中 */
lm.addObservation(x, y);
}
/* 用LM法求解 */
lm.solve();
return 0;
}
4、相机标定的步骤
(1)、准备一个张正友标定法的棋盘格,棋盘格大小已知,用相机对其进行不同角度的拍摄,得到一组图像;
(2)、对图像中的特征点如标定板角点进行检测,得到标定板角点的像素坐标值,根据已知的棋盘格大小和世界坐标系原点,计算得到标定板角点的物理坐标值;
(3)、求解内参矩阵与外参矩阵。
根据物理坐标值和像素坐标值的关系,求出
(4)、求解畸变参数。
利用
(5)、利用 L-M(Levenberg-Marquardt)算法对上述参数进行优化。
这是相机标定的代码Camera_calibration