基于MLS的3CCD变形模拟和校正(附C++程序)

前些天刚在博客介绍过基于移动最小二乘（MLS）的图像变形，这两天恰恰就接到了一个有关图像变形的任务。说来也简单，就是3CCD是用黏合剂粘到棱镜的三个面上，由于环境的改变，黏合剂的物理性质可能发生改变导致3个CCD接受的图像产生不同程度的变形，从而造成图像边缘上出现色散效果（类似“抖音”文字效果），要求对其进行模拟并提出校正方案。由于目前手头没有3CCD相机，没法实测其在不同环境下可能出现的变形种类和严重程度，所以模拟时就把情况想得稍复杂些，假定每一个小区域的变形都是刚性变形，每个区域可以对应不同的形变，也就是对应了MLS的Rigid情况。模拟变形效果的时候是将图像划分成的网格，每个网格最多取一个特征点控制变形，模拟校正的时候是先筛选出一些匹配较好的特征点对作为变形前后的控制点，然后同样是将图像划分为网格，每个网格里最多取一个控制点控制校正。

废话不多说，直接上图（代码在最后）。

输入图像（尺寸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;
	surf->detect(refImg, vecPt1); //特征点检测
	surf->detect(inImg, 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;
}