//***************************************************************************************/
// 
// File name: 3dModelHeightDefect.cpp  
// Location:  ...\Matrox Imaging\MILxxx\Examples\Processing\3dAlignment\3dModelHeightDefect\C++
//             
//
// Synopsis:  This program contains an example of 3d surface alignment followed 
//            by defect detection using the 3dmap module.
//            See the PrintHeader() function below for detailed description.
//
// Copyright (C) Matrox Electronic Systems Ltd., 1992-2015.
// All Rights Reserved
//***************************************************************************************/

#include <mil.h>
#include <math.h>
#include <stdlib.h>

//****************************************************************************
// Example description.
//****************************************************************************
void PrintHeader()   
   {
   MosPrintf(MIL_TEXT("[EXAMPLE NAME]\n"));
   MosPrintf(MIL_TEXT("3dModelHeightDefect\n\n"));

   MosPrintf(MIL_TEXT("[SYNOPSIS]\n"));
   MosPrintf(MIL_TEXT("This example demonstrates how to use the 3D surface alignment   \n"));
   MosPrintf(MIL_TEXT("operation to align the acquired point cloud of a 3D object with \n"));
   MosPrintf(MIL_TEXT("its 3D reference model in order to detect defects.              \n"));
   MosPrintf(MIL_TEXT("\n"));

   MosPrintf(MIL_TEXT("[MODULES USED]\n"));
   MosPrintf(MIL_TEXT("Modules used: 3dMap, Buffer, Calibration, Display,\n")
             MIL_TEXT("Graphics, Image Processing.\n\n"));

   MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n"));

   MosGetch();
   }

// Functions declarations. 
void   AddLutColorForInvalidDepth(MIL_ID MilLut, MIL_UINT8 InvalidDepthGray);
MIL_ID CreateLutLegend(MIL_ID MilSystem);
void   DrawLutLegend(MIL_ID DstImage, MIL_ID MilLutLegend);
void   GetBoxPoints(MIL_ID     MilDepthMap, 
                    MIL_DOUBLE BoxPtsInAbsX[], 
                    MIL_DOUBLE BoxPtsInAbsY[], 
                    MIL_DOUBLE BoxPtsInAbsZ[]);
void   Draw3dBoxes(MIL_ID MilSystem, 
                   MIL_ID MilAlignmentResult, 
                   MIL_ID MilDepthMap[], 
                   MIL_ID MilDisplay[], 
                   MIL_ID MilPointCloud[]);
void   DrawBoxInOneDepthMap(MIL_ID     MilSystem, 
                            MIL_ID     MilDisplay, 
                            MIL_ID     MilDepthMap, 
                            MIL_DOUBLE BoxPtsInAbsX[],
                            MIL_DOUBLE BoxPtsInAbsY[],
                            MIL_DOUBLE BoxPtsInAbsZ[]);

// Enumerators definitions.
enum { eModel = 0, eObject = 1, eDefects = 2 };   

// Input data files.
static const MIL_TEXT_CHAR* const FILE_3DCONTEXT[] = 
   {M_IMAGE_PATH MIL_TEXT("3dModelHeightDefect/LaserContext_L.m3d" ),
    M_IMAGE_PATH MIL_TEXT("3dModelHeightDefect/LaserContext_R.m3d")};

static const MIL_TEXT_CHAR* const FILE_OBJECT_UNCORRECTED_DEPTHMAP[] = 
   {M_IMAGE_PATH MIL_TEXT("3dModelHeightDefect/LaserScanUncorrectedDepthmap_L.mim"),
    M_IMAGE_PATH MIL_TEXT("3dModelHeightDefect/LaserScanUncorrectedDepthmap_R.mim")};

static const MIL_TEXT_CHAR* const FILE_MODEL_POINT_CLOUD =  
    M_IMAGE_PATH MIL_TEXT("3dModelHeightDefect/3dModel.ply");

// Depth maps parameters definitions.
static const MIL_INT    NUM_DEPTHMAP_VALUES    =  65536;                
static const MIL_INT    DEPTHMAP_INVALID_VALUE =  NUM_DEPTHMAP_VALUES-1;
static const MIL_UINT8  DEPTHMAP_INVALID_COLOR =  128;
static const MIL_INT    DEPTH_MAP_SIZE_X       =  380; 
static const MIL_INT    DEPTH_MAP_SIZE_Y       =  400;

static const MIL_DOUBLE EXTRACTION_BOX_MIN_X   =  -3.0;
static const MIL_DOUBLE EXTRACTION_BOX_MIN_Y   = -10.0;
static const MIL_DOUBLE EXTRACTION_BOX_MIN_Z   =  13.0;
static const MIL_DOUBLE EXTRACTION_BOX_MAX_X   = 190.0;
static const MIL_DOUBLE EXTRACTION_BOX_MAX_Y   = 190.0;
static const MIL_DOUBLE EXTRACTION_BOX_MAX_Z   = -53.0;

