最小二乘法的拟合做相机标定

做~自己de王妃 提交于 2020-03-09 19:41:16

最小二乘法的拟合做相机标定

	我们知道opencv已经有了成熟的图像畸变的校正算法,但是要获取一些畸变的中间量,有时候并不方便,在这里尝试用最小二乘法来做一个鱼眼相机图像畸变的校正。
先来了解一下最小二乘法:最小二乘法是一种优化算法,最小二乘法名字的缘由有两个:一是要将误差最小化,二是将误差最小化的方法是使误差的平方和最小化。利用最小二乘法可以简便地求得未知的数据,并使得这些求得的数据与实际数据之间误差的平方和为最小。最小二乘法还可用于曲线拟合,所拟合的曲线可以是线性拟合与非线性拟合。
我们看一下工业相机的畸变图像:

如图,是用棋盘格来体现的畸变情况,在这里选择的是7x6的黑白棋盘格,这个可以自己制作。
然后来看一下算法,在这里,在这里我们需要用到findChessboardCorners方法来获取棋盘格的黑白格角点,获取角点之后,选取最靠近中心的点,以这个点作为圆心,向图像四周画同心圆。如果没有畸变,每隔一个棋盘格宽度画一个同心圆,那么画出的圆应该是等距的,现在由于畸变,同心圆直接的距离会发生变化而且是越靠近图像边缘,距离越小。

如图:由于这张图的畸变不是特别明显,所以同心圆的距离也不是特别明显。这里主要是为了讲解一下算法。我们的拟合就是以半径的畸变来做拟合。将第一个同心圆的半径作为一个标准,在findChessboardCorners方法中,角点的位置是已知的,worldPoints中保存的是点的位置。那么我们就能得到棋盘格各个点到同心圆圆心的位置距离,比如在这张图上左下角点的位置是(0,0),那么图中圆心的位置是(2,3),那么位置距离就是sqrt(2^2 + 3^2) = 3.6,而像素距离则是两点之间的像素坐标计算出来然后利用理论上没有畸变的像素距离,就是位置距离乘上最小同心圆的半径,那就是实际像素距离 / 理论像素距离,就可以得到畸变的系数。得到这些点之后,可以画出来看一下。

因为畸变不是很明显,我们并不难看出来具体是一次还是二次,那么就都尝试一下,在计算的过程中,还要考虑到图像横向和纵向可能有差异,所以要在x和y方向分别选取一个半径,然后将y方向的同步到x方向。在这里提几点,计算距离图像中心最近的点是用所有的点去和图像正中心计算像素距离,取最近的,接下来看一下这些点是怎么计算的。
//计算两点距离
double pointDis(int x, int y, int cen_x, int cen_y) {
	double dis = sqrt((x - cen_x) * (x - cen_x) + (y - cen_y) * (y - cen_y));

	return dis;
}
//计算距离最近,z_points1是所有点的集合,cen_flag 是中点得我位置。
for (int i = 1; i < 42; i++)
	{
		double p_dis = pointDis(z_points1[i].x, z_points1[i].y, cen_x, cen_y);
		if (p_dis < p_dis1) {
			p_dis1 = p_dis;
			c_points = z_points1[i];
			cen_flag = i;
		}
	}
