//***************************************************************************************/
// 
// File name: MicrosoftKinect.cpp  
// Location:  ...\Matrox Imaging\MILxxx\Examples\Processing\3dCamera\MicrosoftKinect\C++
//             
//
// Synopsis:  This program demonstrate how to interface the Microsoft Kinect camera to create
//            depth maps in MIL.
//            See the PrintHeader() function below for detailed description.
//
// Copyright (C) Matrox Electronic Systems Ltd., 1992-2015.
// All Rights Reserved


#include <mil.h>
#include <Windows.h>
#include <float.h>
#include <math.h>
#include "KinectCamera.h"
#include "KinectCameraStandalone.h"
#include "BestPlaneFitter.h"
#include "DepthDataMgr.h"

#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE
/* Include DirectX display only on Windows. */
#include "MdispD3D.h"

/* D3D display parameters */
static const MIL_INT D3D_DISPLAY_SIZE_X = 640;
static const MIL_INT D3D_DISPLAY_SIZE_Y = 480;
static const MIL_DOUBLE MIN_Z           = 0;
static const MIL_DOUBLE MAX_Z           = 3000; // 3000 mm is the maximum Z in near mode
static const MIL_DOUBLE MAX_DISTANCE_Z  = 100;  // 3d points with distance > 100mm are not linked in the 3d display  
#endif

//****************************************************************************
// Example description.
//****************************************************************************
void PrintHeader()   
   {
   MosPrintf(MIL_TEXT("[EXAMPLE NAME]\n")
             MIL_TEXT("Microsoft Kinect\n\n")

             MIL_TEXT("[SYNOPSIS]\n")
             MIL_TEXT("This example illustrates how to interface with the Microsoft Kinect to \n")
             MIL_TEXT("create and use depth maps in MIL. Specifically, this example shows:\n")
             MIL_TEXT("   - Kinect camera system calibration.\n")
             MIL_TEXT("   - Acquisition of color and depth images.\n")
             MIL_TEXT("   - Mapping of the color image on the depth image.\n")
             MIL_TEXT("   - Extraction of the depth map and mapping of the color image.\n")
             MIL_TEXT("   - Plane fitting of the floor.\n")
             MIL_TEXT("   - Localization of the 3D bounding box of an object on the floor.\n\n")
             MIL_TEXT("The example is designed to work with both the first generation Kinect\n")
             MIL_TEXT("and the second generation Kinect, also known as the Kinect One.\n")
             MIL_TEXT("Change KINECT_CAMERA_VERSION to select the Kinect generation to use.\n\n")


             MIL_TEXT("[MODULES USED]\n")
             MIL_TEXT("Modules used: Application, system, image, calibration, 3dmap\n\n")
  
             MIL_TEXT("Press <Enter> to continue.\n\n"));
   MosGetch();
   }

// User's processing function hook data structures.
struct SGetDepthProcFuncData
   {
   CKinectCameraInterface* pKinectCamera;   

   MIL_ID MilDepthImageDisplay;

   MIL_ID MilColorImage;
   MIL_ID MilDepthImage;
   MIL_ID MilDepthProcImage;
   MIL_ID MilDepthProc2Image;
   MIL_ID MilDepthThresholdImage;
   };

struct SGetDepthColorProcFuncData
   {   
   SGetDepthProcFuncData GetDepthData;
   MIL_ID MilColorCameraCalibration;

   CDepthDataMgr* pDepthDataMgr;
   
   MIL_ID MilColorDepthOrWorldImage;
   bool IsDepthMapLut;
   };

struct SGet3DMeshColorProcFuncData
   {   
   SGetDepthColorProcFuncData GetDepthColorData;   
   MIL_ID MilColorGraList;
   MIL_ID MilColorDisplay;
   MIL_ID MilDepthMapImage;
   MIL_ID MilDepthMapDisplay;
   MIL_ID MilColorDepthMapImage;
   MIL_ID MilColorDepthMapDisplay;

   CBestPlaneFitter* pBestPlaneFitter;
   MIL_ID MilBinDepthMapImage;
   MIL_ID MilBlobResult;
   MIL_ID MilBlobFeatureList;

   MIL_DISP_D3D_HANDLE DispD3DHandle;
   MIL_ID Mil3dDepthMapImage;
   MIL_ID Mil3dColorDepthMapImage;
   MIL_ID MilD3DSetImagesThread;
   MIL_ID MilD3DUpdateEvent;
   MIL_ID MilStopUpdate3dDisplayEvent;

   bool BoxFound;
   };

// Box result structure.
struct SBox
   {
   MIL_DOUBLE CenterPosX;
   MIL_DOUBLE CenterPosY;
   MIL_DOUBLE CenterPosZ;
   MIL_DOUBLE ZAxisAngle;
   MIL_DOUBLE Width;
   MIL_DOUBLE Length;
   MIL_DOUBLE Height;
   };

// Camera calibration functions.
void CalibrateCamera(CKinectCameraInterface* pKinectCamera,
                     MIL_ID MilCameraCalibration,
                     MIL_ID MilGrabImage,
                     MIL_ID MilDisplay,
                     ColorStreamTypeEnum ColorStreamType);

void MoveColorToolCoordSystemOnIRCameraCoordSystem(MIL_ID MilSystem,
                                                     MIL_ID MilIRCameraCalibration,
                                                     MIL_ID MilColorCameraCalibration);


// Object finding functions.
bool FindObjectOnFloor(MIL_ID MilDepthMapImage,
                       MIL_ID MilBinDepthMapImage,
                       MIL_ID MilBlobResult,
                       MIL_ID MilBlobFeatureList,
                       MIL_DOUBLE FloorZ,
                       MIL_DOUBLE MinThresholdZ,
                       MIL_DOUBLE MaxThresholdZ,
                       SBox* pFoundBox);

void DrawFound3dBox(MIL_ID MilGraContext, MIL_ID MilDrawDest, MIL_ID MilCalibration, const SBox& rFoundBox);

// 3d display update functions.
MIL_UINT32 MFTYPE Update3dDisplayThread(void *UserDataPtr);
void Update3dDisplay(const SGet3DMeshColorProcFuncData* pGet3DMeshColorData);

// Helper functions.
void SetupDisplay(MIL_ID MilDisplay, MIL_CONST_TEXT_PTR WindowTitle, MIL_INT WindowPosX, MIL_INT WindowPosY);
void FatalError(CKinectCameraInterface* pKinectCamera, const MIL_TEXT_CHAR* pMsg);
void FillDisplayLut(MIL_ID MilDisplayLut);

// User's processing functions. 
MIL_INT MFTYPE CopyFunc(MIL_INT HookType, MIL_ID MilKinectImage, void* UserDataPtr);
MIL_INT MFTYPE GetDepthProcFunc(MIL_INT HookType, MIL_ID MilKinectImage, void* UserDataPtr);
MIL_INT MFTYPE GetDepthColorProcFunc(MIL_INT HookType, MIL_ID MilKinectImage, void* UserDataPtr);
MIL_INT MFTYPE Get3DMeshColorProcFunc(MIL_INT HookType, MIL_ID MilKinectImage, void* UserDataPtr);

// Depth map generation parameters.
static const MIL_INT    DEPTH_MAP_SIZE_X =   400;
static const MIL_INT    DEPTH_MAP_SIZE_Y =   300;

// The (0,0) world position is at the pinhole of the Kinect camera.
static const MIL_DOUBLE WORLD_POS_X       = -1000.0;                                                       // in mm 
static const MIL_DOUBLE WORLD_POS_Y       = WORLD_POS_X * (MIL_DOUBLE)DEPTH_MAP_SIZE_Y / DEPTH_MAP_SIZE_X; // in mm 
static const MIL_DOUBLE WORLD_POS_Z       = 400.0;                                                         // in mm
static const MIL_DOUBLE PIXEL_SIZE_X      = (2 * -WORLD_POS_X) / DEPTH_MAP_SIZE_X;                         // in mm/pixel        
static const MIL_DOUBLE PIXEL_SIZE_Y      = PIXEL_SIZE_X;                                                  // in mm/pixel        
static const MIL_DOUBLE GRAY_LEVEL_SIZE_Z = 1.0;                                                           // in mm/gray val
static const SDepthMapGenParam KINECT_DEPTH_PARAM = {WORLD_POS_X, WORLD_POS_Y, WORLD_POS_Z,
                                                     PIXEL_SIZE_X, PIXEL_SIZE_Y, GRAY_LEVEL_SIZE_Z};

