jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
algoLib/sourceCode/dataFitting.cpp

637 lines
16 KiB
C++
Raw Normal View History

将拟合类函数独立成一个模块
2026-02-08 22:47:35 +08:00
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include <vector>
#ifdef __WIN32
#include <corecrt_math_defines.h>
#endif // __WIN32
#include <cmath>
#include <unordered_map>
#include <Eigen/dense>
void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b)
{
//<2F><>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>߲<EFBFBD><DFB2><EFBFBD>
double xx_sum = 0;
double x_sum = 0;
double y_sum = 0;
double xy_sum = 0;
int num = 0;
for (int i = 0; i < inliers.size(); i++)
{
x_sum += inliers[i].x; //x<><78><EFBFBD>ۼӺ<DBBC>
y_sum += inliers[i].y; //y<><79><EFBFBD>ۼӺ<DBBC>
xx_sum += inliers[i].x * inliers[i].x; //x<><78>ƽ<EFBFBD><C6BD><EFBFBD>ۼӺ<DBBC>
xy_sum += inliers[i].x * inliers[i].y; //x<><78>y<EFBFBD><79><EFBFBD>ۼӺ<DBBC>
num++;
}
*_k = (num * xy_sum - x_sum * y_sum) / (num * xx_sum - x_sum * x_sum); //<2F><><EFBFBD>ݹ<EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD>k
*_b = (-x_sum * xy_sum + xx_sum * y_sum) / (num * xx_sum - x_sum * x_sum);//<2F><><EFBFBD>ݹ<EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD>b
}
//<2F><><EFBFBD>ϳ<EFBFBD>ͨ<EFBFBD><CDA8>ֱ<EFBFBD>߷<EFBFBD><DFB7><EFBFBD>ax+by+c=0<><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ
void lineFitting_abc(std::vector< SVzNL3DPoint>& inliers, double* _a, double* _b, double* _c)
{
//<2F>ж<EFBFBD><D0B6>Ƿ<EFBFBD>Ϊ<EFBFBD><CEAA>ֱ
int dataSize = (int)inliers.size();
if (dataSize < 2)
return;
double deltaX = abs(inliers[0].x - inliers[dataSize - 1].x);
double deltaY = abs(inliers[0].y - inliers[dataSize - 1].y);
std::vector< SVzNL3DPoint> fittingData;
if (deltaX < deltaY)
{
//x=ky+b <20><><EFBFBD><EFBFBD>
for (int i = 0; i < dataSize; i++)
{
SVzNL3DPoint a_fitPt;
a_fitPt.x = inliers[i].y;
a_fitPt.y = inliers[i].x;
a_fitPt.z = inliers[i].z;
fittingData.push_back(a_fitPt);
}
double k = 0, b = 0;
lineFitting(fittingData, &k, &b);
//ax+by+c
*_a = 1.0;
*_b = -k;
*_c = -b;
}
else
{
//y = kx+b<><62><EFBFBD><EFBFBD>
double k = 0, b = 0;
lineFitting(inliers, &k, &b);
//ax+by+c
*_a = k;
*_b = -1;
*_c = b;
}
return;
}
//Բ<><D4B2>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double fitCircleByLeastSquare(
const std::vector<SVzNL3DPoint>& pointArray,
SVzNL3DPoint& center,
double& radius)
{
int N = pointArray.size();
if (N < 3) {
return std::numeric_limits<double>::max();
}
double sumX = 0.0;
double sumY = 0.0;
double sumX2 = 0.0;
double sumY2 = 0.0;
double sumX3 = 0.0;
double sumY3 = 0.0;
double sumXY = 0.0;
double sumXY2 = 0.0;
double sumX2Y = 0.0;
for (int pId = 0; pId < N; ++pId) {
sumX += pointArray[pId].x;
sumY += pointArray[pId].y;
double x2 = pointArray[pId].x * pointArray[pId].x;
double y2 = pointArray[pId].y * pointArray[pId].y;
sumX2 += x2;
sumY2 += y2;
sumX3 += x2 * pointArray[pId].x;
sumY3 += y2 * pointArray[pId].y;
sumXY += pointArray[pId].x * pointArray[pId].y;
sumXY2 += pointArray[pId].x * y2;
sumX2Y += x2 * pointArray[pId].y;
}
double C, D, E, G, H;
double a, b, c;
C = N * sumX2 - sumX * sumX;
D = N * sumXY - sumX * sumY;
E = N * sumX3 + N * sumXY2 - (sumX2 + sumY2) * sumX;
G = N * sumY2 - sumY * sumY;
H = N * sumX2Y + N * sumY3 - (sumX2 + sumY2) * sumY;
a = (H * D - E * G) / (C * G - D * D);
b = (H * C - E * D) / (D * D - G * C);
c = -(a * sumX + b * sumY + sumX2 + sumY2) / N;
center.x = -a / 2.0;
center.y = -b / 2.0;
radius = sqrt(a * a + b * b - 4 * c) / 2.0;
double err = 0.0;
double e;
double r2 = radius * radius;
for (int pId = 0; pId < N; ++pId) {
e = pow(pointArray[pId].x - center.x, 2) + pow(pointArray[pId].y - center.y, 2) - r2;
if (e > err) {
err = e;
}
}
return err;
}
#if 0
bool leastSquareParabolaFit(const std::vector<cv::Point2d>& points,
double& a, double& b, double& c,
double& mse, double& max_err)
{
// У<><D0A3><EFBFBD>㼯<EFBFBD><E3BCAF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ߣ<EFBFBD>
int n = points.size();
if (n < 3) {
return false;
}
// <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ͳ<EFBFBD><CDB2><EFBFBD>
double sum_x = 0.0, sum_x2 = 0.0, sum_x3 = 0.0, sum_x4 = 0.0;
double sum_y = 0.0, sum_xy = 0.0, sum_x2y = 0.0;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
for (const auto& p : points) {
double x = p.x;
double y = p.y;
double x2 = x * x;
double x3 = x2 * x;
double x4 = x3 * x;
sum_x += x;
sum_x2 += x2;
sum_x3 += x3;
sum_x4 += x4;
sum_y += y;
sum_xy += x * y;
sum_x2y += x2 * y;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Է<EFBFBD><D4B7><EFBFBD><EFBFBD><EFBFBD> M * [a,b,c]^T = N
// M<><4D><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3x3
double M[3][3] = {
{sum_x4, sum_x3, sum_x2},
{sum_x3, sum_x2, sum_x},
{sum_x2, sum_x, (double)n}
};
// N<><4E><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3x1
double N[3] = { sum_x2y, sum_xy, sum_y };
// <20><>˹<EFBFBD><CBB9>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Է<EFBFBD><D4B7><EFBFBD><EFBFBD>飨3Ԫһ<D4AA>η<EFBFBD><CEB7><EFBFBD><EFBFBD>飩
// <20><><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD>Mת<4D><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD>Ǿ<EFBFBD><C7BE><EFBFBD>
for (int i = 0; i < 3; i++) {
// ѡ<><D1A1>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ0<CEAA><30>
int pivot = i;
for (int j = i; j < 3; j++) {
if (fabs(M[j][i]) > fabs(M[pivot][i])) {
pivot = j;
}
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
std::swap(M[i], M[pivot]);
std::swap(N[i], N[pivot]);
// <20><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>
double div = M[i][i];
if (fabs(div) < 1e-10) {
return false;
}
for (int j = i; j < 3; j++) {
M[i][j] /= div;
}
N[i] /= div;
// <20><>ȥ<EFBFBD>·<EFBFBD><C2B7><EFBFBD>
for (int j = i + 1; j < 3; j++) {
double factor = M[j][i];
for (int k = i; k < 3; k++) {
M[j][k] -= factor * M[i][k];
}
N[j] -= factor * N[i];
}
}
// <20><><EFBFBD><EFBFBD>2<EFBFBD><32><EFBFBD>ش<EFBFBD><D8B4><EFBFBD><EFBFBD><EFBFBD>
c = N[2];
b = N[1] - M[1][2] * c;
a = N[0] - M[0][1] * b - M[0][2] * c;
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
mse = 0.0;
max_err = 0.0;
for (const auto& p : points) {
double y_fit = a * p.x * p.x + b * p.x + c;
double err = y_fit - p.y;
double err_abs = fabs(err);
mse += err * err;
if (err_abs > max_err) {
max_err = err_abs;
}
}
mse /= n; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
return true;
}
#endif
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> y=ax^2 + bx + c
bool leastSquareParabolaFitEigen(
const std::vector<cv::Point2d>& points,
double& a, double& b, double& c,
double& mse, double& max_err)
{
int n = points.size();
if (n < 3) {
return false;
}
// <20><><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>A<EFBFBD><41>Ŀ<EFBFBD><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>B
Eigen::MatrixXd A(n, 3);
Eigen::VectorXd B(n);
for (int i = 0; i < n; i++) {
double x = points[i].x;
A(i, 0) = x * x;
A(i, 1) = x;
A(i, 2) = 1.0;
B(i) = points[i].y;
}
// <20><>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⣺Ax = B<><42>ֱ<EFBFBD>ӵ<EFBFBD><D3B5><EFBFBD>Eigen<65><6E><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Eigen::Vector3d coeffs = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);
a = coeffs(0);
b = coeffs(1);
c = coeffs(2);
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
mse = 0.0;
max_err = 0.0;
for (const auto& p : points) {
double y_fit = a * p.x * p.x + b * p.x + c;
double err = y_fit - p.y;
double err_abs = fabs(err);
mse += err * err;
if (err_abs > max_err) {
max_err = err_abs;
}
}
mse /= n;
return true;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>: z = Ax + By + C
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
void vzCaculateLaserPlane(std::vector<cv::Point3f> Points3ds, std::vector<double>& res)
{
//<2F><>С<EFBFBD><D0A1><EFBFBD>˷<EFBFBD><CBB7><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>
//<2F><>ȡcv::Mat<61><74><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊx<CEAA>ᣬ<EFBFBD><E1A3AC><EFBFBD><EFBFBD>Ϊy<CEAA>ᣬ<EFBFBD><E1A3AC>cvPoint<6E><74><EFBFBD><EFBFBD><EFBFBD>෴
//ϵ<><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
cv::Mat A = cv::Mat::zeros(3, 3, CV_64FC1);
//
cv::Mat B = cv::Mat::zeros(3, 1, CV_64FC1);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
cv::Mat X = cv::Mat::zeros(3, 1, CV_64FC1);
double x2 = 0, xiyi = 0, xi = 0, yi = 0, zixi = 0, ziyi = 0, zi = 0, y2 = 0;
for (int i = 0; i < Points3ds.size(); i++)
{
x2 += (double)Points3ds[i].x * (double)Points3ds[i].x;
y2 += (double)Points3ds[i].y * (double)Points3ds[i].y;
xiyi += (double)Points3ds[i].x * (double)Points3ds[i].y;
xi += (double)Points3ds[i].x;
yi += (double)Points3ds[i].y;
zixi += (double)Points3ds[i].z * (double)Points3ds[i].x;
ziyi += (double)Points3ds[i].z * (double)Points3ds[i].y;
zi += (double)Points3ds[i].z;
}
A.at<double>(0, 0) = x2;
A.at<double>(1, 0) = xiyi;
A.at<double>(2, 0) = xi;
A.at<double>(0, 1) = xiyi;
A.at<double>(1, 1) = y2;
A.at<double>(2, 1) = yi;
A.at<double>(0, 2) = xi;
A.at<double>(1, 2) = yi;
A.at<double>(2, 2) = (double)((int)Points3ds.size());
B.at<double>(0, 0) = zixi;
B.at<double>(1, 0) = ziyi;
B.at<double>(2, 0) = zi;
//<2F><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>ϵ<EFBFBD><CFB5>
X = A.inv() * B;
//A
res.push_back(X.at<double>(0, 0));
//B
res.push_back(X.at<double>(1, 0));
//Z<><5A>ϵ<EFBFBD><CFB5>Ϊ-1
res.push_back(-1.0);
//C
res.push_back(X.at<double>(2, 0));
return;
}
/**
* @brief <EFBFBD>ռ<EFBFBD>ֱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @param points <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ά<EFBFBD>㼯<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><EFBFBD><EFBFBD>㣬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>壩
* @param center <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ߵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><EFBFBD><EFBFBD>׼<EFBFBD><EFBFBD>P0<EFBFBD><EFBFBD>
* @param direction <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>ߵķ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>v<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* @return <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD>ɹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>㼯<EFBFBD><EFBFBD>Ч<EFBFBD><EFBFBD><EFBFBD><EFBFBD>true<EFBFBD><EFBFBD>
*/
bool fitLine3DLeastSquares(const std::vector<SVzNL3DPoint>& points, SVzNL3DPoint& center, SVzNL3DPoint& direction)
{
// <20><><EFBFBD><EFBFBD><EFBFBD>㼯<EFBFBD><E3BCAF>Ч<EFBFBD><D0A7>
if (points.size() < 2) {
std::cerr << "Error: <20>㼯<EFBFBD><E3BCAF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD>2<EFBFBD><32>" << std::endl;
return false;
}
int n = points.size();
Eigen::MatrixXd A(n, 3); // <20>㼯<EFBFBD><E3BCAF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(x,y,z)
// 1. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD>center<65><72>
double cx = 0.0, cy = 0.0, cz = 0.0;
for (const auto& p : points) {
cx += p.x;
cy += p.y;
cz += p.z;
A.row(points.size() - n) << p.x, p.y, p.z; // <20><><EFBFBD><EFBFBD><EFBFBD>㼯<EFBFBD><E3BCAF><EFBFBD><EFBFBD>
n--;
}
cx /= points.size();
cy /= points.size();
cz /= points.size();
center = { cx, cy, cz };
// 2. <20><><EFBFBD><EFBFBD>ȥ<EFBFBD><C8A5><EFBFBD>Ļ<EFBFBD><C4BB><EFBFBD>Э<EFBFBD><D0AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3x3<78><33>
// <20>ؼ<EFBFBD><D8BC>޸<EFBFBD><DEB8><EFBFBD>ʹ<EFBFBD><CAB9>RowVector3d<33><64><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>rowwise<73><65><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƥ<EFBFBD><C6A5>ά<EFBFBD><CEAC>
Eigen::RowVector3d centroid_row(cx, cy, cz);
Eigen::MatrixXd centered = A.rowwise() - centroid_row; // ά<><CEAC>ƥ<EFBFBD>䣬<EFBFBD>ޱ<EFBFBD><DEB1><EFBFBD>
// Э<><D0AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㣨n-1Ϊ<31><CEAA>ƫ<EFBFBD><C6AB><EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҳ<EFBFBD><D2B2>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>n<EFBFBD><6E>
Eigen::Matrix3d cov = centered.transpose() * centered; // / (points.size() - 1);
// 3. <20><><EFBFBD><EFBFBD>ֵ<EFBFBD>ֽ⣺<D6BD><E2A3BA>Э<EFBFBD><D0AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(cov);
if (eigensolver.info() != Eigen::Success) {
std::cerr << "Error: <20><><EFBFBD><EFBFBD>ֵ<EFBFBD>ֽ<EFBFBD>ʧ<EFBFBD>ܣ<EFBFBD>" << std::endl;
return false;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>Ӧ<EFBFBD><D3A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>EigenĬ<6E>ϰ<EFBFBD><CFB0><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD>
Eigen::Vector3d dir = eigensolver.eigenvectors().col(2);
// <20><>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><D1A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD>׼<EFBFBD><D7BC><EFBFBD><EFBFBD>
dir.normalize();
direction = { dir(0), dir(1), dir(2) };
return true;
}
workpieceHolePositioning version 1.4.2 : 华航孔定位改进：（1）法向量计算改进（2）添加3x3平滑（3）修正了bug
2026-03-27 00:01:16 +08:00
// ============================== <20><><EFBFBD>ߺ<EFBFBD><DFBA><EFBFBD> ==============================
// <20>㵽ƽ<E3B5BD><C6BD><EFBFBD><EFBFBD><EFBFBD>루<EFBFBD><EBA3A8><EFBFBD><EFBFBD><EFBFBD>ţ<EFBFBD>
double pointToPlaneSignedDist(const cv::Point3f& p, const Plane& plane) {
return (plane.A * p.x + plane.B * p.y + plane.C * p.z + plane.D);
}
// <20>㵽ƽ<E3B5BD><C6BD><EFBFBD>ľ<EFBFBD><C4BE>루<EFBFBD><EBA3A8><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>
float pointToPlaneDistance(const cv::Point3f& p, const Plane& plane) {
return fabsf(plane.A * p.x + plane.B * p.y + plane.C * p.z + plane.D)
/ sqrtf(plane.A * plane.A + plane.B * plane.B + plane.C * plane.C);
}
// <20><>һ<EFBFBD><D2BB>ƽ<EFBFBD>棨<EFBFBD><E6A3A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3>=1<><31>
void normalizePlane(Plane& plane) {
double norm = sqrt(plane.A * plane.A + plane.B * plane.B + plane.C * plane.C);
if (norm < 1e-6) return;
plane.A /= norm;
plane.B /= norm;
plane.C /= norm;
plane.D /= norm;
}
// ============================== ³<><C2B3><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7><EFBFBD><EFBFBD> ==============================
// Huber Ȩ<><C8A8>
double huberWeight(double r, double delta) {
r = fabs(r);
if (r <= delta) return 1.0;
else return delta / r;
}
// Tukey Ȩ<>أ<EFBFBD><D8A3><EFBFBD>Ⱥ<EFBFBD><C8BA>=0<><30><EFBFBD><EFBFBD><EFBFBD>ʺϰ<CABA><CFB0><EFBFBD>/ǿ<><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double tukeyWeight(double r, double c) {
r = fabs(r);
if (r > c) return 0.0;
double t = 1.0 - (r * r) / (c * c);
return t * t;
}
// ============================== ³<><C2B3><EFBFBD><EFBFBD>Ȩ<EFBFBD><C8A8>С<EFBFBD><D0A1><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ==============================
Plane robustFitPlane(
const std::vector< cv::Point3f>& points,
RobustType type,
double delta, // <20><>ֵ<EFBFBD><D6B5>><3E><>ֵ<EFBFBD><D6B5>Ϊ<EFBFBD><CEAA>Ⱥ<EFBFBD>㣨mm<6D><6D>
int maxIter, // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double convergeThresh // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>ƽ<EFBFBD><C6BD><EFBFBD>仯<EFBFBD>㹻С<E3B9BB><D0A1>ͣ<EFBFBD><CDA3>
)
{
int n = points.size();
if (n < 3) return Plane();
// 1. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8>С<EFBFBD><D0A1><EFBFBD>˳<EFBFBD>ʼ<EFBFBD><CABC>ƽ<EFBFBD><C6BD>
double cx = 0, cy = 0, cz = 0;
for (auto& p : points) { cx += p.x; cy += p.y; cz += p.z; }
cx /= n; cy /= n; cz /= n;
double xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;
for (auto& p : points) {
double dx = p.x - cx;
double dy = p.y - cy;
double dz = p.z - cz;
xx += dx * dx; xy += dx * dy; xz += dx * dz;
yy += dy * dy; yz += dy * dz; zz += dz * dz;
}
double detX = yy * zz - yz * yz;
double detY = xx * zz - xz * xz;
double detZ = xx * yy - xy * xy;
double maxDet = std::max({ detX, detY, detZ });
Plane plane;
if (maxDet == detX) {
plane.A = 1;
plane.B = (xy * yz - xz * yy) / detX;
plane.C = (xz * yz - xy * zz) / detX;
}
else if (maxDet == detY) {
plane.A = (xy * yz - xz * yy) / detY;
plane.B = 1;
plane.C = (xz * xy - xx * yz) / detY;
}
else {
plane.A = (xz * yz - xy * zz) / detZ;
plane.B = (yz * xy - xz * yy) / detZ;
plane.C = 1;
}
plane.D = -(plane.A * cx + plane.B * cy + plane.C * cz);
normalizePlane(plane);
// 2. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ȩ<EFBFBD><C8A8>С<EFBFBD><D0A1><EFBFBD><EFBFBD> (IRLS)
std::vector<double> weights(n, 1.0);
for (int iter = 0; iter < maxIter; iter++) {
Plane prevPlane = plane;
double sum_w = 0;
double swx = 0, swy = 0, swz = 0;
double swxx = 0, swxy = 0, swxz = 0, swyy = 0, swyz = 0;
for (int i = 0; i < n; i++) {
double r = pointToPlaneSignedDist(points[i], plane);
if (type == HUBER) weights[i] = huberWeight(r, delta);
else weights[i] = tukeyWeight(r, delta);
double w = weights[i];
sum_w += w;
swx += w * points[i].x;
swy += w * points[i].y;
swz += w * points[i].z;
}
double mx = swx / sum_w;
double my = swy / sum_w;
double mz = swz / sum_w;
for (int i = 0; i < n; i++) {
double w = weights[i];
double dx = points[i].x - mx;
double dy = points[i].y - my;
double dz = points[i].z - mz;
swxx += w * dx * dx;
swxy += w * dx * dy;
swxz += w * dx * dz;
swyy += w * dy * dy;
swyz += w * dy * dz;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double detXw = swyy * swyz - swyz * swyz;
double detYw = swxx * swyz - swxz * swxz;
double detZw = swxx * swyy - swxy * swxy;
double maxDw = std::max({ detXw, detYw, detZw });
if (maxDw == detXw) {
plane.A = 1;
plane.B = (swxy * swyz - swxz * swyy) / detXw;
plane.C = (swxz * swyz - swxy * swyz) / detXw;
}
else if (maxDw == detYw) {
plane.A = (swxy * swyz - swxz * swyy) / detYw;
plane.B = 1;
plane.C = (swxz * swxy - swxx * swyz) / detYw;
}
else {
plane.A = (swxz * swyz - swxy * swyz) / detZw;
plane.B = (swyz * swxy - swxz * swyy) / detZw;
plane.C = 1;
}
plane.D = -(plane.A * mx + plane.B * my + plane.C * mz);
normalizePlane(plane);
// ========== <20>ؼ<EFBFBD><D8BC><EFBFBD><EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD> ==========
double da = fabs(plane.A - prevPlane.A);
double db = fabs(plane.B - prevPlane.B);
double dc = fabs(plane.C - prevPlane.C);
double dd = fabs(plane.D - prevPlane.D);
double maxDiff = std::max({ da, db, dc, dd });
// ƽ<>漸<EFBFBD><E6BCB8><EFBFBD><EFBFBD><EFBFBD>ٱ仯 <20><> <20><>ǰ<EFBFBD><C7B0>ֹ
if (maxDiff < convergeThresh) {
break;
}
}
return plane;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>
Plane planeFrom3Points(const cv::Point3f& p1, const cv::Point3f& p2, const cv::Point3f& p3) {
float v1x = p2.x - p1.x;
float v1y = p2.y - p1.y;
float v1z = p2.z - p1.z;
float v2x = p3.x - p1.x;
float v2y = p3.y - p1.y;
float v2z = p3.z - p1.z;
float A = v1y * v2z - v1z * v2y;
float B = v1z * v2x - v1x * v2z;
float C = v1x * v2y - v1y * v2x;
float D = -(A * p1.x + B * p1.y + C * p1.z);
float norm = sqrtf(A * A + B * B + C * C);
if (norm > 1e-6) { A /= norm; B /= norm; C /= norm; D /= norm; }
return Plane(A, B, C, D);
}
// ==============================================
// <20><> <20><>ǰ<EFBFBD><C7B0>ֹ <20><> RANSAC ƽ<><C6BD><EFBFBD><EFBFBD><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD>ҵ<EFBFBD><D2B5>ʽ<EFBFBD>棩
// ==============================================
Plane ransacFitPlane(
const std::vector<cv::Point3f>& points,
std::vector<cv::Point3f>& out_inliers,
float dist_thresh, // <20>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
int max_iter, // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
int stop_no_improve // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٴ<EFBFBD><D9B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD>˳<EFBFBD>
)
{
out_inliers.clear();
int n = points.size();
if (n < 3) return Plane();
int best_inlier = 0;
Plane best_plane;
srand((unsigned)time(nullptr));
int no_improve_count = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
for (int i = 0; i < max_iter; ++i) {
// <20><><EFBFBD><EFBFBD>3<EFBFBD><33><EFBFBD><EFBFBD><EFBFBD>ظ<EFBFBD><D8B8><EFBFBD>
int idx[3];
idx[0] = rand() % n;
do { idx[1] = rand() % n; } while (idx[1] == idx[0]);
do { idx[2] = rand() % n; } while (idx[2] == idx[0] || idx[2] == idx[1]);
Plane plane = planeFrom3Points(
points[idx[0]],
points[idx[1]],
points[idx[2]]
);
// ͳ<><CDB3><EFBFBD>ڵ<EFBFBD>
int cnt = 0;
for (auto& p : points) {
if (pointToPlaneDistance(p, plane) < dist_thresh) cnt++;
}
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3>
if (cnt > best_inlier) {
best_inlier = cnt;
best_plane = plane;
no_improve_count = 0; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD>
}
else {
no_improve_count++;
}
// ====================== <20><>ǰ<EFBFBD><C7B0>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD> ======================
if (no_improve_count >= stop_no_improve) {
break;
}
}
// <20>ռ<EFBFBD><D5BC>ڵ<EFBFBD>
for (auto& p : points) {
if (pointToPlaneDistance(p, best_plane) < dist_thresh)
out_inliers.push_back(p);
}
return best_plane;
}
Reference in New Issue Copy Permalink