Find Closest Line Between Points In 2D By Least Squares

There are a few points (x1, y1), (x2, y2), (x3, y3) … We want to find the line which is closest from these points. The line is described as .

The problem can be convert to how to make the value of smallest.
We get the new function which has variables b and m.

If the derivatives of to b and m, respectively become zero, the minimum value will occur.

=>

=>

So we get the following two equations.

=>

=>

we have:

Let’s create a few random points and use algorithm library CGAL and the visualization toolkit (VTK) to find and draw the closest line by a demo.

main.cpp

#include <CGAL/Linear_algebraCd.h>
#include <CGAL/Linear_algebraHd.h>
#include <iostream>
#include <vtkPolyData.h>
#include <vtkProperty.h>
#include <vtkPolyDataMapper.h>
#include <vtkActor.h>
#include <vtkPlane.h>
#include <vtkRenderer.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkPoints.h>

#include "point.hpp"

using namespace std;
using namespace CGAL::Linear_Algebra;

typedef CGAL::Linear_algebraCd<double> LA;
typedef LA::Matrix Matrix;
typedef LA::Vector Vector;

#define vtkSPtr vtkSmartPointer
#define vtkSPtrNew(Var, Type) vtkSPtr<Type> Var = vtkSPtr<Type>::New();

template <typename Type>
Matrix BuildMatrix( int rowCount, int colCount, std::vector<Type> values )
{
    std::vector<Vector> elements;
    for( int col = 0; col < colCount; ++col )
    {
        std::vector<Type> tmp;
        for( int row = 0; row < rowCount; ++row )
        {
            tmp.push_back( values[row*colCount + col] );
        }
        elements.push_back( Vector( tmp.begin(), tmp.end() ) );
    }

    Matrix M( elements.begin(), elements.end() );
    return M;
}

int main()
{
    CGAL::set_mode( cout, CGAL::IO::PRETTY );

    std::vector<Point> samplePts{{4, 17, 0 },
                                 {5, 15, 0 },
                                 {9.5, 13.5, 0},
                                 {16.5, 7.9, 0},
                                 {21, 5,0 } };
    // build matrix M
    std::vector<double> values;
    for( int i = 0; i < samplePts.size(); i++ ){
        values.push_back( samplePts[i][0] );
        values.push_back( 1 );
    }
    Matrix M = BuildMatrix( samplePts.size(), 2, values );
    cout << M << endl;

    values.clear();
    for( int i = 0; i < samplePts.size(); ++i ){
        values.push_back( samplePts[i][1] );
    }
    Matrix Y = BuildMatrix( samplePts.size(), 1, values );
    double det;
    Matrix tmp1 = LA::inverse( LA::transpose( M ) * M, det );

    Matrix result = tmp1 * ( LA::transpose( M ) * Y );
    cout << "result: " << result << endl;

    // =================== draw line ==================
    double m = result[0][0];
    double b = result[1][0];
    Point p0( 0, 0*m+b, 0 );
    Point p1( 25, 25*m+b, 0 );
    vtkSPtrNew( lineData, vtkPolyData );
    vtkSPtrNew( linePts, vtkPoints );
    vtkSPtrNew( lineCells, vtkCellArray );
    linePts->InsertNextPoint( p0.point );
    linePts->InsertNextPoint( p1.point );
    vtkIdType lineIds[2] = { 0, 1 };
    lineCells->InsertNextCell( 2, lineIds );
    lineData->SetPoints( linePts );
    lineData->SetLines( lineCells );
    lineData->Modified();
    vtkSPtrNew( lineMapper, vtkPolyDataMapper );
    lineMapper->SetInputData( lineData );

    vtkSPtrNew( lineActor, vtkActor );
    lineActor->SetMapper( lineMapper );
    lineActor->GetProperty()->SetColor( 1, 0, 0 );
    lineActor->GetProperty()->SetLineWidth( 2 );

    // =============== draw points =============
    vtkSPtrNew( polyData, vtkPolyData );
    vtkSPtrNew( points, vtkPoints );
    vtkSPtrNew( cells, vtkCellArray );
    for( int i = 0; i < samplePts.size(); ++i )
    {
        points->InsertNextPoint( samplePts[i].point );
        vtkIdType ids[] = { i };
        cells->InsertNextCell( 1, ids );
    }
    polyData->SetPoints( points );
    polyData->SetVerts( cells );
    polyData->Modified();

    vtkSPtrNew( mapper, vtkPolyDataMapper );
    mapper->SetInputData( polyData );

    vtkSPtrNew( actor, vtkActor );
    actor->SetMapper( mapper );
    actor->GetProperty()->SetPointSize( 5 );

    vtkSPtrNew( renderer, vtkRenderer );
    renderer->AddActor( actor );
    renderer->AddActor( lineActor );
    renderer->SetBackground( 0, 0, 0 );

    vtkSPtrNew( renderWindow, vtkRenderWindow );
    renderWindow->AddRenderer( renderer );

    vtkSPtrNew( renderWindowInteractor, vtkRenderWindowInteractor );
    renderWindowInteractor->SetRenderWindow( renderWindow );

    renderer->ResetCamera();
    renderWindow->Render();
    renderWindowInteractor->Start();

    return 0;
}

The header file point.hpp contains the implementations of class Point. You can find it in Project Point On Line And Plane By Special Direction.

Result:

I have to put the content of CMakeLists.txt to the next page due to a few Latex render errors. The file configures our project to import CGAL and VTK easily.