braddodson/pointmatcher.net

Implementation of ICP in C#

pointmatcher.net's Issues

how to initialize?

silly question from my side:
given:

DataPoints reading = ...; // initialize your point cloud reading here
DataPoints reference = ...; // initialize your reference point cloud here
EuclideanTransform initialTransform = ...; // your initial guess at the transform from reading to reference
ICP icp = new ICP();
icp.ReadingDataPointsFilters = new RandomSamplingDataPointsFilter(prob: 0.1f);
icp.ReferenceDataPointsFilters = new SamplingSurfaceNormalDataPointsFilter(SamplingMethod.RandomSampling, ratio: 0.2f);
icp.OutlierFilter = new TrimmedDistOutlierFilter(ratio: 0.5f);
var transform = icp.Compute(reading, reference, initialTransform);

how do i initialize the point cloud, initialize the reference point and the initialTransform? I have a camera in unity and want to run ICP every time a new frame shows up

Typo: DataPoints

In the DataPoints class:
public bool contiansNormals; // should be containsNormals

System.ArgumentException: 'Matrix must be positive definite.'

Hello Developers, i got this Exception when i Use this Code,
"System.ArgumentOutOfRangeException: 'Specified argument was out of the range of valid values.
Parameter name: The number of columns of a matrix must be positive.'"
Here is My Code, and thanks in Advance for your help :

       var readingPoints = new List<DataPoint>();
        var referencePoints = new List<DataPoint>();

        Vector3 dVec1 = new Vector3(-0.2967872f, 0.07480811f, 0.3788449f);
        Vector3 dVec2 = new Vector3(-0.2375435f, 0.07482694f, 0.3390627f);
        Vector3 dVec3 = new Vector3(-0.1699704f, 0.09031584f, 0.2926779f);
        Vector3 dVec4 = new Vector3(-0.08458908f, 0.09454262f, 0.3008334f);
        Vector3 dVec5 = new Vector3(-0.008116714f, 0.1069966f, 0.3251371f);
        Vector3 dVec6 = new Vector3(0.06125673f, 0.1056816f, 0.3781575f);
        Vector3 dVec7 = new Vector3(-0.122625f, 0.09119529f, 0.3357856f);

        DataPoint dpoint1 = new DataPoint { point = dVec1, normal = Vector3.Normalize(dVec1) };
        DataPoint dpoint2 = new DataPoint { point = dVec2, normal = Vector3.Normalize(dVec2) };
        DataPoint dpoint3 = new DataPoint { point = dVec3, normal = Vector3.Normalize(dVec3) };
        DataPoint dpoint4 = new DataPoint { point = dVec4, normal = Vector3.Normalize(dVec4) };
        DataPoint dpoint5 = new DataPoint { point = dVec5, normal = Vector3.Normalize(dVec5) };
        DataPoint dpoint6 = new DataPoint { point = dVec6, normal = Vector3.Normalize(dVec6) };
        DataPoint dpoint7 = new DataPoint { point = dVec7, normal = Vector3.Normalize(dVec7) };

        readingPoints.Add(dpoint1);
        readingPoints.Add(dpoint2);
        readingPoints.Add(dpoint3);
        readingPoints.Add(dpoint4);
        readingPoints.Add(dpoint5);
        readingPoints.Add(dpoint6);
        readingPoints.Add(dpoint7);

        var reading = new DataPoints
        {
            points = readingPoints.ToArray(),
            contiansNormals = true,
        };

        Vector3 rVec1 = new Vector3(-0.3207868f, 0.07550032f, 0.3456057f);
        Vector3 rVec2 = new Vector3(-0.2668087f, 0.08369328f, 0.2892195f);
        Vector3 rVec3 = new Vector3(-0.07828052f, 0.07665844f, 0.2688065f);
        Vector3 rVec4 = new Vector3(-0.07828052f, 0.07665844f, 0.2688065f);
        Vector3 rVec5 = new Vector3(-0.004118666f, 0.1029833f, 0.2820587f);
        Vector3 rVec6 = new Vector3(0.0516735f, 0.1072405f, 0.3227094f);
        Vector3 rVec7 = new Vector3(-0.1334717f, 0.08830917f, 0.2922978f);


        DataPoint rpoint1 = new DataPoint { point = rVec1, normal = Vector3.Normalize(rVec1) };
        DataPoint rpoint2 = new DataPoint { point = rVec2, normal = Vector3.Normalize(rVec2) };
        DataPoint rpoint3 = new DataPoint { point = rVec3, normal = Vector3.Normalize(rVec3) };
        DataPoint rpoint4 = new DataPoint { point = rVec4, normal = Vector3.Normalize(rVec4) };
        DataPoint rpoint5 = new DataPoint { point = rVec5, normal = Vector3.Normalize(rVec5) };
        DataPoint rpoint6 = new DataPoint { point = rVec6, normal = Vector3.Normalize(rVec6) };
        DataPoint rpoint7 = new DataPoint { point = rVec7, normal = Vector3.Normalize(rVec7) };

        referencePoints.Add(rpoint1);
        referencePoints.Add(rpoint2);
        referencePoints.Add(rpoint3);
        referencePoints.Add(rpoint4);
        referencePoints.Add(rpoint5);
        referencePoints.Add(rpoint6);
        referencePoints.Add(rpoint7);

        var reference = new DataPoints
        {
            points = referencePoints.ToArray(),
            contiansNormals = true,
        };

        EuclideanTransform initialTransform = new EuclideanTransform();
        initialTransform.translation = new Vector3(-0.02399f, 0f, 0.0332392f);
       
        var axis = Vector3.Normalize(RandomVector());
        initialTransform.rotation = Quaternion.CreateFromAxisAngle(axis, (float)(0.02 * Math.PI * 2));
        initialTransform.rotation = Quaternion.Normalize(initialTransform.rotation);

        ICP icp = new ICP();
        icp.ReadingDataPointsFilters = new RandomSamplingDataPointsFilter(prob: 0.1f);
        icp.ReferenceDataPointsFilters = new 
        SamplingSurfaceNormalDataPointsFilter(SamplingMethod.Bin, ratio: 0.2f);
        icp.OutlierFilter = new TrimmedDistOutlierFilter(ratio: 0.5f);
        var transform = icp.Compute(reading, reference, initialTransform);

'An algorithm failed to converge.'

Hello
I'm trying to use this ICP implementation to align two kinect depth map.
Yet, I always end up with

MathNet.Numerics.NonConvergenceException : 'An algorithm failed to converge.'

even if I start the ICP on a duplicated depth map with an identity initial transform (i.e. already aligned dataset)

Can someone help me ?
Thanks

Resulting Euclidean transformation

Hello, first of all very grateful for the excellent work you have done. I have the following problem and need help understanding the algebraic logic of the ICP.
Initially I translated two real point clouds using the following vector <1; -1; 10>. The algorithm initially returned a Euclidean transformation that further separated the data.
I start debugging the code and noticed that the translation vector was present in the computeWithTransformedReference method in T_iter, but method returns T_refIn_refMean * T_iter * T_refMean_dataIn instead T_iter. Returning just T_iter gives this translation vector <-0.994;1.004;-9.998> which is quite close to the original vector, so i can´t understand why the original method returns T_refIn_refMean * T_iter * T_refMean_dataIn.
On the other hand, when applying the transformation quaternion, the data moves away from the original cloud again.