// Align context controls definitions.
static const MIL_INT    DECIMATION_STEP_MODEL              = 8;
static const MIL_INT    DECIMATION_STEP_OBJECT             = 8;
static const MIL_DOUBLE MODEL_OVERLAP                      = 95.0; // %
static const MIL_INT    MAX_ITERATIONS                     = 20;
static const MIL_DOUBLE ALIGN_RMS_ERROR_RELATIVE_THRESHOLD = 0.5;  // %

// Visualization variables definitions.
static const MIL_INT    NUM_BOX_POINTS       =  24; // A 3d cube box has 24 points
static const MIL_DOUBLE DRAW_BOX_MIN_X       =  EXTRACTION_BOX_MIN_X + 30.0;
static const MIL_DOUBLE DRAW_BOX_MIN_Y       =  EXTRACTION_BOX_MIN_Y + 40.0;
static const MIL_DOUBLE DRAW_BOX_MIN_Z       =  EXTRACTION_BOX_MIN_Z - 60.0;
static const MIL_DOUBLE DRAW_BOX_MAX_X       =  DRAW_BOX_MIN_X +  98.0;
static const MIL_DOUBLE DRAW_BOX_MAX_Y       =  DRAW_BOX_MIN_Y + 136.0;
static const MIL_DOUBLE DRAW_BOX_MAX_Z       =  DRAW_BOX_MIN_Z + 100.0;
static const MIL_DOUBLE DEFECT_SCALE_Z_RATIO =  0.12;

