基于MLS的3CCD变形模拟和校正(附C++程序)_移动最小二乘 c++

前些天刚在博客介绍过基于移动最小二乘（MLS）的图像变形，这两天恰恰就接到了一个有关图像变形的任务。说来也简单，就是3CCD是用黏合剂粘到3个棱镜面上，由于棱镜平面切割质量或者黏合剂的分布情况可能导致3个CCD接受的图像产生不同程度的变形，从而造成图像边缘上出现色散效果（类似“抖音”文字效果），要求对其进行模拟并提出校正方案。下面首先分析其可能出现的形变。

一、可能出现的形变分析

1. 若棱镜的切割与装配等都准确无误，仅仅是贴合的时候各传感器摆得不够正，则可能出现的是各通道图像之间的相对旋转；
2. 若由于棱镜切割精度不够、装配不准或是中间介质（黏合剂）呈楔形分布等原因导致成像面与出射光线的光轴不垂直，那么可能出现的是各通道图像间的投影变换；
3. 若切割不平整或是黏合剂的分布不均匀导致光线偏转角度不一致，则可能出现图像扭曲的情况。

考虑存在局部性扭曲的最极端情况，此时可将图像平面分为若干子块，每个子块的变形近似于刚性变换，即仅存在旋转与缩放。此时可用移动最小二乘变形进行模拟和校正。事实上，基于移动最小二乘的变形模拟也是目前各种变形技术中结果最真实自然的变形方法。

基于最小二乘变形的校正的出发点是最小二乘变形的相对性，总体思想是以某个色彩通道为基准，认为另两个通道的无变形标准图像可以通过对有变形的实际图像做反向的最小二乘变形获得。校正的整体流程是首先找到某色彩通道图像与基准通道图像中若干匹配的点对，以该色彩通道中的对应点为源控制点，以基准通道中的对应点为目标控制点，然后基于移动最小二乘法计算该通道图像中每个像素点变形后的坐标（浮点型），进而得到变形后每个像素点坐标（整型）对应的源点（在校正前图像中的对应点）坐标（并由此得到每个像素点的偏移量），再由插值算法得到基于移动最小二乘的变形结果作为校正结果。

三、算法的具体实施与模拟结果

为使各区域都能获得良好的校正效果，控制点应该尽可能均匀地分布于图像的各个区域。这可以通过将图像分块并在每个子块中挑选最多一个控制点的方式来实现。匹配的控制点对采用特征点检测与匹配算法获得。若采用棋盘格图像作为校正时的输入图像，由于此时各特征点在整图上缺乏独特性，因而匹配应该从一开始就限制在小于一个格子的小区域内实施以减少误匹配。若采用具有复杂纹理的图像作为输入图像，则可以先进行全局的匹配，再根据变形的局部性对错误匹配进行过滤。

由于对焦过于模糊或是场景过于简单会导致特征点检测和匹配存在失败的风险，而程序可以通过信息反馈要求使用者重新采集图像，因此这里只考虑采用棋盘格图案或者具有复杂纹理场景对焦质量尚可的图样进行校正的情形。为了降低算法复杂度使用户获得更好的操作体验，算法采用了对图像进行降采样进行校正，然后对降采样图像的偏移量进行缩放和插值的方式来获得图像中每个像素点的偏移量。

模拟变形效果的时候是将图像划分成$n_1 \times n_1$的网格，每个网格最多取一个特征点控制变形，模拟校正的时候是先筛选出一些匹配较好的特征点对作为变形前后的控制点，然后将图像划分为$n_2 \times n_2$网格，每个网格里最多取一个控制点控制校正。

废话不多说，直接上图（代码在最后，博客中贴出的是一个比较粗糙的版本，改进了模拟效果和校正精度的进阶版程序作为商业机密自然是不会在这里贴出的）。

输入图像（尺寸690*460）：

模拟变形结果（控制点网格4*4，控制点偏移量-5~5像素）：

校正结果（控制点网格划分为10*10，采样点网格大小为30*30） ：

用于校正的控制点越多、采样网格划分越精细，校正效果越好，当然，相应的计算量也越大。用更大尺寸的图像做测试，控制点偏移量不变（以像素计）的话，变形没那么明显，校正效果也好得多（这里只是给个示意，就没必要多上图了），当然，同样的，计算量也越大。演示程序使用的是SURF算子提取特征点，速度较慢，可以更换为更高效的特征点提取算子，并使用更简单的描述子去做匹配。事实上，这种应用场景下也没必要考虑多尺度的问题，所以没有必要使用多尺度的检测方法和描述子。由于这里只是做个演示，这些改进工作就不展现出来了。

