depth_to_pointcloud_node.cpp

#include <ros/package.h>
#include <ros/ros.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud.h>
#include <atomic>
#include <mutex>
#include <string>

/**
 * Encodes a 3bit signed index and 13bit depth pixel values covering up to 8m
 * depth with 1mm resolution.
 *
 * \note This is just to show how the pixel encoding was done. This function is
 * not used in here.
 */
uint16_t encode_pixel(int16_t object_index, uint16_t pixel_depth)
{
    return object_index << 13 | 0x1FFF & pixel_depth;
}

/**
 * \brief Decodes the given pixel value into a 3 bit object index and a 13 bit
 * depth value. The object index may take up to 7 values for up to 6 objects
 * and -1 for unassociated points.
 */
void decode_pixel(uint16_t pixel_value, int16_t& object_index, uint16_t& depth)
{
    depth = 0x1FFF & pixel_value;
    object_index = pixel_value >> 13;
    object_index = object_index == 0x7 ? -1 : object_index;
}

float index_to_color(int16_t object_index)
{
    float c = 0;
    // (R,G,B) in LSB required by rviz ...
    unsigned char* lsb_c_enc = reinterpret_cast<unsigned char*>(&c);
    unsigned char& red = lsb_c_enc[2];
    unsigned char& green = lsb_c_enc[1];
    unsigned char& blue = lsb_c_enc[0];
    switch (object_index)
    {
        case -1:
            red = 0;
            green = 0;
            blue = 0;
            break;
        case 0:
            red = 190;
            green = 190;
            blue = 190;
            break;
        case 1:
            red = 112;
            green = 211;
            blue = 69;
            break;
        default:
            red = 250;
            green = 160;
            blue = 0;
    };
    return c;
}

class DepthToPointcloud
{
  public:
    DepthToPointcloud(const std::string& encoded_depth_image_topic,
                      const std::string& decoded_pointcloud_topic,
                      const std::string& depth_camera_info_topic)
        : depth_camera_info_available_(false)
    {
        ros::NodeHandle nh;
        depth_image_subscriber_ =
            nh.subscribe(encoded_depth_image_topic,
                         10,
                         &DepthToPointcloud::depth_image_callback,
                         this);

        pointcloud_publisher_ =
            nh.advertise<sensor_msgs::PointCloud>(decoded_pointcloud_topic, 10);

        wait_for_camera_info(depth_camera_info_topic);
    }

  private:
    geometry_msgs::Point32 project_depth(int index, uint16_t depth_in_mm)
    {
        geometry_msgs::Point32 point;
        float u = index % width_;
        float v = index / width_;
        float depth = depth_in_mm / 1000.f;
        point.x = ((u - px_) / fx_) * depth;
        point.y = ((v - py_) / fy_) * depth;
        point.z = depth;
        return point;
    }

    void depth_image_callback(const sensor_msgs::Image::ConstPtr& image_msg)
    {
        sensor_msgs::ChannelFloat32 depth_channel;
        sensor_msgs::PointCloud pointcloud;

        pointcloud.header = image_msg->header;
        depth_channel.name = "rgb";
        const uint16_t* data =
            reinterpret_cast<const uint16_t*>(&image_msg->data[0]);

        const int point_count = width_ * height_;
        for (int i = 0; i < point_count; ++i)
        {
            int16_t object_index;
            uint16_t depth_in_mm;
            decode_pixel(data[i], object_index, depth_in_mm);
            if (depth_in_mm)
            {
                pointcloud.points.push_back(project_depth(i, depth_in_mm));
                depth_channel.values.push_back(index_to_color(object_index));
            }
        }
        pointcloud.channels.push_back(depth_channel);
        pointcloud_publisher_.publish(pointcloud);
    }
    void depth_camera_info_callback(
        const sensor_msgs::CameraInfo::ConstPtr& msg)
    {
        sensor_msgs::CameraInfo depth_camera_info = *msg;
        fx_ = depth_camera_info.K[0];
        fy_ = depth_camera_info.K[4];
        px_ = depth_camera_info.K[2];
        py_ = depth_camera_info.K[5];
        width_ = depth_camera_info.width;
        height_ = depth_camera_info.height;
        depth_camera_info_available_ = true;
    }

    void wait_for_camera_info(const std::string& depth_camera_info_topic)
    {
        ros::NodeHandle nh;
        ros::Subscriber depth_camera_info_subscriber =
            nh.subscribe(depth_camera_info_topic,
                         1000,
                         &DepthToPointcloud::depth_camera_info_callback,
                         this);

        ROS_INFO("Waiting for camera info ...");
        auto rate = ros::Rate(100);
        while (ros::ok() && !depth_camera_info_available_.load())
        {
            rate.sleep();
            ros::spinOnce();
        }

        if (!depth_camera_info_available_.load())
        {
            std::cout << "ROS shutdown before receiving camera info."
                      << std::endl;
            exit(0);
        }
        ROS_INFO("Received camera info");
    }

  private:
    ros::Subscriber depth_image_subscriber_;
    ros::Publisher pointcloud_publisher_;
    std::atomic<bool> depth_camera_info_available_;
    int width_;
    int height_;
    float fx_;
    float fy_;
    float px_;
    float py_;
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "associated_point_cloud_visualizer");
    ros::NodeHandle nh;

    DepthToPointcloud depth_to_pointcloud(
        "/debug/tracker/associated_depth_image",
        "/debug/tracker/associated_point_cloud",
        "/camera/depth_registered/sw_registered/camera_info");

    ROS_INFO("DepthToPointcloud up and running ...");
    ros::spin();

    return 0;
}