bool lineFit(const std::vector &points, double &a, double &b, double &c)
{
? ? int size = points.size();
? ? if(size < 2)
? ? {
? ? ? ? a = 0;
? ? ? ? b = 0;
? ? ? ? c = 0;
? ? ? ? return false;
? ? }
? ? double x_mean = 0;
? ? double y_mean = 0;
? ? for(int i = 0; i < size; i++)
? ? {
? ? ? ? x_mean += points[i].x;
? ? ? ? y_mean += points[i].y;
? ? }
? ? x_mean /= size;
? ? y_mean /= size; //至此赞咙，計算出了 x y 的均值? ? double Dxx = 0, Dxy = 0, Dyy = 0;
? ? for(int i = 0; i < size; i++)
? ? {
? ? ? ? Dxx += (points[i].x - x_mean) * (points[i].x - x_mean);
? ? ? ? Dxy += (points[i].x - x_mean) * (points[i].y - y_mean);
? ? ? ? Dyy += (points[i].y - y_mean) * (points[i].y - y_mean);
? ? }
? ? double lambda = ( (Dxx + Dyy) - sqrt( (Dxx - Dyy) * (Dxx - Dyy) + 4 * Dxy * Dxy) ) / 2.0;
? ? double den = sqrt( Dxy * Dxy + (lambda - Dxx) * (lambda - Dxx) );
? ? if(fabs(den) < 1e-5)
? ? {
? ? ? ? if( fabs(Dxx / Dyy - 1) < 1e-5) //這時沒有一個特殊的直線方向，無法擬合? ? ? ? {
? ? ? ? ? ? return false;
? ? ? ? }
? ? ? ? else? ? ? ? {
? ? ? ? ? ? a = 1;
? ? ? ? ? ? b = 0;
? ? ? ? ? ? c = - x_mean;
? ? ? ? }
? ? }
? ? else? ? {
? ? ? ? a = Dxy / den;
? ? ? ? b = (lambda - Dxx) / den;
? ? ? ? c = - a * x_mean - b * y_mean;
? ? }
? ? return true;
}