然后计算畸变的系数,这里需要考虑到findChessboardCorners方法取得的点可能在中点的左边还是右边,如果在中心左边,就向右去一天边作为半径,反之则像左取,上下也是一样。R1和R2是为了考虑x和y方向的差异,得到各点的位置距离和像素距离之和,要做一个排序
if (c_points.x > cen_x) {
		if (poi[cen_flag - 7].z_points.x < c_points.x)
		{
			R1 = pointDis(poi[cen_flag - 7].z_points.x, poi[cen_flag - 7].z_points.y, c_points.x, c_points.y);
		}
		else {
			R1 = pointDis(poi[cen_flag + 7].z_points.x, poi[cen_flag + 7].z_points.y, c_points.x, c_points.y);
		}
	}
	else {
		if (poi[cen_flag + 7].z_points.x > c_points.x)
		{
			R1 = pointDis(poi[cen_flag + 7].z_points.x, poi[cen_flag + 7].z_points.y, c_points.x, c_points.y);
		}
		else {
			R1 = pointDis(poi[cen_flag - 7].z_points.x, poi[cen_flag - 7].z_points.y, c_points.x, c_points.y);
		}
	}

	if (c_points.y > cen_y) {
		if (poi[cen_flag + 1].z_points.y < c_points.y) {
			R2 = pointDis(poi[cen_flag + 1].z_points.x, poi[cen_flag + 1].z_points.y, c_points.x, c_points.y);
		}
		else {
			R2 = pointDis(poi[cen_flag - 1].z_points.x, poi[cen_flag - 1].z_points.y, c_points.x, c_points.y);
		}
	}
	else {
		if (poi[cen_flag - 1].z_points.y > c_points.y) {
			R2 = pointDis(poi[cen_flag - 1].z_points.x, poi[cen_flag - 1].z_points.y, c_points.x, c_points.y);
		}
		else {
			R2 = pointDis(poi[cen_flag + 1].z_points.x, poi[cen_flag + 1].z_points.y, c_points.x, c_points.y);
		}
	}
	if (R1 < R2) {
		R = R1 / R2;
	}
	else {
		R = R2 / R1;
	}
	for (int i = 0; i < 42; i++)
	{
		poi[i].p_dis = pointDis1(poi[i].z_points.x, poi[i].z_points.y, c_points.x, c_points.y, R);
		poi[i].l_dis = pointDis(poi[i].worldPoints.x, poi[i].worldPoints.y, poi[cen_flag].worldPoints.x, poi[cen_flag].worldPoints.y);
	}

	for (int i = 0; i < 42; i++) {
		if (i != cen_flag) {
			poi[i].r_dis = poi[i].l_dis * R1;
			poi[i].pr_s = poi[i].r_dis / poi[i].p_dis;
		}
	}	
	for (int i = 0; i < 42; i++) {
		xa[i] = poi[i].p_dis;
		ya[i] = poi[i].pr_s;
	}

	sort(xa, xa + 42);
sort(ya, ya + 42);

接下来对获取的点做拟合,得到拟合系数,由于C++中没有像python一样现成的集成库,所以最小二乘法要自己来做,以二次拟合为例,二次方程为y = ax^2 + bx + c,要实现拟合,就要解出a,b,c三个系数,从线性代数的方法来看,就是要解线性方程组,在这里就要用到矩阵的知识了,首先将方程组简化为一个3x3矩阵和一个1x3的列矩阵。有矩阵原理可知,AB = C的矩阵乘法,假设B为3x3矩阵,C为1x3列矩阵,那要想求出A,就得求出B的逆矩阵。首先先得到矩阵B和C
double mata[3][3];
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			double tx = 0.0;
			for (int k = 1; k < 42; k++) {
				double dx = 1.0;
				for (int l = 0; l < j + i; l++) {
					dx = dx * xa[k];
				}
				tx += dx;
			}
			mata[i][j] = tx;
		}
	}

	double matb[3];
	for (int i = 0; i < 3; i++) {
		double ty = 0.0;
		for (int k = 1; k < 42; k++) {
			double dy = 1.0;
			for (int l = 0; l < i; l++) {
				dy = dy * xa[k];
			}
			ty += ya[k] * dy;
		}
		matb[i] = ty;
	}

这里的mata和matb就是矩阵B和C,那么接下来就求mata的逆矩阵
bool GetMatrixInverse(double src[3][3], int n, double des[3][3]) {
	double flag = getA(src, n);
	double t[3][3];
	if (flag == 0)
	{
		return false;
	}
	else
	{
		getAStart(src, n, t);
		for (int i = 0; i < n; i++)
		{
			for (int j = 0; j < n; j++)
			{
				des[i][j] = t[i][j] / flag;
			}

		}
	}

	return true;
}