// The top view parameters.
static const MIL_DOUBLE TOP_VIEW_WORLD_POS_Z = -1500;   // in mm 
static const MIL_DOUBLE OBJECT_THRESHOLD_Z   = 100;     // in mm
static const MIL_DOUBLE TOP_REFINE_RANGE     = 20;      // in mm
static const SDepthMapGenParam TOP_DEPTH_PARAM = {WORLD_POS_X, WORLD_POS_Y, TOP_VIEW_WORLD_POS_Z,
                                                  PIXEL_SIZE_X, PIXEL_SIZE_Y, GRAY_LEVEL_SIZE_Z};

// The calibration grid parameters.
static const MIL_DOUBLE GRID_WORLD_OFFSET_X  = 0.0; // in mm
static const MIL_DOUBLE GRID_WORLD_OFFSET_Y  = 0.0; // in mm
static const MIL_INT    GRID_NB_ROWS         = 15;   
static const MIL_INT    GRID_NB_COLUMNS      = 11;
static const MIL_DOUBLE GRID_SPACING         = 50;  // in mm

// The minimum requirement of the selected blobs.
static const MIL_DOUBLE MIN_BLOB_AREA      = 20000; // in mm2
static const MIL_DOUBLE MIN_BLOB_FERET_MIN = 100;   // in mm
static const MIL_DOUBLE MAX_SIGMA_PIXEL    = 75;    // in mm 
static const MIL_DOUBLE MIN_RECTANGULARITY = 0.7;    // in percentage 


// The blob extraction parameters.
static const MIL_INT NB_FERETS = M_INFINITE;

// The depth image invalid data masking.
static const MIL_INT DEPTH_THIRD_DERIV_FACTOR = 16;

// The display text line offset.
static const MIL_INT DISPLAY_TEXT_LINE_OFFSET = 16;

// Display windows positions.
static const MIL_INT WINDOWS_OFFSET_X = 15;
static const MIL_INT WINDOWS_OFFSET_Y = 38;
static const MIL_INT DEPTH_DISPLAY_X = 0;
static const MIL_INT DEPTH_DISPLAY_Y = 0;
static const MIL_INT COLOR_DISPLAY_X = KINECT_DEPTH_IMAGE_SIZE_X + 100;
static const MIL_INT COLOR_DISPLAY_Y = 0;
static const MIL_INT COLOR_DEPTH_DISPLAY_X = 100;
static const MIL_INT COLOR_DEPTH_DISPLAY_Y = 0;
static const MIL_INT DEPTH_MAP_DISPLAY_X = 0;
static const MIL_INT DEPTH_MAP_DISPLAY_Y = D3D_DISPLAY_SIZE_Y + WINDOWS_OFFSET_Y;
static const MIL_INT COLOR_MAP_DISPLAY_X = DEPTH_MAP_SIZE_X + WINDOWS_OFFSET_X;
static const MIL_INT COLOR_MAP_DISPLAY_Y = D3D_DISPLAY_SIZE_Y + WINDOWS_OFFSET_Y;


//*****************************************************************************
// Main.
//*****************************************************************************
int MosMain(void)
   {
   // Allocate the application.
   MIL_ID MilApplication = MappAlloc(M_DEFAULT, M_NULL);
   
   // Allocate the system.
   MIL_ID MilSystem = MsysAlloc(M_SYSTEM_HOST, M_DEFAULT, M_DEFAULT, M_NULL);

   // Allocate the displays.
   MIL_ID MilDepthImageDisplay = MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, M_NULL);
   SetupDisplay(MilDepthImageDisplay, MIL_TEXT("Depth Image"), DEPTH_DISPLAY_X, DEPTH_DISPLAY_Y);
   MIL_ID MilColorDisplay = MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, M_NULL);
   SetupDisplay(MilColorDisplay, MIL_TEXT("Color Image"), COLOR_DISPLAY_X, COLOR_DISPLAY_Y);
   MIL_ID MilColorDepthImageDisplay = MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, M_NULL);
   SetupDisplay(MilColorDepthImageDisplay, MIL_TEXT("Color Depth Image"), COLOR_DEPTH_DISPLAY_X, COLOR_DEPTH_DISPLAY_Y);
   MIL_ID MilDepthMapDisplay = MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, M_NULL);
   SetupDisplay(MilDepthMapDisplay, MIL_TEXT("Depth Map"), DEPTH_MAP_DISPLAY_X, DEPTH_MAP_DISPLAY_Y);
   MIL_ID MilColorDepthMapDisplay = MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, M_NULL);
   SetupDisplay(MilColorDepthMapDisplay, MIL_TEXT("Color Depth Map"), COLOR_MAP_DISPLAY_X, COLOR_MAP_DISPLAY_Y);

   // Allocate the graphic list associated to the color display.
   MIL_ID MilColorGraList = MgraAllocList(MilSystem, M_DEFAULT, M_NULL);
   MdispControl(MilColorDisplay, M_ASSOCIATED_GRAPHIC_LIST_ID, MilColorGraList);
         
   MosPrintf(MIL_TEXT("MICROSOFT KINECT:\n"));
   MosPrintf(MIL_TEXT("---------------------------------------\n\n"));
   
   // Print the header.
   PrintHeader();

   // Create a Kinect camera and initialize it.
   CKinectCameraInterface* pKinectCamera;
#if USE_REAL_CAMERA
   #if KINECT_CAMERA_VERSION == 1
      pKinectCamera = new CKinectCamera(MilSystem);
   #elif KINECT_CAMERA_VERSION == 2
      pKinectCamera = new CKinectOneCamera(MilSystem);
   #else
      FatalError(pKinectCamera, MIL_TEXT("Unable to create camera sensor. Invalid KINECT_CAMERA_VERSION.\n"));
   #endif
#else
   pKinectCamera = new CKinectCameraStandalone(MilSystem);
