Point cloud coloring about r3live HOT 7 CLOSED

            int image_rows = src_img.rows;
            int image_cols = src_img.cols;
                            vector<vector<int>> color_vector;
            color_vector.resize(image_rows*image_cols);
            for (int v = 0; v < image_rows; ++v) {
                for (int u = 0; u < image_cols; ++u) {

                    auto color_inst =  (cv::Vec3b) src_img.at<cv::Vec3b>(v, u);
                    // std::cout<<"COLORS::"<<color_inst<<std::endl;
                    // for .bmp photo, the 3 channels are BGR
                    // std::cout<<"BNumbers"<<(int)color_inst[0]<<std::endl;
                    color_vector[v*image_cols + u].resize(3);
                    
                    color_vector[v*image_cols + u][0] = color_inst[2];
                    color_vector[v*image_cols + u][1] = color_inst[1];
                    color_vector[v*image_cols + u][2] = color_inst[0];
                }
            }

            


            // color_vector.resize(row*col);
            for (int nidx = 0; nidx < Measures.lidar->points.size(); nidx++){

                float UV[2] = {0, 0};
                // void getUV(const cv::Mat &matrix_in, const cv::Mat &matrix_out, float x, float y, float z, float* UV) {
                double matrix3[4][1] = {Measures.lidar->points[nidx].x, Measures.lidar->points[nidx].y, Measures.lidar->points[nidx].z, 1};
                cv::Mat coordinate(4, 1, CV_64F, matrix3);
                
                // calculate the result of u and v
                cv::Mat result = intrisicMat*extrinsicMat_RT*coordinate;
                float u = result.at<double>(0, 0);
                float v = result.at<double>(1, 0);
                float depth = result.at<double>(2, 0);
                
                UV[0] = u / depth;
                UV[1] = v / depth; 
                
                int u_internal = int(UV[0]);
                int v_internal = int(UV[1]);

                int32_t index = v_internal*image_cols + u_internal;
                std::uint8_t r = 0, g = 255, b = 0;
                if (index < image_rows*image_cols && index >= 0) {
                    r = color_vector[index][0];
                    g = color_vector[index][1];
                    b = color_vector[index][2];
                    std::uint32_t rgb = ((std::uint32_t)r << 16 | (std::uint32_t)g << 8 | (std::uint32_t)b);
                    Measures.lidar->points[nidx].rgb = *reinterpret_cast<float*>(&rgb);
                }
                
            }
        }