| Click here to show toolbars of the Web Online Help System: show toolbars |
//******************************************************************************* // // File name: CookieDetection.cpp // Location: See Matrox Example Launcher in the MIL Control Center // // // Synopsis: Demonstrations inspection of cookies using 3d data. // // Copyright (C) Matrox Electronic Systems Ltd., 1992-2016. // All Rights Reserved #include "CookieDetection.h" //**************************************************************************** // Example description. //**************************************************************************** void PrintHeader() { MosPrintf(MIL_TEXT("[EXAMPLE NAME]\n")); MosPrintf(MIL_TEXT("CookieDetection\n\n")); MosPrintf(MIL_TEXT("[SYNOPSIS]\n")); MosPrintf(MIL_TEXT("This example demonstrates how to count cookies using 3d sheet-") MIL_TEXT("of-light\nprofiling. The system consists of two cameras and one ") MIL_TEXT("laser. Note\nthat during the setup of the grab, the cameras were ") MIL_TEXT("synchronized so\nthe same laser scan was provided to all cameras ") MIL_TEXT("at the same time.\n")); MosPrintf(MIL_TEXT("\n\n")); MosPrintf(MIL_TEXT("[MODULES USED]\n")); MosPrintf(MIL_TEXT("Modules used: Application, system, display, buffer, ") MIL_TEXT("graphic, \nimage processing, calibration,")); MosPrintf(MIL_TEXT(" 3d reconstruction, model finder. \n")); } //***************************************************************************** // Main. //***************************************************************************** int MosMain(void) { PrintHeader(); // Allocate the MIL application. MIL_ID MilApplication = MappAlloc(M_NULL, M_DEFAULT, M_NULL); // Initialization. CExampleManagerFor3D* pExampleMngrFor3D = MakeExampleManager(); if (!pExampleMngrFor3D) { MappFree(MilApplication); return -1; } MosPrintf(MIL_TEXT("Press <Enter> to start.\n\n")); MosGetch(); //....................................................................... // 1. To calibrate the setup, the first step is to calibrate the cameras. // Camera calibration specifications. const MIL_DOUBLE COL_SPACING [NUM_CAMERAS] = { 8.83, 8.83 }; const MIL_DOUBLE ROW_SPACING [NUM_CAMERAS] = { 8.83, 8.83 }; const MIL_INT NB_ROWS [NUM_CAMERAS] = { 22, 22 }; const MIL_INT NB_COLS [NUM_CAMERAS] = { 18, 18 }; const MIL_DOUBLE CORNER_HINT_X [NUM_CAMERAS] = { 1200, 1200 }; const MIL_DOUBLE CORNER_HINT_Y [NUM_CAMERAS] = { 120, 120 }; const MIL_DOUBLE OFFSET_Z [NUM_CAMERAS] = { -16.77, -16.77 }; const MIL_INT64 CALIBRATION_TYPE [NUM_CAMERAS] = { M_CHESSBOARD_GRID, M_CHESSBOARD_GRID}; const MIL_CONST_TEXT_PTR GRID_IMG_FILENAME [NUM_CAMERAS] = { EX_PATH("Cam1_grid.mim"), EX_PATH("Cam2_grid.mim") }; // Initialize data. SCameraCalibrationInfo CAMERA_CALIBRATION_INFO[NUM_CAMERAS]; for (MIL_INT c = 0; c < NUM_CAMERAS; c++) { SCameraCalibrationInfo& CCI = CAMERA_CALIBRATION_INFO[c]; CCI.CornerHintX = CORNER_HINT_X[c]; CCI.CornerHintY = CORNER_HINT_Y[c]; CCI.OffsetZ = OFFSET_Z[c]; CCI.NbRows = NB_ROWS[c]; CCI.NbCols = NB_COLS[c]; CCI.RowSpacing = ROW_SPACING[c]; CCI.ColSpacing = COL_SPACING[c]; CCI.CalibrationType = CALIBRATION_TYPE[c]; CCI.GridImageFilename = GRID_IMG_FILENAME[c]; CCI.Relocate = NO_RELOCATE; CCI.RelocatedGridImageFilename = NULL; } //................................. // 1.1 Execute cameras calibration. MIL_ID CameraCalibrations[NUM_CAMERAS]; bool CameraCalibrationOk = pExampleMngrFor3D->CalibrateCameras(CAMERA_CALIBRATION_INFO, NUM_CAMERAS, &CameraCalibrations[0]); //.................................................................. // 2. Then continue to calibrate the laser planes (sheets-of-light). if(CameraCalibrationOk) { MosPrintf(MIL_TEXT("Press <Enter> to calibrate laser planes.\n\n")); MosGetch(); // Sheet-of-Light (laser plane) calibration. const MIL_INT NUM_REF_PLANES = 5; const MIL_DOUBLE CAL_MIN_CONTRAST [NUM_CAMERAS] = { 150, 150 }; const MIL_INT CAL_NB_REF_PLANES [NUM_CAMERAS] = { NUM_REF_PLANES, NUM_REF_PLANES }; const MIL_INT CAL_SCAN_ORIENTATION[NUM_CAMERAS] = { M_HORIZONTAL, M_HORIZONTAL }; const MIL_INT CAL_PEAK_WIDTH [NUM_CAMERAS] = { 4, 4 }; const MIL_INT CAL_PEAK_WIDTH_DELTA[NUM_CAMERAS] = { 3, 3 }; const MIL_INT LASER_LABELS [NUM_CAMERAS] = { 1, 1 }; const MIL_INT CAMERA_LABELS [NUM_CAMERAS] = { 1, 2 }; const MIL_DOUBLE PLANE_Z[NUM_CAMERAS][MAX_NB_REF_PLANES] = { { -5.05, -10.91, -16.77, -22.63, -28.49 }, { -5.05, -10.91, -16.77, -22.63, -28.49 } }; const SRefPlaneInfo LASER_CALIBRATION_PLANES[NUM_CAMERAS][MAX_NB_REF_PLANES] = { { // first camera // RefImageName Zs { EX_PATH("Cam1RefPlanes/Cam1_laserline_h0.mim"), PLANE_Z[0][0] }, { EX_PATH("Cam1RefPlanes/Cam1_laserline_h1.mim"), PLANE_Z[0][1] }, { EX_PATH("Cam1RefPlanes/Cam1_laserline_h2.mim"), PLANE_Z[0][2] }, { EX_PATH("Cam1RefPlanes/Cam1_laserline_h3.mim"), PLANE_Z[0][3] }, { EX_PATH("Cam1RefPlanes/Cam1_laserline_h4.mim"), PLANE_Z[0][4] } }, { // second camera // RefImageName Zs { EX_PATH("Cam2RefPlanes/Cam2_laserline_h0.mim"), PLANE_Z[1][0] }, { EX_PATH("Cam2RefPlanes/Cam2_laserline_h1.mim"), PLANE_Z[1][1] }, { EX_PATH("Cam2RefPlanes/Cam2_laserline_h2.mim"), PLANE_Z[1][2] }, { EX_PATH("Cam2RefPlanes/Cam2_laserline_h3.mim"), PLANE_Z[1][3] }, { EX_PATH("Cam2RefPlanes/Cam2_laserline_h4.mim"), PLANE_Z[1][4] } } }; const MIL_INT NUM_LASERS_PER_IMAGE = 1; SCameraLaserInfo LASER_CALIBRATION_INFO[NUM_CAMERAS * NUM_LASERS_PER_IMAGE]; for(MIL_INT c = 0; c < NUM_CAMERAS; c++) { SCameraLaserInfo& LCI = LASER_CALIBRATION_INFO[c]; LCI.NumLasersPerImage = NUM_LASERS_PER_IMAGE; LCI.NumRefPlanes = NUM_REF_PLANES; LCI.CalMinContrast = CAL_MIN_CONTRAST[c]; LCI.CalNbRefPlanes = CAL_NB_REF_PLANES[c]; LCI.CalScanOrientation = CAL_SCAN_ORIENTATION[c]; LCI.CalPeakWidthNominal= CAL_PEAK_WIDTH[c]; LCI.CalPeakWidthDelta = CAL_PEAK_WIDTH_DELTA[c]; for(MIL_INT l = 0; l < LCI.CalNbRefPlanes; l++) { LCI.LaserCalibrationPlanes[l] = LASER_CALIBRATION_PLANES[c][l]; } LCI.LaserLabel = LASER_LABELS[c]; LCI.CameraLabel = CAMERA_LABELS[c]; LCI.LineExtractionInROI = eLineNoROI; } //............................................................ // 2.1 Execute the calibration of the laser planes. // Generates the needed calibrated camera-laser pair contexts. MIL_ID CameraLaserCtxts[NUM_CAMERAS * NUM_LASERS_PER_IMAGE]; bool SheetOfLightOk = pExampleMngrFor3D->CalibrateSheetOfLight(&LASER_CALIBRATION_INFO[0], &CameraCalibrations[0], &CameraLaserCtxts[0]); if (SheetOfLightOk) { // Map generation specifications. const MIL_DOUBLE D3D_DISPLAY_REFRESH_PER_SEC = 2.0; // 3d Display FPS const MIL_DOUBLE D3D_DISPLAY_LOOK_AT_X = 90.16; const MIL_DOUBLE D3D_DISPLAY_LOOK_AT_Y = 128.86; const MIL_DOUBLE D3D_DISPLAY_LOOK_AT_Z = 48.35; const MIL_DOUBLE D3D_DISPLAY_EYE_DIST = 810.23; const MIL_DOUBLE D3D_DISPLAY_EYE_THETA = -52.14; const MIL_DOUBLE D3D_DISPLAY_EYE_PHI = 56.15; const MIL_INT CAMERA_MAP_MIN_CONTRAST[] = { 150, 150 }; const MIL_INT CAMERA_MAP_PEAK_WIDTH[] = { 10 , 10 }; const MIL_INT CAMERA_MAP_PEAK_DELTA[] = { 9 , 9 }; const MIL_DOUBLE CAMERA_MAP_SCAN_SPEED[] = { 0.3158, 0.3158 }; const MIL_DOUBLE CAMERA_MAX_FRAMES = 1024; const MIL_DOUBLE CAMERA_DISPLACEMENT_MODE = M_CURRENT; // Visualization volume information. SMapGeneration MapData; MapData.BoxCornerX = - 10.00; MapData.BoxCornerY = 0.00; MapData.BoxCornerZ = 1.00; MapData.BoxSizeX = 220.00; MapData.BoxSizeY = 220.00; MapData.BoxSizeZ = - 39.00; MapData.MapSizeX = 695; MapData.MapSizeY = 695; MapData.PixelSizeX = 0.317; MapData.PixelSizeY = 0.317; MapData.GrayScaleZ = (MapData.BoxSizeZ / 65534.0); MapData.IntensityMapType = 8 + M_UNSIGNED; MapData.SetExtractOverlap= true; MapData.ExtractOverlap = M_MIN; MapData.FillXThreshold = 1.0; MapData.FillYThreshold = 1.0; // Scan and analyze information. SPointCloudAcquisitionInfo SCAN_INFO = { // SD3DSysInfo { D3D_DISPLAY_REFRESH_PER_SEC, SHOW_NO_COLOR, D3D_DISPLAY_LOOK_AT_X, D3D_DISPLAY_LOOK_AT_Y, D3D_DISPLAY_LOOK_AT_Z, D3D_DISPLAY_EYE_DIST, D3D_DISPLAY_EYE_THETA, D3D_DISPLAY_EYE_PHI }, { CAMERA_MAP_MIN_CONTRAST[0] , CAMERA_MAP_MIN_CONTRAST[1] }, { CAMERA_MAP_PEAK_WIDTH[0] , CAMERA_MAP_PEAK_WIDTH[1] }, { CAMERA_MAP_PEAK_DELTA[0] , CAMERA_MAP_PEAK_DELTA[1] }, { CAMERA_MAP_SCAN_SPEED[0] , CAMERA_MAP_SCAN_SPEED[1] }, CAMERA_MAX_FRAMES, CAMERA_DISPLACEMENT_MODE, eLineNoROI, // SLineExtractionInROI { 0, 0, 0, 0 }, MapData, { // DigInfo // DigFormat SX SY SB Type NbFrames { EX_PATH("Cam1_cookies.avi"), 0, 0, 0, 0, 0 }, { EX_PATH("Cam2_cookies.avi"), 0, 0, 0, 0, 0 } }, MIL_TEXT("") // ScanDisplayText }; // Update some information from the sequences on disk. for(MIL_INT d = 0; d < NUM_CAMERAS; d++) { SCAN_INFO.DigInfo[d].UpdateInfoFromDisk(); } //.................................................... // 3. Acquire a 3d point cloud by scanning the object. // The point cloud container will hold one point cloud per camera-laser pair. MIL_ID PointCloudContainer = M_NULL; bool PointCloudOk = pExampleMngrFor3D->AcquirePointCloud(eScan, &SCAN_INFO, CameraLaserCtxts, &PointCloudContainer); //..................................................................................... // 4. Generate the depth map (orthogonal 2d-projection) of the acquired 3d point cloud. MIL_ID CookieBoxDepthmap = M_NULL; pExampleMngrFor3D->GenerateDepthMap(PointCloudContainer, SCAN_INFO.MapVisualizationData, &CookieBoxDepthmap); //.................................... // 5. Analyze the generated depth map. CCookieCounting ProbObj; pExampleMngrFor3D->AnalyzeDepthMap(&ProbObj, CookieBoxDepthmap); // Free camera-laser contexts. for (MIL_INT c = 0; c < NUM_CAMERAS; c++) { for (MIL_INT l = 0; l < NUM_LASERS_PER_IMAGE; l++) { MIL_ID& CameraLaserCtx = CameraLaserCtxts[(c*NUM_LASERS_PER_IMAGE) + l]; if (CameraLaserCtx != M_NULL) { M3dmapFree(CameraLaserCtx); CameraLaserCtx = M_NULL; } } } M3dmapFree(PointCloudContainer); if(CookieBoxDepthmap != M_NULL) { MbufFree(CookieBoxDepthmap); } } } else { // A problem occurred calibrating the cameras. MosPrintf(MIL_TEXT("Press <Enter> to end.\n\n")); MosGetch(); } // Free camera calibrations. for (MIL_INT c = 0; c < NUM_CAMERAS; c++) { if(CameraCalibrations[c] != M_NULL) { McalFree(CameraCalibrations[c]); CameraCalibrations[c] = M_NULL; } } delete pExampleMngrFor3D; pExampleMngrFor3D = NULL; // Free the MIL application. MappFree(MilApplication); return 0; } //******************************************************************************* // Function that analyzes the scanned object. //******************************************************************************* void CCookieCounting::Analyze(SCommonAnalysisObjects& CommonAnalysisObjects) { // Processing display zoom factor. const MIL_DOUBLE PROC_DISPLAY_ZOOM_FACTOR_X = 1; const MIL_DOUBLE PROC_DISPLAY_ZOOM_FACTOR_Y = 1; // Color specifications. const MIL_DOUBLE PROC_TEXT_PASS_COLOR = M_COLOR_DARK_GREEN; const MIL_DOUBLE PROC_TEXT_FAIL_COLOR = M_COLOR_RED; const MIL_DOUBLE MOD_BOX_COLOR = M_COLOR_CYAN; const MIL_DOUBLE MOD_EDGE_COLOR = M_COLOR_GREEN; const MIL_INT NUM_COOKIE_POSITIONS = 6; const MIL_INT COOKIE_RELATIVE_OFFSETS[NUM_COOKIE_POSITIONS][2] = { { 40, 42 }, { 267, 49 }, { 24, 230 }, { 258, 224 }, { 36, 424 }, { 269, 419 } }; const MIL_INT COOKIE_CHILD_SIZE[2] = { 75, 67 }; const MIL_DOUBLE COOKIE_HEIGHT = -10.0; const MIL_INT EXPECTED_NUM_COOKIES = 3; MIL_ID MilSystem = CommonAnalysisObjects.MilSystem; MIL_ID MilGraphics = CommonAnalysisObjects.MilGraphics; MIL_ID MilGraphicList = CommonAnalysisObjects.MilGraphicList; MIL_ID MilDepthMap = CommonAnalysisObjects.MilDepthMap; CMILDisplayManager* MilDisplayMngr = CommonAnalysisObjects.MilDisplays; // Disable graphics list update. MdispControl(MilDisplayMngr->GetDisplayID(), M_UPDATE_GRAPHIC_LIST, M_DISABLE); // Set the invalid data to 0. MbufClearCond(MilDepthMap, 0, 0, 0, MilDepthMap, M_EQUAL, 65535); // Setup the display. MgraClear(M_DEFAULT, MilGraphicList); MilDisplayMngr->Zoom(PROC_DISPLAY_ZOOM_FACTOR_X, PROC_DISPLAY_ZOOM_FACTOR_Y); // Allocate the necessary buffers for processing. MIL_ID MilRemapped8BitImage; MbufAlloc2d(MilSystem, MbufInquire(MilDepthMap, M_SIZE_X, M_NULL), MbufInquire(MilDepthMap, M_SIZE_Y, M_NULL), 8 + M_UNSIGNED, M_IMAGE + M_PROC + M_DISP, &MilRemapped8BitImage); MbufClear(MilRemapped8BitImage, 0); // Remap to 8 bit. MimShift(MilDepthMap, MilRemapped8BitImage, -8); MgraClear(M_DEFAULT, MilGraphicList); // Find the cookie box. MmodFind(m_MilModel, MilRemapped8BitImage, m_MilModelResult); MIL_INT NumOfOccurences = 0; MIL_INT PositionX, PositionY; // Get information on the find. MmodGetResult(m_MilModelResult, M_DEFAULT, M_NUMBER + M_TYPE_MIL_INT, &NumOfOccurences); MmodControl(m_MilModelResult, M_DEFAULT, M_RESULT_OUTPUT_UNITS, M_PIXEL); MmodGetResult(m_MilModelResult, M_DEFAULT, M_POSITION_X + M_TYPE_MIL_INT, &PositionX); MmodGetResult(m_MilModelResult, M_DEFAULT, M_POSITION_Y + M_TYPE_MIL_INT, &PositionY); if (NumOfOccurences >= 1) { // Draw the box if found. MgraControl(MilGraphics, M_BACKGROUND_MODE, M_OPAQUE); MgraControl(MilGraphics, M_FONT_SIZE, TEXT_FONT_SIZE_MEDIUM); MgraControl(MilGraphics, M_BACKGROUND_MODE, M_TRANSPARENT); MgraColor(MilGraphics, MOD_BOX_COLOR); MmodDraw(MilGraphics, m_MilModelResult, MilGraphicList, M_DRAW_BOX, M_ALL, M_DEFAULT); MgraColor(MilGraphics, MOD_EDGE_COLOR); MmodDraw(MilGraphics, m_MilModelResult, MilGraphicList, M_DRAW_EDGES, M_ALL, M_DEFAULT); MIL_DOUBLE DeviationMean = 0.0; MIL_INT NumCookies = 0; MIL_TEXT_CHAR CookieString[MAX_STRING_LEN]; MIL_TEXT_CHAR TempString[MAX_STRING_LEN]; for (MIL_INT i = 0; i < NUM_COOKIE_POSITIONS; i++) { // Create a child for each location of cookies in the box. MIL_ID CookieChild = MbufChild2d(MilDepthMap, PositionX+COOKIE_RELATIVE_OFFSETS[i][0], PositionY+COOKIE_RELATIVE_OFFSETS[i][1], COOKIE_CHILD_SIZE[0], COOKIE_CHILD_SIZE[1], M_NULL); // Calculate 3d statistics for each cookie location. M3dmapStat(CookieChild, M_NULL, M_NULL, M_NULL, M_DEVIATION_MEAN, M_DEFAULT, M_DEFAULT, &DeviationMean); // Determine the number of cookies according to the height. NumCookies = (MIL_INT)(DeviationMean/COOKIE_HEIGHT); if (NumCookies == EXPECTED_NUM_COOKIES) { MgraColor(MilGraphics, PROC_TEXT_PASS_COLOR); } else { MgraColor(MilGraphics, PROC_TEXT_FAIL_COLOR); } if (NumCookies == 1) { MosSprintf(TempString, MAX_STRING_LEN, MIL_TEXT("%s"), MIL_TEXT("")); } else { MosSprintf(TempString, MAX_STRING_LEN, MIL_TEXT("%s"), MIL_TEXT("s")); } MosSprintf(CookieString, MAX_STRING_LEN, MIL_TEXT("%d cookie%s"), NumCookies, TempString); MgraText(MilGraphics, MilGraphicList, PositionX+COOKIE_RELATIVE_OFFSETS[i][0]-10, TEXT_OFFSET_Y+PositionY+COOKIE_RELATIVE_OFFSETS[i][1], CookieString); MbufFree(CookieChild); } // Enable graphics list update. MdispControl(MilDisplayMngr->GetDisplayID(), M_UPDATE_GRAPHIC_LIST, M_ENABLE); // Show the result. MilDisplayMngr->Show(MilRemapped8BitImage); MosPrintf(MIL_TEXT("The number of cookies in each location has been ") MIL_TEXT("calculated using the height.\n")); MosPrintf(MIL_TEXT("Press <Enter> to end.\n\n")); MosGetch(); } else { // Enable graphics list update. MdispControl(MilDisplayMngr->GetDisplayID(), M_UPDATE_GRAPHIC_LIST, M_ENABLE); MosPrintf(MIL_TEXT("Could not find the cookie box.\n")); MosPrintf(MIL_TEXT("Press <Enter> to end.\n\n")); MosGetch(); } MbufFree(MilRemapped8BitImage); MilRemapped8BitImage = M_NULL; } //******************************************************************************* // Function that allocates processing objects. //******************************************************************************* void CCookieCounting::AllocProcessingObjects(MIL_ID MilSystem) { MmodAllocResult(MilSystem, M_DEFAULT, &m_MilModelResult); MmodRestore(COOKIE_BOX_MODEL, MilSystem, M_DEFAULT, &m_MilModel); // Associate the depth map calibration. McalRestore(DEPTH_MAP_CALIBRATION, MilSystem, M_DEFAULT, &m_MilDepthMapCalibration); MmodControl(m_MilModel, M_ALL, M_ASSOCIATED_CALIBRATION, m_MilDepthMapCalibration); // Preprocess the model. MmodPreprocess(m_MilModel, M_DEFAULT); } //******************************************************************************* // Frees processing objects. //******************************************************************************* void CCookieCounting::FreeProcessingObjects() { MmodFree(m_MilModel); m_MilModel = M_NULL; MmodFree(m_MilModelResult); m_MilModelResult = M_NULL; McalFree(m_MilDepthMapCalibration); m_MilDepthMapCalibration = M_NULL; }