程序(工作环境为VS2017+OpenCV4.5.0，程序为基于点和基于线的变形预留了接口，为仿射变换、相似变换、刚性变换预留了接口，不过校正时像素插值部分只实现了双线性插值算法，感兴趣的读者可以自己进行扩充)：

test.h

#pragma once
#include<iostream> 
#include<string>
#include<vector>
#include <math.h>
#include "opencv2/highgui/highgui.hpp"  
#include "opencv2/core/core.hpp"
#include "opencv2/features2d.hpp"  
#include "opencv2/xfeatures2d/nonfree.hpp"
#include "opencv2/opencv.hpp" 
using namespace std;
using namespace cv;
 
enum ePointLine
{
	POINT,
	LINE
};
 
enum eTransform
{
	AFFINE,
	SIMILAR,
	RIGID
};
 
struct stA
{
	float a11;
	float a12;
	float a21;
	float a22;
};
 
class CMyPoint2f : public Point2f
{
public:
	CMyPoint2f() {};
	~CMyPoint2f() {};
	CMyPoint2f(const Point2f& pt);
	CMyPoint2f(float x, float y);
	CMyPoint2f operator+(const CMyPoint2f& pt);
	CMyPoint2f operator-(const CMyPoint2f& pt);
};
 
template<class T>
CMyPoint2f operator*(T fator, const CMyPoint2f& pt)
{
	CMyPoint2f newPt;
	newPt.x = fator * pt.x;
	newPt.y = fator * pt.y;
	return newPt;
}
 
template<class T>
CMyPoint2f operator*(const CMyPoint2f& pt, T fator)
{
	return operator*(fator, pt);
}
 
struct stMLSD
{
	int iInterval = 30;
	ePointLine ePL = POINT;
	eTransform eTrans = RIGID;
	vector<CMyPoint2f> vecCtrlPt;
	vector<CMyPoint2f> vecGridPt;
	vector<vector<float>> vec2Weight;
	vector<CMyPoint2f> vecPstar;
 
	vector<vector<stA>> vec2A;
	vector<float> vecNormOfPstar;
};
 
void RandomImageDeformation(Mat& img, Mat&outImg, stMLSD& mlsd, int iXGridNum, int iYGridNum, float fMaxShift = 3.0);
 
void CtrlPtSelect(Mat & refImg, Mat & inImg, int iXGridNum, int iYGridNum, vector<CMyPoint2f>& vecCtrlPt, vector<CMyPoint2f>& vecDesCtrlPt, int radius = 5);
 
void GetGridPts(int iWidth, int iHeight, int iInterval, vector<CMyPoint2f>& vecGridPt);
 
void MLSD2DWarp(Mat& img, Mat& outImg, stMLSD& mlsd, vector<CMyPoint2f>& vecDesCtrlPt);
 
void MLSD2DTransform(stMLSD& mlsd, vector<CMyPoint2f> vecDesCtrlPt, vector<CMyPoint2f>& vecDesGridPt);
 
void PointsTransformRigid(stMLSD& mlsd, vector<CMyPoint2f> vecDesCtrlPt, vector<CMyPoint2f>& vecDesGridPt);
 
void BilinearInterp(vector<vector<CMyPoint2f>>& vec2GridPt, vector<vector<CMyPoint2f>>& vec2dXY, vector<vector<CMyPoint2f>>& vec2NewdXY);
void PixelInterp(Mat& img, Mat& outImg, vector<vector<CMyPoint2f>>& vec2NewdXY);
void MLSD2DpointsPrecompute(stMLSD& mlsd, float fOrder = 2.0);
 
void PrecomputeWeights(vector<CMyPoint2f>& vecCtrlPt, vector<CMyPoint2f>& vecGridPt, vector<vector<float>>& vec2Weight, float fOrder = 2.0);
void PrecomputeWCentroids(vector<CMyPoint2f>& vecCtrlPt, vector<vector<float>>& vec2Weight, vector<CMyPoint2f>& vecPstar);
void PrecomputePhat(vector<CMyPoint2f>& vecCtrlPt, vector<CMyPoint2f>& vecPstar, vector<vector<CMyPoint2f>>& vecPhat);
void PrecomputeA(vector<vector<CMyPoint2f>>& vecPhat, stMLSD& mlsd);
void PrecomputeRigid(stMLSD& mlsd);

