| Click here to show toolbars of the Web Online Help System: show toolbars |
//***************************************************************************************/ // // File name: Simple3dStiching.cpp // Location: See Matrox Example Launcher in the MIL Control Center // // // Synopsis: This program contains an example of 3D surface alignment. It aligns two partial // point clouds of a 3D object and stitches them into a single complete point cloud. // 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> // Preprocessor switch to enable a 3d display (not available on Linux). #if M_MIL_USE_LINUX #define ENABLE_DISPLAY_3D 0 #else #define ENABLE_DISPLAY_3D 1 #include <MdispD3D.h> #endif //**************************************************************************** // Example description. //**************************************************************************** void PrintHeader() { MosPrintf(MIL_TEXT("[EXAMPLE NAME]\n")); MosPrintf(MIL_TEXT("Simple3dStiching\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 two partial point clouds of a 3D object \n")); MosPrintf(MIL_TEXT("and stitch them into a single complete point cloud.\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")); } // 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 DrawExpectedOverlap3dBoxes(MIL_ID MilSystem, MIL_ID MilDepthMap[], MIL_ID MilDisplay[]); void DrawResultOverlap3dBoxes(MIL_ID MilSystem, 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[], MIL_INT BoxColor); void GenerateUnorganizedPointClouds(MIL_ID MilPointCloud[]); bool CheckForRequiredMILFile(MIL_CONST_TEXT_PTR FileName); // Enumerators definitions. enum { eReference = 0, eTarget, eStitched, ePrealigned, e3dDispReference, e3dDispAlignedTarget, eNbDepthMap}; enum StitchMethodEnum { eOverlapRegionAlignOnly = 0, eOverlapFullAlignOnly, eBothAlign }; // The number of point clouds. static const MIL_INT NB_POINT_CLOUD = 2; // 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 = 525; static const MIL_INT DEPTH_MAP_SIZE_Y = 400; // Extraction box definitions. static const MIL_DOUBLE EXTRACTION_BOX_SIZE_X = 220.0; static const MIL_DOUBLE EXTRACTION_BOX_SIZE_Y = 200.0; static const MIL_DOUBLE EXTRACTION_BOX_SIZE_Z = -66; // Expected target location. static const MIL_DOUBLE BOX_OVERLAP = 0.20; static const MIL_DOUBLE BOX_USED_OVERLAP = 0.9 * BOX_OVERLAP; static const MIL_DOUBLE EXP_TX = 0.0; static const MIL_DOUBLE EXP_TY = -15.0; static const MIL_DOUBLE EXP_TZ = 0.0; static const MIL_DOUBLE EXP_RX = 0.0; static const MIL_DOUBLE EXP_RY = 0.0; static const MIL_DOUBLE EXP_RZ = 6.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; // % static const MIL_INT MAX_ITERATIONS = 100; static const MIL_DOUBLE ALIGN_RMS_ERROR_RELATIVE_THRESHOLD = 0.5; // % static const MIL_INT ALIGN_ERROR_MINIMIZATION_METRIC = M_POINT_TO_POINT; static StitchMethodEnum STITCH_ALIGN_METHOD = eBothAlign; // 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_SIZE_X/2; static const MIL_DOUBLE DRAW_BOX_MIN_Y = -EXTRACTION_BOX_SIZE_Y/2 * BOX_USED_OVERLAP; static const MIL_DOUBLE DRAW_BOX_MIN_Z = EXTRACTION_BOX_SIZE_Z/2; static const MIL_DOUBLE DRAW_BOX_MAX_X = EXTRACTION_BOX_SIZE_X/2; static const MIL_DOUBLE DRAW_BOX_MAX_Y = EXTRACTION_BOX_SIZE_Y/2 * BOX_USED_OVERLAP; static const MIL_DOUBLE DRAW_BOX_MAX_Z = -EXTRACTION_BOX_SIZE_Z/2; // Displays constants. static const MIL_INT WINDOWS_OFFSET_X = 15; static const MIL_INT NB_DISPLAY = 3; static MIL_CONST_TEXT_PTR DISPLAY_NAMES[NB_DISPLAY] = { MIL_TEXT("Reference partial point"), MIL_TEXT("Target partial point"), MIL_TEXT("Stitched point cloud") }; static const MIL_INT DISP_3D_SIZE_X = 906; static const MIL_INT DISP_3D_SIZE_Y = 680; static const MIL_DOUBLE DISP_3D_LOOK_AT_X = 0.0; static const MIL_DOUBLE DISP_3D_LOOK_AT_Y = 0.0; static const MIL_DOUBLE DISP_3D_LOOK_AT_Z = 0.0; static const MIL_DOUBLE DISP_3D_EYE_DIST = 263.28; static const MIL_DOUBLE DISP_3D_EYE_THETA = 59; static const MIL_DOUBLE DISP_3D_EYE_PHI = 36.61; // Point clouds generation information. // Input data files. static const MIL_TEXT_CHAR* const FILE_SOURCE_POINT_CLOUD[2] = { M_IMAGE_PATH MIL_TEXT("/Simple3dStitching/StitchReference.ply"), M_IMAGE_PATH MIL_TEXT("/Simple3dStitching/StitchTarget.ply") }; //***************************************************************************** // Main. //***************************************************************************** int MosMain(void) { // Print example information in console. PrintHeader(); // Allocate MIL objects. MIL_ID MilApplication = MappAlloc(M_NULL, M_DEFAULT, M_NULL); //Check for required example files. if (!CheckForRequiredMILFile(FILE_SOURCE_POINT_CLOUD[0])) { MappFree(MilApplication); return -1; } MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n")); MosGetch(); MIL_ID MilSystem = MsysAlloc(M_DEFAULT, M_SYSTEM_HOST, M_DEFAULT, M_DEFAULT, &MilSystem); //------------------------------------------------------------------------------------------- // Create the unorganized point cloud. // Allocate 3D point cloud containers. MIL_ID MilPointCloud[NB_POINT_CLOUD]; M3dmapAllocResult(MilSystem, M_POINT_CLOUD_CONTAINER, M_DEFAULT, &MilPointCloud[eReference]); M3dmapAllocResult(MilSystem, M_POINT_CLOUD_CONTAINER, M_DEFAULT, &MilPointCloud[eTarget]); MgraFont(M_DEFAULT, M_FONT_DEFAULT_SMALL); // Generate the unorganized point cloud. MosPrintf(MIL_TEXT("The reference and target point clouds are being generated...")); GenerateUnorganizedPointClouds(MilPointCloud); MosPrintf(MIL_TEXT("done.\n\n")); //------------------------------------------------------------------------------- // Initialize displays that will show the two partial depth maps and the stitched depth map. // Jet LUT. MIL_ID MilLut = MbufAllocColor(MilSystem, 3, NUM_DEPTHMAP_VALUES, 1, 8 + M_UNSIGNED, M_LUT, M_NULL); AddLutColorForInvalidDepth(MilLut, DEPTHMAP_INVALID_COLOR); // LUT legend. MIL_ID MilLutLegend = CreateLutLegend(MilSystem); // Initialize displays. MIL_ID MilDisplay[NB_DISPLAY]; for (MIL_INT d = 0; d < NB_DISPLAY; d++) { // Allocate the display. MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, &MilDisplay[d]); // Associate the jet LUT MdispLut(MilDisplay[d], MilLut); // Some display controls MdispControl(MilDisplay[d], M_OVERLAY, M_ENABLE); MdispControl(MilDisplay[d], M_WINDOW_INITIAL_POSITION_X, (MIL_INT)(d*(WINDOWS_OFFSET_X + DEPTH_MAP_SIZE_X))); MdispControl(MilDisplay[d], M_UPDATE, M_DISABLE); // Add titles to displays. MdispControl(MilDisplay[d], M_TITLE, M_PTR_TO_DOUBLE(DISPLAY_NAMES[d])); } //-------------------------------------------------------------------------- // Fully corrected depth maps from partial point cloud containers. // Generate fully corrected depth maps before 3d alignment. MIL_ID MilDepthMap[eNbDepthMap]; for(MIL_INT d = 0; d < eNbDepthMap; d++) MbufAlloc2d(MilSystem, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, 16 + M_UNSIGNED, M_IMAGE + M_PROC + M_DISP, &MilDepthMap[d]); for (MIL_INT p = 0; p < NB_POINT_CLOUD; p++) { // Point cloud containers controls for depth map extraction. M3dmapControl(MilPointCloud[p], M_DEFAULT, M_EXTRACTION_OVERLAP , M_MAX); M3dmapControl(MilPointCloud[p], M_DEFAULT, M_EXTRACTION_SATURATION, M_DISABLE); // Set the box to extract the whole object. M3dmapSetBox(MilPointCloud[p], M_EXTRACTION_BOX, M_CENTER_AND_DIMENSION, 0.0, 0.0, 0.0, EXTRACTION_BOX_SIZE_X, EXTRACTION_BOX_SIZE_Y, EXTRACTION_BOX_SIZE_Z); // Fully corrected depth maps extraction. M3dmapExtract(MilPointCloud[p], MilDepthMap[p], M_NULL, M_CORRECTED_DEPTH_MAP, M_ALL, M_DEFAULT); // Display the depth map. MdispSelect(MilDisplay[p], MilDepthMap[p]); DrawLutLegend(MilDepthMap[p], MilLutLegend); MdispControl(MilDisplay[p], M_UPDATE, M_NOW); } // Get the total number of points of the reference point cloud. MIL_INT ReferenceTotalNbPoints; M3dmapGet(MilPointCloud[eReference], M_ALL, M_POSITION, M_INCLUDE_POINTS_INSIDE_BOX_ONLY, 64 + M_FLOAT, M_NULL, M_NULL, M_NULL, M_NULL, &ReferenceTotalNbPoints); // Get the number of points of the reference point cloud expected overlap. MIL_INT ReferenceOverlapNbOfPoints; M3dmapSetBox(MilPointCloud[eReference], M_DEFAULT, M_CENTER_AND_DIMENSION, 0.0, 0.0, 0.0, EXTRACTION_BOX_SIZE_X, EXTRACTION_BOX_SIZE_Y* BOX_OVERLAP, EXTRACTION_BOX_SIZE_Z); M3dmapGet(MilPointCloud[eReference], M_ALL, M_POSITION, M_INCLUDE_POINTS_INSIDE_BOX_ONLY, 64 + M_FLOAT, M_NULL, M_NULL, M_NULL, M_NULL, &ReferenceOverlapNbOfPoints); //-------------------------------------------------------------------------- // 3D alignment. // Set the relative coordinate system of the target point cloud at the expected location with regards to the // reference point cloud. McalSetCoordinateSystem(MilPointCloud[eTarget], M_RELATIVE_COORDINATE_SYSTEM, M_RELATIVE_COORDINATE_SYSTEM, M_TRANSLATION, M_NULL, EXP_TX, EXP_TY, EXP_TZ, M_DEFAULT); McalSetCoordinateSystem(MilPointCloud[eTarget], M_RELATIVE_COORDINATE_SYSTEM, M_RELATIVE_COORDINATE_SYSTEM, M_ROTATION_XYZ, M_NULL, EXP_RX, EXP_RY, EXP_RZ, M_DEFAULT); // Fully corrected depth maps extraction. M3dmapExtract(MilPointCloud[eTarget], MilDepthMap[ePrealigned], M_NULL, M_CORRECTED_DEPTH_MAP, M_ALL, M_DEFAULT); // Draw the overlap boxes. DrawExpectedOverlap3dBoxes(MilSystem, MilDepthMap, MilDisplay); MosPrintf(MIL_TEXT("The object's partial depth map, for both the reference and target, are \n") MIL_TEXT("displayed using pseudo colors. A rectangular box is also displayed to indicate\n") MIL_TEXT("the expected common overlap region for both partial point clouds.\n\n")); MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n")); MosGetch(); // 3D pairwise registration (align) context and result. MIL_ID MilAlignmentContext = M3dmapAlloc(MilSystem, M_PAIRWISE_ALIGNMENT_CONTEXT, M_DEFAULT, M_NULL); MIL_ID MilAlignmentResult = M3dmapAllocResult(MilSystem, M_ALIGNMENT_RESULT, M_DEFAULT, M_NULL); // 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_MAX_ITERATIONS , MAX_ITERATIONS); M3dmapControl(MilAlignmentContext, M_DEFAULT, M_ALIGN_RMS_ERROR_RELATIVE_THRESHOLD, ALIGN_RMS_ERROR_RELATIVE_THRESHOLD); M3dmapControl(MilAlignmentContext, M_DEFAULT, M_ERROR_MINIMIZATION_METRIC , ALIGN_ERROR_MINIMIZATION_METRIC); M3dmapControl(MilAlignmentContext, M_DEFAULT, M_EXTRACTION_BOX_MODEL , M_USE ); M3dmapControl(MilAlignmentContext, M_DEFAULT, M_EXTRACTION_BOX_SCENE , M_USE ); // Alignment. MIL_INT64 AlignStatus = M_NULL; MIL_DOUBLE AlignComputationTime = 0.0; MappTimer(M_TIMER_RESET, M_NULL); if(STITCH_ALIGN_METHOD == eOverlapRegionAlignOnly || STITCH_ALIGN_METHOD == eBothAlign) { // Set the box to the expected overlap region. for(MIL_INT p = 0; p < NB_POINT_CLOUD; p++) M3dmapSetBox(MilPointCloud[p], M_DEFAULT, M_CENTER_AND_DIMENSION, 0.0, 0.0, 0.0, EXTRACTION_BOX_SIZE_X, EXTRACTION_BOX_SIZE_Y* BOX_USED_OVERLAP, EXTRACTION_BOX_SIZE_Z); // Pre-align. M3dmapControl(MilAlignmentContext, M_DEFAULT, M_MODEL_OVERLAP, MODEL_OVERLAP); M3dmapAlign(MilAlignmentContext, MilPointCloud[eReference], M_ALL, MilPointCloud[eTarget], M_ALL, M_NULL, MilAlignmentResult, M_DEFAULT, &AlignStatus); } if(STITCH_ALIGN_METHOD == eOverlapFullAlignOnly || STITCH_ALIGN_METHOD == eBothAlign) { MIL_ID MilPrealignment = eBothAlign ? MilAlignmentResult : M_NULL; // Set the box to the full point clouds. for(MIL_INT p = 0; p < NB_POINT_CLOUD; p++) M3dmapSetBox(MilPointCloud[p], M_DEFAULT, M_CENTER_AND_DIMENSION, 0.0, 0.0, 0.0, EXTRACTION_BOX_SIZE_X, EXTRACTION_BOX_SIZE_Y, EXTRACTION_BOX_SIZE_Z); // Set the full model overlap based on the expected overlap between the two point clouds. MIL_DOUBLE FullModelOverlap = ((MIL_DOUBLE)ReferenceOverlapNbOfPoints / ReferenceTotalNbPoints)*MODEL_OVERLAP; M3dmapControl(MilAlignmentContext, M_DEFAULT, M_MODEL_OVERLAP, FullModelOverlap); // Align. M3dmapAlign(MilAlignmentContext, MilPointCloud[eReference], M_ALL, MilPointCloud[eTarget], M_ALL, MilPrealignment, MilAlignmentResult, M_DEFAULT, &AlignStatus); } MappTimer(M_TIMER_READ, &AlignComputationTime); MosPrintf(MIL_TEXT("The 3D stitching between the two partial point clouds has been performed with \n") MIL_TEXT("the help of the points within the expected common overlap regions.\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 overlapping.\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 the two partial point clouds\n") MIL_TEXT("succeeded in %.2f ms with a final RMS error of %f mm.\n\n"), AlignComputationTime*1000, AlignRmsError); break; default: MosPrintf(MIL_TEXT("Unknown alignment status.\n\n")); } // The object depth map buffer changed, so redraw the LUT legend. DrawLutLegend(MilDepthMap[eTarget], MilLutLegend); MdispControl(MilDisplay[eTarget], M_UPDATE, M_NOW); //-------------------------------------------------------------------------- // Fixturing and stitching // Use alignment result to fixture the target point cloud on the reference point cloud. McalFixture(MilPointCloud[eTarget], M_NULL, M_MOVE_RELATIVE, M_RESULT_ALIGNMENT_3DMAP, MilAlignmentResult, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT); // Get the data of the second point cloud and put it in the first point cloud container. MIL_INT NbPoints; M3dmapGet(MilPointCloud[eTarget], M_ALL, M_POSITION, M_EXCLUDE_INVALID_POINTS, 64 + M_FLOAT, M_NULL, M_NULL, M_NULL, M_NULL, &NbPoints); MIL_DOUBLE* X = new MIL_DOUBLE[NbPoints]; MIL_DOUBLE* Y = new MIL_DOUBLE[NbPoints]; MIL_DOUBLE* Z = new MIL_DOUBLE[NbPoints]; M3dmapGet(MilPointCloud[eTarget], M_ALL, M_POSITION, M_EXCLUDE_INVALID_POINTS, 64 + M_FLOAT, NbPoints, &X[0], &Y[0], &Z[0], M_NULL); M3dmapPut(MilPointCloud[eReference], M_POINT_CLOUD_LABEL(eTarget+1), M_POSITION, 64 + M_FLOAT, NbPoints, X, Y, Z, MilPointCloud[0], M_DEFAULT); delete [] X; delete [] Y; delete [] Z; // Extract the stitched depth map. M3dmapSetBox(MilPointCloud[eReference], M_EXTRACTION_BOX, M_CENTER_AND_DIMENSION, 0, 0, 0, EXTRACTION_BOX_SIZE_X, EXTRACTION_BOX_SIZE_Y, EXTRACTION_BOX_SIZE_Z); M3dmapExtract(MilPointCloud[eReference], MilDepthMap[eStitched], M_NULL, M_CORRECTED_DEPTH_MAP, M_ALL, M_DEFAULT); // Display stitched image. DrawLutLegend(MilDepthMap[eStitched], MilLutLegend); MdispSelect(MilDisplay[eStitched], MilDepthMap[eStitched]); // Draw a black 3D box in the stitched depth map to show the original overlap regions of the second partial point cloud. DrawResultOverlap3dBoxes(MilSystem, MilDepthMap, MilDisplay, MilPointCloud); MosPrintf(MIL_TEXT("The two point clouds have been stitched into a single point cloud. The depth\n") MIL_TEXT("map of the resulting stitched point cloud is displayed. A rectangular cube is\n") MIL_TEXT("also displayed to indicate the expected overlap region that has been\n") MIL_TEXT("transformed.\n\n")); MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n")); MosGetch(); #if ENABLE_DISPLAY_3D // Extract the depth maps for the 3d display. M3dmapExtract(MilPointCloud[eReference], MilDepthMap[e3dDispReference], M_NULL, M_CORRECTED_DEPTH_MAP, M_POINT_CLOUD_INDEX(eReference), M_DEFAULT); M3dmapExtract(MilPointCloud[eReference], MilDepthMap[e3dDispAlignedTarget], M_NULL, M_CORRECTED_DEPTH_MAP, M_POINT_CLOUD_INDEX(eTarget), M_DEFAULT); // Allocate the colored buffer for the 3d display. MIL_ID MilDepthMapsColors[2]; MbufAllocColor(MilSystem, 3, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, 8+M_UNSIGNED, M_IMAGE+M_PROC, &MilDepthMapsColors[0]); MbufClear(MilDepthMapsColors[0], M_COLOR_BLUE); MbufAllocColor(MilSystem, 3, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, 8+M_UNSIGNED, M_IMAGE+M_PROC, &MilDepthMapsColors[1]); MbufClear(MilDepthMapsColors[1], M_COLOR_RED); // Allocate the 3d display. MIL_DISP_D3D_HANDLE Mil3dDisplayHandle = MdepthSysD3DAlloc(M_NULL, M_NULL, M_NULL, M_NULL, &MilDepthMap[e3dDispReference], MilDepthMapsColors, 2, DISP_3D_SIZE_X, DISP_3D_SIZE_Y, M_DEFAULT, M_NULL); // Setup the display and show it. MdispD3DControl(Mil3dDisplayHandle, MD3D_LOOK_AT_X, DISP_3D_LOOK_AT_X); MdispD3DControl(Mil3dDisplayHandle, MD3D_LOOK_AT_Y, DISP_3D_LOOK_AT_Y); MdispD3DControl(Mil3dDisplayHandle, MD3D_LOOK_AT_Z, DISP_3D_LOOK_AT_Z); MdispD3DControl(Mil3dDisplayHandle, MD3D_EYE_DIST, DISP_3D_EYE_DIST); MdispD3DControl(Mil3dDisplayHandle, MD3D_EYE_THETA, DISP_3D_EYE_THETA); MdispD3DControl(Mil3dDisplayHandle, MD3D_EYE_PHI, DISP_3D_EYE_PHI); MdispD3DControl(Mil3dDisplayHandle, MD3D_POINT, MD3D_ENABLE); MdispD3DControl(Mil3dDisplayHandle, MD3D_ROTATE, MD3D_TRUE); MdispD3DShow(Mil3dDisplayHandle); MosPrintf(MIL_TEXT("The aligned point clouds, as contained in the resulting stitched point cloud,\n") MIL_TEXT("are displayed in the 3d display:\n") MIL_TEXT(" - The depth map of the reference point cloud is in blue.\n") MIL_TEXT(" - The depth map of the aligned target point cloud is in red.\n\n") MIL_TEXT("Press <Enter> to end.\n\n")); MosGetch(); MbufFree(MilDepthMapsColors[0]); MbufFree(MilDepthMapsColors[1]); MdispD3DFree(Mil3dDisplayHandle); #endif //-------------------------------------------------------------------------- // Free MIL objects. M3dmapFree(MilAlignmentContext); M3dmapFree(MilAlignmentResult); for (MIL_INT d = 0; d < NB_DISPLAY; d++) MdispFree(MilDisplay[d]); for (MIL_INT d = 0; d < eNbDepthMap; d++) MbufFree(MilDepthMap[d]); for (MIL_INT p = 0; p < NB_POINT_CLOUD; p++) M3dmapFree(MilPointCloud[p]); MbufFree(MilLutLegend); 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 overlap. //***************************************************************************** void DrawExpectedOverlap3dBoxes(MIL_ID MilSystem, MIL_ID MilDepthMap[], MIL_ID MilDisplay[]) { // Draw the expected overlap in the reference depth map. MIL_DOUBLE OverlapBoxPtsInAbsX[NUM_BOX_POINTS]; MIL_DOUBLE OverlapBoxPtsInAbsY[NUM_BOX_POINTS]; MIL_DOUBLE OverlapBoxPtsInAbsZ[NUM_BOX_POINTS]; GetBoxPoints(MilDepthMap[eReference], OverlapBoxPtsInAbsX, OverlapBoxPtsInAbsY, OverlapBoxPtsInAbsZ); DrawBoxInOneDepthMap(MilSystem, MilDisplay[eReference], MilDepthMap[eReference], OverlapBoxPtsInAbsX, OverlapBoxPtsInAbsY, OverlapBoxPtsInAbsZ, M_COLOR_BLACK); MdispControl(MilDisplay[eReference], M_UPDATE, M_NOW); // Draw the expected overlap in the target depth map. GetBoxPoints(MilDepthMap[ePrealigned], OverlapBoxPtsInAbsX, OverlapBoxPtsInAbsY, OverlapBoxPtsInAbsZ); DrawBoxInOneDepthMap(MilSystem, MilDisplay[eTarget], MilDepthMap[eTarget], OverlapBoxPtsInAbsX, OverlapBoxPtsInAbsY, OverlapBoxPtsInAbsZ, M_COLOR_MAGENTA); MdispControl(MilDisplay[eTarget], M_UPDATE, M_NOW); } //***************************************************************************** // Draw a black 3d box in the stitched depth map to illustrate the overlap. //***************************************************************************** void DrawResultOverlap3dBoxes(MIL_ID MilSystem, MIL_ID MilDepthMap[], MIL_ID MilDisplay[], MIL_ID MilPointCloud[]) { MIL_DOUBLE OverlapBoxPtsInAbsX[NUM_BOX_POINTS]; MIL_DOUBLE OverlapBoxPtsInAbsY[NUM_BOX_POINTS]; MIL_DOUBLE OverlapBoxPtsInAbsZ[NUM_BOX_POINTS]; // Draw the expected overlap in the target depth map. GetBoxPoints(MilDepthMap[ePrealigned], OverlapBoxPtsInAbsX, OverlapBoxPtsInAbsY, OverlapBoxPtsInAbsZ); M3dmapExtract(MilPointCloud[eTarget], MilDepthMap[eTarget], M_NULL, M_CORRECTED_DEPTH_MAP, M_ALL, M_DEFAULT); McalTransformCoordinate3dList(MilDepthMap[eTarget], M_ABSOLUTE_COORDINATE_SYSTEM, M_RELATIVE_COORDINATE_SYSTEM, 24, OverlapBoxPtsInAbsX, OverlapBoxPtsInAbsY, OverlapBoxPtsInAbsZ, OverlapBoxPtsInAbsX, OverlapBoxPtsInAbsY, OverlapBoxPtsInAbsZ, M_DEFAULT); McalTransformCoordinate3dList(MilDepthMap[eStitched], M_RELATIVE_COORDINATE_SYSTEM, M_ABSOLUTE_COORDINATE_SYSTEM, 24, OverlapBoxPtsInAbsX, OverlapBoxPtsInAbsY, OverlapBoxPtsInAbsZ, OverlapBoxPtsInAbsX, OverlapBoxPtsInAbsY, OverlapBoxPtsInAbsZ, M_DEFAULT); DrawBoxInOneDepthMap(MilSystem, MilDisplay[eStitched], MilDepthMap[eStitched], OverlapBoxPtsInAbsX, OverlapBoxPtsInAbsY, OverlapBoxPtsInAbsZ, M_COLOR_MAGENTA); MdispControl(MilDisplay[eStitched], M_UPDATE, M_NOW); } //***************************************************************************** // Get list of 3d points, expressed in absolute coordinates, corresponding to // the 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 the relative coordinate system. 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 BoxColor) { MIL_INT NbLines = 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 + NbLines; MIL_DOUBLE* BoxPixelEndY = BoxPixelsInFixtureY + NbLines; // 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, NbLines - 4, &BoxPixelStartX[4], &BoxPixelStartY[4], &BoxPixelEndX[4], &BoxPixelEndY[4], M_DEFAULT); // Draw, in the depth map overlay of the display, the box created. MdispControl(MilDisplay, M_OVERLAY_CLEAR, M_DEFAULT); MIL_ID MilOverlay = MdispInquire(MilDisplay, M_OVERLAY_ID, M_NULL); MbufClearCond(MilOverlay, M_RGB888_R(BoxColor), M_RGB888_G(BoxColor), M_RGB888_B(BoxColor), 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); } //***************************************************************************** // Generates the point clouds for the example. //***************************************************************************** void GenerateUnorganizedPointClouds(MIL_ID MilPointCloud[]) { MIL_ID MilSystem = M3dmapInquire(MilPointCloud[eReference], M_GENERAL, M_OWNER_SYSTEM, M_NULL); MIL_ID MilSourcePointCloud[2]; M3dmapAllocResult(MilSystem, M_POINT_CLOUD_CONTAINER, M_DEFAULT, &MilSourcePointCloud[0]); M3dmapAllocResult(MilSystem, M_POINT_CLOUD_CONTAINER, M_DEFAULT, &MilSourcePointCloud[1]); // Import a point cloud from a PLY file. M3dmapImport(FILE_SOURCE_POINT_CLOUD[0], M_PLY, MilSourcePointCloud[eReference], M_POINT_CLOUD_LABEL(1), M_NULL, M_DEFAULT); M3dmapImport(FILE_SOURCE_POINT_CLOUD[1], M_PLY, MilSourcePointCloud[eTarget], M_POINT_CLOUD_LABEL(2), M_NULL, M_DEFAULT); // Crop the two point clouds. const MIL_DOUBLE BoxCropRatio = 0.5 + BOX_OVERLAP / 2.0; for(MIL_INT p = 0; p < 2; p++) { MIL_DOUBLE CropExtractionBoxMinY = -EXTRACTION_BOX_SIZE_Y/2; if(p == 1) CropExtractionBoxMinY += EXP_TY + EXTRACTION_BOX_SIZE_Y * (BoxCropRatio - BOX_OVERLAP); M3dmapSetBox(MilSourcePointCloud[p], M_DEFAULT, M_CORNER_AND_DIMENSION, -EXTRACTION_BOX_SIZE_X/2, CropExtractionBoxMinY, -EXTRACTION_BOX_SIZE_Z/2, EXTRACTION_BOX_SIZE_X, EXTRACTION_BOX_SIZE_Y* BoxCropRatio, EXTRACTION_BOX_SIZE_Z); // Get the points. MIL_INT NbPoints; M3dmapGet(MilSourcePointCloud[p], M_ALL, M_POSITION, M_INCLUDE_POINTS_INSIDE_BOX_ONLY, 64 + M_FLOAT, 0, NULL, NULL, NULL, &NbPoints); MIL_DOUBLE* X = new MIL_DOUBLE[NbPoints]; MIL_DOUBLE* Y = new MIL_DOUBLE[NbPoints]; MIL_DOUBLE* Z = new MIL_DOUBLE[NbPoints]; M3dmapGet(MilSourcePointCloud[p], M_ALL, M_POSITION, M_INCLUDE_POINTS_INSIDE_BOX_ONLY, 64 + M_FLOAT, NbPoints, X, Y, Z, M_NULL); // Put the point. M3dmapPut(MilPointCloud[p], M_POINT_CLOUD_LABEL(1), M_POSITION, 64 + M_FLOAT, NbPoints, X, Y, Z, MilSourcePointCloud[p], M_DEFAULT); delete [] X; delete [] Y; delete [] Z; } M3dmapFree(MilSourcePointCloud[eTarget]); M3dmapFree(MilSourcePointCloud[eReference]); } //**************************************************************************** // 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); }