C# Vector.At方法代码示例

        public override void ComputeMappingFucntion(Vector<double> mapFuncResult)
        {
            int measuredPointsCount = (MeasurementsVector.Count) / 5;
            // exi = lxi / mi, eyi = lyi / mi
            for(int i = 0; i < measuredPointsCount; ++i)
            {
                mapFuncResult.At(2 * i, _Lx.At(i) / _M.At(i));
                mapFuncResult.At(2 * i + 1, _Ly.At(i) / _M.At(i));
            }

            // Compute s:
            _cm[0, 0] = ResultsVector[0];
            _cm[0, 1] = ResultsVector[1];
            _cm[0, 2] = ResultsVector[2];
            _cm[1, 0] = ResultsVector[4];
            _cm[1, 1] = ResultsVector[5];
            _cm[1, 2] = ResultsVector[6];
            _cm[2, 0] = ResultsVector[8];
            _cm[2, 0] = ResultsVector[9];
            _cm[2, 0] = ResultsVector[10];

            var RQ = _cm.QR();

            double s = RQ.R[0, 1];// / RQ.R[1, 1];
            mapFuncResult[mapFuncResult.Count - 1] = s;
        }

        /// <summary>
        /// Create a new Vector3DHomogeneous from a Math.NET Numerics vector of length 4.
        /// </summary>
        public static Vector3DHomogeneous OfVector(Vector<double> vector)
        {
            if (vector.Count != 4)
            {
                throw new ArgumentException("The vector length must be 4 in order to convert it to a Vector3D");
            }

            return new Vector3DHomogeneous(vector.At(0), vector.At(1), vector.At(2), vector.At(2));
        }

 public static double ComputeAt(double x, Vector<float> coeffs)
 {
     double val = coeffs.At(coeffs.Count - 1);
     double xn = x;
     for(int c = coeffs.Count - 2; c >= 0; --c)
     {
         val += xn * coeffs.At(c);
         xn *= x;
     }
     return val;
 }

 public override void ComputeErrorVector(Vector<double> error)
 {
     ComputeMappingFucntion(error);
     int measuredPointsCount = (MeasurementsVector.Count) / 5;
     for(int i = 0; i < measuredPointsCount; ++i)
     {
         error.At(2 * i, MeasurementsVector[3 * measuredPointsCount + 2 * i] - error[2 * i]);
         error.At(2 * i + 1, MeasurementsVector[3 * measuredPointsCount + 2 * i + 1] - error[2 * i + 1]);
     }
     error[error.Count - 1] = _w * error[error.Count - 1];
 }

 public Vector3(Vector<double> other)
 {
     if(other.Count == 4)
     {
         // Treat input vector as homogenous 3d vector
         _x = other.At(0) / other.At(3);
         _y = other.At(1) / other.At(3);
         _z = other.At(2) / other.At(3);
     }
     else
     {
         _x = other.At(0);
         _y = other.At(1);
         _z = other.At(2);
     }
 }

 // Converts 3x3 rotation matrix to XYZ euler angles (assumes vector is correctly allocated)
 public static void MatrixToEuler(Vector<double> euler, Matrix<double> matrix)
 {
     if(matrix.At(0, 2) < 1.0 - 1e-9)
     {
         if(matrix.At(0, 2) > -1.0 + 1e-9)
         {
             //      t he t aY = a s i n(r 0 2);
             //     t he t aX = a t a n 2(−r12, r 2 2);
             //    t h e t aZ = a t a n 2(−r01, r 0 0);
             euler.At(1, Math.Asin(matrix.At(0, 2)));
             euler.At(0, Math.Atan2(-matrix.At(1, 2), matrix.At(2, 2)));
             euler.At(2, Math.Atan2(-matrix.At(0, 1), matrix.At(0, 0)));
         }
         else // r 0 2 = −1
         {
             // Not a u n i q u e s o l u t i o n : t h e t aZ − t he t aX = a t a n 2 ( r10 , r 1 1 )
             //  t he t aY = −PI / 2;
             // t he t aX = −a t a n 2(r10, r 1 1);
             // t h e t aZ = 0;
             euler.At(1, -Math.PI * 0.5);
             euler.At(0, -Math.Atan2(-matrix.At(1, 0), matrix.At(1, 1)));
             euler.At(2, 0.0);
         }
     }
     else // r 0 2 = +1
     {
         // Not a u n i q u e s o l u t i o n : t h e t aZ + t he t aX = a t a n 2 ( r10 , r 1 1 )
         // t he t aY = +PI / 2;
         // t he t aX = a t a n 2(r10, r 1 1);
         // t h e t aZ = 0;
         euler.At(1, Math.PI * 0.5);
         euler.At(0, -Math.Atan2(-matrix.At(1, 0), matrix.At(1, 1)));
         euler.At(2, 0.0);
     }
 }

 // Converts XYZ euler angles to 3x3 rotation matrix (assumes matrix is correctly allocated)
 public static void EulerToMatrix(Vector<double> euler, Matrix<double> matrix)
 {
     // | cycz           −cysz           sy    |
     // | czsxsy + cxsz  cxcz − sxsysz   −cysx |
     // | −cxczsy + sxsz czsx + cxsysz   cxcy  |
     //
     double sx = Math.Sin(euler.At(0));
     double cx = Math.Cos(euler.At(0));
     double sy = Math.Sin(euler.At(1));
     double cy = Math.Cos(euler.At(1));
     double sz = Math.Sin(euler.At(2));
     double cz = Math.Cos(euler.At(2));
     matrix.At(0, 0, cy * cz);
     matrix.At(0, 1, -cy * sz);
     matrix.At(0, 2, sy);
     matrix.At(1, 0, cz * sx * sy + cx * sz);
     matrix.At(1, 1, cx * cz - sx * sy * sz);
     matrix.At(1, 2, -cy * sx);
     matrix.At(2, 0, -cx * cz * sy + sz * sz);
     matrix.At(2, 1, cz * sx + cx * sy * sz);
     matrix.At(2, 2, cx * cy);
 }

 /// <summary>
 /// Pointwise multiplies this vector with another vector and stores the result into the result vector.
 /// </summary>
 /// <param name="other">The vector to pointwise multiply with this one.</param>
 /// <param name="result">The vector to store the result of the pointwise multiplication.</param>
 protected override void DoPointwiseMultiply(Vector<float> other, Vector<float> result)
 {
     if (ReferenceEquals(this, other))
     {
         for (var i = 0; i < _storage.ValueCount; i++)
         {
             _storage.Values[i] *= _storage.Values[i];
         }
     }
     else
     {
         for (var i = 0; i < _storage.ValueCount; i++)
         {
             var index = _storage.Indices[i];
             result.At(index, other.At(index) * _storage.Values[i]);
         }
     }
 }

        /// <summary>
        /// Negates vector and saves result to <paramref name="result"/>
        /// </summary>
        /// <param name="result">Target vector</param>
        protected override void DoNegate(Vector<float> result)
        {
            var sparseResult = result as SparseVector;
            if (sparseResult == null)
            {
                result.Clear();
                for (var index = 0; index < _storage.ValueCount; index++)
                {
                    result.At(_storage.Indices[index], -_storage.Values[index]);
                }
            }
            else
            {
                if (!ReferenceEquals(this, result))
                {
                    sparseResult._storage.ValueCount = _storage.ValueCount;
                    sparseResult._storage.Indices = new int[_storage.ValueCount];
                    Buffer.BlockCopy(_storage.Indices, 0, sparseResult._storage.Indices, 0, _storage.ValueCount * Constants.SizeOfInt);
                    sparseResult._storage.Values = new float[_storage.ValueCount];
                    Array.Copy(_storage.Values, sparseResult._storage.Values, _storage.ValueCount);
                }

                Control.LinearAlgebraProvider.ScaleArray(-1.0f, sparseResult._storage.Values, sparseResult._storage.Values);
            }
        }

 /// <summary>
 /// Computes the dot product between this vector and another vector.
 /// </summary>
 /// <param name="other">The other vector.</param>
 /// <returns>The sum of a[i]*b[i] for all i.</returns>
 protected override float DoDotProduct(Vector<float> other)
 {
     var result = 0f;
     if (ReferenceEquals(this, other))
     {
         for (var i = 0; i < _storage.ValueCount; i++)
         {
             result += _storage.Values[i] * _storage.Values[i];
         }
     }
     else
     {
         for (var i = 0; i < _storage.ValueCount; i++)
         {
             result += _storage.Values[i] * other.At(_storage.Indices[i]);
         }
     }
     return result;
 }

        public override void FinalizeForPixel(IntVector2 pixelBase)
        {
            if(_minIdx == -1)
            {
                // There was no disparity for pixel : set as invalid
                DisparityMap.Set(pixelBase.Y, pixelBase.X,
                    new Disparity(pixelBase, pixelBase, double.PositiveInfinity, 0.0, (int)DisparityFlags.Invalid));
                return;
            }

            Disparity bestDisp = _dispForPixel[_minIdx];
            bestDisp.Confidence = ConfidenceComp.ComputeConfidence(_dispForPixel, _minIdx, _min2Idx);

            if(_minIdx > 0 && _minIdx < _dispForPixel.Count - 1)
            {
                // D'[d-1] = D[d] + c[d-1]/c[d] * (D[d-1] - D[d])
                // D'[d+1] = D[d] + c[d+1]/c[d] * (D[d+1] - D[d])
                // Fit quadratic func. to d,d-1,d+1
                double c_n = _dispForPixel[_minIdx - 1].Cost / _minCost;
                double c_p = _dispForPixel[_minIdx + 1].Cost / _minCost;
                double dx = _dispForPixel[_minIdx].DX;
                double dy = _dispForPixel[_minIdx].DY;
                double dx_n = dx + c_n * (_dispForPixel[_minIdx - 1].DX - dx);
                double dy_n = dy + c_n * (_dispForPixel[_minIdx - 1].DY - dy);
                double dx_p = dx + c_p * (_dispForPixel[_minIdx + 1].DX - dx);
                double dy_p = dy + c_p * (_dispForPixel[_minIdx + 1].DY - dy);

                _D.At(0, 0, (_minIdx - 1) * (_minIdx - 1));
                _D.At(0, 1, _minIdx - 1);
                _D.At(1, 0, _minIdx * _minIdx);
                _D.At(1, 1, _minIdx);
                _D.At(2, 0, (_minIdx + 1) * (_minIdx + 1));
                _D.At(2, 1, (_minIdx + 1));

                _d.At(0, dx_n);
                _d.At(1, dx);
                _d.At(2, dx_p);

                _a = SvdSolver.Solve(_D, _d);
                bestDisp.SubDX = _a.At(0) * _minIdx * _minIdx + _a.At(1) * _minIdx + _a.At(2);

                _d.At(0, dy_n);
                _d.At(1, dy);
                _d.At(2, dy_p);

                _a = SvdSolver.Solve(_D, _d);
                bestDisp.SubDY = _a.At(0) * _minIdx * _minIdx + _a.At(1) * _minIdx + _a.At(2);
            }

            IntVector2 pm = bestDisp.GetMatchedPixel(pixelBase);

            DisparityMap.Set(pm.Y, pm.X, bestDisp);

            _dispForPixel = new List<Disparity>(2 * _dispForPixel.Count);
            _minIdx = 0;
            _min2Idx = 0;
            _minCost = double.PositiveInfinity;
            _min2Cost = double.PositiveInfinity;
        }

        /// <summary>
        /// Conjugates vector and save result to <paramref name="result"/>
        /// </summary>
        /// <param name="result">Target vector</param>
        protected override void DoConjugate(Vector<Complex32> result)
        {
            var sparseResult = result as SparseVector;
            if (sparseResult != null)
            {
                if (!ReferenceEquals(this, result))
                {
                    sparseResult._storage.ValueCount = _storage.ValueCount;
                    sparseResult._storage.Indices = new int[_storage.ValueCount];
                    Buffer.BlockCopy(_storage.Indices, 0, sparseResult._storage.Indices, 0, _storage.ValueCount*Constants.SizeOfInt);
                    sparseResult._storage.Values = new Complex32[_storage.ValueCount];
                    Array.Copy(_storage.Values, sparseResult._storage.Values, _storage.ValueCount);
                }

                Control.LinearAlgebraProvider.ConjugateArray(sparseResult._storage.Values, sparseResult._storage.Values);
                return;
            }

            result.Clear();
            for (var index = 0; index < _storage.ValueCount; index++)
            {
                result.At(_storage.Indices[index], _storage.Values[index].Conjugate());
            }
        }

        /// <summary>
        /// Multiplies the conjugate transpose of this matrix with a vector and places the results into the result vector.
        /// </summary>
        /// <param name="rightSide">The vector to multiply with.</param>
        /// <param name="result">The result of the multiplication.</param>
        protected override void DoConjugateTransposeThisAndMultiply(Vector<Complex32> rightSide, Vector<Complex32> result)
        {
            var d = Math.Min(ColumnCount, RowCount);
            if (d < ColumnCount)
            {
                result.ClearSubVector(RowCount, ColumnCount - RowCount);
            }

            if (d == RowCount)
            {
                var denseOther = rightSide.Storage as DenseVectorStorage<Complex32>;
                var denseResult = result.Storage as DenseVectorStorage<Complex32>;
                if (denseOther != null && denseResult != null)
                {
                    // TODO: merge/MulByConj
                    Control.LinearAlgebraProvider.ConjugateArray(_data, denseResult.Data);
                    Control.LinearAlgebraProvider.PointWiseMultiplyArrays(denseResult.Data, denseOther.Data, denseResult.Data);
                    return;
                }
            }

            for (var i = 0; i < d; i++)
            {
                result.At(i, _data[i].Conjugate()*rightSide.At(i));
            }
        }

        private void FindDisparitiesCosts(IntVector2 pb, Vector<double> epiLine)
        {
            epiLine.DivideThis(epiLine.At(1));
            IntVector2 pm = new IntVector2();
            int xmax = FindXmax(epiLine);
            int x0 = FindX0(epiLine);
            xmax = xmax - 1;

            for(int xm = x0 + 1; xm < xmax; ++xm)
            {
                double ym0 = FindYd(xm, epiLine);
                double ym1 = FindYd(xm + 1, epiLine);
                for(int ym = (int)ym0; ym < (int)ym1; ++ym)
                {
                    if(ImageMatched.HaveValueAt(ym, xm))
                    {
                        pm.X = xm;
                        pm.Y = ym;
                        double cost = CostComp.GetCost_Border(CurrentPixel, pm);
                        DispComp.StoreDisparity(CurrentPixel, pm, cost);
                    }
                }
            }
        }

        /// <summary>
        /// Subtracts another vector to this vector and stores the result into the result vector.
        /// </summary>
        /// <param name="other">
        /// The vector to subtract from this one.
        /// </param>
        /// <param name="result">
        /// The vector to store the result of the subtraction.
        /// </param>
        protected override void DoSubtract(Vector<Complex32> other, Vector<Complex32> result)
        {
            if (ReferenceEquals(this, other))
            {
                result.Clear();
                return;
            }

            var otherSparse = other as SparseVector;
            if (otherSparse == null)
            {
                base.DoSubtract(other, result);
                return;
            }

            var resultSparse = result as SparseVector;
            if (resultSparse == null)
            {
                base.DoSubtract(other, result);
                return;
            }

            // TODO (ruegg, 2011-10-11): Options to optimize?

            if (ReferenceEquals(this, resultSparse))
            {
                int i = 0, j = 0;
                while (i < NonZerosCount || j < otherSparse.NonZerosCount)
                {
                    if (i < NonZerosCount && j < otherSparse.NonZerosCount && _nonZeroIndices[i] == otherSparse._nonZeroIndices[j])
                    {
                        _nonZeroValues[i++] -= otherSparse._nonZeroValues[j++];
                    }
                    else if (j >= otherSparse.NonZerosCount || i < NonZerosCount && _nonZeroIndices[i] < otherSparse._nonZeroIndices[j])
                    {
                        i++;
                    }
                    else
                    {
                        var otherValue = otherSparse._nonZeroValues[j];
                        if (otherValue != Complex32.Zero)
                        {
                            InsertAtUnchecked(i++, otherSparse._nonZeroIndices[j], -otherValue);
                        }
                        j++;
                    }
                }
            }
            else
            {
                result.Clear();
                int i = 0, j = 0, last = -1;
                while (i < NonZerosCount || j < otherSparse.NonZerosCount)
                {
                    if (j >= otherSparse.NonZerosCount || i < NonZerosCount && _nonZeroIndices[i] <= otherSparse._nonZeroIndices[j])
                    {
                        var next = _nonZeroIndices[i];
                        if (next != last)
                        {
                            last = next;
                            result.At(next, _nonZeroValues[i] - otherSparse.At(next));
                        }
                        i++;
                    }
                    else
                    {
                        var next = otherSparse._nonZeroIndices[j];
                        if (next != last)
                        {
                            last = next;
                            result.At(next, At(next) - otherSparse._nonZeroValues[j]);
                        }
                        j++;
                    }
                }
            }
        }