diff --git a/src/ros/FindObjectROS.cpp b/src/ros/FindObjectROS.cpp index 01bd6cd8..8650ab5f 100644 --- a/src/ros/FindObjectROS.cpp +++ b/src/ros/FindObjectROS.cpp @@ -332,7 +332,7 @@ cv::Vec3f FindObjectROS::getDepth(const cv::Mat & depthImage, else { depth = depthImage.at(y,x); - isValid = std::isfinite(depth); + isValid = std::isfinite(depth) && depth > 0.0f; } // Check for invalid measurements