<commit_before>/////////////////////////////////////////////////////////
/// Copyright (c) 2014, <NAME>, WPI ///
/////////////////////////////////////////////////////////
// System include files
#include <stdio.h>
#include <stdint.h>
#include <signal.h>
#include <vector>
#include <exception>
#include <iostream>
#include <iomanip>
#include <fstream>
// ROS include files
#include <ros/ros.h>
#include <resource_retriever/retriever.h>
// Pointcloud include files
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/io/pcd_io.h>
#include <pcl/io/io.h>
#include <pcl/point_types.h>
// OpenCV + camera include files
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <sensor_msgs/image_encodings.h>
#include <camera_calibration_parsers/parse_yml.h>
#include <camera_calibration_parsers/parse_ini.h>
// Camera SDK include files
#include <DepthSense.hxx>

// Macro to disable unused parameter compiler warnings
#define UNUSED(x) (void)(x)

typedef struct
{
    DepthSense::ColorNode::Configuration base_config;
    DepthSense::ExposureAuto auto_exposure_mode;
    int32_t brightness;
    int32_t contrast;
    int32_t saturation;
    int32_t hue;
    int32_t gamma;
    int32_t white_balance;
    int32_t sharpness;
    int32_t exposure;
    bool enable_priority_auto_exposure_mode;
    bool enable_auto_white_balance;
    bool enable_color_map;
    bool enable_compressed_data;
} complete_color_config;

typedef struct
{
    DepthSense::DepthNode::Configuration base_config;
    int32_t illumination_level;
    bool enable_xyz_depth;
    bool enable_floatingpoint_xyz_depth;
    bool enable_accelerometer;
    bool enable_confidence_map;
    bool enable_depth_map;
    bool enable_floatingpoint_depth_map;
    bool enable_phase_map;
    bool enable_uv_map;
    bool enable_vertices;
    bool enable_floatingpoint_vertices;
} complete_depth_config;

// DepthSense SDK global variables
DepthSense::Context g_context;
DepthSense::ColorNode g_color_node;
DepthSense::DepthNode g_depth_node;
complete_color_config g_color_config;
complete_depth_config g_depth_config;
// ROS global variables
ros::Publisher g_pointcloud_pub;
ros::Publisher g_rgb_pointcloud_pub;
ros::Publisher g_uv_pointcloud_pub;
image_transport::CameraPublisher g_rgb_pub;
image_transport::CameraPublisher g_depth_pub;
image_transport::CameraPublisher g_index_pub;
image_transport::CameraPublisher g_registered_depth_pub;
// Config global variables
std::string g_rgb_frame_name;
std::string g_rgb_optical_frame_name;
std::string g_depth_frame_name;
std::string g_depth_optical_frame_name;
bool g_rgb_camerainfo_set = false;
bool g_depth_camerainfo_set = false;
sensor_msgs::CameraInfo g_rgb_camerainfo;
sensor_msgs::CameraInfo g_depth_camerainfo;
// Config for the confidence filter
int32_t g_confidence_threshold = 0;
// Save the current color map for coloring pointclouds
cv::Mat g_current_color_image;

void dealocate_pcl_pointcloud_fn(pcl::PointCloud<pcl::PointXYZ>* p)
{
    UNUSED(p);
}

void dealocate_pcl_rgb_pointcloud_fn(pcl::PointCloud<pcl::PointXYZRGB>* p)
{
    UNUSED(p);
}

void SetupCameraInfo(sensor_msgs::CameraInfo& camera_info, const DepthSense::IntrinsicParameters& camera_parameters)
{
    // Set general camera model information
    camera_info.distortion_model = "plumb_bob";
    camera_info.height = camera_parameters.height;
    camera_info.width = camera_parameters.width;
    // Set distortion parameters "D" = [k1, k2, t1, t2, k3] <- for the SoftKinetic API, p1 == t1, p2 == t2
    camera_info.D = {camera_parameters.k1, camera_parameters.k2, camera_parameters.p1, camera_parameters.p2, camera_parameters.k3};
    // Set camera intrinsic matrix "K"
    camera_info.K = { {camera_parameters.fx, 0.0, camera_parameters.cx,
                       0.0, camera_parameters.fy, camera_parameters.cy,
                       0.0,                  0.0,                  1.0} };
    // Set camera rectification matrix "R"
    camera_info.R = { {1.0, 0.0, 0.0,
                       0.0, 1.0, 0.0,
                       0.0, 0.0, 1.0} };
    // Set projection/camera matrix
    // For monocular cameras like ours, Tx = Ty = 0
    // NOTE - this uses the same values as the raw camera, and assumes no correction is applied
    camera_info.P = { {camera_parameters.fx, 0.0, camera_parameters.cx, 0.0,
                       0.0, camera_parameters.fy, camera_parameters.cy, 0.0,
                       0.0,                  0.0,                  1.0, 0.0} };
}

