Пример #1
0
        private Matrix4d AdjustSourceTargetByTranslation(Matrix4d myMatrixFound, PointCloudVertices pointCloudSource, PointCloudVertices pointCloudTarget)
        {
            Matrix3d R = myMatrixFound.ExtractMatrix3d();
            Vector3d T = SVD.CalculateTranslation(pointCloudSource.CentroidVector, pointCloudTarget.CentroidVector, R);

            myMatrixFound = myMatrixFound.AddTranslation(T);
            return(myMatrixFound);
        }
Пример #2
0
        private void SVD_OfPointCloud(PointCloudVertices pointsSource, bool normalsCovariance)
        {
            //calculate correlation matrix

            Matrix3d C = PointCloudVertices.CovarianceMatrix(pointsSource, normalsCovariance);

            SVD.Eigenvalues_Helper(C);

            EV = SVD.EV;
            VT = SVD.VT;
            U  = SVD.U;
        }
Пример #3
0
        private void SVD_ForListVectorsMassCentered(List <Vector3d> pointsSource, bool normalsCovariance)
        {
            //calculate correlation matrix
            Matrix3d C = PointCloudVertices.CovarianceMatrix(pointsSource, normalsCovariance);

            SVD.Eigenvalues_Helper(C);
            EV = SVD.EV;
            VT = SVD.VT;
            U  = SVD.U;

            V = Matrix3d.Transpose(VT);
        }
Пример #4
0
        /// <summary>
        /// calculates Matrix for alignment of sourceAxes and targetAxes; sets pointCloudResult
        /// </summary>
        /// <param name="i"></param>
        /// <param name="j"></param>
        /// <param name="bestResultMeanDistance"></param>
        /// <param name="meanDistance"></param>
        /// <param name="myMatrixBestResult"></param>
        /// <param name="sourceAxes"></param>
        private void SVD_ForTwoPointCloudAlignment(int i, int j, ref double bestResultMeanDistance, ref double meanDistance, ref Matrix4d myMatrixBestResult, PointCloudVertices sourceAxes)
        {
            PointCloudVertices targetAxes = InvertAxes(pointCloudTargetCentered, pointCloudTargetCentered.PCAAxes, j);

            Matrix4d myMatrix = SVD.FindTransformationMatrix(PointCloudVertices.ToVectors(sourceAxes), PointCloudVertices.ToVectors(targetAxes), ICP_VersionUsed.Scaling_Umeyama);
            //Matrix4d myMatrix = SVD.FindTransformationMatrix_WithoutCentroids(PointCloudVertices.ToVectors(sourceAxes), PointCloudVertices.ToVectors(targetAxes), ICP_VersionUsed.Scaling_Umeyama);

            //-----------------------
            //for check - should give TargetPCVectors
            List <Vector3d> resultAxes = Matrix4dExtension.TransformPoints(myMatrix, PointCloudVertices.ToVectors(sourceAxes));

            resultAxes = resultAxes.Subtract(PointCloudVertices.ToVectors(targetAxes));



            List <Vector3d>    myPointsResult     = myMatrix.TransformPoints(PointCloudVertices.ToVectors(pointCloudSourceCentered));
            PointCloudVertices myPointsResultTemp = PointCloudVertices.FromVectors(myPointsResult);

            PointCloudVertices myPointCloudTargetTemp = kdtree.FindNearest_Rednaxela_Parallel(ref myPointsResultTemp, pointCloudTargetCentered, -1);
            //PointCloudVertices myPointCloudTargetTemp = kdtree.FindNearest_Rednaxela(ref myPointsResultTemp, pointCloudTargetCentered, -1);

            double trace = myMatrix.Trace;

            meanDistance = kdtree.MeanDistance;

            //double trace = kdtree.MeanDistance;
            //double meanDistance = myMatrix.Trace;
            //Check:

            System.Diagnostics.Debug.WriteLine("   in iteration: MeanDistance between orientations: " + i.ToString() + " : " + j.ToString() + " : " + meanDistance.ToString("G") + " : Trace: " + trace.ToString("G"));

            if (meanDistance < bestResultMeanDistance)
            {
                myMatrixBestResult     = myMatrix;
                bestResultMeanDistance = meanDistance;
                pointCloudResultBest   = PointCloudVertices.FromVectors(myPointsResult);
            }



            pointCloudResult       = PointCloudVertices.FromVectors(myPointsResult);
            pointCloudTargetKDTree = myPointCloudTargetTemp;
        }
Пример #5
0
        public Matrix4d CalculateRegistrationMatrix(PointCloud pcOriginal)
        {
            Vector3d v        = new Vector3d();
            Matrix4d myMatrix = Matrix4d.Identity;

            if (this.GLrender.RenderableObjects.Count > 1)
            {
                RenderableObject o   = this.GLrender.RenderableObjects[0];
                PointCloud       pc1 = o.PointCloud;


                myMatrix = SVD.FindTransformationMatrix(v.ArrayVector3ToList(pcOriginal.Vectors), v.ArrayVector3ToList(pc1.Vectors), ICP_VersionUsed.Scaling_Umeyama);

                //myMatrix = SVD.FindTransformationMatrix_WithoutCentroids(v.ArrayVector3ToList(pc1.Vectors), v.ArrayVector3ToList(pc2.Vectors), ICP_VersionUsed.Scaling_Umeyama);
            }
            else
            {
                System.Windows.Forms.MessageBox.Show("Please load two Point Clouds first");
            }
            return(myMatrix);
        }
