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,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<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_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
+	  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_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<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.
@@ -222,4 +345,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