class mola::RgbdAssembler

Overview

Builds mrpt::obs::CObservation3DRangeScan observations out of the ROS topics of an RGB-D camera: a depth image, an optional intensity image (color or IR), and the camera_info of each.

Depth and intensity frames are paired by header stamp. The observation keeps the raw sensor geometry, following the MRPT conventions:

• rangeImage and cameraParams : the depth image and its intrinsics.

• intensityImage and cameraParamsIntensity : the intensity image and its intrinsics, which may differ from the depth ones.

• relativePoseIntensityWRTDepth : from /tf (depth -> intensity optical frames). Depth that is already registered to the intensity camera (e.g. a RealSense “aligned_depth_to_color” topic) yields doDepthAndIntensityCamerasCoincide() == true.

• sensorPose : the depth camera on the robot, in the MRPT +X-forward convention, from /tf or from a fixed pose.

Depth encodings: 16UC1/mono16 (millimeters, REP 118) and 32FC1 (meters).

#include <RgbdAssembler.h>

class RgbdAssembler
{
public:
    // typedefs

    typedef std::vector<mrpt::obs::CObservation::Ptr> Obs;

    // structs

    struct Parameters;
    struct Pending;

    // construction

    RgbdAssembler(
        Parameters p,
        std::shared_ptr<const tf2::BufferCore> tf,
        std::string baseLinkFrame
        );

    // methods

    void onDepthCameraInfo(const sensor_msgs::msg::CameraInfo& info);
    void onIntensityCameraInfo(const sensor_msgs::msg::CameraInfo& info);
    Obs onDepthImage(sensor_msgs::msg::Image&& img);
    Obs onIntensityImage(sensor_msgs::msg::Image&& img);
    size_t droppedUnpaired() const;
    size_t droppedWithoutCameraInfo() const;
    double lastEvictedClosestDt() const;

    static void decodeDepth(
        const sensor_msgs::msg::Image& img,
        mrpt::obs::CObservation3DRangeScan& obs
        );

    static mrpt::img::TCamera cameraFromROS(const sensor_msgs::msg::CameraInfo& info);
    static const mrpt::poses::CPose3D& opticalFrameInSensorFrame();
};

Methods

Obs onDepthImage(sensor_msgs::msg::Image&& img)

Returns the assembled observation, if this image completes a pair.

size_t droppedUnpaired() const

Images evicted from the waiting queues without a counterpart.

size_t droppedWithoutCameraInfo() const

Frames dropped because their camera_info had not arrived yet.

double lastEvictedClosestDt() const

Smallest stamp difference seen for the last evicted image [s].

static void decodeDepth(
    const sensor_msgs::msg::Image& img,
    mrpt::obs::CObservation3DRangeScan& obs
    )

Decodes a ROS depth image into obs.rangeImage and obs.rangeUnits.

static mrpt::img::TCamera cameraFromROS(const sensor_msgs::msg::CameraInfo& info)

Intrinsics and lens distortion from a sensor_msgs/CameraInfo.

static const mrpt::poses::CPose3D& opticalFrameInSensorFrame()

Pose of a camera optical frame (+Z forward, ROS REP 103) in the MRPT 3D range scan sensor frame (+X forward): (0,0,0,-90deg,0,-90deg).