Example #1
0
        public static PointCloudVertices ReadXYZFile_ToVertices(string fileNameLong, bool rotatePoints)
        {
            List <Vector3d>    listVector3d = ReadXYZFile(fileNameLong, rotatePoints);
            PointCloudVertices listVertices = PointCloudVertices.FromVectors(listVector3d);

            return(listVertices);
        }
Example #2
0
        public PointCloudVertices CalculatePCA(PointCloudVertices pointsSource, int eigenvectorUsed)
        {
            List <Vector3d> vector3dSource = PointCloudVertices.ToVectors(pointsSource);

            vector3dSource = CalculatePCA(vector3dSource, eigenvectorUsed);

            return(PointCloudVertices.FromVectors(vector3dSource));
        }
Example #3
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;
        }
Example #4
0
        private PointCloudVertices ICPOnPoints_WithSubset(PointCloudVertices myPCLTarget, PointCloudVertices myPCLToBeMatched, PointCloudVertices myPointsTargetSubset, PointCloudVertices mypointsSourceSubset)
        {
            List <Vector3d>    myVectorsTransformed = null;
            PointCloudVertices myPCLTransformed     = null;

            try
            {
                Matrix4d m;


                PerformICP(mypointsSourceSubset, myPointsTargetSubset);
                myVectorsTransformed = PointCloudVertices.ToVectors(PTransformed);
                m = Matrix;

                //DebugWriteUtils.WriteTestOutput(m, mypointsSourceSubset, myPointsTransformed, myPointsTargetSubset);
                //extend points:
                //myPointsTransformed = icpSharp.TransformPointsToPointsData(mypointsSourceSubset, m);
                //-----------------------------
                //DebugWriteUtils.WriteTestOutput(m, mypointsSourceSubset, myPointsTransformed, myPointsTargetSubset);

                //now with all other points as well...
                myVectorsTransformed = new List <Vector3d>();

                myVectorsTransformed = m.TransformPoints(PointCloudVertices.ToVectors(myPCLToBeMatched));
                myPCLTransformed     = PointCloudVertices.FromVectors(myVectorsTransformed);
                //write all results in debug output
                DebugWriteUtils.WriteTestOutputVertex("Soluation of Points With Subset", m, myPCLToBeMatched, myPCLTransformed, myPCLTarget);
            }
            catch (Exception err)
            {
                System.Diagnostics.Debug.WriteLine("Error in ICP : " + err.Message);
                return(null);
            }
            //for output:


            return(myPCLTransformed);
        }