//*****************************************************************************
// Main.
//*****************************************************************************
int MosMain(void)
   {
   // Print example information in console.
   PrintHeader();

   //--------------------------------------------------------------------------
   // Allocate MIL objects. 

   MIL_ID MilApplication;           // Application identifier.
   MIL_ID MilSystem;                // System identifier.  

   // 3D reconstruction
   MIL_ID MilObjectUncorrDepth[2];  // Object uncorrected depth map of left and right cameras.
   MIL_ID Mil3dContext[2];          // 3d context of left and right cameras.
   MIL_ID MilPointCloud[2];         // Model and object point cloud containers.

   // 3D alignment and defect detection
   MIL_ID MilAlignmentContext;      // Pairwise alignment context.
   MIL_ID MilAlignmentResult;       // Pairwise alignment result.
   MIL_ID MilDepthMap[3];           // Model and object depth maps, and defects distance map.

   // Display
   MIL_ID MilLut;                   // Display color LUT.
   MIL_ID MilLutLegend;             // Display color LUT legend.
   MIL_ID MilDisplay[3];            // Display for model and object depth maps, and defects distance map.    

   MappAlloc(M_NULL, M_DEFAULT, &MilApplication);
   MsysAlloc(M_DEFAULT, M_SYSTEM_HOST, M_DEFAULT, M_DEFAULT, &MilSystem);

   //-------------------------------------------------------------------------------------------
   // Import 3D model from file and reconstruct 3D object from laser scan uncorrected depth maps.

   // Allocate 3D model point cloud container.
   M3dmapAllocResult(MilSystem, M_POINT_CLOUD_CONTAINER, M_DEFAULT, &MilPointCloud[eModel]);

   MosPrintf(MIL_TEXT("The model's 3D point cloud is imported from a PLY file.\n\n"));

   // Import 3D model from PLY file
   M3dmapImport(FILE_MODEL_POINT_CLOUD, M_PLY, MilPointCloud[eModel], M_POINT_CLOUD_LABEL(1), M_NULL, M_DEFAULT); // FILE_MODEL_POINT_CLOUD

   // Allocate 3D object point cloud container.
   M3dmapAllocResult(MilSystem, M_POINT_CLOUD_CONTAINER, M_DEFAULT, &MilPointCloud[eObject]);

   MosPrintf(MIL_TEXT("The object's raw depth maps acquired using a 3D camera-laser scanner are restored.\n\n"));

   // 3D reconstruction of object from 2 uncorrected depth maps (1 laser, 2 cameras setup)
   const MIL_INT NumCameras = 2;
   for (MIL_INT i = 0; i < NumCameras; i++)
      {
      // Uncorrected depth map.
      MbufRestore(FILE_OBJECT_UNCORRECTED_DEPTHMAP[i], MilSystem, &MilObjectUncorrDepth[i]);

      // 3D context.
      M3dmapRestore(FILE_3DCONTEXT[i], MilSystem, M_DEFAULT, &Mil3dContext[i]);

      // 3D reconstruction result (point cloud) controls
      M3dmapControl(MilPointCloud[eObject], M_GENERAL, M_MAX_FRAMES, 2048);

      // 3D reconstruction from uncorrected depth maps
      M3dmapAddScan(Mil3dContext[i], MilPointCloud[eObject], MilObjectUncorrDepth[i], M_NULL, M_NULL, M_POINT_CLOUD_LABEL(i+1), M_LINE_ALREADY_EXTRACTED);

      // Free 3D reconstruction related allocations 
      M3dmapFree(Mil3dContext[i]);
      MbufFree(MilObjectUncorrDepth[i]);
      }

   //-------------------------------------------------------------------------------
   // Initialize displays that will show model and object fully corrected depth maps.

   // Jet LUT.
   MbufAllocColor(MilSystem, 3, NUM_DEPTHMAP_VALUES, 1, 8 + M_UNSIGNED, M_LUT, &MilLut);
   AddLutColorForInvalidDepth(MilLut, DEPTHMAP_INVALID_COLOR);

   // LUT legend
   MilLutLegend = CreateLutLegend(MilSystem);

   // Initialize model and object depth map displays
   const MIL_INT NumDisplays = 3;
   for (MIL_INT i = 0; i < NumDisplays; i++)
      {
      // Allocate the display.
      MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, &MilDisplay[i]);

      // Associate the jet LUT
      MdispLut(MilDisplay[i], MilLut);

      // Some display controls
      MdispControl(MilDisplay[i], M_OVERLAY, M_ENABLE);
      MdispControl(MilDisplay[i], M_WINDOW_INITIAL_POSITION_X, (MIL_INT)(i*1.04 * DEPTH_MAP_SIZE_X));
      MdispControl(MilDisplay[i], M_UPDATE, M_DISABLE);

      // Add titles to displays.
      switch (i)
         {
         case eModel:
            MdispControl(MilDisplay[eModel]  , M_TITLE, M_PTR_TO_DOUBLE(MIL_TEXT("Model Depth Map"))); break;
         case eObject:                        
            MdispControl(MilDisplay[eObject] , M_TITLE, M_PTR_TO_DOUBLE(MIL_TEXT("Object Depth Map"))); break;
         case eDefects:
            MdispControl(MilDisplay[eDefects], M_TITLE, M_PTR_TO_DOUBLE(MIL_TEXT("Defect Height Map"))); break;
         }
      }

   //--------------------------------------------------------------------------
   // Fully corrected depth maps from point cloud containers

   // Generate model and object fully corrected depth maps before 3d alignment
   const MIL_INT NumPtCloudContainers = 2;
   MIL_INT NumPoints[NumPtCloudContainers];
   for (MIL_INT i = 0; i < NumPtCloudContainers; i++)
      {
      // Get the number of 3d points in each point cloud containers.
      M3dmapGetResult(MilPointCloud[i], M_DEFAULT,
                      M_NUMBER_OF_3D_POINTS + M_NO_INVALID_POINT + M_TYPE_MIL_INT,
                      &NumPoints[i]);

      // Corrected depth map.
      MbufAlloc2d(MilSystem, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, 16 + M_UNSIGNED, M_IMAGE + M_PROC + M_DISP, &MilDepthMap[i]);
      
      // Point cloud containers controls for depth map extraction.
      M3dmapControl(MilPointCloud[i], M_DEFAULT, M_EXTRACTION_OVERLAP   , M_MAX                );
      M3dmapControl(MilPointCloud[i], M_DEFAULT, M_EXTRACTION_SATURATION, M_DISABLE            );
      M3dmapControl(MilPointCloud[i], M_DEFAULT, M_FILL_MODE            , M_DISABLE            );
      
      M3dmapSetBox(MilPointCloud[i], M_EXTRACTION_BOX, M_BOTH_CORNERS,
                   EXTRACTION_BOX_MIN_X, EXTRACTION_BOX_MIN_Y, EXTRACTION_BOX_MIN_Z,
                   EXTRACTION_BOX_MAX_X, EXTRACTION_BOX_MAX_Y, EXTRACTION_BOX_MAX_Z);

      // Fully corrected depth maps extraction.
      M3dmapExtract(MilPointCloud[i], MilDepthMap[i], M_NULL, M_CORRECTED_DEPTH_MAP, M_ALL, M_DEFAULT);
      
      // Display the depth map.
      MdispSelect(MilDisplay[i], MilDepthMap[i]);

      DrawLutLegend(MilDepthMap[i], MilLutLegend);

      MdispControl(MilDisplay[i], M_UPDATE, M_NOW);
      }

   MosPrintf(MIL_TEXT("The model's and object's corrected depth maps are displayed using pseudo colors.\n\n"));
   MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n"));
   MosGetch();

   //--------------------------------------------------------------------------
   // 3D alignment

   // 3D pairwise registration (align) context and result.
   M3dmapAlloc(MilSystem, M_PAIRWISE_ALIGNMENT_CONTEXT, M_DEFAULT, &MilAlignmentContext);
   M3dmapAllocResult(MilSystem, M_ALIGNMENT_RESULT, M_DEFAULT, &MilAlignmentResult);

   // Pairwise alignment context controls
   M3dmapControl(MilAlignmentContext, M_DEFAULT, M_DECIMATION_STEP_MODEL             , DECIMATION_STEP_MODEL           );
   M3dmapControl(MilAlignmentContext, M_DEFAULT, M_DECIMATION_STEP_SCENE             , DECIMATION_STEP_OBJECT          );
   M3dmapControl(MilAlignmentContext, M_DEFAULT, M_PREALIGNMENT_MODE                 , M_CENTROID                      );
   M3dmapControl(MilAlignmentContext, M_DEFAULT, M_MODEL_OVERLAP                     , MODEL_OVERLAP                   );
   M3dmapControl(MilAlignmentContext, M_DEFAULT, M_MAX_ITERATIONS                    , MAX_ITERATIONS                  );
   M3dmapControl(MilAlignmentContext, M_DEFAULT, M_ALIGN_RMS_ERROR_RELATIVE_THRESHOLD, ALIGN_RMS_ERROR_RELATIVE_THRESHOLD);

   // Alignment
   MIL_INT64  AlignStatus = M_NULL;
   MIL_DOUBLE AlignComputationTime = 0.0;

   MappTimer(M_TIMER_RESET, M_NULL);

   M3dmapAlign(MilAlignmentContext     ,    // MIL_ID      AlignmentContextId
               MilPointCloud[eModel]   ,    // MIL_ID      ModelPtCloudContainerId
               M_ALL                   ,    // MIL_INT     ModelPtCloudIndexOrLabel
               MilPointCloud[eObject]  ,    // MIL_ID      ScenePtCloudContainerId
               M_ALL                   ,    // MIL_INT     ScenePtCloudIndexOrLabel
               M_NULL                  ,    // MIL_ID      PreAlignAlignmentResultOrMatrixId
               MilAlignmentResult      ,    // MIL_ID      AlignmentResultOrMatrixId
               M_DEFAULT               ,    // MIL_INT64   ControlFlag
               &AlignStatus            );   // MIL_INT64*  StatusPtr

   MappTimer(M_TIMER_READ, &AlignComputationTime);

   MosPrintf(MIL_TEXT("The 3D alignment between the model and the object has been performed.\n\n"));

   // Interpret the result status.
   switch (AlignStatus)
      {
      case M_NOT_INITIALIZED: 
         MosPrintf(MIL_TEXT("Alignment failed: the alignment result is not initialized.\n\n")); 
         break;
      case M_NOT_ENOUGH_POINT_PAIRS:
         MosPrintf(MIL_TEXT("Alignment failed: point clouds are not overlaping.\n\n"));
         break;
      case M_MAX_ITERATIONS_REACHED:
         MosPrintf(MIL_TEXT("Alignment reached the maximum number of iterations allowed (%d)\n")
                   MIL_TEXT("in %.2f ms. Resulting fixture may or may not be valid.\n\n"), 
                   MAX_ITERATIONS, AlignComputationTime*1000);
         break;
      case M_ALIGN_RMS_ERROR_THRESHOLD_REACHED:
      case M_ALIGN_RMS_ERROR_RELATIVE_THRESHOLD_REACHED:
         MIL_DOUBLE AlignRmsError;
         M3dmapGetResult(MilAlignmentResult, M_DEFAULT, M_ALIGN_RMS_ERROR + M_TYPE_MIL_DOUBLE, &AlignRmsError);
         MosPrintf(MIL_TEXT("The alignment of %d model points with %d object points\n")
                   MIL_TEXT("succeeded in %.2f ms with a final RMS error of %f mm.\n\n"),
                   NumPoints[eModel], NumPoints[eObject], AlignComputationTime*1000, AlignRmsError);
         break;
      default:
         MosPrintf(MIL_TEXT("Unknown alignment status.\n\n"));
      }

   // Draw a black 3D box in each depth map to visualize object pose obtained from alignment
   Draw3dBoxes(MilSystem, MilAlignmentResult, MilDepthMap, MilDisplay, MilPointCloud);

   MosPrintf(MIL_TEXT("3D boxes are drawn to highlight the 3D pose estimation of the object relative to the model.\n\n"));

   // The object depth map buffer changed, so redraw the LUT legend
   DrawLutLegend(MilDepthMap[eObject], MilLutLegend);
   MdispControl(MilDisplay[eObject], M_UPDATE, M_NOW);

   //--------------------------------------------------------------------------
   // Fixturing and defects highlighting.     

   // Using alignment result to fixture object's point cloud with model's point cloud.
   McalFixture(MilPointCloud[eObject], M_NULL, M_MOVE_RELATIVE, M_RESULT_ALIGNMENT_3DMAP, MilAlignmentResult, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT);
   M3dmapExtract(MilPointCloud[eObject], MilDepthMap[eObject], M_NULL, M_CORRECTED_DEPTH_MAP, M_ALL, M_DEFAULT);

   // Create a buffer for defect highlighting (difference between model and object depth maps)
   MbufAlloc2d(MilSystem, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, 16 + M_UNSIGNED, M_IMAGE + M_PROC + M_DISP, &MilDepthMap[eDefects]);
   
   // Adjust defect highlighting buffer's calibration for displaying absolute difference with maximum contrast.
   MIL_DOUBLE ScaleZ;
   McalAssociate(MilDepthMap[eModel], MilDepthMap[eDefects], M_DEFAULT);
   McalControl(MilDepthMap[eDefects], M_WORLD_POS_Z, 0.0);
   McalInquire(MilDepthMap[eDefects], M_GRAY_LEVEL_SIZE_Z, &ScaleZ);
   McalControl(MilDepthMap[eDefects], M_GRAY_LEVEL_SIZE_Z, fabs(ScaleZ) * DEFECT_SCALE_Z_RATIO);

   MosPrintf(MIL_TEXT("The object's depth map is corrected according to the found 3D pose relative to the model\n")
             MIL_TEXT("and is subtracted from the model reference to determine differences in heights.\n\n"));

   // Defect highlighting: absolute difference between model depth map and fixtured object depth map
   M3dmapArith(MilDepthMap[eModel], MilDepthMap[eObject], MilDepthMap[eDefects], M_NULL, M_SUB_ABS, M_USE_DESTINATION_SCALES);

   // Display highlighted defects
   DrawLutLegend(MilDepthMap[eDefects], MilLutLegend);
   MdispSelect(MilDisplay[eDefects], MilDepthMap[eDefects]);

   MosPrintf(MIL_TEXT("The resulting depth map of differences is displayed using pseudo colors.\n\n"));
   MosPrintf(MIL_TEXT("Press <Enter> to end.\n\n"));
   MosGetch();

   //--------------------------------------------------------------------------
   // Free MIL objects.    

   for (MIL_INT i = 0; i < NumPtCloudContainers; i++)
      {
      M3dmapFree(MilPointCloud[i]);
      MbufFree(MilDepthMap[i]);
      MdispControl(MilDisplay[i], M_ASSOCIATED_GRAPHIC_LIST_ID, M_NULL);
      MdispFree(MilDisplay[i]);
      }

   M3dmapFree(MilAlignmentContext);
   M3dmapFree(MilAlignmentResult);

   MbufFree(MilLutLegend);

   MbufFree(MilDepthMap[eDefects]);
   MdispFree(MilDisplay[eDefects]);

   MbufFree(MilLut);
   MsysFree(MilSystem);
   MappFree(MilApplication);

   return 0;
   }
   