#endif
   if(pKinectCamera->GetCameraStatus() != KINECT_CAMERA_OK)
      FatalError(pKinectCamera, MIL_TEXT("Unable to create camera sensor.\n"));
      
   // Allocate the color images.
   MIL_ID MilColorImage = MbufAllocColor(MilSystem, 3, KINECT_COLOR_IMAGE_SIZE_X, KINECT_COLOR_IMAGE_SIZE_Y, 8+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP, M_NULL);
   MIL_ID MilColorDepthImage = MbufAllocColor(MilSystem, 3, KINECT_DEPTH_IMAGE_SIZE_X, KINECT_DEPTH_IMAGE_SIZE_Y, 8+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP+M_BGR32+M_PACKED, M_NULL);
   MIL_ID MilColorWorldImageContainer = MbufAllocColor(MilSystem, 3, KINECT_DEPTH_IMAGE_SIZE_X*KINECT_DEPTH_IMAGE_SIZE_Y, 1, 8 + M_UNSIGNED, M_IMAGE + M_PROC + M_DISP + M_BGR32 + M_PACKED, M_NULL);
   MIL_ID MilColorWorldImage = MbufChild1d(MilColorWorldImageContainer, 0, 1, M_NULL);
   MIL_ID MilColorDepthMapImage = MbufAllocColor(MilSystem, 3, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, 8+M_UNSIGNED, M_IMAGE+M_PROC+M_BGR32+M_PACKED+M_DISP, M_NULL);  
   MIL_ID Mil3dColorDepthMapImage = MbufAllocColor(MilSystem, 3, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, 8+M_UNSIGNED, M_IMAGE+M_PROC+M_BGR32+M_PACKED+M_DISP, M_NULL);

   // Allocate the images for the depth.
   MIL_ID MilDepthImage = MbufAlloc2d(MilSystem, KINECT_DEPTH_IMAGE_SIZE_X, KINECT_DEPTH_IMAGE_SIZE_Y, 16+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP, M_NULL);
   MIL_ID MilDepthProcImage = MbufAlloc2d(MilSystem, KINECT_DEPTH_IMAGE_SIZE_X, KINECT_DEPTH_IMAGE_SIZE_Y, 16+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP, M_NULL);
   MIL_ID MilDepthProc2Image = MbufAlloc2d(MilSystem, KINECT_DEPTH_IMAGE_SIZE_X, KINECT_DEPTH_IMAGE_SIZE_Y, 16 + M_UNSIGNED, M_IMAGE + M_PROC + M_DISP, M_NULL);
   MIL_ID MilDepthThresholdImage = MbufAlloc2d(MilSystem, KINECT_DEPTH_IMAGE_SIZE_X, KINECT_DEPTH_IMAGE_SIZE_Y, 16 + M_UNSIGNED, M_IMAGE + M_PROC + M_DISP, M_NULL);

   // Allocate the depth map images
   MIL_ID MilDepthMapImage = MbufAlloc2d(MilSystem, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, 16+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP, M_NULL);
   MIL_ID MilBinDepthMapImage = MbufAlloc2d(MilSystem, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, 8+M_UNSIGNED, M_IMAGE+M_PROC, M_NULL);
   MIL_ID Mil3dDepthMapImage = MbufAlloc2d(MilSystem, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, 16+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP, M_NULL);

   // Allocate blob to find the objects box and set its parameters and features.
   MIL_ID MilBlobResult = MblobAllocResult(MilSystem, M_NULL);
   MIL_ID MilBlobFeatureList = MblobAllocFeatureList(MilSystem, M_NULL);
   MblobControl(MilBlobResult, M_RESULT_OUTPUT_UNITS, M_WORLD);
   MblobControl(MilBlobResult, M_INPUT_SELECT_UNITS, M_WORLD);
   MblobControl(MilBlobResult, M_NUMBER_OF_FERETS, NB_FERETS);
   MblobSelectFeature(MilBlobFeatureList, M_RECTANGULARITY);
   MblobSelectFeature(MilBlobFeatureList, M_MEAN_PIXEL);
   MblobSelectFeature(MilBlobFeatureList, M_SIGMA_PIXEL);
   MblobSelectFeature(MilBlobFeatureList, M_MIN_AREA_BOX);

   // Allocate the calibration objects of both cameras of the Kinect.
   MIL_ID MilColorCameraCalibration = McalAlloc(MilSystem, M_TSAI_BASED, M_DEFAULT, M_NULL);
   MIL_ID MilIRCameraCalibration = McalAlloc(MilSystem, M_TSAI_BASED, M_DEFAULT, M_NULL);

   // Create the display lut and set it on the display.
   MIL_ID MilDisplayLut = MbufAllocColor(MilSystem, 3, MIL_UINT16_MAX, 1, 8+M_UNSIGNED, M_LUT, M_NULL);
   FillDisplayLut(MilDisplayLut);
   MdispLut(MilDepthImageDisplay, MilDisplayLut);
   MdispLut(MilDepthMapDisplay, MilDisplayLut);

   // Select the IR image on the display.
   MbufClear(MilDepthImage, 0.0);
   MdispSelect(MilColorDisplay, MilDepthImage);
   MdispControl(MilColorDisplay, M_VIEW_MODE, M_BIT_SHIFT);
   MdispControl(MilColorDisplay, M_VIEW_BIT_SHIFT, 8);

    // Calibrate the IR Camera.
   CalibrateCamera(pKinectCamera, MilIRCameraCalibration, MilDepthImage, MilColorDisplay, enIR);
 
   // Select the color image.    
   MdispSelect(MilColorDisplay, MilColorImage);
   MdispControl(MilColorDisplay, M_VIEW_MODE, M_DEFAULT);
         
   // Calibrate the color camera.
   CalibrateCamera(pKinectCamera, MilColorCameraCalibration, MilColorImage, MilColorDisplay, enColor);

   // Remove the link between the tool and the camera coordinate system.
   McalControl(MilColorCameraCalibration, M_LINK_TOOL_AND_CAMERA, M_DISABLE);

   // Move some coordinate systems on the IRCamera.
   MoveColorToolCoordSystemOnIRCameraCoordSystem(MilSystem, MilIRCameraCalibration, MilColorCameraCalibration);
         
   // Reinitialize the camera to grab the right type of image.
   if(pKinectCamera->InitCamera(enColor, true) != KINECT_CAMERA_OK)   
      FatalError(pKinectCamera, MIL_TEXT("Unable to initialize camera system.\n"));
           
   // Fill FrameGrabberProcess hook function structure for the GetDepthProcFunc.
   SGetDepthProcFuncData GetDepthData;
   GetDepthData.pKinectCamera = pKinectCamera;
   GetDepthData.MilColorImage = MilColorImage;
   GetDepthData.MilDepthImage = MilDepthImage;
   GetDepthData.MilDepthProcImage = MilDepthProcImage;
   GetDepthData.MilDepthProc2Image = MilDepthProc2Image;
   GetDepthData.MilDepthThresholdImage = MilDepthThresholdImage;
   GetDepthData.MilDepthImageDisplay = MilDepthImageDisplay;

   // Select the depth image on the display.
   MdispSelect(MilDepthImageDisplay, MilDepthImage);

   // Grab depth image continuously.
   pKinectCamera->FrameGrabberProcess(M_START, M_ASYNCHRONOUS, M_DEFAULT, &GetDepthProcFunc, &GetDepthData);

   MosPrintf(MIL_TEXT("The depth images from the Kinect are displayed.\n")
             MIL_TEXT("Press <Enter> to stop the grab.\n\n"));

   MosGetch();
   
   // Stop the grab.
   pKinectCamera->FrameGrabberProcess(M_STOP, M_DEFAULT);   
   
   // Fill FrameGrabberProcess hook function structure for the GetDepthColorProcFunc.
   SGetDepthColorProcFuncData GetDepthColorData;
   GetDepthColorData.GetDepthData               = GetDepthData;
   GetDepthColorData.MilColorCameraCalibration  = MilColorCameraCalibration;
   GetDepthColorData.MilColorDepthOrWorldImage  = MilColorDepthImage;
   GetDepthColorData.IsDepthMapLut = false;
      
   // Allocate the depth data manager.
   CDepthDataMgr* pDepthDataMgr = new CDepthDataMgr(MilSystem, KINECT_DEPTH_IMAGE_SIZE_X, KINECT_DEPTH_IMAGE_SIZE_Y, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, KINECT_COLOR_IMAGE_SIZE_X, KINECT_COLOR_IMAGE_SIZE_Y);
   pDepthDataMgr->CalculateMultipliers(MilIRCameraCalibration);
   GetDepthColorData.pDepthDataMgr = pDepthDataMgr;   

   // Select the color depth image on the display.
   MdispSelect(MilColorDepthImageDisplay, MilColorDepthImage);
   
   // Grab colored depth continuously.
   pKinectCamera->FrameGrabberProcess(M_START, M_ASYNCHRONOUS, M_DEFAULT, &GetDepthColorProcFunc, &GetDepthColorData);

   MosPrintf(MIL_TEXT("The colored depth images are displayed.\n"));

   MosPrintf(MIL_TEXT("Press <Enter> to stop the grab.\n\n"));
   MosGetch();

   // Stop the grab.
   pKinectCamera->FrameGrabberProcess(M_STOP, M_DEFAULT);
  
   // Create the depth map.
   if(pDepthDataMgr->GetNbWorld())
      {
      // Generate the color to world luts and move the color world image child.
      pDepthDataMgr->GenColorLuts(MilColorCameraCalibration, true);
      MbufChildMove(MilColorWorldImage, 0, 0, pDepthDataMgr->GetNbWorld(), 1, M_DEFAULT);
      
      // Create the color world image.
      pDepthDataMgr->WarpColorImage(MilColorImage, MilColorWorldImage, true);

      // Create the depth map.
      pDepthDataMgr->CreateDepthMap(MilColorWorldImage, MilDepthMapImage, MilColorDepthMapImage, KINECT_DEPTH_PARAM);
      }

#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE
   MIL_DISP_D3D_HANDLE DispD3DHandle = M_NULL;
   
   // Try to allocate D3D display.       
   DispD3DHandle = MdepthD3DAlloc(MilDepthMapImage, MilColorDepthMapImage,
                                  D3D_DISPLAY_SIZE_X,
                                  D3D_DISPLAY_SIZE_Y,
                                  M_DEFAULT,
                                  M_DEFAULT,
                                  M_DEFAULT,
                                  MIN_Z,
                                  MAX_Z,
                                  MAX_DISTANCE_Z,
                                  M_NULL);

   if (DispD3DHandle != NULL)
      {
      // Show the display and print the help.
      MdispD3DShow(DispD3DHandle);
      MdispD3DPrintHelp(DispD3DHandle);
      }

