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();
}