double getA(double arcs[3][3], int n) {
	if (n == 1)
	{
		return arcs[0][0];
	}
	double ans = 0;
	double temp[3][3] = { 0.0 };
	int i, j, k;

	for (i = 0; i < n; i++)
	{
		for (j = 0; j < n - 1; j++)
		{
			for (k = 0; k < n - 1; k++)
			{
				temp[j][k] = arcs[j + 1][(k >= i) ? k + 1 : k];

			}
		}
		double t = getA(temp, n - 1);
		if (i % 2 == 0)
		{
			ans += arcs[0][i] * t;
		}
		else
		{
			ans -= arcs[0][i] * t;
		}
	}
	return ans;
}

void  getAStart(double arcs[3][3], int n, double ans[3][3])
{
	if (n == 1)
	{
		ans[0][0] = 1;
		return;
	}
	int i, j, k, t;
	double temp[3][3];
	for (i = 0; i < n; i++)
	{
		for (j = 0; j < n; j++)
		{
			for (k = 0; k < n - 1; k++)
			{
				for (t = 0; t < n - 1; t++)
				{
					temp[k][t] = arcs[k >= i ? k + 1 : k][t >= j ? t + 1 : t];
				}
			}


			ans[j][i] = getA(temp, n - 1);
			if ((i + j) % 2 == 1)
			{
				ans[j][i] = -ans[j][i];
			}
		}
	}
}
得到了mata的逆之后,就可以求出二次方程的系数了
for (int i = 0; i < 3; i++) {
		double xaa = 0.0;
		for (int j = 0; j < 3; j++) {
			xaa += des[i][j] * matb[j];
		}
		mataa[i] = xaa;
	}
	return true;
这里得到的mataa中就是二次项的三个系数了。得到了畸变方程后,按照之前的算法,计算出畸变点本来的坐标位置,然后重新绘制一张图,就能得到校正后的图像了。
Point lenscenter(652, 470); //镜头中心在图像中的位置
	uchar *data = (uchar*)img.data; //原图像数
	uchar *newimgdata = new uchar[img.step*img.rows]; //新图像数据
	for (int row = 0; row < img.rows; row++) { //操作数据区,要注意OpenCV的RGB的存储顺序为GBR 
		for (int col = 0; col < img.cols; col++) { //示例为亮度调节 
			int pointsrc = row * img.step + col * 3;
			double r = sqrt((row - lenscenter.y)*(row - lenscenter.y) + (col - lenscenter.x)*(col - lenscenter.x));
			double s = mataa[0] + mataa[1] * r + mataa[2] * pow(r, 2);//比例		
			int x1 = (col - lenscenter.x)*s + lenscenter.x;
			int y1 = (row - lenscenter.y)*s + lenscenter.y;
			int x = (x1 + col) / 2;
			int y = (y1 + row) / 2;


			if (x >= 0 && x < img.cols && y >= 0 && y < img.rows)
			{
				int pointdrc = y * img.step + x * 3;
				newimgdata[pointdrc + 0] = data[pointsrc + 0];
				newimgdata[pointdrc + 1] = data[pointsrc + 1];
				newimgdata[pointdrc + 2] = data[pointsrc + 2];
			}
		}
	}
	memset(data, 0, img.step*img.rows);
	memcpy(data, newimgdata, img.step*img.rows);

这张图是修复后的图像效果,可以看到还是有一定校正效果的,修复之后由于像素偏移,有的地方会没有像素点,出现图图中的白色空隙,这个可以自行修补。另外如果要计算图像中点的实际距离,可以用CalcDistance这个函数。详细见完整代码。
附完整代码:
#include "pch.h"
#include <iostream>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <vector>
#include <string.h>
#include <math.h>

using namespace cv;
using namespace std;

double findBit(Mat src, double xa[42], double ya[42]);
double pointDis(int x, int y, int cen_x, int cen_y);
double pointDis1(int x, int y, int cen_x, int cen_y, double R);
bool GetMatrixInverse(double src[3][3], int n, double des[3][3]);
double getA(double arcs[3][3], int n);
void  getAStart(double arcs[3][3], int n, double ans[3][3]);
bool matrix_multiply(double des[3][3], double matb[3], double mataa[3]);
double CalcDistance(Point point1, Point point2, double mataa[3]);
Mat ToRight(double mataa[3]);

