C# DenseVector.Subtract方法代码示例

        private static void SetRealVertices(Workspace workspace)
        {
            Vector<double> fittedPlaneVector = GeometryHelper.FitPlaneToPoints(workspace.PointCloud.ToArray());

            if (fittedPlaneVector == null)
            {
                return;
            }

            Point3D projectedPoint = GeometryHelper.ProjectPoint3DToPlane(workspace.PointCloud.First(), fittedPlaneVector);

            Vector<double> planeNormal = new DenseVector(new[] { fittedPlaneVector[0], fittedPlaneVector[1], fittedPlaneVector[2] });

            Point3D[] vertices3D = workspace.Vertices3D;

            Point[] vertices = workspace.Vertices.ToArray();

            for (int i = 0; i < vertices.Length; i++)
            {

                Vector<double> pointOnPlane = new DenseVector(new[] { projectedPoint.X, projectedPoint.Y, projectedPoint.Z });
                Vector<double> pointOnLine = new DenseVector(new double[] { vertices3D[i].X, vertices3D[i].Y, vertices3D[i].Z });

                double d = (pointOnPlane.Subtract(pointOnLine)).DotProduct(planeNormal) / (pointOnLine.DotProduct(planeNormal));

                Vector<double> intersection = pointOnLine + pointOnLine.Multiply(d);

                workspace.FittedVertices[i] = new Point3D(intersection[0], intersection[1], intersection[2]);
            }

            workspace.PlaneVector = fittedPlaneVector;
        }

