问基于特征矩阵库的C++码翻译

EN

template <typename PointT> inline unsigned int
pcl::SamplingSurfaceNormal<PointT>::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
                                                                    Eigen::Matrix3f &covariance_matrix,
                                                                    Eigen::Vector4f &centroid)
{
    // create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
    Eigen::Matrix<float, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 9, Eigen::RowMajor>::Zero ();
    std::size_t point_count = 0;
    for (std::size_t i = 0; i < cloud.points.size (); i++)
    {
        if (!isFinite (cloud[i]))
        {
          continue;
        }

        ++point_count;
        accu [0] += cloud[i].x * cloud[i].x;
        accu [1] += cloud[i].x * cloud[i].y;
        accu [2] += cloud[i].x * cloud[i].z;
        accu [3] += cloud[i].y * cloud[i].y; // 4
        accu [4] += cloud[i].y * cloud[i].z; // 5
        accu [5] += cloud[i].z * cloud[i].z; // 8
        accu [6] += cloud[i].x;
        accu [7] += cloud[i].y;
        accu [8] += cloud[i].z;
    }

    accu /= static_cast<float> (point_count);
    centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
    centroid[3] = 0;
    covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];
    covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];
    covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];
    covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];
    covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];
    covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];
    covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);
    covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);
    covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);

    return (static_cast<unsigned int> (point_count));
}

private int ComputeMeanAndCovarianceMatrix(
    PointCloud cloud,
    Matrix<float> covariance_matrix,
    MathNet.Numerics.LinearAlgebra.Vector<float> centroid)
{
    int point_count = 0;
    Matrix<float> accu = Matrix<float>.Build.DenseOfRowMajor(1, 9, Enumerable.Repeat(0.0f, 9));

    for (int i = 0; i < cloud.Vertices.Length; ++i)
    {
        //if (!isFinite(cloud.Vertices[i].Point.))
        //{
        //  continue;
        //}

        ++point_count;
        accu[0, 0] += cloud.Vertices[i].Point.X * cloud.Vertices[i].Point.X;
        accu[0, 1] += cloud.Vertices[i].Point.X * cloud.Vertices[i].Point.Y;
        accu[0, 2] += cloud.Vertices[i].Point.X * cloud.Vertices[i].Point.Z;
        accu[0, 3] += cloud.Vertices[i].Point.Y * cloud.Vertices[i].Point.Y; // 4
        accu[0, 4] += cloud.Vertices[i].Point.Y * cloud.Vertices[i].Point.Z; // 5
        accu[0, 5] += cloud.Vertices[i].Point.Z * cloud.Vertices[i].Point.Z; // 8
        accu[0, 6] += cloud.Vertices[i].Point.X;
        accu[0, 7] += cloud.Vertices[i].Point.Y;
        accu[0, 8] += cloud.Vertices[i].Point.Z;
    }
    accu /= point_count;

    centroid[0] = accu[0, 6];
    centroid[1] = accu[0, 7];
    centroid[2] = accu[0, 8];
    centroid[3] = 0;

    covariance_matrix[0, 0] = accu[0, 0] - accu[0, 6] * accu[0, 6];
    covariance_matrix[0, 1] = accu[0, 1] - accu[0, 6] * accu[0, 7];
    covariance_matrix[0, 2] = accu[0, 2] - accu[0, 6] * accu[0, 8];
    covariance_matrix[1, 1] = accu[0, 3] - accu[0, 7] * accu[0, 7];
    covariance_matrix[1, 2] = accu[0, 4] - accu[0, 7] * accu[0, 8];
    covariance_matrix[2, 2] = accu[0, 5] - accu[0, 8] * accu[0, 8];
    covariance_matrix[1, 0] = covariance_matrix[0, 1];
    covariance_matrix[2, 0] = covariance_matrix[0, 2];
    covariance_matrix[2, 1] = covariance_matrix[1, 2];

    return point_count;
}