struct pointsInfo
{
	Point2f z_points;
	Point3f worldPoints;
	double p_dis;
	double l_dis;
	double r_dis;
	double pr_s;
};


vector<Point2f> z_points1;
vector<Point3f> worldPoints1;
Point2f c_points;
vector<Point2f> t_points;
vector<Point2f> train_points;


int main()
{
	Mat src = imread("D:\\code\\code\\0003\\hb4.bmp");
	Mat src_gray, src_threshold;
	cvtColor(src, src_gray, COLOR_BGR2GRAY);
	double xa[42];
	double ya[42];
	findBit(src_gray, xa, ya);
	double mata[3][3];
	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			double tx = 0.0;
			for (int k = 1; k < 42; k++) {
				double dx = 1.0;
				for (int l = 0; l < j + i; l++) {
					dx = dx * xa[k];
				}
				tx += dx;
			}
			mata[i][j] = tx;
		}
	}

	double matb[3];
	for (int i = 0; i < 3; i++) {
		double ty = 0.0;
		for (int k = 1; k < 42; k++) {
			double dy = 1.0;
			for (int l = 0; l < i; l++) {
				dy = dy * xa[k];
			}
			ty += ya[k] * dy;
		}
		matb[i] = ty;
	}
	double des[3][3];
	GetMatrixInverse(mata, 3, des);
	double mataa[3];
	matrix_multiply(des, matb, mataa);
	for (int i = 0; i < 3; i++) {
		cout<< mataa[i] <<endl;
	}
	//CalcDistance(p1, p2, mataa);
	ToRight(mataa);
	waitKey(0);

	return 0;
}

double pointDis(int x, int y, int cen_x, int cen_y) {
	double dis = sqrt((x - cen_x) * (x - cen_x) + (y - cen_y) * (y - cen_y));

	return dis;
}

double pointDis1(int x, int y, int cen_x, int cen_y, double R) {
	double dis = sqrt((x - cen_x) * (x - cen_x) + (y - cen_y) * R * (y - cen_y) * R);

	return dis;
}

