背景： 在图像处理邻域常常都会用到直线、曲线、平面、曲面的拟合任务，以方便基于拟合的直线、曲线、平面、以及曲面去扩展到各种图像处理任务之中。关于各种拟合，目前多使用基于最小二乘法的方式去实现，具体的工具库有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;
}
3558
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