//*****************************************************************************
// Set LUT value associated to M_INVALID_POINT to a gray level value.
//*****************************************************************************
void AddLutColorForInvalidDepth(MIL_ID MilLut, MIL_UINT8 InvalidDepthGray)
   {
   // Create LUT 
   MgenLutFunction(MilLut, M_COLORMAP_JET, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT);

   // Get LUT values into array
   MIL_UINT8 LutJetArray[3][NUM_DEPTHMAP_VALUES];
   MbufGetColor(MilLut, M_PLANAR, M_ALL_BANDS, LutJetArray);

   // Replace LUT color associated with invalid depth value
   MIL_UINT8 InvalidDepthColor[3] = {InvalidDepthGray, InvalidDepthGray, InvalidDepthGray};
   LutJetArray[0][DEPTHMAP_INVALID_VALUE] = InvalidDepthColor[0]; // Invalid value color R component (0-255)
   LutJetArray[1][DEPTHMAP_INVALID_VALUE] = InvalidDepthColor[1]; // Invalid value color G component (0-255)
   LutJetArray[2][DEPTHMAP_INVALID_VALUE] = InvalidDepthColor[2]; // Invalid value color B component (0-255)

   // Put modified LUT values back
   MbufPutColor(MilLut, M_PLANAR, M_ALL_BANDS, LutJetArray);
   }

