#include <mil.h>
#include <math.h>
void PrintHeader()
{
MosPrintf(MIL_TEXT("[EXAMPLE NAME]\n"));
MosPrintf(MIL_TEXT("3dPlaneFit\n\n"));
MosPrintf(MIL_TEXT("[SYNOPSIS]\n"));
MosPrintf(MIL_TEXT("This example demonstrates the definition and usage of "));
MosPrintf(MIL_TEXT("a 3d plane fit.\n\n"));
MosPrintf(MIL_TEXT("[MODULES USED]\n"));
MosPrintf(MIL_TEXT("Modules used: application, system, display, buffer, graphic, ")
MIL_TEXT("calibration, \n3d reconstruction.\n\n"));
MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n"));
MosGetch();
}
struct ROIStruct
{
MIL_INT OffsetX;
MIL_INT OffsetY;
MIL_INT SizeX;
MIL_INT SizeY;
};
static const MIL_INT STRING_LENGTH = 1024;
static const MIL_TEXT_CHAR DEPTH_MAP_IMAGE_FILE[STRING_LENGTH] =
M_IMAGE_PATH MIL_TEXT("3dPlaneFit/MechanicalPartDepthMap.mim");
static const MIL_TEXT_CHAR RECONSTRUCTION_IMAGE_FILE[STRING_LENGTH] =
M_IMAGE_PATH MIL_TEXT("3dPlaneFit/Inclination.mim");
static const MIL_TEXT_CHAR SIDE_VIEW_IMAGE_FILE[STRING_LENGTH] =
M_IMAGE_PATH MIL_TEXT("3dPlaneFit/SideView.png");
static const MIL_DOUBLE DISPLAY_ZOOM_FACTOR_X = 0.75;
static const MIL_DOUBLE DISPLAY_ZOOM_FACTOR_Y = 0.75;
static const ROIStruct PLANE_REGION =
{
388,
349,
75,
75
};
static const MIL_INT NUM_LOCATIONS = 8;
static const ROIStruct MEASURE_REGION[NUM_LOCATIONS] =
{
{400, 440, 20, 20},
{316, 278, 20, 20},
{572, 544, 20, 20},
{660, 633, 20, 20},
{132, 174, 20, 20},
{580, 675, 20, 20},
{626, 785, 10, 10},
{600, 395, 20, 20}
};
MIL_DOUBLE GetDistanceErrorFactor(MIL_ID MilGeometry);
int MosMain(void)
{
MIL_ID MilApplication,
MilDisplay,
MilSystem,
MilDepthMapImage,
MilCalibration;
MIL_ID MilIllustrationImage;
MIL_ID MilIllustrationDisplay;
MIL_ID MilGeometry;
MIL_ID MilGraphics;
MIL_ID MilGraphicsList;
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);
MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED,
&MilIllustrationDisplay);
PrintHeader();
MgraAlloc(MilSystem, &MilGraphics);
MgraAllocList(MilSystem, M_DEFAULT, &MilGraphicsList);
MgraColor(MilGraphics, M_COLOR_GREEN);
MgraControl(MilGraphics, M_BACKGROUND_MODE, M_TRANSPARENT);
MdispControl(MilDisplay, M_ASSOCIATED_GRAPHIC_LIST_ID, MilGraphicsList);
MbufRestore(DEPTH_MAP_IMAGE_FILE, MilSystem, &MilDepthMapImage);
McalRestore(DEPTH_MAP_IMAGE_FILE, MilSystem, M_DEFAULT, &MilCalibration);
McalAssociate(MilCalibration, MilDepthMapImage, M_DEFAULT);
MdispZoom(MilDisplay, DISPLAY_ZOOM_FACTOR_X, DISPLAY_ZOOM_FACTOR_Y);
MdispControl(MilDisplay, M_VIEW_MODE, M_AUTO_SCALE);
MdispControl(MilDisplay, M_WINDOW_INITIAL_POSITION_X, 0);
MdispControl(MilDisplay, M_WINDOW_INITIAL_POSITION_Y, 0);
MdispControl(MilDisplay, M_TITLE,
(MIL_DOUBLE)M_PTR_TO_DOUBLE(MIL_TEXT("Depth Map")));
MdispSelect(MilDisplay, MilDepthMapImage);
MbufRestore(RECONSTRUCTION_IMAGE_FILE, MilSystem, &MilIllustrationImage);
MIL_INT InitialWindowPositionX =
(MIL_INT)(MbufInquire(MilDepthMapImage, M_SIZE_X, M_NULL)*
DISPLAY_ZOOM_FACTOR_X+20);
MdispControl(MilIllustrationDisplay, M_WINDOW_INITIAL_POSITION_X,
(MIL_DOUBLE)(InitialWindowPositionX));
MdispControl(MilIllustrationDisplay, M_WINDOW_INITIAL_POSITION_Y, 0);
MdispControl(MilIllustrationDisplay, M_TITLE,
(MIL_DOUBLE)M_PTR_TO_DOUBLE(MIL_TEXT("3D Reconstruction")));
MdispSelect(MilIllustrationDisplay, MilIllustrationImage);
MIL_ID MilMaskImage;
MbufAlloc2d(MilSystem, MbufInquire(MilDepthMapImage, M_SIZE_X, M_NULL),
MbufInquire(MilDepthMapImage, M_SIZE_Y, M_NULL),
MbufInquire(MilDepthMapImage, M_TYPE, M_NULL),
M_IMAGE+M_PROC, &MilMaskImage);
MbufClear(MilMaskImage, 0);
MIL_ID MilMaskChild;
MbufChild2d(MilMaskImage, PLANE_REGION.OffsetX, PLANE_REGION.OffsetY,
PLANE_REGION.SizeX, PLANE_REGION.SizeY, &MilMaskChild);
MbufClear(MilMaskChild, 65535);
MgraRect(MilGraphics, MilGraphicsList, PLANE_REGION.OffsetX, PLANE_REGION.OffsetY,
PLANE_REGION.OffsetX+PLANE_REGION.SizeX, PLANE_REGION.OffsetY+PLANE_REGION.SizeY);
MosPrintf(MIL_TEXT("The displayed depth map of a 3d reconstruction shows a mechanical ")
MIL_TEXT("part\nwith an inclination relative to the z plane as illustrated ")
MIL_TEXT("on the right.\nUsing the depth map region shown in green, a plane ")
MIL_TEXT("with this inclination\nwill be fitted and used to calculate ")
MIL_TEXT("relative measurements for the object.\n"));
MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n"));
MosGetch();
M3dmapAlloc(MilSystem, M_GEOMETRY, M_DEFAULT, &MilGeometry);
M3dmapSetGeometry(MilGeometry, M_PLANE, M_FIT, (MIL_DOUBLE)MilDepthMapImage,
(MIL_DOUBLE)MilMaskImage, M_DEFAULT, M_DEFAULT, M_DEFAULT);
MIL_DOUBLE DistanceErrorFactor = GetDistanceErrorFactor(MilGeometry);
if(M3dmapInquire(MilGeometry, M_DEFAULT, M_STATUS, M_NULL) == M_SUCCESS)
{
MdispControl(MilDisplay, M_UPDATE_GRAPHIC_LIST, M_DISABLE);
MgraColor(MilGraphics, M_COLOR_CYAN);
MIL_ID MilDepthMapChild;
MIL_TEXT_CHAR OutputText[STRING_LENGTH] = MIL_TEXT("");
MIL_DOUBLE AverageHeight=0.0;
for (MIL_INT i=0; i<NUM_LOCATIONS; i++)
{
MbufChild2d(MilDepthMapImage, MEASURE_REGION[i].OffsetX,
MEASURE_REGION[i].OffsetY, MEASURE_REGION[i].SizeX,
MEASURE_REGION[i].SizeY, &MilDepthMapChild);
M3dmapStat(MilDepthMapChild, MilGeometry, M_NULL, M_NULL, M_DEVIATION_MEAN,
M_DEFAULT, M_DEFAULT, &AverageHeight);
AverageHeight *= DistanceErrorFactor;
MgraRect(MilGraphics, MilGraphicsList, MEASURE_REGION[i].OffsetX,
MEASURE_REGION[i].OffsetY, MEASURE_REGION[i].OffsetX+MEASURE_REGION[i].SizeX,
MEASURE_REGION[i].OffsetY+MEASURE_REGION[i].SizeY);
MosSprintf(OutputText, STRING_LENGTH, MIL_TEXT("%.2f"), AverageHeight);
MgraText(MilGraphics, MilGraphicsList,
MEASURE_REGION[i].OffsetX+MEASURE_REGION[i].SizeX+10,
MEASURE_REGION[i].OffsetY, OutputText);
MbufFree(MilDepthMapChild);
}
MdispSelect(MilIllustrationDisplay, M_NULL);
MbufFree(MilIllustrationImage);
MbufRestore(SIDE_VIEW_IMAGE_FILE, MilSystem, &MilIllustrationImage);
MdispControl(MilIllustrationDisplay, M_TITLE,
(MIL_DOUBLE)M_PTR_TO_DOUBLE(MIL_TEXT("Height side view")));
MdispSelect(MilIllustrationDisplay, MilIllustrationImage);
MdispControl(MilDisplay, M_UPDATE_GRAPHIC_LIST, M_ENABLE);
MosPrintf(MIL_TEXT("The average differences in height of the shown regions are ")
MIL_TEXT("displayed in\ncyan. These values are relative to the defined ")
MIL_TEXT("plane, displayed in green.\n\n"));
MosPrintf(MIL_TEXT("Press <Enter> to end.\n\n"));
MosGetch();
}
else
{
MosPrintf(MIL_TEXT("Plane fit unsuccessful.\n"));
MosPrintf(MIL_TEXT("Press <Enter> to end.\n\n"));
MosGetch();
}
MgraFree(MilGraphics);
MgraFree(MilGraphicsList);
M3dmapFree(MilGeometry);
MbufFree(MilMaskChild);
MbufFree(MilMaskImage);
McalFree(MilCalibration);
MbufFree(MilDepthMapImage);
MbufFree(MilIllustrationImage);
MdispFree(MilDisplay);
MdispFree(MilIllustrationDisplay);
MsysFree(MilSystem);
MappFree(MilApplication);
return 0;
}
MIL_DOUBLE GetDistanceErrorFactor(MIL_ID MilGeometry)
{
MIL_DOUBLE A, B, Z0;
M3dmapInquire(MilGeometry, M_DEFAULT, M_FIT_PARAM_AX, &A);
M3dmapInquire(MilGeometry, M_DEFAULT, M_FIT_PARAM_AY, &B);
M3dmapInquire(MilGeometry, M_DEFAULT, M_FIT_PARAM_Z0, &Z0);
MIL_DOUBLE PlaneNormalsDotProduct = -Z0;
MIL_DOUBLE RefVectorLength = sqrt(A*A+B*B+Z0*Z0);
MIL_DOUBLE Z0VectorLength = 1;
return PlaneNormalsDotProduct/(RefVectorLength*Z0VectorLength);
}