test.cpp 

#include "pch.h"
#include "test.h"
#include <opencv2\imgproc\types_c.h>
#include "opencv2/imgproc/imgproc.hpp"
#include <math.h>
using namespace cv::xfeatures2d;
 
int main()
{
	Mat img = imread("LYF.jpg");
	//resize(img, img, Size(img.cols / 3, img.rows / 3));
	vector<Mat> vecChannels(3);
	split(img, vecChannels);
	Mat R = vecChannels[0];
	Mat G = vecChannels[1];
	Mat B = vecChannels[2];
 
	//模拟3CCD扭曲错位
	stMLSD mlsdG, mlsdB;
	Mat outR, outG, outB;
	float fMaxShift = 5.0;
	RandomImageDeformation(G, outG, mlsdG, 4, 4, fMaxShift);
	RandomImageDeformation(B, outB, mlsdB, 4, 4, fMaxShift);
	G = outG;
	B = outB;
	Mat outImg;
	vecChannels[0] = R;
	vecChannels[1] = G;
	vecChannels[2] = B;
	merge(vecChannels, outImg);
 
 
	stMLSD mlsdGR, mlsdBR;
	vector<CMyPoint2f> vecDesCtrlPtGR, vecDesCtrlPtBR;
	CtrlPtSelect(R, G, 10, 10, mlsdGR.vecCtrlPt, vecDesCtrlPtGR);
	CtrlPtSelect(R, B, 10, 10, mlsdBR.vecCtrlPt, vecDesCtrlPtBR);
	//计算采样格点坐标
	GetGridPts(img.cols, img.rows, mlsdGR.iInterval, mlsdGR.vecGridPt);
	mlsdBR.vecGridPt = mlsdGR.vecGridPt;
 
	//预先计算不变量
	MLSD2DpointsPrecompute(mlsdGR);
	MLSD2DpointsPrecompute(mlsdBR);
 
	//执行变形
	MLSD2DWarp(G, outG, mlsdGR, vecDesCtrlPtGR);
	MLSD2DWarp(B, outB, mlsdBR, vecDesCtrlPtBR);
	vecChannels[1] = outG;
	vecChannels[2] = outB;
	merge(vecChannels, outImg);
 
	return 0;
}
 
void RandomImageDeformation(Mat & img, Mat & outImg, stMLSD& mlsd, int iXGridNum, int iYGridNum, float fMaxShift)
{
	auto surf = SURF::create();
	vector<KeyPoint> vecKeyPt;
	surf->detect(img, vecKeyPt);
 
	//选取若干控制点，并生成变形后对应的坐标
	int iKeyPtNum = vecKeyPt.size();
	vector<CMyPoint2f> vecCtrlPt;
	vector<CMyPoint2f> vecDesCtrlPt;
	int iGridWidth = ceil(img.cols / float(iXGridNum));
	int iGridHeight = ceil(img.rows / float(iYGridNum));
	vector<vector<CMyPoint2f>> vec2KeyPt(iXGridNum*iYGridNum, vector<CMyPoint2f>());
	srand(time(0));
	for (int i = 0; i < vecKeyPt.size(); i++)
	{
		int x = vecKeyPt[i].pt.x / iGridWidth;
		int y = vecKeyPt[i].pt.y / iGridHeight;
		vec2KeyPt[y*iXGridNum + x].push_back(vecKeyPt[i].pt);
	}
	for (int i = 0; i < iYGridNum; i++)
	{
		for (int j = 0; j < iXGridNum; j++)
		{
			int idx = i * iXGridNum + j;
			if (vec2KeyPt[idx].size() > 0)
			{
				int idx2 = rand() % vec2KeyPt[idx].size();
				CMyPoint2f ctrlPt = vec2KeyPt[idx][idx2];
				mlsd.vecCtrlPt.push_back(ctrlPt);
				vecDesCtrlPt.push_back(ctrlPt + fMaxShift * CMyPoint2f((rand() % 100 - 50) / 50.0, (rand() % 100 - 50) / 50.0));
			}
		}
	}
 
	//计算采样格点坐标
	GetGridPts(img.cols, img.rows, mlsd.iInterval, mlsd.vecGridPt);
 
	//预先计算不变量
	float fOrder = 2.0;
	MLSD2DpointsPrecompute(mlsd, fOrder);
 
	//执行变形
	MLSD2DWarp(img, outImg, mlsd, vecDesCtrlPt);
}
 