//*****************************************************************************
// Draw a black 3d box in model and object depth maps to illustrate the pose.
//*****************************************************************************
void Draw3dBoxes(MIL_ID MilSystem, 
                 MIL_ID MilAlignmentResult, 
                 MIL_ID MilDepthMap[], 
                 MIL_ID MilDisplay[], 
                 MIL_ID MilPointCloud[])
   {
   // Points, expressed in absolute coordinates, of the reference box centered on the model
   MIL_DOUBLE ModelBoxPtsInAbsX[NUM_BOX_POINTS];
   MIL_DOUBLE ModelBoxPtsInAbsY[NUM_BOX_POINTS];
   MIL_DOUBLE ModelBoxPtsInAbsZ[NUM_BOX_POINTS];
   GetBoxPoints(MilDepthMap[eModel], ModelBoxPtsInAbsX, ModelBoxPtsInAbsY, ModelBoxPtsInAbsZ);

   // Draw the reference box in model's depth map
   DrawBoxInOneDepthMap(MilSystem, 
                        MilDisplay[eModel], 
                        MilDepthMap[eModel], 
                        ModelBoxPtsInAbsX,
                        ModelBoxPtsInAbsY,
                        ModelBoxPtsInAbsZ);

   MdispControl(MilDisplay[eModel], M_UPDATE, M_NOW);

   // Fixture object with alignment final result and generate the resulting depth map
   McalFixture(MilPointCloud[eObject], M_NULL, M_MOVE_RELATIVE, M_RESULT_ALIGNMENT_3DMAP, MilAlignmentResult, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT);
   M3dmapExtract(MilPointCloud[eObject], MilDepthMap[eObject], M_NULL, M_CORRECTED_DEPTH_MAP, M_ALL, M_DEFAULT);

   // Points, expressed in absolute coordinates, of the reference box centered on the fixtured object
   MIL_DOUBLE ObjectBoxPtsInAbsX[NUM_BOX_POINTS];
   MIL_DOUBLE ObjectBoxPtsInAbsY[NUM_BOX_POINTS];
   MIL_DOUBLE ObjectBoxPtsInAbsZ[NUM_BOX_POINTS];
   GetBoxPoints(MilDepthMap[eObject], ObjectBoxPtsInAbsX, ObjectBoxPtsInAbsY, ObjectBoxPtsInAbsZ);

   // Fixture with prealignment (alignment iteration 0) and generate the resulting depth map.
   McalFixture(MilPointCloud[eObject], M_NULL, M_MOVE_RELATIVE, M_RESULT_ALIGNMENT_3DMAP, MilAlignmentResult, 0, M_DEFAULT, M_DEFAULT, M_DEFAULT);
   M3dmapExtract(MilPointCloud[eObject], MilDepthMap[eObject], M_NULL, M_CORRECTED_DEPTH_MAP, M_ALL, M_DEFAULT);

   // Display the reference box in the not-aligned object depth map to visualize object pose.
   DrawBoxInOneDepthMap(MilSystem, 
                        MilDisplay[eObject], 
                        MilDepthMap[eObject], 
                        ObjectBoxPtsInAbsX,
                        ObjectBoxPtsInAbsY,
                        ObjectBoxPtsInAbsZ);

   MdispControl(MilDisplay[eObject], M_UPDATE, M_NOW);
   }

