pcl-opencv cpp example

cmake_minimum_required(VERSION 3.12)
project(PCL_demo1)
 
set(CMAKE_CXX_STANDARD 14)
 
 
find_package(OpenCV REQUIRED)
find_package(PCL REQUIRED)
 
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4")

include_directories(
        ${OpenCV_INCLUDE_DIRS}
)
 
add_executable(PCL_demo1 main.cpp)
target_link_libraries (PCL_demo1 ${PCL_LIBRARIES})
target_link_libraries(PCL_demo1 ${OpenCV_LIBS})

#include <iostream>

#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>

#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/common.h>
#include <pcl/common/common_headers.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>

pcl::PointCloud<pcl::PointXYZ>::Ptr cvMatToPcl(int i, cv::Mat& mat_x, cv::Mat& mat_r, int nAlpha=100);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cvMatToPclWithRGB(int i, cv::Mat& mat_x, cv::Mat& mat_r, cv::Mat& gray_img, int nAlpha=100);

pcl::PointCloud<pcl::PointXYZ>::Ptr cvMatToPcl(int i, cv::Mat& mat_x, cv::Mat& mat_r, int nAlpha) 
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    std::cout << "begin parsing file" << std::endl;
    int point_num = 0;
    int nBias = i * mat_x.rows;
    for (int ki = 0; ki < mat_x.rows; ki++) {
        for (int kj = 0; kj < mat_x.cols; kj++) {
            pcl::PointXYZ pointXYZ;
            pointXYZ.x = mat_x.at<float>(ki, kj);
            pointXYZ.y = (float)ki;
            pointXYZ.z = mat_r.at<float>(ki, kj);

            if (pointXYZ.z < nAlpha)
                continue;

            cloud->points.push_back(pointXYZ);
        }
    }
    cloud->width = 1;
    cloud->height = cloud->points.size();

    return cloud;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cvMatToPclWithRGB(int i, cv::Mat& mat_x, cv::Mat& mat_r, cv::Mat& gray_img, int nAlpha) 
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    std::cout << "begin parsing file" << std::endl;
    int point_num = 0;
    int nBias = i * mat_x.rows;
    for (int ki = 0; ki < mat_x.rows; ki++) {
        for (int kj = 0; kj < mat_x.cols; kj++) {
            pcl::PointXYZRGB pointXYZRGB;
            pointXYZRGB.x = mat_x.at<float>(ki, kj);
            pointXYZRGB.y = (float)ki;
            pointXYZRGB.z = mat_r.at<float>(ki, kj);

            uchar color_gray = gray_img.at<uchar>(ki, kj);

            pointXYZRGB.r = color_gray;
            pointXYZRGB.g = color_gray;
            pointXYZRGB.b = color_gray;

            if (pointXYZRGB.z < nAlpha)
                continue;

            cloud->points.push_back(pointXYZRGB);
        }
    }
    cloud->width = 1;
    cloud->height = cloud->points.size();

    return cloud;
}

int main(int argc, char *argv[]) {

    std::string mat_x_path;
    std::string mat_r_path;
    std::string pointcloud_save_path;
    std::string img_gray_path;
    if (argc > 3)
    {
        mat_x_path = argv[1];
        mat_r_path = argv[2];
        pointcloud_save_path = argv[3];
        img_gray_path = argv[4];
    }
    else
    {
        mat_x_path = "./27_X.tiff";
        mat_r_path = "./27_R.tiff";
        pointcloud_save_path = "./27.pcd";
        img_gray_path = "./26.jpg";
    }


    cv::Mat mat_x_image = cv::imread(mat_x_path, -1);

    if (mat_x_image.empty())
    {
        cout << "Could not open or find the mat_x_path" << std::endl;
        return -1;
    }

    cv::Mat mat_r_image = cv::imread(mat_r_path, -1);

    if (mat_r_image.empty())
    {
        cout << "Could not open or find the mat_r_image" << std::endl;
        return -1;
    }


    cv::Mat gray_image = cv::imread(img_gray_path, -1);

    if (gray_image.empty())
    {
        cout << "Could not open or find the gray_image" << std::endl;
        return -1;
    }


    // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    // cloud_Ptr = cvMatToPcl(0, mat_x_image, mat_r_image, 100);
    // pcl::io::savePCDFileASCII ("test_pcd.pcd", *cloud_Ptr);
    // std::cerr << "Saved " << cloud_Ptr->points.size () << " data points to test_pcd.pcd." << std::endl;

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
    cloud_Ptr = cvMatToPclWithRGB(0, mat_x_image, mat_r_image, gray_image, 100);
    pcl::io::savePCDFileASCII ("test_pcd.pcd", *cloud_Ptr);
    std::cerr << "Saved " << cloud_Ptr->points.size () << " data points to test_pcd.pcd." << std::endl;

    return 0;
}