#endif

   // Fill FrameGrabberProcess hook function structure for the Get3DMeshColorProcFunc.
   SGet3DMeshColorProcFuncData Get3DMeshColorData;
   Get3DMeshColorData.GetDepthColorData         = GetDepthColorData;
   Get3DMeshColorData.MilColorGraList           = MilColorGraList;
   Get3DMeshColorData.MilColorDisplay           = MilColorDisplay;
   Get3DMeshColorData.MilDepthMapImage          = MilDepthMapImage;
   Get3DMeshColorData.MilDepthMapDisplay        = MilDepthMapDisplay;
   Get3DMeshColorData.MilColorDepthMapImage     = MilColorDepthMapImage;
   Get3DMeshColorData.MilColorDepthMapDisplay   = MilColorDepthMapDisplay;
   Get3DMeshColorData.Mil3dDepthMapImage        = Mil3dDepthMapImage;
   Get3DMeshColorData.Mil3dColorDepthMapImage   = Mil3dColorDepthMapImage;
   Get3DMeshColorData.DispD3DHandle             = DispD3DHandle;
   
   // Allocate the best plane fitter and set the pointer in the structure.
   CBestPlaneFitter* pBestPlaneFitter = new CBestPlaneFitter(MilSystem, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y);
   Get3DMeshColorData.pBestPlaneFitter = pBestPlaneFitter;

   Get3DMeshColorData.MilBinDepthMapImage = MilBinDepthMapImage;
   Get3DMeshColorData.MilBlobResult       = MilBlobResult;
   Get3DMeshColorData.MilBlobFeatureList  = MilBlobFeatureList;

   Get3DMeshColorData.BoxFound = false;

   // Set the generation of the color image to be a color map.
   Get3DMeshColorData.GetDepthColorData.IsDepthMapLut = true;
   Get3DMeshColorData.GetDepthColorData.MilColorDepthOrWorldImage = MilColorWorldImage;
   
   // Start the 3d display update thread.
   MthrAlloc(MilSystem, M_EVENT, M_NOT_SIGNALED + M_MANUAL_RESET, M_NULL, M_NULL, &Get3DMeshColorData.MilStopUpdate3dDisplayEvent);
   MthrAlloc(MilSystem, M_EVENT, M_NOT_SIGNALED + M_AUTO_RESET, M_NULL, M_NULL, &Get3DMeshColorData.MilD3DUpdateEvent);
   MthrAlloc(MilSystem, M_THREAD, M_DEFAULT, Update3dDisplayThread,
             &Get3DMeshColorData, &Get3DMeshColorData.MilD3DSetImagesThread);
   
   // Deselect the color depth image display since it is not generated anymore.
   MdispSelect(MilColorDepthImageDisplay, M_NULL);

   // Select the depth and color map on the displays
   MdispSelect(MilColorDepthMapDisplay, MilColorDepthMapImage);
   MdispSelect(MilDepthMapDisplay, MilDepthMapImage);
   
   // Grab depth maps continuously.
   pKinectCamera->FrameGrabberProcess(M_START, M_ASYNCHRONOUS, M_DEFAULT, &Get3DMeshColorProcFunc, &Get3DMeshColorData);
   
   MosPrintf(MIL_TEXT("The corrected and colored depth map as well as the fitted floor\n")
             MIL_TEXT("and the 3D bounding box are displayed live.\n")
             MIL_TEXT("Press <Enter> to stop the grab.\n\n"));
   MosGetch();
   
   // Stop the grab.
   pKinectCamera->FrameGrabberProcess(M_STOP, M_DEFAULT);   

   if(Get3DMeshColorData.BoxFound)
      MosPrintf(MIL_TEXT("The final corrected and colored depth map as well as the fitted floor \n")
                MIL_TEXT("and the 3D bounding box are displayed.\n")
                MIL_TEXT("Press <Enter> to end.\n\n"));
   else
      MosPrintf(MIL_TEXT("The final corrected and colored depth map as well as the fitted floor \n")
                MIL_TEXT("are displayed.\n")
                MIL_TEXT("Press <Enter> to end.\n\n"));
   MosGetch();
   
   // Stop the update of the 3d display and free the event and thread.
   MthrControl(Get3DMeshColorData.MilStopUpdate3dDisplayEvent, M_EVENT_SET, M_SIGNALED);
   MthrControl(Get3DMeshColorData.MilD3DUpdateEvent, M_EVENT_SET, M_SIGNALED);
   MthrWait(Get3DMeshColorData.MilD3DSetImagesThread, M_THREAD_END_WAIT, M_NULL);
   MthrFree(Get3DMeshColorData.MilD3DSetImagesThread);
   MthrFree(Get3DMeshColorData.MilD3DUpdateEvent);
   MthrFree(Get3DMeshColorData.MilStopUpdate3dDisplayEvent);

   // Free the 3d display.
#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE
   if (DispD3DHandle != M_NULL)
      {
      MdispD3DHide(DispD3DHandle);
      MdispD3DFree(DispD3DHandle);
      }
#endif

   // Delete the plane fitter.
   delete pBestPlaneFitter;

   // Delete the depth data manager.
   delete pDepthDataMgr;

   // Close down the camera
   pKinectCamera->CloseDown();

   // Delete the camera.
   delete pKinectCamera;
 
   // Free the calibrations.
   McalFree(MilColorCameraCalibration);
   McalFree(MilIRCameraCalibration);
  
   // Free the buffers.
   MbufFree(MilDisplayLut);
   MbufFree(MilColorDepthMapImage);
   MbufFree(MilColorWorldImage);
   MbufFree(MilColorWorldImageContainer);
   MbufFree(MilColorDepthImage);
   MbufFree(MilColorImage);
   MbufFree(MilDepthMapImage);
   MbufFree(MilDepthThresholdImage);
   MbufFree(MilDepthProc2Image);
   MbufFree(MilDepthProcImage);
   MbufFree(MilDepthImage);
   MbufFree(Mil3dColorDepthMapImage);
   MbufFree(Mil3dDepthMapImage);
   MbufFree(MilBinDepthMapImage);

   // Free blob
   MblobFree(MilBlobResult);
   MblobFree(MilBlobFeatureList);

   // Free the graphic list.
   MgraFree(MilColorGraList);

   // Free the displays.     
   MdispFree(MilColorDisplay);
   MdispFree(MilDepthImageDisplay);
   MdispFree(MilColorDepthImageDisplay);
   MdispFree(MilDepthMapDisplay);
   MdispFree(MilColorDepthMapDisplay);

   // Free the system.
   MsysFree(MilSystem);

   // Free the application.
   MappFree(MilApplication);

   return 0;
   }

//*****************************************************************************
// Hook function for FrameGrabberProcess(). Copy the Kinect image.
//*****************************************************************************
MIL_INT MFTYPE CopyFunc(MIL_INT HookType, MIL_ID MilKinectImage, void* UserDataPtr)
   {
   MIL_ID* pMilImage = (MIL_ID*) UserDataPtr;
   CKinectCameraInterface::ConvertKinectDepthToMilDepth(MilKinectImage, *pMilImage);
   return M_NULL;
   }

