Ejemplo n.º 1
0
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);
}
Ejemplo n.º 2
0
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);
}