这里引用了高翔博士打《SLAM 十四讲》,首先谢谢高博啦!
理论知识
说明:本文参考的应该是《SLAM 十四讲》第一版,在这里有个错误,后版中改了过来,这里提出,免得误导博友。公式5.11 - 5.14 中打Xcorrected和Ycorrected表示的是未矫正的坐标,也就是存在畸变打点的坐标。这里描述的是相机畸变的过程,将该过程取反即是去畸变过程)
畸变参数获取
- 在标定相机时,即可获得径向畸变参数k,和切向畸变参数p。
- 在式中出现的(r,theta)是由像素坐标转到极坐标得到。
- fx和fy,cx和cy由相机内参得到。
- x,y:理想图像中不存在畸变的点的坐标
- Xcorrected和Ycorrected:畸变之后点的坐标
目的:求取x,y
矫正效果
说明:图片中打红色的圆是矫正之后打点,其相邻的角点是存在畸变的点。
(不要盯着我萌萌哒打师兄看哦!!!)
实现
for(int i=0; i<size; i++)
{
Mx = P_Norm[i].x;
My = P_Norm[i].y;
rSquare = pow(Mx, 2) + pow(My, 2);
A = (1 + strCamPara.k1 * rSquare + strCamPara.k2 * pow(rSquare, 2)
+ strCamPara.k3 * pow(rSquare, 3));
B = 2*strCamPara.p1;
C = 2*strCamPara.p2;
D = strCamPara.p2*rSquare;
E = strCamPara.p1*rSquare;
a = (My - E)/(Mx - D);
b = a*B + C;
d = D - Mx;
//solve the formula "b*x^2 + A*x + d = 0", then we will get 'x'
e = pow(A, 2) - 4*b*d;
//b=0 ?
if(b==0)
{
//if b=0, then get the formula "A*x + d = 0", solve it and get 'x'
x1 = d/A;
y1 = a*x1;
//cout<<i<<" e=0, and the point is "<<Point2f(x1,y1)<<endl;
p_temp.push_back(Point2f(x1,y1));
}
else
{
if(e >= 0)
{
x1 = (-A + sqrt(e))*0.5/b;
x2 = (-A - sqrt(e))*0.5/b;
y1 = a*x1;
y2 = a*x2;
//give up these points Point2f(x1,y2),Point2f(x2,y1),Point2f(x2,y2)
//cout<<i<<" point is "<<Point2f(x1,y1)<<endl;
//cout<<i<<" point is "<<Point2f(x1,y2)<<endl;
//cout<<i<<" point is "<<Point2f(x2,y2)<<endl;
//cout<<i<<" point is "<<Point2f(x2,y2)<<endl;
p_temp.push_back(Point2f(x1,y1));
}
else
{
cerr<<"e < 0, is "<<e<<endl;
continue;
}
}
}
完整的实现过程点击下载,内附存在畸变打照片及相机参数。
================分割线=====================
更正一下,SLAM十四讲的内容应该是没有问题的,而且很多资料上都和其是一致的
让我陷入怀疑书本有误的是上述代码(代码是按照公式推导出来的,没有问题)运行出来的结果,如上图,给人的感觉就是矫正,相反如果照着书本上编写相应的代码(这个比较容易,可以自己试试一下)并且运行之后的结果给人的感觉畸变过程,而且畸变更严重了,很奇怪的感觉