![](http://static.blog.csdn.net/images/arrow_triangle_up.jpg)
版权声明:本文为博主原创文章,转载须注明出处。
上一节《关于OpenCV的那些事——跟踪点选取方式和特征点跟踪恢复》讲了两种跟踪和恢复的方法,本来项目就此结束了,想开一个新项目3D重建的,但是老板让发一篇journal,心想着这点东西肯定发不了,于是对项目进行了优化。11月半-1月半,两个月(一个月写,一个月改,老板对格式要求极其严格。。)发了香港的一个图像信号处理的期刊,3月的时候收到了accepted的通知。在这期间,我一边优化着项目,一边思考着我做的到底是什么,下一个项目究竟做什么(3D重建?SLAM?)。当然这个话题在我的下下一篇博客中会提到(视觉slam),这一篇主要讲第一个优化,使用random sample consensus收敛相机姿态。下一篇讲使用最小二乘多项式平滑消除姿态抖动。
我们知道在计算相机姿态的时候,opencv中提供了两种函数:solvePnP, solvePnPRansac。 第二个函数即时利用ransac的思想计算更加精确的姿态。 不过速度真是不敢恭维,完全没法实现实时系统。鉴于之前章节《关于OpenCV的那些事——相机姿态更新》里讲到的相机姿态更新至少使用4组2D/3D点对,我们的思想是对于追踪的n个特征点对,我们先随机生成m个4组点对(m < Cn4(排列组合)),分别计算出m个姿态,然后对于每一个姿态计算重投影误差,小于一定阀值的记录下来,并更新最佳姿态(最小误差),最终返回这个最佳姿态。m也叫迭代次数。当然选择合适的m,既能节省时间,有能找到最佳姿态。重投影误差的阀值也需要做实验找到最合适的。
C++代码如下:
- bool collinear_ornot(Point2f p1, Point2f p2, Point2f p3) // 三点是否共线
- {
- if (abs((p2.x - p1.x)*p3.y - (p2.y - p1.y)*p3.x - p1.y*p2.x + p1.x*p2.y) < 1e-5)
- return true;
- else
- return false;
- }
bool collinear_ornot(Point2f p1, Point2f p2, Point2f p3) // 三点是否共线
{
if (abs((p2.x - p1.x)*p3.y - (p2.y - p1.y)*p3.x - p1.y*p2.x + p1.x*p2.y) < 1e-5)
return true;
else
return false;
}
- void random_n_4p(vector<Point2f>& imgP)
- {
- srand((unsigned)time(NULL));
- int n = imgP.size(); //n为追踪的特征点的个数
- for (int i = 0; i < ransac_1; i++) // ransac_1为上文中的m迭代次数,本项目中取20
- {
- do
- {
- n_4[i][0] = rand() % n;
- do
- {
- n_4[i][1] = rand() % n;
- } while (n_4[i][1] == n_4[i][0]);
- do
- {
- n_4[i][2] = rand() % n;
- } while (n_4[i][2] == n_4[i][1] || n_4[i][2] == n_4[i][0]);
- do
- {
- n_4[i][3] = rand() % n;
- } while (n_4[i][3] == n_4[i][2] || n_4[i][3] == n_4[i][1] || n_4[i][3] == n_4[i][0]);
- }while (collinear_ornot(imgP[n_4[i][0]], imgP[n_4[i][1]], imgP[n_4[i][2]]) || collinear_ornot(imgP[n_4[i][0]], imgP[n_4[i][1]], imgP[n_4[i][3]]) || collinear_ornot(imgP[n_4[i][1]], imgP[n_4[i][2]], imgP[n_4[i][3]]));
- } //确保四点中每每三点不共线
- }
void random_n_4p(vector<Point2f>& imgP)
{
srand((unsigned)time(NULL));
int n = imgP.size(); //n为追踪的特征点的个数
for (int i = 0; i < ransac_1; i++) // ransac_1为上文中的m迭代次数,本项目中取20
{
do
{
n_4[i][0] = rand() % n;
do
{
n_4[i][1] = rand() % n;
} while (n_4[i][1] == n_4[i][0]);
do
{
n_4[i][2] = rand() % n;
} while (n_4[i][2] == n_4[i][1] || n_4[i][2] == n_4[i][0]);
do
{
n_4[i][3] = rand() % n;
} while (n_4[i][3] == n_4[i][2] || n_4[i][3] == n_4[i][1] || n_4[i][3] == n_4[i][0]);
}while (collinear_ornot(imgP[n_4[i][0]], imgP[n_4[i][1]], imgP[n_4[i][2]]) || collinear_ornot(imgP[n_4[i][0]], imgP[n_4[i][1]], imgP[n_4[i][3]]) || collinear_ornot(imgP[n_4[i][1]], imgP[n_4[i][2]], imgP[n_4[i][3]]));
} //确保四点中每每三点不共线
}
- void ransac_cc(vector<Point2f>& imgP, Mat& r, Mat& t)
- {
- random_n_4p(imgP);
- vector<Point2f> temp4imgP;
- vector<Point3f> temp4objP;
- vector<Point2f> temprepP;
- temprepP.resize(imgP.size());
- Mat tempobjPM;
- vector<double> temprv(3), temptv(3), bestr(3),bestt(3);
- Mat temp_r(temprv),temp_t(temptv),best_r(bestr),best_t(bestt);
- float minerror = 1,temperror = 0, errorthreshold = 1; //重投影误差阀值为1 pixel
- float testerror = 0;
- for (size_t iteration = 0; iteration < ransac_1; iteration++)
- {
- temp4imgP.push_back(imgP[n_4[iteration][0]]);
- temp4imgP.push_back(imgP[n_4[iteration][1]]);
- temp4imgP.push_back(imgP[n_4[iteration][2]]);
- temp4imgP.push_back(imgP[n_4[iteration][3]]);
- temp4objP.push_back(objP[n_4[iteration][0]]);
- temp4objP.push_back(objP[n_4[iteration][1]]);
- temp4objP.push_back(objP[n_4[iteration][2]]);
- temp4objP.push_back(objP[n_4[iteration][3]]);
- Mat(temp4objP).convertTo(tempobjPM, CV_32F);
- solvePnP(tempobjPM, Mat(temp4imgP), camera_matrix, distortion_coefficients, temp_r, temp_t);
- projectPoints(objPM, temp_r, temp_t, camera_matrix, distortion_coefficients, temprepP);
- testerror = norm(imgP, temprepP, NORM_L2);
- for (size_t i=0; i < imgP.size();i++)
- {
- temperror += sqrt((imgP[i].x - temprepP[i].x)*(imgP[i].x - temprepP[i].x) + (imgP[i].y - temprepP[i].y)*(imgP[i].y - temprepP[i].y));
- }
- if (temperror < errorthreshold) //重投影误差阀值为1 pixel
- {
- if (temperror < minerror)
- {
- minerror = temperror;
- best_r = temp_r;
- best_t = temp_t;
- }
- }
- temp4imgP.clear();
- temp4objP.clear();
- tempobjPM.setTo(0);
- temperror = 0;
- testerror = 0;
- }
- r = best_r;
- t = best_t;
- }
void ransac_cc(vector<Point2f>& imgP, Mat& r, Mat& t)
{
random_n_4p(imgP);
vector<Point2f> temp4imgP;
vector<Point3f> temp4objP;
vector<Point2f> temprepP;
temprepP.resize(imgP.size());
Mat tempobjPM;
vector<double> temprv(3), temptv(3), bestr(3),bestt(3);
Mat temp_r(temprv),temp_t(temptv),best_r(bestr),best_t(bestt);
float minerror = 1,temperror = 0, errorthreshold = 1; //重投影误差阀值为1 pixel
float testerror = 0;
for (size_t iteration = 0; iteration < ransac_1; iteration++)
{
temp4imgP.push_back(imgP[n_4[iteration][0]]);
temp4imgP.push_back(imgP[n_4[iteration][1]]);
temp4imgP.push_back(imgP[n_4[iteration][2]]);
temp4imgP.push_back(imgP[n_4[iteration][3]]);
temp4objP.push_back(objP[n_4[iteration][0]]);
temp4objP.push_back(objP[n_4[iteration][1]]);
temp4objP.push_back(objP[n_4[iteration][2]]);
temp4objP.push_back(objP[n_4[iteration][3]]);
Mat(temp4objP).convertTo(tempobjPM, CV_32F);
solvePnP(tempobjPM, Mat(temp4imgP), camera_matrix, distortion_coefficients, temp_r, temp_t);
projectPoints(objPM, temp_r, temp_t, camera_matrix, distortion_coefficients, temprepP);
testerror = norm(imgP, temprepP, NORM_L2);
for (size_t i=0; i < imgP.size();i++)
{
temperror += sqrt((imgP[i].x - temprepP[i].x)*(imgP[i].x - temprepP[i].x) + (imgP[i].y - temprepP[i].y)*(imgP[i].y - temprepP[i].y));
}
if (temperror < errorthreshold) //重投影误差阀值为1 pixel
{
if (temperror < minerror)
{
minerror = temperror;
best_r = temp_r;
best_t = temp_t;
}
}
temp4imgP.clear();
temp4objP.clear();
tempobjPM.setTo(0);
temperror = 0;
testerror = 0;
}
r = best_r;
t = best_t;
}
通过找到最小重投影误差,我们找到了最佳姿态,而且速度非常快。下一篇讲使用最小二乘多项式平滑消除姿态抖动。