写在前面

本学期数字摄影测量课程学习中，老师让同学实现书上提到的特征点提取、相关系数影像匹配、最小二乘影像匹配的相关算法，特征点提取确实有很多参考的，但是影像匹配的代码几乎都是付费下载的，自己也去找过，大多还是Matlab的代码。最后还是跟同学交流，然后用C++造轮子（因为选修的计算机视觉是用C++做的，所以这个就也用C++了），希望对有用到的同学有所帮助。

特征点提取

特征点提取有很多算子，例如Moravec、Harris、SIFT等等，毕竟造轮子的能力有限，就选择最简单的Moravec算子实现，下面介绍相关代码，因为在写代码的时候就打算发博客了，注释比较详细，就不啰嗦了。

1. 求兴趣值

//寻找数组最大值、最小值 void find(float a[], int m, float &max, float &min) {  min = a[0];  max = a[0]; for (int i = 0; i < m; i++) { if (a[i] > max) {    max = a[i]; continue; } else if (a[i] < min) {    min = a[i]; continue; } } } //求兴趣值，输入(图像矩阵、Moravec窗口大小、该像素的x,y) float getInterestValue(cv::Mat m_srcimg, int Moravecsize, int i_x, int j_y) { int halfsize = (Moravecsize) / 2;//定义小半个窗口大小 float temp4[4];//创立四个方向的数组来进行"*"型的平方和 //数组初始化 for (int i = 0; i < 4; i++) {   temp4[i] = 0; } //累加四个方向求平方和 for (int i = 0; i < Moravecsize; i++) { float l = m_srcimg.at<uchar>(i_x - halfsize + i, j_y);   temp4[0] += pow(m_srcimg.at<uchar>(i_x - halfsize + i, j_y) - m_srcimg.at<uchar>(i_x - halfsize + i + 1, j_y), 2);   temp4[1] += pow(m_srcimg.at<uchar>(i_x, j_y - halfsize + i) - m_srcimg.at<uchar>(i_x, j_y - halfsize + i + 1), 2);   temp4[2] += pow(m_srcimg.at<uchar>(i_x - halfsize + i, j_y - halfsize + i) - m_srcimg.at<uchar>(i_x - halfsize + i + 1, j_y - halfsize + i + 1), 2);   temp4[3] += pow(m_srcimg.at<uchar>(i_x - halfsize + i, j_y + halfsize - i) - m_srcimg.at<uchar>(i_x - halfsize + i + 1, j_y + halfsize - i - 1), 2); } float min, max; find(temp4, 4, max, min);//给极小值赋值 return min;//返回极小值，即该像素点的兴趣值 } 

2. 特征点提取

