| Click here to show toolbars of the Web Online Help System: show toolbars |
//***************************************************************************************/ // // File name: 3dModelHeightDefect.cpp // Location: See Matrox Example Launcher in the MIL Control Center // // // 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-2016. // 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; // % // Defects detection parameters. static const MIL_INT ARITH_DIST_NN = 7; // 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_EXCLUDE_INVALID_POINTS + 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 (distance 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 distances 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 the distance map between the object and model depth maps is computed. \n\n")); // Defect highlighting: absolute nearest-neighbor distances between model depth map // and fixtured object depth map. M3dmapArith(MilDepthMap[eModel], MilDepthMap[eObject], MilDepthMap[eDefects], M_NULL, M_DIST_NN(ARITH_DIST_NN), M_USE_DESTINATION_SCALES); // Display highlighted defects. DrawLutLegend(MilDepthMap[eDefects], MilLutLegend); MdispSelect(MilDisplay[eDefects], MilDepthMap[eDefects]); MosPrintf(MIL_TEXT("The resulting distance map 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); // Replace the last value with a special color for invalid points MIL_UINT8 ColorForInvalidDepth[3] = {InvalidDepthGray, InvalidDepthGray, InvalidDepthGray}; MbufPutColor2d(MilLut, M_PLANAR, M_ALL_BANDS, DEPTHMAP_INVALID_VALUE, 0, 1, 1, ColorForInvalidDepth); } //***************************************************************************** // 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); }