XnStatus XnSensorFixedParams::Init()
{
XnStatus nRetVal = XN_STATUS_OK;
// get fixed params
XnFixedParams FixedParams;
nRetVal = XnHostProtocolGetFixedParams(m_pDevicePrivateData, FixedParams);
if (nRetVal != XN_STATUS_OK)
{
// Ugly patch since get param is not supported in maintenance mode!
if (nRetVal != XN_STATUS_DEVICE_PROTOCOL_INVALID_COMMAND)
{
return nRetVal;
}
return nRetVal;
}
if (m_pDevicePrivateData->FWInfo.nFWVer < XN_SENSOR_FW_VER_5_4)
{
sprintf(m_strSensorSerial, "%d", FixedParams.nSerialNumber);
}
else
{
nRetVal = XnHostProtocolGetSerialNumber(m_pDevicePrivateData, m_strSensorSerial);
if (nRetVal != XN_STATUS_OK)
{
return nRetVal;
}
}
xnLogVerbose(XN_MASK_DEVICE_SENSOR, "Sensor serial number: %s", m_strSensorSerial);
// fill in properties
m_nZeroPlaneDistance = (OniDepthPixel)FixedParams.fReferenceDistance;
m_dZeroPlanePixelSize = FixedParams.fReferencePixelSize;
m_dEmitterDCmosDistance = FixedParams.fDCmosEmitterDistance;
m_dDCmosRCmosDistance = FixedParams.fDCmosRCmosDistance;
m_nSensorDepthCMOSI2CBus = (XnUInt16)FixedParams.nDepthCmosI2CBus;
m_nSensorDepthCMOSI2CSlaveAddress = (XnUInt16)FixedParams.nDepthCmosI2CAddress;
m_nSensorImageCMOSI2CBus = (XnUInt16)FixedParams.nImageCmosI2CBus;
m_nSensorImageCMOSI2CSlaveAddress = (XnUInt16)FixedParams.nImageCmosI2CAddress;
m_nImageCmosType = (XnUInt32)FixedParams.nImageCmosType;
m_nDepthCmosType = (XnUInt32)FixedParams.nDepthCmosType;
nRetVal = XnHostProtocolAlgorithmParams(m_pDevicePrivateData, XN_HOST_PROTOCOL_ALGORITHM_DEVICE_INFO,
&m_deviceInfo, sizeof(m_deviceInfo), (XnResolutions)0, 0);
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnHostProtocolGetPlatformString(m_pDevicePrivateData, m_strPlatformString);
XN_IS_STATUS_OK(nRetVal);
return (XN_STATUS_OK);
}
XnStatus XnRegistration::BuildRegTable1000()
{
XnStatus nRetVal = XN_STATUS_OK;
// take needed parameters to perform registration
XnRegistrationInformation1000 regInfo1000;
nRetVal = XnHostProtocolAlgorithmParams(m_pDevicePrivateData, XN_HOST_PROTOCOL_ALGORITHM_REGISTRATION, ®Info1000, sizeof(regInfo1000), m_pDepthStream->GetResolution(), (XnUInt16)m_pDepthStream->GetFPS());
XN_IS_STATUS_OK(nRetVal);
XnUInt16* pRegTable = m_pRegistrationTable;
XnDouble dDeltaX, dDeltaY;
XnDouble dNewX = 0,
dNewY = 0;
XnUInt32 nDepthXRes = m_pDepthStream->GetXRes();
XnUInt32 nDepthYRes = m_pDepthStream->GetYRes();
const XnUInt16 nIllegalValue = XnUInt16(nDepthXRes*4);
for (XnUInt16 nY = 0; nY < nDepthYRes; nY++)
{
for (XnUInt16 nX = 0; nX < nDepthXRes; nX++)
{
dDeltaX = XnXRegistrationFunction1000(regInfo1000, nX, nY, nDepthXRes, nDepthYRes);
dDeltaY = XnYRegistrationFunction1000(regInfo1000, nX, nY, nDepthXRes, nDepthYRes);
dNewX = (nX + dDeltaX);
dNewY = nY + dDeltaY;
if (dNewY < 1 || dNewY > nDepthYRes)
{
dNewY = 1;
dNewX = nIllegalValue;
}
if (dNewX < 1 || dNewX > nDepthXRes)
{
dNewX = nIllegalValue;
}
dNewX *= XN_REG_X_SCALE;
*pRegTable = (XnUInt16)dNewX;
*(pRegTable + 1) = (XnUInt16)dNewY;
pRegTable += 2;
}
}
m_dShiftFactor = regInfo1000.dBeta;
return (XN_STATUS_OK);
}
XnStatus XnSensorDepthStream::PopulateSensorCalibrationInfo()
{
XnDouble dPlanePixelSize;
GetProperty(XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE, &dPlanePixelSize);
XnUInt64 nPlaneDsr;
GetProperty(XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE, &nPlaneDsr);
XnDouble dDCRCDist;
GetProperty(XN_STREAM_PROPERTY_DCMOS_RCMOS_DISTANCE, &dDCRCDist);
m_calibrationInfo.magic = ONI_DEPTH_UTILS_CALIBRATION_INFO_MAGIC;
m_calibrationInfo.version = 1;
m_calibrationInfo.params1080.zpd = (int)nPlaneDsr;
m_calibrationInfo.params1080.zpps = dPlanePixelSize;
m_calibrationInfo.params1080.dcrcdist = dDCRCDist;
xnOSStrCopy(m_calibrationInfo.deviceName, "PS1080", 80);
xnOSMemSet(m_calibrationInfo.serial, 0, 80);
m_calibrationInfo.params1080.rgbRegXRes = RGB_REG_X_RES;
m_calibrationInfo.params1080.rgbRegYRes = RGB_REG_Y_RES;
m_calibrationInfo.params1080.cmosVGAOutputXRes = XN_CMOS_VGAOUTPUT_XRES;
m_calibrationInfo.params1080.sensorWinOffsetX = XN_SENSOR_WIN_OFFET_X;
m_calibrationInfo.params1080.sensorWinOffsetY = XN_SENSOR_WIN_OFFET_Y;
m_calibrationInfo.params1080.rgbRegXValScale = RGB_REG_X_VAL_SCALE;
m_calibrationInfo.params1080.s2dPelConst = S2D_PEL_CONST;
m_calibrationInfo.params1080.s2dConstOffset = S2D_CONST_OFFSET;
XnStatus nRetVal = XnHostProtocolAlgorithmParams(m_Helper.GetPrivateData(), XN_HOST_PROTOCOL_ALGORITHM_REGISTRATION, &m_calibrationInfo.params1080.registrationInfo_QQVGA, sizeof(m_calibrationInfo.params1080.registrationInfo_QQVGA), XN_RESOLUTION_QQVGA, 30);
if (nRetVal != XN_STATUS_OK)
{
xnOSMemSet(&m_calibrationInfo.params1080.registrationInfo_QQVGA, 0, sizeof(m_calibrationInfo.params1080.registrationInfo_QQVGA));
}
nRetVal = XnHostProtocolAlgorithmParams(m_Helper.GetPrivateData(), XN_HOST_PROTOCOL_ALGORITHM_REGISTRATION, &m_calibrationInfo.params1080.registrationInfo_QVGA, sizeof(m_calibrationInfo.params1080.registrationInfo_QVGA), XN_RESOLUTION_QVGA, 30);
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnHostProtocolAlgorithmParams(m_Helper.GetPrivateData(), XN_HOST_PROTOCOL_ALGORITHM_REGISTRATION, &m_calibrationInfo.params1080.registrationInfo_VGA, sizeof(m_calibrationInfo.params1080.registrationInfo_VGA), XN_RESOLUTION_VGA, 30);
XN_IS_STATUS_OK(nRetVal);
nRetVal = XnHostProtocolAlgorithmParams(m_Helper.GetPrivateData(), XN_HOST_PROTOCOL_ALGORITHM_PADDING, &m_calibrationInfo.params1080.padInfo_QQVGA, sizeof(m_calibrationInfo.params1080.padInfo_QQVGA), XN_RESOLUTION_QQVGA, 30);
if (nRetVal != XN_STATUS_OK)
{
xnOSMemSet(&m_calibrationInfo.params1080.padInfo_QQVGA, 0, sizeof(m_calibrationInfo.params1080.padInfo_QQVGA));
}
xnOSMemSet(&m_calibrationInfo.params1080.padInfo_QVGA, 0, sizeof(m_calibrationInfo.params1080.padInfo_QVGA));
nRetVal = XnHostProtocolAlgorithmParams(m_Helper.GetPrivateData(), XN_HOST_PROTOCOL_ALGORITHM_PADDING, &m_calibrationInfo.params1080.padInfo_QVGA, sizeof(m_calibrationInfo.params1080.padInfo_QVGA), XN_RESOLUTION_QVGA, 30);
XN_IS_STATUS_OK(nRetVal);
xnOSMemSet(&m_calibrationInfo.params1080.padInfo_VGA, 0, sizeof(m_calibrationInfo.params1080.padInfo_VGA));
nRetVal = XnHostProtocolAlgorithmParams(m_Helper.GetPrivateData(), XN_HOST_PROTOCOL_ALGORITHM_PADDING, &m_calibrationInfo.params1080.padInfo_VGA, sizeof(m_calibrationInfo.params1080.padInfo_VGA), XN_RESOLUTION_VGA, 30);
XN_IS_STATUS_OK(nRetVal);
return XN_STATUS_OK;
}
XnStatus XnCmosInfo::SetCmosConfig(XnCMOSType nCmos, XnResolutions nResolution, XnUInt32 nFPS)
{
XnStatus nRetVal = XN_STATUS_OK;
if (m_pFirmware->GetInfo()->nFWVer >= XN_SENSOR_FW_VER_5_1)
{
// take blanking info
XnCmosBlankingInformation* pInfo = NULL;
// search the list if we already have this info
for (XnCmosBlankingDataList::Iterator it = m_CmosBlankingInfo.Begin(); it != m_CmosBlankingInfo.End(); ++it)
{
XnCmosBlankingData& data = *it;
if (data.nRes == nResolution && data.nFPS == nFPS)
{
pInfo = &data.BlankingInfo;
break;
}
}
if (pInfo == NULL)
{
// not found in list. fetch it from FW
XnCmosBlankingData data;
data.nRes = nResolution;
data.nFPS = nFPS;
nRetVal = XnHostProtocolAlgorithmParams(m_pDevicePrivateData, XN_HOST_PROTOCOL_ALGORITHM_BLANKING, &data.BlankingInfo, sizeof(XnCmosBlankingInformation), nResolution, (XnUInt16)nFPS);
XN_IS_STATUS_OK(nRetVal);
// add to list
nRetVal = m_CmosBlankingInfo.AddFirst(data);
XN_IS_STATUS_OK(nRetVal);
// take its info (take a pointer to the object in the list, and not to the one on the stack)
pInfo = &m_CmosBlankingInfo.Begin()->BlankingInfo;
}
m_pCurrCmosBlankingInfo[nCmos] = &pInfo->Coefficients[nCmos];
}
return (XN_STATUS_OK);
}
XnStatus XnSensor::InitReading()
{
XnStatus nRetVal = XN_STATUS_OK;
// open data endpoints
nRetVal = m_SensorIO.OpenDataEndPoints((XnSensorUsbInterface)m_Interface.GetValue(), *m_Firmware.GetInfo());
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_Interface.UnsafeUpdateValue(m_SensorIO.GetCurrentInterface());
XN_IS_STATUS_OK(nRetVal);
// take frequency information
XnFrequencyInformation FrequencyInformation;
nRetVal = XnHostProtocolAlgorithmParams(&m_DevicePrivateData, XN_HOST_PROTOCOL_ALGORITHM_FREQUENCY, &FrequencyInformation, sizeof(XnFrequencyInformation), (XnResolutions)0, 0);
if (nRetVal != XN_STATUS_OK)
return nRetVal;
m_DevicePrivateData.fDeviceFrequency = XN_PREPARE_VAR_FLOAT_IN_BUFFER(FrequencyInformation.fDeviceFrequency);
// Init Dumps
m_DevicePrivateData.BandwidthDump = xnDumpFileOpen(XN_DUMP_BANDWIDTH, "Bandwidth.csv");
xnDumpFileWriteString(m_DevicePrivateData.BandwidthDump, "Timestamp,Frame Type,Frame ID,Size\n");
m_DevicePrivateData.TimestampsDump = xnDumpFileOpen(XN_DUMP_TIMESTAMPS, "Timestamps.csv");
xnDumpFileWriteString(m_DevicePrivateData.TimestampsDump, "Host Time (us),Stream,Device TS,Time (ms),Comments\n");
m_DevicePrivateData.MiniPacketsDump = xnDumpFileOpen(XN_DUMP_MINI_PACKETS, "MiniPackets.csv");
xnDumpFileWriteString(m_DevicePrivateData.MiniPacketsDump, "HostTS,Type,ID,Size,Timestamp\n");
m_DevicePrivateData.nGlobalReferenceTS = 0;
nRetVal = xnOSCreateCriticalSection(&m_DevicePrivateData.hEndPointsCS);
XN_IS_STATUS_OK(nRetVal);
// NOTE: when we go up, some streams might be open, and so we'll receive lots of garbage.
// wait till streams are turned off, and then start reading.
// pDevicePrivateData->bIgnoreDataPackets = TRUE;
// open input threads
nRetVal = XnDeviceSensorOpenInputThreads(GetDevicePrivateData(), (XnBool)m_ReadFromEP1.GetValue(), (XnBool)m_ReadFromEP2.GetValue(), (XnBool)m_ReadFromEP3.GetValue());
XN_IS_STATUS_OK(nRetVal);
return XN_STATUS_OK;
}
XnStatus XnSensorStreamHelper::Open()
{
XnStatus nRetVal = XN_STATUS_OK;
// configure the stream
nRetVal = Configure();
XN_IS_STATUS_OK(nRetVal);
// Update frequency (it might change on specific stream configuration)
XnFrequencyInformation FrequencyInformation;
nRetVal = XnHostProtocolAlgorithmParams(m_pObjects->pDevicePrivateData, XN_HOST_PROTOCOL_ALGORITHM_FREQUENCY, &FrequencyInformation, sizeof(XnFrequencyInformation), (XnResolutions)0, 0);
XN_IS_STATUS_OK(nRetVal);
m_pObjects->pDevicePrivateData->fDeviceFrequency = XN_PREPARE_VAR_FLOAT_IN_BUFFER(FrequencyInformation.fDeviceFrequency);
// and now turn it on
nRetVal = FinalOpen();
XN_IS_STATUS_OK(nRetVal);
return (XN_STATUS_OK);
}
XnStatus XnSensorDepthStream::Init()
{
XnStatus nRetVal = XN_STATUS_OK;
nRetVal = SetBufferPool(&m_BufferPool);
XN_IS_STATUS_OK(nRetVal);
// init base
nRetVal = XnDepthStream::Init();
XN_IS_STATUS_OK(nRetVal);
m_InputFormat.UpdateSetCallback(SetInputFormatCallback, this);
m_DepthRegistration.UpdateSetCallback(SetRegistrationCallback, this);
m_HoleFilter.UpdateSetCallback(SetHoleFilterCallback, this);
m_WhiteBalance.UpdateSetCallback(SetWhiteBalanceCallback, this);
m_Gain.UpdateSetCallback(SetGainCallback, this);
m_RegistrationType.UpdateSetCallback(SetRegistrationTypeCallback, this);
m_AGCBin.UpdateSetCallback(SetAGCBinCallback, this);
m_AGCBin.UpdateGetCallback(GetAGCBinCallback, this);
m_GMCMode.UpdateSetCallback(SetGMCModeCallback, this);
XN_VALIDATE_ADD_PROPERTIES(this, &m_InputFormat, &m_DepthRegistration, &m_HoleFilter,
&m_WhiteBalance, &m_Gain, &m_AGCBin, &m_SharedBufferName, &m_ActualRead, &m_GMCMode,
&m_RegistrationType);
if (m_Helper.GetPrivateData()->pSensor->IsLowBandwidth())
{
nRetVal = m_InputFormat.UnsafeUpdateValue(XN_IO_DEPTH_FORMAT_COMPRESSED_PS);
XN_IS_STATUS_OK(nRetVal);
}
// set base properties default values
nRetVal = ResolutionProperty().UnsafeUpdateValue(XN_DEPTH_STREAM_DEFAULT_RESOLUTION);
XN_IS_STATUS_OK(nRetVal);
nRetVal = FPSProperty().UnsafeUpdateValue(XN_DEPTH_STREAM_DEFAULT_FPS);
XN_IS_STATUS_OK(nRetVal);
nRetVal = OutputFormatProperty().UnsafeUpdateValue(XN_DEPTH_STREAM_DEFAULT_OUTPUT_FORMAT);
XN_IS_STATUS_OK(nRetVal);
nRetVal = ParamCoefficientProperty().UnsafeUpdateValue(XN_SHIFTS_PARAM_COEFFICIENT);
XN_IS_STATUS_OK(nRetVal);
nRetVal = ShiftScaleProperty().UnsafeUpdateValue(XN_SHIFTS_SHIFT_SCALE);
XN_IS_STATUS_OK(nRetVal);
// read some data from firmware
XnDepthInformation DepthInformation;
nRetVal = XnHostProtocolAlgorithmParams(m_Helper.GetPrivateData(), XN_HOST_PROTOCOL_ALGORITHM_DEPTH_INFO, &DepthInformation, sizeof(XnDepthInformation), XN_RESOLUTION_VGA, 30);
XN_IS_STATUS_OK(nRetVal);
nRetVal = ConstShiftProperty().UnsafeUpdateValue(DepthInformation.nConstShift);
XN_IS_STATUS_OK(nRetVal);
nRetVal = ZeroPlaneDistanceProperty().UnsafeUpdateValue(m_Helper.GetFixedParams()->GetZeroPlaneDistance());
XN_IS_STATUS_OK(nRetVal);
nRetVal = ZeroPlanePixelSizeProperty().UnsafeUpdateValue(m_Helper.GetFixedParams()->GetZeroPlanePixelSize());
XN_IS_STATUS_OK(nRetVal);
nRetVal = EmitterDCmosDistanceProperty().UnsafeUpdateValue(m_Helper.GetFixedParams()->GetEmitterDCmosDistance());
XN_IS_STATUS_OK(nRetVal);
nRetVal = GetDCmosRCmosDistanceProperty().UnsafeUpdateValue(m_Helper.GetFixedParams()->GetDCmosRCmosDistance());
XN_IS_STATUS_OK(nRetVal);
// init helper
nRetVal = m_Helper.Init(this, this);
XN_IS_STATUS_OK(nRetVal);
if (m_Helper.GetFirmwareVersion() < XN_SENSOR_FW_VER_3_0)
{
nRetVal = m_GMCMode.UnsafeUpdateValue(FALSE);
XN_IS_STATUS_OK(nRetVal);
}
if (m_Helper.GetFirmwareVersion() < XN_SENSOR_FW_VER_4_0)
{
nRetVal = m_WhiteBalance.UnsafeUpdateValue(FALSE);
XN_IS_STATUS_OK(nRetVal);
}
// on old firmwares, the host decides on the default gain. On new firmwares, we read it from firmware
if (m_Helper.GetFirmware()->GetInfo()->nFWVer > XN_SENSOR_FW_VER_1_2)
{
nRetVal = m_Gain.UnsafeUpdateValue(GetFirmwareParams()->m_DepthGain.GetValue());
XN_IS_STATUS_OK(nRetVal);
}
// registration
XnCallbackHandle hCallbackDummy;
nRetVal = ResolutionProperty().OnChangeEvent().Register(DecideFirmwareRegistrationCallback, this, &hCallbackDummy);
XN_IS_STATUS_OK(nRetVal);
nRetVal = DecideFirmwareRegistration((XnBool)m_DepthRegistration.GetValue(), (XnProcessingType)m_RegistrationType.GetValue(), GetResolution());
XN_IS_STATUS_OK(nRetVal);
// data processor
nRetVal = m_Helper.RegisterDataProcessorProperty(m_InputFormat);
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_Helper.RegisterDataProcessorProperty(ResolutionProperty());
XN_IS_STATUS_OK(nRetVal);
// pixel size factor
nRetVal = GetFirmwareParams()->m_ReferenceResolution.OnChangeEvent().Register(DecidePixelSizeFactorCallback, this, &m_hReferenceSizeChangedCallback);
XN_IS_STATUS_OK(nRetVal);
nRetVal = DecidePixelSizeFactor();
XN_IS_STATUS_OK(nRetVal);
// register supported modes
XnCmosPreset aSupportedModes[] =
{
{ XN_IO_DEPTH_FORMAT_COMPRESSED_PS, XN_RESOLUTION_QVGA, 30 },
{ XN_IO_DEPTH_FORMAT_COMPRESSED_PS, XN_RESOLUTION_QVGA, 60 },
{ XN_IO_DEPTH_FORMAT_COMPRESSED_PS, XN_RESOLUTION_VGA, 30 },
{ XN_IO_DEPTH_FORMAT_UNCOMPRESSED_11_BIT, XN_RESOLUTION_QVGA, 30 },
{ XN_IO_DEPTH_FORMAT_UNCOMPRESSED_11_BIT, XN_RESOLUTION_QVGA, 60 },
{ XN_IO_DEPTH_FORMAT_UNCOMPRESSED_11_BIT, XN_RESOLUTION_VGA, 30 },
{ XN_IO_DEPTH_FORMAT_UNCOMPRESSED_16_BIT, XN_RESOLUTION_QVGA, 30 },
{ XN_IO_DEPTH_FORMAT_UNCOMPRESSED_16_BIT, XN_RESOLUTION_QVGA, 60 },
{ XN_IO_DEPTH_FORMAT_UNCOMPRESSED_16_BIT, XN_RESOLUTION_VGA, 30 },
};
nRetVal = AddSupportedModes(aSupportedModes, sizeof(aSupportedModes)/sizeof(aSupportedModes[0]));
XN_IS_STATUS_OK(nRetVal);
if (m_Helper.GetFirmwareVersion() >= XN_SENSOR_FW_VER_5_2)
{
XnCmosPreset aSupportedModes25[] =
{
{ XN_IO_DEPTH_FORMAT_COMPRESSED_PS, XN_RESOLUTION_QVGA, 25 },
{ XN_IO_DEPTH_FORMAT_COMPRESSED_PS, XN_RESOLUTION_VGA, 25 },
{ XN_IO_DEPTH_FORMAT_UNCOMPRESSED_11_BIT, XN_RESOLUTION_QVGA, 25 },
{ XN_IO_DEPTH_FORMAT_UNCOMPRESSED_11_BIT, XN_RESOLUTION_VGA, 25 },
{ XN_IO_DEPTH_FORMAT_UNCOMPRESSED_16_BIT, XN_RESOLUTION_QVGA, 25 },
{ XN_IO_DEPTH_FORMAT_UNCOMPRESSED_16_BIT, XN_RESOLUTION_VGA, 25 },
};
nRetVal = AddSupportedModes(aSupportedModes25, sizeof(aSupportedModes25)/sizeof(aSupportedModes25[0]));
XN_IS_STATUS_OK(nRetVal);
}
return (XN_STATUS_OK);
}
XnStatus XnSensor::InitSensor(const XnDeviceConfig* pDeviceConfig)
{
XnStatus nRetVal = XN_STATUS_OK;
XnDevicePrivateData* pDevicePrivateData = GetDevicePrivateData();
pDevicePrivateData->pSensor = this;
pDevicePrivateData->nDepthFramePos = 0;
pDevicePrivateData->nImageFramePos = 0;
xnOSMemCopy(&pDevicePrivateData->DeviceConfig, pDeviceConfig, sizeof(XnDeviceConfig));
xnOSMemSet(pDevicePrivateData->cpSensorID, 0, XN_SENSOR_PROTOCOL_SENSOR_ID_LENGTH);
pDevicePrivateData->bSyncAudio = TRUE;
switch (pDeviceConfig->DeviceMode)
{
case XN_DEVICE_MODE_READ:
break;
case XN_DEVICE_MODE_WRITE:
return (XN_STATUS_IO_DEVICE_MODE_NOT_SUPPORTED);
default:
return (XN_STATUS_IO_DEVICE_INVALID_MODE);
}
// Register USB event callback
#if WIN32
nRetVal = m_SensorIO.SetCallback(&USBEventCallback, this);
XN_IS_STATUS_OK(nRetVal);
#endif
// open IO
nRetVal = m_SensorIO.OpenDevice(pDeviceConfig->cpConnectionString);
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_USBPath.UnsafeUpdateValue(m_SensorIO.GetDevicePath());
XN_IS_STATUS_OK(nRetVal);
// initialize
nRetVal = XnDeviceSensorInit(pDevicePrivateData);
XN_IS_STATUS_OK(nRetVal);
// init firmware
nRetVal = m_Firmware.Init((XnBool)m_ResetSensorOnStartup.GetValue());
XN_IS_STATUS_OK(nRetVal);
m_bInitialized = TRUE;
m_ResetSensorOnStartup.UpdateSetCallback(NULL, NULL);
// Init modules
nRetVal = m_FixedParams.Init();
XN_IS_STATUS_OK(nRetVal);
XnDeviceInformation deviceInfo;
strcpy(deviceInfo.strDeviceName, "PrimeSense Sensor");
strcpy(deviceInfo.strVendorData, "");
// try to take device information (only supported from 5.3.25)
if (pDevicePrivateData->Version.nMajor > 5 ||
(pDevicePrivateData->Version.nMajor == 5 && pDevicePrivateData->Version.nMinor > 3) ||
(pDevicePrivateData->Version.nMajor == 5 && pDevicePrivateData->Version.nMinor == 3 && pDevicePrivateData->Version.nBuild >= 25))
{
nRetVal = XnHostProtocolAlgorithmParams(pDevicePrivateData, XN_HOST_PROTOCOL_ALGORITHM_DEVICE_INFO,
&deviceInfo, sizeof(deviceInfo), (XnResolutions)0, 0);
XN_IS_STATUS_OK(nRetVal);
}
nRetVal = m_DeviceName.UnsafeUpdateValue(deviceInfo.strDeviceName);
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_VendorSpecificData.UnsafeUpdateValue(deviceInfo.strVendorData);
XN_IS_STATUS_OK(nRetVal);
// update serial number
nRetVal = m_ID.UnsafeUpdateValue(m_FixedParams.GetSensorSerial());
XN_IS_STATUS_OK(nRetVal);
AddSupportedStream(XN_STREAM_TYPE_DEPTH);
AddSupportedStream(XN_STREAM_TYPE_IMAGE);
AddSupportedStream(XN_STREAM_TYPE_IR);
AddSupportedStream(XN_STREAM_TYPE_AUDIO);
return XN_STATUS_OK;
}
XnStatus XnSensorDepthStream::Init()
{
XnStatus nRetVal = XN_STATUS_OK;
// init base
nRetVal = XnDepthStream::Init();
XN_IS_STATUS_OK(nRetVal);
// start with no cut-off
nRetVal = MaxDepthProperty().UnsafeUpdateValue(XN_DEVICE_SENSOR_MAX_DEPTH_100_UM);
XN_IS_STATUS_OK(nRetVal);
m_InputFormat.UpdateSetCallback(SetInputFormatCallback, this);
m_DepthRegistration.UpdateSetCallback(SetRegistrationCallback, this);
m_HoleFilter.UpdateSetCallback(SetHoleFilterCallback, this);
m_WhiteBalance.UpdateSetCallback(SetWhiteBalanceCallback, this);
m_Gain.UpdateSetCallback(SetGainCallback, this);
m_RegistrationType.UpdateSetCallback(SetRegistrationTypeCallback, this);
m_AGCBin.UpdateSetCallback(SetAGCBinCallback, this);
m_AGCBin.UpdateGetCallback(GetAGCBinCallback, this);
m_GMCMode.UpdateSetCallback(SetGMCModeCallback, this);
m_CloseRange.UpdateSetCallback(SetCloseRangeCallback, this);
m_CroppingMode.UpdateSetCallback(SetCroppingModeCallback, this);
m_PixelRegistration.UpdateGetCallback(GetPixelRegistrationCallback, this);
m_GMCDebug.UpdateSetCallback(SetGMCDebugCallback, this);
m_WavelengthCorrection.UpdateSetCallback(SetWavelengthCorrectionCallback, this);
m_WavelengthCorrectionDebug.UpdateSetCallback(SetWavelengthCorrectionDebugCallback, this);
XN_VALIDATE_ADD_PROPERTIES(this, &m_InputFormat, &m_DepthRegistration, &m_HoleFilter,
&m_WhiteBalance, &m_Gain, &m_AGCBin, &m_ActualRead, &m_GMCMode,
&m_CloseRange, &m_CroppingMode, &m_RegistrationType, &m_PixelRegistration,
&m_HorizontalFOV, &m_VerticalFOV, &m_GMCDebug, &m_WavelengthCorrection, &m_WavelengthCorrectionDebug,
);
// register supported modes
XnCmosPreset* pSupportedModes = m_Helper.GetPrivateData()->FWInfo.depthModes.GetData();
XnUInt32 nSupportedModes = m_Helper.GetPrivateData()->FWInfo.depthModes.GetSize();
nRetVal = AddSupportedModes(pSupportedModes, nSupportedModes);
XN_IS_STATUS_OK(nRetVal);
if (m_Helper.GetPrivateData()->pSensor->IsLowBandwidth())
{
nRetVal = m_InputFormat.UnsafeUpdateValue(XN_IO_DEPTH_FORMAT_COMPRESSED_PS);
XN_IS_STATUS_OK(nRetVal);
}
// make sure default resolution is supported
XnBool bResSupported = FALSE;
for (XnUInt8 i = 0; i < nSupportedModes; ++i)
{
if (pSupportedModes[i].nResolution == XN_DEPTH_STREAM_DEFAULT_RESOLUTION)
{
bResSupported = TRUE;
break;
}
}
XnUInt64 nDefaultResolution = XN_DEPTH_STREAM_DEFAULT_RESOLUTION;
if (!bResSupported)
{
// QVGA was always supported
nDefaultResolution = XN_RESOLUTION_QVGA;
}
nRetVal = ResolutionProperty().UnsafeUpdateValue(nDefaultResolution);
XN_IS_STATUS_OK(nRetVal);
// set other properties default values
nRetVal = FPSProperty().UnsafeUpdateValue(XN_DEPTH_STREAM_DEFAULT_FPS);
XN_IS_STATUS_OK(nRetVal);
nRetVal = OutputFormatProperty().UnsafeUpdateValue(XN_DEPTH_STREAM_DEFAULT_OUTPUT_FORMAT);
XN_IS_STATUS_OK(nRetVal);
nRetVal = ParamCoefficientProperty().UnsafeUpdateValue(XN_SHIFTS_PARAM_COEFFICIENT);
XN_IS_STATUS_OK(nRetVal);
nRetVal = ShiftScaleProperty().UnsafeUpdateValue(XN_SHIFTS_SHIFT_SCALE);
XN_IS_STATUS_OK(nRetVal);
// read some data from firmware
XnDepthInformation DepthInformation;
nRetVal = XnHostProtocolAlgorithmParams(m_Helper.GetPrivateData(), XN_HOST_PROTOCOL_ALGORITHM_DEPTH_INFO, &DepthInformation, sizeof(XnDepthInformation), XN_RESOLUTION_VGA, 30);
XN_IS_STATUS_OK(nRetVal);
nRetVal = ConstShiftProperty().UnsafeUpdateValue(DepthInformation.nConstShift);
XN_IS_STATUS_OK(nRetVal);
nRetVal = ZeroPlaneDistanceProperty().UnsafeUpdateValue(m_Helper.GetFixedParams()->GetZeroPlaneDistance());
XN_IS_STATUS_OK(nRetVal);
nRetVal = ZeroPlanePixelSizeProperty().UnsafeUpdateValue(m_Helper.GetFixedParams()->GetZeroPlanePixelSize());
XN_IS_STATUS_OK(nRetVal);
nRetVal = EmitterDCmosDistanceProperty().UnsafeUpdateValue(m_Helper.GetFixedParams()->GetEmitterDCmosDistance());
XN_IS_STATUS_OK(nRetVal);
nRetVal = GetDCmosRCmosDistanceProperty().UnsafeUpdateValue(m_Helper.GetFixedParams()->GetDCmosRCmosDistance());
XN_IS_STATUS_OK(nRetVal);
XnDouble fZPPS = m_Helper.GetFixedParams()->GetZeroPlanePixelSize();
XnInt nZPD = m_Helper.GetFixedParams()->GetZeroPlaneDistance();
nRetVal = m_HorizontalFOV.UnsafeUpdateValue(2*atan(fZPPS*XN_SXGA_X_RES/2/nZPD));
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_VerticalFOV.UnsafeUpdateValue(2*atan(fZPPS*XN_VGA_Y_RES*2/2/nZPD));
XN_IS_STATUS_OK(nRetVal);
// init helper
nRetVal = m_Helper.Init(this, this);
XN_IS_STATUS_OK(nRetVal);
if (m_Helper.GetFirmwareVersion() < XN_SENSOR_FW_VER_3_0)
{
nRetVal = m_GMCMode.UnsafeUpdateValue(FALSE);
XN_IS_STATUS_OK(nRetVal);
}
if (m_Helper.GetFirmwareVersion() < XN_SENSOR_FW_VER_5_2)
{
nRetVal = m_WavelengthCorrection.UnsafeUpdateValue(FALSE);
XN_IS_STATUS_OK(nRetVal);
}
if (m_Helper.GetFirmwareVersion() < XN_SENSOR_FW_VER_4_0)
{
nRetVal = m_WhiteBalance.UnsafeUpdateValue(FALSE);
XN_IS_STATUS_OK(nRetVal);
}
// on old firmwares, the host decides on the default gain. On new firmwares, we read it from firmware
if (m_Helper.GetFirmware()->GetInfo()->nFWVer > XN_SENSOR_FW_VER_1_2)
{
nRetVal = m_Gain.UnsafeUpdateValue(GetFirmwareParams()->m_DepthGain.GetValue());
XN_IS_STATUS_OK(nRetVal);
}
// registration
XnCallbackHandle hCallbackDummy;
nRetVal = ResolutionProperty().OnChangeEvent().Register(DecideFirmwareRegistrationCallback, this, hCallbackDummy);
XN_IS_STATUS_OK(nRetVal);
nRetVal = DecideFirmwareRegistration((XnBool)m_DepthRegistration.GetValue(), (XnProcessingType)m_RegistrationType.GetValue(), GetResolution());
XN_IS_STATUS_OK(nRetVal);
// data processor
nRetVal = m_Helper.RegisterDataProcessorProperty(m_InputFormat);
XN_IS_STATUS_OK(nRetVal);
nRetVal = m_Helper.RegisterDataProcessorProperty(ResolutionProperty());
XN_IS_STATUS_OK(nRetVal);
// pixel size factor
nRetVal = GetFirmwareParams()->m_ReferenceResolution.OnChangeEvent().Register(DecidePixelSizeFactorCallback, this, m_hReferenceSizeChangedCallback);
XN_IS_STATUS_OK(nRetVal);
nRetVal = DecidePixelSizeFactor();
XN_IS_STATUS_OK(nRetVal);
// initialize registration
if (m_Helper.GetFirmwareVersion() > XN_SENSOR_FW_VER_5_3)
{
nRetVal = PopulateSensorCalibrationInfo();
XN_IS_STATUS_OK(nRetVal);
nRetVal = DepthUtilsInitialize(&m_calibrationInfo, &m_depthUtilsHandle);
XN_IS_STATUS_OK(nRetVal);
nRetVal = DepthUtilsSetDepthConfiguration(m_depthUtilsHandle, GetXRes(), GetYRes(), GetOutputFormat(), IsMirrored());
XN_IS_STATUS_OK(nRetVal);
}
return (XN_STATUS_OK);
}
