#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/point_cloud_handlers.h>
#include <pcl/point_cloud.h>
#include <iostream>

using namespace std;
using namespace pcl;

int main(int argc, char* argv[]) {
    sensor_msgs::PointCloud2::Ptr mainCloud  (new sensor_msgs::PointCloud2);
    if (io::loadPCDFile ("pc1.pcd", *mainCloud) == -1) //* load the file
    {
        PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
        return (-1);
    }
    PointCloud<PointXYZRGB>::Ptr mainCloudPtr (new PointCloud<PointXYZRGB>);
    fromROSMsg (*mainCloud, *mainCloudPtr);

    typedef pcl::visualization::PointCloudColorHandler<sensor_msgs::PointCloud2> ColorHandler;
    typedef pcl::visualization::PointCloudGeometryHandler<sensor_msgs::PointCloud2> GeometryHandler;
    boost::shared_ptr<pcl::visualization::PCLVisualizer> p  (new pcl::visualization::PCLVisualizer (argc, argv, "PCD viewer"));
    ColorHandler::Ptr color_handler(new pcl::visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2> (mainCloud));
    GeometryHandler::Ptr geometry_handler(new pcl::visualization::PointCloudGeometryHandlerXYZ<sensor_msgs::PointCloud2> (mainCloud));

    p->addPointCloud<PointXYZRGB> (mainCloudPtr, geometry_handler, color_handler, "Test", 0);
    if (p)
        p->spin ();
    return 0;
}