//*****************************************************************************
// Get list of 3d points, expressed in absolute coordinates, corresponding to 
// start and end points of line segments of a 3d cube.
//*****************************************************************************
void GetBoxPoints(MIL_ID MilDepthMap, MIL_DOUBLE BoxPtsInAbsX[], MIL_DOUBLE BoxPtsInAbsY[], MIL_DOUBLE BoxPtsInAbsZ[])
   {

   // Define box lines (from points) expressed in relative coordinate system
   //                                        |Box back square ----------------------------------------------| Box front square ---------------------------------------------| Connect box back square with front square --------------------|
   //                                        |xyZ-XyZ         XyZ-XYZ         XYZ-xYZ         xYZ-xyZ       | xyz-Xyz         Xyz-XYz         XYz-xYz         xYz-xyz       | xyz-xyZ         xYz-xYZ         Xyz-XyZ         XYz-XYZ         
   MIL_DOUBLE BoxPtsInRelX[NUM_BOX_POINTS] = {DRAW_BOX_MIN_X, DRAW_BOX_MAX_X, DRAW_BOX_MAX_X, DRAW_BOX_MIN_X, DRAW_BOX_MIN_X, DRAW_BOX_MAX_X, DRAW_BOX_MAX_X, DRAW_BOX_MIN_X, DRAW_BOX_MIN_X, DRAW_BOX_MIN_X, DRAW_BOX_MAX_X, DRAW_BOX_MAX_X,
                                              DRAW_BOX_MAX_X, DRAW_BOX_MAX_X, DRAW_BOX_MIN_X, DRAW_BOX_MIN_X, DRAW_BOX_MAX_X, DRAW_BOX_MAX_X, DRAW_BOX_MIN_X, DRAW_BOX_MIN_X, DRAW_BOX_MIN_X, DRAW_BOX_MIN_X, DRAW_BOX_MAX_X, DRAW_BOX_MAX_X};
   MIL_DOUBLE BoxPtsInRelY[NUM_BOX_POINTS] = {DRAW_BOX_MIN_Y, DRAW_BOX_MIN_Y, DRAW_BOX_MAX_Y, DRAW_BOX_MAX_Y, DRAW_BOX_MIN_Y, DRAW_BOX_MIN_Y, DRAW_BOX_MAX_Y, DRAW_BOX_MAX_Y, DRAW_BOX_MIN_Y, DRAW_BOX_MAX_Y, DRAW_BOX_MIN_Y, DRAW_BOX_MAX_Y,
                                              DRAW_BOX_MIN_Y, DRAW_BOX_MAX_Y, DRAW_BOX_MAX_Y, DRAW_BOX_MIN_Y, DRAW_BOX_MIN_Y, DRAW_BOX_MAX_Y, DRAW_BOX_MAX_Y, DRAW_BOX_MIN_Y, DRAW_BOX_MIN_Y, DRAW_BOX_MAX_Y, DRAW_BOX_MIN_Y, DRAW_BOX_MAX_Y};
   MIL_DOUBLE BoxPtsInRelZ[NUM_BOX_POINTS] = {DRAW_BOX_MAX_Z, DRAW_BOX_MAX_Z, DRAW_BOX_MAX_Z, DRAW_BOX_MAX_Z, DRAW_BOX_MIN_Z, DRAW_BOX_MIN_Z, DRAW_BOX_MIN_Z, DRAW_BOX_MIN_Z, DRAW_BOX_MIN_Z, DRAW_BOX_MIN_Z, DRAW_BOX_MIN_Z, DRAW_BOX_MIN_Z,
                                              DRAW_BOX_MAX_Z, DRAW_BOX_MAX_Z, DRAW_BOX_MAX_Z, DRAW_BOX_MAX_Z, DRAW_BOX_MIN_Z, DRAW_BOX_MIN_Z, DRAW_BOX_MIN_Z, DRAW_BOX_MIN_Z, DRAW_BOX_MAX_Z, DRAW_BOX_MAX_Z, DRAW_BOX_MAX_Z, DRAW_BOX_MAX_Z};
   
   // Convert 3d cube box points into absolute coordinates
   McalTransformCoordinate3dList(MilDepthMap, 
                                 M_RELATIVE_COORDINATE_SYSTEM, M_ABSOLUTE_COORDINATE_SYSTEM, 
                                 NUM_BOX_POINTS,
                                 BoxPtsInRelX, BoxPtsInRelY, BoxPtsInRelZ,
                                 BoxPtsInAbsX, BoxPtsInAbsY, BoxPtsInAbsZ, 
                                 M_DEFAULT);
   }

