public AzureKinectCalibration(Camera depthCamera, Camera colorCamera,
Extrinsics depthToColorExtrinsics)
{
DepthCamera = depthCamera;
ColorCamera = colorCamera;
DepthToColorExtrinsics = depthToColorExtrinsics;
}
private Vector3D MapWorldToWorld(Vector3D original, Extrinsics extrinsics)
{
return(new Vector3D
(
extrinsics.rotation[0] * original[0] + extrinsics.rotation[3] * original[1] + extrinsics.rotation[6] * original[2] + extrinsics.translation[0],
extrinsics.rotation[1] * original[0] + extrinsics.rotation[4] * original[1] + extrinsics.rotation[7] * original[2] + extrinsics.translation[1],
extrinsics.rotation[2] * original[0] + extrinsics.rotation[5] * original[1] + extrinsics.rotation[8] * original[2] + extrinsics.translation[2]
));
}
// gets the given camera extrinsics
private void GetCameraExtrinsics(Extrinsics camExtr, ref KinectInterop.CameraExtrinsics extr)
{
extr = new KinectInterop.CameraExtrinsics();
extr.rotation = new float[camExtr.Rotation.Length];
camExtr.Rotation.CopyTo(extr.rotation, 0);
extr.translation = new float[camExtr.Translation.Length];
camExtr.Translation.CopyTo(extr.translation, 0);
}
/// <summary>
/// Creates a new coordinate mapper with the specified intrinsics and extrinsics parameters.
/// </summary>
/// <param name="colorIntrinsics">The color intrinsics.</param>
/// <param name="colorExtrinsics">The color extrinsics.</param>
/// <param name="depthIntrinsics">The depth intrinsics.</param>
/// <param name="depthExtrinsics">The depth extrinsics.</param>
/// <returns>The coordinate mapper with the given intrinsics and extrinsics.</returns>
public static CoordinateMapper Create(Intrinsics colorIntrinsics, Extrinsics colorExtrinsics, Intrinsics depthIntrinsics, Extrinsics depthExtrinsics)
{
return(new CoordinateMapper
(
colorIntrinsics,
colorExtrinsics,
depthIntrinsics,
depthExtrinsics
));
}
/// <summary>
/// 開啟RealSense相機並進行取像
/// </summary>
internal void Open(out Image <Bgr, byte> ColorImg,
out Image <Rgb, byte> DepthImg,
out Image <Rgb, byte> FilteredImg,
out VideoFrame color,
out DepthFrame depth,
out Frame filtered)
{
DepthImg = null; ColorImg = null; FilteredImg = null; color = null; depth = null; filtered = null;
if (CamState != CameraState.Opened)
{
PipelineProfile = Camera.Start(cfg); // 以cfg設定並開始串流
vsp = PipelineProfile.GetStream <VideoStreamProfile>(Intel.RealSense.Stream.Depth); // 取得內部參數
intrinsics = vsp.GetIntrinsics();
sp = PipelineProfile.GetStream(Intel.RealSense.Stream.Color); // 取得外部參數
extrinsics = vsp.GetExtrinsicsTo(sp);
CamState = CameraState.Opened; // 更新相機狀態
}
else
{
try
{
FrameSet frames = Camera.WaitForFrames();
depth = frames.DepthFrame.DisposeWith(frames);
color = frames.ColorFrame.DisposeWith(frames);
filtered = depth;
if (depth != null)
{
//Thres_Filter.Options[Option.MinDistance].Value = float.Parse(form1.textBox2.Text);
//Thres_Filter.Options[Option.MaxDistance].Value = float.Parse(form1.textBox1.Text);
//filtered = Thres_Filter.Process(filtered);
//Spa_Filter.Options[Option.FilterMagnitude].Value = 1;
//Spa_Filter.Options[Option.FilterSmoothAlpha].Value = 0.6f;
//Spa_Filter.Options[Option.FilterSmoothDelta].Value = 8;
//filtered = Spa_Filter.Process(filtered);
Temp_Filter.Options[Option.FilterSmoothAlpha].Value = 0.5f;
Temp_Filter.Options[Option.FilterSmoothDelta].Value = 20;
Temp_Filter.Options[Option.HolesFill].Value = 2;
filtered = Temp_Filter.Process(filtered);
depColor = colorizer.Colorize(depth);
filteredColor = colorizer.Colorize(filtered);
ColorImg = new Image <Bgr, byte>(color.Width, color.Height, color.Stride, color.Data);
DepthImg = new Image <Rgb, byte>(depColor.Width, depColor.Height, depColor.Stride, depColor.Data);
FilteredImg = new Image <Rgb, byte>(filteredColor.Width, filteredColor.Height, filteredColor.Stride, filteredColor.Data);
}
}
catch (Exception ex)
{
throw ex;
}
}
}
public CameraCalibrationResult(int _width, int _height, Extrinsics __extrinsics, Intrinsics __intrinsics, Distortion __distortion, double _error)
{
width = _width;
height = _height;
this._extrinsics = __extrinsics;
this._intrinsics = __intrinsics;
this._distortion = __distortion;
error = _error;
}
/// <summary>
/// Creates a new coordinate mapper for the specified pipeline.
/// </summary>
/// <param name="pipeline">The specified pipeline.</param>
/// <param name="colorWidth">The desired color frame width.</param>
/// <param name="colorHeight">The desired color frame height.</param>
/// <param name="depthWidth">The desired depth frame width.</param>
/// <param name="depthHeight">The desired depth frame height.</param>
/// <returns>The color/depth coordinate mapper of the current pipline, if all of the supported streams were found. Null otherwise.</returns>
public static CoordinateMapper Create(PipelineProfile pipeline, int colorWidth, int colorHeight, int depthWidth, int depthHeight)
{
if (pipeline == null)
{
return(null);
}
if (pipeline.Streams == null)
{
return(null);
}
if (pipeline.Streams.Count == 0)
{
return(null);
}
StreamProfile colorProfile = null;
StreamProfile depthProfile = null;
foreach (StreamProfile profile in pipeline.Streams)
{
VideoStreamProfile videoProfile = profile as VideoStreamProfile;
if (profile.Stream == Stream.Color && videoProfile.Width == colorWidth && videoProfile.Height == colorHeight)
{
colorProfile = profile;
}
else if (profile.Stream == Stream.Depth && videoProfile.Width == depthWidth && videoProfile.Height == depthHeight)
{
depthProfile = profile;
}
}
if (colorProfile == null)
{
return(null);
}
if (depthProfile == null)
{
return(null);
}
Intrinsics colorIntrinsics = (colorProfile as VideoStreamProfile).GetIntrinsics();
Extrinsics colorExtrinsics = colorProfile.GetExtrinsicsTo(depthProfile);
Intrinsics depthIntrinsics = (depthProfile as VideoStreamProfile).GetIntrinsics();
Extrinsics depthExtrinsics = depthProfile.GetExtrinsicsTo(colorProfile);
return(Create(colorIntrinsics, colorExtrinsics, depthIntrinsics, depthExtrinsics));
}
/// <summary>
/// Creates a new Coordinate Mapper with the specified intrinsics and extrinsics parameters.
/// </summary>
/// <param name="colorIntrinsics">The color intrinsics.</param>
/// <param name="colorExtrinsics">The color extrinsics.</param>
/// <param name="depthIntrinsics">The depth intrinsics.</param>
/// <param name="depthExtrinsics">The depth extrinsics.</param>
private CoordinateMapper(Intrinsics colorIntrinsics, Extrinsics colorExtrinsics, Intrinsics depthIntrinsics, Extrinsics depthExtrinsics)
{
ColorIntrinsics = colorIntrinsics;
ColorExtrinsics = colorExtrinsics;
DepthIntrinsics = depthIntrinsics;
DepthExtrinsics = depthExtrinsics;
if (ColorIntrinsics.width == DepthIntrinsics.width ||
ColorIntrinsics.height == DepthIntrinsics.height ||
ColorIntrinsics.ppx == DepthIntrinsics.ppx ||
ColorIntrinsics.ppy == DepthIntrinsics.ppy ||
ColorIntrinsics.fx == DepthIntrinsics.fx ||
ColorIntrinsics.fy == DepthIntrinsics.fy ||
ColorIntrinsics.model == DepthIntrinsics.model)
{
colorAndDepthMatch = true;
}
}
private K4A.CalibrationCamera CreateCalibrationCamera(CameraCalibration cameraCalibration, int width, int height)
{
K4A.CalibrationCamera calibrationCamera = new K4A.CalibrationCamera();
float[] intrinsicsParameters = cameraCalibration.Intrinsics.Parameters;
Extrinsics extrinsics = cameraCalibration.Extrinsics;
calibrationCamera.resolutionWidth = width;
calibrationCamera.resolutionHeight = height;
calibrationCamera.metricRadius = cameraCalibration.MetricRadius;
calibrationCamera.intrinsics.cx = intrinsicsParameters[0];
calibrationCamera.intrinsics.cy = intrinsicsParameters[1];
calibrationCamera.intrinsics.fx = intrinsicsParameters[2];
calibrationCamera.intrinsics.fy = intrinsicsParameters[3];
calibrationCamera.intrinsics.k1 = intrinsicsParameters[4];
calibrationCamera.intrinsics.k2 = intrinsicsParameters[5];
calibrationCamera.intrinsics.k3 = intrinsicsParameters[6];
calibrationCamera.intrinsics.k4 = intrinsicsParameters[7];
calibrationCamera.intrinsics.k5 = intrinsicsParameters[8];
calibrationCamera.intrinsics.k6 = intrinsicsParameters[9];
calibrationCamera.intrinsics.codx = intrinsicsParameters[10];
calibrationCamera.intrinsics.cody = intrinsicsParameters[11];
calibrationCamera.intrinsics.p2 = intrinsicsParameters[12]; // p2: tangential distortion coefficient y
calibrationCamera.intrinsics.p1 = intrinsicsParameters[13]; // p1: tangential distortion coefficient x
calibrationCamera.intrinsics.metricRadius = intrinsicsParameters[14];
calibrationCamera.extrinsics.rotation[0][0] = extrinsics.Rotation[0];
calibrationCamera.extrinsics.rotation[0][1] = extrinsics.Rotation[1];
calibrationCamera.extrinsics.rotation[0][2] = extrinsics.Rotation[2];
calibrationCamera.extrinsics.rotation[1][0] = extrinsics.Rotation[3];
calibrationCamera.extrinsics.rotation[1][1] = extrinsics.Rotation[4];
calibrationCamera.extrinsics.rotation[1][2] = extrinsics.Rotation[5];
calibrationCamera.extrinsics.rotation[2][0] = extrinsics.Rotation[6];
calibrationCamera.extrinsics.rotation[2][1] = extrinsics.Rotation[7];
calibrationCamera.extrinsics.rotation[2][2] = extrinsics.Rotation[8];
calibrationCamera.extrinsics.translation[0] = extrinsics.Translation[0];
calibrationCamera.extrinsics.translation[1] = extrinsics.Translation[1];
calibrationCamera.extrinsics.translation[2] = extrinsics.Translation[2];
return(calibrationCamera);
}
public double Calibrate(Size resolution)
{
MCvPoint3D32f[][] objectPoints = new MCvPoint3D32f[1][];
PointF[][] imagePoints = new PointF[1][];
int count = this.FObjectPoints.Count;
objectPoints[0] = new MCvPoint3D32f[count];
imagePoints[0] = new PointF[count];
for (int i = 0; i < count; i++)
{
objectPoints[0][i] = FObjectPoints[i];
imagePoints[0][i] = FImagePoints[i];
}
IntrinsicCameraParameters intrinsicParam = new IntrinsicCameraParameters();
ExtrinsicCameraParameters[] extrinsicParams;
Matrix <double> mat = intrinsicParam.IntrinsicMatrix;
mat[0, 0] = resolution.Width;
mat[1, 1] = resolution.Height;
mat[0, 2] = resolution.Width / 2.0d;
mat[1, 2] = resolution.Height / 2.0d;
mat[2, 2] = 1;
CALIB_TYPE flags;
flags = CALIB_TYPE.CV_CALIB_FIX_K1 | CALIB_TYPE.CV_CALIB_FIX_K2 | CALIB_TYPE.CV_CALIB_FIX_K3 | CALIB_TYPE.CV_CALIB_FIX_K4 | CALIB_TYPE.CV_CALIB_FIX_K5 | CALIB_TYPE.CV_CALIB_FIX_K6 | CALIB_TYPE.CV_CALIB_USE_INTRINSIC_GUESS | CALIB_TYPE.CV_CALIB_ZERO_TANGENT_DIST;
double error = CameraCalibration.CalibrateCamera(objectPoints, imagePoints, resolution, intrinsicParam, flags, out extrinsicParams);
this.FIntrinsics = new Intrinsics(intrinsicParam, resolution);
this.FExtrinsics = new Extrinsics(extrinsicParams[0]);
return(error);
}
K4A.CalibrationCamera CreateCalibrationCamera(CameraCalibration cameraCalibration, int width, int height)
{
K4A.CalibrationCamera calibrationCamera = new K4A.CalibrationCamera();
float[] intrinsicsParameters = cameraCalibration.Intrinsics.Parameters;
Extrinsics extrinsics = cameraCalibration.Extrinsics;
calibrationCamera.resolutionWidth = width;
calibrationCamera.resolutionHeight = height;
calibrationCamera.metricRadius = cameraCalibration.MetricRadius;
calibrationCamera.intrinsics.cx = intrinsicsParameters[0];
calibrationCamera.intrinsics.cy = intrinsicsParameters[1];
calibrationCamera.intrinsics.fx = intrinsicsParameters[2];
calibrationCamera.intrinsics.fy = intrinsicsParameters[3];
calibrationCamera.intrinsics.k1 = intrinsicsParameters[4];
calibrationCamera.intrinsics.k2 = intrinsicsParameters[5];
calibrationCamera.intrinsics.k3 = intrinsicsParameters[6];
calibrationCamera.intrinsics.k4 = intrinsicsParameters[7];
calibrationCamera.intrinsics.k5 = intrinsicsParameters[8];
calibrationCamera.intrinsics.k6 = intrinsicsParameters[9];
calibrationCamera.intrinsics.codx = intrinsicsParameters[10];
calibrationCamera.intrinsics.cody = intrinsicsParameters[11];
calibrationCamera.intrinsics.p2 = intrinsicsParameters[12]; // p2: tangential distortion coefficient y
calibrationCamera.intrinsics.p1 = intrinsicsParameters[13]; // p1: tangential distortion coefficient x
calibrationCamera.intrinsics.metricRadius = intrinsicsParameters[14];
calibrationCamera.extrinsics.rotation[0][0] = extrinsics.Rotation[0];
calibrationCamera.extrinsics.rotation[0][1] = extrinsics.Rotation[1];
calibrationCamera.extrinsics.rotation[0][2] = extrinsics.Rotation[2];
calibrationCamera.extrinsics.rotation[1][0] = extrinsics.Rotation[3];
calibrationCamera.extrinsics.rotation[1][1] = extrinsics.Rotation[4];
calibrationCamera.extrinsics.rotation[1][2] = extrinsics.Rotation[5];
calibrationCamera.extrinsics.rotation[2][0] = extrinsics.Rotation[6];
calibrationCamera.extrinsics.rotation[2][1] = extrinsics.Rotation[7];
calibrationCamera.extrinsics.rotation[2][2] = extrinsics.Rotation[8];
calibrationCamera.extrinsics.translation[0] = extrinsics.Translation[0];
calibrationCamera.extrinsics.translation[1] = extrinsics.Translation[1];
calibrationCamera.extrinsics.translation[2] = extrinsics.Translation[2];
// Debug.Log("***** Camera parameters *****");
// Debug.Log(" Intrinsics.cx: " + calibrationCamera.intrinsics.cx);
// Debug.Log(" Intrinsics.cy: " + calibrationCamera.intrinsics.cy);
// Debug.Log(" Intrinsics.fx: " + calibrationCamera.intrinsics.fx);
// Debug.Log(" Intrinsics.fy: " + calibrationCamera.intrinsics.fy);
// Debug.Log(" Intrinsics.k1: " + calibrationCamera.intrinsics.k1);
// Debug.Log(" Intrinsics.k2: " + calibrationCamera.intrinsics.k2);
// Debug.Log(" Intrinsics.k3: " + calibrationCamera.intrinsics.k3);
// Debug.Log(" Intrinsics.k4: " + calibrationCamera.intrinsics.k4);
// Debug.Log(" Intrinsics.k5: " + calibrationCamera.intrinsics.k5);
// Debug.Log(" Intrinsics.k6: " + calibrationCamera.intrinsics.k6);
// Debug.Log(" Intrinsics.codx: " + calibrationCamera.intrinsics.codx);
// Debug.Log(" Intrinsics.cody: " + calibrationCamera.intrinsics.cody);
// Debug.Log(" Intrinsics.p2: " + calibrationCamera.intrinsics.p2);
// Debug.Log(" Intrinsics.p1: " + calibrationCamera.intrinsics.p1);
// Debug.Log(" Intrinsics.metricRadius: " + calibrationCamera.intrinsics.metricRadius);
// Debug.Log(" MetricRadius: " + calibrationCamera.metricRadius);
// Debug.Log("*****************************");
// for (int i = 0; i < extrinsics.Rotation.Length; i++)
// {
// Debug.Log(" Extrinsics.R[" + i + "]: " + extrinsics.Rotation[i]);
// }
// for (int i = 0; i < extrinsics.Translation.Length; i++)
// {
// Debug.Log(" Extrinsics.T[" + i + "]: " + extrinsics.Translation[i]);
// }
// extrinsics = deviceDepthCameraCalibration.Extrinsics;
// for (int i = 0; i < extrinsics.Rotation.Length; i++)
// {
// Debug.Log(" Extrinsics.R[" + i + "]: " + extrinsics.Rotation[i]);
// }
// for (int i = 0; i < extrinsics.Translation.Length; i++)
// {
// Debug.Log(" Extrinsics.T[" + i + "]: " + extrinsics.Translation[i]);
// }
return(calibrationCamera);
}
//called when data for any output pin is requested
public void Evaluate(int SpreadMax)
{
if (FPinInDo[0])
{
int nPointsPerImage = FPinInObject.SliceCount;
bool useVVVVCoords = FPinInCoordSystem[0] == TCoordinateSystem.VVVV;
if (nPointsPerImage == 0)
{
FStatus[0] = "Insufficient points";
return;
}
int nImages = FPinInImage.SliceCount / nPointsPerImage;
MCvPoint3D32f[][] objectPoints = new MCvPoint3D32f[nImages][];
PointF[][] imagePoints = new PointF[nImages][];
Size imageSize = new Size( (int) FPinInSensorSize[0].x, (int) FPinInSensorSize[0].y);
CALIB_TYPE flags = new CALIB_TYPE();
IntrinsicCameraParameters intrinsicParam = new IntrinsicCameraParameters();
ExtrinsicCameraParameters[] extrinsicsPerView;
GetFlags(out flags);
if (flags.HasFlag(CALIB_TYPE.CV_CALIB_USE_INTRINSIC_GUESS))
{
if (FPinInIntrinsics[0] == null)
{
Matrix<double> mat = intrinsicParam.IntrinsicMatrix;
mat[0, 0] = FPinInSensorSize[0].x / 2.0d;
mat[1, 1] = FPinInSensorSize[0].y / 2.0d;
mat[0, 2] = FPinInSensorSize[0].x / 2.0d;
mat[1, 2] = FPinInSensorSize[0].y / 2.0d;
mat[2, 2] = 1;
}
else
{
intrinsicParam.DistortionCoeffs = FPinInIntrinsics[0].intrinsics.DistortionCoeffs.Clone();
intrinsicParam.IntrinsicMatrix = FPinInIntrinsics[0].intrinsics.IntrinsicMatrix.Clone();
}
}
imagePoints = MatrixUtils.ImagePoints(FPinInImage, nPointsPerImage);
for (int i=0; i<nImages; i++)
{
objectPoints[i] = MatrixUtils.ObjectPoints(FPinInObject, useVVVVCoords);
}
try
{
FPinOutError[0] = CameraCalibration.CalibrateCamera(objectPoints, imagePoints, imageSize, intrinsicParam, flags, out extrinsicsPerView);
Intrinsics intrinsics = new Intrinsics(intrinsicParam, imageSize);
FPinOutIntrinsics[0] = intrinsics;
if (useVVVVCoords)
FPinOutProjection[0] = intrinsics.Matrix;
else
FPinOutProjection[0] = intrinsics.Matrix;
FPinOutExtrinsics.SliceCount = nImages;
FPinOutView.SliceCount = nImages;
for (int i = 0; i < nImages; i++)
{
Extrinsics extrinsics = new Extrinsics(extrinsicsPerView[i]);
FPinOutExtrinsics[i] = extrinsics;
if (useVVVVCoords)
FPinOutView[i] = MatrixUtils.ConvertToVVVV(extrinsics.Matrix);
else
FPinOutView[i] = extrinsics.Matrix;
}
FPinOutSuccess[0] = true;
FStatus[0] = "OK";
}
catch (Exception e) {
FPinOutSuccess[0] = false;
FStatus[0] = e.Message;
}
}
}
public ProjectorCalibrationResult(int _width, int _height, Extrinsics _extrinsics, Intrinsics _intrinsics, Distortion _distortion, double _error) :
base(_width, _height, _extrinsics, _intrinsics, _distortion, _error)
{
}
internal static extern void rs2_get_extrinsics(
StreamProfileHandle from,
StreamProfileHandle to,
out Extrinsics extrin,
out RsError error);
//called when data for any output pin is requested
public void Evaluate(int SpreadMax)
{
if (FPinInDo[0])
{
int nPointsPerImage = FPinInObject.SliceCount;
bool useVVVVCoords = FPinInCoordSystem[0] == TCoordinateSystem.VVVV;
if (nPointsPerImage == 0)
{
FStatus[0] = "Insufficient points";
return;
}
int nImages = FPinInImage.SliceCount / nPointsPerImage;
MCvPoint3D32f[][] objectPoints = new MCvPoint3D32f[nImages][];
PointF[][] imagePoints = new PointF[nImages][];
Size imageSize = new Size((int)FPinInSensorSize[0].x, (int)FPinInSensorSize[0].y);
CALIB_TYPE flags = new CALIB_TYPE();
IntrinsicCameraParameters intrinsicParam = new IntrinsicCameraParameters();
ExtrinsicCameraParameters[] extrinsicsPerView;
GetFlags(out flags);
if (flags.HasFlag(CALIB_TYPE.CV_CALIB_USE_INTRINSIC_GUESS))
{
if (FPinInIntrinsics[0] == null)
{
Matrix <double> mat = intrinsicParam.IntrinsicMatrix;
mat[0, 0] = FPinInSensorSize[0].x / 2.0d;
mat[1, 1] = FPinInSensorSize[0].y / 2.0d;
mat[0, 2] = FPinInSensorSize[0].x / 2.0d;
mat[1, 2] = FPinInSensorSize[0].y / 2.0d;
mat[2, 2] = 1;
}
else
{
intrinsicParam.DistortionCoeffs = FPinInIntrinsics[0].intrinsics.DistortionCoeffs.Clone();
intrinsicParam.IntrinsicMatrix = FPinInIntrinsics[0].intrinsics.IntrinsicMatrix.Clone();
}
}
imagePoints = MatrixUtils.ImagePoints(FPinInImage, nPointsPerImage);
for (int i = 0; i < nImages; i++)
{
objectPoints[i] = MatrixUtils.ObjectPoints(FPinInObject, useVVVVCoords);
}
try
{
FPinOutError[0] = CameraCalibration.CalibrateCamera(objectPoints, imagePoints, imageSize, intrinsicParam, flags, out extrinsicsPerView);
Intrinsics intrinsics = new Intrinsics(intrinsicParam, imageSize);
FPinOutIntrinsics[0] = intrinsics;
if (useVVVVCoords)
{
FPinOutProjection[0] = intrinsics.Matrix;
}
else
{
FPinOutProjection[0] = intrinsics.Matrix;
}
FPinOutExtrinsics.SliceCount = nImages;
FPinOutView.SliceCount = nImages;
for (int i = 0; i < nImages; i++)
{
Extrinsics extrinsics = new Extrinsics(extrinsicsPerView[i]);
FPinOutExtrinsics[i] = extrinsics;
if (useVVVVCoords)
{
FPinOutView[i] = MatrixUtils.ConvertToVVVV(extrinsics.Matrix);
}
else
{
FPinOutView[i] = extrinsics.Matrix;
}
}
FPinOutSuccess[0] = true;
FStatus[0] = "OK";
}
catch (Exception e) {
FPinOutSuccess[0] = false;
FStatus[0] = e.Message;
}
}
}
public override IObservable <Mat> Process(IObservable <DeviceEvents> source)
{
return(source.SelectMany(evts =>
{
var device = evts.Device;
Extrinsics depthToColor;
Intrinsics colorIntrinsics, depthIntrinsics;
var colorStream = Observable.FromEvent <FrameCallback, Frame>(handler => evts.ColorFrame += handler, handler => evts.ColorFrame -= handler);
var depthStream = Observable.FromEvent <FrameCallback, Frame>(handler => evts.DepthFrame += handler, handler => evts.DepthFrame -= handler);
device.GetStreamIntrinsics(Stream.Color, out colorIntrinsics);
device.GetStreamIntrinsics(Stream.Depth, out depthIntrinsics);
device.GetExtrinsics(Stream.Depth, Stream.Color, out depthToColor);
var pixelScale = new Vector2(colorIntrinsics.Width, colorIntrinsics.Height);
var depthScale = device.DepthScale;
int depthBufferWidth = 0;
ushort[] depthBuffer = null;
TexVertex[] depthPoints = null;
var depthBufferStream = depthStream.Select(frame =>
{
if (depthBuffer == null)
{
depthBufferWidth = frame.Width;
depthBuffer = new ushort[frame.Width * frame.Height];
depthPoints = new TexVertex[depthBuffer.Length];
}
var depthFrameHeader = new Mat(frame.Height, frame.Width, Depth.U16, 1, frame.FrameData, frame.Stride);
using (var bufferHeader = Mat.CreateMatHeader(depthBuffer, depthFrameHeader.Rows, depthFrameHeader.Cols, depthFrameHeader.Depth, depthFrameHeader.Channels))
{
CV.Copy(depthFrameHeader, bufferHeader);
}
return depthBuffer;
});
return depthBufferStream.Select(depth =>
{
int pindex = 0;
for (int i = 0; i < depthBuffer.Length; i++)
{
Vector2 depthPixel, colorPixel;
depthPixel.X = i % depthBufferWidth;
depthPixel.Y = i / depthBufferWidth;
var depthValue = depth[i] * depthScale;
if (depthValue == 0)
{
continue;
}
Vector3 depthPoint, colorPoint;
Intrinsics.DeprojectPoint(ref depthPixel, ref depthIntrinsics, depthValue, out depthPoint);
Extrinsics.TransformPoint(ref depthPoint, ref depthToColor, out colorPoint);
Intrinsics.ProjectPoint(ref colorPoint, ref colorIntrinsics, out colorPixel);
Vector2.Divide(ref colorPixel, ref pixelScale, out depthPoints[pindex].TexCoord);
depthPoints[pindex].Position = depthPoint;
pindex++;
}
return Mat.FromArray(depthPoints, pindex, 5, Depth.F32, 1);
});
}));
}
