From b5c608512f6fc726d449539ec5c9807b7cfab849 Mon Sep 17 00:00:00 2001 From: Dmitry Kargin Date: Tue, 7 Jun 2016 16:40:07 +0300 Subject: [PATCH 1/3] Improving performance for ARM7-based platforms - most calculations use float instead of double - column-order calculation, less 'atan2' calls - added tests for perfomance comparison. 20x faster so far (tests were not so reialble) - added test node to compare both scans from old and new processor --- CMakeLists.txt | 3 + .../DepthImageToLaserScan.h | 129 +++++++++++++++++- .../DepthImageToLaserScanROS.h | 2 +- .../DepthImageToLaserScanROS_test.h | 111 +++++++++++++++ src/DepthImageToLaserScan.cpp | 92 ++++++++++++- src/DepthImageToLaserScanROS_test.cpp | 107 +++++++++++++++ test/DepthImageToLaserScanTest.cpp | 59 +++++++- 7 files changed, 493 insertions(+), 10 deletions(-) create mode 100644 include/depthimage_to_laserscan/DepthImageToLaserScanROS_test.h create mode 100644 src/DepthImageToLaserScanROS_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a76b054..d971fd3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,6 +30,9 @@ target_link_libraries(DepthImageToLaserScanNodelet DepthImageToLaserScanROS ${ca add_executable(depthimage_to_laserscan src/depthimage_to_laserscan.cpp) target_link_libraries(depthimage_to_laserscan DepthImageToLaserScanROS ${catkin_LIBRARIES}) +add_executable(depthimage_to_laserscan_test src/DepthImageToLaserScanROS_test.cpp) +target_link_libraries(depthimage_to_laserscan_test DepthImageToLaserScan ${catkin_LIBRARIES}) + if(CATKIN_ENABLE_TESTING) # Test the library catkin_add_gtest(libtest test/DepthImageToLaserScanTest.cpp) diff --git a/include/depthimage_to_laserscan/DepthImageToLaserScan.h b/include/depthimage_to_laserscan/DepthImageToLaserScan.h index e2921f7..a76791f 100644 --- a/include/depthimage_to_laserscan/DepthImageToLaserScan.h +++ b/include/depthimage_to_laserscan/DepthImageToLaserScan.h @@ -64,8 +64,45 @@ namespace depthimage_to_laserscan * */ sensor_msgs::LaserScanPtr convert_msg(const sensor_msgs::ImageConstPtr& depth_msg, - const sensor_msgs::CameraInfoConstPtr& info_msg); + const sensor_msgs::CameraInfoConstPtr& info_msg) + { + sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan()); + convert_msg(depth_msg, info_msg, scan_msg); + return scan_msg; + } + + /** + * Allocation-free converter + */ + bool convert_msg(const sensor_msgs::ImageConstPtr& depth_msg, + const sensor_msgs::CameraInfoConstPtr& info_msg, sensor_msgs::LaserScanPtr & scan_msg); + + /** + * Converts the information in a depth image (sensor_msgs::Image) to a sensor_msgs::LaserScan. + * + * This function converts the information in the depth encoded image (UInt16 or Float32 encoding) into + * a sensor_msgs::LaserScan as accurately as possible. To do this, it requires the synchornized Image/CameraInfo + * pair associated with the image. + * + * This function uses float32 for most calculations. ARM/NEON platforms prefer float over double + * + * @param depth_msg UInt16 or Float32 encoded depth image. + * @param info_msg CameraInfo associated with depth_msg + * @return sensor_msgs::LaserScanPtr for the center row(s) of the depth image. + * + */ + sensor_msgs::LaserScanPtr convert_msg_f(const sensor_msgs::ImageConstPtr& depth_msg, + const sensor_msgs::CameraInfoConstPtr& info_msg) + { + sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan()); + convert_msg_f(depth_msg, info_msg, scan_msg); + return scan_msg; + } + + bool convert_msg_f(const sensor_msgs::ImageConstPtr& depth_msg, + const sensor_msgs::CameraInfoConstPtr& info_msg, sensor_msgs::LaserScanPtr & scan_msg); + /** * Sets the scan time parameter. * @@ -125,6 +162,17 @@ namespace depthimage_to_laserscan */ double magnitude_of_ray(const cv::Point3d& ray) const; + /** + * Computes euclidean length of a cv::Point3f (as a ray from origin) + * + * This function computes the length of a cv::Point3f assumed to be a vector starting at the origin (0,0,0). + * + * @param ray The ray for which the magnitude is desired. + * @return Returns the magnitude of the ray. + * + */ + float magnitude_of_ray_f(const cv::Point3f& ray) const; + /** * Computes the angle between two cv::Point3d * @@ -137,6 +185,19 @@ namespace depthimage_to_laserscan * */ double angle_between_rays(const cv::Point3d& ray1, const cv::Point3d& ray2) const; + + /** + * Computes the angle between two cv::Point3f + * + * Computes the angle of two cv::Point3f assumed to be vectors starting at the origin (0,0,0). + * Uses the following equation: angle = arccos(a*b/(|a||b|)) where a = ray1 and b = ray2. + * + * @param ray1 The first ray + * @param ray2 The second ray + * @return The angle between the two rays (in radians) + * + */ + float angle_between_rays_f(const cv::Point3f& ray1, const cv::Point3f& ray2) const; /** * Determines whether or not new_value should replace old_value in the LaserScan. @@ -184,7 +245,8 @@ namespace depthimage_to_laserscan int offset = (int)(cam_model.cy()-scan_height/2); depth_row += offset*row_step; // Offset to center of image - for(int v = offset; v < offset+scan_height_; v++, depth_row += row_step){ + for(int v = offset; v < offset+scan_height_; v++, depth_row += row_step) + { for (int u = 0; u < (int)depth_msg->width; u++) // Loop over each pixel in row { T depth = depth_row[u]; @@ -210,6 +272,67 @@ namespace depthimage_to_laserscan } } + // Optimized version + // 1. Using float instead of double + // 2. Column-order processing + // 3. No shared_ptr interaction for each pixel + template + void convert_f(const sensor_msgs::ImageConstPtr& depth_msg, const image_geometry::PinholeCameraModel& cam_model, + const sensor_msgs::LaserScanPtr& scan_msg, const int& scan_height) const{ + // Use correct principal point from calibration + float center_x = cam_model.cx(); + float center_y = cam_model.cy(); + + // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) + float unit_scaling = depthimage_to_laserscan::DepthTraits::toMeters( T(1) ); + float constant_x = unit_scaling / cam_model.fx(); + float constant_y = unit_scaling / cam_model.fy(); + + //const T* depth_row = reinterpret_cast(&depth_msg->data[0]); + const T* depth_col = reinterpret_cast(&depth_msg->data[0]); + int row_step = depth_msg->step / sizeof(T); + + int offset = (int)(cam_model.cy()-scan_height/2); + //depth_row += offset*row_step; // Offset to center of image + depth_col += offset*row_step; // Offset to center of image + + sensor_msgs::LaserScan &scan = *scan_msg; + + // Row order iteration (Y->X) is not effective here, because of atan2 calculation + for (int u = 0; u < (int)depth_msg->width; u++, depth_col++) // Loop over each pixel in row + { + // This can be precomputed for each column until sensor geometry is not changing + float th = -atan2f((float)(u - center_x) * constant_x, unit_scaling); // Atan2(x, z), but depth divides out + int index = (th - scan_msg->angle_min) / scan_msg->angle_increment; + const T* ptr = depth_col; + + float min_range = scan.range_max*scan.range_max; + + for(int v = offset; v < offset+scan_height_; v++, ptr += row_step) + { + T depth = *ptr; + + float r = depth; // Assign to pass through NaNs and Infs + + if (depthimage_to_laserscan::DepthTraits::valid(depth)){ // Not NaN or Inf + // Calculate in XYZ + float x = (u - center_x) * depth * constant_x; + float z = depthimage_to_laserscan::DepthTraits::toMeters(depth); + // Calculate actual distance + r = x*x + z*z; + if( r < min_range) + min_range = r; + } + } + float r = sqrt(min_range); + // Determine if this point should be used. + if(use_point(r, scan.ranges[index], scan.range_min, scan.range_max)) + { + scan.ranges[index] = r; + } + } + } + image_geometry::PinholeCameraModel cam_model_; ///< image_geometry helper class for managing sensor_msgs/CameraInfo messages. float scan_time_; ///< Stores the time between scans. @@ -222,4 +345,4 @@ namespace depthimage_to_laserscan }; // depthimage_to_laserscan -#endif \ No newline at end of file +#endif diff --git a/include/depthimage_to_laserscan/DepthImageToLaserScanROS.h b/include/depthimage_to_laserscan/DepthImageToLaserScanROS.h index fc402ff..9cd3100 100644 --- a/include/depthimage_to_laserscan/DepthImageToLaserScanROS.h +++ b/include/depthimage_to_laserscan/DepthImageToLaserScanROS.h @@ -107,4 +107,4 @@ namespace depthimage_to_laserscan }; // depthimage_to_laserscan -#endif \ No newline at end of file +#endif diff --git a/include/depthimage_to_laserscan/DepthImageToLaserScanROS_test.h b/include/depthimage_to_laserscan/DepthImageToLaserScanROS_test.h new file mode 100644 index 0000000..65ea5ed --- /dev/null +++ b/include/depthimage_to_laserscan/DepthImageToLaserScanROS_test.h @@ -0,0 +1,111 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Chad Rockey + */ + +#ifndef DEPTH_IMAGE_TO_LASERSCAN_ROS +#define DEPTH_IMAGE_TO_LASERSCAN_ROS + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace depthimage_to_laserscan +{ + class DepthImageToLaserScanROSTest + { + public: + DepthImageToLaserScanROSTest(ros::NodeHandle& n, ros::NodeHandle& pnh); + + ~DepthImageToLaserScanROSTest(); + + private: + /** + * Callback for image_transport + * + * Synchronized callback for depth image and camera info. Publishes laserscan at the end of this callback. + * + * @param depth_msg Image provided by image_transport. + * @param info_msg CameraInfo provided by image_transport. + * + */ + void depthCb(const sensor_msgs::ImageConstPtr& depth_msg, + const sensor_msgs::CameraInfoConstPtr& info_msg); + + /** + * Callback that is called when there is a new subscriber. + * + * Will not subscribe to the image and camera_info until we have a subscriber for our LaserScan (lazy subscribing). + * + */ + void connectCb(const ros::SingleSubscriberPublisher& pub); + + /** + * Callback called when a subscriber unsubscribes. + * + * If all current subscribers of our LaserScan stop listening, stop subscribing (lazy subscribing). + * + */ + void disconnectCb(const ros::SingleSubscriberPublisher& pub); + + /** + * Dynamic reconfigure callback. + * + * Callback that is used to set each of the parameters insde the DepthImageToLaserScan object. + * + * @param config Dynamic Reconfigure object. + * @param level Dynamic Reconfigure level. + * + */ + void reconfigureCb(depthimage_to_laserscan::DepthConfig& config, uint32_t level); + + ros::NodeHandle pnh_; ///< Private nodehandle used to generate the transport hints in the connectCb. + image_transport::ImageTransport it_; ///< Subscribes to synchronized Image CameraInfo pairs. + image_transport::CameraSubscriber sub_; ///< Subscriber for image_transport + ros::Publisher pub_; ///< Publisher for output LaserScan messages + ros::Publisher pub_test_; ///< Publisher for output LaserScan messages + dynamic_reconfigure::Server srv_; ///< Dynamic reconfigure server + + depthimage_to_laserscan::DepthImageToLaserScan dtl_; ///< Instance of the DepthImageToLaserScan conversion class. + + boost::mutex connect_mutex_; ///< Prevents the connectCb and disconnectCb from being called until everything is initialized. + }; + + +}; // depthimage_to_laserscan + +#endif diff --git a/src/DepthImageToLaserScan.cpp b/src/DepthImageToLaserScan.cpp index 6ad0487..18720ac 100644 --- a/src/DepthImageToLaserScan.cpp +++ b/src/DepthImageToLaserScan.cpp @@ -42,7 +42,13 @@ DepthImageToLaserScan::~DepthImageToLaserScan(){ } double DepthImageToLaserScan::magnitude_of_ray(const cv::Point3d& ray) const{ - return sqrt(pow(ray.x, 2.0) + pow(ray.y, 2.0) + pow(ray.z, 2.0)); + //return sqrt(pow(ray.x, 2.0) + pow(ray.y, 2.0) + pow(ray.z, 2.0)); + // Optimizing compiler likes this much more + return sqrt(ray.x*ray.x + ray.y*ray.y + ray.z*ray.z); +} + +float DepthImageToLaserScan::magnitude_of_ray_f(const cv::Point3f& ray) const{ + return sqrtf(ray.x*ray.x + ray.y*ray.y + ray.z*ray.z); } double DepthImageToLaserScan::angle_between_rays(const cv::Point3d& ray1, const cv::Point3d& ray2) const{ @@ -52,6 +58,15 @@ double DepthImageToLaserScan::angle_between_rays(const cv::Point3d& ray1, const return acos(dot_product / (magnitude1 * magnitude2)); } + + +float DepthImageToLaserScan::angle_between_rays_f(const cv::Point3f& ray1, const cv::Point3f& ray2) const{ + float dot_product = ray1.x*ray2.x + ray1.y*ray2.y + ray1.z*ray2.z; + float magnitude1 = magnitude_of_ray_f(ray1); + float magnitude2 = magnitude_of_ray_f(ray2); + return acosf(dot_product / (magnitude1 * magnitude2)); +} + bool DepthImageToLaserScan::use_point(const float new_value, const float old_value, const float range_min, const float range_max) const{ // Check for NaNs and Infs, a real number within our limits is more desirable than these. bool new_finite = std::isfinite(new_value); @@ -80,8 +95,9 @@ bool DepthImageToLaserScan::use_point(const float new_value, const float old_val return shorter_check; } -sensor_msgs::LaserScanPtr DepthImageToLaserScan::convert_msg(const sensor_msgs::ImageConstPtr& depth_msg, - const sensor_msgs::CameraInfoConstPtr& info_msg){ +bool DepthImageToLaserScan::convert_msg(const sensor_msgs::ImageConstPtr& depth_msg, + const sensor_msgs::CameraInfoConstPtr& info_msg, sensor_msgs::LaserScanPtr & scan_msg) +{ // Set camera model cam_model_.fromCameraInfo(info_msg); @@ -102,7 +118,7 @@ sensor_msgs::LaserScanPtr DepthImageToLaserScan::convert_msg(const sensor_msgs:: double angle_min = -angle_between_rays(center_ray, right_ray); // Negative because the laserscan message expects an opposite rotation of that from the depth image // Fill in laserscan message - sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan()); + //sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan()); scan_msg->header = depth_msg->header; if(output_frame_id_.length() > 0){ scan_msg->header.frame_id = output_frame_id_; @@ -141,7 +157,73 @@ sensor_msgs::LaserScanPtr DepthImageToLaserScan::convert_msg(const sensor_msgs:: throw std::runtime_error(ss.str()); } - return scan_msg; + //return scan_msg; + return true; +} + +bool DepthImageToLaserScan::convert_msg_f(const sensor_msgs::ImageConstPtr& depth_msg, + const sensor_msgs::CameraInfoConstPtr& info_msg, sensor_msgs::LaserScanPtr & scan_msg){ + // Set camera model + cam_model_.fromCameraInfo(info_msg); + + // Calculate angle_min and angle_max by measuring angles between the left ray, right ray, and optical center ray + cv::Point2f raw_pixel_left(0, cam_model_.cy()); + cv::Point2f rect_pixel_left = cam_model_.rectifyPoint(raw_pixel_left); + cv::Point3f left_ray = cam_model_.projectPixelTo3dRay(rect_pixel_left); + + cv::Point2f raw_pixel_right(depth_msg->width-1, cam_model_.cy()); + cv::Point2f rect_pixel_right = cam_model_.rectifyPoint(raw_pixel_right); + cv::Point3f right_ray = cam_model_.projectPixelTo3dRay(rect_pixel_right); + + cv::Point2f raw_pixel_center(cam_model_.cx(), cam_model_.cy()); + cv::Point2f rect_pixel_center = cam_model_.rectifyPoint(raw_pixel_center); + cv::Point3f center_ray = cam_model_.projectPixelTo3dRay(rect_pixel_center); + + float angle_max = angle_between_rays_f(left_ray, center_ray); + float angle_min = -angle_between_rays_f(center_ray, right_ray); // Negative because the laserscan message expects an opposite rotation of that from the depth image + + // Fill in laserscan message + //sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan()); + scan_msg->header = depth_msg->header; + if(output_frame_id_.length() > 0){ + scan_msg->header.frame_id = output_frame_id_; + } + scan_msg->angle_min = angle_min; + scan_msg->angle_max = angle_max; + scan_msg->angle_increment = (scan_msg->angle_max - scan_msg->angle_min) / (depth_msg->width - 1); + scan_msg->time_increment = 0.0; + scan_msg->scan_time = scan_time_; + scan_msg->range_min = range_min_; + scan_msg->range_max = range_max_; + + // Check scan_height vs image_height + if(scan_height_/2 > cam_model_.cy() || scan_height_/2 > depth_msg->height - cam_model_.cy()){ + std::stringstream ss; + ss << "scan_height ( " << scan_height_ << " pixels) is too large for the image height."; + throw std::runtime_error(ss.str()); + } + + // Calculate and fill the ranges + uint32_t ranges_size = depth_msg->width; + scan_msg->ranges.assign(ranges_size, std::numeric_limits::quiet_NaN()); + + if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) + { + convert_f(depth_msg, cam_model_, scan_msg, scan_height_); + } + else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) + { + convert_f(depth_msg, cam_model_, scan_msg, scan_height_); + } + else + { + std::stringstream ss; + ss << "Depth image has unsupported encoding: " << depth_msg->encoding; + throw std::runtime_error(ss.str()); + } + + return true; + //return scan_msg; } void DepthImageToLaserScan::set_scan_time(const float scan_time){ diff --git a/src/DepthImageToLaserScanROS_test.cpp b/src/DepthImageToLaserScanROS_test.cpp new file mode 100644 index 0000000..b57ad21 --- /dev/null +++ b/src/DepthImageToLaserScanROS_test.cpp @@ -0,0 +1,107 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Author: Chad Rockey + */ + +#include + +using namespace depthimage_to_laserscan; + +DepthImageToLaserScanROSTest::DepthImageToLaserScanROSTest(ros::NodeHandle& n, ros::NodeHandle& pnh):pnh_(pnh), it_(n), srv_(pnh) { + boost::mutex::scoped_lock lock(connect_mutex_); + + // Dynamic Reconfigure + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&DepthImageToLaserScanROSTest::reconfigureCb, this, _1, _2); + srv_.setCallback(f); + + // Lazy subscription to depth image topic + pub_ = n.advertise("scan", 10, boost::bind(&DepthImageToLaserScanROSTest::connectCb, this, _1), boost::bind(&DepthImageToLaserScanROSTest::disconnectCb, this, _1)); + pub_test_ = n.advertise("scan_test", 10); +} + +DepthImageToLaserScanROSTest::~DepthImageToLaserScanROSTest(){ + sub_.shutdown(); +} + + + +void DepthImageToLaserScanROSTest::depthCb(const sensor_msgs::ImageConstPtr& depth_msg, + const sensor_msgs::CameraInfoConstPtr& info_msg){ + try + { + sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(depth_msg, info_msg); + pub_.publish(scan_msg); + + sensor_msgs::LaserScanPtr scan_test_msg = dtl_.convert_msg_f(depth_msg, info_msg); + pub_test_.publish(scan_test_msg); + } + catch (std::runtime_error& e) + { + ROS_ERROR_THROTTLE(1.0, "Could not convert depth image to laserscan: %s", e.what()); + } +} + +void DepthImageToLaserScanROSTest::connectCb(const ros::SingleSubscriberPublisher& pub) { + boost::mutex::scoped_lock lock(connect_mutex_); + if (!sub_ && pub_.getNumSubscribers() > 0) { + ROS_DEBUG("Connecting to depth topic."); + image_transport::TransportHints hints("raw", ros::TransportHints(), pnh_); + sub_ = it_.subscribeCamera("image", 10, &DepthImageToLaserScanROSTest::depthCb, this, hints); + } +} + +void DepthImageToLaserScanROSTest::disconnectCb(const ros::SingleSubscriberPublisher& pub) { + boost::mutex::scoped_lock lock(connect_mutex_); + if (pub_.getNumSubscribers() == 0) { + ROS_DEBUG("Unsubscribing from depth topic."); + sub_.shutdown(); + } +} + +void DepthImageToLaserScanROSTest::reconfigureCb(depthimage_to_laserscan::DepthConfig& config, uint32_t level){ + dtl_.set_scan_time(config.scan_time); + dtl_.set_range_limits(config.range_min, config.range_max); + dtl_.set_scan_height(config.scan_height); + dtl_.set_output_frame(config.output_frame_id); +} + +int main(int argc, char **argv){ + ros::init(argc, argv, "depthimage_to_laserscan"); + ros::NodeHandle n; + ros::NodeHandle pnh("~"); + + depthimage_to_laserscan::DepthImageToLaserScanROSTest dtl(n, pnh); + + ros::spin(); + + return 0; +} diff --git a/test/DepthImageToLaserScanTest.cpp b/test/DepthImageToLaserScanTest.cpp index 1b4acc2..7e9015a 100644 --- a/test/DepthImageToLaserScanTest.cpp +++ b/test/DepthImageToLaserScanTest.cpp @@ -188,6 +188,63 @@ TEST(ConvertTest, testRandom) } } +// Test a randomly filled image and ensure all values are < range_min +// (range_max is currently violated to fill the messages) +TEST(ConvertTest, testPerformance) +{ + srand ( 8675309 ); // Set seed for repeatable tests + + uint16_t* data = reinterpret_cast(&depth_msg_->data[0]); + for(size_t i = 0; i < depth_msg_->width*depth_msg_->height; i++){ + data[i] = rand() % 500; // Distance between 0 and 0.5m + } + + dtl_.set_scan_height(240); + + int iterations = 1000; + sensor_msgs::LaserScanPtr scan_base(new sensor_msgs::LaserScan()), scan_fast(new sensor_msgs::LaserScan()); + + clock_t time_start; + time_start = clock(); + + for(int i = 0; i < iterations; i++) + { + scan_base->ranges.clear(); + dtl_.convert_msg(depth_msg_, info_msg_, scan_base); + } + + clock_t time_base = clock(); + for(int i = 0; i < iterations; i++) + { + scan_fast->ranges.clear(); + dtl_.convert_msg_f(depth_msg_, info_msg_, scan_fast); + } + + clock_t time_end = clock(); + + int base_ms = 1000*double(time_base - time_start) / CLOCKS_PER_SEC; + int fast_ms = 1000*double(time_end - time_base) / CLOCKS_PER_SEC; + + ASSERT_EQ(scan_base->ranges.size(), scan_fast->ranges.size()); + int size = scan_base->ranges.size(); + EXPECT_GT(size, 0); + double error = 0; + // Make sure all values are greater than or equal to range_min and less than or equal to range_max + for(size_t i = 0; i < size; i++){ + if(std::isfinite(scan_base->ranges[i])){ + float delta = scan_base->ranges[i] - scan_fast->ranges[i]; + error += fabs(delta); + //ASSERT_GE(, scan_msg->range_min); + //ASSERT_LE(scan_msg->ranges[i], scan_msg->range_max); + } + } + + if(size > 0) + error /= size; + + printf("Base conversion = % 4dms, fast conversion=% 4dms, total range error =%f\n", base_ms, fast_ms, error); +} + // Test to preserve NaN TEST(ConvertTest, testNaN) { @@ -282,4 +339,4 @@ TEST(ConvertTest, testNegativeInf) int main(int argc, char **argv){ testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} From be3899ddf7c9411c19b0ff7d1705f738c916824d Mon Sep 17 00:00:00 2001 From: Dmitry Kargin Date: Thu, 22 Dec 2016 14:31:50 +0300 Subject: [PATCH 2/3] Cleaned up whitespaces and styling --- .../DepthImageToLaserScan.h | 259 +++++++++--------- src/DepthImageToLaserScan.cpp | 20 +- 2 files changed, 141 insertions(+), 138 deletions(-) diff --git a/include/depthimage_to_laserscan/DepthImageToLaserScan.h b/include/depthimage_to_laserscan/DepthImageToLaserScan.h index a76791f..357c3ab 100644 --- a/include/depthimage_to_laserscan/DepthImageToLaserScan.h +++ b/include/depthimage_to_laserscan/DepthImageToLaserScan.h @@ -64,44 +64,36 @@ namespace depthimage_to_laserscan * */ sensor_msgs::LaserScanPtr convert_msg(const sensor_msgs::ImageConstPtr& depth_msg, - const sensor_msgs::CameraInfoConstPtr& info_msg) - { - sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan()); - convert_msg(depth_msg, info_msg, scan_msg); - return scan_msg; - } + const sensor_msgs::CameraInfoConstPtr& info_msg); /** * Allocation-free converter */ bool convert_msg(const sensor_msgs::ImageConstPtr& depth_msg, - const sensor_msgs::CameraInfoConstPtr& info_msg, sensor_msgs::LaserScanPtr & scan_msg); - + const sensor_msgs::CameraInfoConstPtr& info_msg, sensor_msgs::LaserScanPtr & scan_msg); + /** - * Converts the information in a depth image (sensor_msgs::Image) to a sensor_msgs::LaserScan. - * - * This function converts the information in the depth encoded image (UInt16 or Float32 encoding) into - * a sensor_msgs::LaserScan as accurately as possible. To do this, it requires the synchornized Image/CameraInfo - * pair associated with the image. - * - * This function uses float32 for most calculations. ARM/NEON platforms prefer float over double - * - * @param depth_msg UInt16 or Float32 encoded depth image. - * @param info_msg CameraInfo associated with depth_msg - * @return sensor_msgs::LaserScanPtr for the center row(s) of the depth image. - * - */ + * Converts the information in a depth image (sensor_msgs::Image) to a sensor_msgs::LaserScan. + * + * This function converts the information in the depth encoded image (UInt16 or Float32 encoding) into + * a sensor_msgs::LaserScan as accurately as possible. To do this, it requires the synchornized Image/CameraInfo + * pair associated with the image. + * + * This function uses float32 for most calculations. ARM/NEON platforms prefer float over double + * + * @param depth_msg UInt16 or Float32 encoded depth image. + * @param info_msg CameraInfo associated with depth_msg + * @return sensor_msgs::LaserScanPtr for the center row(s) of the depth image. + * + */ sensor_msgs::LaserScanPtr convert_msg_f(const sensor_msgs::ImageConstPtr& depth_msg, - const sensor_msgs::CameraInfoConstPtr& info_msg) - { - sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan()); - convert_msg_f(depth_msg, info_msg, scan_msg); - return scan_msg; - } - - bool convert_msg_f(const sensor_msgs::ImageConstPtr& depth_msg, - const sensor_msgs::CameraInfoConstPtr& info_msg, sensor_msgs::LaserScanPtr & scan_msg); + const sensor_msgs::CameraInfoConstPtr& info_msg); + /** + * Allocation-free converter for float version + */ + bool convert_msg_f(const sensor_msgs::ImageConstPtr& depth_msg, + const sensor_msgs::CameraInfoConstPtr& info_msg, sensor_msgs::LaserScanPtr & scan_msg); /** * Sets the scan time parameter. @@ -163,15 +155,15 @@ namespace depthimage_to_laserscan double magnitude_of_ray(const cv::Point3d& ray) const; /** - * Computes euclidean length of a cv::Point3f (as a ray from origin) - * - * This function computes the length of a cv::Point3f assumed to be a vector starting at the origin (0,0,0). - * - * @param ray The ray for which the magnitude is desired. - * @return Returns the magnitude of the ray. - * - */ - float magnitude_of_ray_f(const cv::Point3f& ray) const; + * Computes euclidean length of a cv::Point3f (as a ray from origin) + * + * This function computes the length of a cv::Point3f assumed to be a vector starting at the origin (0,0,0). + * + * @param ray The ray for which the magnitude is desired. + * @return Returns the magnitude of the ray. + * + */ + float magnitude_of_ray_f(const cv::Point3f& ray) const; /** * Computes the angle between two cv::Point3d @@ -187,18 +179,18 @@ namespace depthimage_to_laserscan double angle_between_rays(const cv::Point3d& ray1, const cv::Point3d& ray2) const; /** - * Computes the angle between two cv::Point3f - * - * Computes the angle of two cv::Point3f assumed to be vectors starting at the origin (0,0,0). - * Uses the following equation: angle = arccos(a*b/(|a||b|)) where a = ray1 and b = ray2. - * - * @param ray1 The first ray - * @param ray2 The second ray - * @return The angle between the two rays (in radians) - * - */ + * Computes the angle between two cv::Point3f + * + * Computes the angle of two cv::Point3f assumed to be vectors starting at the origin (0,0,0). + * Uses the following equation: angle = arccos(a*b/(|a||b|)) where a = ray1 and b = ray2. + * + * @param ray1 The first ray + * @param ray2 The second ray + * @return The angle between the two rays (in radians) + * + */ float angle_between_rays_f(const cv::Point3f& ray1, const cv::Point3f& ray2) const; - + /** * Determines whether or not new_value should replace old_value in the LaserScan. * @@ -229,7 +221,7 @@ namespace depthimage_to_laserscan */ template void convert(const sensor_msgs::ImageConstPtr& depth_msg, const image_geometry::PinholeCameraModel& cam_model, - const sensor_msgs::LaserScanPtr& scan_msg, const int& scan_height) const{ + const sensor_msgs::LaserScanPtr& scan_msg, const int& scan_height) const{ // Use correct principal point from calibration float center_x = cam_model.cx(); float center_y = cam_model.cy(); @@ -238,7 +230,7 @@ namespace depthimage_to_laserscan double unit_scaling = depthimage_to_laserscan::DepthTraits::toMeters( T(1) ); float constant_x = unit_scaling / cam_model.fx(); float constant_y = unit_scaling / cam_model.fy(); - + const T* depth_row = reinterpret_cast(&depth_msg->data[0]); int row_step = depth_msg->step / sizeof(T); @@ -247,94 +239,93 @@ namespace depthimage_to_laserscan for(int v = offset; v < offset+scan_height_; v++, depth_row += row_step) { - for (int u = 0; u < (int)depth_msg->width; u++) // Loop over each pixel in row - { - T depth = depth_row[u]; - - double r = depth; // Assign to pass through NaNs and Infs - double th = -atan2((double)(u - center_x) * constant_x, unit_scaling); // Atan2(x, z), but depth divides out - int index = (th - scan_msg->angle_min) / scan_msg->angle_increment; - - if (depthimage_to_laserscan::DepthTraits::valid(depth)){ // Not NaN or Inf - // Calculate in XYZ - double x = (u - center_x) * depth * constant_x; - double z = depthimage_to_laserscan::DepthTraits::toMeters(depth); - - // Calculate actual distance - r = sqrt(pow(x, 2.0) + pow(z, 2.0)); - } - - // Determine if this point should be used. - if(use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)){ - scan_msg->ranges[index] = r; - } - } + for (int u = 0; u < (int)depth_msg->width; u++) // Loop over each pixel in row + { + T depth = depth_row[u]; + + double r = depth; // Assign to pass through NaNs and Infs + double th = -atan2((double)(u - center_x) * constant_x, unit_scaling); // Atan2(x, z), but depth divides out + int index = (th - scan_msg->angle_min) / scan_msg->angle_increment; + + if (depthimage_to_laserscan::DepthTraits::valid(depth)){ // Not NaN or Inf + // Calculate in XYZ + double x = (u - center_x) * depth * constant_x; + double z = depthimage_to_laserscan::DepthTraits::toMeters(depth); + + // Calculate actual distance + r = sqrt(pow(x, 2.0) + pow(z, 2.0)); + } + + // Determine if this point should be used. + if(use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)){ + scan_msg->ranges[index] = r; + } + } } } - // Optimized version - // 1. Using float instead of double - // 2. Column-order processing - // 3. No shared_ptr interaction for each pixel + /** Optimized version of 'convert' function + * 1. Using float instead of double + * 2. Column-order processing + * 3. No shared_ptr interaction for each pixel + */ template - void convert_f(const sensor_msgs::ImageConstPtr& depth_msg, const image_geometry::PinholeCameraModel& cam_model, - const sensor_msgs::LaserScanPtr& scan_msg, const int& scan_height) const{ - // Use correct principal point from calibration - float center_x = cam_model.cx(); - float center_y = cam_model.cy(); - - // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) - float unit_scaling = depthimage_to_laserscan::DepthTraits::toMeters( T(1) ); - float constant_x = unit_scaling / cam_model.fx(); - float constant_y = unit_scaling / cam_model.fy(); - - //const T* depth_row = reinterpret_cast(&depth_msg->data[0]); - const T* depth_col = reinterpret_cast(&depth_msg->data[0]); - int row_step = depth_msg->step / sizeof(T); - - int offset = (int)(cam_model.cy()-scan_height/2); - //depth_row += offset*row_step; // Offset to center of image - depth_col += offset*row_step; // Offset to center of image - - sensor_msgs::LaserScan &scan = *scan_msg; - - // Row order iteration (Y->X) is not effective here, because of atan2 calculation - for (int u = 0; u < (int)depth_msg->width; u++, depth_col++) // Loop over each pixel in row - { - // This can be precomputed for each column until sensor geometry is not changing - float th = -atan2f((float)(u - center_x) * constant_x, unit_scaling); // Atan2(x, z), but depth divides out - int index = (th - scan_msg->angle_min) / scan_msg->angle_increment; - const T* ptr = depth_col; - - float min_range = scan.range_max*scan.range_max; - - for(int v = offset; v < offset+scan_height_; v++, ptr += row_step) - { - T depth = *ptr; - - float r = depth; // Assign to pass through NaNs and Infs - - if (depthimage_to_laserscan::DepthTraits::valid(depth)){ // Not NaN or Inf - // Calculate in XYZ - float x = (u - center_x) * depth * constant_x; - float z = depthimage_to_laserscan::DepthTraits::toMeters(depth); - // Calculate actual distance - r = x*x + z*z; - if( r < min_range) - min_range = r; - } - } - float r = sqrt(min_range); - // Determine if this point should be used. - if(use_point(r, scan.ranges[index], scan.range_min, scan.range_max)) - { - scan.ranges[index] = r; - } - } - } + void convert_f(const sensor_msgs::ImageConstPtr& depth_msg, const image_geometry::PinholeCameraModel& cam_model, + const sensor_msgs::LaserScanPtr& scan_msg, const int& scan_height) const{ + // Use correct principal point from calibration + float center_x = cam_model.cx(); + float center_y = cam_model.cy(); + + // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) + float unit_scaling = depthimage_to_laserscan::DepthTraits::toMeters( T(1) ); + float constant_x = unit_scaling / cam_model.fx(); + float constant_y = unit_scaling / cam_model.fy(); + + const T* depth_col = reinterpret_cast(&depth_msg->data[0]); + int row_step = depth_msg->step / sizeof(T); + + int offset = (int)(cam_model.cy()-scan_height/2); + depth_col += offset*row_step; // Offset to center of image + + sensor_msgs::LaserScan &scan = *scan_msg; + + // Row order iteration (Y->X) is not effective here, because of atan2 calculation + for (int u = 0; u < (int)depth_msg->width; u++, depth_col++) // Loop over each pixel in row + { + // This can be precomputed for each column until sensor geometry is not changing + float th = -atan2f((float)(u - center_x) * constant_x, unit_scaling); // Atan2(x, z), but depth divides out + int index = (th - scan_msg->angle_min) / scan_msg->angle_increment; + const T* ptr = depth_col; + + float min_range = scan.range_max*scan.range_max; + + for(int v = offset; v < offset+scan_height_; v++, ptr += row_step) + { + T depth = *ptr; + + float r = depth; // Assign to pass through NaNs and Infs + + if (depthimage_to_laserscan::DepthTraits::valid(depth)){ // Not NaN or Inf + // Calculate in XYZ + float x = (u - center_x) * depth * constant_x; + float z = depthimage_to_laserscan::DepthTraits::toMeters(depth); + // Calculate actual distance + r = x*x + z*z; + if( r < min_range) + min_range = r; + } + } + float r = sqrt(min_range); + // Determine if this point should be used. + if(use_point(r, scan.ranges[index], scan.range_min, scan.range_max)) + { + scan.ranges[index] = r; + } + } + } image_geometry::PinholeCameraModel cam_model_; ///< image_geometry helper class for managing sensor_msgs/CameraInfo messages. - + float scan_time_; ///< Stores the time between scans. float range_min_; ///< Stores the current minimum range to use. float range_max_; ///< Stores the current maximum range to use. diff --git a/src/DepthImageToLaserScan.cpp b/src/DepthImageToLaserScan.cpp index 18720ac..dd9b7e8 100644 --- a/src/DepthImageToLaserScan.cpp +++ b/src/DepthImageToLaserScan.cpp @@ -95,6 +95,22 @@ bool DepthImageToLaserScan::use_point(const float new_value, const float old_val return shorter_check; } +sensor_msgs::LaserScanPtr DepthImageToLaserScan::convert_msg(const sensor_msgs::ImageConstPtr& depth_msg, + const sensor_msgs::CameraInfoConstPtr& info_msg) +{ + sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan()); + convert_msg(depth_msg, info_msg, scan_msg); + return scan_msg; +} + +sensor_msgs::LaserScanPtr convert_msg_f(const sensor_msgs::ImageConstPtr& depth_msg, + const sensor_msgs::CameraInfoConstPtr& info_msg) +{ + sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan()); + convert_msg_f(depth_msg, info_msg, scan_msg); + return scan_msg; +} + bool DepthImageToLaserScan::convert_msg(const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::CameraInfoConstPtr& info_msg, sensor_msgs::LaserScanPtr & scan_msg) { @@ -118,7 +134,6 @@ bool DepthImageToLaserScan::convert_msg(const sensor_msgs::ImageConstPtr& depth_ double angle_min = -angle_between_rays(center_ray, right_ray); // Negative because the laserscan message expects an opposite rotation of that from the depth image // Fill in laserscan message - //sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan()); scan_msg->header = depth_msg->header; if(output_frame_id_.length() > 0){ scan_msg->header.frame_id = output_frame_id_; @@ -157,7 +172,6 @@ bool DepthImageToLaserScan::convert_msg(const sensor_msgs::ImageConstPtr& depth_ throw std::runtime_error(ss.str()); } - //return scan_msg; return true; } @@ -183,7 +197,6 @@ bool DepthImageToLaserScan::convert_msg_f(const sensor_msgs::ImageConstPtr& dept float angle_min = -angle_between_rays_f(center_ray, right_ray); // Negative because the laserscan message expects an opposite rotation of that from the depth image // Fill in laserscan message - //sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan()); scan_msg->header = depth_msg->header; if(output_frame_id_.length() > 0){ scan_msg->header.frame_id = output_frame_id_; @@ -223,7 +236,6 @@ bool DepthImageToLaserScan::convert_msg_f(const sensor_msgs::ImageConstPtr& dept } return true; - //return scan_msg; } void DepthImageToLaserScan::set_scan_time(const float scan_time){ From 9a796474a95d2462202f2290c6f08fd7bcfe1dda Mon Sep 17 00:00:00 2001 From: Dmitry Kargin Date: Thu, 22 Dec 2016 16:35:53 +0300 Subject: [PATCH 3/3] Fixed compiler error --- src/DepthImageToLaserScan.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/DepthImageToLaserScan.cpp b/src/DepthImageToLaserScan.cpp index dd9b7e8..915d443 100644 --- a/src/DepthImageToLaserScan.cpp +++ b/src/DepthImageToLaserScan.cpp @@ -103,7 +103,7 @@ sensor_msgs::LaserScanPtr DepthImageToLaserScan::convert_msg(const sensor_msgs:: return scan_msg; } -sensor_msgs::LaserScanPtr convert_msg_f(const sensor_msgs::ImageConstPtr& depth_msg, +sensor_msgs::LaserScanPtr DepthImageToLaserScan::convert_msg_f(const sensor_msgs::ImageConstPtr& depth_msg, const sensor_msgs::CameraInfoConstPtr& info_msg) { sensor_msgs::LaserScanPtr scan_msg(new sensor_msgs::LaserScan());