Beispiel #1
0
        private static double CalculateScale_Zinsser(PointCloud pointsSourceShift, PointCloud pointsTargetShift, ref Matrix3d R)
        {
            double   sum1   = 0;
            double   sum2   = 0;
            Matrix3d RT     = Matrix3d.Transpose(R);
            Matrix3d checkT = Matrix3d.Mult(RT, R);

            Matrix4d R4D = new Matrix4d();

            R4D = R4D.PutTheMatrix4dtogether(Vector3d.Zero, R);



            for (int i = 0; i < pointsSourceShift.Count; i++)
            {
                Vector3d vSource = new Vector3d(pointsSourceShift.Vectors[i].X, pointsSourceShift.Vectors[i].Y, pointsSourceShift.Vectors[i].Z);
                Vector3d vTarget = new Vector3d(pointsTargetShift.Vectors[i].X, pointsTargetShift.Vectors[i].Y, pointsTargetShift.Vectors[i].Z);

                Vector3d vd = Vector3d.TransformNormalInverse(new Vector3d(pointsSourceShift.Vectors[i].X, pointsSourceShift.Vectors[i].Y, pointsSourceShift.Vectors[i].Z), R4D);
                vd = R.MultiplyVector(vSource);

                //Vector3 v = new Vector3(vd.X, vd.Y, vd.Z);

                //sum1 += Vector3.Dot(v, pointsTargetShift.Vectors[i]);
                //sum2 += Vector3.Dot(pointsSourceShift.Vectors[i], pointsSourceShift.Vectors[i]);

                sum1 += Vector3d.Dot(vd, vTarget);
                sum2 += Vector3d.Dot(vSource, vSource);
            }

            double c = sum1 / sum2;

            R = R.MultiplyScalar(c);
            return(c);
        }
Beispiel #2
0
        private static double CalculateScale_Zinsser(List <Vector3d> pointsSourceShift, List <Vector3d> pointsTargetShift, ref Matrix3d R)
        {
            double   sum1   = 0;
            double   sum2   = 0;
            Matrix3d RT     = Matrix3d.Transpose(R);
            Matrix3d checkT = Matrix3d.Mult(RT, R);

            Matrix4d R4D = new Matrix4d();

            R4D = R4D.PutTheMatrix4dtogether(Vector3d.Zero, R);



            for (int i = 0; i < pointsSourceShift.Count; i++)
            {
                Vector3d v = Vector3d.TransformNormalInverse(pointsSourceShift[i], R4D);
                v = R.MultiplyVector(pointsSourceShift[i]);

                sum1 += Vector3d.Dot(v, pointsTargetShift[i]);
                sum2 += Vector3d.Dot(pointsSourceShift[i], pointsSourceShift[i]);
            }

            double c = sum1 / sum2;

            R = R.MultiplyScalar(c);
            return(c);
        }
Beispiel #3
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);
        }
Beispiel #4
0
        public static Matrix4d FindTransformationMatrix(PointCloud pointsSource, PointCloud pointsTarget, ICP_VersionUsed icpVersionUsed)
        {
            //shift points to the center of mass (centroid)
            Vector3 centroidTarget = pointsTarget.CentroidVector;
            //Vector3d centroidTarget_Double = new Vector3d(pointsTarget.CentroidVector.X, pointsTarget.CentroidVector.Y, pointsTarget.CentroidVector.Z);

            PointCloud pointsTargetTranslated = pointsTarget.Clone();

            pointsTargetTranslated.SubtractVector(centroidTarget);

            Vector3    centroidSource         = pointsSource.CentroidVector;
            PointCloud pointsSourceTranslated = pointsSource.Clone();

            pointsSourceTranslated.SubtractVector(centroidSource);

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

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

            myMatrix = myMatrix.PutTheMatrix4dtogether(T, R);

            return(myMatrix);
        }