We had found the closest line between points in the 2D world by least squares, link: Find Closest Line Between Points In 2D By Least Squares
I will show how to find the closest plane closest to a few random points in the 3D environment.
We know that the sum of square distances from point to plane can be calculated.

    \[\sum\limits_{i=1}^{n}D_i^2=\sum\limits_{i=1}^n\frac{(A(x_i-x_0)+B(y_i-y_0)+C(z_i-z_0))^2}{A^2+B^2+C^2}\]

Similar to our last example, Find Closest Line Between Points In 2D By Least Squares, If the derivatives of D^2 to variables, respectively become zero, the minimum value will occur.

We have:
M=A^2+B^2+C^2
N_0=A(x_i-x_0)+B(y_i-y_0)+C(z_i-z_0)
N=N_0^2

We know:

    \[f(x) = \frac{N(x)}{M(x)}\]

    \[f^{'}(x) = \frac{N^{'}(x)M(x) - M^{'}(x)N(x)}{M^2(x)}\]

So we get:

    \[\frac{\partial f}{\partial A}=\frac{2N_0(x_i-x_0)M - 2AN}{M^2}\]

    \[\frac{\partial f}{\partial B}=\frac{2N_0(y_i-y_0)M - 2BN}{M^2}\]

    \[\frac{\partial f}{\partial B}=\frac{2N_0(z_i-z_0)M - 2CN}{M^2}\]

    \[\frac{\partial f}{\partial x_0}=\frac{2N_0(-A)M}{M^2}=\frac{2N_0(-A)}{M}\]

    \[\frac{\partial f}{\partial y_0}=\frac{2N_0(-B)M}{M^2}=\frac{2N_0(-B)}{M}\]

    \[\frac{\partial f}{\partial z_0}=\frac{2N_0(-C)M}{M^2}=\frac{2N_0(-C)}{M}\]

we can’t find a simple linear matrix to represent (A, B, C, x_0, y_0, z_0). So it’s impossible to get results by matrix directly. Levenberg-Marquardt is a popular alternative to find the minimum sum of squares of nonlinear functions. It improves the Gauss-Newton method for the nonlinear-least squares optimization problem.
Find the implementation of the algorithm in GitHub, levmar-2.6.
The following demo show how to use it and draw points and the closest plane by VTK.

cmake_minimum_required(VERSION 3.16)
project(explore3D)

set(CMAKE_CXX_STANDARD 14)

find_package( VTK REQUIRED )
include( ${VTK_USE_FILE} )

set( LEVMAR_PATH /Users/weiyang/Downloads/levmar-2.6 )
include_directories( ${LEVMAR_PATH} )

set( header_files
        "../point.hpp" )
set( src_files
        ${LEVMAR_PATH}/Axb.c
        ${LEVMAR_PATH}/lm.c
        ${LEVMAR_PATH}/lmbc.c
        ${LEVMAR_PATH}/lmblec.c
        ${LEVMAR_PATH}/lmbleic.c
        ${LEVMAR_PATH}/lmlec.c
        ${LEVMAR_PATH}/misc.c
         )

add_executable(explore3D main.cpp ${header_files} ${src_files} )

target_link_libraries( ${PROJECT_NAME} ${VTK_LIBRARIES} )

main.cpp:

#include <iostream>
#include <levmar.h>
#include <vector>

#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"
#include <vtkPlane.h>
#include <vtkPlaneSource.h>
#include <vtkTransform.h>

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

void Dis2FromPtToPlane( double *planeParas, double *dis2, int m, int n, void *data );
void DerivativeOfDis2(double *planeParas, double *derivative, int m, int n, void *data);

int main() {
    std::vector<Point> samplePts{ {-25.1339, -5.76816, -0.995864},
                                  {-14.7021, -2.73063, 18.1714},
                                  {-0.458432, -1.09882, 24.679},
                                  {12.8826, -1.73559, 17.6288},
                                  {23.2548, -4.19458, -1.25975},
                                  {23.2548, -2.19458, 6.25975} };
    double data[samplePts.size()*3];
    Point center( 0, 0,0  );
    for( int i = 0; i < samplePts.size(); ++i )
    {
        data[i*3] = samplePts[i][0];
        data[i*3+1] = samplePts[i][1];
        data[i*3+2] = samplePts[i][2];
        center = center + samplePts[i];
    }
    center /= samplePts.size();
    double paras[6] = { 1, 1, 1, center[0], center[1], center[2] };
    double *target = new double[ samplePts.size() ];
    for( int i = 0; i < samplePts.size(); ++i )
    {
        target[i] = 0;
    }
    // m: parameters count; n: points count
    double iterateMax = 1000;
    double opts[LM_OPTS_SZ], info[LM_INFO_SZ];
    opts[0] = LM_INIT_MU;
    opts[1] = opts[2] = 1E-15;
    opts[3] = 1E-20;
    opts[4]= LM_DIFF_DELTA;
    dlevmar_der( Dis2FromPtToPlane, DerivativeOfDis2, paras, target, 6, samplePts.size(), iterateMax,
            opts, info, nullptr, nullptr, data );
    delete [] target;

    // =============== draw plane ==============
    vtkSPtrNew( plane, vtkPlane );
    plane->SetNormal( paras[0], paras[1], paras[2] );
    plane->SetOrigin( paras[3], paras[4], paras[5] );
    vtkSmartPointer<vtkPlaneSource> planeSource =
            vtkSmartPointer<vtkPlaneSource>::New();
    planeSource->SetCenter( plane->GetOrigin() );
    planeSource->SetNormal( plane->GetNormal() );
    planeSource->Update();

    vtkSmartPointer<vtkPolyDataMapper> planeMapper =
            vtkSmartPointer<vtkPolyDataMapper>::New();
    planeMapper->SetInputData( planeSource->GetOutput() );
    planeMapper->Update();

    int scaleValue = 60;
    vtkSmartPointer<vtkActor> planeActor =
            vtkSmartPointer<vtkActor>::New();
    planeActor->SetMapper( planeMapper );
    planeActor->GetProperty()->SetColor( 0, 1, 1 );
    planeActor->GetProperty()->SetOpacity( 0.5 );

    Point originPt( plane->GetOrigin() );
    vtkSPtrNew( trans, vtkTransform );
    trans->Translate( originPt.point );
    trans->Scale( scaleValue, scaleValue, scaleValue );
    trans->Translate( (-originPt).point );
    planeActor->SetUserTransform( trans );

    // =============== 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( planeActor );
    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;
}

void Dis2FromPtToPlane( double *planeParas, double *dis2, int m, int n, void *data )
{
    double *inData = (double *)data;
    double M = planeParas[0]*planeParas[0] + planeParas[1]*planeParas[1] + planeParas[2]*planeParas[2];
    for( int i = 0; i < n; ++i )
    {
        double N0 = (inData[i*3] - planeParas[3])*planeParas[0] +
                    (inData[i*3+1] - planeParas[4])*planeParas[1] +
                    (inData[i*3+2] - planeParas[5])*planeParas[2];
        double N = N0*N0;
        dis2[i] = N/M;
    }
}

void DerivativeOfDis2(double *planeParas, double *derivative, int m, int n, void *data)
{
    double *inData = (double *)data;
    double M = planeParas[0]*planeParas[0] + planeParas[1]*planeParas[1] + planeParas[2]*planeParas[2];
    for( int i = 0; i < n; ++i ) {
        double N0 = (inData[i * 3] - planeParas[3]) * planeParas[0] +
                   (inData[i * 3 + 1] - planeParas[4]) * planeParas[1] +
                   (inData[i * 3 + 2] - planeParas[5]) * planeParas[2];
        double N = N0*N0;

        //dA
        derivative[i*6] = 2*N0*(inData[i*3] - planeParas[3])*M - 2*planeParas[0]*N;
        derivative[i*6] /= (M*M);
        //dB
        derivative[i*6+1] = 2*N0*(inData[i*3+1] - planeParas[4])*M - 2*planeParas[1]*N;
        derivative[i*6+1] /= (M*M);
        //dC
        derivative[i*6+2] = 2*N0*(inData[i*3+2] - planeParas[5])*M - 2*planeParas[2]*N;
        derivative[i*6+2] /= (M*M);
        //dx0
        derivative[i*6+3] = 2*N0*(-planeParas[0]) / M;
        //dy0
        derivative[i*6+4] = 2*N0*(-planeParas[1]) / M;
        //dz0
        derivative[i*6+5] = 2*N0*(-planeParas[2]) / M;
    }
}

Result:


0 0 votes
Article Rating
Subscribe
Notify of
guest
0 Comments
Inline Feedbacks
View all comments

3D Files Converter: Support to write X3D file and VRML file.

X
A prohibited operation
0
Would love your thoughts, please comment.x
()
x