opencv 圆拟合ransc opencv拟合平面

转载

AIGC创想家 2024-06-28 00:22:36

文章标签 opencv 圆拟合ransc 平面计算机视觉视觉检测 opencv 文章分类 计算机视觉人工智能

曲平面拟合

一、基于engine实现的曲面拟合案例（曲面方程：z=ax**2+by**2+cxy+dx+ey+f）
二、基于opencv实现的平面拟合案例（加入了随机抽样一致性思维，剔除异常点）

背景： 在图像处理邻域常常都会用到直线、曲线、平面、曲面的拟合任务，以方便基于拟合的直线、曲线、平面、以及曲面去扩展到各种图像处理任务之中。关于各种拟合，目前多使用基于最小二乘法的方式去实现，具体的工具库有engine和opencv等，下面将介绍使用engine完成曲面拟合和opencv实现的平面拟合案例。

一、基于engine实现的曲面拟合案例（曲面方程：z=ax2+by2+cxy+dx+ey+f）

/*
@brief: 使用最小二乘法对一张图像的局部小面做抛物面拟合，并计算最高点对应的亚像素级坐标, 拟合为椭圆抛物面： ax**2 + by**2 + cx + dy +e -1*z = 0;
@param fitPointImg 用于拟合的点集(图像数据)
@param fitRoi 用于拟合的区域
@param realX 亚像素级x坐标
@param realY 亚像素级y坐标
@param realZ 亚灰度级z向灰度
@param subPixleLevel    亚像素纠正等级
*/
void calcSubPixel(cv::Mat fitPointImg, cv::Rect fitRoi, double * realX, double * realY, double * realZ)
{
	if (fitPointImg.channels() != 1) {
		std::cout << "error:  the channels of fitting surface image not equal 1 !" << std::endl;
		return;
	}

	if (fitRoi.width*fitRoi.height < 6) {
		std::cout << "error:  the point number is not enough to fit surface !" << std::endl;
		return;
	}
	cv::Mat fitImg = fitPointImg(fitRoi).clone();
	fitImg.convertTo(fitImg, CV_64FC1);
	cv::normalize(fitImg, fitImg, 255., 0., cv::NORM_MINMAX);

	Eigen::MatrixXd A(fitImg.rows*fitImg.cols, 6);
	Eigen::VectorXd B(fitImg.rows*fitImg.cols);
	Eigen::VectorXd X(6);

	for (int row = 0; row < fitImg.rows; row++) {
		for (int col = 0; col < fitImg.cols; col++) {
			A(row*fitImg.cols + col, 0) = pow(col, 2);
			A(row*fitImg.cols + col, 1) = pow(row, 2);
			A(row*fitImg.cols + col, 2) = col * row;
			A(row*fitImg.cols + col, 3) = col;
			A(row*fitImg.cols + col, 4) = row;
			A(row*fitImg.cols + col, 5) = 1;
			B(row*fitImg.cols + col) = fitImg.at<double>(row, col);
		}
	}

	X = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B).transpose();

	float a = X(0);
	float b = X(1);
	float c = X(2);
	float d = X(3);
	float e = X(4);
	float f = X(5);

	float x = (2 * b*d - c * e) / (pow(c, 2) - 4 * a*b);
	float y = (2 * a*e - c * d) / (pow(c, 2) - 4 * a*b);
	float z = a * x*x + b * y*y + c * x*y + d * x + e * y + f;  
	// 上面已经获取得到了曲面相关的参数

	// 用于获取使用模板匹配的最大值的在用于拟合的图像中的坐标位置
	cv::Point2i _maxLoc(0, 0);
	_maxLoc.x = *realX - fitRoi.x;
	_maxLoc.y = *realY - fitRoi.y;

	if (isnan(x) == 1 || isnan(y) == 1) {         // 不对匹配的最佳坐标进行亚像素微调
		return;
	}
	else
	{
		if (abs(x - _maxLoc.x) > 1 || abs(y - _maxLoc.y) > 1)      // 指定微调的方向和微调的步长，限制亚像素精度在1的范围之内
		{
			// 计算两个向量v0{_maxLoc.x, _maxLoc.y}的夹角,  计算v1{x, y}的夹角， 最终得到两个向量之间的夹角范围在[0, 2pi]之间。
			float _theTa_v0 = std::atan2f((float)_maxLoc.y, (float)_maxLoc.x);
			float _theTa_v1 = std::atan2f(y, x);
			float _angleV = _theTa_v1 - _theTa_v0;
			if (_angleV > 3.1415926) _angleV -= 2 * 3.1415926;
			if (_angleV < -3.1415926) _angleV += 2 * 3.1415926;
			const float len = 0.5;
			*realX = *realX + len * std::cosf(_angleV);
			*realY = *realY + len * std::sinf(_angleV);
			*realZ = z;
		}
		else                 //  计算的亚像素精度小于1，直接使用。
		{
			*realX = x + fitRoi.x;
			*realY = y + fitRoi.y;
			*realZ = z;
		}
	}
	return;
}

二、基于opencv实现的平面拟合案例（加入了随机抽样一致性思维，剔除异常点）

