text: /* +------------------------------------------------------------------------+
     |                     Mobile Robot Programming Toolkit (MRPT)            |
     |                          http://www.mrpt.org/                          |
     |                                                                        |
     | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
     | See: http://www.mrpt.org/Authors - All rights reserved.                |
     | Released under BSD License. See details in http://www.mrpt.org/License |
     +------------------------------------------------------------------------+ */
  
  /*
    Example  : kinect-to-2d-laser-demo
    Web page : http://www.mrpt.org/Example_Kinect_To_2D_laser_scan (includes video
    demo)
  
    Purpose  : Demonstrate grabbing from CKinect, multi-threading
  			 and converting the 3D range data into an equivalent
  			 2D planar scan.
  */
  
  #include <mrpt/hwdrivers/CKinect.h>
  #include <mrpt/gui/CDisplayWindow3D.h>
  #include <mrpt/system/filesystem.h>
  #include <mrpt/system/CTicTac.h>
  #include <mrpt/config/CConfigFile.h>
  #include <mrpt/opengl/CPointCloudColoured.h>
  #include <mrpt/opengl/CPlanarLaserScan.h>
  #include <mrpt/opengl/CFrustum.h>
  #include <mrpt/opengl/CGridPlaneXY.h>
  #include <mrpt/opengl/stock_objects.h>
  #include <mrpt/img/TColor.h>
  #include <iostream>
  
  using namespace mrpt;
  using namespace mrpt::hwdrivers;
  using namespace mrpt::math;
  using namespace mrpt::gui;
  using namespace mrpt::obs;
  using namespace mrpt::maps;
  using namespace mrpt::opengl;
  using namespace mrpt::system;
  using namespace mrpt::img;
  using namespace std;
  
  // Thread for grabbing: Do this is another thread so we divide rendering and
  // grabbing
  //   and exploit multicore CPUs.
  struct TThreadParam
  {
  	TThreadParam() : quit(false), pushed_key(0), Hz(0) {}
  	volatile bool quit;
  	volatile int pushed_key;
  	volatile double Hz;
  
  	CObservation3DRangeScan::Ptr new_obs;  // RGB+D (+3D points)
  	CObservationIMU::Ptr new_obs_imu;  // Accelerometers
  };
  
  void thread_grabbing(TThreadParam& p)
  {
  	try
  	{
  		CKinect kinect;
  
  		// Set params:
  		// kinect.enableGrab3DPoints(true);
  		// kinect.enablePreviewRGB(true);
  		//...
  		const std::string cfgFile = "kinect_calib.cfg";
  		if (mrpt::system::fileExists(cfgFile))
  		{
  			cout << "Loading calibration from: " << cfgFile << endl;
  			kinect.loadConfig(mrpt::config::CConfigFile(cfgFile), "KINECT");
  		}
  		else
  			cerr << "Warning: Calibration file [" << cfgFile
  				 << "] not found -> Using default params.\n";
  
  		// Open:
  		cout << "Calling CKinect::initialize()...";
  		kinect.initialize();
  		cout << "OK\n";
  
  		CTicTac tictac;
  		int nImgs = 0;
  		bool there_is_obs = true, hard_error = false;
  
  		while (!hard_error && !p.quit)
  		{
  			// Grab new observation from the camera:
  			CObservation3DRangeScan::Ptr obs = mrpt::make_aligned_shared<
  				CObservation3DRangeScan>();  // Smart pointers to observations
  			CObservationIMU::Ptr obs_imu =
  				mrpt::make_aligned_shared<CObservationIMU>();
  
  			kinect.getNextObservation(*obs, *obs_imu, there_is_obs, hard_error);
  
  			if (!hard_error && there_is_obs)
  			{
  				std::atomic_store(&p.new_obs, obs);
  				std::atomic_store(&p.new_obs_imu, obs_imu);
  			}
  
  			if (p.pushed_key != 0)
  			{
  				switch (p.pushed_key)
  				{
  					case 27:
  						p.quit = true;
  						break;
  				}
  
  				// Clear pushed key flag:
  				p.pushed_key = 0;
  			}
  
  			nImgs++;
  			if (nImgs > 10)
  			{
  				p.Hz = nImgs / tictac.Tac();
  				nImgs = 0;
  				tictac.Tic();
  			}
  		}
  	}
  	catch (std::exception& e)
  	{
  		cout << "Exception in Kinect thread: " << e.what() << endl;
  		p.quit = true;
  	}
  }
  
  // ------------------------------------------------------
  //				Test_Kinect
  // ------------------------------------------------------
  void Test_Kinect()
  {
  	// Launch grabbing thread:
  	// --------------------------------------------------------
  	TThreadParam thrPar;
  	std::thread thHandle(thread_grabbing, std::ref(thrPar));
  
  	// Wait until data stream starts so we can say for sure the sensor has been
  	// initialized OK:
  	cout << "Waiting for sensor initialization...\n";
  	do
  	{
  		CObservation3DRangeScan::Ptr possiblyNewObs =
  			std::atomic_load(&thrPar.new_obs);
  		if (possiblyNewObs && possiblyNewObs->timestamp != INVALID_TIMESTAMP)
  			break;
  		else
  			std::this_thread::sleep_for(10ms);
  	} while (!thrPar.quit);
  
  	// Check error condition:
  	if (thrPar.quit) return;
  
  	// Create window and prepare OpenGL object in the scene:
  	// --------------------------------------------------------
  	mrpt::gui::CDisplayWindow3D win3D("Kinect 3D -> 2D laser scan", 800, 600);
  
  	win3D.setCameraAzimuthDeg(140);
  	win3D.setCameraElevationDeg(30);
  	win3D.setCameraZoom(10.0);
  	win3D.setFOV(90);
  	win3D.setCameraPointingToPoint(2.5, 0, 0);
  
  	// The 3D point cloud OpenGL object:
  	mrpt::opengl::CPointCloudColoured::Ptr gl_points =
  		mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
  	gl_points->setPointSize(2.5);
  
  	// The 2D "laser scan" OpenGL object:
  	mrpt::opengl::CPlanarLaserScan::Ptr gl_2d_scan =
  		mrpt::make_aligned_shared<mrpt::opengl::CPlanarLaserScan>();
  	gl_2d_scan->enablePoints(true);
  	gl_2d_scan->enableLine(true);
  	gl_2d_scan->enableSurface(true);
  	gl_2d_scan->setSurfaceColor(0, 0, 1, 0.3);  // RGBA
  
  	mrpt::opengl::CFrustum::Ptr gl_frustum =
  		mrpt::make_aligned_shared<mrpt::opengl::CFrustum>(
  			0.2f, 5.0f, 90.0f, 5.0f, 2.0f, true, true);
  
  	const double aspect_ratio =
  		480.0 / 640.0;  // kinect.rows() / double( kinect.cols() );
  
  	opengl::COpenGLViewport::Ptr viewRange,
  		viewInt;  // Extra viewports for the RGB & D images.
  	{
  		mrpt::opengl::COpenGLScene::Ptr& scene = win3D.get3DSceneAndLock();
  
  		// Create the Opengl object for the point cloud:
  		scene->insert(gl_points);
  		scene->insert(gl_2d_scan);
  		scene->insert(gl_frustum);
  
  		{
  			mrpt::opengl::CGridPlaneXY::Ptr gl_grid =
  				mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>();
  			gl_grid->setColor(0.6, 0.6, 0.6);
  			scene->insert(gl_grid);
  		}
  		{
  			mrpt::opengl::CSetOfObjects::Ptr gl_corner =
  				mrpt::opengl::stock_objects::CornerXYZ();
  			gl_corner->setScale(0.2);
  			scene->insert(gl_corner);
  		}
  
  		const int VW_WIDTH =
  			250;  // Size of the viewport into the window, in pixel units.
  		const int VW_HEIGHT = aspect_ratio * VW_WIDTH;
  		const int VW_GAP = 30;
  
  		// Create the Opengl objects for the planar images, as textured planes,
  		// each in a separate viewport:
  		win3D.addTextMessage(
  			30, -25 - 1 * (VW_GAP + VW_HEIGHT), "Range data", TColorf(1, 1, 1),
  			1, MRPT_GLUT_BITMAP_HELVETICA_12);
  		viewRange = scene->createViewport("view2d_range");
  		viewRange->setViewportPosition(
  			5, -10 - 1 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT);
  
  		win3D.addTextMessage(
  			30, -25 - 2 * (VW_GAP + VW_HEIGHT), "Intensity data",
  			TColorf(1, 1, 1), 2, MRPT_GLUT_BITMAP_HELVETICA_12);
  		viewInt = scene->createViewport("view2d_int");
  		viewInt->setViewportPosition(
  			5, -10 - 2 * (VW_GAP + VW_HEIGHT), VW_WIDTH, VW_HEIGHT);
  
  		win3D.unlockAccess3DScene();
  		win3D.repaint();
  	}
  
  	CObservation3DRangeScan::Ptr last_obs;
  	CObservationIMU::Ptr last_obs_imu;
  
  	while (win3D.isOpen() && !thrPar.quit)
  	{
  		CObservation3DRangeScan::Ptr possiblyNewObs =
  			std::atomic_load(&thrPar.new_obs);
  		if (possiblyNewObs && possiblyNewObs->timestamp != INVALID_TIMESTAMP &&
  			(!last_obs || possiblyNewObs->timestamp != last_obs->timestamp))
  		{
  			// It IS a new observation:
  			last_obs = possiblyNewObs;
  			last_obs_imu = std::atomic_load(&thrPar.new_obs_imu);
  
  			// Update visualization ---------------------------------------
  			bool do_refresh = false;
  
  			// Show 2D ranges as a grayscale image:
  			if (last_obs->hasRangeImage)
  			{
  				mrpt::img::CImage img;
  
  				// Normalize the image
  				static CMatrixFloat range2D;  // Static to save time allocating
  				// the matrix in every iteration
  				range2D = last_obs->rangeImage *
  						  (1.0 / 5.0);  // kinect.getMaxRange());
  
  				img.setFromMatrix(range2D);
  
  				win3D.get3DSceneAndLock();
  				viewRange->setImageView_fast(img);
  				win3D.unlockAccess3DScene();
  				do_refresh = true;
  			}
  
  			// Convert ranges to an equivalent 2D "fake laser" scan:
  			if (last_obs->hasRangeImage)
  			{
  				// Convert to scan:
  				CObservation2DRangeScan::Ptr obs_2d =
  					mrpt::make_aligned_shared<CObservation2DRangeScan>();
  				const float vert_FOV = DEG2RAD(gl_frustum->getVertFOV());
  
  				mrpt::obs::T3DPointsTo2DScanParams sp;
  				sp.angle_inf = .5f * vert_FOV;
  				sp.angle_sup = .5f * vert_FOV;
  				sp.sensorLabel = "KINECT_2D_SCAN";
  
  				last_obs->convertTo2DScan(*obs_2d, sp);
  
  				// And load scan in the OpenGL object:
  				gl_2d_scan->setScan(*obs_2d);
  			}
  
  			// Show intensity image:
  			if (last_obs->hasIntensityImage)
  			{
  				win3D.get3DSceneAndLock();
  				viewInt->setImageView(
  					last_obs->intensityImage);  // This is not "_fast" since the
  				// intensity image is used below
  				// in the coloured point cloud.
  				win3D.unlockAccess3DScene();
  				do_refresh = true;
  			}
  
  			// Show 3D points:
  			if (last_obs->hasPoints3D)
  			{
  				win3D.get3DSceneAndLock();
  				mrpt::obs::T3DPointsProjectionParams pp;
  				pp.takeIntoAccountSensorPoseOnRobot = false;
  
  				last_obs->project3DPointsFromDepthImageInto(*gl_points, pp);
  				win3D.unlockAccess3DScene();
  				do_refresh = true;
  			}
  
  			// Some text messages:
  			win3D.get3DSceneAndLock();
  			// Estimated grabbing rate:
  			win3D.addTextMessage(
  				-100, -20, format("%.02f Hz", thrPar.Hz), TColorf(1, 1, 1),
  				"sans", 15, mrpt::opengl::FILL, 100);
  
  			win3D.addTextMessage(
  				10, 10,
  				"'o'/'i'-zoom out/in, '2'/'3'/'f':show/hide 2D/3D/frustum "
  				"data, mouse: orbit 3D, ESC: quit",
  				TColorf(1, 1, 1), "sans", 10, mrpt::opengl::FILL, 110);
  
  			win3D.addTextMessage(
  				10, 25, format(
  							"Show: 3D=%s 2D=%s Frustum=%s (vert. FOV=%.1fdeg)",
  							gl_points->isVisible() ? "YES" : "NO",
  							gl_2d_scan->isVisible() ? "YES" : "NO",
  							gl_frustum->isVisible() ? "YES" : "NO",
  							gl_frustum->getVertFOV()),
  				TColorf(1, 1, 1), "sans", 10, mrpt::opengl::FILL, 111);
  			win3D.unlockAccess3DScene();
  
  			// Do we have accelerometer data?
  			if (last_obs_imu && last_obs_imu->dataIsPresent[IMU_X_ACC])
  			{
  				win3D.get3DSceneAndLock();
  				win3D.addTextMessage(
  					10, 65, format(
  								"Acc: x=%.02f y=%.02f z=%.02f",
  								last_obs_imu->rawMeasurements[IMU_X_ACC],
  								last_obs_imu->rawMeasurements[IMU_Y_ACC],
  								last_obs_imu->rawMeasurements[IMU_Z_ACC]),
  					TColorf(.7, .7, .7), "sans", 10, mrpt::opengl::FILL, 102);
  				win3D.unlockAccess3DScene();
  				do_refresh = true;
  			}
  
  			// Force opengl repaint:
  			if (do_refresh) win3D.repaint();
  
  		}  // end update visualization:
  
  		// Process possible keyboard commands:
  		// --------------------------------------
  		if (win3D.keyHit() && thrPar.pushed_key == 0)
  		{
  			const int key = tolower(win3D.getPushedKey());
  
  			switch (key)
  			{
  				// Some of the keys are processed in this thread:
  				case 'o':
  					win3D.setCameraZoom(win3D.getCameraZoom() * 1.2);
  					win3D.repaint();
  					break;
  				case 'i':
  					win3D.setCameraZoom(win3D.getCameraZoom() / 1.2);
  					win3D.repaint();
  					break;
  				case '2':
  					gl_2d_scan->setVisibility(!gl_2d_scan->isVisible());
  					break;
  				case '3':
  					gl_points->setVisibility(!gl_points->isVisible());
  					break;
  				case 'F':
  				case 'f':
  					gl_frustum->setVisibility(!gl_frustum->isVisible());
  					break;
  				case '+':
  					gl_frustum->setVertFOV(gl_frustum->getVertFOV() + 1);
  					break;
  				case '-':
  					gl_frustum->setVertFOV(gl_frustum->getVertFOV() - 1);
  					break;
  				// ...and the rest in the kinect thread:
  				default:
  					thrPar.pushed_key = key;
  					break;
  			};
  		}
  
  		std::this_thread::sleep_for(1ms);
  	}
  
  	cout << "Waiting for grabbing thread to exit...\n";
  	thrPar.quit = true;
  	thHandle.join();
  	cout << "Bye!\n";
  }
  
  int main(int argc, char** argv)
  {
  	try
  	{
  		Test_Kinect();
  
  		std::this_thread::sleep_for(50ms);
  		return 0;
  	}
  	catch (std::exception& e)
  	{
  		std::cout << "EXCEPCION: " << e.what() << std::endl;
  		return -1;
  	}
  	catch (...)
  	{
  		printf("Another exception!!");
  		return -1;
  	}
  }
  