| Click here to show toolbars of the Web Online Help System: show toolbars |
//*************************************************************************************** // // File name: 3dPlanarity.cpp // Location: See Matrox Example Launcher in the MIL Control Center // // // Synopsis: Demonstrates object planarity measurements of a // scanned 3d mechanical part. // // Copyright (C) Matrox Electronic Systems Ltd., 1992-2016. // All Rights Reserved #include <mil.h> #include <math.h> #include <vector> using std::vector; // 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 //**************************************************************************** // Example description. //**************************************************************************** void PrintHeader() { MosPrintf(MIL_TEXT("[EXAMPLE NAME]\n")); MosPrintf(MIL_TEXT("3dPlanarity\n\n")); MosPrintf(MIL_TEXT("[SYNOPSIS]\n")); MosPrintf(MIL_TEXT("This example demonstrates how to perform planarity measurements\n") MIL_TEXT("on a 3d reconstruction of a mechanical part.")); MosPrintf(MIL_TEXT("\n\n")); MosPrintf(MIL_TEXT("[MODULES USED]\n")); MosPrintf(MIL_TEXT("Modules used: 3d reconstruction, application, buffer, calibration,\n") MIL_TEXT("display, graphic, image processing, model finder, system.\n\n")); } //**************************************************************************** // Constants. //**************************************************************************** static const MIL_DOUBLE DIV_180_PI = 57.295779513082320866997945294156; static const MIL_INT MAP_SIZE_X = 515; // in pixels static const MIL_INT MAP_SIZE_Y = 880; // in pixels static const MIL_DOUBLE PLANE_FIT_CENTER_X = 47.88; // in mm static const MIL_DOUBLE PLANE_FIT_CENTER_Y = 39.29; // in mm static const MIL_DOUBLE PLANE_FIT_RADIUS = 23; // in mm // Offsets of the windows. static const MIL_INT WINDOWS_OFFSET_X = 15; static const MIL_INT WINDOWS_OFFSET_Y = 38; //**************************************************************************** // 3d display. //**************************************************************************** #if ENABLE_DISPLAY_3D #include <MdispD3D.h> // 3D display parameters. static const MIL_INT DISPLAY_3D_SIZE_X = 800; // in pixels static const MIL_INT DISPLAY_3D_SIZE_Y = 600; // in pixels static const MIL_DOUBLE DISPLAY_3D_VOLUME = MD3D_TRANSPARENT; static const MIL_DOUBLE DISPLAY_3D_LOOK_AT_X = 66.53; // in mm static const MIL_DOUBLE DISPLAY_3D_LOOK_AT_Y = 268.84; // in mm static const MIL_DOUBLE DISPLAY_3D_LOOK_AT_Z = 38.96; // in mm static const MIL_DOUBLE DISPLAY_3D_EYE_DIST = 60.40; // in mm static const MIL_DOUBLE DISPLAY_3D_EYE_THETA = -107.68; // in deg static const MIL_DOUBLE DISPLAY_3D_EYE_PHI = 63.20; // in deg // Window offset parameter. static const MIL_INT ILLUSTRATION_DISPLAY_OFFSET_X = DISPLAY_3D_SIZE_X; #else static const MIL_INT ILLUSTRATION_DISPLAY_OFFSET_X = MAP_SIZE_X; #endif //**************************************************************************** // File names. //**************************************************************************** #define DATA_EX_PATH(x) (M_IMAGE_PATH MIL_TEXT("MechanicalPartScan/") MIL_TEXT(x)) static MIL_CONST_TEXT_PTR METAL_PART_CLOUD_CONTAINER = DATA_EX_PATH("MetalPartCloudContainer.m3d"); #define EX_PATH(x) (M_IMAGE_PATH MIL_TEXT("3dPlanarity/") MIL_TEXT(x)) static MIL_CONST_TEXT_PTR MEASURES_ILLUSTRATIONS[5] = { EX_PATH("DepthVerticalIllustration.mim"), EX_PATH("DepthPlaneVerticalIllustration.mim"), EX_PATH("DepthPlaneNormalIllustration.mim"), EX_PATH("PointPlaneWorldCylinder.mim"), EX_PATH("PointPlaneRaster.mim") }; //**************************************************************************** // Useful structures. //**************************************************************************** struct SMapGeneration { MIL_DOUBLE BoxCornerX; MIL_DOUBLE BoxCornerY; MIL_DOUBLE BoxCornerZ; MIL_DOUBLE BoxSizeX; MIL_DOUBLE BoxSizeY; MIL_DOUBLE BoxSizeZ; MIL_INT MapSizeX; MIL_INT MapSizeY; MIL_INT IntensityMapType; bool SetExtractOverlap; MIL_DOUBLE ExtractOverlap; MIL_DOUBLE FillXThreshold; MIL_DOUBLE FillYThreshold; }; struct SPlanarityMeasure { SPlanarityMeasure(MIL_CONST_TEXT_PTR Name) : MeasureName(Name){}; MIL_CONST_TEXT_PTR MeasureName; MIL_DOUBLE Planarity; }; //**************************************************************************** // Function prototypes. //**************************************************************************** void AnalyzePlanarity(MIL_ID MilSystem, MIL_ID PointCloudContainer); void GenerateDepthMap(MIL_ID MilSystem, MIL_ID PointCloudContainer, const SMapGeneration& GenerationInfo, MIL_ID& OutDepthmap); bool FixturePart(MIL_ID MilSystem, MIL_ID MilDepthMap, MIL_ID MilFixtureDestination, MIL_ID MilGraList); void PrintResultTable(const vector<SPlanarityMeasure>& rPlanarityMeasures); bool CheckForRequiredMILFile(MIL_CONST_TEXT_PTR FileName); //***************************************************************************** // Main. //***************************************************************************** int MosMain(void) { PrintHeader(); // Allocate the MIL application. MIL_ID MilApplication = MappAlloc(M_NULL, M_DEFAULT, M_NULL); // Check for required example files. if (!CheckForRequiredMILFile(METAL_PART_CLOUD_CONTAINER)) { MappFree(MilApplication); return -1; } // Allocate a host system. MIL_ID MilSystem = MsysAlloc(M_DEFAULT, M_SYSTEM_HOST, M_DEFAULT, M_DEFAULT, M_NULL); MosPrintf(MIL_TEXT("Press <Enter> to start.\n\n")); MosGetch(); // Restore a 3d point cloud of the object. MIL_ID PointCloudContainer = M_NULL; M3dmapRestore(METAL_PART_CLOUD_CONTAINER, MilSystem, M_DEFAULT, &PointCloudContainer); // Analyze. AnalyzePlanarity(MilSystem, PointCloudContainer); // Free the MIL 3d point cloud. M3dmapFree(PointCloudContainer); // Free the MIL system and application. MsysFree(MilSystem); MappFree(MilApplication); return 0; } //******************************************************************************* // AnalyzePlanarity. Planarity analysis of the the scanned object. //******************************************************************************* void AnalyzePlanarity(MIL_ID MilSystem, MIL_ID MilPointCloudContainer) { // Allocates the displays and graphic lists MIL_ID MilIllustrationDisplay = MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, M_NULL); MdispControl(MilIllustrationDisplay, M_WINDOW_INITIAL_POSITION_X, ILLUSTRATION_DISPLAY_OFFSET_X + WINDOWS_OFFSET_X); MIL_ID MilDisplayDepthMap = MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, M_NULL); MIL_ID MilDepthMapGraphicList = MgraAllocList(MilSystem, M_DEFAULT, M_NULL); MdispControl(MilDisplayDepthMap, M_ASSOCIATED_GRAPHIC_LIST_ID, MilDepthMapGraphicList); MdispControl(MilDisplayDepthMap, M_VIEW_MODE, M_AUTO_SCALE); // Set the volume information. SMapGeneration MapData; MapData.BoxCornerX = 5.00; MapData.BoxCornerY = 0; MapData.BoxCornerZ = -4.00; MapData.BoxSizeX = 120.00; MapData.BoxSizeY = 250.00; MapData.BoxSizeZ = -30.00; MapData.MapSizeX = MAP_SIZE_X; MapData.MapSizeY = MAP_SIZE_Y; MapData.SetExtractOverlap = true; MapData.ExtractOverlap = M_MIN; MapData.FillXThreshold = 1.0; MapData.FillYThreshold = 1.0; // Generate the top view calibrated depth map of the mechanical part. MIL_ID MilDepthMap = M_NULL; GenerateDepthMap(MilSystem, MilPointCloudContainer, MapData, MilDepthMap); // Allocate the necessary buffers for processing and display. MIL_ID MilPlaneFitMask = MbufAlloc2d(MilSystem, MAP_SIZE_X, MAP_SIZE_Y, 8, M_IMAGE + M_PROC, M_NULL); // Allocate an image that will hold the calibration for the coordinates transformations. MIL_ID MilCalHolder = MbufAlloc2d(MilSystem, 2, 2, 16 + M_UNSIGNED, M_IMAGE + M_PROC, M_NULL); // Allocate the plane geometry. MIL_ID MilPlaneGeometry = M3dmapAlloc(MilSystem, M_GEOMETRY, M_DEFAULT, M_NULL); // Display the depth map and its calibration. MgraClear(M_DEFAULT, MilDepthMapGraphicList); MgraColor(M_DEFAULT, M_COLOR_LIGHT_BLUE); MgraBackColor(M_DEFAULT, M_COLOR_GRAY); McalDraw(M_DEFAULT, MilDepthMap, MilDepthMapGraphicList, M_DRAW_RELATIVE_COORDINATE_SYSTEM, M_DEFAULT, M_DEFAULT); MdispSelect(MilDisplayDepthMap, MilDepthMap); MosPrintf(MIL_TEXT("A top view calibrated depth map of the mechanical part was generated.\n\n") MIL_TEXT("Press <Enter> to continue.\n\n")); MosGetch(); // Locate the part and move the relative coordinate system accordingly. bool PartFound = FixturePart(MilSystem, MilDepthMap, MilDepthMap, MilDepthMapGraphicList); if (PartFound) { // Move the relative to the plane location. McalFixture(MilDepthMap, M_NULL, M_MOVE_RELATIVE, M_POINT_AND_ANGLE, M_DEFAULT, PLANE_FIT_CENTER_X, PLANE_FIT_CENTER_Y, 0.0, M_DEFAULT); MgraColor(M_DEFAULT, M_COLOR_DARK_BLUE); McalDraw(M_DEFAULT, MilDepthMap, MilDepthMapGraphicList, M_DRAW_RELATIVE_COORDINATE_SYSTEM, M_DEFAULT, M_DEFAULT); MosPrintf(MIL_TEXT("The part was located and fixtured using Model Finder in the depth map.\n\n") MIL_TEXT("Press <Enter> to continue.\n\n")); MosGetch(); // Disable the display updates. MdispControl(MilDisplayDepthMap, M_UPDATE, M_DISABLE); // Create a mask where we want to measure the planarity. MbufClear(MilPlaneFitMask, 0); McalAssociate(MilDepthMap, MilPlaneFitMask, M_DEFAULT); MgraControl(M_DEFAULT, M_INPUT_UNITS, M_WORLD); MgraColor(M_DEFAULT, 255); MgraArcFill(M_DEFAULT, MilPlaneFitMask, 0.0, 0.0, PLANE_FIT_RADIUS, PLANE_FIT_RADIUS, 0, 360); MgraColor(M_DEFAULT, M_COLOR_MAGENTA); MgraArcFill(M_DEFAULT, MilDepthMapGraphicList, 0.0, 0.0, PLANE_FIT_RADIUS, PLANE_FIT_RADIUS, 0, 360); MgraControl(M_DEFAULT, M_INPUT_UNITS, M_PIXEL); // Show the region where we want to measure the planarity. MosPrintf(MIL_TEXT("The planarity of the depth data in the magenta region will be evaluated.\n\n") MIL_TEXT("Press <Enter> to continue.\n\n")); MdispControl(MilDisplayDepthMap, M_UPDATE, M_ENABLE); MosGetch(); // Disable the updates and remove the fill of the graphic that represents the region. MdispControl(MilDisplayDepthMap, M_UPDATE, M_DISABLE); MgraControlList(MilDepthMapGraphicList, M_GRAPHIC_INDEX(3), M_DEFAULT, M_FILLED, M_FALSE); // Allocate an empty point cloud. MIL_DOUBLE InvalidVal = M_INVALID_POINT; M3dmapPut(MilPointCloudContainer, M_POINT_CLOUD_LABEL(3), M_POSITION, 64 + M_FLOAT, 1, &InvalidVal, &InvalidVal, &InvalidVal, M_NULL, M_DEFAULT); // Measure the vertical planarity. Get the depth map minimum and maximum Z value // in the mask. MIL_DOUBLE MinZ; MIL_DOUBLE MaxZ; SPlanarityMeasure VerticalDepthMeasure(MIL_TEXT("Depth map with horizonal plane")); M3dmapSetGeometry(MilPlaneGeometry, M_HORIZONTAL_PLANE, M_PARAMETRIC, MapData.BoxCornerZ, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT); M3dmapStat(MilDepthMap, MilPlaneGeometry, MilPlaneFitMask, M_NULL, M_DEVIATION_MAX, M_INFINITE, M_DEFAULT, &VerticalDepthMeasure.Planarity); MinZ = MapData.BoxCornerZ - VerticalDepthMeasure.Planarity; M3dmapSetGeometry(MilPlaneGeometry, M_HORIZONTAL_PLANE, M_PARAMETRIC, MinZ, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT); M3dmapStat(MilDepthMap, MilPlaneGeometry, MilPlaneFitMask, M_NULL, M_DEVIATION_MAX, M_INFINITE, M_DEFAULT, &VerticalDepthMeasure.Planarity); // Re-extract the depth map for display purposes. The resulting depth map only contains data // in the vertical planarity region. MapData.BoxCornerZ = MinZ; MapData.BoxSizeZ = VerticalDepthMeasure.Planarity; GenerateDepthMap(MilSystem, MilPointCloudContainer, MapData, MilDepthMap); // Put back the fixture found previously. McalFixture(MilDepthMap, M_NULL, M_MOVE_RELATIVE, M_SAME_AS_SOURCE, MilPlaneFitMask, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT); // Clear the depth map data. Put all depth map values outside the mask to invalid data. MbufClearCond(MilDepthMap, MIL_UINT16_MAX, 0, 0, MilPlaneFitMask, M_EQUAL, 0.0); // Print message. MosPrintf(MIL_TEXT("The depth data used to measure the planarity is displayed.\n\n") MIL_TEXT("Press <Enter> to continue.\n\n")); MdispControl(MilDisplayDepthMap, M_UPDATE, M_ENABLE); MosGetch(); // Move the relative coordinate system of the point cloud at the fixtured position. // and extract an empty point cloud to propagate the calibration. McalFixture(MilPointCloudContainer, M_NULL, M_MOVE_RELATIVE, M_SAME_AS_SOURCE, MilDepthMap, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT); M3dmapExtract(MilPointCloudContainer, MilCalHolder, M_NULL, M_CORRECTED_DEPTH_MAP, M_POINT_CLOUD_LABEL(3), M_DEFAULT); #if ENABLE_DISPLAY_3D // Display a 3d display where a depth map represents the planarity measure. MIL_DISP_D3D_HANDLE MilDispD3D = MdepthSysD3DAlloc(M_NULL, M_NULL, M_NULL, M_NULL, &MilDepthMap, M_NULL, 1, DISPLAY_3D_SIZE_X, DISPLAY_3D_SIZE_Y, M_DEFAULT, M_NULL); // Set rendering parameters. MdispD3DControl(MilDispD3D, MD3D_VOLUME, DISPLAY_3D_VOLUME); // 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); MdispD3DShow(MilDispD3D); #endif // Show the vertical planarity measure. MIL_ID MilIllustrationImage = MbufRestore(MEASURES_ILLUSTRATIONS[0], MilSystem, M_NULL); MdispSelect(MilIllustrationDisplay, MilIllustrationImage); MosPrintf(MIL_TEXT("First, the planarity of the surface along the Z axis is measured.\n") MIL_TEXT("Since the analyzed plane is not coplanar to the XY plane, the planarity\n") MIL_TEXT("measure mostly reflects the plane's tilt.\n\n")); vector<SPlanarityMeasure> AllMeasures(1, VerticalDepthMeasure); PrintResultTable(AllMeasures); MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n")); MosGetch(); // Fit a plane on the data. M3dmapSetGeometry(MilPlaneGeometry, M_PLANE, M_FIT, (MIL_DOUBLE)MilDepthMap, (MIL_DOUBLE)MilPlaneFitMask, M_DEFAULT, M_DEFAULT, M_DEFAULT); // Get the parameters of the plane. MIL_DOUBLE Ax; MIL_DOUBLE Ay; MIL_DOUBLE Z0; M3dmapInquire(MilPlaneGeometry, M_DEFAULT, M_FIT_PARAM_AX, &Ax); M3dmapInquire(MilPlaneGeometry, M_DEFAULT, M_FIT_PARAM_AY, &Ay); M3dmapInquire(MilPlaneGeometry, M_DEFAULT, M_FIT_PARAM_Z0, &Z0); // Measure the planarity with regards to the fitted plane. MIL_DOUBLE NegativeMaxDeviation; MIL_DOUBLE PositiveMaxDeviation; M3dmapStat(MilDepthMap, MilPlaneGeometry, MilPlaneFitMask, M_NULL, M_DEVIATION_MAX + M_STAT_NEGATIVE, M_INFINITE, M_DEFAULT, &NegativeMaxDeviation); M3dmapStat(MilDepthMap, MilPlaneGeometry, MilPlaneFitMask, M_NULL, M_DEVIATION_MAX + M_STAT_POSITIVE, M_INFINITE, M_DEFAULT, &PositiveMaxDeviation); // Use the plane parameters to move the output coordinate system of the point cloud container. MIL_DOUBLE Norm = sqrt((Ax * Ax) + (Ay * Ay) + 1); MIL_DOUBLE AxisX = -Ay; MIL_DOUBLE AxisY = Ax; MIL_DOUBLE AxisZ = 0; MIL_DOUBLE CrossNorm = sqrt((AxisX * AxisX) + (AxisY * AxisY) + (AxisZ * AxisZ)); MIL_DOUBLE RotationAngleRad = asin(CrossNorm / Norm); MIL_DOUBLE RotationAngle = DIV_180_PI * RotationAngleRad; McalSetCoordinateSystem(MilPointCloudContainer, M_RELATIVE_COORDINATE_SYSTEM, M_RELATIVE_COORDINATE_SYSTEM, M_TRANSLATION + M_COMPOSE_WITH_CURRENT, M_NULL, 0.0, 0.0, Z0, M_DEFAULT); McalSetCoordinateSystem(MilPointCloudContainer, M_RELATIVE_COORDINATE_SYSTEM, M_RELATIVE_COORDINATE_SYSTEM, M_ROTATION_AXIS_ANGLE + M_COMPOSE_WITH_CURRENT, M_NULL, AxisX / CrossNorm, AxisY / CrossNorm, AxisZ / CrossNorm, -RotationAngle); SPlanarityMeasure PlaneDepthVerticalMeasure(MIL_TEXT("Depth map with fitted plane (vertical)")); PlaneDepthVerticalMeasure.Planarity = NegativeMaxDeviation + PositiveMaxDeviation;; NegativeMaxDeviation *= cos(RotationAngleRad); PositiveMaxDeviation *= cos(RotationAngleRad); SPlanarityMeasure PlaneDepthNormalMeasure(MIL_TEXT("Depth map with fitted plane (normal)")); PlaneDepthNormalMeasure.Planarity = NegativeMaxDeviation + PositiveMaxDeviation; // Show the depth map plane vertical planarity measures. MosPrintf(MIL_TEXT("By fitting a plane on the depth map data, the planarity of the surface\n") MIL_TEXT("along the Z axis can be measured.\n\n")); MbufLoad(MEASURES_ILLUSTRATIONS[1], MilIllustrationImage); AllMeasures.push_back(PlaneDepthVerticalMeasure); PrintResultTable(AllMeasures); MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n")); MosGetch(); // Show the depth map plane normal planarity measure. MosPrintf(MIL_TEXT("By calculating the tilt from the plane equation, the planarity along\n") MIL_TEXT("the fitted plane normal can be deduced.\n\n")); MbufLoad(MEASURES_ILLUSTRATIONS[2], MilIllustrationImage); AllMeasures.push_back(PlaneDepthNormalMeasure); PrintResultTable(AllMeasures); MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n")); MosGetch(); // Get the 3d points in the relative coordinate system. MIL_INT NbPoints; M3dmapGet(MilPointCloudContainer, M_POINT_CLOUD_INDEX(0), M_POSITION, M_EXCLUDE_INVALID_POINTS, 64 + M_FLOAT, M_NULL, M_NULL, M_NULL, M_NULL, &NbPoints); vector<MIL_DOUBLE> XRel(NbPoints); vector<MIL_DOUBLE> YRel(NbPoints); vector<MIL_DOUBLE> ZRel(NbPoints); M3dmapGet(MilPointCloudContainer, M_POINT_CLOUD_INDEX(0), M_POSITION, M_EXCLUDE_INVALID_POINTS, 64 + M_FLOAT, NbPoints, &XRel[0], &YRel[0], &ZRel[0], &NbPoints); // Get the minimum and maximum Z value of the points in the arc. MIL_DOUBLE PlaneRadiusSqr = PLANE_FIT_RADIUS * PLANE_FIT_RADIUS; MinZ = MIL_DOUBLE_MAX; MaxZ = -MIL_DOUBLE_MAX; for (MIL_INT p = 0; p < NbPoints; p++) { MIL_DOUBLE DistXYSqr = XRel[p] * XRel[p] + YRel[p] * YRel[p]; if (DistXYSqr < PlaneRadiusSqr) { if (ZRel[p] < MinZ) MinZ = ZRel[p]; else if (ZRel[p] > MaxZ) MaxZ = ZRel[p]; } } SPlanarityMeasure PlanePlanarityDiskMaskMeasure(MIL_TEXT("Point cloud with fitted plane ") MIL_TEXT("(world relative disk region)")); PlanePlanarityDiskMaskMeasure.Planarity = MaxZ - MinZ; // Show the point cloud with virtual cylindrical region measure. MosPrintf(MIL_TEXT("By fixturing the relative coordinate system on the fitted plane, it is\n") MIL_TEXT("possible to get and select the points of the point cloud in a virtual\n") MIL_TEXT("cylindrical region whose axis is aligned with the plane normal.\n") MIL_TEXT("The difference between the minimum and maximum fixtured Z value \n") MIL_TEXT("provides the planarity of the points of the surface's points.\n\n")); MbufLoad(MEASURES_ILLUSTRATIONS[3], MilIllustrationImage); AllMeasures.push_back(PlanePlanarityDiskMaskMeasure); PrintResultTable(AllMeasures); MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n")); MosGetch(); // Convert the points to the absolute. vector<MIL_DOUBLE> XAbs(NbPoints); vector<MIL_DOUBLE> YAbs(NbPoints); vector<MIL_DOUBLE> ZAbs(NbPoints); McalTransformCoordinate3dList(MilCalHolder, M_RELATIVE_COORDINATE_SYSTEM, M_ABSOLUTE_COORDINATE_SYSTEM, NbPoints, &XRel[0], &YRel[0], &ZRel[0], &XAbs[0], &YAbs[0], &ZAbs[0], M_DEFAULT); // Convert to pixel coordinates. vector<MIL_DOUBLE> XPix(NbPoints); vector<MIL_DOUBLE> YPix(NbPoints); McalTransformCoordinate3dList(MilPlaneFitMask, M_ABSOLUTE_COORDINATE_SYSTEM, M_PIXEL_COORDINATE_SYSTEM, NbPoints, &XAbs[0], &YAbs[0], &ZAbs[0], &XPix[0], &YPix[0], M_NULL, M_DEPTH_MAP); // Get the value of the list of pixels. vector<MIL_UINT8> IsInMask(NbPoints); MbufGetList(MilPlaneFitMask, NbPoints, &XPix[0], &YPix[0], M_NEAREST_NEIGHBOR, &IsInMask[0]); // Compute the point cloud relative MinZ and MaxZ of points that fall in the mask. MinZ = MIL_DOUBLE_MAX; MaxZ = -MIL_DOUBLE_MAX; for (MIL_INT p = 0; p < NbPoints; p++) { if (IsInMask[p] == 255) { if (ZRel[p] < MinZ) MinZ = ZRel[p]; else if (ZRel[p] > MaxZ) MaxZ = ZRel[p]; } } SPlanarityMeasure PlanePlanarityRasterMaskMeasure(MIL_TEXT("Point cloud with fitted plane ") MIL_TEXT("(world initial raster region)")); PlanePlanarityRasterMaskMeasure.Planarity = MaxZ - MinZ; // Show the point cloud with raster region measure. MosPrintf(MIL_TEXT("By transforming the points of the point cloud to the raster mask image,\n") MIL_TEXT("it is also possible to select the points based on whether they fall\n") MIL_TEXT("in the raster portion of the mask image.\n\n")); MbufLoad(MEASURES_ILLUSTRATIONS[4], MilIllustrationImage); AllMeasures.push_back(PlanePlanarityRasterMaskMeasure); PrintResultTable(AllMeasures); MosPrintf(MIL_TEXT("Press <Enter> to end.\n\n")); MosGetch(); MbufFree(MilIllustrationImage); #if ENABLE_DISPLAY_3D MdispD3DFree(MilDispD3D); #endif } else { MosPrintf(MIL_TEXT("Unable to find the part in the corrected depth map.\n") MIL_TEXT("Press <Enter> to end.\n\n")); MosGetch(); } // Free the allocated resources. M3dmapFree(MilPlaneGeometry); MbufFree(MilCalHolder); MbufFree(MilPlaneFitMask); MbufFree(MilDepthMap); MdispFree(MilDisplayDepthMap); MdispFree(MilIllustrationDisplay); MgraFree(MilDepthMapGraphicList); } //**************************************************************************** // FixturePart. Find the model, fixture a destination and draw the occurrence // in the graphic list. //**************************************************************************** bool FixturePart(MIL_ID MilSystem, MIL_ID MilDepthMap, MIL_ID MilFixtureDestination, MIL_ID MilGraList) { // Restore and setup the model used to fixture the part. const MIL_TEXT_CHAR* MECHANICAL_PART_MODEL = DATA_EX_PATH("ModelFinderContext.mmf"); MIL_ID ModelCtx = MmodRestore(MECHANICAL_PART_MODEL, MilSystem, M_WITH_CALIBRATION, M_NULL); MIL_ID ModelRes = MmodAllocResult(MilSystem, M_DEFAULT, M_NULL); // Preprocess the model finder context. MmodPreprocess(ModelCtx, M_DEFAULT); // Create the fixturing offset. MIL_ID FixtureOffset = McalAlloc(MilSystem, M_FIXTURING_OFFSET, M_DEFAULT, M_NULL); McalFixture(M_NULL, FixtureOffset, M_LEARN_OFFSET, M_MODEL_MOD, ModelCtx, 0, M_DEFAULT, M_DEFAULT, M_DEFAULT); // Allocate the target image. MIL_INT SizeX, SizeY; MbufInquire(MilDepthMap, M_SIZE_X, &SizeX); MbufInquire(MilDepthMap, M_SIZE_Y, &SizeY); MIL_ID MilSearchImage = MbufAlloc2d(MilSystem, SizeX, SizeY, 8, M_IMAGE + M_PROC, M_NULL); // Remap the depth map to the 8 bit target image. MimShift(MilDepthMap, MilSearchImage, -8); // Find the model. MmodFind(ModelCtx, MilSearchImage, ModelRes); // Retrieve the information. MIL_INT NumOfOccurences = 0; MmodGetResult(ModelRes, M_DEFAULT, M_NUMBER + M_TYPE_MIL_INT, &NumOfOccurences); if (NumOfOccurences) { // Fixture the depth map. McalFixture(MilFixtureDestination, FixtureOffset, M_MOVE_RELATIVE, M_RESULT_MOD, ModelRes, 0, M_DEFAULT, M_DEFAULT, M_DEFAULT); // Draw the found occurrence. MgraColor(M_DEFAULT, M_COLOR_RED); MmodDraw(M_DEFAULT, ModelRes, MilGraList, M_DRAW_EDGES + M_MODEL, M_DEFAULT, M_DEFAULT); } // Release the allocated resources. MmodFree(ModelCtx); MmodFree(ModelRes); MbufFree(MilSearchImage); McalFree(FixtureOffset); return (NumOfOccurences > 0); } //**************************************************************************** // GenerateDepthMap. Depth map extraction from the point cloud container. //**************************************************************************** void GenerateDepthMap(MIL_ID MilSystem, MIL_ID PointCloudContainer, const SMapGeneration& GenerationInfo, MIL_ID& OutDepthmap) { // Allocate the depth map if required. if (OutDepthmap == M_NULL) { MbufAlloc2d(MilSystem, GenerationInfo.MapSizeX, GenerationInfo.MapSizeY, 16 + M_UNSIGNED, M_IMAGE + M_PROC + M_DISP, &OutDepthmap); } // Set the extraction box. M3dmapSetBox(PointCloudContainer, M_EXTRACTION_BOX, M_CORNER_AND_DIMENSION, GenerationInfo.BoxCornerX, GenerationInfo.BoxCornerY, GenerationInfo.BoxCornerZ, GenerationInfo.BoxSizeX, GenerationInfo.BoxSizeY, GenerationInfo.BoxSizeZ); // Set extraction properties. M3dmapControl(PointCloudContainer, M_DEFAULT, M_EXTRACTION_CUMULATIVE, M_DISABLE); M3dmapControl(PointCloudContainer, M_DEFAULT, M_FILL_MODE, M_X_THEN_Y); M3dmapControl(PointCloudContainer, M_DEFAULT, M_FILL_SHARP_ELEVATION, M_DISABLE); M3dmapControl(PointCloudContainer, M_DEFAULT, M_FILL_THRESHOLD_X, GenerationInfo.FillXThreshold); M3dmapControl(PointCloudContainer, M_DEFAULT, M_FILL_THRESHOLD_Y, GenerationInfo.FillYThreshold); if (GenerationInfo.SetExtractOverlap) M3dmapControl(PointCloudContainer, M_GENERAL, M_EXTRACTION_OVERLAP, GenerationInfo.ExtractOverlap); // Generate the depth map. M3dmapExtract(PointCloudContainer, OutDepthmap, M_NULL, M_CORRECTED_DEPTH_MAP, M_ALL, M_DEFAULT); } //**************************************************************************** // PrintResultTable. Prints the result table of all the planarity measures. //**************************************************************************** void PrintResultTable(const vector<SPlanarityMeasure>& rPlanarityMeasures) { MosPrintf(MIL_TEXT("|--------------------------------------------------------------|-----------|\n") MIL_TEXT("| Planarity measure description | Planarity |\n") MIL_TEXT("|--------------------------------------------------------------|-----------|\n")); for(MIL_UINT m = 0; m < rPlanarityMeasures.size(); m++) { MIL_INT NameSize = MosStrlen(rPlanarityMeasures[m].MeasureName); MIL_INT Left = (62 - NameSize) / 2; MIL_INT Right = 62 - NameSize - Left; MosPrintf(MIL_TEXT("|%*s%*s|%8.4f mm|\n"), Left + NameSize, rPlanarityMeasures[m].MeasureName, Right, MIL_TEXT(""), rPlanarityMeasures[m].Planarity); } MosPrintf(MIL_TEXT("|--------------------------------------------------------------|-----------|\n")); } //**************************************************************************** // CheckForRequiredMILFile. Check for required files to run the example. //**************************************************************************** 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); }