/*
@brief: 从给定的数据集中随机抽取3个不共线的点数据
*/
void randchoicePoint(std::vector<cv::Point3f>& pointSet, std::vector<cv::Point3f>& randPoints)
{
	std::random_device seed;//硬件生成随机数种子
	std::ranlux48 engine(seed());//利用种子生成随机数引擎
	std::uniform_int_distribution<> distrib(0, pointSet.size() - 1);//设置随机数范围，并为均匀分布
	int index1 = distrib(engine);//随机数
	int index2 = distrib(engine);//随机数
	int index3 = distrib(engine);//随机数

	while (true) {
		if (!PointOnLine(pointSet[index1], pointSet[index2], pointSet[index3])) {
			randPoints.emplace_back(pointSet[index1]);
			randPoints.emplace_back(pointSet[index2]);
			randPoints.emplace_back(pointSet[index3]);
			break;
		}
		index1 = distrib(engine); //随机数
		index2 = distrib(engine); //随机数
		index3 = distrib(engine); //随机数
	}
}

/*
@brief: 平面拟合
*/
void fitPlane(std::vector<cv::Point3f>& fitPoints, double * A, double * B, double * C, double * D)
{
	if (fitPoints.size() < 3) return;
	cv::Mat matA(fitPoints.size(), 3, CV_64FC1);
	cv::Mat matB(fitPoints.size(), 1, CV_64FC1);

	for (int i = 0; i < fitPoints.size(); i++) {
		matA.at<double>(i, 0) = fitPoints[i].x;
		matA.at<double>(i, 1) = fitPoints[i].y;
		matA.at<double>(i, 2) = 1;
		matB.at<double>(i, 0) = fitPoints[i].z;
	}
	cv::Mat matC;
	cv::solve(matA, matB, matC, cv::DecompTypes::DECOMP_SVD);
	*A = matC.at<double>(0, 0);
	*B = matC.at<double>(1, 0);
	*C = -1;
	*D = matC.at<double>(2, 0);
	return;
}

/*
@brief 平面平整度偏差测量，假设输入的H={x1,y1,z1,x2,y2,z2,x3,y3,z3}, 输出Dis={d1,d2,d3}.
@param[in] heightValues     每一次拍摄图像测量的深度值数据集。(3通道,机械坐标X/Y，和深度值), 长度等于measureNum*3
@param[in] measureNum          表示整张拍摄图片在行列方向上激光采集的次数之和
@param[in] iterNum              查找内点的迭代次数
@param[in] theresh              判定为内点的距离阈值
@param[out] Plane               [A, B, C, D] ,平面方程等于Ax + By + Cz +D = 0
@param[out] heightBias          测量点的绝对高度数组,长度等于measureNum
*/
// 下面函数结合RANSAC实现了精度更高的平面拟合功能
void measureSurfaceOffset(double *heightValues, int measureNum,int iterNum, float theresh, double* PlaneParam, double *heightBias){
 // step1: 将输入的坐标存储至矩阵中
	 std::vector<cv::Point3f> allPoints;
	 for (int index = 0; index < measureNum; index++)
	 {
		 cv::Point3f temPoint;
		 temPoint.x = (float)(*(heightValues + index * 3 + 0));
		 temPoint.y = (float)(*(heightValues + index * 3 + 1));
		 temPoint.z = (float)(*(heightValues + index * 3 + 2));
		 allPoints.emplace_back(temPoint);
	 }

	 std::vector<cv::Point3f> inPointIndex;    // 存储内点
	 double A=0, B=0, C=0, D=0;

	 while (iterNum > 0) {
		 std::vector<cv::Point3f> temInPointIndex;    // 存储内点

		 // step1:随机从给定的数据集中选取5个点
		 std::vector<cv::Point3f> randPoints;

		 randchoicePoint(allPoints, randPoints);
		 if (randPoints.size() < 1) {
			 std::cerr << "Api:measureSurfaceOffset - Function:randchoicePoint - discription:generate data is empty !" << std::endl;
			 return;
		 }

		 // 根据筛选的点进行平面拟合
		 fitPlane(randPoints, &A,& B, &C, &D);
		 if (isnan(A) || isnan(B) || isnan(C) || isnan(D)) {
			 std::cerr << "Api:measureSurfaceOffset - Function:fitPlane - discription:failed to fit plane at fist !" << std::endl;
			 return;
		 }

		 // 计算所有点到平面的距离，并根据内点的数量来筛选出表现最好的模型
		 for (int i = 0; i < allPoints.size(); i++) {
			 float dis = computPFdistance(A, B, C, D, allPoints[i]);
			 if (dis <= theresh) {
				 temInPointIndex.emplace_back(allPoints[i]);
			 }
		 }
		 if (temInPointIndex.size() > inPointIndex.size()) {
			 inPointIndex.swap(temInPointIndex);                       // swap的效率比assigin更快
		 }
		 iterNum--;
	 }
	 // 使用所有内点重新拟合, 并计算所有点到拟合平面的距离，作为返回值
	 fitPlane(inPointIndex, &A, &B, &C, &D);
	 PlaneParam[0] = A;
	 PlaneParam[1] = B;
	 PlaneParam[2] = C;
	 PlaneParam[3] = D;
	
	 if (isnan(A) || isnan(B) || isnan(C) || isnan(D)) {
		 std::cerr << "Api:measureSurfaceOffset - Function:fitPlane - discription:failed to fit plane at second !" << std::endl;
		 return;
	 }
	 for (int index = 0; index < allPoints.size(); index++) {
		 double dis = (double)computPFdistance(A, B, C, D, allPoints[index]);
		 *(heightBias + index) = dis;
	 }
	 return;
}

本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。