//*****************************************************************************
// Hook function for FrameGrabberProcess(). Converts the depth buffer data returned 
// by the Kinect to a depth image.
//*****************************************************************************
MIL_INT MFTYPE GetDepthProcFunc(MIL_INT HookType, MIL_ID MilKinectImage, void* UserDataPtr)
   {
   SGetDepthProcFuncData* pGetDepthData = static_cast<SGetDepthProcFuncData*>(UserDataPtr);

   // If the image grabbed by the Kinect is the color image.
   if (MbufInquire(MilKinectImage, M_SIZE_BAND, M_NULL) == 3)
      CKinectCameraInterface::ConvertKinectColorToMilColor(MilKinectImage, pGetDepthData->MilColorImage);
   else
      {
      // Disable the display updates.
      MdispControl(pGetDepthData->MilDepthImageDisplay, M_UPDATE, M_DISABLE);

      // Get the depth image.
      CKinectCameraInterface::ConvertKinectDepthToMilDepth(MilKinectImage, pGetDepthData->MilDepthImage);     

#if KINECT_CAMERA_VERSION == 2
      // The Kinect 2.0 is much less noisy around edges and may have data along its edges. The depth value
      // on the edges are not valid since they are 3d point interpolated between two surface.
      // We will then try to process the image to ignore edge data.

      // Clean the depth image. The third derivative is sensitive to noise.
      MimRank(pGetDepthData->MilDepthImage, pGetDepthData->MilDepthProcImage, M_5X5_RECT, M_MEDIAN, M_GRAYSCALE);

      // Create the threshold image.
      MimArith(pGetDepthData->MilDepthProcImage, DEPTH_THIRD_DERIV_FACTOR, pGetDepthData->MilDepthThresholdImage, M_DIV_CONST);

      // Get the positive part of the third derivative of the image.      
      MimConvolve(pGetDepthData->MilDepthProcImage, pGetDepthData->MilDepthProc2Image, M_EDGE_DETECT_SOBEL_FAST);
      MimConvolve(pGetDepthData->MilDepthProc2Image, pGetDepthData->MilDepthProcImage, M_LAPLACIAN_8);
      MimConvolve(pGetDepthData->MilDepthProcImage, pGetDepthData->MilDepthProc2Image, M_SMOOTH);
      
      // Find the pixels whose third derivative is greater than the threshold.
      MimArith(pGetDepthData->MilDepthThresholdImage, pGetDepthData->MilDepthProc2Image, pGetDepthData->MilDepthProc2Image, M_SUB + M_SATURATION);
      
      // Add a slight margin around the invalid pixels.
      MimErode(pGetDepthData->MilDepthProc2Image, pGetDepthData->MilDepthProcImage, 1, M_BINARY);
      
      // Set the corresponding pixels as invalid data in the original image.
      MbufClearCond(pGetDepthData->MilDepthImage, 0, 0, 0, pGetDepthData->MilDepthProcImage, M_EQUAL, 0);
#endif

      // Enable the display updates.
      MdispControl(pGetDepthData->MilDepthImageDisplay, M_UPDATE, M_ENABLE);
      }

   return M_NULL;
   }

//*****************************************************************************
// Hook function for FrameGrabberProcess(). Converts the depth buffer data returned 
// by the Kinect and map the colors of the color image on the depth image.
//*****************************************************************************
MIL_INT MFTYPE GetDepthColorProcFunc(MIL_INT HookType, MIL_ID MilKinectImage, void* UserDataPtr)
   {
   SGetDepthColorProcFuncData* pGetDepthColorData = static_cast<SGetDepthColorProcFuncData*>(UserDataPtr);
   SGetDepthProcFuncData& GetDepthData = pGetDepthColorData->GetDepthData;
   CDepthDataMgr* pDepthDataMgr = pGetDepthColorData->pDepthDataMgr;

   // Call the hook to get the depth.
   GetDepthProcFunc(HookType, MilKinectImage, &GetDepthData);
   
   // If the image grabbed by the Kinect is the depth image.
   if(MbufInquire(MilKinectImage, M_SIZE_BAND, M_NULL) == 1)
      {
      // Calculate the world points.
      if(pGetDepthColorData->IsDepthMapLut)
         pDepthDataMgr->CalculateWorldPoints<false>(GetDepthData.MilDepthImage);
      else
         pDepthDataMgr->CalculateWorldPoints<true>(GetDepthData.MilDepthImage);

      // Generate the luts.
      pDepthDataMgr->GenColorLuts(pGetDepthColorData->MilColorCameraCalibration, pGetDepthColorData->IsDepthMapLut);
      
      // Create the color depth or world image.
      if(pDepthDataMgr->GetNbWorld())
         {
         if(pGetDepthColorData->IsDepthMapLut)
            MbufChildMove(pGetDepthColorData->MilColorDepthOrWorldImage, 0, 0, pDepthDataMgr->GetNbWorld(), 1, M_DEFAULT);

         pDepthDataMgr->WarpColorImage(GetDepthData.MilColorImage, pGetDepthColorData->MilColorDepthOrWorldImage,
                                       pGetDepthColorData->IsDepthMapLut);          
         }
      }
   return M_NULL;
   }