double findBit(Mat src, double xa[42], double ya[42]) {
	int h, w, k = 0;
	h = src.rows;
	w = src.cols;
	int cen_x = w / 2;
	int cen_y = h / 2;
	double R;
	struct pointsInfo poi[42];
	struct pointsInfo *p;
	int cen_flag;
	double R1, R2;
	//cvtColor(src, gray, COLOR_BGR2GRAY);
	/*for (int i = 1; i < src.rows - 1; i++)
	{
		for (int j = 1; j < src.cols -1; j++) {
			if (src.at<uchar>(i, j) <= 5 && src.at<uchar>(i+1, j) <= 5 && src.at<uchar>(i, j+1) <= 5 && src.at<uchar>(i-1, j) <= 5 && src.at<uchar>(i, j-1) <= 5 ) {
				z_points.push_back(Point2f(i, j));
				k++;
			}
		}
	}*/
	for (int j = 0; j < 6; j++) {
		for (int k = 0; k < 7; k++) {
			worldPoints1.push_back(Point3f(k*1.0, j*1.0, 0.0f));
		}
	}
	bool found = findChessboardCorners(src, Size(7, 6), z_points1, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE);

	for (int i = 0; i < 42; i++) {
		poi[i].worldPoints = worldPoints1[i];
		poi[i].z_points = z_points1[i];
	}
	double p_dis1 = pointDis(z_points1[0].x, z_points1[0].y, cen_x, cen_y);
	for (int i = 1; i < 42; i++)
	{
		double p_dis = pointDis(z_points1[i].x, z_points1[i].y, cen_x, cen_y);
		if (p_dis < p_dis1) {
			p_dis1 = p_dis;
			c_points = z_points1[i];
			cen_flag = i;
		}
	}
	/*cout << cen_flag << endl;
	cout << poi[cen_flag].z_points << endl;
	cout << poi[cen_flag].worldPoints << endl;
	cout << c_points << endl;*/

	if (c_points.x > cen_x) {
		if (poi[cen_flag - 7].z_points.x < c_points.x)
		{
			R1 = pointDis(poi[cen_flag - 7].z_points.x, poi[cen_flag - 7].z_points.y, c_points.x, c_points.y);
		}
		else {
			R1 = pointDis(poi[cen_flag + 7].z_points.x, poi[cen_flag + 7].z_points.y, c_points.x, c_points.y);
		}
	}
	else {
		if (poi[cen_flag + 7].z_points.x > c_points.x)
		{
			R1 = pointDis(poi[cen_flag + 7].z_points.x, poi[cen_flag + 7].z_points.y, c_points.x, c_points.y);
		}
		else {
			R1 = pointDis(poi[cen_flag - 7].z_points.x, poi[cen_flag - 7].z_points.y, c_points.x, c_points.y);
		}
	}

	if (c_points.y > cen_y) {
		if (poi[cen_flag + 1].z_points.y < c_points.y) {
			R2 = pointDis(poi[cen_flag + 1].z_points.x, poi[cen_flag + 1].z_points.y, c_points.x, c_points.y);
		}
		else {
			R2 = pointDis(poi[cen_flag - 1].z_points.x, poi[cen_flag - 1].z_points.y, c_points.x, c_points.y);
		}
	}
	else {
		if (poi[cen_flag - 1].z_points.y > c_points.y) {
			R2 = pointDis(poi[cen_flag - 1].z_points.x, poi[cen_flag - 1].z_points.y, c_points.x, c_points.y);
		}
		else {
			R2 = pointDis(poi[cen_flag + 1].z_points.x, poi[cen_flag + 1].z_points.y, c_points.x, c_points.y);
		}
	}
	cout << R1 << endl;
	cout << R2 << endl;

	if (R1 < R2) {
		R = R1 / R2;
	}
	else {
		R = R2 / R1;
	}
	for (int i = 0; i < 42; i++)
	{
		poi[i].p_dis = pointDis1(poi[i].z_points.x, poi[i].z_points.y, c_points.x, c_points.y, R);
		poi[i].l_dis = pointDis(poi[i].worldPoints.x, poi[i].worldPoints.y, poi[cen_flag].worldPoints.x, poi[cen_flag].worldPoints.y);
	}

	for (int i = 0; i < 42; i++) {
		if (i != cen_flag) {
			poi[i].r_dis = poi[i].l_dis * R1;
			poi[i].pr_s = poi[i].r_dis / poi[i].p_dis;
		}
	}

	for (int i = 0; i < 42; i++) {
		//cout << poi[i].worldPoints << endl;
		cout << poi[i].z_points << endl;
		/*cout << poi[i].p_dis << endl;
		cout << poi[i].l_dis << endl;
		cout << poi[i].r_dis << endl;
		cout << poi[i].pr_s << endl;*/
	}
	
	for (int i = 0; i < 42; i++) {
		xa[i] = poi[i].p_dis;
		ya[i] = poi[i].pr_s;
	}

	sort(xa, xa + 42);
	sort(ya, ya + 42);

	for (int i = 0; i < 42; i++) {
		cout << xa[i] << endl;
		cout << ya[i] << endl;
	}
	xa[0] = 0;
	ya[0] = 0.98;

	return 1;
}

bool GetMatrixInverse(double src[3][3], int n, double des[3][3]) {
	double flag = getA(src, n);
	double t[3][3];
	if (flag == 0)
	{
		return false;
	}
	else
	{
		getAStart(src, n, t);
		for (int i = 0; i < n; i++)
		{
			for (int j = 0; j < n; j++)
			{
				des[i][j] = t[i][j] / flag;
			}

		}
	}

	return true;
}

double getA(double arcs[3][3], int n) {
	if (n == 1)
	{
		return arcs[0][0];
	}
	double ans = 0;
	double temp[3][3] = { 0.0 };
	int i, j, k;

	for (i = 0; i < n; i++)
	{
		for (j = 0; j < n - 1; j++)
		{
			for (k = 0; k < n - 1; k++)
			{
				temp[j][k] = arcs[j + 1][(k >= i) ? k + 1 : k];

			}
		}
		double t = getA(temp, n - 1);
		if (i % 2 == 0)
		{
			ans += arcs[0][i] * t;
		}
		else
		{
			ans -= arcs[0][i] * t;
		}
	}
	return ans;
}