Пример #6
0
        public static Matrix4d FindTransformationMatrix(List <Vector3d> pointsSource, List <Vector3d> pointsTarget, ICP_VersionUsed icpVersionUsed)
        {
            //shift points to the center of mass (centroid)
            Vector3d        centroidTarget         = pointsTarget.CalculateCentroid();
            List <Vector3d> pointsTargetTranslated = pointsTarget.Clone();

            pointsTargetTranslated.SubtractVector(centroidTarget);

            Vector3d        centroidSource         = pointsSource.CalculateCentroid();
            List <Vector3d> pointsSourceTranslated = pointsSource.Clone();

            pointsSourceTranslated.SubtractVector(centroidSource);

            Matrix3d R = FindRotationMatrix(pointsSourceTranslated, pointsTargetTranslated, icpVersionUsed);

            Vector3d T        = SVD.CalculateTranslation(centroidSource, centroidTarget, R);
            Matrix4d myMatrix = new Matrix4d();

            myMatrix = myMatrix.PutTheMatrix4dtogether(T, R);

            return(myMatrix);
        }
        public List <Vertex> PerformICP_New()
        {
            int iter = 0;

            try
            {
                if (!CheckSourceTarget(PTarget, PSource))
                {
                    return(null);
                }
                if (ResetVertexToOrigin)
                {
                    Vertices.ResetVertexToOrigin(PTarget);
                    Vertices.ResetVertexToOrigin(PSource);
                }

                List <Vertex> pointsTargetIterate = Vertices.CopyVertices(PTarget);
                List <Vertex> pointsSourceIterate = Vertices.CopyVertices(PSource);
                Matrix4d      myMatrix;

                if (!FixedTestPoints)
                {
                    BuildKDTree(pointsTargetIterate, pointsSourceIterate);
                }

                this.Matrix = Matrix4d.Identity;
                double oldMeanDistance = 0;


                for (iter = 0; iter < MaximumNumberOfIterations; iter++)
                {
                    //pointsSourceIterate = PSource;
                    if (!FixedTestPoints)
                    {
                        pointsTargetIterate = SearchNearestNeighbours(pointsSourceIterate, pointsTargetIterate);

                        if (pointsTargetIterate.Count != pointsSourceIterate.Count)
                        {
                            MessageBox.Show("Error finding neighbours, found " + pointsSourceIterate.Count.ToString() + " out of " + pointsTargetIterate.ToString());
                            break;
                        }
                    }
                    List <Vertex> myPointsTransformed = null;
                    if (ICPVersion == ICP_VersionUsed.Quaternions)
                    {
                        TransformPointsUtils.FindTransformationMatrix(Vertices.Vector3dListFromVertexList(pointsSourceIterate), Vertices.Vector3dListFromVertexList(pointsTargetIterate), this.LandmarkTransform);
                        myMatrix            = LandmarkTransform.Matrix;
                        myPointsTransformed = MathUtils.TransformPoints(pointsSourceIterate, myMatrix);
                        DebugWriteUtils.WriteTestOutputVertex("Quaternion Method", myMatrix, pointsSourceIterate, myPointsTransformed, pointsTargetIterate);
                    }
                    else
                    {
                        myMatrix = SVD.FindTransformationMatrix(Vertices.Vector3dListFromVertexList(pointsTargetIterate), Vertices.Vector3dListFromVertexList(pointsSourceIterate), ICPVersion);
                        //DebugWriteUtils.WriteMatrix("Solution with scaling", trialM);

                        myPointsTransformed = MathUtils.TransformPoints(pointsSourceIterate, myMatrix);
                        DebugWriteUtils.WriteTestOutputVertex("New Method WITH Scale", myMatrix, pointsSourceIterate, myPointsTransformed, pointsTargetIterate);
                    }
                    double totaldist = PointUtils.CalculateTotalDistance(pointsTargetIterate, myPointsTransformed);
                    this.MeanDistance = totaldist / Convert.ToDouble(pointsTargetIterate.Count);
                    Debug.WriteLine("--------------Iteration: " + iter.ToString() + " : Mean Distance: " + MeanDistance.ToString("0.00000000000"));

                    //accumulate resulting matrix
                    Matrix4d.Mult(ref myMatrix, ref this.Matrix, out this.Matrix);

                    if (MeanDistance < this.MaximumMeanDistance) //< Math.Abs(MeanDistance - oldMeanDistance) < this.MaximumMeanDistance)
                    {
                        break;
                    }
                    oldMeanDistance = MeanDistance;

                    DebugWriteUtils.WriteMatrix("Concatenated matrix", Matrix);

                    myPointsTransformed = MathUtils.TransformPoints(PSource, myMatrix);

                    //Matrix4.Mult(ref preMatrix, ref myMatrix, out preMatrix);
                    pointsSourceIterate = myPointsTransformed;
                    pointsTargetIterate = Vertices.CopyVertices(PTarget);
                }

                Debug.WriteLine("--------****** Solution of ICP after : " + iter.ToString() + " iterations, and Mean Distance: " + MeanDistance.ToString("0.00000000000"));
                this.PTransformed = MathUtils.TransformPoints(PSource, Matrix);

                DebugWriteUtils.WriteTestOutputVertex("Solution of ICP", Matrix, PSource, PTransformed, PTarget);



                return(PTransformed);
            }
            catch (Exception err)
            {
                System.Windows.Forms.MessageBox.Show("Error in Update ICP at iteration: " + iter.ToString() + " : " + err.Message);
                return(null);
            }
        }