//*****************************************************************************
// Draw 3d cube line segments defined by 3d points in the displayed depth map.
//*****************************************************************************
void DrawBoxInOneDepthMap(MIL_ID     MilSystem, 
                          MIL_ID     MilDisplay, 
                          MIL_ID     MilDepthMap, 
                          MIL_DOUBLE BoxPtsInAbsX[],
                          MIL_DOUBLE BoxPtsInAbsY[],
                          MIL_DOUBLE BoxPtsInAbsZ[])
   {
   MIL_INT NumLines = NUM_BOX_POINTS/2;

   MIL_DOUBLE BoxPixelsInFixtureX[NUM_BOX_POINTS];
   MIL_DOUBLE BoxPixelsInFixtureY[NUM_BOX_POINTS];

   MIL_DOUBLE* BoxPixelStartX = BoxPixelsInFixtureX;
   MIL_DOUBLE* BoxPixelStartY = BoxPixelsInFixtureY;
   MIL_DOUBLE* BoxPixelEndX   = BoxPixelsInFixtureX + NumLines;
   MIL_DOUBLE* BoxPixelEndY   = BoxPixelsInFixtureY + NumLines;

   // Convert box points from absolute to pixel coordinates in the depth map.
   McalTransformCoordinate3dList(MilDepthMap, 
                                 M_ABSOLUTE_COORDINATE_SYSTEM, M_PIXEL_COORDINATE_SYSTEM, 
                                 NUM_BOX_POINTS,
                                 BoxPtsInAbsX       , BoxPtsInAbsY       , BoxPtsInAbsZ,
                                 BoxPixelsInFixtureX, BoxPixelsInFixtureY, M_NULL      , 
                                 M_DEPTH_MAP);

   // Create a buffer in which the box to draw as a depth map overlay will be defined.
   MIL_ID BoxOverlay;
   MbufAlloc2d(MilSystem, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, 1+M_UNSIGNED, M_IMAGE+M_PROC, &BoxOverlay);

   // Draw the back square of the box first (white on black).
   MbufClear(BoxOverlay, 0.0);
   MgraColor(M_DEFAULT, 1.0);
   MgraLines(M_DEFAULT, BoxOverlay, 4, BoxPixelStartX, BoxPixelStartY, BoxPixelEndX, BoxPixelEndY, M_DEFAULT);

   // Erase (set to black) line segments overlapping valid depths of the depth map.
   MbufClearCond(BoxOverlay, 0.0, M_NULL, M_NULL, MilDepthMap, M_NOT_EQUAL, (MIL_DOUBLE)DEPTHMAP_INVALID_VALUE);

   // Draw the rest of the box.
   MgraLines(M_DEFAULT, BoxOverlay, NumLines-4, &BoxPixelStartX[4], &BoxPixelStartY[4], &BoxPixelEndX[4], &BoxPixelEndY[4], M_DEFAULT);
   
   // Draw, in the depth map overlay of the display, the box created.
   MIL_ID MilOverlay;
   MdispControl(MilDisplay, M_OVERLAY_CLEAR, M_DEFAULT);
   MdispInquire(MilDisplay, M_OVERLAY_ID, &MilOverlay);
   MbufCopyCond(BoxOverlay, MilOverlay, BoxOverlay, M_EQUAL, 1.0);

   MbufFree(BoxOverlay);
   }

