Transformation and Projection about Points2Image.hpp

#include <opencv2/core/core.hpp>
#include <pcl/common/transforms.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <iostream>
void Processing(const Eigen::Affine3f &transformation_matrix, const pcl::PointCloud<pcl::PointXYZ> &origin_point_cloud, const cv::Mat &projection_matrix, const cv::Mat &cameraMat, const cv::Size &image_size, cv::Point2f &result, bool log_flag = true);

void Processing(const Eigen::Affine3f &transformation_matrix, const pcl::PointCloud<pcl::PointXYZ> &origin_point_cloud, const cv::Mat &projection_matrix, const cv::Mat &cameraMat, const cv::Size &image_size, cv::Point2f &result, bool log_flag){
    // Transfrom Points with Parameters
    pcl::PointCloud<pcl::PointXYZ> transform_point_cloud;
    pcl::transformPointCloud(origin_point_cloud, transform_point_cloud, transformation_matrix);

    pcl::PointXYZ tmp_XYZ;
    for(pcl::PointCloud<pcl::PointXYZ>::const_iterator iter = transform_point_cloud.points.begin(); iter < transform_point_cloud.points.end(); ++iter){
        // Project point on image view (Projection)
        cv::Mat pt_3D(4, 1, CV_32FC1);
        float *pt_3D_data = (float*) pt_3D.data;
        pt_3D_data[0] = iter->x;
        pt_3D_data[1] = iter->y;
        pt_3D_data[2] = iter->z;
        pt_3D_data[3] = 1.0f;
        cv::Mat pt_2D = projection_matrix * pt_3D;

        // Nomalized Point on plane
        float *pt_2D_data = (float *) pt_2D.data;
        float w = pt_2D_data[2];
        float x = pt_2D_data[0] / w;
        float y = pt_2D_data[1] / w;

        cv::Point2f nomalization_point(x, y);
        if(nomalization_point.inside(cv::Rect(0, 0, image_size.width, image_size.height)) == true)
            result = nomalization_point;

        if(log_flag == true){
            cout << endl;
            cout << "Transformation Point : " << iter->x << ", " << iter->y << ", " << iter->z << endl;
            cout << "Projection Point     : " << pt_2D.at<float>(0) << ", " << pt_2D.at<float>(1) << ", "  << pt_2D.at<float>(2) << endl;
            cout << "Nomalization Point   : " << x << ", " << y << endl;
        }
    }
}