利用点到直线的距离最小二乘法_最小二乘法拟合直线

void fitLine(const TColgp_Array1OfPnt2d&thePoints,const std::string&theFileName,

gp_Lin2d&theLine)

{

math_Vector aB(1, 2, 0.0);

math_Vector aX(1, 2, 0.0);

math_Matrix aM(1, 2, 1, 2);

Standard_Real aSxi= 0.0;

Standard_Real aSyi= 0.0;

Standard_Real aSxx= 0.0;

Standard_Real aSxy= 0.0;

std::ofstream aDrawFile(theFileName);for (Standard_Integer i = thePoints.Lower(); i <= thePoints.Upper(); ++i)

{const gp_Pnt2d& aPoint =thePoints.Value(i);

aSxi+=aPoint.X();

aSyi+=aPoint.Y();

aSxx+= aPoint.X() *aPoint.X();

aSxy+= aPoint.X() *aPoint.Y();

aDrawFile<< "vpoint p" << i << " " <

}

aM(1, 1) =thePoints.Size();

aM(1, 2) =aSxi;

aM(2, 1) =aSxi;

aM(2, 2) =aSxx;

aB(1) =aSyi;

aB(2) =aSxy;

OSD_Chronometer aChronometer;

aChronometer.Start();

math_Gauss aSolver(aM);//math_GaussLeastSquare aSolver(aM);//math_SVD aSolver(aM);

aSolver.Solve(aB, aX);if(aSolver.IsDone())

{

Standard_Real aA= aX(1);

Standard_Real aB= aX(2);

gp_Pnt2d aP1(0.0, aA);

gp_Pnt2d aP2(-aA/aB, 0.0);

theLine.SetLocation(aP1);

theLine.SetDirection(gp_Vec2d(aP1, aP2).XY());

aDrawFile<< "vaxis l"

<< aP1.X() << " " << aP1.Y() << "0"

<< aP2.X() << " " << aP2.Y() << "0" <<:endl>

std::cout<< "===================" <<:endl>

aX.Dump(std::cout);

}

aChronometer.Stop();

aChronometer.Show();

}intmain()

{

gp_Lin2d aLine;//Test data 1

TColgp_Array1OfPnt2d aPoints1(1, 6);

aPoints1.SetValue(1, gp_Pnt2d(36.9, 181.0));

aPoints1.SetValue(2, gp_Pnt2d(46.7, 197.0));

aPoints1.SetValue(3, gp_Pnt2d(63.7, 235.0));

aPoints1.SetValue(4, gp_Pnt2d(77.8, 270.0));

aPoints1.SetValue(5, gp_Pnt2d(84.0, 283.0));

aPoints1.SetValue(6, gp_Pnt2d(87.5, 292.0));

fitLine(aPoints1,"fit1.tcl", aLine);//Test data 2

TColgp_Array1OfPnt2d aPoints2(0, 7);

aPoints2.SetValue(0, gp_Pnt2d(0.0, 27.0));

aPoints2.SetValue(1, gp_Pnt2d(1.0, 26.8));

aPoints2.SetValue(2, gp_Pnt2d(2.0, 26.5));

aPoints2.SetValue(3, gp_Pnt2d(3.0, 26.3));

aPoints2.SetValue(4, gp_Pnt2d(4.0, 26.1));

aPoints2.SetValue(5, gp_Pnt2d(5.0, 25.7));

aPoints2.SetValue(6, gp_Pnt2d(6.0, 25.3));

aPoints2.SetValue(7, gp_Pnt2d(7.0, 24.8));

fitLine(aPoints2,"fit2.tcl", aLine);return 0;

}