Method project

sloam/src/objects/plane.cpp:153–176

}

void Plane::project(const SE3 &tf)
{
    // features
    Matrix4 tfm = tf.matrix();
    Affine3 affine(tfm);
    for (std::size_t i = 0; i < features.size(); ++i)
    {
        if (!std::isfinite(features[i].x) ||
            !std::isfinite(features[i].y) ||
            !std::isfinite(features[i].z))
            continue;
        features[i] = pcl::transformPoint(features[i], affine);
    }

    // model
    model.plane = tfm.inverse().transpose() * model.plane;
    model.centroid = tf * model.centroid;
    // ROS_DEBUG_STREAM("model.plane:" << model.plane);
    // ROS_DEBUG_STREAM("model.centroid:" << model.centroid);

    // computeModel();
    // ROS_DEBUG_STREAM("model.plane:" << model.plane);
    // ROS_DEBUG_STREAM("model.centroid:" << model.centroid);
}