void OnNewColorSample(DepthSense::ColorNode node, DepthSense::ColorNode::NewSampleReceivedData data)
{
    UNUSED(node);
    // Make new OpenCV container
    int32_t width = 0;
    int32_t height = 0;
    DepthSense::FrameFormat_toResolution(data.captureConfiguration.frameFormat, &width, &height);
    cv::Mat new_image_bgr8(height, width, CV_8UC3);
    // If the image is in YUY2 mode, we need to convert it to BGR8 first
    if (data.captureConfiguration.compression == DepthSense::COMPRESSION_TYPE_YUY2)
    {
        // Make the intermediate container for the YUY2 image
        cv::Mat new_image_yuy2(height, width, CV_8UC2);
        // Copy the data in
        new_image_yuy2.data = const_cast<u_int8_t*>(static_cast<const u_int8_t*>(data.colorMap));
        // Convert to BGR
        cv::cvtColor(new_image_yuy2, new_image_bgr8, CV_YUV2BGR_YUY2);
    }
    // If the image is in MJPEG mode, it's already in BGR8
    else
    {
        // Copy the data in
        new_image_bgr8.data = const_cast<u_int8_t*>(static_cast<const u_int8_t*>(data.colorMap));
    }
    // Save the current color image
    g_current_color_image = new_image_bgr8.clone();
    // Convert the OpenCV image to ROS
    std_msgs::Header new_image_header;
    new_image_header.frame_id = g_rgb_optical_frame_name;
    new_image_header.stamp = ros::Time::now();
    sensor_msgs::Image new_image;
    cv_bridge::CvImage new_image_converted(new_image_header, sensor_msgs::image_encodings::BGR8, new_image_bgr8);
    new_image_converted.toImageMsg(new_image);
    if (g_rgb_camerainfo_set == true)
    {
        sensor_msgs::CameraInfo new_image_camerainfo = g_rgb_camerainfo;
        new_image_camerainfo.header = new_image_header;
        // Publish the image
        g_rgb_pub.publish(new_image, new_image_camerainfo);
    }
    else
    {
        ROS_WARN("No RGB CameraInfo set, not publishing RGB image");
    }
    if (!ros::ok())
    {
        g_context.quit();
    }
}

inline bool is_vertex_valid(float x, float y, float z)
{
    if (x != -2.0 || y != -2.0 || z != -2.0)
    {
        return true;
    }
    else
    {
        return false;
    }
}

inline bool is_uv_valid(float u, float v)
{
    if (u != -FLT_MAX || v != -FLT_MAX)
    {
        return true;
    }
    else
    {
        return false;
    }
}

