#include <map>
#include <iostream>
#include <algorithm>
#include <stdio.h>
#include <stdlib.h>
#include <float.h>
#include <math.h>
#include <ros/this_node.h>
#include <pcl/io/pcd_io.h>
#include <pcl_cloud_algos/cloud_algos.h>
#include <pcl_cloud_algos/depth_image_triangulation.h>
#include <pcl_cloud_algos/pcl_cloud_algos_point_types.h>