fix bug arrived due to depth conversion to SI units

Author: ravijo

include/ros_openpose/cameraReader.hpp

@@ -115,7 +115,8 @@ namespace ros_openpose
       // K.at(5) = intrinsic.ppy
 
       // our depth frame type is 16UC1 which has unsigned short as an underlying type
-      auto depth = mDepthImageUsed.at<unsigned short>(static_cast<int>(pixelY), static_cast<int>(pixelX));
+      // auto depth = mDepthImageUsed.at<unsigned short>(static_cast<int>(pixelY), static_cast<int>(pixelX));
+      auto depth = mDepthImageUsed.at<float>(static_cast<int>(pixelY), static_cast<int>(pixelX));
 
       // no need to proceed further if the depth is zero or less than zero
       // the depth represents the distance of an object placed infront of the camera

src/cameraReader.cpp

@@ -82,9 +82,10 @@ namespace ros_openpose
       // however I found that it is only changing encoding.
       // therefore in case of '16UC1' encoding (depth values are in millimeter),
       // a manually conversion from millimeter to meter is required.
-      mDepthImage = (depthMsg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) ?
-                        depthPtr->image * 0.001f : // convert to meter (SI units)
-                        depthPtr->image; // no conversion needed
+      if (depthMsg->encoding == sensor_msgs::image_encodings::TYPE_16UC1)
+        depthPtr->image.convertTo(mDepthImage, CV_32FC1, 0.001f); // convert to meter (SI units)
+      else
+        mDepthImage = depthPtr->image; // no conversion needed
     }
     catch (cv_bridge::Exception& e)
     {