void  getAStart(double arcs[3][3], int n, double ans[3][3])
{
	if (n == 1)
	{
		ans[0][0] = 1;
		return;
	}
	int i, j, k, t;
	double temp[3][3];
	for (i = 0; i < n; i++)
	{
		for (j = 0; j < n; j++)
		{
			for (k = 0; k < n - 1; k++)
			{
				for (t = 0; t < n - 1; t++)
				{
					temp[k][t] = arcs[k >= i ? k + 1 : k][t >= j ? t + 1 : t];
				}
			}


			ans[j][i] = getA(temp, n - 1);
			if ((i + j) % 2 == 1)
			{
				ans[j][i] = -ans[j][i];
			}
		}
	}
}

bool matrix_multiply(double des[3][3], double matb[3], double mataa[3]) {
	for (int i = 0; i < 3; i++) {
		double xaa = 0.0;
		for (int j = 0; j < 3; j++) {
			xaa += des[i][j] * matb[j];
		}
		mataa[i] = xaa;
	}
	return true;
}

double CalcDistance(Point point1, Point point2, double mataa[3]) {
	cout << mataa[0] << endl;
	cout << mataa[1] << endl;
	cout << mataa[2] << endl;

	Point lenscenter(652, 470); //镜头中心在图像中的位置
	double r1 = sqrt((point1.y - lenscenter.y)*(point1.y - lenscenter.y) + (point1.x - lenscenter.x)*(point1.x - lenscenter.x));
	double s1 = mataa[0] + mataa[1] * r1 + mataa[2] * pow(r1, 2);
	double repoint1_x = (point1.x - lenscenter.x) / s1 + lenscenter.x;
	double repoint1_y = (point1.y - lenscenter.y) / s1 + lenscenter.y;

	double r2 = sqrt((point2.y - lenscenter.y)*(point2.y - lenscenter.y) + (point2.x - lenscenter.x)*(point2.x - lenscenter.x));
	double s2 = mataa[0] + mataa[1] * r2 + mataa[2] * pow(r2, 2);
	double repoint2_x = (point2.x - lenscenter.x) / s2 + lenscenter.x;
	double repoint2_y = (point2.y - lenscenter.y) / s2 + lenscenter.y;
	cout << repoint1_x << endl;
	cout << repoint1_y << endl;
	cout << repoint2_x << endl;
	cout << repoint2_y << endl;


	return 1;
}

Mat ToRight(double mataa[3]) {
	cout << mataa[0] << endl;
	cout << mataa[1] << endl;
	cout << mataa[2] << endl;

	char filename[50] = "D:\\code\\code\\0003\\hb1.bmp";//图像路径  
	Mat img = imread(filename);
	Point lenscenter(652, 470); //镜头中心在图像中的位置
	uchar *data = (uchar*)img.data; //原图像数
	uchar *newimgdata = new uchar[img.step*img.rows]; //新图像数据
	for (int row = 0; row < img.rows; row++) { //操作数据区,要注意OpenCV的RGB的存储顺序为GBR 
		for (int col = 0; col < img.cols; col++) { //示例为亮度调节 
			int pointsrc = row * img.step + col * 3;
			double r = sqrt((row - lenscenter.y)*(row - lenscenter.y) + (col - lenscenter.x)*(col - lenscenter.x));
			double s = mataa[0] + mataa[1] * r + mataa[2] * pow(r, 2);//比例		
			int x1 = (col - lenscenter.x)*s + lenscenter.x;
			int y1 = (row - lenscenter.y)*s + lenscenter.y;
			int x = (x1 + col) / 2;
			int y = (y1 + row) / 2;


			if (x >= 0 && x < img.cols && y >= 0 && y < img.rows)
			{
				int pointdrc = y * img.step + x * 3;
				newimgdata[pointdrc + 0] = data[pointsrc + 0];
				newimgdata[pointdrc + 1] = data[pointsrc + 1];
				newimgdata[pointdrc + 2] = data[pointsrc + 2];
			}
		}
	}
	memset(data, 0, img.step*img.rows);
	memcpy(data, newimgdata, img.step*img.rows);
	imshow("aaa", img);
	imwrite("D:\\code\\code\\0003\\xz.jpg", img);
	return img;

}