void CtrlPtSelect(Mat & refImg, Mat & inImg, int iXGridNum, int iYGridNum, vector<CMyPoint2f>& vecCtrlPt, vector<CMyPoint2f>& vecDesCtrlPt, int radius)
{
	auto surf = SURF::create();
	vector<KeyPoint> vecPt1, vecPt2;
	Mat descrip1, descrip2;
	surf->detectAndCompute(refImg, cv::Mat(), vecPt1, descrip1); //特征描述子生成
	surf->detectAndCompute(inImg, cv::Mat(), vecPt2, descrip2);
	vector<DMatch> matches;
 
 
	if (descrip1.type() != CV_32F || descrip2.type() != CV_32F)
	{
		descrip1.convertTo(descrip1, CV_32F);
		descrip2.convertTo(descrip2, CV_32F);
	}
 
	auto matcher = DescriptorMatcher::create(DescriptorMatcher::MatcherType::FLANNBASED);
	matcher->match(descrip1, descrip2, matches); //初始匹配
 
	std::vector< cv::DMatch > finalMatches;
	for (int i = 0; i < matches.size(); i++) //依据设定的错位幅度限制对初始匹配进行过滤
	{
		int queryIdx = matches[i].queryIdx;
		int trainIdx = matches[i].trainIdx;
		CMyPoint2f pt1, pt2;
		pt1 = vecPt1[queryIdx].pt;
		pt2 = vecPt2[trainIdx].pt;
		if (abs(pt1.x - pt2.x) < radius && abs(pt1.y - pt2.y) < radius)
		{
			finalMatches.push_back(matches[i]);
		}
	}
	//cv::Mat imageOutput;
	//cv::drawMatches(refImg, vecPt1, inImg, vecPt2, finalMatches, imageOutput);
 
	int iGridWidth = ceil(refImg.cols / float(iXGridNum));
	int iGridHeight = ceil(refImg.rows / float(iYGridNum));
	vector<vector<CMyPoint2f>> vec2CtrlPt(iXGridNum*iYGridNum, vector<CMyPoint2f>());
	vector<vector<CMyPoint2f>> vec2DesCtrlPt(iXGridNum*iYGridNum, vector<CMyPoint2f>());
	srand(time(0));
	int num = 0;
	for (int i = 0; i < finalMatches.size(); i++)
	{
		CMyPoint2f ctrlPt = vecPt2[finalMatches[i].trainIdx].pt;
		CMyPoint2f desCtrlPt = vecPt1[finalMatches[i].queryIdx].pt;
		int x = ctrlPt.x / iGridWidth;
		int y = ctrlPt.y / iGridHeight;
		vec2CtrlPt[y*iXGridNum + x].push_back(ctrlPt);
		vec2DesCtrlPt[y*iXGridNum + x].push_back(desCtrlPt);
	}
	for (int i = 0; i < iYGridNum; i++)
	{
		for (int j = 0; j < iXGridNum; j++)
		{
			int idx = i * iXGridNum + j;
			if (vec2CtrlPt[idx].size() > 0)
			{
				int idx2 = rand() % vec2CtrlPt[idx].size();
				vecCtrlPt.push_back(vec2CtrlPt[idx][idx2]);
				vecDesCtrlPt.push_back(vec2DesCtrlPt[idx][idx2]);
			}
		}
	}
}
 
void GetGridPts(int iWidth, int iHeight, int iInterval, vector<CMyPoint2f>& vecGridPt)
{
	int iXnum = (iWidth - 1) / iInterval + 1;
	int iYnum = (iHeight - 1) / iInterval + 1;
	vecGridPt.resize(iXnum*iYnum);
	for (int j = 0; j < iYnum; j++)
	{
		for (int i = 0; i < iXnum; i++)
		{
			vecGridPt[j*iXnum + i] = CMyPoint2f(i*iInterval, j*iInterval);
		}
	}
}
 
