Example #1
0
void transformMeans(
    Vector3fVector& means,
    const AffineTransform& transform)
{
    Matrix3f R = transform.rotation();
    Vector3f t = transform.translation();

    unsigned int size = means.size();
    for(unsigned int i = 0; i < size; ++i)
    {
        Vector3f& m = means[i];
        m = R * m + t;
    }
}
Example #2
0
void transformDistributions(
    Vector3fVector& means,
    Matrix3fVector& covariances,
    const AffineTransform& transform)
{
    Matrix3f R = transform.rotation();
    Vector3f t = transform.translation();
    Matrix3f R_T = R.transpose();

    unsigned int size = means.size();
    for(unsigned int i = 0; i < size; ++i)
    {
        Vector3f& m = means[i];
        Matrix3f& c = covariances[i];
        m = R * m + t;
        c = R * c * R_T;
    }
}
Example #3
0
void pointCloudFromMeans(
    const Vector3fVector& means,
    PointCloudFeature& cloud)
{
    unsigned int size = means.size();
    cloud.points.resize(size);
    for(unsigned int i = 0; i < size; ++i)
    {
        const Vector3f& m = means[i];
        PointFeature& p = cloud.points[i];

        p.x = m(0,0);
        p.y = m(1,0);
        p.z = m(2,0);
    }

    cloud.height = 1;
    cloud.width = size;
    cloud.is_dense = true;
}