#include "mil.h"
#include "mil3dcam_sickrangerc.h"
template <bool IsYAxisFlipped>
MIL_INT DoHi3DToMIL(MIL_BUFFER_INFO RangerBuf,
MIL_BUFFER_INFO PositionBuf,
MIL_BUFFER_INFO IntensityBuf,
const RangerParams& rConvParams);
MIL_INT DoHorThrMaxToMIL(MIL_BUFFER_INFO RangerBuf,
MIL_BUFFER_INFO PositionBuf,
MIL_BUFFER_INFO IntensityBuf,
const RangerParams& rConvParams);
bool GetBufInfoAndValidate(MIL_ID MilBuf, MIL_BUFFER_INFO* ppBuf);
MIL_INT MFTYPE M3dcamRangerToMIL(MIL_ID MilRangerImage,
MIL_ID MilPositionImage,
MIL_ID MilIntensityImage,
MIL_ID MilEncoderImage,
const RangerParams* ConvParamsPtr)
{
MIL_BUFFER_INFO RangerBuf, PositionBuf, IntensityBuf;
if ( !GetBufInfoAndValidate(MilRangerImage , &RangerBuf ) )
return CONVERSION_FAILURE;
if ( !GetBufInfoAndValidate(MilPositionImage, &PositionBuf) )
return CONVERSION_FAILURE;
if (ConvParamsPtr->IntensitySizeBit == NO_INTENSITY_INFORMATION)
{
if (MilIntensityImage != M_NULL)
return CONVERSION_FAILURE;
IntensityBuf = NULL;
}
else
{
if ( !GetBufInfoAndValidate(MilIntensityImage, &IntensityBuf) )
return CONVERSION_FAILURE;
}
if (MfuncBufSizeX(RangerBuf) != ConvParamsPtr->ProfileDataSizeByte)
return CONVERSION_FAILURE;
if (MfuncBufSizeX(PositionBuf) != ConvParamsPtr->MeasROISizeX)
return CONVERSION_FAILURE;
if (MfuncBufSizeY(RangerBuf) != MfuncBufSizeY(PositionBuf))
return CONVERSION_FAILURE;
if (MfuncBufSizeBit(RangerBuf) != 8)
return CONVERSION_FAILURE;
if (MfuncBufSizeBit(PositionBuf) != ConvParamsPtr->RangeSizeBit)
return CONVERSION_FAILURE;
if (IntensityBuf != NULL)
{
if (MfuncBufSizeX(IntensityBuf) != ConvParamsPtr->MeasROISizeX)
return CONVERSION_FAILURE;
if (MfuncBufSizeY(RangerBuf) != MfuncBufSizeY(IntensityBuf))
return CONVERSION_FAILURE;
if (MfuncBufSizeBit(IntensityBuf) != ConvParamsPtr->IntensitySizeBit)
return CONVERSION_FAILURE;
}
if (ConvParamsPtr->RangeStreamOffset < 0)
return CONVERSION_FAILURE;
MIL_INT RangeWidth = ConvParamsPtr->MeasROISizeX * (ConvParamsPtr->RangeSizeBit >> 3);
if (ConvParamsPtr->RangeStreamOffset+RangeWidth > ConvParamsPtr->ProfileDataSizeByte)
return CONVERSION_FAILURE;
if (ConvParamsPtr->IntensityStreamOffset != NO_INTENSITY_INFORMATION)
{
if (ConvParamsPtr->IntensityStreamOffset < 0)
return CONVERSION_FAILURE;
MIL_INT IntW = ConvParamsPtr->MeasROISizeX * (ConvParamsPtr->IntensitySizeBit >> 3);
if (ConvParamsPtr->IntensityStreamOffset+IntW > ConvParamsPtr->ProfileDataSizeByte)
return CONVERSION_FAILURE;
}
if ( ConvParamsPtr->RangeSizeBit != 8 &&
ConvParamsPtr->RangeSizeBit != 16 )
return CONVERSION_FAILURE;
if ( ConvParamsPtr->ImageROISizeX < 256 ||
ConvParamsPtr->ImageROISizeX > 1536 ||
ConvParamsPtr->ImageROISizeX % 8 != 0 )
return CONVERSION_FAILURE;
if ( ConvParamsPtr->ImageROISizeY < 2 ||
ConvParamsPtr->ImageROISizeY > 512 )
return CONVERSION_FAILURE;
if ( ConvParamsPtr->MeasROISizeX < 256 ||
ConvParamsPtr->MeasROISizeX > 1536 ||
ConvParamsPtr->MeasROISizeX % 8 != 0 )
return CONVERSION_FAILURE;
if ( ConvParamsPtr->MeasROISizeY < 16 ||
ConvParamsPtr->MeasROISizeY > 512 )
return CONVERSION_FAILURE;
if (ConvParamsPtr->ProfileDataSizeByte <= 0)
return CONVERSION_FAILURE;
if ( ConvParamsPtr->FlipY != DO_FLIP_Y_AXIS &&
ConvParamsPtr->FlipY != DO_NOT_FLIP_Y_AXIS )
return CONVERSION_FAILURE;
if (ConvParamsPtr->CycleTimeMicroseconds <= 0)
return CONVERSION_FAILURE;
switch(ConvParamsPtr->MeasurementMode)
{
case RANGER_HOR_THR_MODE:
{
if ( ConvParamsPtr->IntensityStreamOffset != NO_INTENSITY_INFORMATION ||
ConvParamsPtr->IntensitySizeBit != NO_INTENSITY_INFORMATION )
return CONVERSION_FAILURE;
if ( ConvParamsPtr->FixedPoint != 1 &&
ConvParamsPtr->FixedPoint != 2 )
return CONVERSION_FAILURE;
}
break;
case RANGER_HOR_MAX_MODE:
{
if (ConvParamsPtr->IntensitySizeBit != 8)
return CONVERSION_FAILURE;
if (ConvParamsPtr->FlipY != DO_FLIP_Y_AXIS)
return CONVERSION_FAILURE;
if (ConvParamsPtr->FixedPoint != 0)
return CONVERSION_FAILURE;
}
break;
case M_RANGER_Hi3D_MODE:
{
if (ConvParamsPtr->RangeSizeBit != 16)
return CONVERSION_FAILURE;
if (ConvParamsPtr->IntensitySizeBit != 16)
return CONVERSION_FAILURE;
if ( ConvParamsPtr->FixedPoint < 0 ||
ConvParamsPtr->FixedPoint > 7 )
return CONVERSION_FAILURE;
}
break;
default:
return CONVERSION_FAILURE;
}
if (ConvParamsPtr->MeasurementMode == M_RANGER_Hi3D_MODE)
{
if (ConvParamsPtr->FlipY == DO_FLIP_Y_AXIS)
return DoHi3DToMIL<true >(RangerBuf, PositionBuf, IntensityBuf, *ConvParamsPtr);
else
return DoHi3DToMIL<false>(RangerBuf, PositionBuf, IntensityBuf, *ConvParamsPtr);
}
else
{
return DoHorThrMaxToMIL(RangerBuf, PositionBuf, IntensityBuf, *ConvParamsPtr);
}
}
template <bool IsYAxisFlipped>
MIL_INT DoHi3DToMIL(MIL_BUFFER_INFO RangerBuf,
MIL_BUFFER_INFO PositionBuf,
MIL_BUFFER_INFO IntensityBuf,
const RangerParams& rConvParams)
{
MbufControl(MfuncBufId(RangerBuf ), M_LOCK+M_READ , M_DEFAULT);
MbufControl(MfuncBufId(PositionBuf ), M_LOCK+M_WRITE, M_DEFAULT);
MbufControl(MfuncBufId(IntensityBuf), M_LOCK+M_WRITE, M_DEFAULT);
MIL_INT SizeX = MfuncBufSizeX(PositionBuf);
MIL_INT SizeY = MfuncBufSizeY(PositionBuf);
MIL_INT RangerPitch = MfuncBufPitch(RangerBuf );
MIL_INT PositionPitch = MfuncBufPitch(PositionBuf );
MIL_INT IntensityPitch = MfuncBufPitch(IntensityBuf);
const MIL_UINT8* pRanger = (const MIL_UINT8*) MfuncBufHostAddress(RangerBuf);
MIL_UINT16* pPos = (MIL_UINT16*) MfuncBufHostAddress(PositionBuf );
MIL_UINT16* pInt = (MIL_UINT16*) MfuncBufHostAddress(IntensityBuf);
if (pRanger == NULL || pPos == NULL || pInt == NULL)
return CONVERSION_FAILURE;
const MIL_UINT16* pXSum =
reinterpret_cast<const MIL_UINT16*>( &pRanger[rConvParams.RangeStreamOffset ] );
const MIL_UINT16* pSum =
reinterpret_cast<const MIL_UINT16*>( &pRanger[rConvParams.IntensityStreamOffset] );
RangerPitch /= 2;
MIL_UINT32 LastY = static_cast<MIL_UINT32>( rConvParams.MeasROISizeY - 1 );
MIL_UINT32 FixedPoint = static_cast<MIL_UINT32>( rConvParams.FixedPoint );
for (MIL_INT y = 0; y < SizeY; ++y)
{
for (MIL_INT x = 0; x < SizeX; ++x)
{
MIL_UINT32 XSum = pXSum[x];
MIL_UINT32 Sum = pSum[x];
XSum |= ((Sum & 0xE000) << 3);
Sum &= 0x1FFF;
MIL_INT DstIdx = SizeX-1-x;
if (Sum != 0)
{
if (IsYAxisFlipped)
{
pPos[DstIdx] = static_cast<MIL_UINT16>(
( ((LastY*Sum - XSum) << FixedPoint) + (Sum >> 1) ) / Sum
);
}
else
{
pPos[DstIdx] = static_cast<MIL_UINT16>(
( ( XSum << FixedPoint) + (Sum >> 1) ) / Sum
);
}
}
else
{
pPos[DstIdx] = MIL_UINT16_MAX;
}
pInt[DstIdx] = static_cast<MIL_UINT16>( Sum );
}
pXSum += RangerPitch;
pSum += RangerPitch;
pPos += PositionPitch;
pInt += IntensityPitch;
}
MbufControl(MfuncBufId(RangerBuf ), M_UNLOCK, M_DEFAULT);
MbufControl(MfuncBufId(PositionBuf ), M_UNLOCK, M_DEFAULT);
MbufControl(MfuncBufId(IntensityBuf), M_UNLOCK, M_DEFAULT);
return CONVERSION_SUCCESS;
}
MIL_INT DoHorThrMaxToMIL(MIL_BUFFER_INFO RangerBuf,
MIL_BUFFER_INFO PositionBuf,
MIL_BUFFER_INFO IntensityBuf,
const RangerParams& rConvParams)
{
MIL_ID MilPosition, MilIntensity;
MIL_ID MilDestPosition = MfuncBufId (PositionBuf);
MIL_INT SizeX = MfuncBufSizeX(PositionBuf);
MIL_INT SizeY = MfuncBufSizeY(PositionBuf);
MIL_DOUBLE InvalidPos = static_cast<MIL_DOUBLE>( MfuncBufSizeBit(PositionBuf) == 8 ? MIL_UINT8_MAX : MIL_UINT16_MAX );
MIL_INT RangerPitch = MfuncBufPitch(RangerBuf);
MIL_UINT8* pRanger = (MIL_UINT8*)MfuncBufHostAddress(RangerBuf);
MbufCreate2d(M_DEFAULT_HOST, SizeX, SizeY, rConvParams.RangeSizeBit+M_UNSIGNED,
M_IMAGE+M_PROC, M_HOST_ADDRESS+M_PITCH_BYTE, RangerPitch,
pRanger+rConvParams.RangeStreamOffset, &MilPosition);
if (rConvParams.FlipY == DO_FLIP_Y_AXIS)
{
MIL_DOUBLE LastY = static_cast<MIL_DOUBLE>( (rConvParams.MeasROISizeY - 1) << rConvParams.FixedPoint );
MimArith(LastY, MilPosition, MilDestPosition, M_CONST_SUB);
MimClip(MilDestPosition, MilDestPosition, M_EQUAL, LastY, M_NULL, InvalidPos, M_NULL);
}
else
{
MimClip(MilPosition, MilDestPosition, M_EQUAL, 0.0, M_NULL, InvalidPos, M_NULL);
}
MimFlip(MilDestPosition, MilDestPosition, M_FLIP_HORIZONTAL, M_DEFAULT);
MbufFree(MilPosition);
if (rConvParams.IntensityStreamOffset != NO_INTENSITY_INFORMATION)
{
MbufCreate2d(M_DEFAULT_HOST, SizeX, SizeY, 8+M_UNSIGNED, M_IMAGE+M_PROC,
M_HOST_ADDRESS+M_PITCH_BYTE, RangerPitch,
pRanger+rConvParams.IntensityStreamOffset, &MilIntensity);
MimFlip(MilIntensity, MfuncBufId(IntensityBuf), M_FLIP_HORIZONTAL, M_DEFAULT);
MbufFree(MilIntensity);
}
return CONVERSION_SUCCESS;
}
bool GetBufInfoAndValidate(MIL_ID MilBuf, MIL_BUFFER_INFO* ppBuf)
{
if (MilBuf != M_NULL)
{
MIL_INT64 ObjectType;
MappInquireObject(M_DEFAULT, MilBuf, M_OBJECT_TYPE, &ObjectType);
if (ObjectType != M_IMAGE)
return false;
MIL_BUFFER_INFO pBuf;
MfuncInquire(MilBuf, M_BUFFER_INFO, &pBuf);
MIL_INT ImgSizeBand = MfuncBufSizeBand(pBuf);
MIL_INT ImgType = MfuncBufType(pBuf);
MIL_INT ImgSign = ImgType & (M_SIGNED | M_UNSIGNED | M_FLOAT);
MIL_INT ImgSizeBit = MfuncBufSizeBit(pBuf);
MIL_INT64 ImgFormat = MfuncBufFormat(pBuf);
if ( ImgSizeBand == 1 &&
ImgSign == M_UNSIGNED &&
(ImgSizeBit == 8 || ImgSizeBit == 16) &&
(ImgFormat & M_COMPRESS) != M_COMPRESS &&
(ImgFormat & M_PACKED ) != M_PACKED )
{
*ppBuf = pBuf;
return true;
}
else
{
return false;
}
}
else
{
return false;
}
}