void MLSD2DWarp(Mat & img, Mat& outImg, stMLSD & mlsd, vector<CMyPoint2f>& vecDesCtrlPt)
{
	int iChannels = img.channels();
	int iWidth = img.cols;
	int iHeight = img.rows;
 
	//求变形后的格点坐标
	vector<CMyPoint2f> vecDesGridPt;
	MLSD2DTransform(mlsd, vecDesCtrlPt, vecDesGridPt);
 
	int iGridPtNum = mlsd.vecGridPt.size();
	vector<CMyPoint2f> dXY(iGridPtNum); //原始格点坐标与变形后坐标的差值
	for (int i = 0; i < iGridPtNum; i++)
	{
		dXY[i].x = mlsd.vecGridPt[i].x - vecDesGridPt[i].x;
		dXY[i].y = mlsd.vecGridPt[i].y - vecDesGridPt[i].y;
	}
 
	int iInterval = mlsd.iInterval;
	int iSampHeight = (iHeight - 1) / iInterval + 1;
	int iSampWidth = (iWidth - 1) / iInterval + 1;
	vector<vector<CMyPoint2f>> vec2GridPt(iSampHeight, vector<CMyPoint2f>(iSampWidth));
	vector<vector<CMyPoint2f>> vec2dXY(iSampHeight, vector<CMyPoint2f>(iSampWidth));
	for (int i = 0; i < iSampHeight; i++)
	{
		for (int j = 0; j < iSampWidth; j++)
		{
			vec2GridPt[i][j] = CMyPoint2f(j*iInterval, i*iInterval);
		}
	}
 
	for (int i = 0; i < iSampHeight; i++)
	{
		for (int j = 0; j < iSampWidth; j++)
		{
			vec2dXY[i][j] = dXY[i*iSampWidth + j];
		}
	}
 
	//对每个像素点的偏移量进行双线性插值，由此得到其对应的源点坐标
	vector<vector<CMyPoint2f>> vec2NewdXY(iHeight, vector<CMyPoint2f>(iWidth));
	BilinearInterp(vec2GridPt, vec2dXY, vec2NewdXY);
 
	//像素插值
	PixelInterp(img, outImg, vec2NewdXY);
}
 
void MLSD2DTransform(stMLSD& mlsd, vector<CMyPoint2f> vecDesCtrlPt, vector<CMyPoint2f>& vecDesGridPt)
{
	switch (mlsd.ePL)
	{
	case POINT:
		switch (mlsd.eTrans)
		{
		case AFFINE:
			//TODO
			break;
		case SIMILAR:
			//TODO
			break;
		case RIGID:
			PointsTransformRigid(mlsd, vecDesCtrlPt, vecDesGridPt);
			break;
		default:
			break;
		}
		break;
	case LINE:
		switch (mlsd.eTrans)
		{
		case AFFINE:
			//TODO
			break;
		case SIMILAR:
			//TODO
			break;
		case RIGID:
			//TODO
			break;
		default:
			break;
		}
		break;
	default:
		break;
	}
}
 
void PointsTransformRigid(stMLSD& mlsd, vector<CMyPoint2f> vecDesCtrlPt, vector<CMyPoint2f>& vecDesGridPt)
{
	int iCtrlPtNum = mlsd.vecCtrlPt.size();
	int iGridPtNum = mlsd.vecGridPt.size();
	vector<CMyPoint2f> vecQstar(iGridPtNum);
	PrecomputeWCentroids(vecDesCtrlPt, mlsd.vec2Weight, vecQstar);
	vector<CMyPoint2f> vecFr(iGridPtNum);
	vecDesGridPt.resize(iGridPtNum);
	for (int i = 0; i < iCtrlPtNum; i++)
	{
		for (int j = 0; j < iGridPtNum; j++)
		{
			float fXoffsetDCP2QS = vecDesCtrlPt[i].x - vecQstar[j].x;
			float fYoffsetDCP2QS = vecDesCtrlPt[i].y - vecQstar[j].y;
			vecFr[j].x += fXoffsetDCP2QS * mlsd.vec2A[i][j].a11 + fYoffsetDCP2QS * mlsd.vec2A[i][j].a21;
			vecFr[j].y += fXoffsetDCP2QS * mlsd.vec2A[i][j].a12 + fYoffsetDCP2QS * mlsd.vec2A[i][j].a22;
		}
	}
	vector<float> vecNormFr(iGridPtNum);
	for (int i = 0; i < iGridPtNum; i++)
	{
		vecNormFr[i] = sqrt(pow(vecFr[i].x, 2) + pow(vecFr[i].y, 2));
	}
 
	float delta = pow(10, -9);
	for (int i = 0; i < iGridPtNum; i++)
	{
		float fNormFactor = mlsd.vecNormOfPstar[i] / (vecNormFr[i] + delta);
		vecDesGridPt[i].x = fNormFactor * vecFr[i].x + vecQstar[i].x;
		vecDesGridPt[i].y = fNormFactor * vecFr[i].y + vecQstar[i].y;
	}
}
 
