| Click here to show toolbars of the Web Online Help System: show toolbars |
/************************************************************************************/ /* * File name: BaslerToFExample.cpp * Location: See Matrox Example Launcher in the MIL Control Center * * * Synopsis: This file contains the implementation of the general CBaslerToFExample class that * manages the example as well as the derived CBaslerToFImageExample and * CBaslerToFPointCloudExample. * * Copyright (C) Matrox Electronic Systems Ltd., 1992-2016. * All Rights Reserved */ #include <mil.h> #include <math.h> #include <vector> using std::vector; #include "BoxLocator.h" #include "DepthMapGenerator.h" #include "RangeValidator.h" #include "BaslerToFExample.h" #include "RangeToPointCloudConverter.h" //***************************************************************************** // Constants. //***************************************************************************** // The calibration grid parameters constants. static const MIL_INT GRID_TYPE = M_CIRCLE_GRID; static const MIL_INT GRID_NB_ROWS = 11; static const MIL_INT GRID_NB_COLUMNS = 15; static const MIL_DOUBLE GRID_ROW_SPACING = 300.5 / 6.0; // in mm static const MIL_DOUBLE GRID_COL_SPACING = 300.5 / 6.0; // in mm // The scale and offset of the range image. static const MIL_DOUBLE BASLER_TOF_WORKING_RANGE = 6663.0; // in mm static const MIL_DOUBLE RANGE_SCALE = BASLER_TOF_WORKING_RANGE / 65535; // in mm/gray static const MIL_DOUBLE RANGE_OFFSET = -18; // in mm // The validation parameters of the range image. static const MIL_INT DEPTH_THIRD_DERIV_FACTOR = 16; static const MIL_INT VALIDATOR_MORPH_ITER = 1; // Determine the size Z range of the box. static const MIL_DOUBLE MAX_BOX_SIZE_Z = 300; // in mm static const MIL_DOUBLE MIN_BOX_SIZE_Z = 50; // in mm // Parameters to find the rough rectangle. static const MIL_DOUBLE TOP_REFINE_RANGE = 25; // in mm static const MIL_DOUBLE MIN_RECTANGULARITY_ROUGH = 0.40; static const MIL_DOUBLE MIN_RECTANGULARITY_REFINE = 0.75; static const MIL_DOUBLE MIN_FERET_MIN = 75; // in mm // Depth map generation parameters. static const MIL_INT SMOOTH_KERNEL_SIZE = 7; static const MIL_DOUBLE DEPTH_MAP_PIXEL_SIZE = 2.5; // in mm/pixel // The display text line offset. static const MIL_INT DISPLAY_TEXT_LINE_OFFSET = 16; // Offsets of the windows. static const MIL_INT WINDOWS_OFFSET_X = 15; static const MIL_INT WINDOWS_OFFSET_Y = 38; // The BaslerToF camera NaN value casted to MIL_UINT32. static const MIL_UINT32 BASLER_NAN_UINT32 = 2143289344; // The standalone payload size of the camera. static const MIL_INT DEFAULT_PAYLOAD_SIZE = 6451200; static const MIL_DOUBLE DIV_180_PI = 57.295779513082320866997945294156; //***************************************************************************** // CBaslerToFExample class implementation. //***************************************************************************** //***************************************************************************** // Constructor. //***************************************************************************** CBaslerToFExample::CBaslerToFExample(MIL_ID MilSystem, MIL_ID MilDig) : m_MilSystem(MilSystem), m_pBoxLocator(NULL), m_pDepthMapGenerator(NULL), m_DepthDisplay(MilSystem, MIL_TEXT("Depth")), m_IntensityDisplay(MilSystem, MIL_TEXT("Intensity")), m_RangeDisplay(MilSystem, MIL_TEXT("Range")) { // Set whether the example uses the real camera. // // WARNING! This example was validated with an engineering sample camera and API. // The release camera and API may require an update to this example. // m_IsRealCamera = MsysInquire(MilSystem, M_SYSTEM_TYPE, M_NULL) != M_SYSTEM_HOST_TYPE; m_MilGrabImages[0] = M_NULL; m_MilGrabImages[1] = M_NULL; // Set the Basler ToF camera to grab the intensity image if using a real camera. EnableDisableComponent(MilDig, MIL_TEXT("Confidence"), MIL_TEXT("Mono16"), false); EnableDisableComponent(MilDig, MIL_TEXT("Range"), MIL_TEXT("Mono16"), false); EnableDisableComponent(MilDig, MIL_TEXT("Intensity"), MIL_TEXT("Mono16"), true); // Get the information of the image output by the camera. MdigInquire(MilDig, M_SIZE_X, &m_ImageSizeX); MdigInquire(MilDig, M_SIZE_Y, &m_ImageSizeY); // Allocate a point cloud container. M3dmapAllocResult(MilSystem, M_POINT_CLOUD_CONTAINER, M_DEFAULT, &m_MilPointCloud); // Set the depth display LUT. m_DepthDisplay.SetDisplayLut(MIL_UINT16_MAX, 1, MIL_UINT16_MAX); } //***************************************************************************** // Destructor. //***************************************************************************** CBaslerToFExample::~CBaslerToFExample() { M3dmapFree(m_MilPointCloud); if (m_MilGrabImages[0] != M_NULL) MbufFree(m_MilGrabImages[0]); if (m_MilGrabImages[1] != M_NULL) MbufFree(m_MilGrabImages[1]); } //***************************************************************************** // AllocateGrabImages. Allocates the images used for the grab. //***************************************************************************** void CBaslerToFExample::AllocateGrabImages(MIL_INT GrabImageSizeX, MIL_INT GrabImageSizeY, MIL_INT GrabImageType) { MbufAlloc2d(m_MilSystem, GrabImageSizeX, GrabImageSizeY, GrabImageType, M_IMAGE + M_GRAB, &m_MilGrabImages[0]); MbufAlloc2d(m_MilSystem, GrabImageSizeX, GrabImageSizeY, GrabImageType, M_IMAGE + M_GRAB, &m_MilGrabImages[1]); } //***************************************************************************** // EnableDisableComponent. Enable and disable an image component of the camera. //***************************************************************************** void CBaslerToFExample::EnableDisableComponent(MIL_ID MilDig, MIL_CONST_TEXT_PTR Component, MIL_CONST_TEXT_PTR PixelFormat, bool Enable) { if (m_IsRealCamera) { MdigControlFeature(MilDig, M_FEATURE_VALUE, MIL_TEXT("ImageComponentSelector"), M_TYPE_STRING, Component); MdigControlFeature(MilDig, M_FEATURE_VALUE, MIL_TEXT("ImageComponentEnable"), M_TYPE_STRING, Enable ? MIL_TEXT("1") : MIL_TEXT("0")); MdigControlFeature(MilDig, M_FEATURE_VALUE, MIL_TEXT("PixelFormat"), M_TYPE_STRING, PixelFormat); } } //***************************************************************************** // LocateBoxes. Starts continuous processing of range data to locate a box. //***************************************************************************** void CBaslerToFExample::LocateBoxes(MIL_ID MilBoxDig) { // Allocate the box locator. m_pBoxLocator = new CBoxLocator(m_pDepthMapGenerator->MilDepthMap(), MIN_FERET_MIN); // Locate boxes. Print the expected box parameters. MdigProcess(MilBoxDig, m_MilGrabImages, 2, M_START, M_DEFAULT, LocateBoxHook, this); MosPrintf(MIL_TEXT("Please place a box on the reference plane.\n\n") MIL_TEXT("EXPECTED BOX PARAMETERS:\n") MIL_TEXT("------------------------\n") MIL_TEXT("Box height range: [%.2f, %.2f] mm\n") MIL_TEXT("Min top surface dimension: %.2f mm\n") MIL_TEXT("Min top surface area: %.2f mm\n") MIL_TEXT("Min top rectangularity (SurfaceArea/BoundingRectangleArea): %.2f \n\n") MIL_TEXT("The range data is transformed into a MIL 3d point cloud.\n") MIL_TEXT("The depth map, extracted from the 3d point cloud, is displayed.\n") MIL_TEXT("The extraction box used is above and parallel to the reference plane.\n") MIL_TEXT("The box is located and measured using the depth map information.\n\n") MIL_TEXT("Press <Enter> to stop.\n\n"), MIN_BOX_SIZE_Z, MAX_BOX_SIZE_Z, m_pBoxLocator->MinFeretMin(), m_pBoxLocator->MinSurfaceArea(), MIN_RECTANGULARITY_REFINE); MosGetch(); // Stop the process. MdigProcess(MilBoxDig, m_MilGrabImages, 2, M_STOP + M_WAIT, M_DEFAULT, LocateBoxHook, this); delete m_pBoxLocator; } //***************************************************************************** // LocateBoxHook. Processing callback of the MdigProcess called by the LocateBoxes // function. //***************************************************************************** MIL_INT MFTYPE CBaslerToFExample::LocateBoxHook(MIL_INT HookType, MIL_ID MilHook, void* pHookData) { CBaslerToFExample* pExample = (CBaslerToFExample*)pHookData; return pExample->LocateBox(HookType, MilHook); } //***************************************************************************** // LocateBox. Processing function that locates a box from the range image. // If found, the box and its dimensions are drawn in the intensity display. // Here are the steps of the algorithm to locate a box: // // 1- The range data is retrieved from the camera. // For a range image source // -The data is transformed into a 3d point cloud using the calibration. // For a point cloud source // -The data is retrieved as is and put in a point cloud container. // 2- Based on the size of the grid or of the fitted plane data and the expected // height range of the box, a depth map is extracted from the 3d point cloud // from above the calibrated plane. // 3- Using the expected parameters of the top surface // (min dimension, area, and height sigma), the box is located using // a combination of blob and measurement: // - A rough box is found using blob in a large height range. // - The rough box is refined using blob in a tighter height range. // - The X and Y dimensions are further refined using measurement. // // See BoxLocator.cpp for more details on the blob and measurement usage. // //***************************************************************************** MIL_INT CBaslerToFExample::LocateBox(MIL_INT HookType, MIL_ID MilHook) { // Disable the display updates and clear the annotations. m_IntensityDisplay.Update(M_DISABLE, true); m_DepthDisplay.Update(M_DISABLE, true); m_RangeDisplay.Update(M_DISABLE); // Retrieve the MIL_ID of the grabbed buffer. MIL_ID ModifiedBufferId; MdigGetHookInfo(MilHook, M_MODIFIED_BUFFER + M_BUFFER_ID, &ModifiedBufferId); // Get the 3d data from the grab buffer. Get3dDataFromGrabBuffer(ModifiedBufferId); // Create the depth map. m_pDepthMapGenerator->CreateDepthMap(m_MilPointCloud, M_MAX); // Preprocess the depth map. m_pDepthMapGenerator->PreprocessDepthMap(SMOOTH_KERNEL_SIZE, MIN_FERET_MIN); // Find the rough location of the box in the processed depth map. SBox FoundBox = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; bool BoxIsFound = false; MgraColor(M_DEFAULT, M_COLOR_RED); if (m_pBoxLocator->FindRoughBox(m_pDepthMapGenerator->MilPreprocessedDepthMap(), MIN_BOX_SIZE_Z, MAX_BOX_SIZE_Z, MIN_RECTANGULARITY_ROUGH)) { // Refine the rough box. if (m_pBoxLocator->FindRoughBox(m_pDepthMapGenerator->MilPreprocessedDepthMap(), m_pBoxLocator->FoundBox().Height - TOP_REFINE_RANGE, m_pBoxLocator->FoundBox().Height + TOP_REFINE_RANGE, MIN_RECTANGULARITY_REFINE)) { // Refine the box. if (m_pBoxLocator->RefineBox(m_pDepthMapGenerator->MilPreprocessedDepthMap())) { // Draw the refine step annotations. MgraColor(M_DEFAULT, M_COLOR_CYAN); m_pBoxLocator->DrawDepthBox(M_DEFAULT, m_DepthDisplay.MilGraList()); MgraColor(M_DEFAULT, M_COLOR_MAGENTA); m_pBoxLocator->DrawDepthRefine(M_DEFAULT, m_DepthDisplay.MilGraList()); } else { MgraColor(M_DEFAULT, M_COLOR_CYAN); m_pBoxLocator->DrawDepthRoughBox(M_DEFAULT, m_DepthDisplay.MilGraList()); } // Set the found box. FoundBox = m_pBoxLocator->FoundBox(); BoxIsFound = true; } } // Display the results. DisplayResult(&FoundBox, BoxIsFound); // Print the results. if(BoxIsFound) MgraColor(M_DEFAULT, M_COLOR_CYAN); PrintResults(FoundBox, GetPrintDisplay()); m_RangeDisplay.Update(M_ENABLE); m_DepthDisplay.Update(M_ENABLE); m_IntensityDisplay.Update(M_ENABLE); return 0; } //***************************************************************************** // PrintResults. Print the results in console and in a display. //***************************************************************************** void CBaslerToFExample::PrintResults(const SBox& rFoundBox, const CDisplay& rPrintDisplay) { // Draw the calculated volume. MgraControl(M_DEFAULT, M_INPUT_UNITS, M_DISPLAY); MIL_DOUBLE BoxVolume = rFoundBox.Volume() / 1000.0; MIL_TEXT_CHAR Text[256]; MosSprintf(Text, 256, MIL_TEXT(" Box length: %8.1f cm "), rFoundBox.Length / 10.0); MgraText(M_DEFAULT, rPrintDisplay.MilGraList(), 0, 0, Text); MosSprintf(Text, 256, MIL_TEXT(" Box width: %8.1f cm "), rFoundBox.Width / 10.0); MgraText(M_DEFAULT, rPrintDisplay.MilGraList(), 0, DISPLAY_TEXT_LINE_OFFSET, Text); MosSprintf(Text, 256, MIL_TEXT(" Box height: %8.1f cm "), rFoundBox.Height / 10.0); MgraText(M_DEFAULT, rPrintDisplay.MilGraList(), 0, 2 * DISPLAY_TEXT_LINE_OFFSET, Text); MosSprintf(Text, 256, MIL_TEXT(" Box volume: %8.1f cm^3 "), BoxVolume); MgraText(M_DEFAULT, rPrintDisplay.MilGraList(), 0, 3 * DISPLAY_TEXT_LINE_OFFSET, Text); MgraControl(M_DEFAULT, M_INPUT_UNITS, M_PIXEL); } //***************************************************************************** // CBaslerToFImageExample class implementation. //***************************************************************************** //***************************************************************************** // Constructor. //***************************************************************************** CBaslerToFImageExample::CBaslerToFImageExample(MIL_ID MilSystem, MIL_ID MilGridDig, MIL_ID MilBoxDig) : CBaslerToFExample(MilSystem, MilGridDig), m_pRangeValidator(NULL) { // Set the Basler ToF camera to grab the intensity image if using a real camera. EnableDisableComponent(MilGridDig, MIL_TEXT("Confidence"), MIL_TEXT("Mono16"), false); EnableDisableComponent(MilGridDig, MIL_TEXT("Range"), MIL_TEXT("Mono16"), false); EnableDisableComponent(MilGridDig, MIL_TEXT("Intensity"), MIL_TEXT("Mono16"), true); // Get the information of the image output by the camera. MdigInquire(MilGridDig, M_SIZE_X, &m_ImageSizeX); MdigInquire(MilGridDig, M_SIZE_Y, &m_ImageSizeY); MIL_INT IntImageType = MdigInquire(MilGridDig, M_TYPE, M_NULL); // Allocate the grab buffer to hold up to 2 components. AllocateGrabImages(m_ImageSizeX, m_ImageSizeY * 2, IntImageType); // Allocate the intensity image. MbufAlloc2d(MilSystem, m_ImageSizeX, m_ImageSizeY, IntImageType, M_IMAGE + M_PROC + M_DISP, &m_MilIntImage); // Set the Basler ToF camera to grab the intensity and range component. EnableDisableComponent(MilBoxDig, MIL_TEXT("Confidence"), MIL_TEXT("Mono16"), false); EnableDisableComponent(MilBoxDig, MIL_TEXT("Intensity"), MIL_TEXT("Mono16"), true); EnableDisableComponent(MilBoxDig, MIL_TEXT("Range"), MIL_TEXT("Mono16"), true); // Allocate the range image. MIL_INT RangeImageType = MdigInquire(MilBoxDig, M_TYPE, M_NULL); MIL_INT RangeSizeBit = MdigInquire(MilBoxDig, M_SIZE_BIT, M_NULL); MbufAlloc2d(MilSystem, m_ImageSizeX, m_ImageSizeY, RangeImageType, M_IMAGE + M_PROC + M_DISP + M_GRAB, &m_MilRangeImage); // Allocate the calibration. McalAlloc(MilSystem, M_TSAI_BASED, M_DEFAULT, &m_MilCal); } //***************************************************************************** // Destructor. //***************************************************************************** CBaslerToFImageExample::~CBaslerToFImageExample() { McalFree(m_MilCal); MbufFree(m_MilIntImage); MbufFree(m_MilRangeImage); } //***************************************************************************** // CalibrateCamera. Calibrates the camera according to the constant parameters // defined by the application. //***************************************************************************** bool CBaslerToFImageExample::CalibrateCamera(MIL_ID MilGridDig) { // Select the intensity image. m_IntensityDisplay.Select(m_MilIntImage); // Put the display in bit shift mode. m_IntensityDisplay.SetDisplayBitShift(8); // Set the Basler ToF camera to grab the intensity image if using a real camera. EnableDisableComponent(MilGridDig, MIL_TEXT("Confidence"),MIL_TEXT("Mono16"), false); EnableDisableComponent(MilGridDig, MIL_TEXT("Range"), MIL_TEXT("Mono16"), false); EnableDisableComponent(MilGridDig, MIL_TEXT("Intensity"), MIL_TEXT("Mono16"), true); // Ask the user to place a calibration grid in front of the Basler ToF camera. MdigProcess(MilGridDig, m_MilGrabImages, 2, M_START, M_DEFAULT, GrabContinuousHook, this); MosPrintf(MIL_TEXT("Please place the calibration grid in front of the camera.\n\n")); if (m_IsRealCamera) MosPrintf(MIL_TEXT("EXPECTED GRID PARAMETERS:\n") MIL_TEXT("-------------------------\n") MIL_TEXT("GridType : %s\n") MIL_TEXT("Nb of rows : %d\n") MIL_TEXT("Nb of columns : %d\n") MIL_TEXT("Grid row spacing : %.2f\n") MIL_TEXT("Grid column spacing : %.2f\n\n") MIL_TEXT("Please modify the calibration grid parameters constants at the beginning\n") MIL_TEXT("of the example code according to your calibration grid.\n\n"), GRID_TYPE == M_CIRCLE_GRID ? MIL_TEXT("M_CIRCLE_GRID") : MIL_TEXT("M_CHESSBOARD_GRID"), GRID_NB_ROWS, GRID_NB_COLUMNS, GRID_ROW_SPACING, GRID_COL_SPACING); MosPrintf(MIL_TEXT("Press <Enter> to grab.\n\n")); MosGetch(); // Stop the continuous grab. MdigProcess(MilGridDig, m_MilGrabImages, 2, M_STOP + M_WAIT, M_DEFAULT, GrabContinuousHook, this); // Calibrate the camera. MIL_DOUBLE GridSizeX = (GRID_NB_COLUMNS - 1) * GRID_COL_SPACING; MIL_DOUBLE GridSizeY = (GRID_NB_ROWS - 1) * GRID_ROW_SPACING; MIL_DOUBLE GridWorldOffsetX = -GridSizeX / 2; MIL_DOUBLE GridWorldOffsetY = -GridSizeY / 2; McalGrid(m_MilCal, m_MilIntImage, GridWorldOffsetX, GridWorldOffsetY, 0.0, GRID_NB_ROWS, GRID_NB_COLUMNS, GRID_ROW_SPACING, GRID_COL_SPACING, M_DEFAULT, GRID_TYPE); if (McalInquire(m_MilCal, M_CALIBRATION_STATUS, M_NULL) == M_CALIBRATED) { // Display the calibration points and coordinate system. MgraColor(M_DEFAULT, M_COLOR_CYAN); McalDraw(M_DEFAULT, m_MilCal, m_IntensityDisplay.MilGraList(), M_DRAW_ABSOLUTE_COORDINATE_SYSTEM, M_DEFAULT, M_DEFAULT); MgraColor(M_DEFAULT, M_COLOR_GREEN); McalDraw(M_DEFAULT, m_MilCal, m_IntensityDisplay.MilGraList(), M_DRAW_WORLD_POINTS, M_DEFAULT, M_DEFAULT); MosPrintf(MIL_TEXT("The calibration of the camera is displayed.\n\n") MIL_TEXT("Press <Enter> to continue.\n\n")); MosGetch(); // Clear the graphic list. MgraClear(M_DEFAULT, m_IntensityDisplay.MilGraList()); // Put the relative coordinate system on the camera coordinate system. McalSetCoordinateSystem(m_MilCal, M_RELATIVE_COORDINATE_SYSTEM, M_CAMERA_COORDINATE_SYSTEM, M_IDENTITY, M_NULL, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT); // Set the LUT of the range display according to the calibration. MIL_DOUBLE GridPosX; MIL_DOUBLE GridPosY; MIL_DOUBLE GridPosZ; McalGetCoordinateSystem(m_MilCal, M_CAMERA_COORDINATE_SYSTEM, M_ABSOLUTE_COORDINATE_SYSTEM, M_TRANSLATION, M_NULL, &GridPosX, &GridPosY, &GridPosZ, M_NULL); MIL_DOUBLE GridDist = sqrt(GridPosX*GridPosX + GridPosY*GridPosY + GridPosZ*GridPosZ); MIL_INT MaxGray = (MIL_INT)(2 * GridDist / RANGE_SCALE - RANGE_OFFSET); MaxGray = MaxGray > (MIL_UINT16_MAX - 1) ? MIL_UINT16_MAX - 1 : MaxGray; m_RangeDisplay.SetDisplayLut(MIL_UINT16_MAX, 1, MaxGray); return true; } else { MosPrintf(MIL_TEXT("Calibration generated an exception.\n")); MosPrintf(MIL_TEXT("See User Guide to resolve the situation.\n\n")); return false; } } //***************************************************************************** // LocateBoxes. Starts continuous processing of range image to locate a box. //***************************************************************************** void CBaslerToFImageExample::LocateBoxes(MIL_ID MilBoxDig) { // Allocate the range validator and depth generator. m_pRangeValidator = new CRangeValidator(m_MilRangeImage); MIL_DOUBLE ExtendedGridSizeX = (GRID_NB_COLUMNS + 1) * GRID_COL_SPACING; MIL_DOUBLE ExtendedGridSizeY = (GRID_NB_ROWS + 1) * GRID_ROW_SPACING; m_pRangeToPointCloud = new CRangeToPointCloudConverter(m_MilCal, m_ImageSizeX, m_ImageSizeY); m_pDepthMapGenerator = new CDepthMapGenerator(m_MilSystem, DEPTH_MAP_PIXEL_SIZE, -ExtendedGridSizeX / 2, -ExtendedGridSizeY / 2, -MIN_BOX_SIZE_Z / 2, ExtendedGridSizeX, ExtendedGridSizeY, MAX_BOX_SIZE_Z - MIN_BOX_SIZE_Z / 2, MIN_FERET_MIN); // Set the position of the displays. m_RangeDisplay.SetInitialWindowPos(m_ImageSizeX + WINDOWS_OFFSET_X, 0); m_DepthDisplay.SetInitialWindowPos(m_ImageSizeX - (m_pDepthMapGenerator->DepthMapSizeX() - WINDOWS_OFFSET_X) / 2, m_ImageSizeY / 2 + WINDOWS_OFFSET_Y); // Select the range and depth images. m_RangeDisplay.Select(m_MilRangeImage); m_DepthDisplay.Select(m_pDepthMapGenerator->MilPreprocessedDepthMap()); // Set the BaslerToF camera to grab the intensity and range component. EnableDisableComponent(MilBoxDig, MIL_TEXT("Confidence"), MIL_TEXT("Mono16"), false); EnableDisableComponent(MilBoxDig, MIL_TEXT("Intensity"), MIL_TEXT("Mono16"), true); EnableDisableComponent(MilBoxDig, MIL_TEXT("Range"), MIL_TEXT("Mono16"), true); // Locate the boxes. CBaslerToFExample::LocateBoxes(MilBoxDig); delete m_pRangeValidator; delete m_pDepthMapGenerator; delete m_pRangeToPointCloud; } //***************************************************************************** // DisplayResult. Displays the found box. //***************************************************************************** void CBaslerToFImageExample::DisplayResult(const SBox* pFoundBox, bool BoxIsFound) { if(BoxIsFound) { // Draw the 3d box and a coordinate system on top of the box. MgraColor(M_DEFAULT, M_COLOR_CYAN); m_pBoxLocator->Draw3dBox(M_DEFAULT, m_MilCal, m_IntensityDisplay.MilGraList()); m_pBoxLocator->Draw3dBoxCoordinateSystem(M_DEFAULT, m_MilCal, m_IntensityDisplay.MilGraList()); } } //***************************************************************************** // GrabContinuousHook. Processing callback of MdigProcess to grab the intensity image. //***************************************************************************** MIL_INT MFTYPE CBaslerToFImageExample::GrabContinuousHook(MIL_INT HookType, MIL_ID MilHook, void* pHookData) { CBaslerToFImageExample* pExample = (CBaslerToFImageExample*)pHookData; return pExample->GrabContinuous(HookType, MilHook); } //***************************************************************************** // GrabContinuous. Processing function that copies the grab buffer to the // displayed buffer. //***************************************************************************** MIL_INT CBaslerToFImageExample::GrabContinuous(MIL_INT HookType, MIL_ID MilHook) { MIL_ID ModifiedBufferId; // Retrieve the MIL_ID of the grabbed buffer. MdigGetHookInfo(MilHook, M_MODIFIED_BUFFER + M_BUFFER_ID, &ModifiedBufferId); // Update the displayed image. MbufCopy(ModifiedBufferId, m_MilIntImage); return 0; } //***************************************************************************** // Get3dDataFromGrabBuffer. Gets the 3d data from the grabbed image. Preprocesses // the range image and creates the point cloud from the // result. //***************************************************************************** void CBaslerToFImageExample::Get3dDataFromGrabBuffer(MIL_ID MilModifiedBuffer) { // Get the range and intensity image. MbufCopy(MilModifiedBuffer, m_MilRangeImage); MbufCopyColor2d(MilModifiedBuffer, m_MilIntImage, 0, 0, m_ImageSizeY, 0, 0, 0, m_ImageSizeX, m_ImageSizeY); // Validate the range values of the range image. m_pRangeValidator->PreprocessRange(m_MilRangeImage, DEPTH_THIRD_DERIV_FACTOR, VALIDATOR_MORPH_ITER); // Set the corresponding pixels as invalid data in the original image. MbufClearCond(m_MilRangeImage, 0, 0, 0, m_pRangeValidator->MilRangeValidImage(), M_EQUAL, 0); // Generate the point cloud. m_pRangeToPointCloud->CreatePointCloud(m_MilRangeImage, m_MilPointCloud, m_MilCal, RANGE_OFFSET, RANGE_SCALE); } //***************************************************************************** // CBaslerToFPointCloudExample class implementation. //***************************************************************************** //***************************************************************************** // Constructor. //***************************************************************************** CBaslerToFPointCloudExample::CBaslerToFPointCloudExample(MIL_ID MilSystem, MIL_ID MilIntensityDig, MIL_ID MilPlanePointCloudDig) : CBaslerToFExample(MilSystem, MilIntensityDig) { // Allocate the grab buffer to hold the point cloud. MIL_INT PayloadSize = DEFAULT_PAYLOAD_SIZE; if(m_IsRealCamera) MdigInquireFeature(MilPlanePointCloudDig, M_FEATURE_VALUE, MIL_TEXT("PayloadSize"), M_TYPE_INT64, &PayloadSize); MIL_INT GrabImageSizeY = PayloadSize / m_ImageSizeX / 3 / 4; AllocateGrabImages(m_ImageSizeX * 3, GrabImageSizeY, 32 + M_FLOAT); // Allocate the plane geometry. M3dmapAlloc(MilSystem, M_GEOMETRY, M_DEFAULT, &m_Mil3dPlaneGeometry); // Allocate the camera depth map. MbufAlloc2d(MilSystem, m_ImageSizeX/2, m_ImageSizeY/2, 16 + M_UNSIGNED, M_IMAGE + M_PROC + M_DISP, &m_MilCameraDepthMap); // Allocate the matrix. MbufAlloc2d(MilSystem, 4, 4, 32 + M_FLOAT, M_ARRAY, &m_MilHomogeneousMatrix); // Set the depth display LUT. m_DepthDisplay.SetDisplayLut(MIL_UINT16_MAX, 1, MIL_UINT16_MAX); #if ENABLE_DISPLAY_3D m_MilDispD3d = M_NULL; #endif } //***************************************************************************** // Destructor. //***************************************************************************** CBaslerToFPointCloudExample::~CBaslerToFPointCloudExample() { MbufFree(m_MilHomogeneousMatrix); MbufFree(m_MilCameraDepthMap); M3dmapFree(m_Mil3dPlaneGeometry); #if ENABLE_DISPLAY_3D if (m_MilDispD3d) MdispD3DFree(m_MilDispD3d); #endif } //***************************************************************************** // LearnPlane. Learn the plane from the point cloud of the 3d camera. //***************************************************************************** bool CBaslerToFPointCloudExample::LearnPlane(MIL_ID MilPlaneDig) { // Set the Basler ToF camera to grab the point cloud image if using a real camera. EnableDisableComponent(MilPlaneDig, MIL_TEXT("Confidence"), MIL_TEXT("Mono16"), false); EnableDisableComponent(MilPlaneDig, MIL_TEXT("Intensity"), MIL_TEXT("Mono16"), false); EnableDisableComponent(MilPlaneDig, MIL_TEXT("Range"), MIL_TEXT("Coord3D_ABC32f"), true); // Ask the user to place a calibration grid in front of the Basler ToF camera. MdigProcess(MilPlaneDig, m_MilGrabImages, 2, M_START, M_DEFAULT, GrabPointCloudContinuousHook, this); MosPrintf(MIL_TEXT("Please place the camera to view a flat horizontal surface.\n\n")); MosPrintf(MIL_TEXT("Press <Enter> to grab.\n\n")); MosGetch(); // Stop the continuous grab. MdigProcess(MilPlaneDig, m_MilGrabImages, 2, M_STOP + M_WAIT, M_DEFAULT, GrabPointCloudContinuousHook, this); // Learn the plane from the image. M3dmapSetGeometry(m_Mil3dPlaneGeometry, M_PLANE, M_FIT, (MIL_DOUBLE)m_MilCameraDepthMap, M_NULL, M_INFINITE, M_DEFAULT, M_DEFAULT); // Set the point cloud relative coordinate system on the plane. // Get the parameters of the plane. MIL_DOUBLE Ax; MIL_DOUBLE Ay; MIL_DOUBLE Z0; M3dmapInquire(m_Mil3dPlaneGeometry, M_DEFAULT, M_FIT_PARAM_AX, &Ax); M3dmapInquire(m_Mil3dPlaneGeometry, M_DEFAULT, M_FIT_PARAM_AY, &Ay); M3dmapInquire(m_Mil3dPlaneGeometry, M_DEFAULT, M_FIT_PARAM_Z0, &Z0); // 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(m_MilPointCloud, M_RELATIVE_COORDINATE_SYSTEM, M_ABSOLUTE_COORDINATE_SYSTEM, M_TRANSLATION, M_NULL, 0.0, 0.0, Z0, M_DEFAULT); McalSetCoordinateSystem(m_MilPointCloud, 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); return true; } //***************************************************************************** // LocateBoxes. Starts continuous processing of camera point cloud to locate a box. //***************************************************************************** void CBaslerToFPointCloudExample::LocateBoxes(MIL_ID MilBoxDig) { // Allocate the depth map generator. m_pDepthMapGenerator = new CDepthMapGenerator(m_MilPointCloud, DEPTH_MAP_PIXEL_SIZE, -MIN_BOX_SIZE_Z / 2, MAX_BOX_SIZE_Z - MIN_BOX_SIZE_Z / 2, MIN_FERET_MIN); // Set the position of the display. m_DepthDisplay.SetInitialWindowPos(DEPTH_DISPLAY_OFFSET_X, 0); // Select the depth images. m_DepthDisplay.Select(m_pDepthMapGenerator->MilPreprocessedDepthMap()); #if ENABLE_DISPLAY_3D // Allocate images to display the box in the 3d display. MbufClone(m_pDepthMapGenerator->MilPreprocessedDepthMap(), M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, &m_MilBoxDepthMap); MbufClone(m_pDepthMapGenerator->MilPreprocessedDepthMap(), M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, &m_MilContourDepthMap); MIL_INT DepthMapSizeX = MbufInquire(m_MilBoxDepthMap, M_SIZE_X, M_NULL); MIL_INT DepthMapSizeY = MbufInquire(m_MilBoxDepthMap, M_SIZE_Y, M_NULL); MbufAllocColor(m_MilSystem, 3, DepthMapSizeX, DepthMapSizeY, 8 + M_UNSIGNED, M_IMAGE + M_PROC, &m_MilBoxColorMap); #endif // Set the Basler ToF camera to grab the intensity and range component. EnableDisableComponent(MilBoxDig, MIL_TEXT("Confidence"), MIL_TEXT("Mono16"), false); EnableDisableComponent(MilBoxDig, MIL_TEXT("Intensity"), MIL_TEXT("Mono16"), false); EnableDisableComponent(MilBoxDig, MIL_TEXT("Range"), MIL_TEXT("Coord3D_ABC32f"), true); // Locate the boxes. CBaslerToFExample::LocateBoxes(MilBoxDig); #if ENABLE_DISPLAY_3D // Free the images to display the box in the 3d display. MbufFree(m_MilBoxDepthMap); MbufFree(m_MilContourDepthMap); MbufFree(m_MilBoxColorMap); #endif delete m_pDepthMapGenerator; } //***************************************************************************** // DisplayResult. Displays the found box. //***************************************************************************** void CBaslerToFPointCloudExample::DisplayResult(const SBox* pFoundBox, bool BoxIsFound) { #if ENABLE_DISPLAY_3D // Display the depth map and found box. MIL_ID MilDepthMaps[2] = {m_MilCameraDepthMap, M_NULL}; MIL_ID MilColorMaps[2] = {M_NULL, M_NULL}; MIL_INT NbSystems = 1; if(BoxIsFound) { // Clear the previously found box from the depth map. Put to invalid data. MbufClear(m_MilBoxDepthMap, MIL_UINT16_MAX); MbufClear(m_MilBoxColorMap, M_COLOR_CYAN); // Associate the calibration from the preprocessed depth map. Set the graylevel size z // so that the box height is the maximum value of the depth map. McalAssociate(m_pDepthMapGenerator->MilPreprocessedDepthMap(), m_MilBoxDepthMap, M_DEFAULT); McalAssociate(m_pDepthMapGenerator->MilPreprocessedDepthMap(), m_MilBoxColorMap, M_DEFAULT); McalControl(m_MilBoxDepthMap, M_GRAY_LEVEL_SIZE_Z, -pFoundBox->Height / (MIL_UINT16_MAX - 1)); McalControl(m_MilBoxDepthMap, M_WORLD_POS_Z, 0); // Draw the found box top face in the depth map. MgraColor(M_DEFAULT, MIL_UINT16_MAX -1); MgraControl(M_DEFAULT, M_INPUT_UNITS, M_WORLD); MgraRectAngle(M_DEFAULT, m_MilBoxDepthMap, pFoundBox->CenterPosX, pFoundBox->CenterPosY, pFoundBox->Length, pFoundBox->Width, pFoundBox->ZAxisAngle, M_CENTER_AND_DIMENSION + M_FILLED); MgraControl(M_DEFAULT, M_INPUT_UNITS, M_PIXEL); // Add a contour around the top face that is on the learned plane. MimErode(m_MilBoxDepthMap, m_MilContourDepthMap, 1, M_GRAYSCALE); MimArith(m_MilBoxDepthMap, m_MilContourDepthMap, m_MilContourDepthMap, M_EQUAL); MimArith(m_MilBoxDepthMap, m_MilContourDepthMap, m_MilBoxDepthMap, M_AND); MilDepthMaps[0] = m_MilBoxDepthMap; MilDepthMaps[1] = m_MilCameraDepthMap; MilColorMaps[0] = m_MilBoxColorMap; MilColorMaps[1] = M_NULL; NbSystems = 2; } if(!m_MilDispD3d) { m_MilDispD3d = MdepthSysD3DAlloc(M_NULL, M_NULL, M_NULL, M_NULL, MilDepthMaps, MilColorMaps, NbSystems, DISPLAY_3D_SIZE_X, DISPLAY_3D_SIZE_Y, M_DEFAULT, M_NULL); // Set camera pose. MdispD3DControl(m_MilDispD3d, MD3D_LOOK_AT_X, DISPLAY_3D_LOOK_AT_X); MdispD3DControl(m_MilDispD3d, MD3D_LOOK_AT_Y, DISPLAY_3D_LOOK_AT_Y); MdispD3DControl(m_MilDispD3d, MD3D_LOOK_AT_Z, DISPLAY_3D_LOOK_AT_Z); MdispD3DControl(m_MilDispD3d, MD3D_EYE_THETA, DISPLAY_3D_EYE_THETA); MdispD3DControl(m_MilDispD3d, MD3D_EYE_PHI, DISPLAY_3D_EYE_PHI); MdispD3DControl(m_MilDispD3d, MD3D_EYE_DIST, DISPLAY_3D_EYE_DIST); // Set to display the depth map in point mode. MdispD3DControl(m_MilDispD3d, MD3D_POINT, MD3D_ENABLE); } else MdepthSysD3DSetSystems(m_MilDispD3d, M_NULL, M_NULL, M_NULL, M_NULL, MilDepthMaps, MilColorMaps, NbSystems, M_DEFAULT); MdispD3DShow(m_MilDispD3d); #endif } //***************************************************************************** // Get3dDataFromGrabBuffer. Gets the 3d data from the grabbed image. Manages // the conversion of Basler invalid point value to // MIL invalid point value, then puts the grabbed data // right into the point cloud container. This function // also generates the camera depth map which is a depth // map projected from the point of view of the camera. //***************************************************************************** void CBaslerToFPointCloudExample::Get3dDataFromGrabBuffer(MIL_ID MilModifiedBuffer) { // Process the modified buffer to set invalid data. MIL_ID MilSystem = MbufInquire(MilModifiedBuffer, M_OWNER_SYSTEM, M_NULL); MIL_ID MilPointCloudUnsignedImage = MbufCreate2d(MilSystem, m_ImageSizeX * 3, m_ImageSizeY, 32 + M_UNSIGNED, M_IMAGE + M_PROC, M_MIL_ID + M_PITCH, M_DEFAULT, MilModifiedBuffer, M_NULL); MIL_FLOAT InvalidPoint = M_INVALID_POINT_FLOAT; MIL_UINT32* pInvalidPointUint32 = (MIL_UINT32*)(&InvalidPoint); MbufClearCond(MilPointCloudUnsignedImage, *pInvalidPointUint32, *pInvalidPointUint32, *pInvalidPointUint32, MilPointCloudUnsignedImage, M_EQUAL, BASLER_NAN_UINT32); MbufFree(MilPointCloudUnsignedImage); // Put the point cloud in the container. MIL_FLOAT* pPointCloudData = (MIL_FLOAT*)MbufInquire(MilModifiedBuffer, M_HOST_ADDRESS, M_NULL); M3dmapPut(m_MilPointCloud, M_POINT_CLOUD_LABEL(1), M_XYZ, 32 + M_FLOAT, m_ImageSizeX * m_ImageSizeY * 3, pPointCloudData, M_NULL, M_NULL, M_NULL, M_DEFAULT); // Generate a depth map from the absolute coordinate system point of view. Save the relative and put it back in case it was moved. McalGetCoordinateSystem(m_MilPointCloud, M_RELATIVE_COORDINATE_SYSTEM, M_ABSOLUTE_COORDINATE_SYSTEM, M_HOMOGENEOUS_MATRIX, m_MilHomogeneousMatrix, M_NULL, M_NULL, M_NULL, M_NULL); McalSetCoordinateSystem(m_MilPointCloud, M_RELATIVE_COORDINATE_SYSTEM, M_ABSOLUTE_COORDINATE_SYSTEM, M_IDENTITY, M_NULL, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT); M3dmapSetBox(m_MilPointCloud, M_EXTRACTION_BOX, M_BOUNDING_BOX, M_POINT_CLOUD_LABEL(1), M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT); M3dmapExtract(m_MilPointCloud, m_MilCameraDepthMap, M_NULL, M_CORRECTED_DEPTH_MAP, M_POINT_CLOUD_LABEL(1), M_DEFAULT); McalSetCoordinateSystem(m_MilPointCloud, M_RELATIVE_COORDINATE_SYSTEM, M_ABSOLUTE_COORDINATE_SYSTEM, M_HOMOGENEOUS_MATRIX, m_MilHomogeneousMatrix, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT); } //***************************************************************************** // GrabPointCloudContinuousHook. Processing callback of MdigProcess to grab the // point cloud. //***************************************************************************** MIL_INT MFTYPE CBaslerToFPointCloudExample::GrabPointCloudContinuousHook(MIL_INT HookType, MIL_ID MilHook, void* pHookData) { CBaslerToFPointCloudExample* pExample = (CBaslerToFPointCloudExample*)pHookData; return pExample->GrabPointCloudContinuous(HookType, MilHook); } //***************************************************************************** // GrabPointCloudContinuous. Processing function that gets the point cloud from // the grab buffer. //***************************************************************************** MIL_INT CBaslerToFPointCloudExample::GrabPointCloudContinuous(MIL_INT HookType, MIL_ID MilHook) { MIL_ID ModifiedBufferId; // Retrieve the MIL_ID of the grabbed buffer. MdigGetHookInfo(MilHook, M_MODIFIED_BUFFER + M_BUFFER_ID, &ModifiedBufferId); // Get the point cloud in the container. this->Get3dDataFromGrabBuffer(ModifiedBufferId); // Update the display. DisplayResult(NULL, false); return 0; } //***************************************************************************** // CDisplay class implementation. //***************************************************************************** //***************************************************************************** // Constructor. //***************************************************************************** CDisplay::CDisplay(MIL_ID MilSystem, MIL_CONST_TEXT_PTR DisplayName) : m_MilLut(M_NULL), m_MilSystem(MilSystem) { MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, &m_MilDisp); MgraAllocList(MilSystem, M_DEFAULT, &m_MilGraList); MdispControl(m_MilDisp, M_ASSOCIATED_GRAPHIC_LIST_ID, m_MilGraList); MdispControl(m_MilDisp, M_TITLE, M_PTR_TO_DOUBLE(DisplayName)); } //***************************************************************************** // Destructor. //***************************************************************************** CDisplay::~CDisplay() { MgraFree(m_MilGraList); MdispFree(m_MilDisp); if (m_MilLut) MbufFree(m_MilLut); } //***************************************************************************** // SetInitialWindowPos. Sets the initial position of the display window. //***************************************************************************** void CDisplay::SetInitialWindowPos(MIL_INT WindowX, MIL_INT WindowY) { MdispControl(m_MilDisp, M_WINDOW_INITIAL_POSITION_X, WindowX); MdispControl(m_MilDisp, M_WINDOW_INITIAL_POSITION_Y, WindowY); } //***************************************************************************** // Update. Sets the update(M_ENABLE/M_DISABLE) parameter of the display. Optionally // clears the graphic list. //***************************************************************************** void CDisplay::Update(MIL_INT Update, bool ClearAnnotations /* = false */) { MdispControl(m_MilDisp, M_UPDATE, Update); if (ClearAnnotations) MgraClear(M_DEFAULT, m_MilGraList); } //***************************************************************************** // Select. Selects the image on the display. //***************************************************************************** void CDisplay::Select(MIL_ID MilImage) { MdispSelect(m_MilDisp, MilImage); } //***************************************************************************** // SetDisplayLut. Set a color LUT on the display. The color LUT range is defined // based on the MinGray and Maxgray parameters. //***************************************************************************** void CDisplay::SetDisplayLut(MIL_INT MaxImageValue, MIL_INT MinGray, MIL_INT MaxGray) { // Reallocate the LUT. if (m_MilLut) MbufFree(m_MilLut); MbufAllocColor(m_MilSystem, 3, MaxImageValue, 1, 8 + M_UNSIGNED, M_LUT, &m_MilLut); // Create the color map LUT. MbufClear(m_MilLut, M_COLOR_BLACK); MIL_ID MilDispLutChild = MbufChild1d(m_MilLut, MinGray, MaxGray - MinGray, M_NULL); MgenLutFunction(MilDispLutChild, M_COLORMAP_JET, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT); MbufFree(MilDispLutChild); // Set the LUT to the display. MdispLut(m_MilDisp, m_MilLut); } //***************************************************************************** // SetDisplayBitShift. Set the display to use a bit shift view mode. //***************************************************************************** void CDisplay::SetDisplayBitShift(MIL_INT ViewBitShift) { MdispControl(m_MilDisp, M_VIEW_MODE, M_BIT_SHIFT); MdispControl(m_MilDisp, M_VIEW_BIT_SHIFT, ViewBitShift); }