C# DenseVector.CopyTo方法代码示例

        public static double Kurtosis(this double[] data)
        {
            var d = new DenseVector(data);
            var num = new DenseVector(d.Count);
            var denom = new DenseVector(d.Count);
            d.CopyTo(num);
            d.CopyTo(denom);

            for (int i = 0; i < num.Count; i++)
            {
                num[i] = Math.Pow(d[i] - d.Mean(), 4);
                denom[i] = Math.Pow(d[i] - d.Mean(), 2);
            }

            return (num.Sum()*num.Count)/(Math.Pow(denom.Sum(), 2));
        }

 public static DenseVector Normalize0to1(this DenseVector data)
 {
     var d = new DenseVector(data);
     var result = new DenseVector(d.Count);
     d.CopyTo(result);
     return (DenseVector) (result - d.Min())/(d.Max() - d.Min());
 }

 public static double[] NormalizeZScore(this double[] data)
 {
     var d = new DenseVector(data);
     var result = new DenseVector(d.Count);
     d.CopyTo(result);
     result = (DenseVector) ((result - d.Mean())/(d.StandardDeviation()));
     return result.ToArray();
 }

 public static double[] NormalizeNeg1to1(this double[] data)
 {
     var d = new DenseVector(data);
     var result = new DenseVector(d.Count);
     d.CopyTo(result);
     result = (DenseVector) (result - ((d.Max() + d.Min())/2))/((d.Max() - d.Min())/2);
     return result.ToArray();
 }

        /// <summary>
        /// Run example
        /// </summary>
        public void Run()
        {
            // Format vector output to console
            var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
            formatProvider.TextInfo.ListSeparator = " ";

            // Create new empty vector
            var vectorA = new DenseVector(10);
            Console.WriteLine(@"Empty vector A");
            Console.WriteLine(vectorA.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 1. Fill vector by data using indexer []
            for (var i = 0; i < vectorA.Count; i++)
            {
                vectorA[i] = i;
            }

            Console.WriteLine(@"1. Fill vector by data using indexer []");
            Console.WriteLine(vectorA.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 2. Fill vector by data using SetValues method
            vectorA.SetValues(new[] { 9.0, 8.0, 7.0, 6.0, 5.0, 4.0, 3.0, 2.0, 1.0, 0.0 });
            Console.WriteLine(@"2. Fill vector by data using SetValues method");
            Console.WriteLine(vectorA.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 3. Convert Vector to double[]
            var data = vectorA.ToArray();
            Console.WriteLine(@"3. Convert vector to double array");
            for (var i = 0; i < data.Length; i++)
            {
                Console.Write(data[i].ToString("#0.00\t", formatProvider) + @" ");
            }

            Console.WriteLine();
            Console.WriteLine();

            // 4. Convert Vector to column matrix. A matrix based on this vector in column form (one single column)
            var columnMatrix = vectorA.ToColumnMatrix();
            Console.WriteLine(@"4. Convert vector to column matrix");
            Console.WriteLine(columnMatrix.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 5. Convert Vector to row matrix. A matrix based on this vector in row form (one single row)
            var rowMatrix = vectorA.ToRowMatrix();
            Console.WriteLine(@"5. Convert vector to row matrix");
            Console.WriteLine(rowMatrix.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 6. Clone vector
            var cloneA = vectorA.Clone();
            Console.WriteLine(@"6. Clone vector");
            Console.WriteLine(cloneA.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 7. Clear vector
            cloneA.Clear();
            Console.WriteLine(@"7. Clear vector");
            Console.WriteLine(cloneA.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 8. Copy part of vector into another vector. If you need to copy all data then use CopoTy(vector) method.
            vectorA.CopyTo(cloneA, 3, 3, 4);
            Console.WriteLine(@"8. Copy part of vector into another vector");
            Console.WriteLine(cloneA.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

            // 9. Get part of vector as another vector
            var subvector = vectorA.SubVector(0, 5);
            Console.WriteLine(@"9. Get subvector");
            Console.WriteLine(subvector.ToString("#0.00\t", formatProvider));
            Console.WriteLine();

               // 10. Enumerator usage
            Console.WriteLine(@"10. Enumerator usage");
            foreach (var value in vectorA)
            {
                Console.Write(value.ToString("#0.00\t", formatProvider) + @" ");
            }

            Console.WriteLine();
            Console.WriteLine();

            // 11. Indexed enumerator usage
            Console.WriteLine(@"11. Enumerator usage");
            foreach (var value in vectorA.GetIndexedEnumerator())
            {
                Console.WriteLine(@"Index = {0}; Value = {1}", value.Item1, value.Item2.ToString("#0.00\t", formatProvider));
            }

            Console.WriteLine();
        }

        public override void Init()
        {
            //ParametersVector = new DenseVector(24);
            //for(int i = 0; i < 12; ++i)
            //{
            //    ParametersVector.At(i, CameraLeft.At(i / 4, i & 3));
            //    ParametersVector.At(i + 12, CameraRight.At(i / 4, i & 3));
            //}

            // Parameters:
            // Full: [fx, fy, s, px, py, eaX, eaY, eaZ, Cx, Cy, Cz]
            // Center fixed: [fx, fy, s, eaX, eaY, eaZ, Cx, Cy, Cz]
            ParametersVector = new DenseVector(_cameraParamsCount * 2);
            if(_fxIdx >= 0) ParametersVector.At(_fxIdx, CalibrationData.Data.CalibrationLeft.At(0, 0));
            if(_fyIdx >= 0) ParametersVector.At(_fyIdx, CalibrationData.Data.CalibrationLeft.At(1, 1));
            if(_skIdx >= 0) ParametersVector.At(_skIdx, CalibrationData.Data.CalibrationLeft.At(0, 1));
            if(_pxIdx >= 0) ParametersVector.At(_pxIdx, CalibrationData.Data.CalibrationLeft.At(0, 2));
            if(_pyIdx >= 0) ParametersVector.At(_pyIdx, CalibrationData.Data.CalibrationLeft.At(1, 2));

            Vector<double> euler = new DenseVector(3);
            RotationConverter.MatrixToEuler(euler, CalibrationData.Data.RotationLeft);
            if(_euXIdx >= 0) ParametersVector.At(_euXIdx, euler.At(0));
            if(_euYIdx >= 0) ParametersVector.At(_euYIdx, euler.At(1));
            if(_euZIdx >= 0) ParametersVector.At(_euZIdx, euler.At(2));

            if(_cXIdx >= 0) ParametersVector.At(_cXIdx, CalibrationData.Data.TranslationLeft.At(0));
            if(_cYIdx >= 0) ParametersVector.At(_cYIdx, CalibrationData.Data.TranslationLeft.At(1));
            if(_cZIdx >= 0) ParametersVector.At(_cZIdx, CalibrationData.Data.TranslationLeft.At(2));

            int n0 = _cameraParamsCount;
            if(_fxIdx >= 0) ParametersVector.At(_fxIdx + n0, CalibrationData.Data.CalibrationRight.At(0, 0));
            if(_fyIdx >= 0) ParametersVector.At(_fyIdx + n0, CalibrationData.Data.CalibrationRight.At(1, 1));
            if(_skIdx >= 0) ParametersVector.At(_skIdx + n0, CalibrationData.Data.CalibrationRight.At(0, 1));
            if(_pxIdx >= 0) ParametersVector.At(_pxIdx + n0, CalibrationData.Data.CalibrationRight.At(0, 2));
            if(_pyIdx >= 0) ParametersVector.At(_pyIdx + n0, CalibrationData.Data.CalibrationRight.At(1, 2));

            RotationConverter.MatrixToEuler(euler, CalibrationData.Data.RotationRight);
            if(_euXIdx >= 0) ParametersVector.At(_euXIdx + n0, euler.At(0));
            if(_euYIdx >= 0) ParametersVector.At(_euYIdx + n0, euler.At(1));
            if(_euZIdx >= 0) ParametersVector.At(_euZIdx + n0, euler.At(2));

            if(_cXIdx >= 0) ParametersVector.At(_cXIdx + n0, CalibrationData.Data.TranslationRight.At(0));
            if(_cYIdx >= 0) ParametersVector.At(_cYIdx + n0, CalibrationData.Data.TranslationRight.At(1));
            if(_cZIdx >= 0) ParametersVector.At(_cZIdx + n0, CalibrationData.Data.TranslationRight.At(2));

            //_imgCenterLeft = new Vector2(CalibrationData.Data.CalibrationLeft.At(0, 2),
            //    CalibrationData.Data.CalibrationLeft.At(1, 2));
            //_imgCenterRight = new Vector2(CalibrationData.Data.CalibrationRight.At(0, 2),
            //    CalibrationData.Data.CalibrationRight.At(1, 2));

            ResultsVector = new DenseVector(ParametersVector.Count + CalibGrids.Count * 12);
            BestResultVector = new DenseVector(ResultsVector.Count);
            ParametersVector.CopySubVectorTo(ResultsVector, 0, 0, ParametersVector.Count);

            _grids = new RealGridData[CalibGrids.Count];

            int N = ParametersVector.Count;
            for(int i = 0; i < CalibGrids.Count; ++i)
            {
                ResultsVector.At(N + i * 12, CalibGrids[i].TopLeft.X);
                ResultsVector.At(N + i * 12 + 1, CalibGrids[i].TopLeft.Y);
                ResultsVector.At(N + i * 12 + 2, CalibGrids[i].TopLeft.Z);
                ResultsVector.At(N + i * 12 + 3, CalibGrids[i].TopRight.X);
                ResultsVector.At(N + i * 12 + 4, CalibGrids[i].TopRight.Y);
                ResultsVector.At(N + i * 12 + 5, CalibGrids[i].TopRight.Z);
                ResultsVector.At(N + i * 12 + 6, CalibGrids[i].BotLeft.X);
                ResultsVector.At(N + i * 12 + 7, CalibGrids[i].BotLeft.Y);
                ResultsVector.At(N + i * 12 + 8, CalibGrids[i].BotLeft.Z);
                ResultsVector.At(N + i * 12 + 9, CalibGrids[i].BotRight.X);
                ResultsVector.At(N + i * 12 + 10, CalibGrids[i].BotRight.Y);
                ResultsVector.At(N + i * 12 + 11, CalibGrids[i].BotRight.Z);
                _grids[i] = new RealGridData();
                _grids[i].Columns = CalibGrids[i].Columns;
                _grids[i].Rows = CalibGrids[i].Rows;
            }
            ResultsVector.CopyTo(BestResultVector);

            _coeffMatch = MatchedPointsLeft.Count > 0 ? Math.Sqrt(MatchErrorCoeff * 0.5 / (double)MatchedPointsLeft.Count) : 0;
            _coeffImages = Math.Sqrt(ImagesErrorCoeff * 0.5 / (double)(CalibPointsLeft.Count + CalibPointsRight.Count));
            _coeffGrids = Math.Sqrt(GridsErrorCoeff * (CalibPointsLeft.Count + CalibPointsRight.Count) / (double)(CalibGrids.Count * 12));
            _coeffTriang = Math.Sqrt(TriangulationErrorCoeff * 0.33 / (double)CalibPointsLeft.Count);

            _currentErrorVector = new DenseVector(
                MatchedPointsLeft.Count * 2 + // Matched
                CalibPointsLeft.Count * 3 + // Triangulation
                CalibPointsLeft.Count * 2 + CalibPointsRight.Count * 2 + // Image
                CalibGrids.Count * 12); // Grids

            _J = new DenseMatrix(_currentErrorVector.Count, ResultsVector.Count);
            _Jt = new DenseMatrix(ResultsVector.Count, _currentErrorVector.Count);
            _JtJ = new DenseMatrix(ResultsVector.Count, ResultsVector.Count);
            _Jte = new DenseVector(ResultsVector.Count);
            _delta = new DenseVector(ResultsVector.Count);

            _reals = new Vector<double>[CalibPointsLeft.Count];
            _imgsLeft = new Vector<double>[CalibPointsLeft.Count];
            _imgsRight = new Vector<double>[CalibPointsRight.Count];

            _triangulation.UseLinearEstimationOnly = true;
            _triangulation.PointsLeft = new List<Vector<double>>(CalibPointsLeft.Count);
//.........这里部分代码省略.........

        public override void Init()
        {
            ResultsVector = new DenseVector(ParametersVector.Count + CalibrationGrids.Count * 12);
            BestResultVector = new DenseVector(ParametersVector.Count + CalibrationGrids.Count * 12);
            ParametersVector.CopySubVectorTo(ResultsVector, 0, 0, 12);
            for(int i = 0; i < CalibrationGrids.Count; ++i)
            {
                ResultsVector.At(12 + i * 12, CalibrationGrids[i].TopLeft.X);
                ResultsVector.At(12 + i * 12 + 1, CalibrationGrids[i].TopLeft.Y);
                ResultsVector.At(12 + i * 12 + 2, CalibrationGrids[i].TopLeft.Z);
                ResultsVector.At(12 + i * 12 + 3, CalibrationGrids[i].TopRight.X);
                ResultsVector.At(12 + i * 12 + 4, CalibrationGrids[i].TopRight.Y);
                ResultsVector.At(12 + i * 12 + 5, CalibrationGrids[i].TopRight.Z);
                ResultsVector.At(12 + i * 12 + 6, CalibrationGrids[i].BotLeft.X);
                ResultsVector.At(12 + i * 12 + 7, CalibrationGrids[i].BotLeft.Y);
                ResultsVector.At(12 + i * 12 + 8, CalibrationGrids[i].BotLeft.Z);
                ResultsVector.At(12 + i * 12 + 9, CalibrationGrids[i].BotRight.X);
                ResultsVector.At(12 + i * 12 + 10, CalibrationGrids[i].BotRight.Y);
                ResultsVector.At(12 + i * 12 + 11, CalibrationGrids[i].BotRight.Z);
            }
            ResultsVector.CopyTo(BestResultVector);
            _grids = new List<RealGridData>(CalibrationGrids);
            _gridErrorCoef = Math.Sqrt((double)CalibrationPoints.Count / (double)(CalibrationGrids.Count * 12.0));

            _currentErrorVector = new DenseVector(CalibrationGrids.Count * 12 + CalibrationPoints.Count * 2);
            _J = new DenseMatrix(_currentErrorVector.Count, ResultsVector.Count);
            _Jt = new DenseMatrix(ResultsVector.Count, _currentErrorVector.Count);
            _JtJ = new DenseMatrix(ResultsVector.Count, ResultsVector.Count);
            _Jte = new DenseVector(ResultsVector.Count);
            _delta = new DenseVector(ResultsVector.Count);
            _Lx = new DenseVector(CalibrationPoints.Count);
            _Ly = new DenseVector(CalibrationPoints.Count);
            _M = new DenseVector(CalibrationPoints.Count);
            _reals = new Vector3[CalibrationPoints.Count];

            UpdateAll();

            //if(DumpingMethodUsed == DumpingMethod.Additive)
            //{
            //    // Compute initial lambda lam = 10^-3*diag(J'J)/size(J'J)
            //    ComputeJacobian(_J);
            //    _J.TransposeToOther(_Jt);
            //    _Jt.MultiplyToOther(_J, _JtJ);
            //    _lam = 1e-3f * _JtJ.Trace() / (double)_JtJ.ColumnCount;
            //}
            //else
            if(DumpingMethodUsed == DumpingMethod.Multiplicative)
            {
                _lam = 1e-3f;
            }
            else
                _lam = 0.0;

            _lastResidiual = _currentResidiual;
            Solver = new SvdSolver();
        }