//*****************************************************************************
// Hook function for FrameGrabberProcess(). Converts the depth buffer data returned 
// by the Kinect to a MIL depth map with its associated color depth map. A plane is
// fitted on the floor data and the bounding box of an object is localized. The 3d
// display is updated live.
//*****************************************************************************
MIL_INT MFTYPE Get3DMeshColorProcFunc(MIL_INT HookType, MIL_ID MilKinectImage, void* UserDataPtr)
   {
   SGet3DMeshColorProcFuncData* pGet3DMeshColorData = static_cast<SGet3DMeshColorProcFuncData*>(UserDataPtr);
   SGetDepthColorProcFuncData& GetDepthColorData = pGet3DMeshColorData->GetDepthColorData;
   CBestPlaneFitter* pBestPlaneFitter = pGet3DMeshColorData->pBestPlaneFitter;
   CDepthDataMgr* pDepthDataMgr = GetDepthColorData.pDepthDataMgr;

   // Call the hook function to get the world points and the color associated to each point.
   GetDepthColorProcFunc(HookType, MilKinectImage, &GetDepthColorData);

   // If the image grabbed by the Kinect is the depth image.
   if(MbufInquire(MilKinectImage, M_SIZE_BAND, M_NULL) == 1)
      { 
      // Disable the display updates.
      MdispControl(pGet3DMeshColorData->MilDepthMapDisplay, M_UPDATE, M_DISABLE);
      MdispControl(pGet3DMeshColorData->MilColorDepthMapDisplay, M_UPDATE, M_DISABLE);
      MdispControl(pGet3DMeshColorData->MilColorDisplay, M_UPDATE, M_DISABLE);

      // Transform the world points to be according to the point of view of the color camera.
      pDepthDataMgr->MoveWorldPoints(GetDepthColorData.MilColorCameraCalibration,
                                     M_TOOL_COORDINATE_SYSTEM, M_CAMERA_COORDINATE_SYSTEM);

      // Create the depth map and color depth map.
      pDepthDataMgr->CreateDepthMap(GetDepthColorData.MilColorDepthOrWorldImage,
                                    pGet3DMeshColorData->MilDepthMapImage,
                                    pGet3DMeshColorData->MilColorDepthMapImage,
                                    KINECT_DEPTH_PARAM);

      // Initialize the found box to 0.
      SBox FoundBox = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

      // Fit the floor plane.
      if(pBestPlaneFitter->CalculateBestPlane(pGet3DMeshColorData->MilDepthMapImage))
         {
         // Update the 3d display.
         Update3dDisplay(pGet3DMeshColorData);    

         // Move the relative coordinate system on the fitted plane. The fitted plane was found in
         // the IR camera coordinate system, which is where we had put the M_TOOL_COORDINATE_SYSTEM
         // of the color camera calibration.
         pBestPlaneFitter->MoveCoordinateSystemOnPlane(GetDepthColorData.MilColorCameraCalibration,
                                                       M_RELATIVE_COORDINATE_SYSTEM, M_CAMERA_COORDINATE_SYSTEM);
         
         // Transform the chains of the plane region which were expressed in the M_TOOL_COORDINATE_SYSTEM.
         pBestPlaneFitter->MoveWorldChains(GetDepthColorData.MilColorCameraCalibration,
                                           M_CAMERA_COORDINATE_SYSTEM, M_RELATIVE_COORDINATE_SYSTEM);

         // Get the overlay of the color display and clear it.
         MIL_ID MilOverlay = MdispInquire(pGet3DMeshColorData->MilColorDisplay, M_OVERLAY_ID, M_NULL);
         MdispControl(pGet3DMeshColorData->MilColorDisplay, M_OVERLAY_CLEAR, M_DEFAULT); 
         MgraClear(M_DEFAULT, pGet3DMeshColorData->MilColorGraList);
         
         // Set the graphic context and draw the plane in the image.
         MgraColor(M_DEFAULT, M_COLOR_RED);
         MgraControl(M_DEFAULT, M_INPUT_UNITS, M_WORLD);
         McalAssociate(GetDepthColorData.MilColorCameraCalibration, MilOverlay, M_DEFAULT);
         MIL_INT TransparentColor = MdispInquire(pGet3DMeshColorData->MilColorDisplay, M_TRANSPARENT_COLOR, M_NULL);
         pBestPlaneFitter->DrawPlaneInImage(M_DEFAULT,
                                            MilOverlay,
                                            TransparentColor);

         // Move the world points in the plane coordinate system.
         pDepthDataMgr->MoveWorldPoints(GetDepthColorData.MilColorCameraCalibration,
                                        M_CAMERA_COORDINATE_SYSTEM, M_RELATIVE_COORDINATE_SYSTEM);

         // Create the depth map seen from above the floor.
         pDepthDataMgr->CreateDepthMap(GetDepthColorData.MilColorDepthOrWorldImage,
                                       pGet3DMeshColorData->MilDepthMapImage,
                                       M_NULL,
                                       TOP_DEPTH_PARAM);

         // Find the object on the floor and draw its bounding box.   
         pGet3DMeshColorData->BoxFound = FindObjectOnFloor(pGet3DMeshColorData->MilDepthMapImage,
                                                           pGet3DMeshColorData->MilBinDepthMapImage,
                                                           pGet3DMeshColorData->MilBlobResult,
                                                           pGet3DMeshColorData->MilBlobFeatureList,
                                                           -TOP_VIEW_WORLD_POS_Z,
                                                           OBJECT_THRESHOLD_Z,
                                                           -TOP_VIEW_WORLD_POS_Z,
                                                           &FoundBox);
         if(pGet3DMeshColorData->BoxFound)
            {
            // Refine the position of the box based on the found height.
            FindObjectOnFloor(pGet3DMeshColorData->MilDepthMapImage,
                              pGet3DMeshColorData->MilBinDepthMapImage,
                              pGet3DMeshColorData->MilBlobResult,
                              pGet3DMeshColorData->MilBlobFeatureList,
                              -TOP_VIEW_WORLD_POS_Z,
                              FoundBox.Height - TOP_REFINE_RANGE,
                              FoundBox.Height + TOP_REFINE_RANGE,
                              &FoundBox);

            MgraColor(M_DEFAULT, M_COLOR_CYAN);
            DrawFound3dBox(M_DEFAULT, pGet3DMeshColorData->MilColorGraList,
                           GetDepthColorData.MilColorCameraCalibration, FoundBox);
            }  
         else
            MgraColor(M_DEFAULT, M_COLOR_RED);
         }

      // Draw the calculated volume.
      MgraControl(M_DEFAULT, M_INPUT_UNITS, M_DISPLAY);
      MIL_DOUBLE BoxVolume = FoundBox.Width * FoundBox.Length * FoundBox.Height / 1000.0;
      MIL_TEXT_CHAR DisplayText[256];
      MosSprintf(DisplayText, 256, MIL_TEXT(" Box width:  %8.1f cm   "), FoundBox.Width / 10.0);
      MgraText(M_DEFAULT, pGet3DMeshColorData->MilColorGraList, 0, 0, DisplayText);
      MosSprintf(DisplayText, 256, MIL_TEXT(" Box length: %8.1f cm   "), FoundBox.Length / 10.0);
      MgraText(M_DEFAULT, pGet3DMeshColorData->MilColorGraList, 0, DISPLAY_TEXT_LINE_OFFSET, DisplayText);
      MosSprintf(DisplayText, 256, MIL_TEXT(" Box height: %8.1f cm   "), FoundBox.Height / 10.0);
      MgraText(M_DEFAULT, pGet3DMeshColorData->MilColorGraList, 0, 2 * DISPLAY_TEXT_LINE_OFFSET, DisplayText);
      MosSprintf(DisplayText, 256, MIL_TEXT(" Box volume: %8.1f cm^3 "), BoxVolume);
      MgraText(M_DEFAULT, pGet3DMeshColorData->MilColorGraList, 0, 3 * DISPLAY_TEXT_LINE_OFFSET, DisplayText);
                  
      // Enable the display updates.
      MdispControl(pGet3DMeshColorData->MilColorDisplay, M_UPDATE, M_ENABLE);
      MdispControl(pGet3DMeshColorData->MilColorDepthMapDisplay, M_UPDATE, M_ENABLE);
      MdispControl(pGet3DMeshColorData->MilDepthMapDisplay, M_UPDATE, M_ENABLE);     
      }
   return M_NULL;
   }

//*****************************************************************************
// Function to calibrate the Kinect cameras.
//*****************************************************************************
void CalibrateCamera(CKinectCameraInterface* pKinectCamera,
                     MIL_ID MilCameraCalibration,
                     MIL_ID MilGrabImage,
                     MIL_ID MilDisplay,
                     ColorStreamTypeEnum ColorStreamType)
   {
   MIL_CONST_TEXT_PTR CameraFlag = (ColorStreamType == enColor) ? MIL_TEXT("color") : MIL_TEXT("infrared");
      
   // Reinitialize the camera to grab the right type of image.
   if(pKinectCamera->InitCamera(ColorStreamType, false) != KINECT_CAMERA_OK)
      FatalError(pKinectCamera, MIL_TEXT("Unable to initialize camera system.\n"));

   MIL_ID MilDrawDest = MdispInquire(MilDisplay, M_ASSOCIATED_GRAPHIC_LIST_ID, M_NULL);
   MgraClear(M_DEFAULT, MilDrawDest);
  
#if USE_REAL_CAMERA && CALIBRATE_WITH_REAL_CAMERA
   // Grab image continuously.
   pKinectCamera->FrameGrabberProcess(M_START, M_ASYNCHRONOUS, M_DEFAULT, &CopyFunc, &MilGrabImage);
   
   // Print message.
   MosPrintf(MIL_TEXT("The grabbed %s image from the Kinect is displayed.\n\n"), CameraFlag);
   if(ColorStreamType == enIR)
      MosPrintf(MIL_TEXT("Place a tilted chessboard calibration grid to calibrate the %s camera.\n"), CameraFlag);
   else
      MosPrintf(MIL_TEXT("Use the same tilted chessboard calibration grid to calibrate the %s camera.\n"), CameraFlag);   
   MosPrintf(MIL_TEXT("Press <Enter> to grab and calibrate the %s camera.\n\n"), CameraFlag);
   MosGetch();

   // Stop the grab.
   pKinectCamera->FrameGrabberProcess(M_STOP, M_DEFAULT);
#else
   MbufLoad(ColorStreamType == enColor? COLOR_GRID : IR_GRID, MilGrabImage);
#endif

   // Calibrate the camera.
   McalGrid(MilCameraCalibration,
            MilGrabImage,
            GRID_WORLD_OFFSET_X,
            GRID_WORLD_OFFSET_Y,
            0.0,
            GRID_NB_ROWS,
            GRID_NB_COLUMNS,
            GRID_SPACING,
            GRID_SPACING, 
            M_DEFAULT,
            M_CIRCLE_GRID);

   if(McalInquire(MilCameraCalibration, M_CALIBRATION_STATUS, M_NULL) != M_CALIBRATED)
      FatalError(pKinectCamera, MIL_TEXT("Unable to calibrate the camera."));

   // Display the calibration points.
   MgraColor(M_DEFAULT, M_COLOR_GREEN);
   McalDraw(M_DEFAULT, MilCameraCalibration, MilDrawDest, M_DRAW_WORLD_POINTS, M_DEFAULT, M_DEFAULT);
   MosPrintf(MIL_TEXT("The calibration of the %s camera is displayed.\n")
             MIL_TEXT("Press <Enter> to continue.\n\n"),
             CameraFlag);
   MosGetch();

   MgraClear(M_DEFAULT, MilDrawDest);
   }

