| Click here to show toolbars of the Web Online Help System: show toolbars |
//***************************************************************************************/ // // File name: Simple3dBinPicking.cpp // Location: See Matrox Example Launcher in the MIL Control Center // // // Synopsis: This program contains an example of simple 3D bin picking // by combining a 2D Model Finder and a 3D alignment. // // Copyright (C) Matrox Electronic Systems Ltd., 1992-2016. // All Rights Reserved //***************************************************************************************/ #include <mil.h> #include <math.h> #include <stdlib.h> #include <vector> //**************************************************************************** // Example description. //**************************************************************************** void PrintHeader() { MosPrintf(MIL_TEXT("[EXAMPLE NAME]\n")); MosPrintf(MIL_TEXT("Simple3dBinPicking\n\n")); MosPrintf(MIL_TEXT("[SYNOPSIS]\n")); MosPrintf(MIL_TEXT("This example shows how to combine 2D Model Finder and \n")); MosPrintf(MIL_TEXT("3D alignment to estimate the pose of 3D objects stacked \n")); MosPrintf(MIL_TEXT("with minor variations in pitch and roll. \n\n")); MosPrintf(MIL_TEXT("[MODULES USED]\n")); MosPrintf(MIL_TEXT("Modules used: 3dMap, Geometric Model Finder, Buffer, \n") MIL_TEXT("Calibration, Display, Graphics, Image Processing. \n\n")); } // Enumerator to index model and scene objects (values must not change). enum {eModel = 0, eScene = 1}; // File names of the point clouds acquired from a 3D device such as a Gocator 3110 from LMI Technologies static const MIL_INT NUM_SCENE_SCANS = 3; static const MIL_TEXT_CHAR* const FILE_POINT_CLOUD[NUM_SCENE_SCANS + 1] = {M_IMAGE_PATH MIL_TEXT("Simple3dBinPicking/3dPlugCloudModel.ply" ), // Equivalent index: eModel (0) M_IMAGE_PATH MIL_TEXT("Simple3dBinPicking/BinCloudScene_0.ply" ), // Equivalent index: eScene + 0 (1) M_IMAGE_PATH MIL_TEXT("Simple3dBinPicking/BinCloudScene_1.ply" ), // Equivalent index: eScene + 1 (2) M_IMAGE_PATH MIL_TEXT("Simple3dBinPicking/BinCloudScene_2.ply" )}; // Equivalent index: eScene + 2 (3) #define MAX_STRING_SIZE 256L // Maximum number of characters a string can have. // Set this constant to 1 if the example should run without live grabbing // with a third-party 3D scanner; in this case, 3D point cloud data will // be loaded from PLY files. Otherwise, setting this constant to 0, the // user must provide the code to acquire the data from an installed 3D // scanner. See function 'AcquirePointCloud()' for more details. #define STANDALONE_DEMO 1 // Depth map parameters. static const MIL_INT DEPTHMAP_SIZE_X = 300; // (pixels) static const MIL_INT DEPTHMAP_SIZE_Y = 480; // (pixels) static const MIL_INT DEPTHMAP_NUM_VALUES = 65536; // 16 bits image (2^16) static const MIL_DOUBLE DEPTHMAP_MISSING_DATA = DEPTHMAP_NUM_VALUES-1; // Numeric value of missing depth-map data. // 3D scanner field of view, excluding the floor. static const MIL_DOUBLE SCANNER_FOV_MIN_X = -44.0; // (mm) static const MIL_DOUBLE SCANNER_FOV_MIN_Y = -80.0; // (mm) static const MIL_DOUBLE SCANNER_FOV_MIN_Z = -6.0; // (mm) static const MIL_DOUBLE SCANNER_FOV_MAX_X = 50.0; // (mm) static const MIL_DOUBLE SCANNER_FOV_MAX_Y = 80.0; // (mm) static const MIL_DOUBLE SCANNER_FOV_MAX_Z = -60.0; // (mm) // ROI margins to add to model's bounding box. static const MIL_DOUBLE MODEL_ROI_MARGIN_X = 5.0; // (mm) static const MIL_DOUBLE MODEL_ROI_MARGIN_Y = 5.0; // (mm) static const MIL_DOUBLE MODEL_ROI_MARGIN_Z = 20.0; // (mm) // 2d Model Finder parameters. static const MIL_DOUBLE FINDER_ACCEPTANCE = 50.0; // (%) // 2d Model Finder graphic colors. #define FOUND_OCCURRENCES_COLOR M_RGB888(192, 0, 0) #define SELECTED_OCCURRENCE_COLOR M_RGB888(0, 255, 0) // 3D alignment parameters. static const MIL_INT ALIGN_DECIMATION_STEP_MODEL = 4; // Corresponds to a decimation factor of 4*4=16. static const MIL_INT ALIGN_DECIMATION_STEP_SCENE = 4; // Corresponds to a decimation factor of 4*4=16. static const MIL_DOUBLE ALIGN_MODEL_OVERLAP = 90.0; // (%) static const MIL_INT ALIGN_MAX_ITERATIONS = 50; static const MIL_INT ALIGN_ERROR_MINIMIZATION_METRIC = M_POINT_TO_POINT; // Structure grouping the six components of a 3D pose. struct SPose { MIL_DOUBLE Tx, Ty, Tz, Rx, Ry, Rz; }; // Structure defining a 3D box and its corresponding 2d ROI in a depth-map. struct SBox { MIL_DOUBLE MinX, MinY, MinZ, MaxX, MaxY, MaxZ; // 3D bounding intervals in each dimension. MIL_INT OffsetX, OffsetY, SizeX, SizeY; // 2d ROI in the depth-map. }; // Utility function. void CropPointCloudToBox(MIL_ID MilPtCldCtn); void DefineModelRoiBox(MIL_ID MilPtCldCtn, MIL_ID MilDepthMap, SBox& ModelRoiBox); void MapDynamicRangeTo8Bits(MIL_ID MilSystem, MIL_ID MilScrImage, MIL_ID MilTgtImage); void AcquirePointCloud(MIL_ID MilPtCldCtn, MIL_INT Index); void RemoveFixture(MIL_ID Id); void GetPose(MIL_ID PtCldCtn, MIL_INT64 TargetCoordinateSystem, MIL_INT64 ReferenceCoordinateSystem, SPose& Pose); bool FindPrealignmentWithTopFoundOccurrence(MIL_ID MilSystem, MIL_ID MilPtCldCtn[], MIL_DOUBLE ModelMeanElevation, MIL_ID MilDepthMap[], MIL_ID MilFinderFixturingOffset, MIL_ID MilFinderResult, MIL_ID MilPrealignmentMatrix, MIL_INT* pTopOccIdx); void ExtractAlignedDepthMaps(MIL_ID MilSystem, MIL_ID MilPtCldCtn[], MIL_ID MilDepthMap[], MIL_ID MilAlignmentResult, SPose& AlignmentPose); bool AlignmentCriterionReached(MIL_INT64 AlignmentStatus); void DisplayAlignment2d(MIL_ID MilDepthMap[], MIL_ID MilGraphicList, SBox ModelBox); bool CheckForRequiredMILFile(MIL_CONST_TEXT_PTR FileName); // Preprocessor switch to enable a 3D display. #if M_MIL_USE_WINDOWS && !M_MIL_USE_CE && !M_MIL_USE_RT #define ENABLE_DISPLAY_3D 1 #else #define ENABLE_DISPLAY_3D 0 #endif #if ENABLE_DISPLAY_3D #include <MdispD3D.h> // 3D display parameters. static const MIL_INT DISPLAY_3D_SIZE_X = 640; // (pixels) static const MIL_INT DISPLAY_3D_SIZE_Y = 480; // (pixels) static const MIL_DOUBLE DISPLAY_3D_ROTATE = MD3D_FALSE; // MD3D_FALSE: do not rotate the display. static const MIL_DOUBLE DISPLAY_3D_POINT = MD3D_ENABLE; // MD3D_ENABLE: display points. static const MIL_DOUBLE DISPLAY_3D_LOOK_AT_X = 0.0; // (mm) static const MIL_DOUBLE DISPLAY_3D_LOOK_AT_Y = 0.0; // (mm) static const MIL_DOUBLE DISPLAY_3D_LOOK_AT_Z = 0.0; // (mm) static const MIL_DOUBLE DISPLAY_3D_EYE_THETA = 45.0; // (deg) static const MIL_DOUBLE DISPLAY_3D_EYE_PHI = 45.0; // (deg) static const MIL_DOUBLE DISPLAY_3D_EYE_DIST = 180.0; // (mm) void Init3dDisplay(MIL_DISP_D3D_HANDLE MilDispD3D); void Print3dDisplayHelp(); void DisplayPointCloud3d(MIL_ID MilSystem, MIL_DISP_D3D_HANDLE& MilDispD3D, MIL_ID MilDepthMap); void DisplayAlignment3d(MIL_ID MilSystem, MIL_DISP_D3D_HANDLE& MilDispD3D, MIL_ID MilDepthMap[]); #endif //***************************************************************************** // Main. //***************************************************************************** int MosMain(void) { // Print example information in console. PrintHeader(); //-------------------------------------------------------------------------- // Allocate MIL objects. MIL_ID MilApplication; // Application identifier. MIL_ID MilSystem; // System identifier. MIL_ID MilDisplay; // Display for model and object depth maps, and defects distance map. MIL_ID MilGraphicList; // Graphic list in which to draw. MIL_ID MilPtCldCtn[2]; // Model and scene point cloud containers. MIL_ID MilDepthMap[2]; // Model and scene depth maps. MIL_DOUBLE ModelMeanElevation; // Model's mean elevation computed in depth-map. SBox ModelRoiBox; // Model's 3D bounding box and 2d ROI. MIL_ID MilFinderImage[2]; // Images used by model finder: the model and the scene (target). MIL_ID MilFinderContext; // Model finder context. MIL_ID MilFinderFixturingOffset; // Fixturing offset between model's reference point and world's origin. MIL_ID MilFinderResult; // Model finder result. MIL_INT TopOccurrenceIdx; // Index of the found occurrence of top of the stack. MIL_ID MilAlignmentContext; // Pairwise alignment context. MIL_ID MilAlignmentResult; // Pairwise alignment result. MIL_ID MilPrealignmentMatrix; // Prealignment homogeneous matrix array. MIL_INT64 AlignmentStatus; // Status of 3D alignment. SPose AlignmentPose; // The six components of the 3D pose found with 3D alignment. #if ENABLE_DISPLAY_3D MIL_DISP_D3D_HANDLE MilDispD3D = M_NULL; #endif // Allocate application and system. MappAlloc(M_NULL, M_DEFAULT, &MilApplication); //Check for required file. if (!CheckForRequiredMILFile(FILE_POINT_CLOUD[0])) { MappFree(MilApplication); return -1; } MsysAlloc(M_DEFAULT, M_SYSTEM_HOST, M_DEFAULT, M_DEFAULT, &MilSystem); // Allocate and initialize the display. MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, &MilDisplay); MgraAllocList(MilSystem, M_DEFAULT, &MilGraphicList); MdispControl(MilDisplay, M_ASSOCIATED_GRAPHIC_LIST_ID, MilGraphicList); MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n")); MosGetch(); //-------------------------------------------------------------------------- // Import model's point cloud and generate its 16 bits depth-map. // Allocate the model's 3D point cloud container. M3dmapAllocResult(MilSystem, M_POINT_CLOUD_CONTAINER, M_DEFAULT, &MilPtCldCtn[eModel]); // Acquire a point cloud representing the top-view of a single object, defined as the model. AcquirePointCloud(MilPtCldCtn[eModel], eModel); // Allocate the model's depth-map. MbufAlloc2d(MilSystem, DEPTHMAP_SIZE_X, DEPTHMAP_SIZE_Y, M_UNSIGNED + 16, M_IMAGE + M_DISP + M_PROC, &MilDepthMap[eModel]); // Set the model's extraction box to the scanner field of view, but removing the floor. M3dmapSetBox(MilPtCldCtn[eModel], M_EXTRACTION_BOX, M_BOTH_CORNERS, SCANNER_FOV_MIN_X, SCANNER_FOV_MIN_Y, SCANNER_FOV_MIN_Z, SCANNER_FOV_MAX_X, SCANNER_FOV_MAX_Y, SCANNER_FOV_MAX_Z); // Crop the point cloud to the extraction box, defined as the // scanner field of view, rejecting the floor. CropPointCloudToBox(MilPtCldCtn[eModel]); // Set depth-map's extraction overlap property such that points on top // overwrites points below. M3dmapControl(MilPtCldCtn[eModel], M_GENERAL, M_EXTRACTION_OVERLAP, M_MAX); // Generate model's top-view depth-map. M3dmapExtract(MilPtCldCtn[eModel], MilDepthMap[eModel], M_NULL, M_CORRECTED_DEPTH_MAP, M_ALL, M_DEFAULT); // Compute model's mean depth-map elevation. M3dmapStat(MilDepthMap[eModel], M_NULL, M_NULL, M_NULL, M_DEVIATION_MEAN+M_STAT_ALL, M_DEFAULT, M_DEFAULT, &ModelMeanElevation); //-------------------------------------------------------------------------- // Generate an 8 bits image with optimal dynamic range from the ROI in the model's // depth-map. Define it as Model Finder's model and preprocess its context. // Compute model's 3D bounding box and corresponding ROI in the depth-map, // from which the Model Finder's model will be defined. DefineModelRoiBox(MilPtCldCtn[eModel], MilDepthMap[eModel], ModelRoiBox); // Allocate a buffer that will contain a copy of the model's depth-map's ROI image from // which the Model Finder's model will be defined. MbufAlloc2d(MilSystem, ModelRoiBox.SizeX, ModelRoiBox.SizeY, M_UNSIGNED + 8, M_IMAGE + M_DISP + M_PROC, &MilFinderImage[eModel]); // Create an 8 bits gray-scale image from 16 bits depth-map's dynamic range, // since Model Finder only works with 8 bits images. Use a child buffer to specify the // ROI in the model's depth-map. MIL_ID MilModelRoiChild = MbufChild2d(MilDepthMap[eModel], ModelRoiBox.OffsetX, ModelRoiBox.OffsetY, ModelRoiBox.SizeX, ModelRoiBox.SizeY, M_NULL); MapDynamicRangeTo8Bits(MilSystem, MilModelRoiChild, MilFinderImage[eModel]); MbufFree(MilModelRoiChild); // Allocate model finder objects. MmodAlloc(MilSystem, M_GEOMETRIC, M_DEFAULT, &MilFinderContext); MmodAllocResult(MilSystem, M_DEFAULT, &MilFinderResult); // Add the model's depth-map's ROI as the ModelFinder model. MmodDefine(MilFinderContext, M_IMAGE, MilFinderImage[eModel], M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT); // Set control to find all occurrences in the scene. MmodControl(MilFinderContext, M_ALL, M_NUMBER, M_ALL); // Set acceptance relatively low to account for small deformations // caused by object's 3D pose. MmodControl(MilFinderContext, M_ALL, M_ACCEPTANCE, FINDER_ACCEPTANCE); // Allocate the fixturing offset that will contain the relation between // the model's reference point and the relative coordinate system's origin. McalAlloc(MilSystem, M_FIXTURING_OFFSET, M_DEFAULT, &MilFinderFixturingOffset); // Determine the fixturing offset from the model defined in the context. McalFixture(M_NULL, MilFinderFixturingOffset, M_LEARN_OFFSET, M_MODEL_MOD, MilFinderContext, 0, M_DEFAULT, M_DEFAULT, M_DEFAULT); // Preprocess ModelFinder context. MmodPreprocess(MilFinderContext, M_DEFAULT); // Draw model's edges. MdispSelect(MilDisplay, MilFinderImage[eModel]); MgraColor(M_DEFAULT, SELECTED_OCCURRENCE_COLOR); MmodDraw(M_DEFAULT, MilFinderContext, MilGraphicList, M_DRAW_EDGES+M_DRAW_BOX+M_DRAW_POSITION, 0, M_DEFAULT); MosPrintf(MIL_TEXT("A scanned object sample, acquired using a third-party 3D scanner, \n" )); MosPrintf(MIL_TEXT("is restored. A top-view depth-map of the object is generated and \n" )); MosPrintf(MIL_TEXT("used to define a 2-dimensional Model Finder model (displayed \n" )); MosPrintf(MIL_TEXT("in green). \n\n")); MosPrintf(MIL_TEXT("Press any key to continue. \n\n")); MosGetch(); // ---------------------------------------------------------------------------------- // Find model occurrences' 3D poses in bin stack scenes. // Allocate the scene's 3D point cloud container. M3dmapAllocResult(MilSystem, M_POINT_CLOUD_CONTAINER, M_DEFAULT, &MilPtCldCtn[eScene]); // Allocate the scene's depth-map. MbufAlloc2d(MilSystem, DEPTHMAP_SIZE_X, DEPTHMAP_SIZE_Y, M_UNSIGNED + 16, M_IMAGE + M_DISP + M_PROC, &MilDepthMap[eScene]); // Set the scene's extraction box to the scanner field of view, but removing the floor. M3dmapSetBox(MilPtCldCtn[eScene], M_EXTRACTION_BOX, M_BOTH_CORNERS, SCANNER_FOV_MIN_X, SCANNER_FOV_MIN_Y, SCANNER_FOV_MIN_Z, SCANNER_FOV_MAX_X, SCANNER_FOV_MAX_Y, SCANNER_FOV_MAX_Z); // Set depth-map's extraction overlap property such that points on top // overwrites points below. M3dmapControl(MilPtCldCtn[eScene], M_GENERAL, M_EXTRACTION_OVERLAP, M_MAX); // Allocate a buffer that will contain a copy of the scene's depth-map image in // which Model Finder will find occurrences of the model. MbufAlloc2d(MilSystem, DEPTHMAP_SIZE_X, DEPTHMAP_SIZE_Y, M_UNSIGNED+8, M_IMAGE+M_DISP+M_PROC, &MilFinderImage[eScene]); // Allocate 3D pairwise alignment objects. M3dmapAlloc(MilSystem, M_PAIRWISE_ALIGNMENT_CONTEXT, M_DEFAULT, &MilAlignmentContext); M3dmapAllocResult(MilSystem, M_ALIGNMENT_RESULT, M_DEFAULT, &MilAlignmentResult); MbufAlloc2d(MilSystem, 4, 4, M_FLOAT+32, M_ARRAY, &MilPrealignmentMatrix); // Set 3D alignment parameters M3dmapControl(MilAlignmentContext, M_DEFAULT, M_DECIMATION_STEP_MODEL , ALIGN_DECIMATION_STEP_MODEL ); M3dmapControl(MilAlignmentContext, M_DEFAULT, M_DECIMATION_STEP_SCENE , ALIGN_DECIMATION_STEP_SCENE ); M3dmapControl(MilAlignmentContext, M_DEFAULT, M_MODEL_OVERLAP , ALIGN_MODEL_OVERLAP ); M3dmapControl(MilAlignmentContext, M_DEFAULT, M_MAX_ITERATIONS , ALIGN_MAX_ITERATIONS ); M3dmapControl(MilAlignmentContext, M_DEFAULT, M_ERROR_MINIMIZATION_METRIC, ALIGN_ERROR_MINIMIZATION_METRIC); // For each scene, find the model occurrence on top of the stack using // 2d model finder and determine its 3D pose using 3D alignment. for (MIL_INT iScene = 0; iScene < NUM_SCENE_SCANS; iScene++) { // Acquire a point cloud of the scene, representing a stack of model occurrences. AcquirePointCloud(MilPtCldCtn[eScene], eScene + iScene); // Generate the scene's top-view depth-map. M3dmapExtract(MilPtCldCtn[eScene], MilDepthMap[eScene], M_NULL, M_CORRECTED_DEPTH_MAP, M_ALL, M_DEFAULT); // Create an 8 bits gray-scale image from 16 bits depth-map's dynamic range, // since Model Finder only works with 8 bits images. MapDynamicRangeTo8Bits(MilSystem, MilDepthMap[eScene], MilFinderImage[eScene]); // Display the scene's depth-map. MgraClear(M_DEFAULT, MilGraphicList); MdispSelect(MilDisplay, MilFinderImage[eScene]); #if ENABLE_DISPLAY_3D // Show the scene's depth-map as a point cloud using a 3D display. DisplayPointCloud3d(MilSystem, MilDispD3D, MilDepthMap[eScene]); #endif // Find occurrences of the model in the scene's depth-map. MmodFind(MilFinderContext, MilFinderImage[eScene], MilFinderResult); // Draw edges of all found occurrences. MgraColor(M_DEFAULT, FOUND_OCCURRENCES_COLOR); MmodDraw(M_DEFAULT, MilFinderResult, MilGraphicList, M_DRAW_EDGES, M_ALL, M_DEFAULT); // Determine which found occurrence is on top of the stack and define a fixturing transform that // prealigns the model and this occurrence, to be used as a starting point for M3dmapAlign. if (!FindPrealignmentWithTopFoundOccurrence(MilSystem, MilPtCldCtn, ModelMeanElevation, MilDepthMap, MilFinderFixturingOffset, MilFinderResult, MilPrealignmentMatrix, &TopOccurrenceIdx)) { MosPrintf(MIL_TEXT("No occurrence found. Press any key to continue.\n\n")); MosGetch(); continue; } // Highlight the selected found occurrence, determined to be on top of the stack. MgraColor(M_DEFAULT, SELECTED_OCCURRENCE_COLOR); MmodDraw(M_DEFAULT, MilFinderResult, MilGraphicList, M_DRAW_EDGES+M_DRAW_BOX, TopOccurrenceIdx, M_DEFAULT); // Display information in prompt about the stack. if (iScene == 0) MosPrintf(MIL_TEXT("The stack of objects has been scanned. \n\n")); else { MosPrintf(MIL_TEXT("The first object located was removed from the stack of objects \n")); MosPrintf(MIL_TEXT("and a new scan was done. \n\n")); } // Display Model Finder information in prompt. MosPrintf(MIL_TEXT("Object occurrences that are on top are located in the 2-dimensional depth-map. \n")); MosPrintf(MIL_TEXT("The top-most occurrence is detected (displayed in green). \n\n")); #if ENABLE_DISPLAY_3D // Print the 3D display help in command prompt (only once). if (iScene == 0) Print3dDisplayHelp(); #endif MosPrintf(MIL_TEXT("Press any key to continue.\n\n")); MosGetch(); // Perform 3D alignment of the model and the selected found occurrence in the scene. M3dmapAlign(MilAlignmentContext, MilPtCldCtn[eModel], M_ALL, MilPtCldCtn[eScene], M_ALL, MilPrealignmentMatrix, MilAlignmentResult, M_DEFAULT, &AlignmentStatus); // Verify the success of the alignment. if (AlignmentCriterionReached(AlignmentStatus)) { // Use the 3D alignment result to generate a depth-map of the model aligned // on the occurrence. ExtractAlignedDepthMaps(MilSystem, MilPtCldCtn, MilDepthMap, MilAlignmentResult, AlignmentPose); MosPrintf(MIL_TEXT("The 3D pose of the top-most occurrence has been estimated using \n")); MosPrintf(MIL_TEXT("3D alignment with the 3D model sample: \n\n")); MosPrintf(MIL_TEXT("\t(X, Y, Z) : (%9.4f mm ,%9.4f mm ,%9.4f mm ) \n" ), AlignmentPose.Tx, AlignmentPose.Ty, AlignmentPose.Tz); MosPrintf(MIL_TEXT("\t(Roll, Pitch, Yaw): (%9.4f deg,%9.4f deg,%9.4f deg) \n\n"), AlignmentPose.Rx, AlignmentPose.Ry, AlignmentPose.Rz); // Show the result of the 3D alignment using a 2d display. DisplayAlignment2d(MilDepthMap, MilGraphicList, ModelRoiBox); #if ENABLE_DISPLAY_3D // Show the result of the 3D alignment using a 3D display. DisplayAlignment3d(MilSystem, MilDispD3D, MilDepthMap); #endif } else { MosPrintf(MIL_TEXT("Occurrence's pose was not successfully determined. \n\n")); } MosPrintf(MIL_TEXT("Press any key to continue.\n\n")); MosGetch(); } // Hide the display by unselecting any buffer associated. MdispSelect(MilDisplay, M_NULL); #if ENABLE_DISPLAY_3D MdispD3DHide(MilDispD3D); #endif MosPrintf(MIL_TEXT("Press any key to end.\n\n")); MosGetch(); //------------------------------------------------------------------------------------------- // Free allocations. MbufFree(MilPrealignmentMatrix); M3dmapFree(MilAlignmentResult); M3dmapFree(MilAlignmentContext); McalFree(MilFinderFixturingOffset); MmodFree(MilFinderResult); MmodFree(MilFinderContext); for (MIL_INT i = 0; i < 2; i++) { MbufFree(MilFinderImage[i]); M3dmapFree(MilPtCldCtn[i]); MbufFree(MilDepthMap[i]); } MgraFree(MilGraphicList); #if ENABLE_DISPLAY_3D MdispD3DFree(MilDispD3D); #endif MdispFree(MilDisplay); MsysFree(MilSystem); MappFree(MilApplication); return 0; } //**************************************************************************** // Compute 3D bounding box and corresponding 2d ROI in the depth-map. //**************************************************************************** void DefineModelRoiBox(MIL_ID MilPtCldCtn, MIL_ID MilDepthMap, SBox& ModelRoiBox) { // Compute a robust bounding box of all points. M3dmapControl(MilPtCldCtn, M_GENERAL, M_BOUNDING_BOX_ALGORITHM, M_ROBUST); M3dmapSetBox(MilPtCldCtn, M_EXTRACTION_BOX, M_BOUNDING_BOX, M_ALL, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT); // Inquire the computed bounding box. MIL_DOUBLE BoxMinX, BoxMinY, BoxMinZ; MIL_DOUBLE BoxMaxX, BoxMaxY, BoxMaxZ; M3dmapInquire(MilPtCldCtn, M_GENERAL, M_EXTRACTION_BOX_MIN_X + M_TYPE_MIL_DOUBLE, &BoxMinX); M3dmapInquire(MilPtCldCtn, M_GENERAL, M_EXTRACTION_BOX_MIN_Y + M_TYPE_MIL_DOUBLE, &BoxMinY); M3dmapInquire(MilPtCldCtn, M_GENERAL, M_EXTRACTION_BOX_MIN_Z + M_TYPE_MIL_DOUBLE, &BoxMinZ); M3dmapInquire(MilPtCldCtn, M_GENERAL, M_EXTRACTION_BOX_MAX_X + M_TYPE_MIL_DOUBLE, &BoxMaxX); M3dmapInquire(MilPtCldCtn, M_GENERAL, M_EXTRACTION_BOX_MAX_Y + M_TYPE_MIL_DOUBLE, &BoxMaxY); M3dmapInquire(MilPtCldCtn, M_GENERAL, M_EXTRACTION_BOX_MAX_Z + M_TYPE_MIL_DOUBLE, &BoxMaxZ); // Add a margin to the bounding box. BoxMinX -= MODEL_ROI_MARGIN_X; BoxMinY -= MODEL_ROI_MARGIN_Y; BoxMinZ -= MODEL_ROI_MARGIN_Z; BoxMaxX += MODEL_ROI_MARGIN_X; BoxMaxY += MODEL_ROI_MARGIN_Y; BoxMaxZ += MODEL_ROI_MARGIN_Z; // Convert x-y bounding corners from world to pixel units in the depth-map. MIL_DOUBLE RoiMinX, RoiMinY; MIL_DOUBLE RoiMaxX, RoiMaxY; McalTransformCoordinate(MilDepthMap, M_WORLD_TO_PIXEL, BoxMinX, BoxMinY, &RoiMinX, &RoiMinY); McalTransformCoordinate(MilDepthMap, M_WORLD_TO_PIXEL, BoxMaxX, BoxMaxY, &RoiMaxX, &RoiMaxY); // Copy values to output structure. ModelRoiBox.MinX = BoxMinX; ModelRoiBox.MinY = BoxMinY; ModelRoiBox.MinZ = BoxMinZ; ModelRoiBox.MaxX = BoxMaxX; ModelRoiBox.MaxY = BoxMaxY; ModelRoiBox.MaxZ = BoxMaxZ; ModelRoiBox.OffsetX = static_cast<MIL_INT>(RoiMinX); ModelRoiBox.OffsetY = static_cast<MIL_INT>(RoiMinY); ModelRoiBox.SizeX = static_cast<MIL_INT>(RoiMaxX - RoiMinX); ModelRoiBox.SizeY = static_cast<MIL_INT>(RoiMaxY - RoiMinY); // Redefine the extraction box as it was. M3dmapSetBox(MilPtCldCtn, M_EXTRACTION_BOX, M_BOTH_CORNERS, SCANNER_FOV_MIN_X, SCANNER_FOV_MIN_Y, SCANNER_FOV_MIN_Z, SCANNER_FOV_MAX_X, SCANNER_FOV_MAX_Y, SCANNER_FOV_MAX_Z); } //**************************************************************************** // Remove the points outside the extraction box from the point cloud. //**************************************************************************** void CropPointCloudToBox(MIL_ID MilPtCldCtn) { // Allocate enough memory to hold the maximum number of points. This is faster than // computing the actual number of points inside the extraction box. MIL_INT MaxNumElements = M3dmapGet(MilPtCldCtn, M_POINT_CLOUD_INDEX(0), M_XYZ, M_EXCLUDE_INVALID_POINTS, M_FLOAT+64, M_NULL, M_NULL, M_NULL, M_NULL, M_NULL); std::vector<MIL_DOUBLE> xyz(MaxNumElements); // Get the points inside the extraction box. MIL_INT ActualNumElements = M3dmapGet(MilPtCldCtn, M_POINT_CLOUD_INDEX(0), M_XYZ, M_INCLUDE_POINTS_INSIDE_BOX_ONLY, M_FLOAT+64, MaxNumElements, &xyz[0], M_NULL, M_NULL, M_NULL); // Put back the points in the same point cloud. // Since the number of points changed, the point cloud must be cleared before calling M3dmapPut(). M3dmapClear(MilPtCldCtn, M_POINT_CLOUD_INDEX(0), M_CLEAR, M_DEFAULT); M3dmapPut (MilPtCldCtn, M_POINT_CLOUD_INDEX(0), M_XYZ, M_FLOAT+64, ActualNumElements, &xyz[0], M_NULL, M_NULL, M_NULL, M_DEFAULT); } //**************************************************************************** // Create an 8 bits gray-scale image from 16 bits depth-map's dynamic range. //**************************************************************************** void MapDynamicRangeTo8Bits(MIL_ID MilSystem, MIL_ID MilScrImage, MIL_ID MilTgtImage) { // Allocate a statistics result and context to compute depth-map's min and max values. MIL_ID MilStatContext = MimAlloc(MilSystem, M_STATISTICS_CONTEXT, M_DEFAULT, M_NULL); MIL_ID MilStatResult = MimAllocResult(MilSystem, M_DEFAULT, M_STATISTICS_RESULT, M_NULL); MimControl(MilStatContext, M_STAT_MIN, M_ENABLE); MimControl(MilStatContext, M_STAT_MAX, M_ENABLE); MimControl(MilStatContext, M_CONDITION, M_NOT_EQUAL); MimControl(MilStatContext, M_COND_LOW, DEPTHMAP_MISSING_DATA); // Allocate a ramp LUT that will map the dynamic range of the 16 bits depth-map // into an 8 bits gray-scale image. MIL_ID MilDynRangeLut = MbufAlloc1d(MilSystem, DEPTHMAP_NUM_VALUES, 8+M_UNSIGNED, M_LUT, M_NULL); // Compute depth-map's extreme values, excluding missing data. MIL_INT DepthMapMin, DepthMapMax; MimStatCalculate(MilStatContext, MilScrImage, MilStatResult, M_DEFAULT); MimGetResult(MilStatResult, M_STAT_MIN+M_TYPE_MIL_INT, &DepthMapMin); MimGetResult(MilStatResult, M_STAT_MAX+M_TYPE_MIL_INT, &DepthMapMax); // Define a ramp LUT mapping the dynamic 16 bits range to the full 8 bits range. // The LUT also set all pixels outside this range to 0. MbufClear(MilDynRangeLut, 0.0); MgenLutRamp(MilDynRangeLut, DepthMapMin, 1.0, DepthMapMax, 255.0); // Perform the LUT mapping. MimLutMap(MilScrImage, MilTgtImage, MilDynRangeLut); // Free allocations. MbufFree(MilDynRangeLut); MimFree(MilStatResult); MimFree(MilStatContext); } //**************************************************************************** // Acquires a point cloud. In STANDALONE_DEMO mode, it imports the point cloud // from a PLY file. Otherwise, the user must insert the code to grab the 3D // data from an installed 3D scanner and connect it to the already existing // code snippet in order to put the 3D data in the point cloud container. //**************************************************************************** void AcquirePointCloud(MIL_ID MilPtCldCtn, MIL_INT Index) { // Do not accumulate scene point clouds in container, so clear it before acquisition. M3dmapClear(MilPtCldCtn, M_ALL, M_DELETE, M_DEFAULT); #if STANDALONE_DEMO // Import 3D stack scene acquired using a 3D device (e.g. a Gocator 3110 from LMI Technologies) // and exported to a PLY file. M3dmapImport(FILE_POINT_CLOUD[Index], M_PLY, MilPtCldCtn, M_POINT_CLOUD_LABEL(Index+1), M_NULL, M_DEFAULT); #else // <==== Insert here the code that grabs a point cloud using a third party 3D scanner. MIL_INT NumPoints = 1; // Number of grabbed 3D points (initial value should be overwritten). // <==== Insert here the code that inquires the number of grabbed 3D points and overwrites // the 'NumPoints' variable above. std::vector<MIL_DOUBLE> x(NumPoints); // x-coordinates of grabbed 3D points. std::vector<MIL_DOUBLE> y(NumPoints); // y-coordinates of grabbed 3D points. std::vector<MIL_DOUBLE> z(NumPoints); // z-coordinates of grabbed 3D points. // <==== Insert here the code that initializes the three 3D point coordinate // arrays above (x, y, z) from the grabbed 3D point cloud. // Put the grabbed 3D points into a MIL 3D point cloud. M3dmapPut(MilPtCldCtn, M_POINT_CLOUD_LABEL(Index+1), M_POSITION, M_FLOAT+64, NumPoints, &x[0], &y[0], &z[0], M_NULL, M_DEFAULT); // Display a warning in prompt if the point cloud does not seem to have been // properly grabbed or initialized. if (NumPoints == 1) { MosPrintf(MIL_TEXT("! WARNING: 3D data grab does not seem to have acquired \n")); MosPrintf(MIL_TEXT(" enough points or the point cloud does not seem \n")); MosPrintf(MIL_TEXT(" to have been properly initialized. Following \n")); MosPrintf(MIL_TEXT(" execution of the example may give unexpected results. \n\n")); MosPrintf(MIL_TEXT("Press any key to continue.\n\n")); MosGetch(); } #endif } //**************************************************************************** // Remove the fixture on an object by moving the relative coordinate system // to be on the absolute coordinate system. //**************************************************************************** void RemoveFixture(MIL_ID Id) { McalSetCoordinateSystem(Id, M_RELATIVE_COORDINATE_SYSTEM, M_ABSOLUTE_COORDINATE_SYSTEM, M_IDENTITY+M_ASSIGN, M_NULL, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT); } //**************************************************************************** // Get 3D pose of specified point cloud container. //**************************************************************************** void GetPose(MIL_ID MilPtCldCtn, MIL_INT64 TargetCoordinateSystem, MIL_INT64 ReferenceCoordinateSystem, SPose& Pose) { // Get 6 pose elements. McalGetCoordinateSystem(MilPtCldCtn, TargetCoordinateSystem, ReferenceCoordinateSystem, M_TRANSLATION , M_NULL, &Pose.Tx, &Pose.Ty, &Pose.Tz, M_NULL); McalGetCoordinateSystem(MilPtCldCtn, TargetCoordinateSystem, ReferenceCoordinateSystem, M_ROTATION_XYZ, M_NULL, &Pose.Rx, &Pose.Ry, &Pose.Rz, M_NULL); // Keep absolute rotation values below 180 degrees for clarity. if (fabs(Pose.Rx) > 180.0) Pose.Rx = fmod(Pose.Rx - 360.0, 360.0); if (fabs(Pose.Ry) > 180.0) Pose.Ry = fmod(Pose.Ry - 360.0, 360.0); if (fabs(Pose.Rz) > 180.0) Pose.Rz = fmod(Pose.Rz - 360.0, 360.0); } //**************************************************************************** // Determine which found occurrence is on top of the stack and define a // fixturing transform that prealigns the model and this occurrence. //**************************************************************************** bool FindPrealignmentWithTopFoundOccurrence(MIL_ID MilSystem, MIL_ID MilPtCldCtn[], MIL_DOUBLE ModelMeanElevation, MIL_ID MilDepthMap[], MIL_ID MilFinderFixturingOffset, MIL_ID MilFinderResult, MIL_ID MilPrealignmentMatrix, MIL_INT* pTopOccIdx) { // Get the number of found occurrences in the scene. MIL_INT NumOccurrences; MmodGetResult(MilFinderResult, M_GENERAL, M_NUMBER+M_TYPE_MIL_INT, &NumOccurrences); // Return if no occurrence. if (NumOccurrences == 0) return false; // Generate the top-view depth-map of the model in its initial pose // (with any fixturing removed). RemoveFixture(MilPtCldCtn[eModel]); M3dmapExtract(MilPtCldCtn[eModel], MilDepthMap[eModel], M_NULL, M_CORRECTED_DEPTH_MAP, M_ALL, M_DEFAULT); // Create a mask buffer of valid model points in the model's depth-map. MIL_ID ModelMask = MbufAlloc2d(MilSystem, DEPTHMAP_SIZE_X, DEPTHMAP_SIZE_Y, M_UNSIGNED+1, M_PROC+M_IMAGE+M_DISP, M_NULL); // In the model's mask, set to zero all pixels corresponding to missing data and // to one all valid pixels. MbufClear(ModelMask, 0.0); MbufCopyCond(MilDepthMap[eModel], ModelMask, MilDepthMap[eModel], M_NOT_EQUAL, DEPTHMAP_MISSING_DATA); // Loop on each found occurrence in the scene, compute its mean elevation in the depth-map // (using the model's mask) and select the higher (the one on top). MIL_DOUBLE CurTopOccMeanElevation = 0.0; for (MIL_INT iOcc = 0; iOcc < NumOccurrences; iOcc++) { // Fixture the scene's point cloud on the current found occurrence, taking // into account the fixturing offset between the model's reference point // and the world's origin. RemoveFixture(MilPtCldCtn[eScene]); McalFixture(MilPtCldCtn[eScene], MilFinderFixturingOffset, M_MOVE_RELATIVE, M_RESULT_MOD, MilFinderResult, iOcc, M_DEFAULT, M_DEFAULT, M_DEFAULT); // Extract the top-view depth-map of the scene fixtured on the occurrence. M3dmapExtract(MilPtCldCtn[eScene], MilDepthMap[eScene], M_NULL, M_CORRECTED_DEPTH_MAP, M_ALL, M_DEFAULT); // Compute occurrence's mean elevation in the scene only from the pixels // enabled by the model's mask. MIL_DOUBLE OccMeanElevation; M3dmapStat(MilDepthMap[eScene], M_NULL, ModelMask, M_NULL, M_DEVIATION_MEAN+M_STAT_ALL, M_DEFAULT, M_DEFAULT, &OccMeanElevation); // Since the z scale is negative, we actually search for the lowest elevation. if ((iOcc == 0) || (iOcc > 0 && (OccMeanElevation < CurTopOccMeanElevation)) ) { // Save the index and mean elevation of the currently selected occurrence. *pTopOccIdx = iOcc; CurTopOccMeanElevation = OccMeanElevation; // Apply an additional z offset to the scene's point cloud to align the // mean elevation of the occurrence with the mean elevation of the model. MIL_DOUBLE OffsetZ = CurTopOccMeanElevation - ModelMeanElevation; McalSetCoordinateSystem(MilPtCldCtn[eScene], M_RELATIVE_COORDINATE_SYSTEM, M_RELATIVE_COORDINATE_SYSTEM, M_TRANSLATION, M_NULL, 0.0, 0.0, OffsetZ, M_DEFAULT); // The resulting pose of the scene's point cloud, expressed as an homogeneous matrix, // defines a transformation that coarsely 3D aligns the model and the occurrence and // it can be used as a prealignment for M3dmapAlign. McalGetCoordinateSystem(MilPtCldCtn[eScene], M_RELATIVE_COORDINATE_SYSTEM, M_ABSOLUTE_COORDINATE_SYSTEM, M_HOMOGENEOUS_MATRIX, MilPrealignmentMatrix, M_NULL, M_NULL, M_NULL, M_NULL); } } // Free allocations. MbufFree(ModelMask); // Reset scene's pose and regenerate the scene's depth-map. RemoveFixture(MilPtCldCtn[eScene]); M3dmapExtract(MilPtCldCtn[eScene], MilDepthMap[eScene], M_NULL, M_CORRECTED_DEPTH_MAP, M_ALL, M_DEFAULT); return true; } //**************************************************************************** // Use the 3D alignment result to generate a depth-map of the model aligned // on the occurrence. //**************************************************************************** void ExtractAlignedDepthMaps(MIL_ID MilSystem, MIL_ID MilPtCldCtn[], MIL_ID MilDepthMap[], MIL_ID MilAlignmentResult, SPose& AlignmentPose) { // Use the 3D alignment result to fixture the model with the found occurrence in the scene. McalFixture(MilPtCldCtn[eScene], M_NULL, M_MOVE_RELATIVE, M_RESULT_ALIGNMENT_3DMAP, MilAlignmentResult, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT); // Determine the found occurrence 3D pose after 3D alignment. GetPose(MilPtCldCtn[eScene], M_RELATIVE_COORDINATE_SYSTEM, M_ABSOLUTE_COORDINATE_SYSTEM, AlignmentPose); // Get the full 3D pose of the occurrence. MIL_ID Pose = MbufAlloc2d(MilSystem, 4, 4, M_FLOAT+32, M_ARRAY, M_NULL); McalGetCoordinateSystem(MilPtCldCtn[eScene], M_ABSOLUTE_COORDINATE_SYSTEM, M_RELATIVE_COORDINATE_SYSTEM, M_HOMOGENEOUS_MATRIX, Pose, M_NULL, M_NULL, M_NULL, M_NULL); // Fixture the model to the found occurrence in the scene using its 3D pose. McalSetCoordinateSystem(MilPtCldCtn[eModel], M_RELATIVE_COORDINATE_SYSTEM, M_ABSOLUTE_COORDINATE_SYSTEM, M_HOMOGENEOUS_MATRIX, Pose, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT); M3dmapExtract(MilPtCldCtn[eModel], MilDepthMap[eModel], M_NULL, M_CORRECTED_DEPTH_MAP, M_ALL, M_DEFAULT); // Remove fixturing of the model and scene's point clouds. RemoveFixture(MilPtCldCtn[eModel]); RemoveFixture(MilPtCldCtn[eScene]); MbufFree(Pose); } //**************************************************************************** // Returns true if an alignment stop criterion has been successfully reached. //**************************************************************************** bool AlignmentCriterionReached(MIL_INT64 AlignmentStatus) { switch(AlignmentStatus) { case M_MAX_ITERATIONS_REACHED: case M_ALIGN_RMS_ERROR_THRESHOLD_REACHED: case M_ALIGN_RMS_ERROR_RELATIVE_THRESHOLD_REACHED: return true; case M_NOT_ENOUGH_POINT_PAIRS: case M_NO_VALID_POINTS: return false; default: return false; } } //**************************************************************************** // Show the result of 3D alignment using a 2d display. //**************************************************************************** void DisplayAlignment2d(MIL_ID MilDepthMap[], MIL_ID MilGraphicList, SBox ModelBox) { // The number of points needed to define the 12 lines representing a 3D box. const MIL_INT NumBoxPoints = 24; // Define box lines (from pairs of points) expressed in absolute coordinate system units. // | 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 BoxInAbsX[NumBoxPoints] = { ModelBox.MinX, ModelBox.MaxX, ModelBox.MaxX, ModelBox.MinX , ModelBox.MinX, ModelBox.MaxX, ModelBox.MaxX, ModelBox.MinX , ModelBox.MinX, ModelBox.MinX, ModelBox.MaxX, ModelBox.MaxX , ModelBox.MaxX, ModelBox.MaxX, ModelBox.MinX, ModelBox.MinX , ModelBox.MaxX, ModelBox.MaxX, ModelBox.MinX, ModelBox.MinX , ModelBox.MinX, ModelBox.MinX, ModelBox.MaxX, ModelBox.MaxX }; MIL_DOUBLE BoxInAbsY[NumBoxPoints] = { ModelBox.MinY, ModelBox.MinY, ModelBox.MaxY, ModelBox.MaxY , ModelBox.MinY, ModelBox.MinY, ModelBox.MaxY, ModelBox.MaxY , ModelBox.MinY, ModelBox.MaxY, ModelBox.MinY, ModelBox.MaxY , ModelBox.MinY, ModelBox.MaxY, ModelBox.MaxY, ModelBox.MinY , ModelBox.MinY, ModelBox.MaxY, ModelBox.MaxY, ModelBox.MinY , ModelBox.MinY, ModelBox.MaxY, ModelBox.MinY, ModelBox.MaxY }; MIL_DOUBLE BoxInAbsZ[NumBoxPoints] = { ModelBox.MaxZ, ModelBox.MaxZ, ModelBox.MaxZ, ModelBox.MaxZ , ModelBox.MinZ, ModelBox.MinZ, ModelBox.MinZ, ModelBox.MinZ , ModelBox.MinZ, ModelBox.MinZ, ModelBox.MinZ, ModelBox.MinZ , ModelBox.MaxZ, ModelBox.MaxZ, ModelBox.MaxZ, ModelBox.MaxZ , ModelBox.MinZ, ModelBox.MinZ, ModelBox.MinZ, ModelBox.MinZ , ModelBox.MaxZ, ModelBox.MaxZ, ModelBox.MaxZ, ModelBox.MaxZ }; // Convert 3D cube box points from absolute to pixel units in the depth-map. MIL_DOUBLE BoxInPixX[NumBoxPoints]; MIL_DOUBLE BoxInPixY[NumBoxPoints]; McalTransformCoordinate3dList(MilDepthMap[eModel], M_ABSOLUTE_COORDINATE_SYSTEM, M_PIXEL_COORDINATE_SYSTEM, NumBoxPoints, BoxInAbsX, BoxInAbsY, BoxInAbsZ, BoxInPixX, BoxInPixY, M_NULL , M_DEPTH_MAP); // Define each line as the start and end pixels in the image. MIL_INT NumLines = NumBoxPoints/2; MIL_DOUBLE* BoxInPixStartX = BoxInPixX; MIL_DOUBLE* BoxInPixStartY = BoxInPixY; MIL_DOUBLE* BoxInPixEndX = BoxInPixX + NumLines; MIL_DOUBLE* BoxInPixEndY = BoxInPixY + NumLines; // Remove any graphics. MgraClear(M_DEFAULT, MilGraphicList); // Draw lines of the box's back square. Make them darker for a clearer 3D effect. MIL_DOUBLE DarkColorR = 0.4 * M_RGB888_R(SELECTED_OCCURRENCE_COLOR); MIL_DOUBLE DarkColorG = 0.4 * M_RGB888_G(SELECTED_OCCURRENCE_COLOR); MIL_DOUBLE DarkColorB = 0.4 * M_RGB888_B(SELECTED_OCCURRENCE_COLOR); MgraColor(M_DEFAULT, M_RGB888(DarkColorR,DarkColorG,DarkColorB)); MgraLines(M_DEFAULT, MilGraphicList, 4, BoxInPixStartX, BoxInPixStartY, BoxInPixEndX, BoxInPixEndY, M_DEFAULT); // Draw lines connecting the front and back box's squares. MgraColor(M_DEFAULT, SELECTED_OCCURRENCE_COLOR); MgraLines(M_DEFAULT, MilGraphicList, 4, BoxInPixStartX+4, BoxInPixStartY+4, BoxInPixEndX+4, BoxInPixEndY+4, M_DEFAULT); // Draw lines of the box's front square. MgraColor(M_DEFAULT, SELECTED_OCCURRENCE_COLOR); MgraLines(M_DEFAULT, MilGraphicList, 4, BoxInPixStartX+8, BoxInPixStartY+8, BoxInPixEndX+8, BoxInPixEndY+8, M_DEFAULT); } #if ENABLE_DISPLAY_3D //**************************************************************************** // Initialize 3D display parameters. //**************************************************************************** void Init3dDisplay(MIL_DISP_D3D_HANDLE MilDispD3D) { // Set rendering parameters. MdispD3DControl(MilDispD3D, MD3D_ROTATE, DISPLAY_3D_ROTATE ); MdispD3DControl(MilDispD3D, MD3D_POINT , DISPLAY_3D_POINT ); // Set camera pose. MdispD3DControl(MilDispD3D, MD3D_LOOK_AT_X, DISPLAY_3D_LOOK_AT_X); MdispD3DControl(MilDispD3D, MD3D_LOOK_AT_Y, DISPLAY_3D_LOOK_AT_Y); MdispD3DControl(MilDispD3D, MD3D_LOOK_AT_Z, DISPLAY_3D_LOOK_AT_Z); MdispD3DControl(MilDispD3D, MD3D_EYE_THETA, DISPLAY_3D_EYE_THETA); MdispD3DControl(MilDispD3D, MD3D_EYE_PHI, DISPLAY_3D_EYE_PHI); MdispD3DControl(MilDispD3D, MD3D_EYE_DIST, DISPLAY_3D_EYE_DIST); } //**************************************************************************** // Print 3D display commands help in prompt. //**************************************************************************** void Print3dDisplayHelp() { MosPrintf(MIL_TEXT("D3D display controls (note: you must set the focus on the D3D display window):\n") MIL_TEXT(" . Left-click to move the object.\n") MIL_TEXT(" . Right-click to rotate the object.\n") MIL_TEXT(" . Scroll wheel to zoom the object.\n") MIL_TEXT(" . Press 'R' to start/stop the animation.\n") MIL_TEXT(" . Press 'P' to enable/disable the point cloud.\n") MIL_TEXT(" . Press 'V' to change the display of the 3d map visualization volume.\n") MIL_TEXT(" . Press 'L' to change the depth color mode (intensity or LUT (gray or Color)).\n\n")); } //**************************************************************************** // Show a depth-map as a point cloud using a 3D display. //**************************************************************************** void DisplayPointCloud3d(MIL_ID MilSystem, MIL_DISP_D3D_HANDLE& MilDispD3D, MIL_ID MilDepthMap) { // Allocate a texture color map for each depth-map. MIL_ID MilTexture = MbufAllocColor(MilSystem, 3, DEPTHMAP_SIZE_X, DEPTHMAP_SIZE_Y, 8+M_UNSIGNED, M_IMAGE+M_PROC, M_NULL); // The depth maps' textures are uniform. MbufClearCond(MilTexture, 150, 150, 150, MilDepthMap, M_NOT_EQUAL, DEPTHMAP_MISSING_DATA); // If the 3D display is not allocated yet, allocate and initialize it. if (MilDispD3D == M_NULL) { MilDispD3D = MdepthSysD3DAlloc(M_NULL, M_NULL, M_NULL, M_NULL, &MilDepthMap, &MilTexture, 1, DISPLAY_3D_SIZE_X, DISPLAY_3D_SIZE_Y, 8, M_NULL); // Detect the case where the 3D display could not be allocated. It can happen if // it is not supported on the current system. If it is the case, free allocations // and quit without doing anything. if (MilDispD3D == M_NULL) { MbufFree(MilTexture); return; } Init3dDisplay(MilDispD3D); } else // If already allocated, only update its data to display MdepthSysD3DSetSystems(MilDispD3D, M_NULL, M_NULL, M_NULL, M_NULL, &MilDepthMap, &MilTexture, 1, 8); // Show the 3D display. MdispD3DShow(MilDispD3D); // Free allocations. MbufFree(MilTexture); } //**************************************************************************** // Show the result of 3D alignment using a 3D display. //**************************************************************************** void DisplayAlignment3d(MIL_ID MilSystem, MIL_DISP_D3D_HANDLE& MilDispD3D, MIL_ID MilDepthMap[]) { // Allocate a texture color map for each depth-map. MIL_ID MilTexture[2]; for (MIL_INT i = 0; i < 2; i++) MbufAllocColor(MilSystem, 3, DEPTHMAP_SIZE_X, DEPTHMAP_SIZE_Y, 8 + M_UNSIGNED, M_IMAGE + M_PROC, &MilTexture[i]); // The depth maps' textures are uniform. MIL_DOUBLE ModelColorR = M_RGB888_R(SELECTED_OCCURRENCE_COLOR); MIL_DOUBLE ModelColorG = M_RGB888_G(SELECTED_OCCURRENCE_COLOR); MIL_DOUBLE ModelColorB = M_RGB888_B(SELECTED_OCCURRENCE_COLOR); MbufClearCond(MilTexture[eScene], 150, 150, 150, MilDepthMap[eScene], M_NOT_EQUAL, DEPTHMAP_MISSING_DATA); MbufClearCond(MilTexture[eModel], ModelColorR, ModelColorG, ModelColorB, MilDepthMap[eModel], M_NOT_EQUAL, DEPTHMAP_MISSING_DATA); // Associate the calibration so that both maps are expressed in the // same coordinate system. McalAssociate(MilDepthMap[eScene], MilDepthMap[eModel], M_DEFAULT); // Set to invalid pixels of the model's depth-map that are invalid in the // scene's depth-map to not display them. MbufClearCond(MilDepthMap[eModel], DEPTHMAP_MISSING_DATA, 0, 0, MilDepthMap[eScene], M_EQUAL, DEPTHMAP_MISSING_DATA); // If the 3D display is not allocated yet, allocate and initialize it. if (MilDispD3D == M_NULL) { MilDispD3D = MdepthSysD3DAlloc(M_NULL, M_NULL, M_NULL, M_NULL, MilDepthMap, MilTexture, 2, DISPLAY_3D_SIZE_X, DISPLAY_3D_SIZE_Y, 8, M_NULL); Init3dDisplay(MilDispD3D); } else // If already allocated, only update its data to display MdepthSysD3DSetSystems(MilDispD3D, M_NULL, M_NULL, M_NULL, M_NULL, MilDepthMap, MilTexture, 2, 8); // Show the 3D display. MdispD3DShow(MilDispD3D); // Free allocations. for (MIL_INT i = 0; i < 2; i++) MbufFree(MilTexture[i]); } #endif //**************************************************************************** // Check that the given file is present. //**************************************************************************** bool CheckForRequiredMILFile(MIL_CONST_TEXT_PTR FileName) { MIL_INT FilePresent; MappFileOperation(M_DEFAULT, FileName, M_NULL, M_NULL, M_FILE_EXISTS, M_DEFAULT, &FilePresent); if (FilePresent == M_NO) { MosPrintf(MIL_TEXT("\n") MIL_TEXT("The footage needed to run this example is missing. You need \n") MIL_TEXT("to obtain and apply a separate specific update to have it.\n\n")); MosPrintf(MIL_TEXT("Press <Enter> to end.\n\n")); MosGetch(); } return (FilePresent == M_YES); }