void OnNewDepthSample(DepthSense::DepthNode node, DepthSense::DepthNode::NewSampleReceivedData data)
{
    UNUSED(node);
    ros::Time depth_timestamp = ros::Time::now();
    // Setup the camerainfo messages if they aren't already populated
    if (g_rgb_camerainfo_set == false)
    {
        SetupCameraInfo(g_rgb_camerainfo, data.stereoCameraParameters.colorIntrinsics);
        g_rgb_camerainfo_set = true;
        ROS_INFO("Set RGB CameraInfo with parameters provided by the camera");
    }
    if (g_depth_camerainfo_set == false)
    {
        SetupCameraInfo(g_depth_camerainfo, data.stereoCameraParameters.depthIntrinsics);
        g_depth_camerainfo_set = true;
        ROS_INFO("Set Depth CameraInfo with parameters provided by the camera");
    }
    // First, we generate the raw depth image
    int32_t width = 0;
    int32_t height = 0;
    DepthSense::FrameFormat_toResolution(data.captureConfiguration.frameFormat, &width, &height);
    // Copy and build the depth image
    float* raw_depth_floats = const_cast<float*>(static_cast<const float*>(data.depthMapFloatingPoint));
    std::vector<float> depth_floats(raw_depth_floats, raw_depth_floats + (width * height));
    cv::Mat new_image_depth = cv::Mat(depth_floats).reshape(1, height);
    // Copy and build the confidences
    int16_t* raw_confidence_shorts = const_cast<int16_t*>(static_cast<const int16_t*>(data.confidenceMap));
    std::vector<int16_t> confidence_shorts(raw_confidence_shorts, raw_confidence_shorts + (width * height));
    // Filter the depth image
    cv::Mat new_image_depth_filtered(new_image_depth.rows, new_image_depth.cols, CV_32FC1);
    cv::Mat depth_mean_kernel(3, 3, CV_32FC1);
    depth_mean_kernel.setTo(1.0);
    cv::filter2D(new_image_depth, new_image_depth_filtered, CV_32FC1, depth_mean_kernel);
    // Convert the OpenCV depth image to ROS
    std_msgs::Header new_image_header;
    new_image_header.frame_id = g_depth_optical_frame_name;
    new_image_header.stamp = depth_timestamp;
    sensor_msgs::Image new_depth_image;
    cv_bridge::CvImage new_depth_image_converted(new_image_header, sensor_msgs::image_encodings::TYPE_32FC1, new_image_depth_filtered);
    new_depth_image_converted.toImageMsg(new_depth_image);
    sensor_msgs::CameraInfo new_depth_image_camerainfo = g_depth_camerainfo;
    new_depth_image_camerainfo.header = new_image_header;
    // Publish the image
    g_depth_pub.publish(new_depth_image, new_depth_image_camerainfo);
    // Second, generate the pointcloud
    DepthSense::FPVertex* raw_vertices = const_cast<DepthSense::FPVertex*>(static_cast<const DepthSense::FPVertex*>(data.verticesFloatingPoint));
    std::vector<DepthSense::FPVertex> vertices(raw_vertices, raw_vertices + (width * height));
    // Make the XYZ pointcloud
    pcl::PointCloud<pcl::PointXYZI> pcl_pointcloud;
    for (size_t idx = 0; idx < vertices.size(); idx++)
    {
        float x = vertices[idx].x;
        float y = -vertices[idx].y;
        float z = vertices[idx].z;
        if (is_vertex_valid(x, y, z))
        {
            // Filter based on confidence
            int32_t confidence = (int32_t)confidence_shorts[idx];
            if (confidence >= g_confidence_threshold)
            {
                pcl::PointXYZI new_point;
                new_point.x = x;
                new_point.y = y;
                new_point.z = z;
                new_point.intensity = (float)confidence;
                pcl_pointcloud.push_back(new_point);
            }
        }
    }
    // Publish the cloud without filtering
    // Convert the XYZ pointcloud to ROS message
    sensor_msgs::PointCloud2 ros_pointcloud;
    pcl::toROSMsg(pcl_pointcloud, ros_pointcloud);
    ros_pointcloud.header.frame_id = g_depth_optical_frame_name;
    ros_pointcloud.header.stamp = new_image_header.stamp;
    g_pointcloud_pub.publish(ros_pointcloud);
    // Make the XYZRGB pointcloud (if enabled)
    if (g_color_config.enable_color_map && !g_current_color_image.empty())
    {
        // Get the UV map that maps the color map and vertices together
        DepthSense::UV* raw_uv = const_cast<DepthSense::UV*>(static_cast<const DepthSense::UV*>(data.uvMap));
        std::vector<DepthSense::UV> uv(raw_uv, raw_uv + (width * height));
        // Make the XYZRGB pointcloud
        pcl::PointCloud<pcl::PointXYZRGB> pcl_rgb_pointcloud;
        // Make the matching UV map
        pcl::PointCloud<pcl::PointUV> pcl_uv_pointcloud;
        for (size_t idx = 0; idx < vertices.size(); idx++)
        {
            float x = vertices[idx].x;
            float y = -vertices[idx].y;
            float z = vertices[idx].z;
            float u = uv[idx].u;
            float v = uv[idx].v;
            if (is_vertex_valid(x, y, z) && is_uv_valid(u, v))
            {
                // Lookup RGB colors
                size_t image_height = (size_t)(v * g_current_color_image.rows);
                size_t image_width = (size_t)(u * g_current_color_image.cols);
                cv::Vec3b pixel = g_current_color_image.at<cv::Vec3b>(image_height, image_width);
                uint8_t blue = pixel[0];
                uint8_t green = pixel[1];
                uint8_t red = pixel[2];
                // Filter based on confidence
                int32_t confidence = (int32_t)confidence_shorts[idx];
                if (confidence >= g_confidence_threshold)
                {
                    // Make the point
                    pcl::PointXYZRGB new_xyzrgb_point;
                    new_xyzrgb_point.x = x;
                    new_xyzrgb_point.y = y;
                    new_xyzrgb_point.z = z;
                    new_xyzrgb_point.r = red;
                    new_xyzrgb_point.g = green;
                    new_xyzrgb_point.b = blue;
                    pcl_rgb_pointcloud.push_back(new_xyzrgb_point);
                    // Make the UV
                    pcl::PointUV new_uv_point;
                    new_uv_point.u = u;
                    new_uv_point.v = v;
                    pcl_uv_pointcloud.push_back(new_uv_point);
                }
            }
        }
        // Publish the cloud without filtering
        // Convert the XYZRGB pointcloud to ROS message
        sensor_msgs::PointCloud2 ros_rgb_pointcloud;
        pcl::toROSMsg(pcl_rgb_pointcloud, ros_rgb_pointcloud);
        ros_rgb_pointcloud.header.frame_id = g_depth_optical_frame_name;
        ros_rgb_pointcloud.header.stamp = depth_timestamp;
        g_rgb_pointcloud_pub.publish(ros_rgb_pointcloud);
        // Convert the UV pointcloud to ROS message
        sensor_msgs::PointCloud2 ros_uv_pointcloud;
        pcl::toROSMsg(pcl_uv_pointcloud, ros_uv_pointcloud);
        ros_uv_pointcloud.header.frame_id = g_depth_optical_frame_name;
        ros_uv_pointcloud.header.stamp = depth_timestamp;
        g_uv_pointcloud_pub.publish(ros_uv_pointcloud);
        // Make the "registered" depth image
        cv::Mat new_image_depth_registered(new_image_depth.rows, new_image_depth.cols, CV_32FC3, cv::Scalar(0.0, 0.0, 0.0));
        // Make the "index image" that maps pixels in the image to the corresponding index in the pointcloud
        cv::Mat new_image_depth_indices(new_image_depth.rows, new_image_depth.cols, CV_32SC1, -1);
        for (size_t idx = 0; idx < vertices.size(); idx++)
        {
            float x = vertices[idx].x;
            float y = -vertices[idx].y;
            float z = vertices[idx].z;
            float u = uv[idx].u;
            float v = uv[idx].v;
            if (is_vertex_valid(x, y, z) && is_uv_valid(u, v))
            {
                // Filter based on confidence
                int32_t confidence = (int32_t)confidence_shorts[idx];
                if (confidence >= g_confidence_threshold)
                {
                    // Get the matching position in the image
                    size_t image_height = (size_t)(v * new_image_depth_registered.rows);
                    size_t image_width = (size_t)(u * new_image_depth_registered.cols);
                    // Set that location in the image with the current vertex
                    new_image_depth_registered.at<cv::Vec3f>(image_height, image_width) = cv::Vec3f(x, y, z);
                    new_image_depth_indices.at<int32_t>(image_height, image_width) = (int32_t)idx;
                }
            }
        }
        // Convert the "registered" depth image to ROS and publish it
        std_msgs::Header new_registered_depth_image_header;
        new_registered_depth_image_header.frame_id = g_depth_optical_frame_name;
        new_registered_depth_image_header.stamp = depth_timestamp;
        sensor_msgs::Image new_registed_depth_image;
        cv_bridge::CvImage new_registered_depth_image_converted(new_registered_depth_image_header, sensor_msgs::image_encodings::TYPE_32FC3, new_image_depth_registered);
        new_registered_depth_image_converted.toImageMsg(new_registed_depth_image);
        sensor_msgs::CameraInfo new_registered_depth_image_camerainfo = g_depth_camerainfo;
        new_registered_depth_image_camerainfo.header = new_registered_depth_image_header;
        // Publish the image
        g_registered_depth_pub.publish(new_registed_depth_image, new_registered_depth_image_camerainfo);
        // Convert the "index image" to ROS and publish it
        std_msgs::Header new_indices_depth_image_header;
        new_indices_depth_image_header.frame_id = g_depth_optical_frame_name;
        new_indices_depth_image_header.stamp = depth_timestamp;
        sensor_msgs::Image new_indices_depth_image;
        cv_bridge::CvImage new_indices_depth_image_converted(new_indices_depth_image_header, sensor_msgs::image_encodings::TYPE_32SC1, new_image_depth_indices);
        new_indices_depth_image_converted.toImageMsg(new_indices_depth_image);
        sensor_msgs::CameraInfo new_indices_depth_image_camerainfo = g_depth_camerainfo;
        new_indices_depth_image_camerainfo.header = new_indices_depth_image_header;
        // Publish the image
        g_index_pub.publish(new_indices_depth_image, new_indices_depth_image_camerainfo);
    }
    else if (g_color_config.enable_color_map && g_current_color_image.empty())
    {
        ROS_WARN("XYZRGB pointclouds enabled, but no color image received yet. Not publishing XYZRGB pointcloud");
    }
    if (!ros::ok())
    {
        g_context.quit();
    }
}