//*****************************************************************************
// Function to move the color camera tool coordinate system on the 
// camera coordinate system of the IR camera. Here is the list of coordinate system
// and their usage.
//
//                              | ColorCameraCalibration | IRCameraCalibration |
// M_ABSOLUTE_COORDINATE_SYSTEM |             Same for both cameras            |
// M_RELATIVE_COORDINATE_SYSTEM | Put on the floor plane |        Unused       |
// M_TOOL_COORDINATE_SYSTEM     | Put on the IR camera   |        Unused       |
//*****************************************************************************
void MoveColorToolCoordSystemOnIRCameraCoordSystem(MIL_ID MilSystem,
                             MIL_ID MilIRCameraCalibration,
                             MIL_ID MilColorCameraCalibration)
   {
   // Allocate the homogeneous matrix.
   MIL_ID MilAbsoluteToIRCameraMatrix = MbufAlloc2d(MilSystem, 4, 4, 32+M_FLOAT, M_ARRAY, M_NULL);

   // Get the homogeneous matrix between the absolute and the IR camera coordinate system.
   McalGetCoordinateSystem(MilIRCameraCalibration,
                           M_CAMERA_COORDINATE_SYSTEM,
                           M_ABSOLUTE_COORDINATE_SYSTEM,
                           M_HOMOGENEOUS_MATRIX,
                           MilAbsoluteToIRCameraMatrix,
                           M_NULL, 
                           M_NULL, 
                           M_NULL,
                           M_NULL);
      
   // Set the tool coordinate system of the Color calibration on the IR calibration camera coordinate system.
   McalSetCoordinateSystem(MilColorCameraCalibration,
                           M_TOOL_COORDINATE_SYSTEM,
                           M_ABSOLUTE_COORDINATE_SYSTEM,
                           M_HOMOGENEOUS_MATRIX,
                           MilAbsoluteToIRCameraMatrix,
                           M_DEFAULT,
                           M_DEFAULT,
                           M_DEFAULT,
                           M_DEFAULT);

   // Free the homogeneous matrix.
   MbufFree(MilAbsoluteToIRCameraMatrix);
   }

//*****************************************************************************
// Function to find the object on the floor.
//*****************************************************************************
bool FindObjectOnFloor(MIL_ID MilDepthMapImage,
                       MIL_ID MilBinDepthMapImage,
                       MIL_ID MilBlobResult,
                       MIL_ID MilBlobFeatureList,
                       MIL_DOUBLE FloorZ,
                       MIL_DOUBLE MinThresholdZ,
                       MIL_DOUBLE MaxThresholdZ,
                       SBox* pFoundBox)
   {
   // Binarize the depth map.
   MimBinarize(MilDepthMapImage, MilBinDepthMapImage, M_FIXED + M_IN_RANGE, FloorZ - MaxThresholdZ, FloorZ - MinThresholdZ);

   // Calculate the blobs.
   MblobCalculate(MilBinDepthMapImage, MilDepthMapImage, MilBlobFeatureList, MilBlobResult);

   // Select the blobs that meet the area and feret criterion
   MblobSelect(MilBlobResult, M_INCLUDE_ONLY, M_AREA, M_GREATER_OR_EQUAL, MIN_BLOB_AREA, M_NULL);
   MblobSelect(MilBlobResult, M_EXCLUDE, M_FERET_MIN_DIAMETER, M_LESS, MIN_BLOB_FERET_MIN, M_NULL);
   MblobSelect(MilBlobResult, M_EXCLUDE, M_SIGMA_PIXEL, M_GREATER, MAX_SIGMA_PIXEL, M_NULL);
   MblobSelect(MilBlobResult, M_EXCLUDE, M_RECTANGULARITY, M_LESS, MIN_RECTANGULARITY, M_NULL);

   MIL_INT NbBlobs;
   if(MblobGetNumber(MilBlobResult, &NbBlobs))
      {
      MIL_INT* pBlobLabel = new MIL_INT[NbBlobs];
      MIL_INT* pBlobArea = new MIL_INT[NbBlobs];
      MIL_DOUBLE* pBlobBoxCenterX = new MIL_DOUBLE[NbBlobs];
      MIL_DOUBLE* pBlobBoxCenterY = new MIL_DOUBLE[NbBlobs];
      
      // Get the labels of the blobs
      MblobGetResult(MilBlobResult, M_LABEL_VALUE + M_TYPE_MIL_INT, pBlobLabel);

      // Get the area and positions of the blob
      MblobGetResult(MilBlobResult, M_AREA + M_TYPE_MIL_INT, pBlobArea);
      MblobGetResult(MilBlobResult, M_MIN_AREA_BOX_CENTER_X, pBlobBoxCenterX);
      MblobGetResult(MilBlobResult, M_MIN_AREA_BOX_CENTER_Y, pBlobBoxCenterY);

      // Calculate a pseudo score for each blob based on the area and Manhattan distance
      // from plane origin and keep the best blob.
      MIL_DOUBLE ManhattanDistance = (abs(pBlobBoxCenterX[0]) + abs(pBlobBoxCenterY[0]));
      MIL_DOUBLE MaxScore = pBlobArea[0] / (ManhattanDistance + 100);
      MIL_INT MaxScoreIdx = 0;
      for(MIL_INT BlobIdx = 1; BlobIdx < NbBlobs; BlobIdx++)
         {
         ManhattanDistance = (fabs(pBlobBoxCenterX[BlobIdx]) + fabs(pBlobBoxCenterY[BlobIdx]));
         MIL_DOUBLE CurScore = pBlobArea[BlobIdx] / (ManhattanDistance + 100);
         if(CurScore > MaxScore)
            {
            MaxScore = CurScore;
            MaxScoreIdx = BlobIdx;
            }
         }         
      
      // Get the Z results.
      MIL_DOUBLE BoxTopZ;
      MblobGetResultSingle(MilBlobResult, pBlobLabel[MaxScoreIdx], M_MEAN_PIXEL, &BoxTopZ);
      pFoundBox->Height = FloorZ - BoxTopZ;
      
      // Get the center of the box
      pFoundBox->CenterPosZ = -pFoundBox->Height/2;
      pFoundBox->CenterPosX = pBlobBoxCenterX[MaxScoreIdx];
      pFoundBox->CenterPosY = pBlobBoxCenterY[MaxScoreIdx];

      // Get the min area box parameter
      MblobGetResultSingle(MilBlobResult, pBlobLabel[MaxScoreIdx], M_MIN_AREA_BOX_ANGLE, &pFoundBox->ZAxisAngle);
      MblobGetResultSingle(MilBlobResult, pBlobLabel[MaxScoreIdx], M_MIN_AREA_BOX_WIDTH, &pFoundBox->Width);
      MblobGetResultSingle(MilBlobResult, pBlobLabel[MaxScoreIdx], M_MIN_AREA_BOX_HEIGHT, &pFoundBox->Length);

      // Delete the arrays
      delete [] pBlobBoxCenterX;
      delete [] pBlobBoxCenterY;
      delete [] pBlobArea;
      delete [] pBlobLabel;

      return true;
      }
   return false;   
   }

