#include <iostream>
#include <fstream>
#include <numeric>
#include <regex>
#include <chrono>
#include <thread>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp> // For publishing LiDAR points
#include <sensor_msgs/msg/image.hpp> // For publishing camera images
#include <memory>
#include <std_msgs/msg/string.hpp>
#include <opencv2/opencv.hpp>

struct LidarPoint { // single lidar point in space
    float x,y,z,r; 
};

using namespace std;
std::thread cameraLidarProjectionThread;
std::mutex imageMutex;
std::mutex pointMutex;
bool img, poi;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr publisher;
cv::Mat image;
std::vector<LidarPoint> lidarPoints;
// store calibration data in OpenCV matrices
cv::Mat P_rect_00(3, 4, cv::DataType<double>::type); // 3x4 projection matrix after rectification
cv::Mat R_rect_00(4, 4, cv::DataType<double>::type); // 3x3 rectifying rotation to make image planes co-planar
cv::Mat RT(4, 4, cv::DataType<double>::type); // rotation matrix and translation vector

void loadCalibrationData(cv::Mat &P_rect_00, cv::Mat &R_rect_00, cv::Mat &RT) {
    RT.at<double>(0, 0) = 7.533745e-03;
    RT.at<double>(0, 1) = -9.999714e-01;
    RT.at<double>(0, 2) = -6.166020e-04;
    RT.at<double>(0, 3) = -4.069766e-03;
    RT.at<double>(1, 0) = 1.480249e-02;
    RT.at<double>(1, 1) = 7.280733e-04;
    RT.at<double>(1, 2) = -9.998902e-01;
    RT.at<double>(1, 3) = -7.631618e-02;
    RT.at<double>(2, 0) = 9.998621e-01;
    RT.at<double>(2, 1) = 7.523790e-03;
    RT.at<double>(2, 2) = 1.480755e-02;
    RT.at<double>(2, 3) = -2.717806e-01;
    RT.at<double>(3, 0) = 0.0;
    RT.at<double>(3, 1) = 0.0;
    RT.at<double>(3, 2) = 0.0;
    RT.at<double>(3, 3) = 1.0;

    R_rect_00.at<double>(0, 0) = 9.999239e-01;
    R_rect_00.at<double>(0, 1) = 9.837760e-03;
    R_rect_00.at<double>(0, 2) = -7.445048e-03;
    R_rect_00.at<double>(0, 3) = 0.0;
    R_rect_00.at<double>(1, 0) = -9.869795e-03;
    R_rect_00.at<double>(1, 1) = 9.999421e-01;
    R_rect_00.at<double>(1, 2) = -4.278459e-03;
    R_rect_00.at<double>(1, 3) = 0.0;
    R_rect_00.at<double>(2, 0) = 7.402527e-03;
    R_rect_00.at<double>(2, 1) = 4.351614e-03;
    R_rect_00.at<double>(2, 2) = 9.999631e-01;
    R_rect_00.at<double>(2, 3) = 0.0;
    R_rect_00.at<double>(3, 0) = 0;
    R_rect_00.at<double>(3, 1) = 0;
    R_rect_00.at<double>(3, 2) = 0;
    R_rect_00.at<double>(3, 3) = 1;

    P_rect_00.at<double>(0, 0) = 7.215377e+02;
    P_rect_00.at<double>(0, 1) = 0.000000e+00;
    P_rect_00.at<double>(0, 2) = 6.095593e+02;
    P_rect_00.at<double>(0, 3) = 0.000000e+00;
    P_rect_00.at<double>(1, 0) = 0.000000e+00;
    P_rect_00.at<double>(1, 1) = 7.215377e+02;
    P_rect_00.at<double>(1, 2) = 1.728540e+02;
    P_rect_00.at<double>(1, 3) = 0.000000e+00;
    P_rect_00.at<double>(2, 0) = 0.000000e+00;
    P_rect_00.at<double>(2, 1) = 0.000000e+00;
    P_rect_00.at<double>(2, 2) = 1.000000e+00;
    P_rect_00.at<double>(2, 3) = 0.000000e+00;

}
void cameraLidarProjection(void){

while(rclcpp::ok()){
if(img && poi){
imageMutex.lock(); pointMutex.lock();

std::cout << "CameraLidarProjection" << std::endl;
    // TODO: project lidar points
    cv::Mat visImg = image.clone();
    cv::Mat overlay = visImg.clone();

    cv::Mat X(4, 1, cv::DataType<double>::type);
    cv::Mat Y(3, 1, cv::DataType<double>::type);
    for (auto it = lidarPoints.begin(); it != lidarPoints.end(); ++it) {
        // filter the not needed points
        float MaxX = 25.0, maxY = 6.0, minZ = -1.40;
        if (it->x > MaxX ||it->x < 0.0 || abs(it->y) > maxY || it->z < minZ || it->r < 0.01) {
            continue;
        }

        // 1. Convert current Lidar point into homogeneous coordinates and store it in the 4D variable X.
        X.at<double>(0, 0) = it->x;
        X.at<double>(1, 0) = it->y;
        X.at<double>(2, 0) = it->z;
        X.at<double>(3, 0) = 1;

        // 2. Then, apply the projection equation as detailed in lesson 5.1 to map X onto the image plane of the camera. 
        // Store the result in Y.
        Y = P_rect_00 * R_rect_00 * RT * X;
        // 3. Once this is done, transform Y back into Euclidean coordinates and store the result in the variable pt.
        cv::Point pt;
        pt.x = Y.at<double>(0, 0) / Y.at<double>(2, 0);
        pt.y = Y.at<double>(1, 0) / Y.at<double>(2, 0);

        float val = it->x;
        float maxVal = 20.0;
        int red = min(255, (int) (255 * abs((val - maxVal) / maxVal)));
        int green = min(255, (int) (255 * (1 - abs((val - maxVal) / maxVal))));
        cv::circle(overlay, pt, 5, cv::Scalar(0, green, red), -1);
    }
        
    float opacity = 0.6;
    cv::addWeighted(overlay, opacity, visImg, 1 - opacity, 0, visImg);

    string windowName = "LiDAR data on image overlay";
    //cv::namedWindow(windowName, 3);
    sensor_msgs::msg::Image::UniquePtr msg = std::make_unique<sensor_msgs::msg::Image>();
    //msg->header.stamp = node->now();
    msg->height = visImg.rows;
    msg->width = visImg.cols;
    msg->encoding = "bgr8";
    msg->is_bigendian = false;
    msg->step = visImg.step;
    size_t size = visImg.total() * visImg.elemSize();
    msg->data.resize(size);
    memcpy(msg->data.data(), visImg.data, size);

    publisher->publish(std::move(msg));

    //cv::imshow(windowName, visImg);
    
    //cv::waitKey(1); // wait for key to be pressed
imageMutex.unlock(); pointMutex.unlock();
img = false;
poi = false;
}

}

}

 class cameraLidarNode : public rclcpp::Node
    {
    public:
        cameraLidarNode()
            : Node("image_lidar_subscriber")
        {
            image_subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
                "/kitti/image/color/left", 200, std::bind(&cameraLidarNode::imageCallback, this, std::placeholders::_1));

            point_cloud_subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
                "/kitti/point_cloud", 200, std::bind(&cameraLidarNode::topic_callback, this, std::placeholders::_1));
            
            publisher = this->create_publisher<sensor_msgs::msg::Image>("projection_frames", 10);
            
            cameraLidarProjectionThread = std::thread(cameraLidarProjection);
        }

        void imageCallback(const sensor_msgs::msg::Image::SharedPtr msg)
        {
            imageMutex.lock();
//            
            image = cv::Mat(msg->height, msg->width, CV_8UC3, const_cast<unsigned char *>(msg->data.data()));
            
            img = true;
            imageMutex.unlock();
        }

        void topic_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) const
        {
            pointMutex.lock();

            lidarPoints.clear();
//            
        
                // Convert the bytes in the array to floating-point numbers
                for (int i = 0; i < msg->data.size() / msg->point_step; i++) {
                LidarPoint temp;
                memcpy(&temp.x, &msg->data.data()[i * msg->point_step], 4);
                memcpy(&temp.y, &msg->data.data()[i * msg->point_step + 4], 4);
                memcpy(&temp.z, &msg->data.data()[i * msg->point_step + 8], 4);
                memcpy(&temp.r, &msg->data.data()[i * msg->point_step + 16], 4);
                lidarPoints.push_back(temp);
                }
            poi = true;   
            pointMutex.unlock();
            // std::cout << "X: " <<  lidarPoints[0].x << ", Y: " << lidarPoints[0].y << ", Z: " << lidarPoints[0].z << ", R: " << lidarPoints[0].r << std::endl;
            //  std::cout << "X: " <<  lidarPoints[1].x << ", Y: " << lidarPoints[1].y << ", Z: " << lidarPoints[1].z << ", R: " << lidarPoints[1].r << std::endl;
        }
        
        ~cameraLidarNode()
        {
         	if(cameraLidarProjectionThread.joinable()){
         		cameraLidarProjectionThread.join();
         	}
        }

    private:
        rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_subscription_;
        rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud_subscription_;
    };

int main() {
 img = false;
 poi = false;
 loadCalibrationData(P_rect_00, R_rect_00, RT);
 rclcpp::init(0,NULL);
 std::cout << "CameraLidarProjectionStart" << std::endl;
 auto ImageLidar_node = std::make_shared<cameraLidarNode>();
 rclcpp::spin(ImageLidar_node);
 rclcpp::shutdown();
}


Generate a block diagram explaining above code