#include <mil.h>
#include <algorithm>
#include <cassert>
#include <cmath>
#include <iterator>
#include "vec3.h"
void PrintHeader()
{
MosPrintf(MIL_TEXT("[EXAMPLE NAME]\n"));
MosPrintf(MIL_TEXT("StereoRectification\n\n"));
MosPrintf(MIL_TEXT("[SYNOPSIS]\n"));
MosPrintf(MIL_TEXT("This example shows how to perform stereo rectification and 3D\n"));
MosPrintf(MIL_TEXT("reconstruction using MIL.\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();
}
enum ECamIdx
{
LeftCam = 0,
RightCam,
NUM_CAMS
};
struct StereoParams
{
MIL_DOUBLE Num;
MIL_DOUBLE Denom;
};
struct PrincipalAxis
{
Vec3 Origin, Direction;
};
struct Box
{
MIL_DOUBLE x1, y1, x2, y2;
};
const MIL_STRING GridFiles[NUM_CAMS] =
{
M_IMAGE_PATH MIL_TEXT("StereoRectification\\left_grid.png"),
M_IMAGE_PATH MIL_TEXT("StereoRectification\\right_grid.png"),
};
const MIL_STRING ObjectFiles[NUM_CAMS] =
{
M_IMAGE_PATH MIL_TEXT("StereoRectification\\left_object.png"),
M_IMAGE_PATH MIL_TEXT("StereoRectification\\right_object.png"),
};
constexpr MIL_INT NUM_POINTS = 4;
const MIL_DOUBLE NoRectPixelsX[NUM_CAMS][NUM_POINTS] =
{
{
268.0, 665.0,
884.0, 1024.0
},
{
202.0, 526.0,
690.0, 819.0
},
};
const MIL_DOUBLE NoRectPixelsY[NUM_CAMS][NUM_POINTS] =
{
{
480.0, 284.0,
400.0, 596.0
},
{
570.0, 362.0,
447.0, 612.0
},
};
const MIL_DOUBLE PixelsX[NUM_CAMS][NUM_POINTS] =
{
{
275.0, 658.0,
824.0, 959.0
},
{
338.0, 741.0,
865.0, 980.0
},
};
const MIL_DOUBLE PixelsY[NUM_CAMS][NUM_POINTS] =
{
{
481.0, 293.0,
435.0, 626.0
},
{
481.0, 293.0,
435.0, 626.0
},
};
StereoParams StereoRectifyPreprocess(MIL_ID LeftCalId, MIL_ID RightCalId, MIL_INT ImgSizeX,
MIL_INT ImgSizeY, MIL_ID* LeftRectifiedImgIdPtr, MIL_ID* RightRectifiedImgIdPtr);
MIL_DOUBLE SetRectifiedPlane(MIL_ID LeftCalId, MIL_ID RightCalId);
PrincipalAxis GetPrincipalAxis(MIL_ID CalId);
Vec3 ComputeRelativeZAxis(const Vec3& RelXAxis, const Vec3& LeftZAxis, const Vec3& RightZAxis);
MIL_ID ConstructHMatrix(const Vec3& RelXAxis, const Vec3& RelYAxis, const Vec3& RelZAxis,
const Vec3& RelOrig);
Box ComputeBBox(MIL_ID CalId, MIL_INT ImgSizeX, MIL_INT ImgSizeY);
void AdjustBoxes(Box* pLeftBBox, Box* pRightBBox, MIL_DOUBLE );
MIL_DOUBLE AllocateAndCalibrateRectifiedImage(MIL_ID SysId, const Box& BBox, MIL_DOUBLE PixelSize,
MIL_ID* RectifiedImageIdPtr);
MIL_DOUBLE ComputePixelSize(MIL_ID LeftCalId, MIL_ID RightCalId);
void DrawPoints(MIL_ID MilGraphics, MIL_ID MilGraList, MIL_INT XOffset,
const MIL_DOUBLE PixelsX[NUM_CAMS][NUM_POINTS],
const MIL_DOUBLE PixelsY[NUM_CAMS][NUM_POINTS]);
void DrawLines(MIL_ID MilGraphics, MIL_ID MilGraList, MIL_INT XOffset,
const MIL_DOUBLE PixelsX[NUM_CAMS][NUM_POINTS],
const MIL_DOUBLE PixelsY[NUM_CAMS][NUM_POINTS]);
int MosMain(void)
{
MIL_ID MilApplication,
MilSystem,
MilDisplay,
MilOverlay,
MilGraphics,
MilGraList,
MilCalibration[NUM_CAMS],
GridImgs[NUM_CAMS],
ObjectImgs[NUM_CAMS],
RectifiedImgs[NUM_CAMS],
DisplayParentImg,
DisplayChildImgs[NUM_CAMS];
MIL_INT CamImgSizeX,
CamImgSizeY;
PrintHeader();
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);
MgraAlloc(MilSystem, &MilGraphics);
MgraAllocList(MilSystem, M_DEFAULT, &MilGraList);
MdispControl(MilDisplay, M_ASSOCIATED_GRAPHIC_LIST_ID, MilGraList);
MdispControl(MilDisplay, M_SCALE_DISPLAY, M_ENABLE);
MdispZoom(MilDisplay, 0.5, 0.5);
MosPrintf(MIL_TEXT("Calibrating cameras...\n"));
for(MIL_INT CamIdx = 0; CamIdx < NUM_CAMS; ++CamIdx)
{
MilCalibration[CamIdx] = McalAlloc(MilSystem, M_TSAI_BASED, M_DEFAULT, M_NULL);
McalControl(MilCalibration[CamIdx], M_GRID_PARTIAL, M_ENABLE);
McalControl(MilCalibration[CamIdx], M_GRID_FIDUCIAL, M_DATAMATRIX);
MbufRestore(GridFiles[CamIdx], MilSystem, &GridImgs[CamIdx]);
McalGrid(MilCalibration[CamIdx], GridImgs[CamIdx], 0.0, 0.0, 0.0,
M_UNKNOWN, M_UNKNOWN, M_FROM_FIDUCIAL, M_FROM_FIDUCIAL,
M_DEFAULT, M_CHESSBOARD_GRID);
if(McalInquire(MilCalibration[CamIdx], M_CALIBRATION_STATUS, M_NULL) != M_CALIBRATED)
{
MosPrintf(MIL_TEXT("Calibration failed. Stop execution.\n\n"));
MosGetch();
exit(0);
}
}
CamImgSizeX = MbufInquire(GridImgs[0], M_SIZE_X, M_NULL);
CamImgSizeY = MbufInquire(GridImgs[0], M_SIZE_Y, M_NULL);
MbufAlloc2d(MilSystem, 2 * CamImgSizeX, CamImgSizeY, 8 + M_UNSIGNED, M_IMAGE + M_DISP + M_PROC,
&DisplayParentImg);
MbufChild2d(DisplayParentImg, 0, 0, CamImgSizeX, CamImgSizeY, &DisplayChildImgs[0]);
MbufChild2d(DisplayParentImg, CamImgSizeX, 0, CamImgSizeX, CamImgSizeY, &DisplayChildImgs[1]);
MbufCopy(GridImgs[0], DisplayChildImgs[0]);
MbufCopy(GridImgs[1], DisplayChildImgs[1]);
MdispControl(MilDisplay, M_UPDATE, M_DISABLE);
MdispSelect(MilDisplay, DisplayParentImg);
MgraFont(MilGraphics, M_FONT_DEFAULT_LARGE);
MdispInquire(MilDisplay, M_OVERLAY_ID, &MilOverlay);
MgraText(MilGraphics, MilOverlay, 15, 15, MIL_TEXT("Left Camera"));
MgraText(MilGraphics, MilOverlay, 1295, 15, MIL_TEXT("Right Camera"));
MdispControl(MilDisplay, M_UPDATE, M_ENABLE);
MosPrintf(MIL_TEXT("Calibration done: the images acquired by the two cameras are displayed.\n"));
MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n"));
MosGetch();
const StereoParams Params = StereoRectifyPreprocess(MilCalibration[0], MilCalibration[1],
CamImgSizeX, CamImgSizeY,
&RectifiedImgs[0], &RectifiedImgs[1]);
MgraColor(MilGraphics, M_COLOR_YELLOW);
MdispControl(MilDisplay, M_UPDATE, M_DISABLE);
for(MIL_INT CamIdx = 0; CamIdx < NUM_CAMS; ++CamIdx)
{
ObjectImgs[CamIdx] = MbufRestore(ObjectFiles[CamIdx], MilSystem, M_NULL);
MbufCopy(ObjectImgs[CamIdx], DisplayChildImgs[CamIdx]);
}
DrawPoints(MilGraphics, MilGraList, CamImgSizeX, NoRectPixelsX, NoRectPixelsY);
DrawLines(MilGraphics, MilGraList, CamImgSizeX, NoRectPixelsX, NoRectPixelsY);
MdispControl(MilDisplay, M_UPDATE, M_ENABLE);
MosPrintf(MIL_TEXT("Images before rectification: the epipolar lines from same features are\n"));
MosPrintf(MIL_TEXT("not horizontal.\n"));
MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n"));
MosGetch();
MdispControl(MilDisplay, M_UPDATE, M_DISABLE);
for(MIL_INT CamIdx = 0; CamIdx < NUM_CAMS; ++CamIdx)
{
McalTransformImage(ObjectImgs[CamIdx], RectifiedImgs[CamIdx], MilCalibration[CamIdx],
M_BILINEAR + M_OVERSCAN_CLEAR, M_FULL_CORRECTION,
M_WARP_IMAGE + M_USE_DESTINATION_CALIBRATION);
MbufCopy(RectifiedImgs[CamIdx], DisplayChildImgs[CamIdx]);
}
MgraClear(MilGraphics, MilGraList);
DrawPoints(MilGraphics, MilGraList, CamImgSizeX, PixelsX, PixelsY);
DrawLines(MilGraphics, MilGraList, CamImgSizeX, PixelsX, PixelsY);
MdispControl(MilDisplay, M_UPDATE, M_ENABLE);
MosPrintf(MIL_TEXT("Images after rectification: the epipolar lines from same features are\n"));
MosPrintf(MIL_TEXT("now horizontal.\n"));
MosPrintf(MIL_TEXT("Press <Enter> to continue.\n\n"));
MosGetch();
MIL_DOUBLE WldPtX[NUM_POINTS];
MIL_DOUBLE WldPtY[NUM_POINTS];
MIL_DOUBLE WldPtZ[NUM_POINTS];
McalTransformCoordinateList(RectifiedImgs[0], M_PIXEL_TO_WORLD, NUM_POINTS, PixelsX[LeftCam],
PixelsY[LeftCam], WldPtX, WldPtY);
for(MIL_INT PtIdx = 0; PtIdx < NUM_POINTS; ++PtIdx)
{
const MIL_DOUBLE Disparity = PixelsX[LeftCam][PtIdx] - PixelsX[RightCam][PtIdx];
const MIL_DOUBLE Divisor = Params.Denom + Disparity;
if(Divisor != 0.0)
{
const MIL_DOUBLE ScaleFactor = Params.Num / Divisor;
WldPtX[PtIdx] *= ScaleFactor;
WldPtY[PtIdx] *= ScaleFactor;
WldPtZ[PtIdx] = ScaleFactor;
}
else
{
MosPrintf(MIL_TEXT("Point at infinity. Stop execution.\n"));
MosGetch();
exit(0);
}
}
const MIL_DOUBLE Length1 = Distance(Vec3 {WldPtX[0], WldPtY[0], WldPtZ[0]},
Vec3 {WldPtX[1], WldPtY[1], WldPtZ[1]});
const MIL_DOUBLE Length2 = Distance(Vec3 {WldPtX[2], WldPtY[2], WldPtZ[2]},
Vec3 {WldPtX[3], WldPtY[3], WldPtZ[3]});
MgraClear(MilGraphics, MilGraList);
DrawPoints(MilGraphics, MilGraList, CamImgSizeX, PixelsX, PixelsY);
MdispControl(MilDisplay, M_UPDATE, M_DISABLE);
MgraColor(MilGraphics, M_COLOR_RED);
MgraLine(MilGraphics, MilGraList, PixelsX[0][0], PixelsY[0][0],
PixelsX[0][1], PixelsY[0][1]);
MgraLine(MilGraphics, MilGraList, PixelsX[1][0] + CamImgSizeX, PixelsY[1][0],
PixelsX[1][1] + CamImgSizeX, PixelsY[1][1]);
MgraColor(MilGraphics, M_COLOR_BLUE);
MgraLine(MilGraphics, MilGraList, PixelsX[0][2], PixelsY[0][2],
PixelsX[0][3], PixelsY[0][3]);
MgraLine(MilGraphics, MilGraList, PixelsX[1][2] + CamImgSizeX, PixelsY[1][2],
PixelsX[1][3] + CamImgSizeX, PixelsY[1][3]);
MdispControl(MilDisplay, M_UPDATE, M_ENABLE);
MosPrintf(MIL_TEXT("The points displayed in yellow are converted to 3D world units.\n"));
MosPrintf(MIL_TEXT("Length of first feature (in red) is %.1f mm.\n"), Length1);
MosPrintf(MIL_TEXT("Length of second feature (in blue) is %.1f mm.\n\n"), Length2);
MosPrintf(MIL_TEXT("Press <Enter> to end.\n"));
MosGetch();
for(MIL_INT CamIdx = 0; CamIdx < NUM_CAMS; ++CamIdx)
{
MbufFree(GridImgs[CamIdx]);
MbufFree(ObjectImgs[CamIdx]);
MbufFree(RectifiedImgs[CamIdx]);
McalFree(MilCalibration[CamIdx]);
MbufFree(DisplayChildImgs[CamIdx]);
}
MbufFree(DisplayParentImg);
MgraFree(MilGraList);
MdispFree(MilDisplay);
MgraFree(MilGraphics);
MsysFree(MilSystem);
MappFree(MilApplication);
}
StereoParams StereoRectifyPreprocess(MIL_ID LeftCalId, MIL_ID RightCalId, MIL_INT ImgSizeX,
MIL_INT ImgSizeY, MIL_ID* LeftRectifiedImgIdPtr,
MIL_ID* RightRectifiedImgIdPtr)
{
const MIL_DOUBLE NormBaseline = SetRectifiedPlane(LeftCalId, RightCalId);
const MIL_DOUBLE PixelSize = ComputePixelSize(LeftCalId, RightCalId);
Box LeftBBox = ComputeBBox(LeftCalId, ImgSizeX, ImgSizeY);
Box RightBBox = ComputeBBox(RightCalId, ImgSizeX, ImgSizeY);
AdjustBoxes(&LeftBBox, &RightBBox, NormBaseline);
MIL_ID SysId;
MobjInquire(LeftCalId, M_OWNER_SYSTEM, &SysId);
MIL_DOUBLE LeftWorldOffsetX = AllocateAndCalibrateRectifiedImage(SysId, LeftBBox, PixelSize,
LeftRectifiedImgIdPtr);
MIL_DOUBLE RightWorldOffsetX = AllocateAndCalibrateRectifiedImage(SysId, RightBBox, PixelSize,
RightRectifiedImgIdPtr);
return {NormBaseline / PixelSize,
(LeftWorldOffsetX - RightWorldOffsetX + NormBaseline) / PixelSize};
}
MIL_DOUBLE SetRectifiedPlane(MIL_ID LeftCalId, MIL_ID RightCalId)
{
const PrincipalAxis LeftPrincipalAxis = GetPrincipalAxis(LeftCalId);
const PrincipalAxis RightPrincipalAxis = GetPrincipalAxis(RightCalId);
const Vec3 Baseline = RightPrincipalAxis.Origin - LeftPrincipalAxis.Origin;
const MIL_DOUBLE NormBaseline = Norm(Baseline);
if(NormBaseline == 0.0)
{
MosPrintf(MIL_TEXT("Computation of rectification plane failed. Stop execution.\n"));
MosGetch();
exit(0);
}
const Vec3 RelXAxis = Baseline / NormBaseline;
const Vec3 RelZAxis = ComputeRelativeZAxis(RelXAxis, LeftPrincipalAxis.Direction,
RightPrincipalAxis.Direction);
const Vec3 RelYAxis = Cross(RelZAxis, RelXAxis);
const Vec3 RelOrig = LeftPrincipalAxis.Origin + RelZAxis;
const MIL_ID Rel2AbsMatId = ConstructHMatrix(RelXAxis, RelYAxis, RelZAxis, RelOrig);
McalSetCoordinateSystem(LeftCalId, M_RELATIVE_COORDINATE_SYSTEM, M_ABSOLUTE_COORDINATE_SYSTEM,
M_HOMOGENEOUS_MATRIX, Rel2AbsMatId, M_DEFAULT, M_DEFAULT, M_DEFAULT,
M_DEFAULT);
McalSetCoordinateSystem(RightCalId, M_RELATIVE_COORDINATE_SYSTEM, M_ABSOLUTE_COORDINATE_SYSTEM,
M_HOMOGENEOUS_MATRIX, Rel2AbsMatId, M_DEFAULT, M_DEFAULT, M_DEFAULT,
M_DEFAULT);
MbufFree(Rel2AbsMatId);
return NormBaseline;
}
PrincipalAxis GetPrincipalAxis(MIL_ID CalId)
{
MIL_DOUBLE ZAxisExtremesX[2] = {0.0, 0.0};
MIL_DOUBLE ZAxisExtremesY[2] = {0.0, 0.0};
MIL_DOUBLE ZAxisExtremesZ[2] = {0.0, 1.0};
McalTransformCoordinate3dList(CalId, M_CAMERA_COORDINATE_SYSTEM, M_ABSOLUTE_COORDINATE_SYSTEM, 2,
ZAxisExtremesX, ZAxisExtremesY, ZAxisExtremesZ,
ZAxisExtremesX, ZAxisExtremesY, ZAxisExtremesZ,
M_DEFAULT);
PrincipalAxis Axis;
Axis.Origin = {ZAxisExtremesX[0], ZAxisExtremesY[0], ZAxisExtremesZ[0]};
Axis.Direction = Vec3 {ZAxisExtremesX[1], ZAxisExtremesY[1], ZAxisExtremesZ[1]} - Axis.Origin;
return Axis;
}
Vec3 ComputeRelativeZAxis(const Vec3& RelXAxis, const Vec3& LeftZAxis, const Vec3& RightZAxis)
{
const Vec3 OrthoLeftZAxis = LeftZAxis - ProjectUnit(LeftZAxis, RelXAxis);
const Vec3 OrthoRightZAxis = RightZAxis - ProjectUnit(RightZAxis, RelXAxis);
const Vec3 AvgZDir = 0.5 * (OrthoLeftZAxis + OrthoRightZAxis);
const MIL_DOUBLE NormAvgZDir = Norm(AvgZDir);
if(NormAvgZDir == 0.0)
{
MosPrintf(MIL_TEXT("Computation of relative z axis failed. Stop execution.\n"));
MosGetch();
exit(0);
}
return AvgZDir / NormAvgZDir;
}
MIL_ID ConstructHMatrix(const Vec3& RelXAxis, const Vec3& RelYAxis, const Vec3& RelZAxis,
const Vec3& RelOrig)
{
const MIL_FLOAT HMatData[] =
{
static_cast<MIL_FLOAT>(RelXAxis.x), static_cast<MIL_FLOAT>(RelYAxis.x),
static_cast<MIL_FLOAT>(RelZAxis.x), static_cast<MIL_FLOAT>(RelOrig.x),
static_cast<MIL_FLOAT>(RelXAxis.y), static_cast<MIL_FLOAT>(RelYAxis.y),
static_cast<MIL_FLOAT>(RelZAxis.y), static_cast<MIL_FLOAT>(RelOrig.y),
static_cast<MIL_FLOAT>(RelXAxis.z), static_cast<MIL_FLOAT>(RelYAxis.z),
static_cast<MIL_FLOAT>(RelZAxis.z), static_cast<MIL_FLOAT>(RelOrig.z),
0.0f, 0.0f, 0.0f, 1.0f
};
MIL_ID HMatId = MbufAlloc2d(M_DEFAULT_HOST, 4, 4, 32 + M_FLOAT, M_ARRAY, M_NULL);
MbufPut(HMatId, HMatData);
return HMatId;
}
MIL_DOUBLE ComputePixelSize(MIL_ID LeftCalId, MIL_ID RightCalId)
{
MIL_DOUBLE LeftFocal, RightFocal;
McalInquire(LeftCalId, M_FOCAL_LENGTH, &LeftFocal);
McalInquire(RightCalId, M_FOCAL_LENGTH, &RightFocal);
MIL_DOUBLE AvgFocal = 0.5 * (LeftFocal + RightFocal);
if(AvgFocal == 0.0)
{
MosPrintf(MIL_TEXT("Computation of pixel size failed. Stop execution.\n"));
MosGetch();
exit(0);
}
return 1.0 / AvgFocal;
}
Box ComputeBBox(MIL_ID CalId, MIL_INT ImgSizeX, MIL_INT ImgSizeY)
{
constexpr MIL_INT NUM_POINTS = 4;
MIL_DOUBLE X[NUM_POINTS] = {-0.5, ImgSizeX - 0.5, ImgSizeX - 0.5, -0.5};
MIL_DOUBLE Y[NUM_POINTS] = {-0.5, -0.5, ImgSizeY - 0.5, ImgSizeY - 0.5};
McalTransformCoordinateList(CalId, M_PIXEL_TO_WORLD + M_NO_POINTS_BEHIND_CAMERA,
NUM_POINTS, X, Y, X, Y);
for(MIL_INT PtIdx = 0; PtIdx < NUM_POINTS; ++PtIdx)
{
if(X[PtIdx] == M_INVALID_POINT || Y[PtIdx] == M_INVALID_POINT)
{
MosPrintf(MIL_TEXT("Conversion of the four corners to world units failed:\n"));
MosPrintf(MIL_TEXT("No intersection found.\n"));
MosPrintf(MIL_TEXT("Stop execution.\n"));
MosGetch();
exit(0);
}
}
const auto ExtremeX = std::minmax_element(std::begin(X), std::end(X));
const auto ExtremeY = std::minmax_element(std::begin(Y), std::end(Y));
return {*ExtremeX.first, *ExtremeY.first, *ExtremeX.second, *ExtremeY.second};
}
void AdjustBoxes(Box* pLeftBBox, Box* pRightBBox, MIL_DOUBLE )
{
const MIL_DOUBLE MinY = std::max(pLeftBBox->y1, pRightBBox->y1);
const MIL_DOUBLE MaxY = std::min(pLeftBBox->y2, pRightBBox->y2);
if(MinY > MaxY)
{
MosPrintf(MIL_TEXT("Bounding box approximation failed. Stop execution.\n"));
MosGetch();
exit(0);
}
pLeftBBox->y1 = pRightBBox->y1 = MinY;
pLeftBBox->y2 = pRightBBox->y2 = MaxY;
}
MIL_DOUBLE AllocateAndCalibrateRectifiedImage(MIL_ID SysId, const Box& BBox, MIL_DOUBLE PixelSize,
MIL_ID* RectifiedImageIdPtr)
{
const MIL_DOUBLE BoxSizeX = BBox.x2 - BBox.x1;
const MIL_DOUBLE BoxSizeY = BBox.y2 - BBox.y1;
const MIL_INT RectifiedSizeX = static_cast<MIL_INT>(std::ceil(BoxSizeX / PixelSize));
const MIL_INT RectifiedSizeY = static_cast<MIL_INT>(std::ceil(BoxSizeY / PixelSize));
const MIL_DOUBLE WorldOffsetX = BBox.x1 + 0.5*PixelSize;
const MIL_DOUBLE WorldOffsetY = BBox.y1 + 0.5*PixelSize;
MbufAlloc2d(SysId, RectifiedSizeX, RectifiedSizeY, 8 + M_UNSIGNED, M_IMAGE + M_PROC + M_DISP,
RectifiedImageIdPtr);
McalUniform(*RectifiedImageIdPtr, WorldOffsetX, WorldOffsetY, PixelSize, PixelSize, 0.0,
M_DEFAULT);
return WorldOffsetX;
}
void DrawPoints(MIL_ID MilGraphics, MIL_ID MilGraList, MIL_INT XOffset,
const MIL_DOUBLE PixelsX[NUM_CAMS][NUM_POINTS],
const MIL_DOUBLE PixelsY[NUM_CAMS][NUM_POINTS])
{
for(MIL_INT CamIdx = 0; CamIdx < NUM_CAMS; ++CamIdx)
{
for(MIL_INT PtIdx = 0; PtIdx < NUM_POINTS; ++PtIdx)
{
constexpr MIL_DOUBLE Dim = 6.0;
const MIL_DOUBLE Px = PixelsX[CamIdx][PtIdx];
const MIL_DOUBLE Py = PixelsY[CamIdx][PtIdx];
MgraRect(MilGraphics, MilGraList, Px - Dim + CamIdx * XOffset, Py - Dim,
Px + Dim + CamIdx * XOffset, Py + Dim);
}
}
}
void DrawLines(MIL_ID MilGraphics, MIL_ID MilGraList, MIL_INT XOffset,
const MIL_DOUBLE PixelsX[NUM_CAMS][NUM_POINTS],
const MIL_DOUBLE PixelsY[NUM_CAMS][NUM_POINTS])
{
for(MIL_INT PtIdx = 0; PtIdx < NUM_POINTS; ++PtIdx)
{
MgraLine(MilGraphics, MilGraList, PixelsX[0][PtIdx], PixelsY[0][PtIdx],
PixelsX[1][PtIdx] + XOffset, PixelsY[1][PtIdx]);
}
}