private void CoordinateMapper_CoordinateMappingChanged(object sender, CoordinateMappingChangedEventArgs e)
{
if (this.DepthFrameToCameraSpaceTable.HasSubscribers)
{
this.DepthFrameToCameraSpaceTable.Post(this.kinectSensor.CoordinateMapper.GetDepthFrameToCameraSpaceTable(), this.pipeline.GetCurrentTime());
}
if (this.configuration.OutputCalibration)
{
if (!this.calibrationPosted)
{
// Extract and created old style calibration
var kinectInternalCalibration = new KinectInternalCalibration();
kinectInternalCalibration.RecoverCalibrationFromSensor(this.kinectSensor);
Matrix <double> colorCameraMatrix = Matrix <double> .Build.Dense(3, 3);
Matrix <double> depthCameraMatrix = Matrix <double> .Build.Dense(3, 3);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
colorCameraMatrix[i, j] = kinectInternalCalibration.colorCameraMatrix[i, j];
depthCameraMatrix[i, j] = kinectInternalCalibration.depthCameraMatrix[i, j];
}
}
Vector <double> colorLensDistortion = Vector <double> .Build.Dense(5);
Vector <double> depthLensDistortion = Vector <double> .Build.Dense(5);
for (int i = 0; i < 5; i++)
{
colorLensDistortion[i] = kinectInternalCalibration.colorLensDistortion[i];
depthLensDistortion[i] = kinectInternalCalibration.depthLensDistortion[i];
}
Matrix <double> depthToColorTransform = Matrix <double> .Build.Dense(4, 4);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
depthToColorTransform[i, j] = kinectInternalCalibration.depthToColorTransform[i, j];
}
}
// Kinect uses a basis under the hood that assumes Forward=Z, Left=X, Up=Y.
var kinectBasis = new CoordinateSystem(default, UnitVector3D.ZAxis, UnitVector3D.XAxis, UnitVector3D.YAxis);
private void CoordinateMapper_CoordinateMappingChanged(object sender, CoordinateMappingChangedEventArgs e)
{
if (this.DepthFrameToCameraSpaceTable.HasSubscribers)
{
this.DepthFrameToCameraSpaceTable.Post(this.kinectSensor.CoordinateMapper.GetDepthFrameToCameraSpaceTable(), this.pipeline.GetCurrentTime());
}
if (this.configuration.OutputCalibration)
{
if (!this.calibrationPosted)
{
// Extract and created old style calibration
var kinectInternalCalibration = new KinectInternalCalibration();
kinectInternalCalibration.RecoverCalibrationFromSensor(this.kinectSensor);
// Kinect uses a basis under the hood that assumes Forward=Z, Left=X, Up=Y.
var kinectBasis = new CoordinateSystem(default, UnitVector3D.ZAxis, UnitVector3D.XAxis, UnitVector3D.YAxis);
private void CoordinateMapper_CoordinateMappingChanged(object sender, CoordinateMappingChangedEventArgs e)
{
if (this.DepthFrameToCameraSpaceTable.HasSubscribers)
{
this.DepthFrameToCameraSpaceTable.Post(this.kinectSensor.CoordinateMapper.GetDepthFrameToCameraSpaceTable(), this.pipeline.GetCurrentTime());
}
if (this.configuration.OutputCalibration)
{
if (!this.calibrationPosted)
{
// Extract and created old style calibration
var kinectInternalCalibration = new KinectInternalCalibration();
kinectInternalCalibration.RecoverCalibrationFromSensor(this.kinectSensor);
Matrix <double> colorCameraMatrix = Matrix <double> .Build.Dense(3, 3);
Matrix <double> depthCameraMatrix = Matrix <double> .Build.Dense(3, 3);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
colorCameraMatrix[i, j] = kinectInternalCalibration.colorCameraMatrix[i, j];
depthCameraMatrix[i, j] = kinectInternalCalibration.depthCameraMatrix[i, j];
}
}
Vector <double> colorLensDistortion = Vector <double> .Build.Dense(5);
Vector <double> depthLensDistortion = Vector <double> .Build.Dense(5);
for (int i = 0; i < 5; i++)
{
colorLensDistortion[i] = kinectInternalCalibration.colorLensDistortion[i];
depthLensDistortion[i] = kinectInternalCalibration.depthLensDistortion[i];
}
Matrix <double> depthToColorTransform = Matrix <double> .Build.Dense(4, 4);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
depthToColorTransform[i, j] = kinectInternalCalibration.depthToColorTransform[i, j];
}
}
// Extract and create new style calibration
this.kinectCalibration = new KinectCalibration(
this.kinectSensor.ColorFrameSource.FrameDescription.Width,
this.kinectSensor.ColorFrameSource.FrameDescription.Height,
colorCameraMatrix,
kinectInternalCalibration.colorLensDistortion[0],
kinectInternalCalibration.colorLensDistortion[1],
0.0,
0.0,
depthToColorTransform,
this.kinectSensor.DepthFrameSource.FrameDescription.Width,
this.kinectSensor.DepthFrameSource.FrameDescription.Height,
depthCameraMatrix,
kinectInternalCalibration.depthLensDistortion[0],
kinectInternalCalibration.depthLensDistortion[1],
0.0,
0.0);
/* Warning about comments being preceded by blank line */
#pragma warning disable SA1515
// KinectCalibrationOld's original ToColorSpace() method flips the Y axis (height-Y). To account
// for this we adjust our Transform.
// Matrix<double> flipY = Matrix<double>.Build.DenseIdentity(3, 3);
// flipY[1, 1] = -1.0;
// flipY[1, 2] = this.kinectSensor.ColorFrameSource.FrameDescription.Height;
// this.kinectCalibration.ColorIntrinsics.Transform = flipY * this.kinectCalibration.ColorIntrinsics.Transform;
#pragma warning restore SA1515
this.Calibration.Post(this.kinectCalibration, this.pipeline.GetCurrentTime());
this.calibrationPosted = true;
}
}
}