//.........这里部分代码省略.........
            Console.WriteLine(@"Dot product between two vectors using operator *. (result = X * Y)");
            Console.WriteLine(dotProduct);
            Console.WriteLine();

            // 2. Using DotProduct method and getting result into different vector instance
            dotProduct = vectorX.DotProduct(vectorY);
            Console.WriteLine(@"Dot product between two vectors using method DotProduct. (result = X.DotProduct(Y))");
            Console.WriteLine(dotProduct.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // Pointwise multiplies vector with another vector
            // 1. Using PointwiseMultiply method and getting result into different vector instance
            resultV = vectorX.PointwiseMultiply(vectorY);
            Console.WriteLine(@"Pointwise multiplies vector with another vector using method PointwiseMultiply. (result = X.PointwiseMultiply(Y))");
            Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 2. Using PointwiseMultiply method and updating vector itself
            vectorX.PointwiseMultiply(vectorY, vectorX);
            Console.WriteLine(@"Pointwise multiplies vector with another vector using method PointwiseMultiply. (X.PointwiseMultiply(Y, X))");
            Console.WriteLine(vectorX.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // Pointwise divide vector with another vector
            // 1. Using PointwiseDivide method and getting result into different vector instance
            resultV = vectorX.PointwiseDivide(vectorY);
            Console.WriteLine(@"Pointwise divide vector with another vector using method PointwiseDivide. (result = X.PointwiseDivide(Y))");
            Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 2. Using PointwiseDivide method and updating vector itself
            vectorX.PointwiseDivide(vectorY, vectorX);
            Console.WriteLine(@"Pointwise divide vector with another vector using method PointwiseDivide. (X.PointwiseDivide(Y, X))");
            Console.WriteLine(vectorX.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // Addition
            // 1. Using operator "+"
            resultV = vectorX + vectorY;
            Console.WriteLine(@"Add vectors using operator +. (result = X + Y)");
            Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 2. Using Add method and getting result into different vector instance
            resultV = vectorX.Add(vectorY);
            Console.WriteLine(@"Add vectors using method Add. (result = X.Add(Y))");
            Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 3. Using Add method and updating vector itself
            vectorX.Add(vectorY, vectorX);
            Console.WriteLine(@"Add vectors using method Add. (X.Add(Y, X))");
            Console.WriteLine(vectorX.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // Subtraction
            // 1. Using operator "-"
            resultV = vectorX - vectorY;
            Console.WriteLine(@"Subtract vectors using operator -. (result = X - Y)");
            Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 2. Using Subtract method and getting result into different vector instance
            resultV = vectorX.Subtract(vectorY);
            Console.WriteLine(@"Subtract vectors using method Subtract. (result = X.Subtract(Y))");
            Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 3. Using Subtract method and updating vector itself
            vectorX.Subtract(vectorY, vectorX);
            Console.WriteLine(@"Subtract vectors using method Subtract. (X.Subtract(Y, X))");
            Console.WriteLine(vectorX.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // Divide by scalar
            // 1. Using Divide method and getting result into different vector instance
            resultV = vectorX.Divide(3.0);
            Console.WriteLine(@"Divide vector by scalar using method Divide. (result = A.Divide(3.0))");
            Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 2. Using Divide method and updating vector itself
            vectorX.Divide(3.0, vectorX);
            Console.WriteLine(@"Divide vector by scalar using method Divide. (X.Divide(3.0, X))");
            Console.WriteLine(vectorX.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // Outer Product of two vectors
            // 1. Using instanse method OuterProduct
            var resultM = vectorX.OuterProduct(vectorY);
            Console.WriteLine(@"Outer Product of two vectors using method OuterProduct. (X.OuterProduct(Y))");
            Console.WriteLine(resultM.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 2. Using static method of the Vector class
            resultM = Vector.OuterProduct(vectorX, vectorY);
            Console.WriteLine(@"Outer Product of two vectors using method OuterProduct. (Vector.OuterProduct(X,Y))");
            Console.WriteLine(resultM.ToString("#0.00\t", formatProvider));
            Console.WriteLine();
        }

        private void solveActuator2CartesianDisp(double[] adisp)
        {
            bool check = false;
            DenseVector cartDisp = new DenseVector(6);
            DenseVector newAct = new DenseVector(adisp);
            DenseVector actError = (DenseVector)newAct.Subtract(actuatorDisp);
            cartesianDisp.CopyTo(cartDisp);
            int iterations = 0;

            while (check == false)
            {
                List2String l2s = new List2String();

                DenseMatrix JacobianMatrix = new DenseMatrix(6, 6);

                for (int i = 0; i < 6; i++)
                {
                    DenseVector DL_Dd = actuators[i].calcNewDiffs(cartDisp.Values);
                    JacobianMatrix.SetRow(i, DL_Dd);
                }
                DenseVector diffCart = (DenseVector)JacobianMatrix.LU().Solve(actError);
                log.Debug("Cartesian differences " + l2s.ToString(diffCart.Values));
                cartDisp = (DenseVector)cartDisp.Add(diffCart);
                setCartesianDisp(cartDisp.Values);
                log.Debug("New cartesian estimate " + this);
                actError = (DenseVector)newAct.Subtract(actuatorDisp);
                log.Debug("Actuator error " + l2s.ToString(actError.Values));

                check = withinErrorWindow(actError);
                if (iterations > 20)
                {
                    check = true;
                    log.Error("Calculations for " + label + " won't converge with " + this);
                }
                iterations++;
            }
        }

        public void SetRealVertices(Workspace workspace)
        {
            Vector<double> fittedPlaneVector = GeometryHelper.FitPlaneToPoints(workspace.PointCloud.ToArray());

            if (fittedPlaneVector == null)
            {
                return;
            }

            Point3D projectedPoint = GeometryHelper.ProjectPoint3DToPlane(workspace.PointCloud.First(), fittedPlaneVector);

            Vector<double> planeNormal = new DenseVector(new[] { fittedPlaneVector[0], fittedPlaneVector[1], fittedPlaneVector[2] });

            CameraSpacePoint[] csps = { new CameraSpacePoint() };

            Point[] vertices = workspace.Vertices.ToArray();

            for (int i = 0; i < vertices.Length; i++)
            {
                Point vertex = vertices[i];

                kinectStreamer.CoordinateMapper.MapDepthPointsToCameraSpace(
                    new[] {
                        new DepthSpacePoint {
                            X = (float)vertex.X,
                            Y = (float)vertex.Y
                        }
                    },
                    new ushort[] { 1 }, csps);

                Vector<double> pointOnPlane = new DenseVector(new[] { projectedPoint.X, projectedPoint.Y, projectedPoint.Z });
                Vector<double> pointOnLine = new DenseVector(new double[] { csps[0].X, csps[0].Y, csps[0].Z });

                double d = (pointOnPlane.Subtract(pointOnLine)).DotProduct(planeNormal) / (pointOnLine.DotProduct(planeNormal));

                Vector<double> intersection = pointOnLine + pointOnLine.Multiply(d);

                workspace.FittedVertices[i] = new Point3D(intersection[0], intersection[1], intersection[2]);
            }

            workspace.PlaneVector = fittedPlaneVector;
        }

        public static Point3D ProjectPoint3DToPlane(Point3D point, Vector<double> planeVectors)
        {
            double distance = CalculatePointPlaneDistance(point, planeVectors);

            double a = planeVectors[0];
            double b = planeVectors[1];
            double c = planeVectors[2];

            Vector<double> planeNormal = new DenseVector(new[] { a, b, c });

            double x = point.X;
            double y = point.Y;
            double z = point.Z;

            Vector<double> pointVector = new DenseVector(new[] { x, y, z });

            pointVector.Subtract(planeNormal.Multiply(distance));

            return new Point3D
            {
                X = pointVector[0],
                Y = pointVector[1],
                Z = pointVector[2]
            };
        }