基于OpenCV实现相近两幅图像的特征匹配

#include <stdio.h> #include <stdlib.h> #include "cv.h" #include "cvaux.h" #include "highgui.h" #include "image.h" #include "matrix.h" #include "Util.h" #include <vector> #include "opengl.h" const int MAX_CORNERS = 200; // 特征点数目上限 /* ==========不同的测试数据============ */ // char *img1 = "image/house_4.jpg"; // char *img2 = "image/house_5.jpg"; char *img1 = "image/arti.seq1.png"; char *img2 = "image/arti.seq100.png"; // char *img1 = "image/hotel.seq0.png"; // char *img2 = "image/hotel.seq40.png"; // char *img1 = "image/0.l.png"; // char *img2 = "image/25.l.png"; // char *img1 = "image/bt.005.pgm"; // char *img2 = "image/bt.000.pgm"; /* ===================================== */ using namespace std; using namespace cvutImage; using namespace cvutMatrix; using namespace cvutUtil; int main(int *argc,char** argv) { // 读入两幅图 Image<uchar> left(img1); Image<uchar> right(img2); CvSize img_sz = left.size(); // 匹配窗口大小 int win_size = 10; // 提取特征时用到的临时图像 Image<uchar> eig_image(img_sz, IPL_DEPTH_32F, 1); Image<uchar> tmp_image(img_sz, IPL_DEPTH_32F, 1); int corner_count = MAX_CORNERS; // 保存角点数据 CvPoint2D32f* cornersA = new CvPoint2D32f[ corner_count ]; CvPoint2D32f* cornersB = new CvPoint2D32f[ corner_count ]; // 焦点检测用到的灰度图像 Image<uchar> gray_left(img_sz, 8, 1); Image<uchar> gray_right(img_sz, 8, 1); rgb2gray(left, gray_left); rgb2gray(right, gray_right); // Harris角点检测 cvGoodFeaturesToTrack( gray_left.cvimage, eig_image.cvimage, tmp_image.cvimage, cornersA, &corner_count, 0.05, 5.0, 0, 3, 0, 0.04 ); printf("角点提取完毕\n"); printf("检测到角点数目: %d\n", corner_count); // 亚像素矫正 cvFindCornerSubPix( gray_left.cvimage, cornersA, corner_count, cvSize( win_size, win_size ),cvSize( -1, -1 ), cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03) ); // LK光流跟踪 char *features_found = new char[ corner_count ]; float *feature_errors = new float[ corner_count ]; CvSize pyr_sz = cvSize( left.width + 8, right.height / 3); // 临时图像用于跟踪 Image<uchar> pyrA(pyr_sz, IPL_DEPTH_32F, 1); Image<uchar> pyrB(pyr_sz, IPL_DEPTH_32F, 1); // 光流跟踪特征点 cvCalcOpticalFlowPyrLK( gray_left.cvimage, gray_right.cvimage, pyrA.cvimage, pyrB.cvimage, cornersA, cornersB, corner_count, cvSize( win_size, win_size ), 5, features_found, feature_errors, cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.3 ), 0 ); printf("特征点匹配完毕\n"); // 用小叉叉标记特征点 #define draw_cross( image ,center, color, d ) \ cvLine( image, cvPoint( center.x - d, center.y - d ), \ cvPoint( center.x + d, center.y + d ), color, 1, CV_AA, 0); \ cvLine( image, cvPoint( center.x + d, center.y - d ), \ cvPoint( center.x - d, center.y + d ), color, 1, CV_AA, 0 ) for( int i=0; i<corner_count; i++) { CvPoint p0 = cvPoint( cvRound( cornersA[i].x ), cvRound( cornersA[i].y ) ); draw_cross(left.cvimage, p0, CV_RGB(255,0,0),3); CvPoint p1 = cvPoint( cvRound( cornersB[i].x ), cvRound( cornersB[i].y ) ); draw_cross(right.cvimage, p1, CV_RGB(255,0,0),3); // 特征点移动的轨迹线 cvLine(right.cvimage, p0, p1, CV_RGB(255,255,0), 1); } Matrix<float> points1(2, corner_count); Matrix<float> points2(2, corner_count); for (i = 0; i < corner_count; i++) { cvmSet(points1.cvmat, 0, i, cornersA[i].x); cvmSet(points1.cvmat, 1, i, cornersA[i].y); cvmSet(points2.cvmat, 0, i, cornersB[i].x); cvmSet(points2.cvmat, 1, i, cornersB[i].y); } Matrix<float> fundMatr(3, 3); Matrix<float> status(1, corner_count); // 标准RANSAC计算基础矩阵 int num = cvFindFundamentalMat(points1.cvmat, points2.cvmat, fundMatr.cvmat, CV_FM_RANSAC,1.0,0.99, status.cvmat); if( num == 1 ) printf("基础矩阵计算完毕\n"); else printf("未能计算基础矩阵\n"); // 输出基础矩阵 printf("matrix fundMatr:\n"); show_mat(fundMatr); //显示特征点匹配结果 left.show("Left"); right.show("Right"); cvWaitKey(); left.close(); right.close(); // 读入图像显示所有内点 Image<uchar> left_inlier(img1); Image<uchar> right_inlier(img2); int inlier_number = 0; for (i = 0; i < corner_count; ++i) { if (status(0, i) == 1) { CvPoint p0 = cvPoint( cvRound( cornersA[i].x ), cvRound( cornersA[i].y ) ); draw_cross(left_inlier.cvimage, cornersA[i], CV_RGB(255, 0, 0), 3); CvPoint p1 = cvPoint( cvRound( cornersB[i].x ), cvRound( cornersB[i].y ) ); draw_cross(right_inlier.cvimage, cornersB[i], CV_RGB(255, 0, 0), 3); cvLine(right_inlier.cvimage, p0, p1, CV_RGB(255,255,0), 1); inlier_number++; } } printf("内点数量为: %d\n", inlier_number); left_inlier.show("左图内点"); right_inlier.show("右图内点"); cvWaitKey(); left_inlier.close(); right_inlier.close(); /* ============================= 三维重建部分 ============================= */ // 公式 P1 = [I | 0] , P2 = [M2 + e2*b | e2] // 其中M2 * [e2]x = F, b = [1, 1, 1], 符号[]x代表反对称矩阵 fundMatr = transpose(fundMatr); Matrix<float> W(3, 3); Matrix<float> U(3, 3); Matrix<float> V(3, 3); cvSVD(fundMatr.cvmat, W.cvmat, U.cvmat, V.cvmat); printf("matrix V:\n"); show_mat(V); // 得到e2 Matrix<float> e2(3, 1); for (i = 0; i < 3; ++i) { cvmSet(e2.cvmat, i, 0, V(i, 2)); } printf("e2 is :\n"); show_mat(e2); // 以下部分计算M2 Matrix<float> skew_e2(3, 3); skew_e2 = skew_sym(e2); skew_e2 = invert(skew_e2); Matrix<float> M2(3, 3); fundMatr = transpose(fundMatr); M2 = skew_e2 * fundMatr; for (i = 0; i < 3; i++) { for (int j = 0; j< 3; j++) { cvmSet(M2.cvmat, i, j, M2(i, j)/* + e2(i, 0)*/); } } printf("M2 + e2 x b :\n"); show_mat(M2); // P1 Matrix<float> P1(3, 4); cvmSet(P1.cvmat, 0, 0, 1.0); cvmSet(P1.cvmat, 1, 1, 1.0); cvmSet(P1.cvmat, 2, 2, 1.0); printf("P1 is :\n"); show_mat(P1); // P2 Matrix<float> P2(3, 4); for (i = 0; i < 3; i++) { for (int j = 0; j< 3; j++) { cvmSet(P2.cvmat, i, j, M2(i, j)); } } cvmSet(P2.cvmat, 0, 3, e2(0, 0)); cvmSet(P2.cvmat, 1, 3, e2(1, 0)); cvmSet(P2.cvmat, 2, 3, e2(2, 0)); printf("p2 is : \n"); show_mat(P2); int u1, v1, u2, v2; u1 = v1 = u2 = v2 = 0; Matrix<float> A(4, 4); Matrix<float> W_4x4(4, 4); Matrix<float> U_4x4(4, 4); Matrix<float> V_4x4(4, 4); printf("X is :\n"); vector<vector<double> > points3D; vector<double> points; for (i = 0; i < corner_count; i++) { if (status(0, i) == 1) { u1 = cornersA[i].x; v1 = cornersA[i].y; u2 = cornersB[i].x; v2 = cornersB[i].y; A(0, 0) = P1(2, 0) * u1 - P1(0, 0); A(0, 1) = P1(2, 1) * u1 - P1(0, 1); A(0, 2) = P1(2, 2) * u1 - P1(0, 2); A(0, 3) = P1(2, 3) * u1 - P1(0, 3); A(1, 0) = P1(2, 0) * v1 - P1(1, 0); A(1, 1) = P1(2, 1) * v1 - P1(1, 1); A(1, 2) = P1(2, 2) * v1 - P1(1, 2); A(1, 3) = P1(2, 3) * v1 - P1(1, 3); A(2, 0) = P2(2, 0) * u2 - P2(0, 0); A(2, 1) = P2(2, 1) * u2 - P2(0, 1); A(2, 2) = P2(2, 2) * u2 - P2(0, 2); A(2, 3) = P2(2, 3) * u2 - P2(0, 3); A(3, 0) = P2(2, 0) * v2 - P2(1, 0); A(3, 1) = P2(2, 1) * v2 - P2(1, 1); A(3, 2) = P2(2, 2) * v2 - P2(1, 2); A(3, 3) = P2(2, 3) * v2 - P2(1, 3); cvSVD(A.cvmat, W_4x4.cvmat, U_4x4.cvmat, V_4x4.cvmat); printf("%f, %f, %f, %f \n", V_4x4(0, 3)/V_4x4(3, 3), V_4x4(1, 3)/V_4x4(3, 3), V_4x4(2, 3)/V_4x4(3, 3), 1.0); points.push_back(V_4x4(0, 3)/V_4x4(3, 3)*2); points.push_back(V_4x4(1, 3)/V_4x4(3, 3)*2); points.push_back(V_4x4(2, 3)/V_4x4(3, 3)*2); points3D.push_back(points); points.clear(); } } /* ======================== 三维显示 ======================== */ three_dimensional_show(points3D, argc, argv); delete [] cornersA; delet