void BilinearInterp(vector<vector<CMyPoint2f>>& vec2GridPt, vector<vector<CMyPoint2f>>& vec2dXY, vector<vector<CMyPoint2f>>& vec2NewdXY)
{
	//对采样格点边界所包围的点进行插值，程序允许非均匀采样间隔
	for (int i = 0; i < vec2GridPt.size() - 1; i++)
	{
		for (int j = 0; j < vec2GridPt[0].size() - 1; j++)
		{
			int x1 = vec2GridPt[i][j].x;
			int x2 = vec2GridPt[i][j + 1].x;
			int y1 = vec2GridPt[i][j].y;
			int y2 = vec2GridPt[i + 1][j].y;
			CMyPoint2f v11 = vec2dXY[i][j];
			CMyPoint2f v12 = vec2dXY[i][j + 1];
			CMyPoint2f v21 = vec2dXY[i + 1][j];
			CMyPoint2f v22 = vec2dXY[i + 1][j + 1];
			float dx = x2 - x1;
			float dy = y2 - y1;
			for (int m = 0; m <= dy; m++)
			{
				for (int n = 0; n <= dx; n++)
				{
					float w11 = (1 - m / dy)*(1 - n / dx);
					float w12 = (1 - m / dy)*(n / dx);
					float w21 = (m / dy)*(1 - n / dx);
					float w22 = (m*n) / (dx*dy);
					vec2NewdXY[y1 + m][x1 + n] = w11 * v11 + w12 * v12 + w21 * v21 + w22 * v22;
				}
			}
		}
	}
 
	//采样网格右方的采样点插值
	int iXend = vec2GridPt[0][vec2GridPt[0].size() - 1].x;
	int iYend = vec2GridPt[vec2GridPt.size() - 1][vec2GridPt[0].size() - 1].y;
	for (int i = int(vec2GridPt[0][0].y); i < iYend; i++)
	{
		for (int j = iXend + 1; j < vec2NewdXY[0].size(); j++)
		{
			vec2NewdXY[i][j] = 2 * vec2NewdXY[i][j - 1] - vec2NewdXY[i][j - 2];
		}
	}
 
	//采样网格下方的采样点插值
	for (int i = iYend + 1; i < vec2NewdXY.size(); i++)
	{
		for (int j = int(vec2GridPt[0][0].x); j < iXend; j++)
		{
			vec2NewdXY[i][j] = 2 * vec2NewdXY[i - 1][j] - vec2NewdXY[i - 2][j];
		}
	}
 
	//采样网格右下方的采样点插值
	for (int i = iYend; i < vec2NewdXY.size(); i++)
	{
		for (int j = iXend + 1; j < vec2NewdXY[0].size(); j++)
		{
			vec2NewdXY[i][j] = vec2NewdXY[i - 1][j] + vec2NewdXY[i][j - 1] - vec2NewdXY[i - 1][j - 1];
		}
	}
}
 
