text: /* +---------------------------------------------------------------------------+
     |                 The Mobile Robot Programming Toolkit (MRPT)               |
     |                                                                           |
     |                          https://www.mrpt.org/                            |
     |                                                                           |
     | Copyright (c) 2005-2013, Individual contributors, see AUTHORS file        |
     | Copyright (c) 2005-2013, MAPIR group, University of Malaga                |
     | Copyright (c) 2012-2013, University of Almeria                            |
     | All rights reserved.                                                      |
     |                                                                           |
     | Redistribution and use in source and binary forms, with or without        |
     | modification, are permitted provided that the following conditions are    |
     | met:                                                                      |
     |    * Redistributions of source code must retain the above copyright       |
     |      notice, this list of conditions and the following disclaimer.        |
     |    * Redistributions in binary form must reproduce the above copyright    |
     |      notice, this list of conditions and the following disclaimer in the  |
     |      documentation and/or other materials provided with the distribution. |
     |    * Neither the name of the copyright holders nor the                    |
     |      names of its contributors may be used to endorse or promote products |
     |      derived from this software without specific prior written permission.|
     |                                                                           |
     | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS       |
     | 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED |
     | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR|
     | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE |
     | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL|
     | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR|
     |  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)       |
     | HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,       |
     | STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN  |
     | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE           |
     | POSSIBILITY OF SUCH DAMAGE.                                               |
     +---------------------------------------------------------------------------+ */
  
  #include <mrpt/obs.h>    // For loading from the rawlog
  #include <mrpt/maps.h>   // For converting into point maps
  #include <mrpt/io/CFileGZInputStream.h>
  #include <mrpt/system.h>
  #include <mrpt/synch.h>
  
  #include <pcl/visualization/cloud_viewer.h>
  #include <iostream>
  #include <pcl/io/io.h>
  #include <pcl/io/pcd_io.h>
  
  
  size_t rawlogEntry=0;
  
  
  struct ThreadData
  {
  	ThreadData() : new_timestamp(INVALID_TIMESTAMP) { }
  
  	pcl::PointCloud<pcl::PointXYZRGB>::Ptr  new_cloud;
  	mrpt::system::TTimeStamp  new_timestamp;
  };
  
  ThreadData                    td;
  std::mutex td_cs;
  
  
  void viewerUpdate(pcl::visualization::PCLVisualizer& viewer)
  {
      std::stringstream ss;
      ss << "Rawlog entry: " << rawlogEntry;
      viewer.removeShape ("text", 0);
      viewer.addText (ss.str(), 10,50, "text", 0);
  
      static mrpt::system::TTimeStamp last_time = INVALID_TIMESTAMP;
  
      {  // Mutex protected
      	std::lock_guard<std::mutex> lock(td_cs);
      	if (td.new_timestamp!=last_time)
      	{
      		last_time = td.new_timestamp;
  			viewer.removePointCloud("cloud", 0);
  			viewer.addPointCloud (td.new_cloud,"cloud",0);
  			viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3.0);
  
  			const size_t N = td.new_cloud->size();
  			std::cout << "Showing new point cloud of size=" << N << std::endl;
  
  			static bool first = true;
  			if (N && first)
  			{
  				first = false;
  			//viewer.resetCameraViewpoint("cloud");
  			}
  
  #if 0
  			std::cout << mrpt::format(
  				"camera: clip = %f %f\n"
  				"        focal = %f %f %f\n"
  				"        pos   = %f %f %f\n"
  				"        view  = %f %f %f\n",
  				viewer.camera_.clip[0],viewer.camera_.clip[1],
  				viewer.camera_.focal[0],viewer.camera_.focal[1],viewer.camera_.focal[2],
  				viewer.camera_.pos[0],viewer.camera_.pos[1],viewer.camera_.pos[2],
  				viewer.camera_.view[0],viewer.camera_.view[1],viewer.camera_.view[2]);
  #endif
      	}
      }
  }
  
  void  viewerOneOff(pcl::visualization::PCLVisualizer& viewer)
  {
      viewer.setBackgroundColor (0.3, 0.3, 0.3);
      viewer.addCoordinateSystem(1.0, 0);
      viewer.initCameraParameters();
      viewer.camera_.pos[2] = 30;
  	viewer.updateCamera();
  }
  
  int main(int argc, char**argv)
  {
  	try
  	{
  		if (argc!=2)
  		{
  			std::cerr << "Usage: " << argv[0] << " <DATASET.rawlog>\n";
  			return 1;
  		}
  
  
  		std::cout << "Opening: " << argv[1] << std::endl;
  		mrpt::io::CFileGZInputStream  fil(argv[1]);
  		bool rawlog_eof = false;
  
  		pcl::visualization::CloudViewer viewer("Cloud Viewer from MRPT's rawlog");
  
  		//This will only get called once
  		viewer.runOnVisualizationThreadOnce (viewerOneOff);
  
  		//This will get called once per visualization iteration
  		viewer.runOnVisualizationThread (viewerUpdate);
  
  		while (!viewer.wasStopped ())
  		{
  			mrpt::obs::CActionCollection::Ptr	actions;
  			mrpt::obs::CSensoryFrame::Ptr		SF;
  			mrpt::obs::CObservation::Ptr			obs;
  
  			if (!rawlog_eof)
  			{
  				if (!mrpt::obs::CRawlog::getActionObservationPairOrObservation(fil, actions, SF, obs,rawlogEntry))
  				{
  					rawlog_eof = true;
  					std::cerr << "End of rawlog file!! Close the window to exit\n";
  				}
  				else
  				{
  					// Can generate a point cloud from this data?
  					// TODO: Process Kinect observations differently to extract RGB data.
  					mrpt::maps::CPointsMap::Ptr  new_map;
  					if (SF)
  					{
  						new_map = mrpt::make_aligned_shared<mrpt::maps::CSimplePointsMap>();
  						// new_map->insertionOptions.minDistBetweenLaserPoints = 0;
  						SF->insertObservationsInto(new_map);
  					}
  					else if (obs)
  					{
  						new_map = mrpt::make_aligned_shared<mrpt::maps::CSimplePointsMap>();
  						// new_map->insertionOptions.minDistBetweenLaserPoints = 0;
  						new_map->insertObservation(obs.get());
  					}
  
  					if (new_map)
  					{
  						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
  
  						// Convert MRPT point maps -> PCL point cloud.
  						new_map->getPCLPointCloud(*cloud);
  
  						{  // Mutex protected
  							std::lock_guard<std::mutex> lock(td_cs);
  							td.new_timestamp = mrpt::system::now();
  							td.new_cloud = cloud;
  						}
  
  						std::this_thread::sleep_for(30ms);  // Delay to allow the point cloud to show up.
  					}
  
  				}
  			}
  
  			std::this_thread::sleep_for(1ms);
  		}
  		return 0;
  	}
  	catch (std::exception &e)
  	{
  		std::cerr << e.what() << std::endl;
  		return 1;
  	}
  }
  