public static DS4CalibrationRecord Calibrate(PXCMCapture.Device device) { PXCMProjection projection = device.CreateProjection(); /* Get a calibration instance */ PXCMCalibration calib = projection.QueryInstance <PXCMCalibration>(); PXCMCalibration.StreamCalibration calibration; PXCMCalibration.StreamTransform transformation; calib.QueryStreamProjectionParameters(PXCMCapture.StreamType.STREAM_TYPE_COLOR, out calibration, out transformation); float[] translation = transformation.translation; DS4CalibrationRecord record = new DS4CalibrationRecord { DeviceCapture = new DS4CalibrationRecordInternal { ColorFocalLength = device.QueryColorFocalLength().toFloatArray(), DepthFocalLength = device.QueryDepthFocalLength().toFloatArray(), ColorFOV = device.QueryColorFieldOfView().toFloatArray(), DepthFOV = device.QueryDepthFieldOfView().toFloatArray(), ColorPrincipalPoint = device.QueryColorPrincipalPoint().toFloatArray(), DepthPrincipalPoint = device.QueryDepthPrincipalPoint().toFloatArray(), ColorWidth = Frame.COLOR_WIDTH, ColorHeight = Frame.COLOR_HEIGHT, DepthWidth = Frame.DEPTH_WIDTH, DepthHeight = Frame.DEPTH_HEIGHT, DepthStride = Frame.DEPTH_STRIDE, LowConfValue = device.QueryDepthLowConfidenceValue(), Extrinsics = new float[] { 1.0f, 0.0f, 0.0f, translation[0], 0.0f, 1.0f, 0.0f, translation[1], 0.0f, 0.0f, 1.0f, translation[2], 0.0f, 0.0f, 0.0f, 1.0f } }, API = "RSSDK" }; return(record); }
/// <summary> /// Device-specific implementation of Connect. /// Connects the camera. /// </summary> /// <remarks>This method is implicitely called by <see cref="Camera.Connect"/> inside a camera lock.</remarks> /// <seealso cref="Camera.Connect"/> protected override void ConnectImpl() { log.EnterMethod(); if (deviceInfo.Count == 0) { ScanForCameras(); } if (deviceInfo.Count == 0) { log.Error(Name + ": No devices found."); ExceptionBuilder.Throw(typeof(ConnectionFailedException), this, "No devices found."); } int deviceIdx; for (deviceIdx = 0; deviceIdx < deviceInfo.Count; deviceIdx++) { if (deviceInfo[deviceIdx].name != "Intel(R) RealSense(TM) 3D Camera R200") { continue; } log.Debug("RealSense R200 found"); break; } if (deviceIdx >= deviceInfo.Count) { log.Error("Failed to find Intel Real Sense R200 camera!"); ExceptionBuilder.Throw(typeof(ConnectionFailedException), this, "No R200 found!"); } ScanForProfiles(deviceIdx); // Create an instance of the PXCSenseManager interface pp = PXCMSenseManager.CreateInstance(); if (pp == null) { log.Error(Name + ": Failed to create an SDK pipeline object"); return; } pp.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_COLOR, widthColor, heightColor, fpsColor); pp.EnableStream(PXCMCapture.StreamType.STREAM_TYPE_DEPTH, widthZImage, heightZImage, fpsZImage); pxcmStatus retStat = pp.Init(); if (retStat < pxcmStatus.PXCM_STATUS_NO_ERROR) { log.Error(Name + ": Init() failed."); ExceptionBuilder.Throw(typeof(ConnectionFailedException), this, "Failed to Initialize Real Sense SDK"); } pp.captureManager.device.ResetProperties(PXCMCapture.StreamType.STREAM_TYPE_ANY); // Find calibration data PXCMProjection projection = pp.captureManager.device.CreateProjection(); PXCMCalibration calib = projection.QueryInstance <PXCMCalibration>(); retStat = calib.QueryStreamProjectionParameters(PXCMCapture.StreamType.STREAM_TYPE_COLOR, out calibDataColor, out calibTransColor); if (retStat < pxcmStatus.PXCM_STATUS_NO_ERROR) { log.WarnFormat("Could not get calibration for color channel."); } retStat = calib.QueryStreamProjectionParameters(PXCMCapture.StreamType.STREAM_TYPE_DEPTH, out calibDataDepth, out calibTransDepth); if (retStat < pxcmStatus.PXCM_STATUS_NO_ERROR) { log.WarnFormat("Could not get calibration for depth channel."); } projection.Dispose(); log.InfoFormat("Color calibration: f=({0}; {1}) c=({2}; {3}) k=({4}; {5}; {6}) p=({7}; {8})", calibDataColor.focalLength.x, calibDataColor.focalLength.y, calibDataColor.principalPoint.x, calibDataColor.principalPoint.y, calibDataColor.radialDistortion[0], calibDataColor.radialDistortion[1], calibDataColor.radialDistortion[2], calibDataColor.tangentialDistortion[0], calibDataColor.tangentialDistortion[1] ); log.InfoFormat("Color translation: t=({0}; {1}; {2})", calibTransColor.translation[0], calibTransColor.translation[1], calibTransColor.translation[2] ); log.InfoFormat("Depth calibration: f=({0}; {1}) c=({2}; {3}) k=({4}; {5}; {6}) p=({7}; {8})", calibDataDepth.focalLength.x, calibDataDepth.focalLength.y, calibDataDepth.principalPoint.x, calibDataDepth.principalPoint.y, calibDataDepth.radialDistortion[0], calibDataDepth.radialDistortion[1], calibDataDepth.radialDistortion[2], calibDataDepth.tangentialDistortion[0], calibDataDepth.tangentialDistortion[1] ); log.InfoFormat("Depth translation: t=({0}; {1}; {2})", calibTransDepth.translation[0], calibTransDepth.translation[1], calibTransDepth.translation[2] ); ActivateChannel(ChannelNames.ZImage); ActivateChannel(ChannelNames.Color); SelectChannel(ChannelNames.ZImage); log.LeaveMethod(); }