#include <mil.h>
#include <math.h>
void PrintHeader()
{
MosPrintf(MIL_TEXT("[EXAMPLE NAME]\n"));
MosPrintf(MIL_TEXT("CalibrationFromList\n\n"));
MosPrintf(MIL_TEXT("[SYNOPSIS]\n"));
MosPrintf(MIL_TEXT("This program demonstrates how to setup a 3d calibration using\n"));
MosPrintf(MIL_TEXT("a list of points generated by multiple 2d calibrations.\n\n"));
MosPrintf(MIL_TEXT("[MODULES USED]\n")
MIL_TEXT("Modules used: application, system, display, buffer, graphic,\n")
MIL_TEXT(" image processing, calibration.\n\n"));
MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n"));
MosGetch();
}
#define GRIDS_IMAGE_FILE M_IMAGE_PATH MIL_TEXT("CalibrationFromList/Grids.mim")
enum GridLocation { eTop, eLeft, eRight, eNumGrids };
static const MIL_INT NUM_POLYGON_POINTS = 4;
static const MIL_INT NUM_GRIDS = eNumGrids;
static const MIL_DOUBLE ANNOTATIONS_COLOR = M_COLOR_GREEN;
struct PolygonPointsStruct
{
MIL_DOUBLE x[NUM_POLYGON_POINTS];
MIL_DOUBLE y[NUM_POLYGON_POINTS];
MIL_DOUBLE z[NUM_POLYGON_POINTS];
};
struct GridCalInfoStruct
{
MIL_DOUBLE GridOffsetX;
MIL_DOUBLE GridOffsetY;
MIL_DOUBLE GridOffsetZ;
MIL_INT RowNumber;
MIL_INT ColumnNumber;
MIL_DOUBLE RowSpacing;
MIL_DOUBLE ColumnSpacing;
MIL_DOUBLE CornerHintX;
MIL_DOUBLE CornerHintY;
};
struct CalibrationPointsStruct
{
MIL_DOUBLE* PixelPointsX;
MIL_DOUBLE* PixelPointsY;
MIL_DOUBLE* WorldPointsX;
MIL_DOUBLE* WorldPointsY;
MIL_DOUBLE* WorldPointsZ;
MIL_INT NumPoints;
};
static const MIL_DOUBLE GRIDS_Z_OFFSET = 20;
static const PolygonPointsStruct TOP_GRID_POLYGON =
{
{ 29, 321, 591, 301},
{ 112, 10, 184, 324}
};
static const GridCalInfoStruct TOP_GRID_CAL_INFO =
{
8,
12,
0,
8,
9,
8,
8,
68,
122
};
static const PolygonPointsStruct TOP_POLYGON_WORLD_COORDINATES =
{
{ 0, 0, 80, 80},
{ 0, 80, 80, 0},
{ 40+GRIDS_Z_OFFSET, 40+GRIDS_Z_OFFSET,
40+GRIDS_Z_OFFSET, 40+GRIDS_Z_OFFSET}
};
static const PolygonPointsStruct LEFT_GRID_POLYGON =
{
{ 29, 297, 267, 20},
{ 121, 323, 468, 275}
};
static const GridCalInfoStruct LEFT_GRID_CAL_INFO =
{
8,
8,
0,
4,
9,
8,
8,
36,
143
};
static const PolygonPointsStruct LEFT_POLYGON_WORLD_COORDINATES =
{
{ 0, 0, 80, 80},
{ 0, 0, 0, 0},
{ 40+GRIDS_Z_OFFSET, 0, 0, 40+GRIDS_Z_OFFSET}
};
static const PolygonPointsStruct RIGHT_GRID_POLYGON =
{
{ 304, 589, 558, 282},
{ 326, 188, 336, 471}
};
static const GridCalInfoStruct RIGHT_GRID_CAL_INFO =
{
8,
8,
0,
4,
9,
8,
8,
311,
328
};
static const PolygonPointsStruct RIGHT_POLYGON_WORLD_COORDINATES =
{
{ 80, 80, 80, 80},
{ 0, 0, 80, 80},
{ 40+GRIDS_Z_OFFSET, 0, 0, 40+GRIDS_Z_OFFSET}
};
void CreateCalibrationPoints(MIL_ID MilDisplay, MIL_ID MilDisplayImage,
MIL_ID MilSystem, MIL_ID MilGridsImage, const PolygonPointsStruct& GridPolygon,
const GridCalInfoStruct& GridCalInfo, CalibrationPointsStruct& CalibrationPoints);
void Create3dCalibrationPoints(const CalibrationPointsStruct* CalibrationPoints2d,
CalibrationPointsStruct* CalibrationPoints3d);
void DestroyCalibrationPoints(CalibrationPointsStruct& CalibrationPoints);
void CalibrateWithMask(MIL_ID MilDisplay, MIL_ID MilDisplayImage, MIL_ID MilCalibration,
MIL_ID MilGridsImage, const PolygonPointsStruct& GridPolygon,
const GridCalInfoStruct& GridCalInfo);
void AllocAndGetCalibrationPoints(MIL_ID MilCalibration,
CalibrationPointsStruct& CalibrationPoints);
int MosMain(void)
{
MIL_ID MilApplication,
MilDisplay,
MilSystem,
MilDisplayImage,
MilGridsImage,
MilCalibration;
CalibrationPointsStruct CalibrationPoints[NUM_GRIDS];
CalibrationPointsStruct CalibrationPoints3d;
MappAlloc(M_NULL, M_DEFAULT, &MilApplication);
MsysAlloc(M_DEFAULT, M_SYSTEM_HOST, M_DEFAULT, M_DEFAULT, &MilSystem);
MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED,
&MilDisplay);
PrintHeader();
MbufRestore(GRIDS_IMAGE_FILE, MilSystem, &MilGridsImage);
MbufAlloc2d(MilSystem,
MbufInquire(MilGridsImage, M_SIZE_X, M_NULL),
MbufInquire(MilGridsImage, M_SIZE_Y, M_NULL),
8,
M_IMAGE+M_PROC+M_DISP,
&MilDisplayImage);
MbufCopy(MilGridsImage, MilDisplayImage);
MdispSelect(MilDisplay, MilDisplayImage);
MosPrintf(MIL_TEXT("A 3d calibration will be set up using the 2d calibration data \n")
MIL_TEXT("generated with the grids on the displayed object.\n"));
MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n"));
MosGetch();
MosPrintf(MIL_TEXT("Calibrating with the top grid...\n"));
CreateCalibrationPoints(MilDisplay, MilDisplayImage, MilSystem, MilGridsImage,
TOP_GRID_POLYGON, TOP_GRID_CAL_INFO, CalibrationPoints[eTop]);
MosPrintf(MIL_TEXT("Calibrating with the left grid...\n"));
CreateCalibrationPoints(MilDisplay, MilDisplayImage, MilSystem, MilGridsImage,
LEFT_GRID_POLYGON, LEFT_GRID_CAL_INFO, CalibrationPoints[eLeft]);
MosPrintf(MIL_TEXT("Calibrating with the right grid...\n"));
CreateCalibrationPoints(MilDisplay, MilDisplayImage, MilSystem, MilGridsImage,
RIGHT_GRID_POLYGON, RIGHT_GRID_CAL_INFO, CalibrationPoints[eRight]);
McalAlloc(MilSystem, M_TSAI_BASED, M_DEFAULT, &MilCalibration);
Create3dCalibrationPoints(CalibrationPoints, &CalibrationPoints3d);
McalControl(MilCalibration, M_PRINCIPAL_POINT_X,
MbufInquire(MilGridsImage, M_SIZE_X, M_NULL)/2);
McalControl(MilCalibration, M_PRINCIPAL_POINT_Y,
MbufInquire(MilGridsImage, M_SIZE_Y, M_NULL)/2);
McalList(MilCalibration, CalibrationPoints3d.PixelPointsX,
CalibrationPoints3d.PixelPointsY, CalibrationPoints3d.WorldPointsX,
CalibrationPoints3d.WorldPointsY, CalibrationPoints3d.WorldPointsZ,
CalibrationPoints3d.NumPoints, M_FULL_CALIBRATION, M_DEFAULT);
MIL_ID MilGraphicsContext, MilGraphicList;
MgraAlloc(MilSystem, &MilGraphicsContext);
MgraAllocList(MilSystem, M_DEFAULT, &MilGraphicList);
MgraColor(MilGraphicsContext, ANNOTATIONS_COLOR);
MdispControl(MilDisplay, M_ASSOCIATED_GRAPHIC_LIST_ID, MilGraphicList);
MbufCopy(MilGridsImage, MilDisplayImage);
McalDraw(MilGraphicsContext, MilCalibration, MilGraphicList,
M_DRAW_ABSOLUTE_COORDINATE_SYSTEM+M_DRAW_FRAME, M_DEFAULT, M_DEFAULT);
PolygonPointsStruct TopPolygonPixelCoordinates;
PolygonPointsStruct LeftPolygonPixelCoordinates;
PolygonPointsStruct RightPolygonPixelCoordinates;
McalTransformCoordinate3dList(MilCalibration, M_ABSOLUTE_COORDINATE_SYSTEM,
M_PIXEL_COORDINATE_SYSTEM, NUM_POLYGON_POINTS, TOP_POLYGON_WORLD_COORDINATES.x,
TOP_POLYGON_WORLD_COORDINATES.y, TOP_POLYGON_WORLD_COORDINATES.z,
TopPolygonPixelCoordinates.x, TopPolygonPixelCoordinates.y, M_NULL, M_DEFAULT);
MgraLines(MilGraphicsContext, MilGraphicList, NUM_POLYGON_POINTS,
TopPolygonPixelCoordinates.x, TopPolygonPixelCoordinates.y,
M_NULL, M_NULL, M_POLYGON);
McalTransformCoordinate3dList(MilCalibration, M_ABSOLUTE_COORDINATE_SYSTEM,
M_PIXEL_COORDINATE_SYSTEM, NUM_POLYGON_POINTS, LEFT_POLYGON_WORLD_COORDINATES.x,
LEFT_POLYGON_WORLD_COORDINATES.y, LEFT_POLYGON_WORLD_COORDINATES.z,
LeftPolygonPixelCoordinates.x, LeftPolygonPixelCoordinates.y, M_NULL, M_DEFAULT);
MgraLines(MilGraphicsContext, MilGraphicList, NUM_POLYGON_POINTS,
LeftPolygonPixelCoordinates.x, LeftPolygonPixelCoordinates.y,
M_NULL, M_NULL, M_POLYLINE);
McalTransformCoordinate3dList(MilCalibration, M_ABSOLUTE_COORDINATE_SYSTEM,
M_PIXEL_COORDINATE_SYSTEM, NUM_POLYGON_POINTS, RIGHT_POLYGON_WORLD_COORDINATES.x,
RIGHT_POLYGON_WORLD_COORDINATES.y, RIGHT_POLYGON_WORLD_COORDINATES.z,
RightPolygonPixelCoordinates.x, RightPolygonPixelCoordinates.y, M_NULL, M_DEFAULT);
MgraLines(MilGraphicsContext, MilGraphicList, NUM_POLYGON_POINTS,
RightPolygonPixelCoordinates.x, RightPolygonPixelCoordinates.y,
M_NULL, M_NULL, M_POLYLINE);
const MIL_INT TEXT_SIZE = 256;
MIL_TEXT_CHAR OutputText[TEXT_SIZE];
MgraControl(MilGraphicsContext, M_BACKGROUND_MODE, M_OPAQUE);
for (MIL_INT i=0; i<NUM_POLYGON_POINTS; i++)
{
MosSprintf(OutputText, TEXT_SIZE, MIL_TEXT("(%.0f, %.0f, %.0f)"),
TOP_POLYGON_WORLD_COORDINATES.x[i], TOP_POLYGON_WORLD_COORDINATES.y[i],
TOP_POLYGON_WORLD_COORDINATES.z[i]);
if (i == NUM_POLYGON_POINTS-2)
{
MgraControl(MilGraphicsContext, M_TEXT_ALIGN_HORIZONTAL, M_RIGHT);
MgraControl(MilGraphicsContext, M_TEXT_ALIGN_VERTICAL, M_BOTTOM);
}
MgraText(MilGraphicsContext, MilGraphicList, TopPolygonPixelCoordinates.x[i],
TopPolygonPixelCoordinates.y[i], OutputText);
}
MgraControl(MilGraphicsContext, M_TEXT_ALIGN_HORIZONTAL, M_RIGHT);
MgraControl(MilGraphicsContext, M_TEXT_ALIGN_VERTICAL, M_BOTTOM);
MgraControl(MilGraphicsContext, M_INPUT_UNITS, M_DISPLAY);
MgraText(MilGraphicsContext, MilGraphicList, MbufInquire(MilDisplayImage,
M_SIZE_X, M_NULL)-1, MbufInquire(MilDisplayImage, M_SIZE_Y, M_NULL)-1,
MIL_TEXT("Coordinates shown in mm"));
MosPrintf(MIL_TEXT("The 3d calibration has been defined and the object's wire frame \n")
MIL_TEXT("along with its world coordinates (in mm) are shown.\n"));
MosPrintf(MIL_TEXT("Press <Enter> to end.\n\n"));
MosGetch();
DestroyCalibrationPoints(CalibrationPoints[eTop]);
DestroyCalibrationPoints(CalibrationPoints[eLeft]);
DestroyCalibrationPoints(CalibrationPoints[eRight]);
DestroyCalibrationPoints(CalibrationPoints3d);
MgraFree(MilGraphicsContext);
MgraFree(MilGraphicList);
MbufFree(MilGridsImage);
MbufFree(MilDisplayImage);
McalFree(MilCalibration);
MdispFree(MilDisplay);
MsysFree(MilSystem);
MappFree(MilApplication);
return 0;
}
void CreateCalibrationPoints(MIL_ID MilDisplay, MIL_ID MilDisplayImage,
MIL_ID MilSystem, MIL_ID MilGridsImage, const PolygonPointsStruct& GridPolygon,
const GridCalInfoStruct& GridCalInfo, CalibrationPointsStruct& CalibrationPoints)
{
MIL_ID MilCalibration;
McalAlloc(MilSystem, M_LINEAR_INTERPOLATION , M_DEFAULT, &MilCalibration);
CalibrateWithMask(MilDisplay, MilDisplayImage, MilCalibration, MilGridsImage,
GridPolygon, GridCalInfo);
AllocAndGetCalibrationPoints(MilCalibration, CalibrationPoints);
McalFree(MilCalibration);
}
void CalibrateWithMask(MIL_ID MilDisplay, MIL_ID MilDisplayImage, MIL_ID MilCalibration,
MIL_ID MilGridsImage, const PolygonPointsStruct& GridPolygon,
const GridCalInfoStruct& GridCalInfo)
{
MIL_ID MilMaskImage, MilWorkGridImage;
MIL_ID MilGraphicList, MilGraphicsContext;
MIL_ID MilSystem = MbufInquire(MilGridsImage, M_OWNER_SYSTEM, M_NULL);
MbufAlloc2d(MilSystem,
MbufInquire(MilGridsImage, M_SIZE_X, M_NULL),
MbufInquire(MilGridsImage, M_SIZE_Y, M_NULL),
8,
M_IMAGE+M_PROC+M_DISP,
&MilMaskImage);
MbufAlloc2d(MilSystem,
MbufInquire(MilGridsImage, M_SIZE_X, M_NULL),
MbufInquire(MilGridsImage, M_SIZE_Y, M_NULL),
8,
M_IMAGE+M_PROC+M_DISP,
&MilWorkGridImage);
MbufClear(MilMaskImage, 0);
MbufClear(MilWorkGridImage, 0);
MgraAlloc(MilSystem, &MilGraphicsContext);
MgraAllocList(MilSystem, M_DEFAULT, &MilGraphicList);
MdispControl(MilDisplay, M_UPDATE, M_DISABLE);
MgraLines(MilGraphicsContext, MilGraphicList, NUM_POLYGON_POINTS,
GridPolygon.x, GridPolygon.y, M_NULL, M_NULL, M_POLYGON);
MbufSetRegion(MilMaskImage, MilGraphicList, M_DEFAULT,
M_RASTERIZE+M_FILL_REGION, M_DEFAULT);
MbufClear(MilMaskImage, 0xFF);
MbufCopyCond(MilGridsImage, MilWorkGridImage, MilMaskImage, M_EQUAL, 0xFF);
McalControl(MilCalibration, M_GRID_CORNER_HINT_X, GridCalInfo.CornerHintX);
McalControl(MilCalibration, M_GRID_CORNER_HINT_Y, GridCalInfo.CornerHintY);
McalGrid(MilCalibration, MilWorkGridImage,
GridCalInfo.GridOffsetX,
GridCalInfo.GridOffsetY,
GridCalInfo.GridOffsetZ,
GridCalInfo.RowNumber,
GridCalInfo.ColumnNumber,
GridCalInfo.RowSpacing,
GridCalInfo.ColumnSpacing,
M_DEFAULT, M_DEFAULT);
MgraClear(M_DEFAULT, MilGraphicList);
MgraColor(MilGraphicsContext, ANNOTATIONS_COLOR);
MdispControl(MilDisplay, M_ASSOCIATED_GRAPHIC_LIST_ID, MilGraphicList);
McalDraw(MilGraphicsContext, MilCalibration, MilGraphicList, M_DRAW_IMAGE_POINTS,
M_DEFAULT, M_DEFAULT);
MdispControl(MilDisplay, M_UPDATE, M_ENABLE);
MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n"));
MbufCopy(MilWorkGridImage, MilDisplayImage);
MosGetch();
MdispControl(MilDisplay, M_ASSOCIATED_GRAPHIC_LIST_ID, M_NULL);
MgraFree(MilGraphicsContext);
MgraFree(MilGraphicList);
MbufFree(MilWorkGridImage);
MbufFree(MilMaskImage);
}
void Create3dCalibrationPoints(const CalibrationPointsStruct* CalibrationPoints2d,
CalibrationPointsStruct* CalibrationPoints3d)
{
CalibrationPoints3d->NumPoints = 0;
for (MIL_INT i=0; i<NUM_GRIDS; i++)
CalibrationPoints3d->NumPoints += CalibrationPoints2d[i].NumPoints;
CalibrationPoints3d->PixelPointsX = new MIL_DOUBLE[CalibrationPoints3d->NumPoints];
CalibrationPoints3d->PixelPointsY = new MIL_DOUBLE[CalibrationPoints3d->NumPoints];
CalibrationPoints3d->WorldPointsX = new MIL_DOUBLE[CalibrationPoints3d->NumPoints];
CalibrationPoints3d->WorldPointsY = new MIL_DOUBLE[CalibrationPoints3d->NumPoints];
CalibrationPoints3d->WorldPointsZ = new MIL_DOUBLE[CalibrationPoints3d->NumPoints];
MIL_INT Point3d=0;
MIL_INT MaxPoint = 0;
for (MIL_INT i=0; i<NUM_GRIDS; i++)
{
MaxPoint += CalibrationPoints2d[i].NumPoints;
for (MIL_INT j=0;Point3d<MaxPoint; j++,Point3d++)
{
CalibrationPoints3d->PixelPointsX[Point3d] =
CalibrationPoints2d[i].PixelPointsX[j];
CalibrationPoints3d->PixelPointsY[Point3d] =
CalibrationPoints2d[i].PixelPointsY[j];
if (i==eLeft)
{
CalibrationPoints3d->WorldPointsX[Point3d] =
CalibrationPoints2d[i].WorldPointsX[j];
CalibrationPoints3d->WorldPointsY[Point3d] = 0;
}
else if (i==eRight)
{
CalibrationPoints3d->WorldPointsX[Point3d] =
(LEFT_GRID_CAL_INFO.ColumnNumber+1)*
LEFT_GRID_CAL_INFO.ColumnSpacing;
CalibrationPoints3d->WorldPointsY[Point3d] =
CalibrationPoints2d[i].WorldPointsX[j];
}
else
{
CalibrationPoints3d->WorldPointsX[Point3d] =
CalibrationPoints2d[i].WorldPointsY[j];
CalibrationPoints3d->WorldPointsY[Point3d] =
CalibrationPoints2d[i].WorldPointsX[j];
}
}
}
for (MIL_INT i=0; i<CalibrationPoints3d->NumPoints; i++)
CalibrationPoints3d->WorldPointsZ[i] = 0;
MIL_INT CurrPoint=0;
MIL_INT TotalPoints=CalibrationPoints2d[eTop].NumPoints;
for (; CurrPoint<TotalPoints; CurrPoint++)
{
CalibrationPoints3d->WorldPointsZ[CurrPoint] =
(LEFT_GRID_CAL_INFO.RowNumber+1)*LEFT_GRID_CAL_INFO.RowSpacing+GRIDS_Z_OFFSET;
}
MIL_DOUBLE CurrentHeight=0;
TotalPoints+= CalibrationPoints2d[eLeft].NumPoints;
for (MIL_INT i=CalibrationPoints2d[eLeft].NumPoints; CurrPoint<TotalPoints;
i--,CurrPoint++)
{
CurrentHeight = ( ceil((MIL_DOUBLE)i/LEFT_GRID_CAL_INFO.ColumnNumber) ) *
LEFT_GRID_CAL_INFO.RowSpacing+GRIDS_Z_OFFSET;
CalibrationPoints3d->WorldPointsZ[CurrPoint] = CurrentHeight;
}
TotalPoints+= CalibrationPoints2d[eRight].NumPoints;
for (MIL_INT i=CalibrationPoints2d[eRight].NumPoints; CurrPoint<TotalPoints;
i--,CurrPoint++)
{
CurrentHeight = ( ceil((MIL_DOUBLE)i/RIGHT_GRID_CAL_INFO.ColumnNumber) ) *
RIGHT_GRID_CAL_INFO.RowSpacing+GRIDS_Z_OFFSET;
CalibrationPoints3d->WorldPointsZ[CurrPoint] = CurrentHeight;
}
}
void AllocAndGetCalibrationPoints(MIL_ID MilCalibration,
CalibrationPointsStruct& CalibrationPoints)
{
McalInquire(MilCalibration, M_NUMBER_OF_CALIBRATION_POINTS+M_TYPE_MIL_INT,
&CalibrationPoints.NumPoints);
CalibrationPoints.PixelPointsX = new MIL_DOUBLE[CalibrationPoints.NumPoints];
CalibrationPoints.PixelPointsY = new MIL_DOUBLE[CalibrationPoints.NumPoints];
CalibrationPoints.WorldPointsX = new MIL_DOUBLE[CalibrationPoints.NumPoints];
CalibrationPoints.WorldPointsY = new MIL_DOUBLE[CalibrationPoints.NumPoints];
CalibrationPoints.WorldPointsZ = M_NULL;
McalInquire(MilCalibration, M_CALIBRATION_IMAGE_POINTS_X,
CalibrationPoints.PixelPointsX);
McalInquire(MilCalibration, M_CALIBRATION_IMAGE_POINTS_Y,
CalibrationPoints.PixelPointsY);
McalInquire(MilCalibration, M_CALIBRATION_WORLD_POINTS_X,
CalibrationPoints.WorldPointsX);
McalInquire(MilCalibration, M_CALIBRATION_WORLD_POINTS_Y,
CalibrationPoints.WorldPointsY);
}
void DestroyCalibrationPoints(CalibrationPointsStruct& CalibrationPoints)
{
delete [] CalibrationPoints.PixelPointsX;
delete [] CalibrationPoints.PixelPointsY;
delete [] CalibrationPoints.WorldPointsX;
delete [] CalibrationPoints.WorldPointsY;
delete [] CalibrationPoints.WorldPointsZ;
}