| Click here to show toolbars of the Web Online Help System: show toolbars |
/*****************************************************************************/ /* * File name: SICKrangerC.cpp * Location: See Matrox Example Launcher in the MIL Control Center * * * Synopsis: Example demonstrating how to use a SICK Ranger camera to feed mil3dmap * module to generate depth maps. * * Copyright (C) Matrox Electronic Systems Ltd., 1992-2016. * All Rights Reserved */ #include "cameraconfig.h" /* User options - change them to suit your needs. */ #include <mil.h> #include "mil3dcam_sickrangerc.h" /* Prototypes for conversion routines. */ #include "sickcamera_rangerc.h" /* Prototypes to interact with Ranger camera. */ /***************************************************************************** Example description. *****************************************************************************/ void PrintHeader() { MosPrintf(MIL_TEXT("[EXAMPLE NAME]\n") MIL_TEXT("SICKrangerC\n\n") MIL_TEXT("[SYNOPSIS]\n") MIL_TEXT("This program calibrates a 3d reconstruction setup with a SICK\n") MIL_TEXT("Ranger C camera and scans an object to generate its depth map.\n\n") MIL_TEXT("[MODULES USED]\n") MIL_TEXT("Modules used: application, system, display, digitizer, buffer, graphic,\n") MIL_TEXT(" image processing, calibration, 3d reconstruction.\n\n") MIL_TEXT("Press <Enter> to start.\n\n")); MosGetch(); } /***************************************************************************** Constants. *****************************************************************************/ /* All the values below should be adjusted to suit your needs. */ #define MAX_DISPLAY_SIZE_X 1024 #define NB_DIG_PROCESS_BUFFERS 2 #define NB_GRABS 2 #define NB_SCANS_PER_GRAB 512 #define NB_SCANS_IN_RESULT (NB_SCANS_PER_GRAB*NB_GRABS) /* Calibration values for M_DEPTH_CORRECTION context type. */ #define NUMBER_OF_CALIBRATION_DEPTHS 5 static const double CORRECTED_DEPTHS[NUMBER_OF_CALIBRATION_DEPTHS] = {0.0, 12.0, 24.0, 36.0, 48.0}; /* calibration heights in mm. */ #define SCALE_FACTOR 1000.0 /* (depth in world units) * SCALE_FACTOR gives gray levels */ /* Calibration grid parameters. */ #define GRID_NB_ROWS 12 #define GRID_NB_COLS 13 #define GRID_ROW_SPACING 10.0 /* in mm */ #define GRID_COL_SPACING 10.0 /* in mm */ /* Depth map generation parameters. */ #define DEPTH_MAP_SIZE_X 512 /* in pixels */ #define DEPTH_MAP_SIZE_Y 220 /* in pixels */ #define GAP_DEPTH 1.5 /* in mm */ #define CONVEYOR_SPEED 20000.0 /* microns per second, 20cm/s. */ /* Filenames. */ #if !USE_REAL_CAMERA #define EXAMPLE_IMAGE_PATH M_IMAGE_PATH MIL_TEXT("SICKrangerC/") /* Images used for calibration in M_DEPTH_CORRECTION mode. */ static const MIL_TEXT_CHAR* RefPlaneFilenames[NUMBER_OF_CALIBRATION_DEPTHS] = { EXAMPLE_IMAGE_PATH MIL_TEXT("calibplane0.mim"), EXAMPLE_IMAGE_PATH MIL_TEXT("calibplane1.mim"), EXAMPLE_IMAGE_PATH MIL_TEXT("calibplane2.mim"), EXAMPLE_IMAGE_PATH MIL_TEXT("calibplane3.mim"), EXAMPLE_IMAGE_PATH MIL_TEXT("calibplane4.mim") }; /* Images used for calibration in M_CALIBRATED_CAMERA_LINEAR_MOTION mode. */ static const MIL_TEXT_CHAR* CalibGridFilename = EXAMPLE_IMAGE_PATH MIL_TEXT("CalibGrid.mim"); static const MIL_TEXT_CHAR* CalibLineFilename = EXAMPLE_IMAGE_PATH MIL_TEXT("CalibLine.mim"); /* Images used for runtime scan of an object (in both modes). */ static const MIL_TEXT_CHAR* ScanFilenames[NB_GRABS] = { EXAMPLE_IMAGE_PATH MIL_TEXT("scan0.mim"), EXAMPLE_IMAGE_PATH MIL_TEXT("scan1.mim") }; #endif /***************************************************************************** Functions declarations. *****************************************************************************/ /* Example function declaration. */ void RangerCExample(MIL_ID MilSystem, MIL_ID MilDisplay, MIL_INT ContextType); /* Helper function prototypes. */ void FatalError(const MIL_TEXT_CHAR* pMsg); void SwithToMeasure(MIL_ID MilSystem, icon::RangerC* pCam, MIL_INT SizeX, MIL_INT SizeY, MIL_ID* MilDigitizerPtr); void SwithToImage(MIL_ID MilSystem, icon::RangerC* pCam, MIL_ID* MilDigitizerPtr); void DisplayLaserLine(MIL_ID MilDisplay, MIL_ID MilPositionLine, MIL_ID MilIntensityLine, MIL_ID MilMeasImage, MIL_INT FixedPoint); void DisplayCalibrationPoints(MIL_ID MilDisplay, MIL_ID MilCalGridImage, MIL_ID MilCalibration); /* User's processing function prototype. */ MIL_INT MFTYPE AddScanProcFct(MIL_INT HookType, MIL_ID MilEvent, void* UserDataPtr); /* User's processing function hook data structure. */ struct AddScanProcFctDataStruct { MIL_ID MilLaser; MIL_ID MilScan; MIL_INT PointCloudLbl; MIL_ID MilPositionImage; MIL_ID MilIntensityImage; RangerParams* ConvParamsPtr; }; /***************************************************************************** Main. *****************************************************************************/ int MosMain(void) { /* Print example description. */ PrintHeader(); #if USE_REAL_CAMERA MIL_CONST_TEXT_PTR SystemDescriptor = M_SYSTEM_DEFAULT; #else MIL_CONST_TEXT_PTR SystemDescriptor = M_SYSTEM_HOST; #endif /* Allocate application, system and display. */ MIL_ID MilApplication = MappAlloc(M_NULL, M_DEFAULT, M_NULL); MIL_ID MilSystem = MsysAlloc(M_DEFAULT, SystemDescriptor, M_DEFAULT, M_DEFAULT, M_NULL); MIL_ID MilDisplay = MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, M_NULL); /* Run the depth correction example. */ RangerCExample(MilSystem, MilDisplay, M_DEPTH_CORRECTION); /* Run the calibrated camera example. */ RangerCExample(MilSystem, MilDisplay, M_CALIBRATED_CAMERA_LINEAR_MOTION); /* Free MIL objects. */ MdispFree(MilDisplay); MsysFree(MilSystem); MappFree(MilApplication); return 0; } /***************************************************************************** Function that calibrates a 3d reconstruction setup with a SICK Ranger C camera and scans an object to generate its depth map. *****************************************************************************/ void RangerCExample(MIL_ID MilSystem, MIL_ID MilDisplay, MIL_INT ContextType) { MIL_ID MilDigitizer, /* Digitizer for SICK camera. */ MilImage, /* Image buffer identifier (for continuous grab). */ MilMeasImage, /* Image buffer to hold image of meas. ROI (MimDraw).*/ MilCalGridImage, /* Image buffer to hold calibration grid image. */ MilTwoLinesImage, /* Image buffer to hold two laser lines. */ MilOneLineImage, /* Image buffer to hold one laser line. */ MilManyLinesImage[NB_DIG_PROCESS_BUFFERS], /* Image buffers to hold lines.*/ MilPositionImage, /* Image buffer to hold converted position results. */ MilIntensityImage, /* Image buffer to hold converted intensity results. */ MilPositionLine, /* Image buffer to hold one line of converted pos. */ MilIntensityLine, /* Image buffer to hold one line of converted int. */ MilCalibration, /* Calibration context identifier. */ MilDepthMap, /* Image buffer identifier (for depth map result) */ MilIntensityMap, /* Image buffer identifier (for intensity map result)*/ MilLaser, /* 3dmap laser profiling context identifier. */ MilCalibScan, /* 3dmap result buffer identifier for calibration. */ MilScan; /* 3dmap result buffer identifier for scanning. */ MIL_INT n, /* Counter. */ Ret; /* Return value (error code) of subfunctions. */ MosPrintf(MIL_TEXT("\n3D RECONSTRUCTION USING SICK RANGER C:\n")); MosPrintf(MIL_TEXT("------------------------------------\n\n")); MosPrintf(MIL_TEXT("This program calibrates a 3d reconstruction setup with a SICK\n")); MosPrintf(MIL_TEXT("Ranger C camera and scans an object to generate its depth map.\n\n")); if (ContextType == M_DEPTH_CORRECTION) MosPrintf(MIL_TEXT("3dmap M_DEPTH_CORRECTION mode is used.\n\n")); else MosPrintf(MIL_TEXT("3dmap M_CALIBRATED_CAMERA_LINEAR_MOTION mode is used.\n\n")); /* CONVEYOR_CODE: Initialize conveyor. */ /* Create a camera object and initialize it. */ icon::RangerC* pCam; Ret = CreateCamera(&pCam); if (Ret != SICK_CAMERA_OK) FatalError(MIL_TEXT("Unable to create camera object.\n")); Ret = InitCamera(pCam); if (Ret != SICK_CAMERA_OK) FatalError(MIL_TEXT("Unable to initialize camera.\n")); /* Inquire camera parameters and fill in conversion structure. */ RangerParams ConvParams; Ret = FillParams(pCam, &ConvParams, RANGER_C55_PRM, MEAS_COMP_NAME); if (Ret != SICK_CAMERA_OK) FatalError(MIL_TEXT("Unable to inquire camera parameters.\n")); #if USE_REAL_CAMERA MIL_INT GrabAttr = M_GRAB; #else MIL_INT GrabAttr = 0; #endif /* Allocate buffers to hold images returned by SICK camera in measurement mode. */ for (n = 0; n < NB_DIG_PROCESS_BUFFERS; ++n) { MbufAlloc2d(MilSystem, ConvParams.ProfileDataSizeByte, NB_SCANS_PER_GRAB, 8+M_UNSIGNED, M_IMAGE+M_PROC+GrabAttr, &MilManyLinesImage[n]); } /* Allocate buffers to hold images returned by SICK camera in image mode. */ MbufAlloc2d(MilSystem, ConvParams.ImageROISizeX, ConvParams.ImageROISizeY, 8+M_UNSIGNED, M_IMAGE+M_PROC+GrabAttr+M_DISP, &MilImage); MbufAlloc2d(MilSystem, ConvParams.ImageROISizeX, ConvParams.ImageROISizeY, 8+M_UNSIGNED, M_IMAGE+M_PROC+GrabAttr+M_DISP, &MilCalGridImage); /* Allocate buffers to hold images returned by MimDraw(), same size as meas. ROI. */ MIL_INT MimDrawSizeBit = (ConvParams.IntensitySizeBit == NO_INTENSITY_INFORMATION ? 8 : ConvParams.IntensitySizeBit); MbufAlloc2d(MilSystem, ConvParams.MeasROISizeX, ConvParams.MeasROISizeY, MimDrawSizeBit+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP, &MilMeasImage); /* Allocate position image returned by conversion routine and fed to 3dmap. */ MbufAlloc2d(M_DEFAULT_HOST, ConvParams.MeasROISizeX, NB_SCANS_PER_GRAB, ConvParams.RangeSizeBit+M_UNSIGNED, M_IMAGE+M_PROC, &MilPositionImage); /* * When calibrating, we won't need the full images since we want to grab only one * laser line. So, we create 1-line child buffers for SICK measurement output buffer and * for conversion routine buffers. However, because of a hardware limitation of the * Solios framegrabber, it is not possible to grab less than 2 lines, so we also create * a 2-lines child buffer for grabbing purposes. */ MbufChild2d(MilManyLinesImage[0], 0, 0, ConvParams.ProfileDataSizeByte, 2, &MilTwoLinesImage); MbufChild2d(MilManyLinesImage[0], 0, 0, ConvParams.ProfileDataSizeByte, 1, &MilOneLineImage ); MbufChild2d(MilPositionImage , 0, 0, ConvParams.MeasROISizeX , 1, &MilPositionLine ); /* If camera mode returns intensity information, we also allocate the related buffers. */ if (ConvParams.IntensitySizeBit != NO_INTENSITY_INFORMATION) { MbufAlloc2d(M_DEFAULT_HOST, ConvParams.MeasROISizeX, NB_SCANS_PER_GRAB, ConvParams.IntensitySizeBit+M_UNSIGNED, M_IMAGE+M_PROC, &MilIntensityImage); MbufChild2d(MilIntensityImage, 0, 0, ConvParams.MeasROISizeX, 1, &MilIntensityLine); } else { MilIntensityImage = M_NULL; MilIntensityLine = M_NULL; } /* Allocate 3dmap context and result, set some parameters. */ M3dmapAlloc(MilSystem, M_LASER, ContextType, &MilLaser); M3dmapControl(MilLaser, M_DEFAULT, M_EXTRACTION_FIXED_POINT, ConvParams.FixedPoint); if (ContextType == M_CALIBRATED_CAMERA_LINEAR_MOTION) { /* * Compute conveyor speed in mm/frame. * CONVEYOR_SPEED is in um/s, ConvParams.CycleTimeMicroseconds is in us/frame. * ConvParams.CycleTimeMicroseconds * CONVEYOR_SPEED is in 10^-12 m/frame */ double ConveyorSpeed = 1.0e-9 * ConvParams.CycleTimeMicroseconds * CONVEYOR_SPEED; M3dmapControl(MilLaser, M_DEFAULT, M_EXTRACTION_CHILD_OFFSET_X, ConvParams.ROIOffsetX); M3dmapControl(MilLaser, M_DEFAULT, M_EXTRACTION_CHILD_OFFSET_Y, ConvParams.ROIOffsetY); M3dmapControl(MilLaser, M_DEFAULT, M_SCAN_SPEED , ConveyorSpeed ); } M3dmapAllocResult(MilSystem, M_LASER_CALIBRATION_DATA, M_DEFAULT, &MilCalibScan); /* Set SICK camera in image mode. */ Ret = SwitchCameraMode(pCam, IMAGE_MODE_NAME); if (Ret != SICK_CAMERA_OK) FatalError(MIL_TEXT("Unable to switch camera to image mode.\n")); #if USE_REAL_CAMERA /* Allocate digitizer in area scan mode, for continuous grab. */ MdigAlloc(MilSystem, M_DEFAULT, IMAGE_DCF, M_DEFAULT, &MilDigitizer); /* SICK camera grabs flipped images. */ MdigControl(MilDigitizer, M_GRAB_DIRECTION_X, M_REVERSE); MosPrintf(MIL_TEXT("Adjust camera and laser position for calibration.\n\n")); #else MilDigitizer = M_NULL; #endif /* Display grabbed image. */ MbufClear(MilImage, 0.0); if (ConvParams.ImageROISizeX > MAX_DISPLAY_SIZE_X) MdispZoom(MilDisplay, 0.5, 0.5); MdispSelect(MilDisplay, MilImage); /* Calibration steps are different, depending on the 3dmap context type. */ if (ContextType == M_CALIBRATED_CAMERA_LINEAR_MOTION) { #if USE_REAL_CAMERA /* Grab a sequence of image until enter is pressed. */ MdigGrabContinuous(MilDigitizer, MilImage); MosPrintf(MIL_TEXT("Place calibration grid, switch laser OFF.\n")); MosPrintf(MIL_TEXT("Press <Enter> to grab.\n\n")); MosGetch(); /* Grab calibration grid image. */ MdigHalt(MilDigitizer); MbufCopy(MilImage, MilCalGridImage); /* Grab a sequence of image until enter is pressed. */ MdigGrabContinuous(MilDigitizer, MilImage); MosPrintf(MIL_TEXT("Switch laser ON.\n")); MosPrintf(MIL_TEXT("Press <Enter> to grab.\n\n")); MosGetch(); /* Switch digitizer to line-scan and camera to measurement mode. */ SwithToMeasure(MilSystem, pCam, ConvParams.ProfileDataSizeByte, 2, &MilDigitizer); /* Grab one laser line for 3d setup calibration. */ MdigGrab(MilDigitizer, MilTwoLinesImage); /* Switch digitizer back to area-scan and camera to image mode. */ SwithToImage(MilSystem, pCam, &MilDigitizer); #else /* Simply reload the images that would have been grabbed above. */ MbufLoad(CalibGridFilename, MilCalGridImage); MbufLoad(CalibLineFilename, MilOneLineImage); #endif /* Allocate calibration context in 3D mode. */ McalAlloc(MilSystem, M_TSAI_BASED, M_DEFAULT, &MilCalibration); /* Calibrate the camera. */ McalGrid(MilCalibration, MilCalGridImage, 0.0, 0.0, 0.0, GRID_NB_ROWS, GRID_NB_COLS, GRID_ROW_SPACING, GRID_COL_SPACING, M_DEFAULT, M_DEFAULT); if (McalInquire(MilCalibration, M_CALIBRATION_STATUS, M_NULL) != M_CALIBRATED) FatalError(MIL_TEXT("Camera calibration failed.\n")); /* Display McalGrid() results. */ DisplayCalibrationPoints(MilDisplay, MilCalGridImage, MilCalibration); /* Convert SICK format to MIL format. */ M3dcamRangerToMIL(MilOneLineImage, MilPositionLine, MilIntensityLine, M_NULL, &ConvParams); /* Display extracted laser line. */ DisplayLaserLine(MilDisplay, MilPositionLine, MilIntensityLine, MilMeasImage, ConvParams.FixedPoint); /* Add laser line informations. */ M3dmapAddScan(MilLaser, MilCalibScan, MilPositionLine, MilIntensityLine, M_NULL, M_DEFAULT, M_LINE_ALREADY_EXTRACTED); } else { /* ContextType == M_DEPTH_CORRECTION */ MilCalibration = M_NULL; for (n = 0; n < NUMBER_OF_CALIBRATION_DEPTHS; n++) { MosPrintf(MIL_TEXT("Current plane: %d (%4.2f mm).\n"), (int)n, CORRECTED_DEPTHS[n]); #if USE_REAL_CAMERA /* Grab a sequence of image until enter is pressed. */ MdigGrabContinuous(MilDigitizer, MilImage); MosPrintf(MIL_TEXT("Press <Enter> to grab.\n\n")); MosGetch(); /* Switch digitizer to line-scan and camera to measurement mode. */ SwithToMeasure(MilSystem, pCam, ConvParams.ProfileDataSizeByte, 2, &MilDigitizer); /* Grab one laser line on the reference plane. */ MdigGrab(MilDigitizer, MilTwoLinesImage); /* Switch digitizer back to area-scan and camera to image mode. */ SwithToImage(MilSystem, pCam, &MilDigitizer); #else /* Simply reload the images that would have been grabbed above. */ MbufLoad(RefPlaneFilenames[n], MilOneLineImage); #endif /* Set desired corrected depth of next reference plane. */ M3dmapControl(MilLaser, M_DEFAULT, M_CORRECTED_DEPTH, CORRECTED_DEPTHS[n]*SCALE_FACTOR); /* Convert SICK format to MIL format. */ M3dcamRangerToMIL(MilOneLineImage, MilPositionLine, MilIntensityLine, M_NULL, &ConvParams); /* Display extracted laser line. */ DisplayLaserLine(MilDisplay, MilPositionLine, MilIntensityLine, MilMeasImage, ConvParams.FixedPoint); /* Add laser line informations. */ M3dmapAddScan(MilLaser, MilCalibScan, MilPositionLine, MilIntensityLine, M_NULL, M_DEFAULT, M_LINE_ALREADY_EXTRACTED); } } /* Calibrate the 3d reconstruction context. */ M3dmapCalibrate(MilLaser, MilCalibScan, MilCalibration, M_DEFAULT); if (M3dmapInquire(MilLaser, M_DEFAULT, M_CALIBRATION_STATUS, M_NULL) != M_CALIBRATED) FatalError(MIL_TEXT("Laser calibration failed.\n")); MosPrintf(MIL_TEXT("The 3d reconstruction setup has been calibrated.\n\n")); /* Free result buffer used for laser line calibration. */ M3dmapFree(MilCalibScan); MilCalibScan = M_NULL; /* Allocate result buffer for scanning. */ MIL_INT ScanObjType = M_NULL; MIL_INT PointCloudLbl = M_NULL; if(ContextType == M_DEPTH_CORRECTION) { ScanObjType = M_DEPTH_CORRECTED_DATA; PointCloudLbl = M_DEFAULT; } else { ScanObjType = M_POINT_CLOUD_CONTAINER; PointCloudLbl = M_POINT_CLOUD_LABEL(1); } M3dmapAllocResult(MilSystem, ScanObjType, M_DEFAULT, &MilScan); M3dmapControl(MilScan, M_DEFAULT, M_MAX_FRAMES, NB_SCANS_IN_RESULT); #if USE_REAL_CAMERA /* Grab a sequence of image until ENTER is pressed. */ MdigGrabContinuous(MilDigitizer, MilImage); MosPrintf(MIL_TEXT("Adjust object position for scanning.\n")); MosPrintf(MIL_TEXT("Press <Enter> to grab.\n\n")); MosGetch(); MosPrintf(MIL_TEXT("The object is being scanned.\n\n")); /* Switch digitizer to line-scan and camera to measurement mode. */ SwithToMeasure(MilSystem, pCam, ConvParams.ProfileDataSizeByte, NB_SCANS_PER_GRAB, &MilDigitizer); /* CONVEYOR_CODE: Start conveyor and wait for its speed to become constant. */ /* Fill MdigProcess hook function structure. */ AddScanProcFctDataStruct AddScanProcFctData; AddScanProcFctData.MilLaser = MilLaser; AddScanProcFctData.MilScan = MilScan; AddScanProcFctData.PointCloudLbl = PointCloudLbl; AddScanProcFctData.MilPositionImage = MilPositionImage; AddScanProcFctData.MilIntensityImage = MilIntensityImage; AddScanProcFctData.ConvParamsPtr = &ConvParams; /* Grab laser line data with buffering. */ MdigProcess(MilDigitizer, MilManyLinesImage, NB_DIG_PROCESS_BUFFERS, M_SEQUENCE+M_COUNT(NB_GRABS), M_SYNCHRONOUS, &AddScanProcFct, &AddScanProcFctData); /* CONVEYOR_CODE: Stop conveyor. */ #else for (n = 0; n < NB_GRABS; ++n) { /* Reload profile data generated by SICK camera. */ MbufLoad(ScanFilenames[n], MilManyLinesImage[0]); /* Convert SICK format to MIL format. */ M3dcamRangerToMIL(MilManyLinesImage[0], MilPositionImage, MilIntensityImage, M_NULL, &ConvParams); /* Analyze the image to extract laser line and correct its depth. */ M3dmapAddScan(MilLaser, MilScan, MilPositionImage, MilIntensityImage, M_NULL, PointCloudLbl, M_LINE_ALREADY_EXTRACTED); } #endif /* Set depth map generation parameters according to 3dmap context type. */ MIL_INT DepthMapSizeX, DepthMapSizeY, DepthMapType, IntensityMapType; M3dmapControl(MilScan, M_DEFAULT, M_FILL_MODE , M_X_THEN_Y); M3dmapControl(MilScan, M_DEFAULT, M_FILL_SHARP_ELEVATION , M_MIN ); M3dmapControl(MilScan, M_DEFAULT, M_FILL_SHARP_ELEVATION_DEPTH, GAP_DEPTH ); if (ContextType == M_CALIBRATED_CAMERA_LINEAR_MOTION) { M3dmapControl(MilScan, M_DEFAULT, M_BOUNDING_BOX_ALGORITHM, M_ROBUST); M3dmapControl(MilScan, M_DEFAULT, M_AUTO_SCALE_XY, M_CLIP); M3dmapSetBox(MilScan, M_EXTRACTION_BOX, M_BOUNDING_BOX, M_ALL, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT); DepthMapSizeX = DEPTH_MAP_SIZE_X; DepthMapSizeY = DEPTH_MAP_SIZE_Y; DepthMapType = 16+M_UNSIGNED; } else { M3dmapGetResult(MilScan, M_DEFAULT, M_CORRECTED_DEPTH_MAP_SIZE_X +M_TYPE_MIL_INT, &DepthMapSizeX ); M3dmapGetResult(MilScan, M_DEFAULT, M_CORRECTED_DEPTH_MAP_SIZE_Y +M_TYPE_MIL_INT, &DepthMapSizeY ); M3dmapGetResult(MilScan, M_DEFAULT, M_CORRECTED_DEPTH_MAP_BUFFER_TYPE+M_TYPE_MIL_INT, &DepthMapType ); } /* Allocate images for depth map. */ MbufAlloc2d(MilSystem, DepthMapSizeX, DepthMapSizeY, DepthMapType , M_IMAGE+M_PROC+M_DISP, &MilDepthMap ); if (ConvParams.IntensitySizeBit != NO_INTENSITY_INFORMATION) { M3dmapGetResult(MilScan, PointCloudLbl, M_INTENSITY_MAP_BUFFER_TYPE+M_TYPE_MIL_INT, &IntensityMapType); MbufAlloc2d(MilSystem, DepthMapSizeX, DepthMapSizeY, IntensityMapType, M_IMAGE+M_PROC+M_DISP, &MilIntensityMap); } else { MilIntensityMap = M_NULL; } /* Get depth map from accumulated information in the result buffer. */ M3dmapExtract(MilScan, MilDepthMap, MilIntensityMap, M_CORRECTED_DEPTH_MAP, M_DEFAULT, M_DEFAULT); /* Display corrected depth map. */ if (DepthMapType != 8+M_UNSIGNED) MdispControl(MilDisplay, M_VIEW_MODE, M_AUTO_SCALE); if (DepthMapSizeX > MAX_DISPLAY_SIZE_X) MdispZoom(MilDisplay, 0.5, 0.5); else MdispZoom(MilDisplay, 1.0, 1.0); MdispSelect(MilDisplay, MilDepthMap); MosPrintf(MIL_TEXT("The corrected depth map of the object is being displayed.\n")); MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n")); MosGetch(); /* Reset display. */ MdispSelect(MilDisplay, M_NULL); MdispControl(MilDisplay, M_VIEW_MODE, M_DEFAULT); MdispZoom(MilDisplay, 1.0, 1.0); /* Free all allocations. */ if (MilIntensityLine != M_NULL) { MbufFree(MilIntensityLine); MbufFree(MilIntensityImage); } CloseDown(pCam); if (MilIntensityMap != M_NULL) MbufFree(MilIntensityMap); MbufFree(MilDepthMap); if (MilCalibration != M_NULL) McalFree(MilCalibration); M3dmapFree(MilScan); M3dmapFree(MilLaser); MbufFree(MilPositionLine); MbufFree(MilPositionImage); MbufFree(MilMeasImage); MbufFree(MilCalGridImage); MbufFree(MilImage); MbufFree(MilOneLineImage); MbufFree(MilTwoLinesImage); for (n = 0; n < NB_DIG_PROCESS_BUFFERS; ++n) MbufFree(MilManyLinesImage[n]); if (MilDigitizer != M_NULL) MdigFree(MilDigitizer); } /***************************************************************************** Hook function for MdigProcess(). Converts the profile data returned by the Ranger camera and feed it to 3dmap. *****************************************************************************/ MIL_INT MFTYPE AddScanProcFct(MIL_INT HookType, MIL_ID MilEvent, void* UserDataPtr) { MIL_ID MilModifiedBuffer; AddScanProcFctDataStruct* pAddScanProcFctData = static_cast<AddScanProcFctDataStruct*>(UserDataPtr); MdigGetHookInfo(MilEvent, M_MODIFIED_BUFFER+M_BUFFER_ID, &MilModifiedBuffer); /* Convert SICK format to MIL format. */ M3dcamRangerToMIL(MilModifiedBuffer, pAddScanProcFctData->MilPositionImage, pAddScanProcFctData->MilIntensityImage, M_NULL, pAddScanProcFctData->ConvParamsPtr); /* Analyze the image to extract laser line and correct its depth. */ M3dmapAddScan(pAddScanProcFctData->MilLaser, pAddScanProcFctData->MilScan, pAddScanProcFctData->MilPositionImage, pAddScanProcFctData->MilIntensityImage, M_NULL, pAddScanProcFctData->PointCloudLbl, M_LINE_ALREADY_EXTRACTED); return M_NULL; } /***************************************************************************** Halts digitizer from continuous grab, reallocates it with line-scan DCF and switch SICK Ranger C camera to measurement mode. MilSystem (in) ID of the system on which the digitizer is allocated. pCam (in/out) Pointer to camera object. SizeX (in) X size of the image to grab. SizeY (in) Y size of the image to grab. MilDigitizerPtr (in/out) Pointer to allocated digitizer, to reallocate. *****************************************************************************/ void SwithToMeasure(MIL_ID MilSystem, icon::RangerC* pCam, MIL_INT SizeX, MIL_INT SizeY, MIL_ID* MilDigitizerPtr) { MIL_INT Ret; /* * Stop grabbing data, MdigHalt() function will wait for the end of the current * frame before returning. If Camera is stopped when executing this command the * program will wait forever... so we add a sleep here. */ MdigHalt(*MilDigitizerPtr); MosSleep(ONE_FRAME_DELAY_MS); MdigFree(*MilDigitizerPtr); /* Switch camera to measurement mode. */ Ret = SwitchCameraMode(pCam, MEASUREMENT_MODE_NAME); if (Ret != SICK_CAMERA_OK) FatalError(MIL_TEXT("Unable to switch camera to measurement mode.\n")); /* Switch digitizer DCF to line-scan mode. */ MdigAlloc(MilSystem, M_DEFAULT, MEAS_DCF, M_DEFAULT, MilDigitizerPtr); MdigControl(*MilDigitizerPtr, M_SOURCE_SIZE_X, SizeX); MdigControl(*MilDigitizerPtr, M_SOURCE_SIZE_Y, SizeY); MdigControl(*MilDigitizerPtr, M_GRAB_TIMEOUT, GRAB_TIMEOUT_MS); } /***************************************************************************** Reallocates digitizre with area-scan DCF and switch SICK Ranger C camera to measurement mode. MilSystem (in) ID of the system on which the digitizer is allocated. pCam (in/out) Pointer to camera object. MilDigitizerPtr (in/out) Pointer to allocated digitizer, to reallocate. *****************************************************************************/ void SwithToImage(MIL_ID MilSystem, icon::RangerC* pCam, MIL_ID* MilDigitizerPtr) { MIL_INT Ret; MdigFree(*MilDigitizerPtr); /* Switch camera back to image mode. */ Ret = SwitchCameraMode(pCam, IMAGE_MODE_NAME); if (Ret != SICK_CAMERA_OK) FatalError(MIL_TEXT("Unable to switch camera to image mode.\n")); /* Switch digitizer DCF back to area-scan mode. */ MdigAlloc(MilSystem, M_DEFAULT, IMAGE_DCF, M_DEFAULT, MilDigitizerPtr); /* SICK camera grabs flipped images. */ MdigControl(*MilDigitizerPtr, M_GRAB_DIRECTION_X, M_REVERSE); } /***************************************************************************** Uses MimDraw() to show the image of the laser line the SICK Ranger C has extracted. MilDisplay (in) ID of the MIL display to use. MilPositionLine (in) Single-line range image (output of M3dcamRangerToMIL()). MilIntensityLine (in) Single-line intensity image (output of M3dcamRangerToMIL()) or M_NULL MilMeasImage (out) Temporary image to use to display laser line. FixedPoint (in) Fixed point used for MilPositionLine (taken from RangerParams struct). *****************************************************************************/ void DisplayLaserLine(MIL_ID MilDisplay, MIL_ID MilPositionLine, MIL_ID MilIntensityLine, MIL_ID MilMeasImage, MIL_INT FixedPoint) { MIL_ID MilPreviousImage; MdispInquire(MilDisplay, M_SELECTED, &MilPreviousImage); MbufClear(MilMeasImage, 0.0); MimDraw(M_DEFAULT, MilPositionLine, MilIntensityLine, MilMeasImage, M_DRAW_PEAKS+M_1D_COLUMNS+M_LINES, 0, 2.0, M_FIXED_POINT+FixedPoint); if (MbufInquire(MilMeasImage, M_SIZE_BIT, M_NULL) != 8) MdispControl(MilDisplay, M_VIEW_MODE, M_AUTO_SCALE); MdispSelect(MilDisplay, MilMeasImage); MosPrintf(MIL_TEXT("The extracted laser line is displayed.\n")); MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n")); MosGetch(); MdispControl(MilDisplay, M_VIEW_MODE, M_DEFAULT); MdispSelect(MilDisplay, MilPreviousImage); } /***************************************************************************** Uses McalDraw() to show the camera calibration grid with crosses on the calibration points in the overlay. MilDisplay (in) ID of the MIL display to use. MilCalGridImage (in) Image of the calibration grid. MilCalibration (in) Calibration object. *****************************************************************************/ void DisplayCalibrationPoints(MIL_ID MilDisplay, MIL_ID MilCalGridImage, MIL_ID MilCalibration) { /* Backup display settings. */ MIL_ID MilPreviousImage; MIL_DOUBLE PreviousZoom, PreviousColor; MdispInquire(MilDisplay, M_SELECTED, &MilPreviousImage); MdispInquire(MilDisplay, M_ZOOM_FACTOR_X, &PreviousZoom); MgraInquire(M_DEFAULT, M_COLOR, &PreviousColor); /* Prepare overlay buffer to draw calibration points. */ MIL_ID MilOverlay; MdispSelect(MilDisplay, M_NULL); MdispZoom(MilDisplay, 1.0, 1.0); MdispSelect(MilDisplay, MilCalGridImage); MdispControl(MilDisplay, M_OVERLAY, M_ENABLE); MdispInquire(MilDisplay, M_OVERLAY_ID, &MilOverlay); MdispControl(MilDisplay, M_OVERLAY_CLEAR, M_DEFAULT); /* Draw calibration points. */ MgraColor(M_DEFAULT, M_COLOR_GREEN); McalDraw(M_DEFAULT, MilCalibration, MilOverlay, M_DRAW_WORLD_POINTS, M_DEFAULT, M_DEFAULT); MosPrintf(MIL_TEXT("Camera calibration points are being displayed.\n")); MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n")); MosGetch(); /* Restore previous settings. */ MgraColor(M_DEFAULT, PreviousColor); MdispSelect(MilDisplay, M_NULL); MdispControl(MilDisplay, M_OVERLAY_CLEAR, M_DEFAULT); MdispControl(MilDisplay, M_OVERLAY, M_DISABLE); MdispZoom(MilDisplay, PreviousZoom, PreviousZoom); MdispSelect(MilDisplay, MilPreviousImage); } /***************************************************************************** Simple function that terminates the program abruptly when an error occurs. *****************************************************************************/ void FatalError(const MIL_TEXT_CHAR* pMsg) { MosPrintf(pMsg); MosPrintf(MIL_TEXT("\nPress <Enter> to end.\n")); MosGetch(); exit(-1); }