int Moravec(std::string path,std::vector<cv::Point3f> &featurePointLeft) {  cv::Mat imageRGB = cv::imread(path, cv::IMREAD_COLOR); if (imageRGB.empty()) {   std::cout << "Fail to read the image!" << path << std::endl; return -1; }  cv::imshow("image", imageRGB);//显示原图  cv::waitKey(0);  cv::Mat imageGray;  cv::cvtColor(imageRGB, imageGray, cv::COLOR_RGB2GRAY);  std::vector<cv::Point3f> f; GaussianBlur(imageGray, imageGray, cv::Size(5, 5), 0, 0);//使用opencv自带高斯滤波预处理  cv::Mat result/*结果32bit*/, resultNorm, resultNormUInt8;  result = cv::Mat::zeros(imageGray.size(), CV_32FC1); int Moravecsize = 5;//计算Moravec兴趣值的窗口大小 int Inhibitionsize = 9;//抑制局部最大的窗口大小 float sum = 0; for (int i = 5; i < imageGray.rows - 5; i++) { for (int j = 5; j < imageGray.cols - 5; j++) { float min = getInterestValue(imageGray, Moravecsize, i, j);    result.at<float>(i, j) = min;    sum += min; } } //对小于阈值的该像素兴趣值置为零 float mean = sum / (result.rows*result.cols);//经验阈值设置 if (mean < 60) { mean = 300; }  mean = 700; for (int i = 0; i <result.rows; i++) { for (int j = 0; j < result.cols; j++) { if (result.at<float>(i, j) < mean) {     result.at<float>(i, j) = 0; } } } int halfInhibitionsize = Inhibitionsize / 2;//定义小半个窗口大小 //抑制局部最大 for (int i = halfInhibitionsize; i < result.rows - 1 - halfInhibitionsize; i++) { for (int j = halfInhibitionsize; j < result.cols - 1 - halfInhibitionsize; j++) { float temp1 = result.at<float>(i, j); for (int m = 0; m < Inhibitionsize; m++) { for (int n = 0; n < Inhibitionsize; n++) { float temp2 = result.at<float>(i - halfInhibitionsize + m, j - halfInhibitionsize + n); if (temp1 < temp2) {       result.at<float>(i, j) = 0;       n = Inhibitionsize;       m = Inhibitionsize; } } } } } //存储特征点 for (int i = halfInhibitionsize; i < result.rows - halfInhibitionsize - 1; i++) { for (int j = halfInhibitionsize; j < result.cols - halfInhibitionsize - 1; j++) { if (result.at<float>(i, j) > 0) {     cv::Point3f temp;     temp.x = i;     temp.y = j;     temp.z = result.at<float>(i, j);     featurePointLeft.push_back(temp); } } } //开始删除同意一窗口值一样的重复点，尽管出现概率较小，但较大的图像往往某些窗口中会存在好几个数值相等的极大值 for (int i = 0; i < featurePointLeft.size() - 1; i++) { for (int j = i + 1; j < featurePointLeft.size(); j++) { if ((featurePointLeft.at(i).z == featurePointLeft.at(j).z)) { if (abs(featurePointLeft.at(i).x - featurePointLeft.at(j).x) < Inhibitionsize || abs(featurePointLeft.at(i).y - featurePointLeft.at(j).y) < Inhibitionsize) {      featurePointLeft.erase(featurePointLeft.begin() + j);      i = 0; break; } } } } //Normalizing image to 0-255  cv::normalize(result, resultNorm, 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat());  cv::convertScaleAbs(resultNorm, resultNormUInt8); //画图展示出来 int radius = 5; for (size_t n = 0; n < featurePointLeft.size(); n++) { int i = int(featurePointLeft.at(n).y); int j = int(featurePointLeft.at(n).x);   cv::circle(resultNormUInt8, cv::Point(i, j), radius, cv::Scalar(255), 1, 8, 0);   cv::circle(imageRGB, cv::Point(i, j), radius, cv::Scalar(0, 255, 255), 1, 4, 0);   cv::line(imageRGB, cv::Point(i - radius - 2, j), cv::Point(i + radius + 2, j), cv::Scalar(0, 255, 255), 1, 8, 0);   cv::line(imageRGB, cv::Point(i, j - radius - 2), cv::Point(i, j + radius + 2), cv::Scalar(0, 255, 255), 1, 8, 0); }   cv::imshow("MoravecResult", resultNormUInt8);  cv::imshow("imageRGB", imageRGB);  cv::waitKey(0); } 

3. 效果展示
   

相关系数影像匹配

相关系数影像匹配的步骤就是先通过特征点提取把左片的特征点提取出来，然后通过选取出的左片特征点，在右片确定的搜索区域寻找相关系数最大的点作为它的右片上的同名点，过程并不复杂。

1. 计算相关系数

//相关系数图像匹配 float Get_coefficient(cv::Mat matchLeftWindow, cv::Mat imageRight, int x, int y) { //根据左搜索窗口确定右搜索窗口的大小  cv::Mat Rmatchwindow;  Rmatchwindow.create(matchLeftWindow.rows, matchLeftWindow.cols, CV_32FC1); float aveRImg = 0; for (int m = 0; m < matchLeftWindow.rows; m++) { for (int n = 0; n < matchLeftWindow.cols; n++) {    aveRImg += imageRight.at<uchar>(x + m, y + n);    Rmatchwindow.at<float>(m, n) = imageRight.at<uchar>(x + m, y + n); } }  aveRImg = aveRImg / (matchLeftWindow.rows*matchLeftWindow.cols); for (int m = 0; m < matchLeftWindow.rows; m++) { for (int n = 0; n < matchLeftWindow.cols; n++) {    Rmatchwindow.at<float>(m, n) -= aveRImg; } } //开始计算相关系数 float cofficent1 = 0; float cofficent2 = 0; float cofficent3 = 0; for (int m = 0; m < matchLeftWindow.rows; m++) { for (int n = 0; n < matchLeftWindow.cols; n++) {    cofficent1 += matchLeftWindow.at<float>(m, n)*Rmatchwindow.at<float>(m, n);    cofficent2 += Rmatchwindow.at<float>(m, n)*Rmatchwindow.at<float>(m, n);    cofficent3 += matchLeftWindow.at<float>(m, n)*matchLeftWindow.at<float>(m, n); } } double cofficent = cofficent1 / sqrt(cofficent2 * cofficent3); return cofficent; } 

2. 将Vector内的最大数据移至首位

void vectorsort(std::vector < cv::Point3f> &Temp_sort) { for (int i = 0; i < Temp_sort.size() - 1; i++) { float tem = 0; float temx = 0; float temy = 0; // 内层for循环控制相邻的两个元素进行比较 for (int j = i + 1; j < Temp_sort.size(); j++) { if (Temp_sort.at(i).z < Temp_sort.at(j).z) {     tem = Temp_sort.at(j).z;     Temp_sort.at(j).z = Temp_sort.at(i).z;     Temp_sort.at(i).z = tem;      temx = Temp_sort.at(j).x;     Temp_sort.at(j).x = Temp_sort.at(i).x;     Temp_sort.at(i).x = temx;      temy = Temp_sort.at(j).y;     Temp_sort.at(j).y = Temp_sort.at(i).y;     Temp_sort.at(i).y = temy; } } } } 

3. 合并左右核线影像，同名点连接

void lastview(cv::Mat imageLeftRGB, cv::Mat imageRightRGB, std::vector<cv::Point3f> featurePointLeft, std::vector<cv::Point3f> featurePointRight) {  cv::Mat bothview;//输出图像  bothview.create(imageLeftRGB.rows, imageLeftRGB.cols + imageRightRGB.cols, imageLeftRGB.type()); for (int i = 0; i <imageLeftRGB.rows; i++) { for (int j = 0; j < imageLeftRGB.cols; j++) {    bothview.at<cv::Vec3b>(i, j) = imageLeftRGB.at<cv::Vec3b>(i, j); } } for (int i = 0; i <imageRightRGB.rows; i++) { for (int j = imageLeftRGB.cols; j <imageLeftRGB.cols + imageRightRGB.cols; j++) {    bothview.at<cv::Vec3b>(i, j) = imageRightRGB.at<cv::Vec3b>(i, j - imageLeftRGB.cols); } }//左右影像合二为一  for (int i = 0; i < featurePointRight.size(); i++) { int a = (rand() % 200); int b = (rand() % 200 + 99); int c = (rand() % 200) - 50; if (a > 100 || a < 0) {    a = 255; } if (b > 255 || b < 0) {    b = 88; } if (c > 255 || c < 0) {    c = 188; } int radius = 5; //左片 int lm = int(featurePointLeft.at(i).y); int ln = int(featurePointLeft.at(i).x);    cv::circle(bothview, cv::Point(lm, ln), radius, cv::Scalar(0, 255, 255), 1, 4, 0);   cv::line(bothview, cv::Point(lm - radius - 2, ln), cv::Point(lm + radius + 2, ln), cv::Scalar(0, 255, 255), 1, 8, 0);   cv::line(bothview, cv::Point(lm, ln - radius - 2), cv::Point(lm, ln + radius + 2), cv::Scalar(0, 255, 255), 1, 8, 0); //右片 int rm = int(featurePointRight.at(i).y + imageLeftRGB.cols); int rn = int(featurePointRight.at(i).x);    cv::circle(bothview, cv::Point(rm, rn), radius,cv::Scalar(0, 255, 255), 1, 4, 0);   cv::line(bothview, cv::Point(rm - radius - 2, rn), cv::Point(rm + radius + 2, rn), cv::Scalar(0, 255, 255), 1, 8, 0);   cv::line(bothview, cv::Point(rm, rn - radius - 2), cv::Point(rm, rn + radius + 2), cv::Scalar(0, 255, 255), 1, 8, 0); //连接   cv::line(bothview, cv::Point(lm,ln), cv::Point(rm, rn),cv::Scalar(a, b, c), 1,8,0); }   cv::imshow("左右片影像同名点展示", bothview);  cv::waitKey(0); } 

4. 相关系数影像匹配

int kyhMatchingImg(std::string pathLeft, std::string pathRight, std::vector<cv::Point3f> featurePointLeft) {  cv::Mat imageLeft, imageLeftRGB = cv::imread(pathLeft, cv::IMREAD_COLOR);  cv::Mat imageRight, imageRightRGB = cv::imread(pathRight, cv::IMREAD_COLOR); if (imageLeftRGB.empty()) {   std::cout << "Fail to read the image:" << pathLeft << std::endl; return -1; } if (imageRightRGB.empty()) {   std::cout << "Fail to read the image:" << pathRight << std::endl; return -1; }  cv::cvtColor(imageLeftRGB, imageLeft, cv::COLOR_BGR2GRAY);  cv::cvtColor(imageRightRGB, imageRight, cv::COLOR_BGR2GRAY); int matchsize = 9;//相关系数的正方形窗口的边长 int half_matchsize = matchsize / 2;//边长的一半   std::vector<cv::Point3f> featurePointRight;//右片匹配到的数据 float lowst_door = 0.7; //相关系数法匹配的阈值 int dist_width = 270;//左相片与右相片的相对距离，在这里通过手动观察 //进行f数据的预处理 删除不符合规范的数据 for (size_t i = 0; i < featurePointLeft.size(); i++) { //这里的 5 = half_matchsize + 1 if ((featurePointLeft.at(i).y + dist_width < imageLeft.cols) || (imageLeft.cols - featurePointLeft.at(i).y <5)) {    featurePointLeft.erase(featurePointLeft.begin() + i);    i--; continue; } if ((featurePointLeft.at(i).x < 5) || (imageLeft.rows - featurePointLeft.at(i).x < 5)) {    featurePointLeft.erase(featurePointLeft.begin() + i);    i--; continue; } } //创建左窗口的小窗口  cv::Mat matchLeftWindow;  matchLeftWindow.create(matchsize, matchsize, CV_32FC1); for (size_t i = 0; i < featurePointLeft.size(); i++) { float aveLImg = 0; for (int m = 0; m <matchsize; m++) { for (int n = 0; n < matchsize; n++) {     aveLImg += imageLeft.at<uchar>(featurePointLeft.at(i).x - half_matchsize + m, featurePointLeft.at(i).y - half_matchsize + n);     matchLeftWindow.at<float>(m, n) = imageLeft.at<uchar>(featurePointLeft.at(i).x - half_matchsize + m, featurePointLeft.at(i).y - half_matchsize + n); } }   aveLImg = aveLImg / (matchsize* matchsize);//求取左窗口平均值 //均除某个值 for (int m = 0; m < matchsize; m++) { for (int n = 0; n < matchsize; n++) {     matchLeftWindow.at<float>(m, n) = matchLeftWindow.at<float>(m, n) - aveLImg; } } //***************************对右窗口进行计算 //首先预估右窗口的位置 int halflengthsize = 10; //搜索区的半径   std::vector < cv::Point3f> tempfeatureRightPoint; //去除跑到窗口外的点 for (int ii = -halflengthsize; ii <= halflengthsize; ii++) { for (int jj = -halflengthsize; jj <= halflengthsize; jj++) { //为了省事…… 把边缘超限的都给整没了 if ((featurePointLeft.at(i).x<(halflengthsize + 5)) || (imageRight.rows - featurePointLeft.at(i).x)<(halflengthsize + 5) || (featurePointLeft.at(i).y + dist_width - imageLeft.cols)<(halflengthsize + 5)) {      cv::Point3f temphalflengthsize;      temphalflengthsize.x = 0;      temphalflengthsize.y = 0;      temphalflengthsize.z = 0;      tempfeatureRightPoint.push_back(temphalflengthsize); } else {      cv::Point3f temphalflengthsize; int x = featurePointLeft.at(i).x + ii - half_matchsize; int y = featurePointLeft.at(i).y + dist_width - imageLeft.cols + jj - half_matchsize; float  coffee = Get_coefficient(matchLeftWindow, imageRight, x, y);      temphalflengthsize.x = featurePointLeft.at(i).x + ii;      temphalflengthsize.y = featurePointLeft.at(i).y + dist_width - imageLeft.cols + jj;      temphalflengthsize.z = coffee;      tempfeatureRightPoint.push_back(temphalflengthsize); } } } vectorsort(tempfeatureRightPoint); //剔除相关系数小于阈值的点 if (tempfeatureRightPoint.at(0).z > lowst_door&&tempfeatureRightPoint.at(0).z < 1) {    cv::Point3f tempr;    tempr.x = tempfeatureRightPoint.at(0).x;    tempr.y = tempfeatureRightPoint.at(0).y;    tempr.z = tempfeatureRightPoint.at(0).z;    featurePointRight.push_back(tempr); } else {    featurePointLeft.erase(featurePointLeft.begin() + i);    i--; continue; } } //展示 lastview(imageLeftRGB, imageRightRGB, featurePointLeft, featurePointRight); return 0; } 

6. 效果演示
   

最小二乘影像匹配

这部分是我做得很久的一次，和磊哥讨论又进行了一些调整。大概是平时python和matlab惯坏了，总之就是调试花了很久，时隔两年再次体会到debug到凌晨三点的感觉（中途从九点半和高中同学喝酒到一点半之后回家再debug的）。原理是参照武汉大学出版社出版的《数字摄影测量学（第二版）》中第五章内容。据介绍，德国 Ackermann教授 提出了一种新的影像匹配方法—— 最小二乘影像匹配（ least aquares image matching）。由于 该方法充分利用了影像窗口内的信息进行平差计算，使影像匹配可以达到 1/10 甚至 1/ 100 像素的高精度，即影像匹配精度可达到子像素（subpixel）等级。为此，最小二乘影像 匹配被称为“高精度影像匹配”。
下面介绍代码部分。

1. 最小二乘匹配
   最小二乘匹配是一个迭代求最优解的过程，需要有初值，所以我们先用相关系数匹配方法得到初值，然后再进行迭代。

int kyhMatchImgbyLST(std::string pathLeft, std::string pathRight, std::vector<cv::Point3f> featurePointLeft) {  cv::Mat imageLeft, imageLeftRGB = cv::imread(pathLeft, cv::IMREAD_COLOR);  cv::Mat imageRight, imageRightRGB = cv::imread(pathRight, cv::IMREAD_COLOR); if (imageLeftRGB.empty()) {   std::cout << "Fail to read the image:" << pathLeft << std::endl; return -1; } if (imageRightRGB.empty()) {   std::cout << "Fail to read the image:" << pathRight << std::endl; return -1; }  cv::cvtColor(imageLeftRGB, imageLeft, cv::COLOR_BGR2GRAY);  cv::cvtColor(imageRightRGB, imageRight, cv::COLOR_BGR2GRAY); int matchsize = 9;//相关系数的正方形窗口的边长 int half_matchsize = matchsize / 2;//边长的一半   std::vector<cv::Point3f> featurePointRight;//右片匹配到的数据 float lowst_door = 0.7; //相关系数法匹配的阈值 int dist_width = 270;//左相片与右相片的相对距离，在这里通过手动观察 //进行f数据的预处理 删除不符合规范的数据 for (size_t i = 0; i < featurePointLeft.size(); i++) { //这里的 5 = half_matchsize + 1 if ((featurePointLeft.at(i).y + dist_width < imageLeft.cols) || (imageLeft.cols - featurePointLeft.at(i).y <5)) {    featurePointLeft.erase(featurePointLeft.begin() + i);    i--; continue; } if ((featurePointLeft.at(i).x < 5) || (imageLeft.rows - featurePointLeft.at(i).x < 5)) {    featurePointLeft.erase(featurePointLeft.begin() + i);    i--; continue; } } //创建左窗口的小窗口  cv::Mat matchLeftWindow;  matchLeftWindow.create(matchsize, matchsize, CV_32FC1); for (size_t i = 0; i < featurePointLeft.size(); i++) { float aveLImg = 0; for (int m = 0; m <matchsize; m++) { for (int n = 0; n < matchsize; n++) {     aveLImg += imageLeft.at<uchar>(featurePointLeft.at(i).x - half_matchsize + m, featurePointLeft.at(i).y - half_matchsize + n);     matchLeftWindow.at<float>(m, n) = imageLeft.at<uchar>(featurePointLeft.at(i).x - half_matchsize + m, featurePointLeft.at(i).y - half_matchsize + n); } }   aveLImg = aveLImg / (matchsize* matchsize);//求取左窗口平均值 //均除某个值 for (int m = 0; m < matchsize; m++) { for (int n = 0; n < matchsize; n++) {     matchLeftWindow.at<float>(m, n) = matchLeftWindow.at<float>(m, n) - aveLImg; } } //***************************对右窗口进行计算 //首先预估右窗口的位置 int halflengthsize = 10; //搜索区的半径   std::vector < cv::Point3f> tempfeatureRightPoint; //去除跑到窗口外的点 for (int ii = -halflengthsize; ii <= halflengthsize; ii++) { for (int jj = -halflengthsize; jj <= halflengthsize; jj++) { //为了省事…… 把边缘超限的都给整没了 if ((featurePointLeft.at(i).x<(halflengthsize + 5)) || (imageRight.rows - featurePointLeft.at(i).x)<(halflengthsize + 5) || (featurePointLeft.at(i).y + dist_width - imageLeft.cols)<(halflengthsize + 5)) {      cv::Point3f temphalflengthsize;      temphalflengthsize.x = 0;      temphalflengthsize.y = 0;      temphalflengthsize.z = 0;      tempfeatureRightPoint.push_back(temphalflengthsize); } else {      cv::Point3f temphalflengthsize; int x = featurePointLeft.at(i).x + ii - half_matchsize; int y = featurePointLeft.at(i).y + dist_width - imageLeft.cols + jj - half_matchsize; float  coffee = Get_coefficient(matchLeftWindow, imageRight, x, y);      temphalflengthsize.x = featurePointLeft.at(i).x + ii;      temphalflengthsize.y = featurePointLeft.at(i).y + dist_width - imageLeft.cols + jj;      temphalflengthsize.z = coffee;      tempfeatureRightPoint.push_back(temphalflengthsize); } } } vectorsort(tempfeatureRightPoint); if (tempfeatureRightPoint.at(0).z > lowst_door&&tempfeatureRightPoint.at(0).z < 1) {    cv::Point3f tempr;    tempr.x = tempfeatureRightPoint.at(0).x;    tempr.y = tempfeatureRightPoint.at(0).y;    tempr.z = tempfeatureRightPoint.at(0).z;    featurePointRight.push_back(tempr); } else {    featurePointLeft.erase(featurePointLeft.begin() + i);    i--; continue; } } //得到左右两片的同名点初始值 /*正式开始最小二乘匹配*/  std::vector<cv::Point3f> featureRightPointLST;//存储最小二乘匹配到的点 //求几何畸变的初始值  cv::Mat formerP = cv::Mat::eye(2 * featurePointLeft.size(), 2 * featurePointLeft.size(), CV_32F)/*权矩阵*/,   formerL = cv::Mat::zeros(2 * featurePointLeft.size(), 1, CV_32F)/*常数项*/,   formerA = cv::Mat::zeros(2 * featurePointLeft.size(), 6, CV_32F)/*系数矩阵*/; for (int i = 0; i < featurePointLeft.size(); i++) { float x1 = featurePointLeft.at(i).x; float y1 = featurePointLeft.at(i).y; float x2 = featurePointRight.at(i).x; float y2 = featurePointRight.at(i).y; float coef = featurePointRight.at(i).z;//初始同名点的相关系数作为权重   formerP.at<float>(2 * i, 2 * i) = coef;   formerP.at<float>(2 * i + 1, 2 * i + 1) = coef;   formerL.at<float>(2 * i, 0) = x2;   formerL.at<float>(2 * i + 1, 0) = y2;   formerA.at<float>(2 * i, 0) = 1; formerA.at<float>(2 * i, 1) = x1; formerA.at<float>(2 * i, 2) = y1;   formerA.at<float>(2 * i + 1, 3) = 1; formerA.at<float>(2 * i + 1, 4) = x1; formerA.at<float>(2 * i + 1, 5) = y1; }  cv::Mat Nbb = formerA.t()*formerP*formerA, U = formerA.t()*formerP*formerL;  cv::Mat formerR = Nbb.inv()*U; //开始进行最小二乘匹配 for (int i = 0; i < featurePointLeft.size(); i++) { //坐标的迭代初始值 float x1 = featurePointLeft.at(i).x; float y1 = featurePointLeft.at(i).y; float x2 = featurePointRight.at(i).x; float y2 = featurePointRight.at(i).y; //几何畸变参数迭代初始值 float a0 = formerR.at<float>(0, 0); float a1 = formerR.at<float>(1, 0); float a2 = formerR.at<float>(2, 0); float b0 = formerR.at<float>(3, 0); float b1 = formerR.at<float>(4, 0); float b2 = formerR.at<float>(5, 0); //辐射畸变迭代初始值 float h0 = 0, h1 = 1; //当后一次相关系数小于前一次，迭代停止 float beforeCorrelationCoe = 0/*前一个相关系数*/, CorrelationCoe = 0; float xs = 0,ys = 0; while (beforeCorrelationCoe <= CorrelationCoe) {    beforeCorrelationCoe = CorrelationCoe;    cv::Mat C = cv::Mat::zeros(matchsize*matchsize, 8, CV_32F);//系数矩阵，matchsize为左片目标窗口大小    cv::Mat L = cv::Mat::zeros(matchsize*matchsize, 1, CV_32F);//常数项    cv::Mat P = cv::Mat::eye(matchsize*matchsize, matchsize*matchsize, CV_32F);//权矩阵 float sumgxSquare = 0, sumgySquare = 0, sumXgxSquare = 0, sumYgySquare = 0; int dimension = 0;//用于矩阵赋值 float sumLImg = 0, sumLImgSquare = 0, sumRImg = 0, sumRImgSquare = 0, sumLR = 0; for (int m = x1 - half_matchsize; m <= x1 + half_matchsize; m++) { for (int n = y1 - half_matchsize; n <= y1 + half_matchsize; n++) { float x2 = a0 + a1*m + a2*n; float y2 = b0 + b1*m + b2*n; int I = std::floor(x2); int J = std::floor(y2);//不大于自变量的最大整数 if (I <= 1 || I >= imageRight.rows - 1 || J <= 1 || J >= imageRight.cols - 1) {       I = 2; J = 2; P.at<float>((m - (y1 -5) - 1)*(2 * 4+ 1) + n - (x1 - 5), (m - (y1 - 5) - 1)*(2 * 4 + 1) + n - (x1 - 5)) = 0; } //双线性内插重采样 float linerGray = (J + 1 - y2)*((I + 1 - x2)*imageRight.at<uchar>(I, J) + (x2-I)*imageRight.at<uchar>(I+1,J)) +(y2 - J)*((I+1-x2)*imageRight.at<uchar>(I,J+1)+(x2-I)*imageRight.at<uchar>(I+1,J+1)); //辐射校正 float radioGray = h0 + h1*linerGray;//得到相应灰度       sumRImg += radioGray;      sumRImgSquare += radioGray*radioGray; //确定系数矩阵 float gy = 0.5*(imageRight.at<uchar>(I, J + 1) - imageRight.at<uchar>(I, J - 1)); float gx = 0.5*(imageRight.at<uchar>(I + 1, J) - imageRight.at<uchar>(I - 1, J));      C.at<float>(dimension, 0) = 1; C.at<float>(dimension, 1) = linerGray;      C.at<float>(dimension, 2) = gx; C.at<float>(dimension, 3) = x2*gx;      C.at<float>(dimension, 4) = y2*gx; C.at<float>(dimension, 5) = gy;      C.at<float>(dimension, 6) = x2*gy; C.at<float>(dimension, 7) = y2*gy; //常数项赋值      L.at<float>(dimension,0) = imageLeft.at<uchar>(m,n)-radioGray;      dimension =dimension + 1; //左窗口加权平均 float gyLeft = 0.5*(imageLeft.at<uchar>(m, n + 1) - imageLeft.at<uchar>(m, n - 1)); float gxLeft = 0.5*(imageLeft.at<uchar>(m + 1, n) - imageLeft.at<uchar>(m - 1, n));      sumgxSquare += gxLeft*gxLeft;      sumgySquare += gyLeft*gyLeft;      sumXgxSquare += m*gxLeft*gxLeft;      sumYgySquare += n*gyLeft*gyLeft; //左片灰度相加用于求相关系数      sumLImg += imageLeft.at<uchar>(m, n);      sumLImgSquare += imageLeft.at<uchar>(m, n)*imageLeft.at<uchar>(m, n);      sumLR += radioGray*imageLeft.at<uchar>(m, n); } } //计算相关系数 float coefficent1 = sumLR - sumLImg*sumRImg / (matchsize*matchsize); float coefficent2 = sumLImgSquare - sumLImg*sumLImg / (matchsize*matchsize); float coefficent3 = sumRImgSquare - sumRImg*sumRImg / (matchsize*matchsize);    CorrelationCoe = coefficent1 / sqrt(coefficent2*coefficent3); //计算辐射畸变和几何变形的参数    cv::Mat Nb = C.t()*P*C, Ub = C.t()*P*L;    cv::Mat parameter = Nb.inv()*Ub; float dh0 = parameter.at<float>(0,0); float dh1 = parameter.at<float>(1,0); float da0 = parameter.at<float>(2,0); float da1 = parameter.at<float>(3,0);float da2 = parameter.at<float>(4,0); float db0 = parameter.at<float>(5,0); float db1 = parameter.at<float>(6,0);float db2 = parameter.at<float>(7,0);     a0 = a0 + da0 + a0*da1 + b0*da2;    a1 = a1 + a1*da1 + b1*da2;    a2 = a2 + a2*da1 + b2*da2;    b0 = b0 + db0 + a0*db1 + b0*db2;    b1 = b1 + a1*db1 + b1*db2;    b2 = b2 + a2*db1 + b2*db2;    h0 = h0 + dh0 + h0*dh1;    h1 = h1 + h1*dh1; float xt = sumXgxSquare / sumgxSquare; float yt = sumYgySquare / sumgySquare;    xs = a0 + a1*xt + a2*yt;    ys = b0 + b1*xt + b2*yt; }   cv::Point3f tempPoint;   tempPoint.x = xs;   tempPoint.y = ys;   tempPoint.z = CorrelationCoe;   featureRightPointLST.push_back(tempPoint); } lastview(imageLeftRGB, imageRightRGB, featurePointLeft, featureRightPointLST); //输出两种匹配策略的结果，观察坐标是否发生变化  std::ofstream outputfile;  outputfile.open("FeturePointOutput.txt"); if (outputfile.is_open()) { for (size_t i = 0; i < featurePointRight.size(); i++) {    outputfile << featurePointRight.at(i).x << ", " << featurePointRight.at(i).y << ", " << featurePointRight.at(i).z << "，" <<     featureRightPointLST.at(i).x << ", " << featureRightPointLST.at(i).y << ", " << featureRightPointLST.at(i).z << std::endl; } }  outputfile.close(); return 0; } 

2. 效果展示
   
3. 结果对比
   从肉眼上看，并没有觉得和相关系数匹配得到的点差太多，也许是这两幅影像质量很好，“棱角分明”。于是我试着输出坐标和相关系数，对比一下两种方法得到的点，发现肉眼看的果然没错。两种方法匹配到的都是有八百六十二个点，只有四对点之间坐标差距大，而且相关系数还变成了负数，负相关？老师也很疑惑。

写在后面

写这些代码的时候并没有考虑到时间复杂度之类的东西，计算起来稍微有点慢，但是能用。感谢磊哥不厌其烦的跟我解释最小二乘，其实到现在我也有点地方没弄明白。就希望如果也在学数字摄影测量的同学，这篇文章能够帮到你，不用去下载付费的代码了。如果有错漏的地方，请评论或者私信我指正，感激不尽。