Improving performance for ARM7-based platforms

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)

include/depthimage_to_laserscan/DepthImageToLaserScan.h

@@ -64,8 +64,37 @@ 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);
+
+    /**
+     * 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);
+    /**
+     * 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.
      * 
@@ -125,6 +154,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,7 +177,20 @@ 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.
      * 
@@ -168,7 +221,7 @@ namespace depthimage_to_laserscan
     */
     template<typename T>
     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();
@@ -177,41 +230,102 @@ namespace depthimage_to_laserscan
       double unit_scaling = depthimage_to_laserscan::DepthTraits<T>::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<const T*>(&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
 
-      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<T>::valid(depth)){ // Not NaN or Inf
-		    // Calculate in XYZ
-		    double x = (u - center_x) * depth * constant_x;
-		    double z = depthimage_to_laserscan::DepthTraits<T>::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 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<T>::valid(depth)){ // Not NaN or Inf
+            // Calculate in XYZ
+            double x = (u - center_x) * depth * constant_x;
+            double z = depthimage_to_laserscan::DepthTraits<T>::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 of 'convert' function
+     * 1. Using float instead of double
+     * 2. Column-order processing
+     * 3. No shared_ptr interaction for each pixel
+     */
+    template<typename T>
+    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<T>::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<const T*>(&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<T>::valid(depth)){ // Not NaN or Inf
+            // Calculate in XYZ
+            float x = (u - center_x) * depth * constant_x;
+            float z = depthimage_to_laserscan::DepthTraits<T>::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.
@@ -222,4 +336,4 @@ namespace depthimage_to_laserscan
 
 }; // depthimage_to_laserscan
 
-#endif
+#endif

include/depthimage_to_laserscan/DepthImageToLaserScanROS.h

@@ -107,4 +107,4 @@ namespace depthimage_to_laserscan
 
 }; // depthimage_to_laserscan
 
-#endif
+#endif

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 <ros/ros.h>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/LaserScan.h>
+#include <boost/thread/mutex.hpp>
+#include <dynamic_reconfigure/server.h>
+#include <depthimage_to_laserscan/DepthConfig.h>
+
+#include <depthimage_to_laserscan/DepthImageToLaserScan.h>
+
+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<depthimage_to_laserscan::DepthConfig> 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