#include <mil.h>
#include <Windows.h>
#include <float.h>
#include <math.h>
#include "KinectCamera.h"
#include "KinectCameraStandalone.h"
#include "BestPlaneFitter.h"
#include "DepthDataMgr.h"
#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE
#include "MdispD3D.h"
static const MIL_INT D3D_DISPLAY_SIZE_X = 640;
static const MIL_INT D3D_DISPLAY_SIZE_Y = 480;
static const MIL_DOUBLE MIN_Z = 0;
static const MIL_DOUBLE MAX_Z = 3000;
static const MIL_DOUBLE MAX_DISTANCE_Z = 100;
#endif
void PrintHeader()
{
MosPrintf(MIL_TEXT("[EXAMPLE NAME]\n")
MIL_TEXT("Microsoft Kinect\n\n")
MIL_TEXT("[SYNOPSIS]\n")
MIL_TEXT("This example illustrates how to interface with the Microsoft Kinect to \n")
MIL_TEXT("create and use depth maps in MIL. Specifically, this example shows:\n")
MIL_TEXT(" - Kinect camera system calibration.\n")
MIL_TEXT(" - Acquisition of color and depth images.\n")
MIL_TEXT(" - Mapping of the color image on the depth image.\n")
MIL_TEXT(" - Extraction of the depth map and mapping of the color image.\n")
MIL_TEXT(" - Plane fitting of the floor.\n")
MIL_TEXT(" - Localization of the 3D bounding box of an object on the floor.\n\n")
MIL_TEXT("The example is designed to work with both the first generation Kinect\n")
MIL_TEXT("and the second generation Kinect, also known as the Kinect One.\n")
MIL_TEXT("Change KINECT_CAMERA_VERSION to select the Kinect generation to use.\n\n")
MIL_TEXT("[MODULES USED]\n")
MIL_TEXT("Modules used: Application, system, image, calibration, 3dmap\n\n")
MIL_TEXT("Press <Enter> to continue.\n\n"));
MosGetch();
}
struct SGetDepthProcFuncData
{
CKinectCameraInterface* pKinectCamera;
MIL_ID MilDepthImageDisplay;
MIL_ID MilColorImage;
MIL_ID MilDepthImage;
MIL_ID MilDepthProcImage;
MIL_ID MilDepthProc2Image;
MIL_ID MilDepthThresholdImage;
};
struct SGetDepthColorProcFuncData
{
SGetDepthProcFuncData GetDepthData;
MIL_ID MilColorCameraCalibration;
CDepthDataMgr* pDepthDataMgr;
MIL_ID MilColorDepthOrWorldImage;
bool IsDepthMapLut;
};
struct SGet3DMeshColorProcFuncData
{
SGetDepthColorProcFuncData GetDepthColorData;
MIL_ID MilColorGraList;
MIL_ID MilColorDisplay;
MIL_ID MilDepthMapImage;
MIL_ID MilDepthMapDisplay;
MIL_ID MilColorDepthMapImage;
MIL_ID MilColorDepthMapDisplay;
CBestPlaneFitter* pBestPlaneFitter;
MIL_ID MilBinDepthMapImage;
MIL_ID MilBlobResult;
MIL_ID MilBlobFeatureList;
MIL_DISP_D3D_HANDLE DispD3DHandle;
MIL_ID Mil3dDepthMapImage;
MIL_ID Mil3dColorDepthMapImage;
MIL_ID MilD3DSetImagesThread;
MIL_ID MilD3DUpdateEvent;
MIL_ID MilStopUpdate3dDisplayEvent;
bool BoxFound;
};
struct SBox
{
MIL_DOUBLE CenterPosX;
MIL_DOUBLE CenterPosY;
MIL_DOUBLE CenterPosZ;
MIL_DOUBLE ZAxisAngle;
MIL_DOUBLE Width;
MIL_DOUBLE Length;
MIL_DOUBLE Height;
};
void CalibrateCamera(CKinectCameraInterface* pKinectCamera,
MIL_ID MilCameraCalibration,
MIL_ID MilGrabImage,
MIL_ID MilDisplay,
ColorStreamTypeEnum ColorStreamType);
void MoveColorToolCoordSystemOnIRCameraCoordSystem(MIL_ID MilSystem,
MIL_ID MilIRCameraCalibration,
MIL_ID MilColorCameraCalibration);
bool FindObjectOnFloor(MIL_ID MilDepthMapImage,
MIL_ID MilBinDepthMapImage,
MIL_ID MilBlobResult,
MIL_ID MilBlobFeatureList,
MIL_DOUBLE FloorZ,
MIL_DOUBLE MinThresholdZ,
MIL_DOUBLE MaxThresholdZ,
SBox* pFoundBox);
void DrawFound3dBox(MIL_ID MilGraContext, MIL_ID MilDrawDest, MIL_ID MilCalibration, const SBox& rFoundBox);
MIL_UINT32 MFTYPE Update3dDisplayThread(void *UserDataPtr);
void Update3dDisplay(const SGet3DMeshColorProcFuncData* pGet3DMeshColorData);
void SetupDisplay(MIL_ID MilDisplay, MIL_CONST_TEXT_PTR WindowTitle, MIL_INT WindowPosX, MIL_INT WindowPosY);
void FatalError(CKinectCameraInterface* pKinectCamera, const MIL_TEXT_CHAR* pMsg);
void FillDisplayLut(MIL_ID MilDisplayLut);
MIL_INT MFTYPE CopyFunc(MIL_INT HookType, MIL_ID MilKinectImage, void* UserDataPtr);
MIL_INT MFTYPE GetDepthProcFunc(MIL_INT HookType, MIL_ID MilKinectImage, void* UserDataPtr);
MIL_INT MFTYPE GetDepthColorProcFunc(MIL_INT HookType, MIL_ID MilKinectImage, void* UserDataPtr);
MIL_INT MFTYPE Get3DMeshColorProcFunc(MIL_INT HookType, MIL_ID MilKinectImage, void* UserDataPtr);
static const MIL_INT DEPTH_MAP_SIZE_X = 400;
static const MIL_INT DEPTH_MAP_SIZE_Y = 300;
static const MIL_DOUBLE WORLD_POS_X = -1000.0;
static const MIL_DOUBLE WORLD_POS_Y = WORLD_POS_X * (MIL_DOUBLE)DEPTH_MAP_SIZE_Y / DEPTH_MAP_SIZE_X;
static const MIL_DOUBLE WORLD_POS_Z = 400.0;
static const MIL_DOUBLE PIXEL_SIZE_X = (2 * -WORLD_POS_X) / DEPTH_MAP_SIZE_X;
static const MIL_DOUBLE PIXEL_SIZE_Y = PIXEL_SIZE_X;
static const MIL_DOUBLE GRAY_LEVEL_SIZE_Z = 1.0;
static const SDepthMapGenParam KINECT_DEPTH_PARAM = {WORLD_POS_X, WORLD_POS_Y, WORLD_POS_Z,
PIXEL_SIZE_X, PIXEL_SIZE_Y, GRAY_LEVEL_SIZE_Z};
static const MIL_DOUBLE TOP_VIEW_WORLD_POS_Z = -1500;
static const MIL_DOUBLE OBJECT_THRESHOLD_Z = 100;
static const MIL_DOUBLE TOP_REFINE_RANGE = 20;
static const SDepthMapGenParam TOP_DEPTH_PARAM = {WORLD_POS_X, WORLD_POS_Y, TOP_VIEW_WORLD_POS_Z,
PIXEL_SIZE_X, PIXEL_SIZE_Y, GRAY_LEVEL_SIZE_Z};
static const MIL_DOUBLE GRID_WORLD_OFFSET_X = 0.0;
static const MIL_DOUBLE GRID_WORLD_OFFSET_Y = 0.0;
static const MIL_INT GRID_NB_ROWS = 15;
static const MIL_INT GRID_NB_COLUMNS = 11;
static const MIL_DOUBLE GRID_SPACING = 50;
static const MIL_DOUBLE MIN_BLOB_AREA = 20000;
static const MIL_DOUBLE MIN_BLOB_FERET_MIN = 100;
static const MIL_DOUBLE MAX_SIGMA_PIXEL = 75;
static const MIL_DOUBLE MIN_RECTANGULARITY = 0.7;
static const MIL_INT NB_FERETS = M_INFINITE;
static const MIL_INT DEPTH_THIRD_DERIV_FACTOR = 16;
static const MIL_INT DISPLAY_TEXT_LINE_OFFSET = 16;
static const MIL_INT WINDOWS_OFFSET_X = 15;
static const MIL_INT WINDOWS_OFFSET_Y = 38;
static const MIL_INT DEPTH_DISPLAY_X = 0;
static const MIL_INT DEPTH_DISPLAY_Y = 0;
static const MIL_INT COLOR_DISPLAY_X = KINECT_DEPTH_IMAGE_SIZE_X + 100;
static const MIL_INT COLOR_DISPLAY_Y = 0;
static const MIL_INT COLOR_DEPTH_DISPLAY_X = 100;
static const MIL_INT COLOR_DEPTH_DISPLAY_Y = 0;
static const MIL_INT DEPTH_MAP_DISPLAY_X = 0;
static const MIL_INT DEPTH_MAP_DISPLAY_Y = D3D_DISPLAY_SIZE_Y + WINDOWS_OFFSET_Y;
static const MIL_INT COLOR_MAP_DISPLAY_X = DEPTH_MAP_SIZE_X + WINDOWS_OFFSET_X;
static const MIL_INT COLOR_MAP_DISPLAY_Y = D3D_DISPLAY_SIZE_Y + WINDOWS_OFFSET_Y;
int MosMain(void)
{
MIL_ID MilApplication = MappAlloc(M_DEFAULT, M_NULL);
MIL_ID MilSystem = MsysAlloc(M_SYSTEM_HOST, M_DEFAULT, M_DEFAULT, M_NULL);
MIL_ID MilDepthImageDisplay = MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, M_NULL);
SetupDisplay(MilDepthImageDisplay, MIL_TEXT("Depth Image"), DEPTH_DISPLAY_X, DEPTH_DISPLAY_Y);
MIL_ID MilColorDisplay = MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, M_NULL);
SetupDisplay(MilColorDisplay, MIL_TEXT("Color Image"), COLOR_DISPLAY_X, COLOR_DISPLAY_Y);
MIL_ID MilColorDepthImageDisplay = MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, M_NULL);
SetupDisplay(MilColorDepthImageDisplay, MIL_TEXT("Color Depth Image"), COLOR_DEPTH_DISPLAY_X, COLOR_DEPTH_DISPLAY_Y);
MIL_ID MilDepthMapDisplay = MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, M_NULL);
SetupDisplay(MilDepthMapDisplay, MIL_TEXT("Depth Map"), DEPTH_MAP_DISPLAY_X, DEPTH_MAP_DISPLAY_Y);
MIL_ID MilColorDepthMapDisplay = MdispAlloc(MilSystem, M_DEFAULT, MIL_TEXT("M_DEFAULT"), M_WINDOWED, M_NULL);
SetupDisplay(MilColorDepthMapDisplay, MIL_TEXT("Color Depth Map"), COLOR_MAP_DISPLAY_X, COLOR_MAP_DISPLAY_Y);
MIL_ID MilColorGraList = MgraAllocList(MilSystem, M_DEFAULT, M_NULL);
MdispControl(MilColorDisplay, M_ASSOCIATED_GRAPHIC_LIST_ID, MilColorGraList);
MosPrintf(MIL_TEXT("MICROSOFT KINECT:\n"));
MosPrintf(MIL_TEXT("---------------------------------------\n\n"));
PrintHeader();
CKinectCameraInterface* pKinectCamera;
#if USE_REAL_CAMERA
#if KINECT_CAMERA_VERSION == 1
pKinectCamera = new CKinectCamera(MilSystem);
#elif KINECT_CAMERA_VERSION == 2
pKinectCamera = new CKinectOneCamera(MilSystem);
#else
FatalError(pKinectCamera, MIL_TEXT("Unable to create camera sensor. Invalid KINECT_CAMERA_VERSION.\n"));
#endif
#else
pKinectCamera = new CKinectCameraStandalone(MilSystem);
#endif
if(pKinectCamera->GetCameraStatus() != KINECT_CAMERA_OK)
FatalError(pKinectCamera, MIL_TEXT("Unable to create camera sensor.\n"));
MIL_ID MilColorImage = MbufAllocColor(MilSystem, 3, KINECT_COLOR_IMAGE_SIZE_X, KINECT_COLOR_IMAGE_SIZE_Y, 8+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP, M_NULL);
MIL_ID MilColorDepthImage = MbufAllocColor(MilSystem, 3, KINECT_DEPTH_IMAGE_SIZE_X, KINECT_DEPTH_IMAGE_SIZE_Y, 8+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP+M_BGR32+M_PACKED, M_NULL);
MIL_ID MilColorWorldImageContainer = MbufAllocColor(MilSystem, 3, KINECT_DEPTH_IMAGE_SIZE_X*KINECT_DEPTH_IMAGE_SIZE_Y, 1, 8 + M_UNSIGNED, M_IMAGE + M_PROC + M_DISP + M_BGR32 + M_PACKED, M_NULL);
MIL_ID MilColorWorldImage = MbufChild1d(MilColorWorldImageContainer, 0, 1, M_NULL);
MIL_ID MilColorDepthMapImage = MbufAllocColor(MilSystem, 3, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, 8+M_UNSIGNED, M_IMAGE+M_PROC+M_BGR32+M_PACKED+M_DISP, M_NULL);
MIL_ID Mil3dColorDepthMapImage = MbufAllocColor(MilSystem, 3, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, 8+M_UNSIGNED, M_IMAGE+M_PROC+M_BGR32+M_PACKED+M_DISP, M_NULL);
MIL_ID MilDepthImage = MbufAlloc2d(MilSystem, KINECT_DEPTH_IMAGE_SIZE_X, KINECT_DEPTH_IMAGE_SIZE_Y, 16+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP, M_NULL);
MIL_ID MilDepthProcImage = MbufAlloc2d(MilSystem, KINECT_DEPTH_IMAGE_SIZE_X, KINECT_DEPTH_IMAGE_SIZE_Y, 16+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP, M_NULL);
MIL_ID MilDepthProc2Image = MbufAlloc2d(MilSystem, KINECT_DEPTH_IMAGE_SIZE_X, KINECT_DEPTH_IMAGE_SIZE_Y, 16 + M_UNSIGNED, M_IMAGE + M_PROC + M_DISP, M_NULL);
MIL_ID MilDepthThresholdImage = MbufAlloc2d(MilSystem, KINECT_DEPTH_IMAGE_SIZE_X, KINECT_DEPTH_IMAGE_SIZE_Y, 16 + M_UNSIGNED, M_IMAGE + M_PROC + M_DISP, M_NULL);
MIL_ID MilDepthMapImage = MbufAlloc2d(MilSystem, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, 16+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP, M_NULL);
MIL_ID MilBinDepthMapImage = MbufAlloc2d(MilSystem, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, 8+M_UNSIGNED, M_IMAGE+M_PROC, M_NULL);
MIL_ID Mil3dDepthMapImage = MbufAlloc2d(MilSystem, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, 16+M_UNSIGNED, M_IMAGE+M_PROC+M_DISP, M_NULL);
MIL_ID MilBlobResult = MblobAllocResult(MilSystem, M_NULL);
MIL_ID MilBlobFeatureList = MblobAllocFeatureList(MilSystem, M_NULL);
MblobControl(MilBlobResult, M_RESULT_OUTPUT_UNITS, M_WORLD);
MblobControl(MilBlobResult, M_INPUT_SELECT_UNITS, M_WORLD);
MblobControl(MilBlobResult, M_NUMBER_OF_FERETS, NB_FERETS);
MblobSelectFeature(MilBlobFeatureList, M_RECTANGULARITY);
MblobSelectFeature(MilBlobFeatureList, M_MEAN_PIXEL);
MblobSelectFeature(MilBlobFeatureList, M_SIGMA_PIXEL);
MblobSelectFeature(MilBlobFeatureList, M_MIN_AREA_BOX);
MIL_ID MilColorCameraCalibration = McalAlloc(MilSystem, M_TSAI_BASED, M_DEFAULT, M_NULL);
MIL_ID MilIRCameraCalibration = McalAlloc(MilSystem, M_TSAI_BASED, M_DEFAULT, M_NULL);
MIL_ID MilDisplayLut = MbufAllocColor(MilSystem, 3, MIL_UINT16_MAX, 1, 8+M_UNSIGNED, M_LUT, M_NULL);
FillDisplayLut(MilDisplayLut);
MdispLut(MilDepthImageDisplay, MilDisplayLut);
MdispLut(MilDepthMapDisplay, MilDisplayLut);
MbufClear(MilDepthImage, 0.0);
MdispSelect(MilColorDisplay, MilDepthImage);
MdispControl(MilColorDisplay, M_VIEW_MODE, M_BIT_SHIFT);
MdispControl(MilColorDisplay, M_VIEW_BIT_SHIFT, 8);
CalibrateCamera(pKinectCamera, MilIRCameraCalibration, MilDepthImage, MilColorDisplay, enIR);
MdispSelect(MilColorDisplay, MilColorImage);
MdispControl(MilColorDisplay, M_VIEW_MODE, M_DEFAULT);
CalibrateCamera(pKinectCamera, MilColorCameraCalibration, MilColorImage, MilColorDisplay, enColor);
McalControl(MilColorCameraCalibration, M_LINK_TOOL_AND_CAMERA, M_DISABLE);
MoveColorToolCoordSystemOnIRCameraCoordSystem(MilSystem, MilIRCameraCalibration, MilColorCameraCalibration);
if(pKinectCamera->InitCamera(enColor, true) != KINECT_CAMERA_OK)
FatalError(pKinectCamera, MIL_TEXT("Unable to initialize camera system.\n"));
SGetDepthProcFuncData GetDepthData;
GetDepthData.pKinectCamera = pKinectCamera;
GetDepthData.MilColorImage = MilColorImage;
GetDepthData.MilDepthImage = MilDepthImage;
GetDepthData.MilDepthProcImage = MilDepthProcImage;
GetDepthData.MilDepthProc2Image = MilDepthProc2Image;
GetDepthData.MilDepthThresholdImage = MilDepthThresholdImage;
GetDepthData.MilDepthImageDisplay = MilDepthImageDisplay;
MdispSelect(MilDepthImageDisplay, MilDepthImage);
pKinectCamera->FrameGrabberProcess(M_START, M_ASYNCHRONOUS, M_DEFAULT, &GetDepthProcFunc, &GetDepthData);
MosPrintf(MIL_TEXT("The depth images from the Kinect are displayed.\n")
MIL_TEXT("Press <Enter> to stop the grab.\n\n"));
MosGetch();
pKinectCamera->FrameGrabberProcess(M_STOP, M_DEFAULT);
SGetDepthColorProcFuncData GetDepthColorData;
GetDepthColorData.GetDepthData = GetDepthData;
GetDepthColorData.MilColorCameraCalibration = MilColorCameraCalibration;
GetDepthColorData.MilColorDepthOrWorldImage = MilColorDepthImage;
GetDepthColorData.IsDepthMapLut = false;
CDepthDataMgr* pDepthDataMgr = new CDepthDataMgr(MilSystem, KINECT_DEPTH_IMAGE_SIZE_X, KINECT_DEPTH_IMAGE_SIZE_Y, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y, KINECT_COLOR_IMAGE_SIZE_X, KINECT_COLOR_IMAGE_SIZE_Y);
pDepthDataMgr->CalculateMultipliers(MilIRCameraCalibration);
GetDepthColorData.pDepthDataMgr = pDepthDataMgr;
MdispSelect(MilColorDepthImageDisplay, MilColorDepthImage);
pKinectCamera->FrameGrabberProcess(M_START, M_ASYNCHRONOUS, M_DEFAULT, &GetDepthColorProcFunc, &GetDepthColorData);
MosPrintf(MIL_TEXT("The colored depth images are displayed.\n"));
MosPrintf(MIL_TEXT("Press <Enter> to stop the grab.\n\n"));
MosGetch();
pKinectCamera->FrameGrabberProcess(M_STOP, M_DEFAULT);
if(pDepthDataMgr->GetNbWorld())
{
pDepthDataMgr->GenColorLuts(MilColorCameraCalibration, true);
MbufChildMove(MilColorWorldImage, 0, 0, pDepthDataMgr->GetNbWorld(), 1, M_DEFAULT);
pDepthDataMgr->WarpColorImage(MilColorImage, MilColorWorldImage, true);
pDepthDataMgr->CreateDepthMap(MilColorWorldImage, MilDepthMapImage, MilColorDepthMapImage, KINECT_DEPTH_PARAM);
}
#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE
MIL_DISP_D3D_HANDLE DispD3DHandle = M_NULL;
DispD3DHandle = MdepthD3DAlloc(MilDepthMapImage, MilColorDepthMapImage,
D3D_DISPLAY_SIZE_X,
D3D_DISPLAY_SIZE_Y,
M_DEFAULT,
M_DEFAULT,
M_DEFAULT,
MIN_Z,
MAX_Z,
MAX_DISTANCE_Z,
M_NULL);
if (DispD3DHandle != NULL)
{
MdispD3DShow(DispD3DHandle);
MdispD3DPrintHelp(DispD3DHandle);
}
#endif
SGet3DMeshColorProcFuncData Get3DMeshColorData;
Get3DMeshColorData.GetDepthColorData = GetDepthColorData;
Get3DMeshColorData.MilColorGraList = MilColorGraList;
Get3DMeshColorData.MilColorDisplay = MilColorDisplay;
Get3DMeshColorData.MilDepthMapImage = MilDepthMapImage;
Get3DMeshColorData.MilDepthMapDisplay = MilDepthMapDisplay;
Get3DMeshColorData.MilColorDepthMapImage = MilColorDepthMapImage;
Get3DMeshColorData.MilColorDepthMapDisplay = MilColorDepthMapDisplay;
Get3DMeshColorData.Mil3dDepthMapImage = Mil3dDepthMapImage;
Get3DMeshColorData.Mil3dColorDepthMapImage = Mil3dColorDepthMapImage;
Get3DMeshColorData.DispD3DHandle = DispD3DHandle;
CBestPlaneFitter* pBestPlaneFitter = new CBestPlaneFitter(MilSystem, DEPTH_MAP_SIZE_X, DEPTH_MAP_SIZE_Y);
Get3DMeshColorData.pBestPlaneFitter = pBestPlaneFitter;
Get3DMeshColorData.MilBinDepthMapImage = MilBinDepthMapImage;
Get3DMeshColorData.MilBlobResult = MilBlobResult;
Get3DMeshColorData.MilBlobFeatureList = MilBlobFeatureList;
Get3DMeshColorData.BoxFound = false;
Get3DMeshColorData.GetDepthColorData.IsDepthMapLut = true;
Get3DMeshColorData.GetDepthColorData.MilColorDepthOrWorldImage = MilColorWorldImage;
MthrAlloc(MilSystem, M_EVENT, M_NOT_SIGNALED + M_MANUAL_RESET, M_NULL, M_NULL, &Get3DMeshColorData.MilStopUpdate3dDisplayEvent);
MthrAlloc(MilSystem, M_EVENT, M_NOT_SIGNALED + M_AUTO_RESET, M_NULL, M_NULL, &Get3DMeshColorData.MilD3DUpdateEvent);
MthrAlloc(MilSystem, M_THREAD, M_DEFAULT, Update3dDisplayThread,
&Get3DMeshColorData, &Get3DMeshColorData.MilD3DSetImagesThread);
MdispSelect(MilColorDepthImageDisplay, M_NULL);
MdispSelect(MilColorDepthMapDisplay, MilColorDepthMapImage);
MdispSelect(MilDepthMapDisplay, MilDepthMapImage);
pKinectCamera->FrameGrabberProcess(M_START, M_ASYNCHRONOUS, M_DEFAULT, &Get3DMeshColorProcFunc, &Get3DMeshColorData);
MosPrintf(MIL_TEXT("The corrected and colored depth map as well as the fitted floor\n")
MIL_TEXT("and the 3D bounding box are displayed live.\n")
MIL_TEXT("Press <Enter> to stop the grab.\n\n"));
MosGetch();
pKinectCamera->FrameGrabberProcess(M_STOP, M_DEFAULT);
if(Get3DMeshColorData.BoxFound)
MosPrintf(MIL_TEXT("The final corrected and colored depth map as well as the fitted floor \n")
MIL_TEXT("and the 3D bounding box are displayed.\n")
MIL_TEXT("Press <Enter> to end.\n\n"));
else
MosPrintf(MIL_TEXT("The final corrected and colored depth map as well as the fitted floor \n")
MIL_TEXT("are displayed.\n")
MIL_TEXT("Press <Enter> to end.\n\n"));
MosGetch();
MthrControl(Get3DMeshColorData.MilStopUpdate3dDisplayEvent, M_EVENT_SET, M_SIGNALED);
MthrControl(Get3DMeshColorData.MilD3DUpdateEvent, M_EVENT_SET, M_SIGNALED);
MthrWait(Get3DMeshColorData.MilD3DSetImagesThread, M_THREAD_END_WAIT, M_NULL);
MthrFree(Get3DMeshColorData.MilD3DSetImagesThread);
MthrFree(Get3DMeshColorData.MilD3DUpdateEvent);
MthrFree(Get3DMeshColorData.MilStopUpdate3dDisplayEvent);
#if M_MIL_USE_WINDOWS && !M_MIL_USE_CE
if (DispD3DHandle != M_NULL)
{
MdispD3DHide(DispD3DHandle);
MdispD3DFree(DispD3DHandle);
}
#endif
delete pBestPlaneFitter;
delete pDepthDataMgr;
pKinectCamera->CloseDown();
delete pKinectCamera;
McalFree(MilColorCameraCalibration);
McalFree(MilIRCameraCalibration);
MbufFree(MilDisplayLut);
MbufFree(MilColorDepthMapImage);
MbufFree(MilColorWorldImage);
MbufFree(MilColorWorldImageContainer);
MbufFree(MilColorDepthImage);
MbufFree(MilColorImage);
MbufFree(MilDepthMapImage);
MbufFree(MilDepthThresholdImage);
MbufFree(MilDepthProc2Image);
MbufFree(MilDepthProcImage);
MbufFree(MilDepthImage);
MbufFree(Mil3dColorDepthMapImage);
MbufFree(Mil3dDepthMapImage);
MbufFree(MilBinDepthMapImage);
MblobFree(MilBlobResult);
MblobFree(MilBlobFeatureList);
MgraFree(MilColorGraList);
MdispFree(MilColorDisplay);
MdispFree(MilDepthImageDisplay);
MdispFree(MilColorDepthImageDisplay);
MdispFree(MilDepthMapDisplay);
MdispFree(MilColorDepthMapDisplay);
MsysFree(MilSystem);
MappFree(MilApplication);
return 0;
}
MIL_INT MFTYPE CopyFunc(MIL_INT HookType, MIL_ID MilKinectImage, void* UserDataPtr)
{
MIL_ID* pMilImage = (MIL_ID*) UserDataPtr;
CKinectCameraInterface::ConvertKinectDepthToMilDepth(MilKinectImage, *pMilImage);
return M_NULL;
}
MIL_INT MFTYPE GetDepthProcFunc(MIL_INT HookType, MIL_ID MilKinectImage, void* UserDataPtr)
{
SGetDepthProcFuncData* pGetDepthData = static_cast<SGetDepthProcFuncData*>(UserDataPtr);
if (MbufInquire(MilKinectImage, M_SIZE_BAND, M_NULL) == 3)
CKinectCameraInterface::ConvertKinectColorToMilColor(MilKinectImage, pGetDepthData->MilColorImage);
else
{
MdispControl(pGetDepthData->MilDepthImageDisplay, M_UPDATE, M_DISABLE);
CKinectCameraInterface::ConvertKinectDepthToMilDepth(MilKinectImage, pGetDepthData->MilDepthImage);
#if KINECT_CAMERA_VERSION == 2
MimRank(pGetDepthData->MilDepthImage, pGetDepthData->MilDepthProcImage, M_5X5_RECT, M_MEDIAN, M_GRAYSCALE);
MimArith(pGetDepthData->MilDepthProcImage, DEPTH_THIRD_DERIV_FACTOR, pGetDepthData->MilDepthThresholdImage, M_DIV_CONST);
MimConvolve(pGetDepthData->MilDepthProcImage, pGetDepthData->MilDepthProc2Image, M_EDGE_DETECT_SOBEL_FAST);
MimConvolve(pGetDepthData->MilDepthProc2Image, pGetDepthData->MilDepthProcImage, M_LAPLACIAN_8);
MimConvolve(pGetDepthData->MilDepthProcImage, pGetDepthData->MilDepthProc2Image, M_SMOOTH);
MimArith(pGetDepthData->MilDepthThresholdImage, pGetDepthData->MilDepthProc2Image, pGetDepthData->MilDepthProc2Image, M_SUB + M_SATURATION);
MimErode(pGetDepthData->MilDepthProc2Image, pGetDepthData->MilDepthProcImage, 1, M_BINARY);
MbufClearCond(pGetDepthData->MilDepthImage, 0, 0, 0, pGetDepthData->MilDepthProcImage, M_EQUAL, 0);
#endif
MdispControl(pGetDepthData->MilDepthImageDisplay, M_UPDATE, M_ENABLE);
}
return M_NULL;
}
MIL_INT MFTYPE GetDepthColorProcFunc(MIL_INT HookType, MIL_ID MilKinectImage, void* UserDataPtr)
{
SGetDepthColorProcFuncData* pGetDepthColorData = static_cast<SGetDepthColorProcFuncData*>(UserDataPtr);
SGetDepthProcFuncData& GetDepthData = pGetDepthColorData->GetDepthData;
CDepthDataMgr* pDepthDataMgr = pGetDepthColorData->pDepthDataMgr;
GetDepthProcFunc(HookType, MilKinectImage, &GetDepthData);
if(MbufInquire(MilKinectImage, M_SIZE_BAND, M_NULL) == 1)
{
if(pGetDepthColorData->IsDepthMapLut)
pDepthDataMgr->CalculateWorldPoints<false>(GetDepthData.MilDepthImage);
else
pDepthDataMgr->CalculateWorldPoints<true>(GetDepthData.MilDepthImage);
pDepthDataMgr->GenColorLuts(pGetDepthColorData->MilColorCameraCalibration, pGetDepthColorData->IsDepthMapLut);
if(pDepthDataMgr->GetNbWorld())
{
if(pGetDepthColorData->IsDepthMapLut)
MbufChildMove(pGetDepthColorData->MilColorDepthOrWorldImage, 0, 0, pDepthDataMgr->GetNbWorld(), 1, M_DEFAULT);
pDepthDataMgr->WarpColorImage(GetDepthData.MilColorImage, pGetDepthColorData->MilColorDepthOrWorldImage,
pGetDepthColorData->IsDepthMapLut);
}
}
return M_NULL;
}
MIL_INT MFTYPE Get3DMeshColorProcFunc(MIL_INT HookType, MIL_ID MilKinectImage, void* UserDataPtr)
{
SGet3DMeshColorProcFuncData* pGet3DMeshColorData = static_cast<SGet3DMeshColorProcFuncData*>(UserDataPtr);
SGetDepthColorProcFuncData& GetDepthColorData = pGet3DMeshColorData->GetDepthColorData;
CBestPlaneFitter* pBestPlaneFitter = pGet3DMeshColorData->pBestPlaneFitter;
CDepthDataMgr* pDepthDataMgr = GetDepthColorData.pDepthDataMgr;
GetDepthColorProcFunc(HookType, MilKinectImage, &GetDepthColorData);
if(MbufInquire(MilKinectImage, M_SIZE_BAND, M_NULL) == 1)
{
MdispControl(pGet3DMeshColorData->MilDepthMapDisplay, M_UPDATE, M_DISABLE);
MdispControl(pGet3DMeshColorData->MilColorDepthMapDisplay, M_UPDATE, M_DISABLE);
MdispControl(pGet3DMeshColorData->MilColorDisplay, M_UPDATE, M_DISABLE);
pDepthDataMgr->MoveWorldPoints(GetDepthColorData.MilColorCameraCalibration,
M_TOOL_COORDINATE_SYSTEM, M_CAMERA_COORDINATE_SYSTEM);
pDepthDataMgr->CreateDepthMap(GetDepthColorData.MilColorDepthOrWorldImage,
pGet3DMeshColorData->MilDepthMapImage,
pGet3DMeshColorData->MilColorDepthMapImage,
KINECT_DEPTH_PARAM);
SBox FoundBox = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
if(pBestPlaneFitter->CalculateBestPlane(pGet3DMeshColorData->MilDepthMapImage))
{
Update3dDisplay(pGet3DMeshColorData);
pBestPlaneFitter->MoveCoordinateSystemOnPlane(GetDepthColorData.MilColorCameraCalibration,
M_RELATIVE_COORDINATE_SYSTEM, M_CAMERA_COORDINATE_SYSTEM);
pBestPlaneFitter->MoveWorldChains(GetDepthColorData.MilColorCameraCalibration,
M_CAMERA_COORDINATE_SYSTEM, M_RELATIVE_COORDINATE_SYSTEM);
MIL_ID MilOverlay = MdispInquire(pGet3DMeshColorData->MilColorDisplay, M_OVERLAY_ID, M_NULL);
MdispControl(pGet3DMeshColorData->MilColorDisplay, M_OVERLAY_CLEAR, M_DEFAULT);
MgraClear(M_DEFAULT, pGet3DMeshColorData->MilColorGraList);
MgraColor(M_DEFAULT, M_COLOR_RED);
MgraControl(M_DEFAULT, M_INPUT_UNITS, M_WORLD);
McalAssociate(GetDepthColorData.MilColorCameraCalibration, MilOverlay, M_DEFAULT);
MIL_INT TransparentColor = MdispInquire(pGet3DMeshColorData->MilColorDisplay, M_TRANSPARENT_COLOR, M_NULL);
pBestPlaneFitter->DrawPlaneInImage(M_DEFAULT,
MilOverlay,
TransparentColor);
pDepthDataMgr->MoveWorldPoints(GetDepthColorData.MilColorCameraCalibration,
M_CAMERA_COORDINATE_SYSTEM, M_RELATIVE_COORDINATE_SYSTEM);
pDepthDataMgr->CreateDepthMap(GetDepthColorData.MilColorDepthOrWorldImage,
pGet3DMeshColorData->MilDepthMapImage,
M_NULL,
TOP_DEPTH_PARAM);
pGet3DMeshColorData->BoxFound = FindObjectOnFloor(pGet3DMeshColorData->MilDepthMapImage,
pGet3DMeshColorData->MilBinDepthMapImage,
pGet3DMeshColorData->MilBlobResult,
pGet3DMeshColorData->MilBlobFeatureList,
-TOP_VIEW_WORLD_POS_Z,
OBJECT_THRESHOLD_Z,
-TOP_VIEW_WORLD_POS_Z,
&FoundBox);
if(pGet3DMeshColorData->BoxFound)
{
FindObjectOnFloor(pGet3DMeshColorData->MilDepthMapImage,
pGet3DMeshColorData->MilBinDepthMapImage,
pGet3DMeshColorData->MilBlobResult,
pGet3DMeshColorData->MilBlobFeatureList,
-TOP_VIEW_WORLD_POS_Z,
FoundBox.Height - TOP_REFINE_RANGE,
FoundBox.Height + TOP_REFINE_RANGE,
&FoundBox);
MgraColor(M_DEFAULT, M_COLOR_CYAN);
DrawFound3dBox(M_DEFAULT, pGet3DMeshColorData->MilColorGraList,
GetDepthColorData.MilColorCameraCalibration, FoundBox);
}
else
MgraColor(M_DEFAULT, M_COLOR_RED);
}
MgraControl(M_DEFAULT, M_INPUT_UNITS, M_DISPLAY);
MIL_DOUBLE BoxVolume = FoundBox.Width * FoundBox.Length * FoundBox.Height / 1000.0;
MIL_TEXT_CHAR DisplayText[256];
MosSprintf(DisplayText, 256, MIL_TEXT(" Box width: %8.1f cm "), FoundBox.Width / 10.0);
MgraText(M_DEFAULT, pGet3DMeshColorData->MilColorGraList, 0, 0, DisplayText);
MosSprintf(DisplayText, 256, MIL_TEXT(" Box length: %8.1f cm "), FoundBox.Length / 10.0);
MgraText(M_DEFAULT, pGet3DMeshColorData->MilColorGraList, 0, DISPLAY_TEXT_LINE_OFFSET, DisplayText);
MosSprintf(DisplayText, 256, MIL_TEXT(" Box height: %8.1f cm "), FoundBox.Height / 10.0);
MgraText(M_DEFAULT, pGet3DMeshColorData->MilColorGraList, 0, 2 * DISPLAY_TEXT_LINE_OFFSET, DisplayText);
MosSprintf(DisplayText, 256, MIL_TEXT(" Box volume: %8.1f cm^3 "), BoxVolume);
MgraText(M_DEFAULT, pGet3DMeshColorData->MilColorGraList, 0, 3 * DISPLAY_TEXT_LINE_OFFSET, DisplayText);
MdispControl(pGet3DMeshColorData->MilColorDisplay, M_UPDATE, M_ENABLE);
MdispControl(pGet3DMeshColorData->MilColorDepthMapDisplay, M_UPDATE, M_ENABLE);
MdispControl(pGet3DMeshColorData->MilDepthMapDisplay, M_UPDATE, M_ENABLE);
}
return M_NULL;
}
void CalibrateCamera(CKinectCameraInterface* pKinectCamera,
MIL_ID MilCameraCalibration,
MIL_ID MilGrabImage,
MIL_ID MilDisplay,
ColorStreamTypeEnum ColorStreamType)
{
MIL_CONST_TEXT_PTR CameraFlag = (ColorStreamType == enColor) ? MIL_TEXT("color") : MIL_TEXT("infrared");
if(pKinectCamera->InitCamera(ColorStreamType, false) != KINECT_CAMERA_OK)
FatalError(pKinectCamera, MIL_TEXT("Unable to initialize camera system.\n"));
MIL_ID MilDrawDest = MdispInquire(MilDisplay, M_ASSOCIATED_GRAPHIC_LIST_ID, M_NULL);
MgraClear(M_DEFAULT, MilDrawDest);
#if USE_REAL_CAMERA && CALIBRATE_WITH_REAL_CAMERA
pKinectCamera->FrameGrabberProcess(M_START, M_ASYNCHRONOUS, M_DEFAULT, &CopyFunc, &MilGrabImage);
MosPrintf(MIL_TEXT("The grabbed %s image from the Kinect is displayed.\n\n"), CameraFlag);
if(ColorStreamType == enIR)
MosPrintf(MIL_TEXT("Place a tilted chessboard calibration grid to calibrate the %s camera.\n"), CameraFlag);
else
MosPrintf(MIL_TEXT("Use the same tilted chessboard calibration grid to calibrate the %s camera.\n"), CameraFlag);
MosPrintf(MIL_TEXT("Press <Enter> to grab and calibrate the %s camera.\n\n"), CameraFlag);
MosGetch();
pKinectCamera->FrameGrabberProcess(M_STOP, M_DEFAULT);
#else
MbufLoad(ColorStreamType == enColor? COLOR_GRID : IR_GRID, MilGrabImage);
#endif
McalGrid(MilCameraCalibration,
MilGrabImage,
GRID_WORLD_OFFSET_X,
GRID_WORLD_OFFSET_Y,
0.0,
GRID_NB_ROWS,
GRID_NB_COLUMNS,
GRID_SPACING,
GRID_SPACING,
M_DEFAULT,
M_CIRCLE_GRID);
if(McalInquire(MilCameraCalibration, M_CALIBRATION_STATUS, M_NULL) != M_CALIBRATED)
FatalError(pKinectCamera, MIL_TEXT("Unable to calibrate the camera."));
MgraColor(M_DEFAULT, M_COLOR_GREEN);
McalDraw(M_DEFAULT, MilCameraCalibration, MilDrawDest, M_DRAW_WORLD_POINTS, M_DEFAULT, M_DEFAULT);
MosPrintf(MIL_TEXT("The calibration of the %s camera is displayed.\n")
MIL_TEXT("Press <Enter> to continue.\n\n"),
CameraFlag);
MosGetch();
MgraClear(M_DEFAULT, MilDrawDest);
}
void MoveColorToolCoordSystemOnIRCameraCoordSystem(MIL_ID MilSystem,
MIL_ID MilIRCameraCalibration,
MIL_ID MilColorCameraCalibration)
{
MIL_ID MilAbsoluteToIRCameraMatrix = MbufAlloc2d(MilSystem, 4, 4, 32+M_FLOAT, M_ARRAY, M_NULL);
McalGetCoordinateSystem(MilIRCameraCalibration,
M_CAMERA_COORDINATE_SYSTEM,
M_ABSOLUTE_COORDINATE_SYSTEM,
M_HOMOGENEOUS_MATRIX,
MilAbsoluteToIRCameraMatrix,
M_NULL,
M_NULL,
M_NULL,
M_NULL);
McalSetCoordinateSystem(MilColorCameraCalibration,
M_TOOL_COORDINATE_SYSTEM,
M_ABSOLUTE_COORDINATE_SYSTEM,
M_HOMOGENEOUS_MATRIX,
MilAbsoluteToIRCameraMatrix,
M_DEFAULT,
M_DEFAULT,
M_DEFAULT,
M_DEFAULT);
MbufFree(MilAbsoluteToIRCameraMatrix);
}
bool FindObjectOnFloor(MIL_ID MilDepthMapImage,
MIL_ID MilBinDepthMapImage,
MIL_ID MilBlobResult,
MIL_ID MilBlobFeatureList,
MIL_DOUBLE FloorZ,
MIL_DOUBLE MinThresholdZ,
MIL_DOUBLE MaxThresholdZ,
SBox* pFoundBox)
{
MimBinarize(MilDepthMapImage, MilBinDepthMapImage, M_FIXED + M_IN_RANGE, FloorZ - MaxThresholdZ, FloorZ - MinThresholdZ);
MblobCalculate(MilBinDepthMapImage, MilDepthMapImage, MilBlobFeatureList, MilBlobResult);
MblobSelect(MilBlobResult, M_INCLUDE_ONLY, M_AREA, M_GREATER_OR_EQUAL, MIN_BLOB_AREA, M_NULL);
MblobSelect(MilBlobResult, M_EXCLUDE, M_FERET_MIN_DIAMETER, M_LESS, MIN_BLOB_FERET_MIN, M_NULL);
MblobSelect(MilBlobResult, M_EXCLUDE, M_SIGMA_PIXEL, M_GREATER, MAX_SIGMA_PIXEL, M_NULL);
MblobSelect(MilBlobResult, M_EXCLUDE, M_RECTANGULARITY, M_LESS, MIN_RECTANGULARITY, M_NULL);
MIL_INT NbBlobs;
if(MblobGetNumber(MilBlobResult, &NbBlobs))
{
MIL_INT* pBlobLabel = new MIL_INT[NbBlobs];
MIL_INT* pBlobArea = new MIL_INT[NbBlobs];
MIL_DOUBLE* pBlobBoxCenterX = new MIL_DOUBLE[NbBlobs];
MIL_DOUBLE* pBlobBoxCenterY = new MIL_DOUBLE[NbBlobs];
MblobGetResult(MilBlobResult, M_LABEL_VALUE + M_TYPE_MIL_INT, pBlobLabel);
MblobGetResult(MilBlobResult, M_AREA + M_TYPE_MIL_INT, pBlobArea);
MblobGetResult(MilBlobResult, M_MIN_AREA_BOX_CENTER_X, pBlobBoxCenterX);
MblobGetResult(MilBlobResult, M_MIN_AREA_BOX_CENTER_Y, pBlobBoxCenterY);
MIL_DOUBLE ManhattanDistance = (abs(pBlobBoxCenterX[0]) + abs(pBlobBoxCenterY[0]));
MIL_DOUBLE MaxScore = pBlobArea[0] / (ManhattanDistance + 100);
MIL_INT MaxScoreIdx = 0;
for(MIL_INT BlobIdx = 1; BlobIdx < NbBlobs; BlobIdx++)
{
ManhattanDistance = (fabs(pBlobBoxCenterX[BlobIdx]) + fabs(pBlobBoxCenterY[BlobIdx]));
MIL_DOUBLE CurScore = pBlobArea[BlobIdx] / (ManhattanDistance + 100);
if(CurScore > MaxScore)
{
MaxScore = CurScore;
MaxScoreIdx = BlobIdx;
}
}
MIL_DOUBLE BoxTopZ;
MblobGetResultSingle(MilBlobResult, pBlobLabel[MaxScoreIdx], M_MEAN_PIXEL, &BoxTopZ);
pFoundBox->Height = FloorZ - BoxTopZ;
pFoundBox->CenterPosZ = -pFoundBox->Height/2;
pFoundBox->CenterPosX = pBlobBoxCenterX[MaxScoreIdx];
pFoundBox->CenterPosY = pBlobBoxCenterY[MaxScoreIdx];
MblobGetResultSingle(MilBlobResult, pBlobLabel[MaxScoreIdx], M_MIN_AREA_BOX_ANGLE, &pFoundBox->ZAxisAngle);
MblobGetResultSingle(MilBlobResult, pBlobLabel[MaxScoreIdx], M_MIN_AREA_BOX_WIDTH, &pFoundBox->Width);
MblobGetResultSingle(MilBlobResult, pBlobLabel[MaxScoreIdx], M_MIN_AREA_BOX_HEIGHT, &pFoundBox->Length);
delete [] pBlobBoxCenterX;
delete [] pBlobBoxCenterY;
delete [] pBlobArea;
delete [] pBlobLabel;
return true;
}
return false;
}
void DrawFound3dBox(MIL_ID MilGraContext, MIL_ID MilDrawDest, MIL_ID MilCalibration, const SBox& rFoundBox)
{
McalSetCoordinateSystem(MilCalibration,
M_RELATIVE_COORDINATE_SYSTEM,
M_RELATIVE_COORDINATE_SYSTEM,
M_TRANSLATION + M_COMPOSE_WITH_CURRENT,
M_NULL,
rFoundBox.CenterPosX,
rFoundBox.CenterPosY,
rFoundBox.CenterPosZ,
M_DEFAULT);
McalSetCoordinateSystem(MilCalibration,
M_RELATIVE_COORDINATE_SYSTEM,
M_RELATIVE_COORDINATE_SYSTEM,
M_ROTATION_Z + M_COMPOSE_WITH_CURRENT,
M_NULL,
-rFoundBox.ZAxisAngle,
M_DEFAULT,
M_DEFAULT,
M_DEFAULT);
MIL_DOUBLE CornersX[8] = {-rFoundBox.Width/2 , rFoundBox.Width/2 , rFoundBox.Width/2 , -rFoundBox.Width/2 ,
-rFoundBox.Width/2 , rFoundBox.Width/2 , rFoundBox.Width/2 , -rFoundBox.Width/2 };
MIL_DOUBLE CornersY[8] = {-rFoundBox.Length/2, -rFoundBox.Length/2, rFoundBox.Length/2 , rFoundBox.Length/2 ,
-rFoundBox.Length/2, -rFoundBox.Length/2, rFoundBox.Length/2 , rFoundBox.Length/2 };
MIL_DOUBLE CornersZ[8] = {-rFoundBox.Height/2, -rFoundBox.Height/2, -rFoundBox.Height/2, -rFoundBox.Height/2,
rFoundBox.Height/2 , rFoundBox.Height/2 , rFoundBox.Height/2 , rFoundBox.Height/2 };
MIL_DOUBLE TopCameraCornersX[4];
MIL_DOUBLE TopCameraCornersY[4];
MIL_DOUBLE TopCameraCornersZ[4];
McalTransformCoordinate3dList(MilCalibration,
M_RELATIVE_COORDINATE_SYSTEM,
M_CAMERA_COORDINATE_SYSTEM,
4,
CornersX,
CornersY,
CornersZ,
TopCameraCornersX,
TopCameraCornersY,
TopCameraCornersZ,
M_DEFAULT);
MIL_INT ClosestCornerIdx = 0;
MIL_DOUBLE MinCornerDistSquare = TopCameraCornersX[0] * TopCameraCornersX[0] +
TopCameraCornersY[0] * TopCameraCornersY[0] +
TopCameraCornersZ[0] * TopCameraCornersZ[0];
for(MIL_INT CornerIdx = 1; CornerIdx < 4; CornerIdx++)
{
MIL_DOUBLE CurCornerDistSquare = TopCameraCornersX[CornerIdx] * TopCameraCornersX[CornerIdx] +
TopCameraCornersY[CornerIdx] * TopCameraCornersY[CornerIdx] +
TopCameraCornersZ[CornerIdx] * TopCameraCornersZ[CornerIdx];
if(CurCornerDistSquare < MinCornerDistSquare)
{
MinCornerDistSquare = CurCornerDistSquare;
ClosestCornerIdx = CornerIdx;
}
}
McalTransformCoordinate3dList(MilCalibration,
M_RELATIVE_COORDINATE_SYSTEM,
M_PIXEL_COORDINATE_SYSTEM,
8,
CornersX,
CornersY,
CornersZ,
CornersX,
CornersY,
M_NULL,
M_DEFAULT);
MgraControl(MilGraContext, M_INPUT_UNITS, M_PIXEL);
MgraLines(MilGraContext, MilDrawDest, 4, CornersX, CornersY, M_NULL, M_NULL, M_POLYGON);
MIL_DOUBLE PrevDrawCornersX[4] = {CornersX[ClosestCornerIdx] , CornersX[(ClosestCornerIdx + 3) % 4],
CornersX[((ClosestCornerIdx + 3) % 4) + 4], CornersX[ClosestCornerIdx + 4] };
MIL_DOUBLE PrevDrawCornersY[4] = {CornersY[ClosestCornerIdx] , CornersY[(ClosestCornerIdx + 3) % 4],
CornersY[((ClosestCornerIdx + 3) % 4) + 4], CornersY[ClosestCornerIdx + 4] };
MgraLines(MilGraContext, MilDrawDest, 4, PrevDrawCornersX, PrevDrawCornersY, M_NULL, M_NULL, M_POLYGON);
MIL_DOUBLE NextDrawCornersX[4] = {CornersX[ClosestCornerIdx] , CornersX[(ClosestCornerIdx + 1) % 4],
CornersX[((ClosestCornerIdx + 1) % 4) + 4], CornersX[ClosestCornerIdx + 4] };
MIL_DOUBLE NextDrawCornersY[4] = {CornersY[ClosestCornerIdx] , CornersY[(ClosestCornerIdx + 1) % 4],
CornersY[((ClosestCornerIdx + 1) % 4) + 4], CornersY[ClosestCornerIdx + 4] };
MgraLines(MilGraContext, MilDrawDest, 4, NextDrawCornersX, NextDrawCornersY, M_NULL, M_NULL, M_POLYGON);
}
void Update3dDisplay(const SGet3DMeshColorProcFuncData* pGet3DMeshColorData)
{
MbufCopy(pGet3DMeshColorData->MilDepthMapImage, pGet3DMeshColorData->Mil3dDepthMapImage);
MbufCopy(pGet3DMeshColorData->MilColorDepthMapImage, pGet3DMeshColorData->Mil3dColorDepthMapImage);
if(pGet3DMeshColorData->MilD3DSetImagesThread)
MthrControl(pGet3DMeshColorData->MilD3DUpdateEvent, M_EVENT_SET, M_SIGNALED);
else
MdepthD3DSetImages(pGet3DMeshColorData->DispD3DHandle, pGet3DMeshColorData->Mil3dDepthMapImage,
pGet3DMeshColorData->Mil3dColorDepthMapImage);
}
MIL_UINT32 MFTYPE Update3dDisplayThread(void *UserDataPtr)
{
SGet3DMeshColorProcFuncData* pGet3DMeshColorData = (SGet3DMeshColorProcFuncData*) UserDataPtr;
while(MthrInquire(pGet3DMeshColorData->MilStopUpdate3dDisplayEvent, M_EVENT_STATE, M_NULL) == M_NOT_SIGNALED)
{
MthrWait(pGet3DMeshColorData->MilD3DUpdateEvent, M_EVENT_WAIT, M_NULL);
if(MthrInquire(pGet3DMeshColorData->MilStopUpdate3dDisplayEvent, M_EVENT_STATE, M_NULL) == M_NOT_SIGNALED)
MdepthD3DSetImages(pGet3DMeshColorData->DispD3DHandle, pGet3DMeshColorData->Mil3dDepthMapImage,
pGet3DMeshColorData->Mil3dColorDepthMapImage);
}
return 0;
}
void SetupDisplay(MIL_ID MilDisplay, MIL_CONST_TEXT_PTR WindowTitle, MIL_INT WindowPosX, MIL_INT WindowPosY)
{
MdispControl(MilDisplay, M_TITLE, M_PTR_TO_DOUBLE(WindowTitle));
MdispControl(MilDisplay, M_WINDOW_INITIAL_POSITION_X, (MIL_DOUBLE)WindowPosX);
MdispControl(MilDisplay, M_WINDOW_INITIAL_POSITION_Y, (MIL_DOUBLE)WindowPosY);
}
void FillDisplayLut(MIL_ID MilDisplayLut)
{
MbufClear(MilDisplayLut, M_COLOR_BLACK);
MIL_ID MilDisplayLutChild = MbufChild1d(MilDisplayLut, 1, 3000, M_NULL);
MgenLutFunction(MilDisplayLutChild, M_COLORMAP_JET, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT, M_DEFAULT);
MbufFree(MilDisplayLutChild);
}
void FatalError(CKinectCameraInterface* pKinectCamera, const MIL_TEXT_CHAR* pMsg)
{
pKinectCamera->CloseDown();
delete pKinectCamera;
MosPrintf(pMsg);
MosPrintf(MIL_TEXT("\nPress <Enter> to end.\n"));
MosGetch();
exit(-1);
}