Utils.h

Go to the documentation of this file.

#pragma once
#ifndef _UTILS_H
#define _UTILS_H

#include "ros/ros.h"
#include "ros/package.h"
#include "pcl_conversions/pcl_conversions.h"
#include <pcl_ros/point_cloud.h>

#include <Viewer.h>

extern const std::string TARGET_CLOUD_TOPIC;
extern const std::string SOURCE_CLOUD_TOPIC;

class Utils
{
    typedef pcl::PointCloud<pcl::PointXYZ> PointCloudXYZ;
    typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloudRGB;
    typedef pcl::PointCloud<pcl::Normal> PointCloudNormal;

private:
    Utils(){};

    ~Utils(){};

public:
    static void setPCpath(const std::string& pointcloud_folder_path);

    static bool getNormals(PointCloudRGB::Ptr& cloud, double normal_radius, PointCloudNormal::Ptr& normals);

    static bool isValidCloud(PointCloudXYZ::Ptr cloud);

    static bool isValidCloud(PointCloudRGB::Ptr cloud);

    static bool isValidCloud(PointCloudNormal::Ptr normals);

    static bool isValidMesh(pcl::PolygonMesh::Ptr mesh);

    static bool isValidCloudMsg(const sensor_msgs::PointCloud2& cloud_msg);

    static bool isValidTransform(Eigen::Matrix4f transform);

    static void colorizeCloud(PointCloudRGB::Ptr cloud_rgb, int R, int G, int B);

    static void cloudToXYZRGB(PointCloudXYZ::Ptr cloud, PointCloudRGB::Ptr cloud_rgb, int R, int G, int B);

    static void cloudToROSMsg(PointCloudRGB::Ptr cloud, sensor_msgs::PointCloud2& cloud_msg, const std::string& frameid="world");
    
    static void cloudToROSMsg(const pcl::PCLPointCloud2& cloud, sensor_msgs::PointCloud2& cloud_msg, const std::string& frameid="world");

    static double computeCloudResolution(PointCloudXYZ::Ptr cloud);

    static double computeCloudResolution(PointCloudRGB::Ptr cloud);

    static void printTransform(const Eigen::Matrix4f& transform);

    static void printMatrix(const Eigen::Ref<const Eigen::MatrixXf>& matrix, const int size);

    static void onePointCloud(PointCloudRGB::Ptr cloud, int size,
                              PointCloudRGB::Ptr one_point_cloud);                            

    static void translateCloud(PointCloudRGB::Ptr cloud_in, PointCloudRGB::Ptr cloud_out, 
                              double x_offset = 0, double y_offset = 0, double z_offset = 0);

    static void rotateCloud(PointCloudRGB::Ptr cloud_in, PointCloudRGB::Ptr cloud_out, 
                            double roll, double pitch, double yaw);

    static void getVectorFromNormal(PointCloudNormal::Ptr normal, double idx,
                                    Eigen::Vector3f& vector);                          

    static bool searchForSameRows(Eigen::MatrixXd source_m, Eigen::MatrixXd target_m,
                                  std::vector<int>& source_indx, std::vector<int>& target_indx);

    static bool areSameVectors(const Eigen::VectorXd v1, 
                              const Eigen::VectorXd v2, 
                              double threshold);

    static int findOnVector(double value, 
                            const Eigen::VectorXd v, 
                            double threshold);

    static int findOnMatrix(const Eigen::VectorXd v, 
                            const Eigen::MatrixXd m,  
                            double threshold); 

    static void extractRowFromMatrix(Eigen::MatrixXd& matrix, int row);   

    static void extractColFromMatrix(Eigen::MatrixXd& matrix, int col);  
};

#endif