附python代码
	import numpy as np
import cv2
import glob

criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

objp = np.zeros((6*7,3), np.float32)
objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)

objpoints = [] 
imgpoints = []

images = glob.glob('xzbd1.jpg')
for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    # thresh,img1 = cv2.threshold(gray, 120, 255, cv2.THRESH_BINARY)
    # cv2.imshow('img',img)

    # Find the chess board corners
    ret, corners = cv2.findChessboardCorners(gray, (7,6),None)
    print(corners)
    if ret == True:
        objpoints.append(objp)

        corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
        imgpoints.append(corners2)

        img = cv2.drawChessboardCorners(img, (7,6), corners2,ret)
        cv2.imshow('img',img)

ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)

import matplotlib.pyplot as plt
import math
import numpy
import random

fig = plt.figure()
ax = fig.add_subplot(111)

order=2

# x = numpy.arange(-1,1,0.02)
# y = [((a*a-1)*(a*a-1)*(a*a-1)+0.5)*numpy.sin(a*2) for a in x]

# i=0
# xa=[143,260.5,341.5,395,430.5,454,469]
# ya=[1,1.0979,1.2562,1.4481,1.6609,1.8899,2.1343]   
xa = [98.0558,99.0446,99.0446,99.0458,138.702,140.04,140.756,140.79,195.122,197.062,198.114,199.062,217.98,219.227,219.277,220.989,221.017,221.41,221.926,221.945,274.59,277.267,277.982,
280.172,290.23,295.179,297.106,304.495,306.917,310.051,311.694,312.176,313.528,346.484,349.894,352.102,353.425,355.259,355.492,410.965,416.786]
ya = [0.994898,0.995124,0.995136,0.997869,0.997957,0.998983,0.999881,0.999894,0.999996,1.00001,1.00001,1.0001,1.00022,1.00028,1.00206,1.00219,1.00331,1.00456,1.00486,1.00522,1.00522,
1.00663,1.00777,1.00823,1.00987,1.01001,1.01009,1.01019,1.01024,1.01037,1.01044,1.01423,1.01522,1.01602,1.02022,1.0205,1.02063,1.02251,1.0238,1.02862,1.03068]
# for xx in x:  
#     yy=y[i]   
#     d=float(random.randint(60,140))/100   
#     i+=1  
#     xa.append(xx*d)   
#     ya.append(yy*d)
# ax.plot(xa,ya,color='m',linestyle='',marker='.')

matA=[]

for i in range(0,order+1):  
    matA1=[]    
    for j in range(0,order+1):      
        tx=0.0
        for k in range(0,len(xa)):
            dx=1.0
            for l in range(0,j+i):              
                dx=dx*xa[k]         
            tx+=dx      
        matA1.append(tx)    
    matA.append(matA1)
matA=numpy.array(matA)

matB=[]
for i in range(0,order+1):
    ty = 0.0        
    for k in range(0,len(xa)):
        dy=1.0
        for l in range(0,i):                
            dy=dy*xa[k]         
        ty+=ya[k]*dy    
    matB.append(ty) 
matB=numpy.array(matB)
print(matA)
print(matB)

matAA=numpy.linalg.solve(matA,matB)
# matAA = [0.963353, 0.000978985, -4.7488e-06]
print(matAA)
xxa= numpy.arange(0,500,1)
yya=[]
for i in range(0,len(xxa)): 
    yy=0.0  
    for j in range(0,order+1):      
        dy=1.0      
        for k in range(0,j):            
            dy*=xxa[i]      
        dy*=matAA[j]    
        yy+=dy  
    yya.append(yy)
ax.scatter(xa, ya, c='r', s=20, alpha=0.5)
# ax.plot(xxa,yya,color='g',linestyle='-',marker='') 
ax.legend()
plt.show()
标签
易学教程内所有资源均来自网络或用户发布的内容,如有违反法律规定的内容欢迎反馈
该文章没有解决你所遇到的问题?点击提问,说说你的问题,让更多的人一起探讨吧!