From 5f54262f39becb577ca9180e5d69819e735f234d Mon Sep 17 00:00:00 2001
From: konstantin <darkkon65@gmail.com>
Date: Wed, 4 Dec 2019 21:08:31 +0300
Subject: [PATCH] fix convertDepthToColorCoordinates - extrinsics from depth to
 color taken into account

---
 wrappers/openni2/src/Rs2Stream.cpp | 16 ++++++++++++++--
 wrappers/openni2/src/Rs2Stream.h   |  2 ++
 2 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/wrappers/openni2/src/Rs2Stream.cpp b/wrappers/openni2/src/Rs2Stream.cpp
index e62bb3a23b..f330d7041b 100755
--- a/wrappers/openni2/src/Rs2Stream.cpp
+++ b/wrappers/openni2/src/Rs2Stream.cpp
@@ -40,6 +40,7 @@ OniStatus Rs2Stream::initialize(class Rs2Device* device, rs2_sensor* sensor, int
 
 	memset(&m_videoMode, 0, sizeof(m_videoMode));
 	memset(&m_intrinsics, 0, sizeof(m_intrinsics));
+	memset(&m_extrinsicsDepthToColor, 0, sizeof(m_extrinsicsDepthToColor));
 
 	m_depthScale = 0;
 	m_fovX = 0;
@@ -155,14 +156,24 @@ OniStatus Rs2Stream::convertDepthToColorCoordinates(StreamBase* colorStream, int
 	{
 		float pixel[2];
 		float point[3] = {0, 0, 0};
+		float transformed_point[3] = { 0 };
+        if (m_needUpdateExtrinsicsDepthToColor) {
+            Rs2StreamProfileInfo* spiDepth = getCurrentProfile();
+            Rs2StreamProfileInfo* spiColor = ((Rs2Stream*)colorStream)->getCurrentProfile();
+            rs2_get_extrinsics(spiDepth->profile, spiColor->profile, &m_extrinsicsDepthToColor, nullptr);
+            m_needUpdateExtrinsicsDepthToColor = false;
+        }
 
 		// depth pixel -> point
 		pixel[0] = (float)depthX;
 		pixel[1] = (float)depthY;
-		rs2_deproject_pixel_to_point(point, &m_intrinsics, pixel, depthZ);
+		rs2_deproject_pixel_to_point(point, &m_intrinsics, pixel, m_depthScale * depthZ);
+
+		// depth point -> color point
+		rs2_transform_point_to_point(transformed_point, &m_extrinsicsDepthToColor, point);
 
 		// point -> color pixel
-		rs2_project_point_to_pixel(pixel, &((Rs2Stream*)colorStream)->m_intrinsics, point);
+		rs2_project_point_to_pixel(pixel, &((Rs2Stream*)colorStream)->m_intrinsics, transformed_point);
 		*pColorX = (int)pixel[0];
 		*pColorY = (int)pixel[1];
 	}
@@ -199,6 +210,7 @@ void Rs2Stream::updateIntrinsics()
 	{
 		m_depthScale = 0;
 	}
+	m_needUpdateExtrinsicsDepthToColor = true;
 }
 
 Rs2StreamProfileInfo* Rs2Stream::getCurrentProfile()
diff --git a/wrappers/openni2/src/Rs2Stream.h b/wrappers/openni2/src/Rs2Stream.h
index 54189e9ccf..6d121fda32 100755
--- a/wrappers/openni2/src/Rs2Stream.h
+++ b/wrappers/openni2/src/Rs2Stream.h
@@ -79,6 +79,8 @@ class Rs2Stream : public StreamBase
 	std::vector<Rs2StreamProfileInfo> m_profiles;
 	OniVideoMode m_videoMode;
 	rs2_intrinsics m_intrinsics;
+	rs2_extrinsics m_extrinsicsDepthToColor;
+	bool m_needUpdateExtrinsicsDepthToColor;
 	float m_depthScale;
 	float m_fovX;
 	float m_fovY;