void PixelInterp(Mat & img, Mat & outImg, vector<vector<CMyPoint2f>>& vec2NewdXY)
{
	//默认采用双线性插值和零填充
	int iChannels = img.channels();
	int iWidth = img.cols;
	int iHeight = img.rows;
	outImg = img.clone();
	outImg.setTo(0);
	if (iChannels == 1)
	{
		for (int i = 0; i < iHeight; i++)
		{
			for (int j = 0; j < iWidth; j++)
			{
				float fX = vec2NewdXY[i][j].x + j;
				float fY = vec2NewdXY[i][j].y + i;
				if (fX >= 0 && fX < iWidth && fY >= 0 && fY < iHeight)
				{
					int iLx = int(fX);
					int iLy = int(fY);
					int iHx = min(iLx + 1, iWidth - 1);
					int iHy = min(iLy + 1, iHeight - 1);
					float dx = fX - iLx;
					float dy = fY - iLy;
					float w11 = (1 - dx)*(1 - dy);
					float w12 = dx * (1 - dy);
					float w21 = (1 - dx)*dy;
					float w22 = dx * dy;
					float v11 = img.at<uchar>(iLy, iLx);
					float v12 = img.at<uchar>(iLy, iHx);
					float v21 = img.at<uchar>(iHy, iLx);
					float v22 = img.at<uchar>(iHy, iHx);
					outImg.at<uchar>(i, j) = (uchar)(w11 * v11 + w12 * v12 + w21 * v21 + w22 * v22);
				}
			}
		}
	}
	else if (iChannels == 3)
	{
		for (int i = 0; i < iHeight; i++)
		{
			for (int j = 0; j < iWidth; j++)
			{
				float fX = vec2NewdXY[i][j].x + j;
				float fY = vec2NewdXY[i][j].y + i;
				if (fX >= 0 && fX < iWidth && fY >= 0 && fY < iHeight)
				{
					int iLx = int(fX);
					int iLy = int(fY);
					int iHx = min(iLx + 1, iWidth - 1);
					int iHy = min(iLy + 1, iHeight - 1);
					float dx = fX - iLx;
					float dy = fY - iLy;
					float w11 = (1 - dx)*(1 - dy);
					float w12 = dx * (1 - dy);
					float w21 = (1 - dx)*dy;
					float w22 = dx * dy;
					Vec3b v11 = img.at<Vec3b>(iLy, iLx);
					Vec3b v12 = img.at<Vec3b>(iLy, iHx);
					Vec3b v21 = img.at<Vec3b>(iHy, iLx);
					Vec3b v22 = img.at<Vec3b>(iHy, iHx);
					outImg.at<Vec3b>(i, j) = (Vec3b)(w11 * v11 + w12 * v12 + w21 * v21 + w22 * v22);
				}
			}
		}
	}
}
 
void MLSD2DpointsPrecompute(stMLSD & mlsd, float fOrder)
{
	//计算每个控制点对于每个格点的权重
	PrecomputeWeights(mlsd.vecCtrlPt, mlsd.vecGridPt, mlsd.vec2Weight, fOrder);
 
	//预先计算每个格点对应的加权中心控制点P*
	PrecomputeWCentroids(mlsd.vecCtrlPt, mlsd.vec2Weight, mlsd.vecPstar);
 
	switch (mlsd.eTrans)
	{
	case AFFINE:
		//PrecomputeAffine(mlsd);  //待实现
		break;
	case SIMILAR:
		//PrecomputeSimilar(mlsd);  //待实现
		break;
	case RIGID:
		PrecomputeRigid(mlsd);
		break;
	default:
		break;
	}
}
 
void PrecomputeWeights(vector<CMyPoint2f>& vecCtrlPt, vector<CMyPoint2f>& vecGridPt, vector<vector<float>>& vec2Weight, float fOrder)
{
	//预分配空间
	vec2Weight.resize(vecCtrlPt.size());
	for (int i = 0; i < vecCtrlPt.size(); i++)
	{
		vec2Weight[i].resize(vecGridPt.size());
 
		CMyPoint2f ctrlPt = vecCtrlPt[i];
		for (int j = 0; j < vecGridPt.size(); j++)
		{
			float fSqureDis = pow(ctrlPt.x - vecGridPt[j].x, 2) + pow(ctrlPt.y - vecGridPt[j].y, 2);
			float delta = pow(10, -8);
			if (fOrder == 2.0)
			{
				vec2Weight[i][j] = 1 / (fSqureDis + delta);
			}
			else
				vec2Weight[i][j] = 1 / (pow(fSqureDis, fOrder / 2) + delta);
		}
	}
}
 