//*****************************************************************************
// Function that draw the 3d box in the image
//*****************************************************************************
void DrawFound3dBox(MIL_ID MilGraContext, MIL_ID MilDrawDest, MIL_ID MilCalibration, const SBox& rFoundBox)
   {
   // Move the coordinate system on the box.
   McalSetCoordinateSystem(MilCalibration,
                           M_RELATIVE_COORDINATE_SYSTEM,
                           M_RELATIVE_COORDINATE_SYSTEM,
                           M_TRANSLATION + M_COMPOSE_WITH_CURRENT,
                           M_NULL,
                           rFoundBox.CenterPosX,
                           rFoundBox.CenterPosY,
                           rFoundBox.CenterPosZ,
                           M_DEFAULT);

   McalSetCoordinateSystem(MilCalibration,
                           M_RELATIVE_COORDINATE_SYSTEM,
                           M_RELATIVE_COORDINATE_SYSTEM,
                           M_ROTATION_Z + M_COMPOSE_WITH_CURRENT,
                           M_NULL,
                           -rFoundBox.ZAxisAngle,
                           M_DEFAULT,
                           M_DEFAULT,
                           M_DEFAULT);

   // Get all the corners of the box. The top corners are first.
   MIL_DOUBLE CornersX[8] = {-rFoundBox.Width/2 , rFoundBox.Width/2  , rFoundBox.Width/2  , -rFoundBox.Width/2 ,
                             -rFoundBox.Width/2 , rFoundBox.Width/2  , rFoundBox.Width/2  , -rFoundBox.Width/2 };  
   MIL_DOUBLE CornersY[8] = {-rFoundBox.Length/2, -rFoundBox.Length/2, rFoundBox.Length/2 , rFoundBox.Length/2 ,
                             -rFoundBox.Length/2, -rFoundBox.Length/2, rFoundBox.Length/2 , rFoundBox.Length/2 };
   MIL_DOUBLE CornersZ[8] = {-rFoundBox.Height/2, -rFoundBox.Height/2, -rFoundBox.Height/2, -rFoundBox.Height/2,
                             rFoundBox.Height/2 , rFoundBox.Height/2 , rFoundBox.Height/2 , rFoundBox.Height/2 };
   MIL_DOUBLE TopCameraCornersX[4]; 
   MIL_DOUBLE TopCameraCornersY[4]; 
   MIL_DOUBLE TopCameraCornersZ[4]; 

   // Transform the 4 top corners in the camera coordinate system.
   McalTransformCoordinate3dList(MilCalibration,
                                 M_RELATIVE_COORDINATE_SYSTEM,
                                 M_CAMERA_COORDINATE_SYSTEM,
                                 4,
                                 CornersX,
                                 CornersY,
                                 CornersZ,
                                 TopCameraCornersX,
                                 TopCameraCornersY,
                                 TopCameraCornersZ,
                                 M_DEFAULT);

   // Get the corner that is the closest to the camera.
   MIL_INT ClosestCornerIdx = 0;
   MIL_DOUBLE MinCornerDistSquare = TopCameraCornersX[0] * TopCameraCornersX[0] +
                                    TopCameraCornersY[0] * TopCameraCornersY[0] +
                                    TopCameraCornersZ[0] * TopCameraCornersZ[0];  
   for(MIL_INT CornerIdx = 1; CornerIdx < 4; CornerIdx++)
      {
      MIL_DOUBLE CurCornerDistSquare = TopCameraCornersX[CornerIdx] * TopCameraCornersX[CornerIdx] +
                                       TopCameraCornersY[CornerIdx] * TopCameraCornersY[CornerIdx] +
                                       TopCameraCornersZ[CornerIdx] * TopCameraCornersZ[CornerIdx];
      if(CurCornerDistSquare < MinCornerDistSquare)
         {
         MinCornerDistSquare = CurCornerDistSquare;
         ClosestCornerIdx = CornerIdx;
         }
      }

   // Transform all the corners to the pixel coordinate system.
   McalTransformCoordinate3dList(MilCalibration,
                                 M_RELATIVE_COORDINATE_SYSTEM,
                                 M_PIXEL_COORDINATE_SYSTEM,
                                 8,
                                 CornersX,
                                 CornersY,
                                 CornersZ,
                                 CornersX,
                                 CornersY,
                                 M_NULL,
                                 M_DEFAULT);

   // Set the input units.
   MgraControl(MilGraContext, M_INPUT_UNITS, M_PIXEL);
   
   // Draw the top polygon.
   MgraLines(MilGraContext, MilDrawDest, 4, CornersX, CornersY, M_NULL, M_NULL, M_POLYGON);

   // Draw the previous polygon.
   MIL_DOUBLE PrevDrawCornersX[4] = {CornersX[ClosestCornerIdx]                , CornersX[(ClosestCornerIdx + 3) % 4],
                                     CornersX[((ClosestCornerIdx + 3) % 4) + 4], CornersX[ClosestCornerIdx + 4]      };
   MIL_DOUBLE PrevDrawCornersY[4] = {CornersY[ClosestCornerIdx]                , CornersY[(ClosestCornerIdx + 3) % 4],
                                     CornersY[((ClosestCornerIdx + 3) % 4) + 4], CornersY[ClosestCornerIdx + 4]      };
   MgraLines(MilGraContext, MilDrawDest, 4, PrevDrawCornersX, PrevDrawCornersY, M_NULL, M_NULL, M_POLYGON);
   
   // Draw the next polygon.
   MIL_DOUBLE NextDrawCornersX[4] = {CornersX[ClosestCornerIdx]                , CornersX[(ClosestCornerIdx + 1) % 4],
                                     CornersX[((ClosestCornerIdx + 1) % 4) + 4], CornersX[ClosestCornerIdx + 4]      };
   MIL_DOUBLE NextDrawCornersY[4] = {CornersY[ClosestCornerIdx]                , CornersY[(ClosestCornerIdx + 1) % 4],
                                     CornersY[((ClosestCornerIdx + 1) % 4) + 4], CornersY[ClosestCornerIdx + 4]      };
   MgraLines(MilGraContext, MilDrawDest, 4, NextDrawCornersX, NextDrawCornersY, M_NULL, M_NULL, M_POLYGON);   
   }

//*****************************************************************************
// Function that updates the 3d display.
//*****************************************************************************
void Update3dDisplay(const SGet3DMeshColorProcFuncData* pGet3DMeshColorData)
   {
   MbufCopy(pGet3DMeshColorData->MilDepthMapImage, pGet3DMeshColorData->Mil3dDepthMapImage);
   MbufCopy(pGet3DMeshColorData->MilColorDepthMapImage, pGet3DMeshColorData->Mil3dColorDepthMapImage);
   if(pGet3DMeshColorData->MilD3DSetImagesThread)
      MthrControl(pGet3DMeshColorData->MilD3DUpdateEvent, M_EVENT_SET, M_SIGNALED);   
   else
      MdepthD3DSetImages(pGet3DMeshColorData->DispD3DHandle, pGet3DMeshColorData->Mil3dDepthMapImage,
                         pGet3DMeshColorData->Mil3dColorDepthMapImage);
   }
//*****************************************************************************
// Thread function that effectively updates the 3d display.
//*****************************************************************************
MIL_UINT32 MFTYPE Update3dDisplayThread(void *UserDataPtr)
   {
   SGet3DMeshColorProcFuncData* pGet3DMeshColorData = (SGet3DMeshColorProcFuncData*) UserDataPtr;
   while(MthrInquire(pGet3DMeshColorData->MilStopUpdate3dDisplayEvent, M_EVENT_STATE, M_NULL) == M_NOT_SIGNALED)
      {
      // Wait for the update event.
      MthrWait(pGet3DMeshColorData->MilD3DUpdateEvent, M_EVENT_WAIT, M_NULL);

      // If we need to update.
      if(MthrInquire(pGet3DMeshColorData->MilStopUpdate3dDisplayEvent, M_EVENT_STATE, M_NULL) == M_NOT_SIGNALED)
         MdepthD3DSetImages(pGet3DMeshColorData->DispD3DHandle, pGet3DMeshColorData->Mil3dDepthMapImage,
         pGet3DMeshColorData->Mil3dColorDepthMapImage);
      }
   return 0;
   }

//*****************************************************************************
// Function that sets the title and position of the display.
//*****************************************************************************
void SetupDisplay(MIL_ID MilDisplay, MIL_CONST_TEXT_PTR WindowTitle, MIL_INT WindowPosX, MIL_INT WindowPosY)
   {
   MdispControl(MilDisplay, M_TITLE, M_PTR_TO_DOUBLE(WindowTitle));
   MdispControl(MilDisplay, M_WINDOW_INITIAL_POSITION_X, (MIL_DOUBLE)WindowPosX);
   MdispControl(MilDisplay, M_WINDOW_INITIAL_POSITION_Y, (MIL_DOUBLE)WindowPosY);
   }

//*****************************************************************************
// Simple function to fill the display lut. 
//*****************************************************************************
void FillDisplayLut(MIL_ID MilDisplayLut)
   {
   MbufClear(MilDisplayLut, M_COLOR_BLACK);
   MIL_ID MilDisplayLutChild = MbufChild1d(MilDisplayLut, 1, 3000, M_NULL);
   MgenLutFunction(MilDisplayLutChild, M_COLORMAP_JET, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT);
   MbufFree(MilDisplayLutChild);
   }

//*****************************************************************************
// Simple function that terminates the program abruptly when an error occurs.
//*****************************************************************************
void FatalError(CKinectCameraInterface* pKinectCamera, const MIL_TEXT_CHAR* pMsg)
   {
   pKinectCamera->CloseDown();
   delete pKinectCamera;
   MosPrintf(pMsg);
   MosPrintf(MIL_TEXT("\nPress <Enter> to end.\n"));
   MosGetch();
   exit(-1);
   }