void ConfigureDepthNode(DepthSense::DepthNode& depth_node)
{
    ROS_INFO("Configuring new DepthNode...");
    // Register the event handler
    depth_node.newSampleReceivedEvent().connect(&OnNewDepthSample);
    // Try to configure the node
    try
    {
        g_context.requestControl(depth_node, 0);
        depth_node.setConfiguration(g_depth_config.base_config);<commit_msg>Fixed bug in the index image creation, and added indices into both types of pointclouds<commit_after>/////////////////////////////////////////////////////////
/// Copyright (c) 2014, <NAME>, WPI ///
/////////////////////////////////////////////////////////
// System include files
#include <stdio.h>
#include <stdint.h>
#include <signal.h>
#include <vector>
#include <exception>
#include <iostream>
#include <iomanip>
#include <fstream>
// ROS include files
#include <ros/ros.h>
#include <resource_retriever/retriever.h>
// Pointcloud include files
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/io/pcd_io.h>
#include <pcl/io/io.h>
#include <pcl/point_types.h>
// OpenCV + camera include files
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <sensor_msgs/image_encodings.h>
#include <camera_calibration_parsers/parse_yml.h>
#include <camera_calibration_parsers/parse_ini.h>
// Camera SDK include files
#include <DepthSense.hxx>

// Macro to disable unused parameter compiler warnings
#define UNUSED(x) (void)(x)

typedef struct
{
    DepthSense::ColorNode::Configuration base_config;
    DepthSense::ExposureAuto auto_exposure_mode;
    int32_t brightness;
    int32_t contrast;
    int32_t saturation;
    int32_t hue;
    int32_t gamma;
    int32_t white_balance;
    int32_t sharpness;
    int32_t exposure;
    bool enable_priority_auto_exposure_mode;
    bool enable_auto_white_balance;
    bool enable_color_map;
    bool enable_compressed_data;
} complete_color_config;

typedef struct
{
    DepthSense::DepthNode::Configuration base_config;
    int32_t illumination_level;
    bool enable_xyz_depth;
    bool enable_floatingpoint_xyz_depth;
    bool enable_accelerometer;
    bool enable_confidence_map;
    bool enable_depth_map;
    bool enable_floatingpoint_depth_map;
    bool enable_phase_map;
    bool enable_uv_map;
    bool enable_vertices;
    bool enable_floatingpoint_vertices;
} complete_depth_config;

// DepthSense SDK global variables
DepthSense::Context g_context;
DepthSense::ColorNode g_color_node;
DepthSense::DepthNode g_depth_node;
complete_color_config g_color_config;
complete_depth_config g_depth_config;
// ROS global variables
ros::Publisher g_pointcloud_pub;
ros::Publisher g_rgb_pointcloud_pub;
ros::Publisher g_uv_pointcloud_pub;
image_transport::CameraPublisher g_rgb_pub;
image_transport::CameraPublisher g_depth_pub;
image_transport::CameraPublisher g_index_pub;
image_transport::CameraPublisher g_registered_depth_pub;
// Config global variables
std::string g_rgb_frame_name;
std::string g_rgb_optical_frame_name;
std::string g_depth_frame_name;
std::string g_depth_optical_frame_name;
bool g_rgb_camerainfo_set = false;
bool g_depth_camerainfo_set = false;
sensor_msgs::CameraInfo g_rgb_camerainfo;
sensor_msgs::CameraInfo g_depth_camerainfo;
// Config for the confidence filter
int32_t g_confidence_threshold = 0;
// Save the current color map for coloring pointclouds
cv::Mat g_current_color_image;

void dealocate_pcl_pointcloud_fn(pcl::PointCloud<pcl::PointXYZ>* p)
{
    UNUSED(p);
}

void dealocate_pcl_rgb_pointcloud_fn(pcl::PointCloud<pcl::PointXYZRGB>* p)
{
    UNUSED(p);
}

void SetupCameraInfo(sensor_msgs::CameraInfo& camera_info, const DepthSense::IntrinsicParameters& camera_parameters)
{
    // Set general camera model information
    camera_info.distortion_model = "plumb_bob";
    camera_info.height = camera_parameters.height;
    camera_info.width = camera_parameters.width;
    // Set distortion parameters "D" = [k1, k2, t1, t2, k3] <- for the SoftKinetic API, p1 == t1, p2 == t2
    camera_info.D = {camera_parameters.k1, camera_parameters.k2, camera_parameters.p1, camera_parameters.p2, camera_parameters.k3};
    // Set camera intrinsic matrix "K"
    camera_info.K = { {camera_parameters.fx, 0.0, camera_parameters.cx,
                       0.0, camera_parameters.fy, camera_parameters.cy,
                       0.0,                  0.0,                  1.0} };
    // Set camera rectification matrix "R"
    camera_info.R = { {1.0, 0.0, 0.0,
                       0.0, 1.0, 0.0,
                       0.0, 0.0, 1.0} };
    // Set projection/camera matrix
    // For monocular cameras like ours, Tx = Ty = 0
    // NOTE - this uses the same values as the raw camera, and assumes no correction is applied
    camera_info.P = { {camera_parameters.fx, 0.0, camera_parameters.cx, 0.0,
                       0.0, camera_parameters.fy, camera_parameters.cy, 0.0,
                       0.0,                  0.0,                  1.0, 0.0} };
}

void OnNewColorSample(DepthSense::ColorNode node, DepthSense::ColorNode::NewSampleReceivedData data)
{
    UNUSED(node);
    // Make new OpenCV container
    int32_t width = 0;
    int32_t height = 0;
    DepthSense::FrameFormat_toResolution(data.captureConfiguration.frameFormat, &width, &height);
    cv::Mat new_image_bgr8(height, width, CV_8UC3);
    // If the image is in YUY2 mode, we need to convert it to BGR8 first
    if (data.captureConfiguration.compression == DepthSense::COMPRESSION_TYPE_YUY2)
    {
        // Make the intermediate container for the YUY2 image
        cv::Mat new_image_yuy2(height, width, CV_8UC2);
        // Copy the data in
        new_image_yuy2.data = const_cast<u_int8_t*>(static_cast<const u_int8_t*>(data.colorMap));
        // Convert to BGR
        cv::cvtColor(new_image_yuy2, new_image_bgr8, CV_YUV2BGR_YUY2);
    }
    // If the image is in MJPEG mode, it's already in BGR8
    else
    {
        // Copy the data in
        new_image_bgr8.data = const_cast<u_int8_t*>(static_cast<const u_int8_t*>(data.colorMap));
    }
    // Save the current color image
    g_current_color_image = new_image_bgr8.clone();
    // Convert the OpenCV image to ROS
    std_msgs::Header new_image_header;
    new_image_header.frame_id = g_rgb_optical_frame_name;
    new_image_header.stamp = ros::Time::now();
    sensor_msgs::Image new_image;
    cv_bridge::CvImage new_image_converted(new_image_header, sensor_msgs::image_encodings::BGR8, new_image_bgr8);
    new_image_converted.toImageMsg(new_image);
    if (g_rgb_camerainfo_set == true)
    {
        sensor_msgs::CameraInfo new_image_camerainfo = g_rgb_camerainfo;
        new_image_camerainfo.header = new_image_header;
        // Publish the image
        g_rgb_pub.publish(new_image, new_image_camerainfo);
    }
    else
    {
        ROS_WARN("No RGB CameraInfo set, not publishing RGB image");
    }
    if (!ros::ok())
    {
        g_context.quit();
    }
}

inline bool is_vertex_valid(float x, float y, float z)
{
    if (x != -2.0 || y != -2.0 || z != -2.0)
    {
        return true;
    }
    else
    {
        return false;
    }
}

inline bool is_uv_valid(float u, float v)
{
    if (u != -FLT_MAX || v != -FLT_MAX)
    {
        return true;
    }
    else
    {
        return false;
    }
}

void OnNewDepthSample(DepthSense::DepthNode node, DepthSense::DepthNode::NewSampleReceivedData data)
{
    UNUSED(node);
    ros::Time depth_timestamp = ros::Time::now();
    // Setup the camerainfo messages if they aren't already populated
    if (g_rgb_camerainfo_set == false)
    {
        SetupCameraInfo(g_rgb_camerainfo, data.stereoCameraParameters.colorIntrinsics);
        g_rgb_camerainfo_set = true;
        ROS_INFO("Set RGB CameraInfo with parameters provided by the camera");
    }
    if (g_depth_camerainfo_set == false)
    {
        SetupCameraInfo(g_depth_camerainfo, data.stereoCameraParameters.depthIntrinsics);
        g_depth_camerainfo_set = true;
        ROS_INFO("Set Depth CameraInfo with parameters provided by the camera");
    }
    // First, we generate the raw depth image
    int32_t width = 0;
    int32_t height = 0;
    DepthSense::FrameFormat_toResolution(data.captureConfiguration.frameFormat, &width, &height);
    // Copy and build the depth image
    float* raw_depth_floats = const_cast<float*>(static_cast<const float*>(data.depthMapFloatingPoint));
    std::vector<float> depth_floats(raw_depth_floats, raw_depth_floats + (width * height));
    cv::Mat new_image_depth = cv::Mat(depth_floats).reshape(1, height);
    // Copy and build the confidences
    int16_t* raw_confidence_shorts = const_cast<int16_t*>(static_cast<const int16_t*>(data.confidenceMap));
    std::vector<int16_t> confidence_shorts(raw_confidence_shorts, raw_confidence_shorts + (width * height));
    // Filter the depth image
    cv::Mat new_image_depth_filtered(new_image_depth.rows, new_image_depth.cols, CV_32FC1);
    cv::Mat depth_mean_kernel(3, 3, CV_32FC1);
    depth_mean_kernel.setTo(1.0);
    cv::filter2D(new_image_depth, new_image_depth_filtered, CV_32FC1, depth_mean_kernel);
    // Convert the OpenCV depth image to ROS
    std_msgs::Header new_image_header;
    new_image_header.frame_id = g_depth_optical_frame_name;
    new_image_header.stamp = depth_timestamp;
    sensor_msgs::Image new_depth_image;
    cv_bridge::CvImage new_depth_image_converted(new_image_header, sensor_msgs::image_encodings::TYPE_32FC1, new_image_depth_filtered);
    new_depth_image_converted.toImageMsg(new_depth_image);
    sensor_msgs::CameraInfo new_depth_image_camerainfo = g_depth_camerainfo;
    new_depth_image_camerainfo.header = new_image_header;
    // Publish the image
    g_depth_pub.publish(new_depth_image, new_depth_image_camerainfo);
    // Second, generate the pointcloud
    DepthSense::FPVertex* raw_vertices = const_cast<DepthSense::FPVertex*>(static_cast<const DepthSense::FPVertex*>(data.verticesFloatingPoint));
    std::vector<DepthSense::FPVertex> vertices(raw_vertices, raw_vertices + (width * height));
    // Make the XYZ pointcloud
    pcl::PointCloud<pcl::PointXYZI> pcl_pointcloud;
    for (size_t idx = 0; idx < vertices.size(); idx++)
    {
        float x = vertices[idx].x;
        float y = -vertices[idx].y;
        float z = vertices[idx].z;
        if (is_vertex_valid(x, y, z))
        {
            // Filter based on confidence
            int32_t confidence = (int32_t)confidence_shorts[idx];
            if (confidence >= g_confidence_threshold)
            {
                pcl::PointXYZI new_point;
                new_point.x = x;
                new_point.y = y;
                new_point.z = z;
                new_point.intensity = (float)confidence;
                pcl_pointcloud.push_back(new_point);
            }
        }
    }
    // Publish the cloud without filtering
    // Convert the XYZ pointcloud to ROS message
    sensor_msgs::PointCloud2 ros_pointcloud;
    pcl::toROSMsg(pcl_pointcloud, ros_pointcloud);
    ros_pointcloud.header.frame_id = g_depth_optical_frame_name;
    ros_pointcloud.header.stamp = new_image_header.stamp;
    g_pointcloud_pub.publish(ros_pointcloud);
    // Make the XYZRGB pointcloud (if enabled)
    if (g_color_config.enable_color_map && !g_current_color_image.empty())
    {
        // Get the UV map that maps the color map and vertices together
        DepthSense::UV* raw_uv = const_cast<DepthSense::UV*>(static_cast<const DepthSense::UV*>(data.uvMap));
        std::vector<DepthSense::UV> uv(raw_uv, raw_uv + (width * height));
        // Make the XYZRGB pointcloud
        pcl::PointCloud<pcl::PointXYZRGB> pcl_rgb_pointcloud;
        // Make the matching UV map
        pcl::PointCloud<pcl::PointUV> pcl_uv_pointcloud;
        for (size_t idx = 0; idx < vertices.size(); idx++)
        {
            float x = vertices[idx].x;
            float y = -vertices[idx].y;
            float z = vertices[idx].z;
            float u = uv[idx].u;
            float v = uv[idx].v;
            if (is_vertex_valid(x, y, z) && is_uv_valid(u, v))
            {
                // Lookup RGB colors
                size_t image_height = (size_t)(v * g_current_color_image.rows);
                size_t image_width = (size_t)(u * g_current_color_image.cols);
                cv::Vec3b pixel = g_current_color_image.at<cv::Vec3b>(image_height, image_width);
                uint8_t blue = pixel[0];
                uint8_t green = pixel[1];
                uint8_t red = pixel[2];
                // Filter based on confidence
                int32_t confidence = (int32_t)confidence_shorts[idx];
                if (confidence >= g_confidence_threshold)
                {
                    // Make the point
                    pcl::PointXYZRGB new_xyzrgb_point;
                    new_xyzrgb_point.x = x;
                    new_xyzrgb_point.y = y;
                    new_xyzrgb_point.z = z;
                    new_xyzrgb_point.r = red;
                    new_xyzrgb_point.g = green;
                    new_xyzrgb_point.b = blue;
                    pcl_rgb_pointcloud.push_back(new_xyzrgb_point);
                    // Make the UV
                    pcl::PointUV new_uv_point;
                    new_uv_point.u = u;
                    new_uv_point.v = v;
                    pcl_uv_pointcloud.push_back(new_uv_point);
                }
            }
        }
        // Publish the cloud without filtering
        // Convert the XYZRGB pointcloud to ROS message
        sensor_msgs::PointCloud2 ros_rgb_pointcloud;
        pcl::toROSMsg(pcl_rgb_pointcloud, ros_rgb_pointcloud);
        ros_rgb_pointcloud.header.frame_id = g_depth_optical_frame_name;
        ros_rgb_pointcloud.header.stamp = depth_timestamp;
        g_rgb_pointcloud_pub.publish(ros_rgb_pointcloud);
        // Convert the UV pointcloud to ROS message
        sensor_msgs::PointCloud2 ros_uv_pointcloud;
        pcl::toROSMsg(pcl_uv_pointcloud, ros_uv_pointcloud);
        ros_uv_pointcloud.header.frame_id = g_depth_optical_frame_name;
        ros_uv_pointcloud.header.stamp = depth_timestamp;
        g_uv_pointcloud_pub.publish(ros_uv_pointcloud);
        // Make the "registered" depth image
        cv::Mat new_image_depth_registered(new_image_depth.rows, new_image_depth.cols, CV_32FC3, cv::Scalar(0.0, 0.0, 0.0));
        // Make the "index image" that maps pixels in the image to the corresponding index in the pointcloud
        cv::Mat new_image_depth_indices(new_image_depth.rows, new_image_depth.cols, CV_32SC2, cv::Scalar(-1, -1));
        int32_t xyz_filtered_vertex_index = 0;
        int32_t uv_and_xyz_filtered_vertex_index = 0;
        for (size_t idx = 0; idx < vertices.size(); idx++)
        {
            // Filter based on confidence
            int32_t confidence = (int32_t)confidence_shorts[idx];
            if (confidence >= g_confidence_threshold)
            {
                float x = vertices[idx].x;
                float y = -vertices[idx].y;
                float z = vertices[idx].z;
                float u = uv[idx].u;
                float v = uv[idx].v;
                if (is_vertex_valid(x, y, z))
                {
                    if (is_uv_valid(u, v))
                    {

                            // Get the matching position in the image
                            size_t image_height = (size_t)(v * new_image_depth_registered.rows);
                            size_t image_width = (size_t)(u * new_image_depth_registered.cols);
                            // Set that location in the image with the current vertex
                            new_image_depth_registered.at<cv::Vec3f>(image_height, image_width) = cv::Vec3f(x, y, z);
                            new_image_depth_indices.at<cv::Vec2i>(image_height, image_width) = cv::Vec2i(xyz_filtered_vertex_index, uv_and_xyz_filtered_vertex_index);
                            uv_and_xyz_filtered_vertex_index++;
                    }
                    xyz_filtered_vertex_index++;
                }
            }
        }
        // Convert the "registered" depth image to ROS and publish it
        std_msgs::Header new_registered_depth_image_header;
        new_registered_depth_image_header.frame_id = g_depth_optical_frame_name;
        new_registered_depth_image_header.stamp = depth_timestamp;
        sensor_msgs::Image new_registed_depth_image;
        cv_bridge::CvImage new_registered_depth_image_converted(new_registered_depth_image_header, sensor_msgs::image_encodings::TYPE_32FC3, new_image_depth_registered);
        new_registered_depth_image_converted.toImageMsg(new_registed_depth_image);
        sensor_msgs::CameraInfo new_registered_depth_image_camerainfo = g_depth_camerainfo;
        new_registered_depth_image_camerainfo.header = new_registered_depth_image_header;
        // Publish the image
        g_registered_depth_pub.publish(new_registed_depth_image, new_registered_depth_image_camerainfo);
        // Convert the "index image" to ROS and publish it
        std_msgs::Header new_indices_depth_image_header;
        new_indices_depth_image_header.frame_id = g_depth_optical_frame_name;
        new_indices_depth_image_header.stamp = depth_timestamp;
        sensor_msgs::Image new_indices_depth_image;
        cv_bridge::CvImage new_indices_depth_image_converted(new_indices_depth_image_header, sensor_msgs::image_encodings::TYPE_32SC2, new_image_depth_indices);
        new_indices_depth_image_converted.toImageMsg(new_indices_depth_image);
        sensor_msgs::CameraInfo new_indices_depth_image_camerainfo = g_depth_camerainfo;
        new_indices_depth_image_camerainfo.header = new_indices_depth_image_header;
        // Publish the image
        g_index_pub.publish(new_indices_depth_image, new_indices_depth_image_camerainfo);
    }
    else if (g_color_config.enable_color_map && g_current_color_image.empty())
    {
        ROS_WARN("XYZRGB pointclouds enabled, but no color image received yet. Not publishing XYZRGB pointcloud");
    }
    if (!ros::ok())
    {
        g_context.quit();
    }
}

void ConfigureDepthNode(DepthSense::DepthNode& depth_node)
{
    ROS_INFO("Configuring new DepthNode...");
    // Register the event handler
    depth_node.newSampleReceivedEvent().connect(&OnNewDepthSample);
    // Try to configure the node
    try
    {
        g_context.requestControl(depth_node, 0);
        depth_node.setConfiguration(g_depth_config.base_config);
{'fraction_non_alphanumeric': 0.09785503995513808, 'fraction_numerical': 0.011692135146502173, 'mean_word_length': 2.9863641444059463, 'pattern_counts': {'":': 0, '<': 107, '<?xml version=': 0, '>': 105, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 6, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1080095', 'n_tokens_mistral': 12032, 'n_tokens_neox': 11449, 'n_words': 2232}