//*****************************************************************************
// Allocate and initialize a buffer used as a color LUT legend.
//*****************************************************************************
MIL_ID CreateLutLegend(MIL_ID MilSystem)
   {
   // Allocate the color LUT legend as a buffer
   MIL_ID MilLutLegend;
   MbufAlloc2d(MilSystem, 1, NUM_DEPTHMAP_VALUES, 16 + M_UNSIGNED, M_IMAGE + M_PROC + M_DISP, &MilLutLegend);

   // Write a linear ramp in an array
   MIL_UINT16* pLUTLegend = new MIL_UINT16[NUM_DEPTHMAP_VALUES];
   for (MIL_INT i = 0; i < NUM_DEPTHMAP_VALUES; i++)
      pLUTLegend[i] = static_cast<MIL_UINT16>(NUM_DEPTHMAP_VALUES - i);

   // Put linear ramp in the legend buffer
   MbufPut(MilLutLegend, pLUTLegend);

   delete[] pLUTLegend;

   return MilLutLegend;
   }

//*****************************************************************************
// Draw the color LUT legend in the image.
//*****************************************************************************
void DrawLutLegend(MIL_ID DstImage, MIL_ID MilLutLegend)
   {
   // Allocate a child buffer to draw the legend
   MIL_INT LegendSizeX = (DEPTH_MAP_SIZE_X) / 8;               // in pixels
   MIL_INT LegendSizeY = (DEPTH_MAP_SIZE_Y * 3) / 4;           // in pixels
   MIL_INT LegendOffX  =  DEPTH_MAP_SIZE_X - LegendSizeX - 10; // in pixels
   MIL_INT LegendOffY  = (DEPTH_MAP_SIZE_Y - LegendSizeY) / 2; // in pixels
   MIL_ID  ChildForLegend = MbufChild2d(DstImage, LegendOffX, LegendOffY, LegendSizeX, LegendSizeY, M_NULL);

   // Draw LUT legend
   MimResize(MilLutLegend, ChildForLegend, M_FILL_DESTINATION, M_FILL_DESTINATION, M_BILINEAR);

   // Write depth values for color at lowest, middle and highest depths
   MIL_DOUBLE GrayLevelSizeZ;
   McalInquire(DstImage, M_GRAY_LEVEL_SIZE_Z, &GrayLevelSizeZ);
   MIL_DOUBLE GrayScaleWorldBoxHeight = fabs(GrayLevelSizeZ) * (NUM_DEPTHMAP_VALUES-1);

   MIL_DOUBLE LegendLowZ = 0.0;                                            // in world units
   MIL_DOUBLE LegendMidZ = LegendLowZ + (GrayScaleWorldBoxHeight * 0.5);   // in world units
   MIL_DOUBLE LegendHiZ  = LegendLowZ +  GrayScaleWorldBoxHeight;          // in world units

   MIL_TEXT_CHAR mmTxt[256];
   MgraControl(M_DEFAULT, M_TEXT_ALIGN_HORIZONTAL, M_CENTER);
   MgraColor(M_DEFAULT, 32767);

   MosSprintf(mmTxt, 256, MIL_TEXT("%.0f mm"), LegendHiZ);
   MgraText(M_DEFAULT, ChildForLegend, LegendSizeX / 2, 5, mmTxt);

   MosSprintf(mmTxt, 256, MIL_TEXT("%.0f mm"), LegendMidZ);
   MgraText(M_DEFAULT, ChildForLegend, LegendSizeX / 2, LegendSizeY / 2, mmTxt);

   MosSprintf(mmTxt, 256, MIL_TEXT("%.0f mm"), LegendLowZ);
   MgraText(M_DEFAULT, ChildForLegend, LegendSizeX / 2, LegendSizeY - 20, mmTxt);

   // Free legend child buffer
   MbufFree(ChildForLegend); 
   }