void PrecomputeWCentroids(vector<CMyPoint2f>& vecCtrlPt, vector<vector<float>>& vec2Weight, vector<CMyPoint2f>& vecPstar)
{
	int iCtrlPtNum = vecCtrlPt.size();
	int iGridPtNum = vec2Weight[0].size();
	vecPstar.resize(iGridPtNum);
 
	for (int i = 0; i < iGridPtNum; i++)
	{
		float fSumX = 0, fSumY = 0;
		float fSumWeight = 0;
		for (int j = 0; j < iCtrlPtNum; j++)
		{
			fSumWeight += vec2Weight[j][i];
			fSumX += vec2Weight[j][i] * vecCtrlPt[j].x;
			fSumY += vec2Weight[j][i] * vecCtrlPt[j].y;
		}
		vecPstar[i] = CMyPoint2f(fSumX / fSumWeight, fSumY / fSumWeight);
	}
}
 
void PrecomputePhat(vector<CMyPoint2f>& vecCtrlPt, vector<CMyPoint2f>& vecPstar, vector<vector<CMyPoint2f>>& vecPhat)
{
	int iCtrlPtNum = vecCtrlPt.size();
	int iGridPtNum = vecPstar.size();
	vecPhat.resize(iCtrlPtNum);
	for (int i = 0; i < iCtrlPtNum; i++)
	{
		vecPhat[i].resize(iGridPtNum);
		for (int j = 0; j < iGridPtNum; j++)
		{
			vecPhat[i][j] = CMyPoint2f(vecCtrlPt[i].x - vecPstar[j].x, vecCtrlPt[i].y - vecPstar[j].y);
		}
	}
}
 
void PrecomputeA(vector<vector<CMyPoint2f>>& vecPhat, stMLSD& mlsd)
{
	int iCtrlPtNum = mlsd.vecCtrlPt.size();
	int iGridPtNum = mlsd.vecGridPt.size();
	//vector<float> vecXoffsetGrid2WC(iGridPtNum);
	//vector<float> vecYoffsetGrid2WC(iGridPtNum);
	mlsd.vec2A.resize(iCtrlPtNum);
	mlsd.vecNormOfPstar.resize(iGridPtNum);
	for (int i = 0; i < iCtrlPtNum; i++)
	{
		mlsd.vec2A[i].resize(iGridPtNum);
		for (int j = 0; j < iGridPtNum; j++)
		{
			float fXoffsetGrid2WC = mlsd.vecGridPt[j].x - mlsd.vecPstar[j].x;
			float fYoffsetGrid2WC = mlsd.vecGridPt[j].y - mlsd.vecPstar[j].y;
			float fXoffsetCtrlPt2WC = vecPhat[i][j].x;
			float fYoffsetCtrlPt2WC = vecPhat[i][j].y;
			if (i == 0)
			{
				mlsd.vecNormOfPstar[j] = sqrt(pow(fXoffsetGrid2WC, 2) + pow(fYoffsetGrid2WC, 2));
			}
			float fWeight = mlsd.vec2Weight[i][j];
			mlsd.vec2A[i][j].a11 = fWeight * (fXoffsetCtrlPt2WC * fXoffsetGrid2WC + fYoffsetCtrlPt2WC * fYoffsetGrid2WC);
			mlsd.vec2A[i][j].a22 = mlsd.vec2A[i][j].a11;
			mlsd.vec2A[i][j].a12 = fWeight * (fXoffsetCtrlPt2WC * fYoffsetGrid2WC - fYoffsetCtrlPt2WC * fXoffsetGrid2WC);
			mlsd.vec2A[i][j].a21 = -mlsd.vec2A[i][j].a12;
		}
	}
}
 
void PrecomputeRigid(stMLSD& mlsd)
{
	vector<vector<CMyPoint2f>> vecPhat;
	PrecomputePhat(mlsd.vecCtrlPt, mlsd.vecPstar, vecPhat);
 
	PrecomputeA(vecPhat, mlsd);
}
 
CMyPoint2f::CMyPoint2f(const Point2f& pt)
{
	this->x = pt.x;
	this->y = pt.y;
}
 
CMyPoint2f::CMyPoint2f(float x, float y)
{
	this->x = x;
	this->y = y;
}
 
CMyPoint2f CMyPoint2f::operator+(const CMyPoint2f& pt)
{
	CMyPoint2f newPt;
	newPt.x = this->x + pt.x;
	newPt.y = this->y + pt.y;
	return newPt;
}
 
CMyPoint2f CMyPoint2f::operator-(const CMyPoint2f & pt)
{
	CMyPoint2f newPt;
	newPt.x = this->x - pt.x;
	newPt.y = this->y - pt.y;
	return newPt;
}