#include <mil.h>
#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE
#define USE_D3D_DISPLAY 1
#else
#define USE_D3D_DISPLAY 0
#endif
#if USE_D3D_DISPLAY
#include "MdispD3D.h"
#endif
void PrintHeader()
{
MosPrintf(MIL_TEXT("[EXAMPLE NAME]\n")
MIL_TEXT("MatroxGatorEye\n\n")
MIL_TEXT("[SYNOPSIS]\n")
MIL_TEXT("This programs calibrates a 3d reconstruction setup, which consists of a\n")
MIL_TEXT("Matrox GatorEye camera and a laser. It then scans an object to generate\n")
MIL_TEXT("its depth map. The GatorEye hardware is used to extract the laser line.\n\n")
MIL_TEXT("Note: this example can be compiled to either acquire images using a Matrox\n")
MIL_TEXT("GatorEye camera, or reload images from disk.\n\n")
MIL_TEXT("[MODULES USED]\n")
MIL_TEXT("Modules used: application, system, display, digitizer, buffer, graphic,\n")
MIL_TEXT(" calibration, 3d reconstruction.\n\n")
MIL_TEXT("Press <Enter> to start.\n\n"));
MosGetch();
}
static const bool USE_REAL_CAMERA = false;
static MIL_CONST_TEXT_PTR SYSTEM_DESCRIPTOR = (USE_REAL_CAMERA ? M_SYSTEM_DEFAULT : M_SYSTEM_HOST);
static const MIL_INT NB_DIG_PROCESS_BUFFERS = 2;
static const MIL_INT NB_REFERENCE_PLANES = 5;
static const MIL_INT NB_LASER_GRABS = 6;
static const MIL_TEXT_CHAR* const SEPARATOR = MIL_TEXT("--------------------\n\n");
#define EXAMPLE_IMAGE_PATH M_IMAGE_PATH MIL_TEXT("MatroxGatorEye/")
static const MIL_INT MAX_FILENAME_LEN = 256;
static const MIL_TEXT_CHAR* const CALIBRATION_GRID_FILE = EXAMPLE_IMAGE_PATH MIL_TEXT("CalibGrid.mim");
static const MIL_TEXT_CHAR* const FULL_MODE_CALIBRATION_LINE_FILE = EXAMPLE_IMAGE_PATH MIL_TEXT("CalibLine.mim");
static const MIL_TEXT_CHAR* const DEPTH_ONLY_CALIBRATION_LINE_FILENAME_PATTERN = EXAMPLE_IMAGE_PATH MIL_TEXT("CalibLine%d.mim");
static const MIL_TEXT_CHAR* const LASER_GRAB_FILENAME_PATTERN = EXAMPLE_IMAGE_PATH MIL_TEXT("Scan%d.mim");
static const MIL_DOUBLE GATOREYE_EXPOSURE_TIME = 17000.0;
static const MIL_INT32 GATOREYE_FIXED_POINT = 5;
static const MIL_INT32 GATOREYE_MIN_INTENSITY = 80;
static const MIL_INT32 GATOREYE_PEAK_WIDTH = 10;
static const bool GATOREYE_OUTPUT_INTENSITY = true;
static const MIL_TEXT_PTR GATOREYE_FORMAT = (GATOREYE_OUTPUT_INTENSITY ? MIL_TEXT("PositionAndIntensity") : MIL_TEXT("Position"));
static const MIL_INT FULL_IMAGE_OFFSET_X = 0;
static const MIL_INT FULL_IMAGE_OFFSET_Y = 0;
static const MIL_INT FULL_IMAGE_SIZE_X = 1280;
static const MIL_INT FULL_IMAGE_SIZE_Y = 960;
static const MIL_INT LASER_ROI_OFFSET_X = 0;
static const MIL_INT LASER_ROI_OFFSET_Y = 416;
static const MIL_INT LASER_ROI_SIZE_X = 1280;
static const MIL_INT LASER_ROI_SIZE_Y = 320;
static const MIL_INT GRAB_ATTR = (USE_REAL_CAMERA ? M_GRAB : 0);
static const MIL_INT NB_BYTES_PER_PIXEL = (GATOREYE_OUTPUT_INTENSITY ? 3 : 2);
static const MIL_INT MAX_NB_SCANS_PER_GRAB = LASER_ROI_SIZE_Y / NB_BYTES_PER_PIXEL;
static const MIL_INT NB_TOTAL_LASER_LINES = NB_LASER_GRABS * MAX_NB_SCANS_PER_GRAB;
static const MIL_INT ROW_NUMBER = 15;
static const MIL_INT COLUMN_NUMBER = 16;
static const MIL_DOUBLE ROW_SPACING = 5.0;
static const MIL_DOUBLE COLUMN_SPACING = 5.0;
static const MIL_INT GRID_TYPE = M_CIRCLE_GRID;
static const MIL_DOUBLE CORRECTED_DEPTHS[NB_REFERENCE_PLANES] = {0.0, 12.0, 24.0, 36.0, 48.0};
static const MIL_DOUBLE SCALE_FACTOR = 1000.0;
static const MIL_INT DEPTH_MAP_SIZE_X = 640;
static const MIL_INT DEPTH_MAP_SIZE_Y = 480;
static const MIL_DOUBLE GAP_DEPTH = 1.5;
static const MIL_DOUBLE CONVEYOR_SPEED = 9.3;
static const MIL_INT D3D_DISPLAY_SIZE_X = 800;
static const MIL_INT D3D_DISPLAY_SIZE_Y = 800;
void GatorEyeExample(MIL_ID MilSystem, MIL_ID MilDisplay, MIL_ID MilDigitizer, MIL_INT ContextType);
bool Initialize(MIL_ID MilSystem ,
MIL_ID MilDisplay ,
MIL_ID MilDigitizer ,
MIL_INT ContextType ,
MIL_ID* pMilCameraImage,
MIL_ID* pMilLaser ,
MIL_ID* pMilScan );
bool Calibrate(MIL_ID MilSystem ,
MIL_ID MilDisplay ,
MIL_ID MilDigitizer ,
MIL_ID MilCameraImage,
MIL_ID MilLaser ,
MIL_ID MilScan );
bool Scan(MIL_ID MilSystem ,
MIL_ID MilDisplay ,
MIL_ID MilDigitizer ,
MIL_ID MilCameraImage,
MIL_ID MilLaser ,
MIL_ID MilScan );
bool ShowResults(MIL_ID MilSystem, MIL_ID MilDisplay, MIL_ID MilScan);
void SwitchToLaserExtraction(MIL_ID MilDigitizer, MIL_INT64 NbScansPerGrab);
void SwitchToImage(MIL_ID MilDigitizer);
bool CalibrateCamera(MIL_ID MilSystem ,
MIL_ID MilDisplay ,
MIL_ID MilDigitizer ,
MIL_ID MilCameraImage ,
MIL_ID* pMilCalibration);
void AddLaserLinesFullMode(MIL_ID MilSystem ,
MIL_ID MilDisplay ,
MIL_ID MilDigitizer ,
MIL_ID MilCameraImage,
MIL_ID MilLaser ,
MIL_ID MilScan );
void AddLaserLinesDepthOnly(MIL_ID MilSystem ,
MIL_ID MilDisplay ,
MIL_ID MilDigitizer ,
MIL_ID MilCameraImage,
MIL_ID MilLaser ,
MIL_ID MilScan );
void DisplayLaserLine(MIL_ID MilSystem, MIL_ID MilDisplay, MIL_ID MilScan);
void AllocateBuffersForLaserExtraction(MIL_ID MilSystem ,
MIL_INT LaserROISizeX ,
MIL_INT NbLaserLinesToExtract,
MIL_INT NbGrabBuffers ,
MIL_ID* MilGrabImageArray ,
MIL_ID* MilNoPaddingImagePtr ,
MIL_ID* MilPositionImagePtr ,
MIL_ID* MilIntensityImagePtr );
void FreeBuffersForLaserExtraction(MIL_INT NbGrabBuffers ,
MIL_ID* MilGrabImageArray,
MIL_ID MilNoPaddingImage,
MIL_ID MilPositionImage ,
MIL_ID MilIntensityImage);
MIL_DOUBLE GetCameraFrameRate(MIL_ID MilDigitizer);
struct AddScanProcFctDataStruct
{
MIL_ID MilLaser;
MIL_ID MilScan;
MIL_ID MilGrabImageArray[NB_DIG_PROCESS_BUFFERS];
MIL_ID MilNoPaddingImage;
MIL_ID MilPositionImage;
MIL_ID MilIntensityImage;
};
MIL_INT MFTYPE AddScanProcFct(MIL_INT HookType, MIL_ID MilEvent, void* UserDataPtr);
void AddGatorEyeScan(const AddScanProcFctDataStruct* pAddScanProcFctData, MIL_INT BufIndex);
int MosMain(void)
{
PrintHeader();
MIL_ID MilApplication = MappAlloc(M_NULL, M_DEFAULT, M_NULL);
MIL_ID MilSystem = MsysAlloc(M_DEFAULT, SYSTEM_DESCRIPTOR, M_DEFAULT, M_DEFAULT, M_NULL);
MIL_ID MilDisplay = MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, M_NULL);
MIL_ID MilDigitizer = M_NULL;
if (USE_REAL_CAMERA)
{
MdigAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_DEFAULT, &MilDigitizer);
MdigControl(MilDigitizer, M_GRAB_TIMEOUT, 50000);
MdigControlFeature(MilDigitizer, M_FEATURE_VALUE, MIL_TEXT("ExposureTime"), M_TYPE_DOUBLE, &GATOREYE_EXPOSURE_TIME);
MdigControlFeature(MilDigitizer, M_FEATURE_VALUE , MIL_TEXT("LocatePeakFixedPointPosition"), M_TYPE_MIL_INT32 , &GATOREYE_FIXED_POINT );
MdigControlFeature(MilDigitizer, M_FEATURE_VALUE , MIL_TEXT("LocatePeakMinIntensity" ), M_TYPE_MIL_INT32 , &GATOREYE_MIN_INTENSITY);
MdigControlFeature(MilDigitizer, M_FEATURE_VALUE , MIL_TEXT("LocatePeakWidth" ), M_TYPE_MIL_INT32 , &GATOREYE_PEAK_WIDTH );
MdigControlFeature(MilDigitizer, M_FEATURE_VALUE_AS_STRING, MIL_TEXT("LocatePeakFormat" ), M_TYPE_ENUMERATION, GATOREYE_FORMAT );
}
MosPrintf(MIL_TEXT("M_DEPTH_CORRECTION 3d reconstruction mode\n")
MIL_TEXT("=========================================\n\n")
MIL_TEXT("For this calibration mode, the following steps will be executed:\n")
MIL_TEXT(" - Calibrating the 3d reconstruction setup.\n")
MIL_TEXT(" - Scanning the object.\n\n"));
GatorEyeExample(MilSystem, MilDisplay, MilDigitizer, M_DEPTH_CORRECTION);
MosPrintf(MIL_TEXT("M_CALIBRATED_CAMERA_LINEAR_MOTION 3d reconstruction mode\n")
MIL_TEXT("========================================================\n\n")
MIL_TEXT("For this calibration mode, the following steps will be executed:\n")
MIL_TEXT(" - Calibrating the camera.\n")
MIL_TEXT(" - Calibrating the 3d reconstruction setup.\n")
MIL_TEXT(" - Scanning the object.\n\n"));
GatorEyeExample(MilSystem, MilDisplay, MilDigitizer, M_CALIBRATED_CAMERA_LINEAR_MOTION);
if (MilDigitizer != M_NULL)
MdigFree(MilDigitizer);
MdispFree(MilDisplay);
MsysFree(MilSystem);
MappFree(MilApplication);
return 0;
}
void GatorEyeExample(MIL_ID MilSystem, MIL_ID MilDisplay, MIL_ID MilDigitizer, MIL_INT ContextType)
{
bool EverythingIsOK;
MIL_ID MilCameraImage, MilLaser, MilScan;
EverythingIsOK = Initialize(MilSystem, MilDisplay, MilDigitizer, ContextType,
&MilCameraImage, &MilLaser, &MilScan);
if (EverythingIsOK)
EverythingIsOK = Calibrate(MilSystem, MilDisplay, MilDigitizer, MilCameraImage, MilLaser, MilScan);
if (EverythingIsOK)
EverythingIsOK = Scan(MilSystem, MilDisplay, MilDigitizer, MilCameraImage, MilLaser, MilScan);
if (EverythingIsOK)
EverythingIsOK = ShowResults(MilSystem, MilDisplay, MilScan);
M3dmapFree(MilScan);
M3dmapFree(MilLaser);
MbufFree(MilCameraImage);
}
bool Initialize(MIL_ID MilSystem ,
MIL_ID MilDisplay ,
MIL_ID MilDigitizer ,
MIL_INT ContextType ,
MIL_ID* pMilCameraImage,
MIL_ID* pMilLaser ,
MIL_ID* pMilScan )
{
bool EverythingIsOK = true;
MbufAlloc2d(MilSystem, FULL_IMAGE_SIZE_X, FULL_IMAGE_SIZE_Y,
8+M_UNSIGNED, M_IMAGE+M_PROC+GRAB_ATTR+M_DISP, pMilCameraImage);
M3dmapAlloc(MilSystem, M_LASER, ContextType, pMilLaser);
M3dmapControl(*pMilLaser, M_DEFAULT, M_EXTRACTION_FIXED_POINT, GATOREYE_FIXED_POINT);
if (ContextType == M_CALIBRATED_CAMERA_LINEAR_MOTION)
{
MIL_DOUBLE FrameRate = GetCameraFrameRate(MilDigitizer);
MIL_DOUBLE ScanSpeed = -CONVEYOR_SPEED / FrameRate;
M3dmapControl(*pMilLaser, M_DEFAULT, M_SCAN_SPEED, ScanSpeed);
M3dmapControl(*pMilLaser, M_DEFAULT, M_EXTRACTION_CHILD_OFFSET_X, LASER_ROI_OFFSET_X-FULL_IMAGE_OFFSET_X);
M3dmapControl(*pMilLaser, M_DEFAULT, M_EXTRACTION_CHILD_OFFSET_Y, LASER_ROI_OFFSET_Y-FULL_IMAGE_OFFSET_Y);
}
M3dmapAllocResult(MilSystem, M_LASER_DATA, M_DEFAULT, pMilScan);
M3dmapControl(*pMilScan, M_DEFAULT, M_MAX_FRAMES, NB_TOTAL_LASER_LINES);
SwitchToImage(MilDigitizer);
MbufClear(*pMilCameraImage, 0.0);
if (USE_REAL_CAMERA)
MdispSelect(MilDisplay, *pMilCameraImage);
return EverythingIsOK;
}
bool Calibrate(MIL_ID MilSystem ,
MIL_ID MilDisplay ,
MIL_ID MilDigitizer ,
MIL_ID MilCameraImage,
MIL_ID MilLaser ,
MIL_ID MilScan )
{
bool EverythingIsOK = true;
MIL_INT ContextType = M3dmapInquire(MilLaser, M_DEFAULT, M_LASER_CONTEXT_TYPE, M_NULL);
MIL_ID MilCalibration = M_NULL;
if (ContextType == M_CALIBRATED_CAMERA_LINEAR_MOTION)
EverythingIsOK = CalibrateCamera(MilSystem, MilDisplay, MilDigitizer, MilCameraImage, &MilCalibration);
if (EverythingIsOK)
{
MosPrintf(MIL_TEXT("Calibrating the 3d reconstruction setup\n")
MIL_TEXT("---------------------------------------\n\n"));
if (ContextType == M_CALIBRATED_CAMERA_LINEAR_MOTION)
AddLaserLinesFullMode(MilSystem, MilDisplay, MilDigitizer, MilCameraImage, MilLaser, MilScan);
else
AddLaserLinesDepthOnly(MilSystem, MilDisplay, MilDigitizer, MilCameraImage, MilLaser, MilScan);
M3dmapCalibrate(MilLaser, MilScan, MilCalibration, M_DEFAULT);
if (M3dmapInquire(MilLaser, M_DEFAULT, M_CALIBRATION_STATUS, M_NULL) == M_CALIBRATED)
{
MosPrintf(MIL_TEXT("The calibration of the 3d reconstruction setup was successful.\n\n"));
}
else
{
MosPrintf(MIL_TEXT("The calibration of the 3d reconstruction setup has failed.\n")
MIL_TEXT("Press <Enter> to end.\n\n"));
MosGetch();
EverythingIsOK = false;
}
}
if (MilCalibration != M_NULL)
McalFree(MilCalibration);
return EverythingIsOK;
}
bool Scan(MIL_ID MilSystem ,
MIL_ID MilDisplay ,
MIL_ID MilDigitizer ,
MIL_ID MilCameraImage,
MIL_ID MilLaser ,
MIL_ID MilScan )
{
bool EverythingIsOK = true;
MosPrintf(MIL_TEXT("Scanning the object\n")
MIL_TEXT("-------------------\n\n"));
M3dmapAddScan(M_NULL, MilScan, M_NULL, M_NULL, M_NULL, M_DEFAULT, M_RESET);
AddScanProcFctDataStruct AddScanProcFctData;
AddScanProcFctData.MilLaser = MilLaser;
AddScanProcFctData.MilScan = MilScan;
AllocateBuffersForLaserExtraction(MilSystem ,
LASER_ROI_SIZE_X ,
MAX_NB_SCANS_PER_GRAB ,
NB_DIG_PROCESS_BUFFERS ,
AddScanProcFctData.MilGrabImageArray,
&AddScanProcFctData.MilNoPaddingImage,
&AddScanProcFctData.MilPositionImage ,
&AddScanProcFctData.MilIntensityImage);
if (USE_REAL_CAMERA)
{
MdigGrabContinuous(MilDigitizer, MilCameraImage);
MosPrintf(MIL_TEXT("Adjust the object's position.\n")
MIL_TEXT("Press <Enter> to start scanning.\n\n"));
MosGetch();
MdigHalt(MilDigitizer);
}
MbufClear(AddScanProcFctData.MilPositionImage, 0.0);
MdispSelect(MilDisplay, AddScanProcFctData.MilPositionImage);
MdispControl(MilDisplay, M_VIEW_BIT_SHIFT, 6 );
MdispControl(MilDisplay, M_VIEW_MODE , M_BIT_SHIFT);
MosPrintf(MIL_TEXT("The object is being scanned. Raw (uncorrected) depth map coming from the\n")
MIL_TEXT("GatorEye is displayed.\n\n"));
if (USE_REAL_CAMERA)
{
SwitchToLaserExtraction(MilDigitizer, MAX_NB_SCANS_PER_GRAB);
MdigProcess(MilDigitizer,
AddScanProcFctData.MilGrabImageArray,
NB_DIG_PROCESS_BUFFERS,
M_SEQUENCE+M_COUNT(NB_LASER_GRABS),
M_SYNCHRONOUS,
&AddScanProcFct,
&AddScanProcFctData);
}
else
{
MosPrintf(MIL_TEXT("GatorEye data obtained during scanning are reloaded from disk.\n\n"));
const MIL_INT BufIndexToUse = 0;
for (MIL_INT ScanIndex = 0; ScanIndex < NB_LASER_GRABS; ++ScanIndex)
{
MosSleep(1000);
MIL_TEXT_CHAR Filename[MAX_FILENAME_LEN];
MosSprintf(Filename, MAX_FILENAME_LEN, LASER_GRAB_FILENAME_PATTERN, ScanIndex);
MbufLoad(Filename, AddScanProcFctData.MilGrabImageArray[BufIndexToUse]);
AddGatorEyeScan(&AddScanProcFctData, BufIndexToUse);
}
}
MdispSelect(MilDisplay, M_NULL);
MdispControl(MilDisplay, M_VIEW_MODE, M_DEFAULT);
FreeBuffersForLaserExtraction(NB_DIG_PROCESS_BUFFERS ,
AddScanProcFctData.MilGrabImageArray,
AddScanProcFctData.MilNoPaddingImage,
AddScanProcFctData.MilPositionImage ,
AddScanProcFctData.MilIntensityImage);
return EverythingIsOK;
}
bool ShowResults(MIL_ID MilSystem, MIL_ID MilDisplay, MIL_ID MilScan)
{
bool EverythingIsOK = true;
MIL_INT CorrectionState;
M3dmapGetResult(MilScan, M_DEFAULT, M_DEPTH_CORRECTION_STATE+M_TYPE_MIL_INT, &CorrectionState);
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 );
M3dmapControl(MilScan, M_DEFAULT, M_EXTRACTION_SCALE_MODE , M_AUTO_SCALE);
MIL_INT DepthMapSizeX, DepthMapSizeY, DepthMapType, IntensityMapType;
if (CorrectionState == M_FULLY_CORRECTED)
{
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 );
}
if (GATOREYE_OUTPUT_INTENSITY)
M3dmapGetResult(MilScan, M_DEFAULT, M_INTENSITY_MAP_BUFFER_TYPE+M_TYPE_MIL_INT, &IntensityMapType);
MIL_ID MilDepthMap = MbufAlloc2d(MilSystem, DepthMapSizeX, DepthMapSizeY, DepthMapType , M_IMAGE+M_PROC+M_DISP, M_NULL);
MIL_ID MilIntensityMap = M_NULL;
if (GATOREYE_OUTPUT_INTENSITY)
MbufAlloc2d(MilSystem, DepthMapSizeX, DepthMapSizeY, IntensityMapType, M_IMAGE+M_PROC+M_DISP, &MilIntensityMap);
M3dmapExtract(MilScan, MilDepthMap, MilIntensityMap, M_CORRECTED_DEPTH_MAP, M_DEFAULT, M_DEFAULT);
bool D3DDisplayShown = false;
#if USE_D3D_DISPLAY
MIL_DISP_D3D_HANDLE DispHandle;
if (CorrectionState == M_FULLY_CORRECTED)
{
DispHandle = MdepthD3DAlloc(MilDepthMap, MilIntensityMap,
D3D_DISPLAY_SIZE_X, D3D_DISPLAY_SIZE_Y,
M_DEFAULT, M_DEFAULT, M_DEFAULT,
M_DEFAULT, M_DEFAULT, M_DEFAULT, 0);
if (DispHandle != NULL)
{
MosPrintf(MIL_TEXT("The 3d model of the scanned object is displayed.\n\n"));
MdispD3DShow(DispHandle);
MdispD3DPrintHelp(DispHandle);
D3DDisplayShown = true;
}
}
#endif
if (!D3DDisplayShown)
{
MosPrintf(MIL_TEXT("The depth map of the scanned object is displayed.\n"));
MdispControl(MilDisplay, M_VIEW_MODE, M_AUTO_SCALE);
MdispSelect(MilDisplay, MilDepthMap);
}
if (CorrectionState == M_FULLY_CORRECTED)
MosPrintf(MIL_TEXT("Press <Enter> to end.\n\n"));
else
MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n"));
MosGetch();
if (!D3DDisplayShown)
{
MdispSelect(MilDisplay, M_NULL);
MdispControl(MilDisplay, M_VIEW_MODE, M_DEFAULT);
}
#if USE_D3D_DISPLAY
if (D3DDisplayShown)
{
MdispD3DHide(DispHandle);
MdispD3DFree(DispHandle);
}
#endif
if (GATOREYE_OUTPUT_INTENSITY)
MbufFree(MilIntensityMap);
MbufFree(MilDepthMap);
return EverythingIsOK;
}
MIL_INT MFTYPE AddScanProcFct(MIL_INT HookType, MIL_ID MilEvent, void* UserDataPtr)
{
const AddScanProcFctDataStruct* pAddScanProcFctData = static_cast<const AddScanProcFctDataStruct*>(UserDataPtr);
MIL_INT BufIndex;
MdigGetHookInfo(MilEvent, M_MODIFIED_BUFFER+M_BUFFER_INDEX, &BufIndex);
AddGatorEyeScan(pAddScanProcFctData, BufIndex);
return M_NULL;
}
void AddGatorEyeScan(const AddScanProcFctDataStruct* pAddScanProcFctData, MIL_INT BufIndex)
{
MbufCopy(pAddScanProcFctData->MilGrabImageArray[BufIndex], pAddScanProcFctData->MilNoPaddingImage);
MbufControl(pAddScanProcFctData->MilPositionImage, M_MODIFIED, M_DEFAULT);
M3dmapAddScan(pAddScanProcFctData->MilLaser,
pAddScanProcFctData->MilScan,
pAddScanProcFctData->MilPositionImage,
pAddScanProcFctData->MilIntensityImage,
M_NULL,
M_DEFAULT,
M_LINE_ALREADY_EXTRACTED);
}
void SwitchToLaserExtraction(MIL_ID MilDigitizer, MIL_INT64 NbScansPerGrab)
{
if (USE_REAL_CAMERA)
{
MdigControl(MilDigitizer, M_GC_STREAMING_STOP, M_DEFAULT );
MdigControl(MilDigitizer, M_SOURCE_OFFSET_X , 0 );
MdigControl(MilDigitizer, M_SOURCE_OFFSET_Y , 0 );
MdigControl(MilDigitizer, M_SOURCE_SIZE_X , LASER_ROI_SIZE_X );
MdigControl(MilDigitizer, M_SOURCE_SIZE_Y , LASER_ROI_SIZE_Y );
MdigControl(MilDigitizer, M_SOURCE_OFFSET_X , LASER_ROI_OFFSET_X);
MdigControl(MilDigitizer, M_SOURCE_OFFSET_Y , LASER_ROI_OFFSET_Y);
MdigControlFeature(MilDigitizer, M_FEATURE_VALUE_AS_STRING, MIL_TEXT("LocatePeakMethod" ), M_TYPE_ENUMERATION, MIL_TEXT("CenterOfGravity"));
MdigControlFeature(MilDigitizer, M_FEATURE_VALUE , MIL_TEXT("LocatePeakFrameCount"), M_TYPE_INT64 , &NbScansPerGrab );
}
}
void SwitchToImage(MIL_ID MilDigitizer)
{
if (USE_REAL_CAMERA)
{
MdigControl(MilDigitizer, M_GC_STREAMING_STOP, M_DEFAULT );
MdigControl(MilDigitizer, M_SOURCE_OFFSET_X , 0 );
MdigControl(MilDigitizer, M_SOURCE_OFFSET_Y , 0 );
MdigControl(MilDigitizer, M_SOURCE_SIZE_X , FULL_IMAGE_SIZE_X );
MdigControl(MilDigitizer, M_SOURCE_SIZE_Y , FULL_IMAGE_SIZE_Y );
MdigControl(MilDigitizer, M_SOURCE_OFFSET_X , FULL_IMAGE_OFFSET_X);
MdigControl(MilDigitizer, M_SOURCE_OFFSET_Y , FULL_IMAGE_OFFSET_Y);
MdigControlFeature(MilDigitizer, M_FEATURE_VALUE_AS_STRING, MIL_TEXT("LocatePeakMethod"), M_TYPE_ENUMERATION, MIL_TEXT("Off"));
}
}
bool CalibrateCamera(MIL_ID MilSystem ,
MIL_ID MilDisplay ,
MIL_ID MilDigitizer ,
MIL_ID MilCameraImage ,
MIL_ID* pMilCalibration)
{
bool EverythingIsOK = true;
MosPrintf(MIL_TEXT("Calibrating the camera\n")
MIL_TEXT("----------------------\n\n"));
if (USE_REAL_CAMERA)
{
MdigGrabContinuous(MilDigitizer, MilCameraImage);
MosPrintf(MIL_TEXT("Place calibration grid at Z=0 mm (laser switched OFF).\n")
MIL_TEXT("Press <Enter> to calibrate the camera.\n\n"));
MosGetch();
MdigHalt(MilDigitizer);
}
else
{
MbufLoad(CALIBRATION_GRID_FILE, MilCameraImage);
MosPrintf(MIL_TEXT("The image of the calibration grid has been reloaded from disk.\n\n"));
}
McalAlloc(MilSystem, M_TSAI_BASED, M_DEFAULT, pMilCalibration);
McalGrid(*pMilCalibration, MilCameraImage, 0.0, 0.0, 0.0, ROW_NUMBER, COLUMN_NUMBER,
ROW_SPACING, COLUMN_SPACING, M_DEFAULT, GRID_TYPE);
if (McalInquire(*pMilCalibration, M_CALIBRATION_STATUS, M_NULL) == M_CALIBRATED)
{
if (!USE_REAL_CAMERA)
MdispSelect(MilDisplay, MilCameraImage);
MIL_ID MilOverlay;
MdispControl(MilDisplay, M_OVERLAY, M_ENABLE);
MdispInquire(MilDisplay, M_OVERLAY_ID, &MilOverlay);
MdispControl(MilDisplay, M_OVERLAY_CLEAR, M_DEFAULT);
MgraColor(M_DEFAULT, M_COLOR_GREEN);
McalDraw(M_DEFAULT, *pMilCalibration, MilOverlay, M_DRAW_WORLD_POINTS, M_DEFAULT, M_DEFAULT);
MosPrintf(MIL_TEXT("Camera calibration was successful. Calibration points are displayed in green.\n")
MIL_TEXT("Press <Enter> to continue.\n\n"));
MosGetch();
MdispControl(MilDisplay, M_OVERLAY_CLEAR, M_DEFAULT);
MdispControl(MilDisplay, M_OVERLAY, M_DISABLE);
if (!USE_REAL_CAMERA)
MdispSelect(MilDisplay, M_NULL);
}
else
{
McalFree(*pMilCalibration);
*pMilCalibration = M_NULL;
EverythingIsOK = false;
MosPrintf(MIL_TEXT("Camera calibration failed.\n")
MIL_TEXT("Press <Enter> to end.\n\n"));
MosGetch();
}
return EverythingIsOK;
}
void AddLaserLinesFullMode(MIL_ID MilSystem ,
MIL_ID MilDisplay ,
MIL_ID MilDigitizer ,
MIL_ID MilCameraImage,
MIL_ID MilLaser ,
MIL_ID MilScan )
{
MIL_ID MilGrabImage, MilNoPaddingImage, MilPositionImage, MilIntensityImage;
AllocateBuffersForLaserExtraction(MilSystem ,
LASER_ROI_SIZE_X ,
1 ,
1 ,
&MilGrabImage ,
&MilNoPaddingImage ,
&MilPositionImage ,
&MilIntensityImage );
if (USE_REAL_CAMERA)
{
MdigGrabContinuous(MilDigitizer, MilCameraImage);
MosPrintf(MIL_TEXT("Set up the reference plane at Z=0 mm.\n")
MIL_TEXT("Press <Enter> to add this plane.\n\n"));
MosGetch();
MdigHalt(MilDigitizer);
SwitchToLaserExtraction(MilDigitizer, 1);
MdigGrab(MilDigitizer, MilGrabImage);
SwitchToImage(MilDigitizer);
}
else
{
MbufLoad(FULL_MODE_CALIBRATION_LINE_FILE, MilGrabImage);
MosPrintf(MIL_TEXT("GatorEye data for the reference plane at Z=0 mm has been reloaded\n")
MIL_TEXT("from disk.\n\n"));
}
MbufCopy(MilGrabImage, MilNoPaddingImage);
M3dmapAddScan(MilLaser, MilScan, MilPositionImage, MilIntensityImage,
M_NULL, M_DEFAULT, M_LINE_ALREADY_EXTRACTED);
DisplayLaserLine(MilSystem, MilDisplay, MilScan);
MosPrintf(SEPARATOR);
FreeBuffersForLaserExtraction(1 ,
&MilGrabImage ,
MilNoPaddingImage,
MilPositionImage ,
MilIntensityImage);
}
void AddLaserLinesDepthOnly(MIL_ID MilSystem ,
MIL_ID MilDisplay ,
MIL_ID MilDigitizer ,
MIL_ID MilCameraImage,
MIL_ID MilLaser ,
MIL_ID MilScan )
{
MIL_ID MilGrabImage, MilNoPaddingImage, MilPositionImage, MilIntensityImage;
AllocateBuffersForLaserExtraction(MilSystem ,
LASER_ROI_SIZE_X ,
1 ,
1 ,
&MilGrabImage ,
&MilNoPaddingImage ,
&MilPositionImage ,
&MilIntensityImage );
for (MIL_INT ReferencePlaneIndex = 0; ReferencePlaneIndex < NB_REFERENCE_PLANES; ++ReferencePlaneIndex)
{
if (USE_REAL_CAMERA)
{
MdigGrabContinuous(MilDigitizer, MilCameraImage);
MosPrintf(MIL_TEXT("Set up the reference plane #%d at Z=%.3g mm.\n")
MIL_TEXT("Press <Enter> to add this plane.\n\n"),
ReferencePlaneIndex, CORRECTED_DEPTHS[ReferencePlaneIndex]);
MosGetch();
MdigHalt(MilDigitizer);
SwitchToLaserExtraction(MilDigitizer, 1);
MdigGrab(MilDigitizer, MilGrabImage);
SwitchToImage(MilDigitizer);
}
else
{
MIL_TEXT_CHAR Filename[MAX_FILENAME_LEN];
MosSprintf(Filename, MAX_FILENAME_LEN, DEPTH_ONLY_CALIBRATION_LINE_FILENAME_PATTERN, ReferencePlaneIndex);
MbufLoad(Filename, MilGrabImage);
MosPrintf(MIL_TEXT("GatorEye data for the reference plane #%d at Z=%.3g mm has been\n")
MIL_TEXT("reloaded from disk.\n\n"),
ReferencePlaneIndex, CORRECTED_DEPTHS[ReferencePlaneIndex]);
}
MbufCopy(MilGrabImage, MilNoPaddingImage);
M3dmapControl(MilLaser, M_DEFAULT, M_CORRECTED_DEPTH,
CORRECTED_DEPTHS[ReferencePlaneIndex]*SCALE_FACTOR);
M3dmapAddScan(MilLaser, MilScan, MilPositionImage, MilIntensityImage,
M_NULL, M_DEFAULT, M_LINE_ALREADY_EXTRACTED);
DisplayLaserLine(MilSystem, MilDisplay, MilScan);
MosPrintf(SEPARATOR);
}
FreeBuffersForLaserExtraction(1 ,
&MilGrabImage ,
MilNoPaddingImage,
MilPositionImage ,
MilIntensityImage);
}
void AllocateBuffersForLaserExtraction(MIL_ID MilSystem ,
MIL_INT LaserROISizeX ,
MIL_INT NbLaserLinesToExtract,
MIL_INT NbGrabBuffers ,
MIL_ID* MilGrabImageArray ,
MIL_ID* MilNoPaddingImagePtr ,
MIL_ID* MilPositionImagePtr ,
MIL_ID* MilIntensityImagePtr )
{
MIL_INT LaserImageSizeY = NbLaserLinesToExtract * NB_BYTES_PER_PIXEL;
MIL_INT LaserPitchByte = LaserROISizeX * NB_BYTES_PER_PIXEL;
for (MIL_INT BufIndex = 0; BufIndex < NbGrabBuffers; ++BufIndex)
{
MbufAlloc2d(MilSystem, LaserROISizeX, LaserImageSizeY,
8+M_UNSIGNED, M_IMAGE+GRAB_ATTR, &MilGrabImageArray[BufIndex]);
}
MsysControl(MilSystem, M_ALLOCATION_OVERSCAN, M_DISABLE);
MsysControl(MilSystem, M_DEFAULT_PITCH_BYTE_MULTIPLE , 1 );
MbufAlloc2d(MilSystem, LaserROISizeX, LaserImageSizeY,
8+M_UNSIGNED, M_IMAGE+M_PROC, MilNoPaddingImagePtr);
MsysControl(MilSystem, M_ALLOCATION_OVERSCAN, M_DEFAULT);
MsysControl(MilSystem, M_DEFAULT_PITCH_BYTE_MULTIPLE , M_DEFAULT);
MIL_UINT8* RawLaserLineDataPtr;
MbufInquire(*MilNoPaddingImagePtr, M_HOST_ADDRESS, &RawLaserLineDataPtr);
MbufCreate2d(MilSystem, LaserROISizeX, NbLaserLinesToExtract, 16+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP,
M_HOST_ADDRESS+M_PITCH_BYTE, LaserPitchByte, RawLaserLineDataPtr,
MilPositionImagePtr);
if (GATOREYE_OUTPUT_INTENSITY)
{
MbufCreate2d(MilSystem, LaserROISizeX, NbLaserLinesToExtract, 8+M_UNSIGNED, M_IMAGE+M_PROC,
M_HOST_ADDRESS+M_PITCH_BYTE, LaserPitchByte, RawLaserLineDataPtr+2*LaserROISizeX,
MilIntensityImagePtr);
}
else
{
*MilIntensityImagePtr = M_NULL;
}
}
void FreeBuffersForLaserExtraction(MIL_INT NbGrabBuffers ,
MIL_ID* MilGrabImageArray,
MIL_ID MilNoPaddingImage,
MIL_ID MilPositionImage ,
MIL_ID MilIntensityImage)
{
if (GATOREYE_OUTPUT_INTENSITY)
MbufFree(MilIntensityImage);
MbufFree(MilPositionImage );
MbufFree(MilNoPaddingImage);
for (MIL_INT BufIndex = 0; BufIndex < NbGrabBuffers; ++BufIndex)
MbufFree(MilGrabImageArray[BufIndex]);
}
void DisplayLaserLine(MIL_ID MilSystem, MIL_ID MilDisplay, MIL_ID MilScan)
{
MIL_ID MilLaserImage = MbufAlloc2d(MilSystem, LASER_ROI_SIZE_X, LASER_ROI_SIZE_Y,
8+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP, M_NULL);
MbufClear(MilLaserImage, 0.0);
MIL_ID MilPreviousImage;
MdispInquire(MilDisplay, M_SELECTED, &MilPreviousImage);
MgraColor(M_DEFAULT, 255.0);
M3dmapDraw(M_DEFAULT, MilScan, MilLaserImage, M_DRAW_PEAKS_LAST, M_DEFAULT, M_DEFAULT);
MdispSelect(MilDisplay, MilLaserImage);
MosPrintf(MIL_TEXT("The extracted laser line is displayed.\n")
MIL_TEXT("Press <Enter> to continue.\n\n"));
MosGetch();
MdispSelect(MilDisplay, MilPreviousImage);
MbufFree(MilLaserImage);
}
MIL_DOUBLE GetCameraFrameRate(MIL_ID MilDigitizer)
{
MIL_DOUBLE FrameRate;
if (USE_REAL_CAMERA)
{
SwitchToLaserExtraction(MilDigitizer, 1);
MIL_ID MilSystem;
MdigInquire(MilDigitizer, M_OWNER_SYSTEM, &MilSystem);
MIL_ID MilGrabImage = MbufAlloc2d(MilSystem, LASER_ROI_SIZE_X, NB_BYTES_PER_PIXEL,
8+M_UNSIGNED, M_IMAGE+GRAB_ATTR, M_NULL);
MdigGrab(MilDigitizer, MilGrabImage);
MbufFree(MilGrabImage);
MdigInquireFeature(MilDigitizer, M_FEATURE_VALUE, MIL_TEXT("AcquisitionFrameRate"), M_TYPE_DOUBLE, &FrameRate);
}
else
{
FrameRate = 55.693066;
}
return FrameRate;
}