Merge pull request #194 from ros-drivers/avoid_unnecessary_computation

Avoid unnecessary computation

velodyne_pointcloud/include/velodyne_pointcloud/calibration.h

@@ -13,6 +13,7 @@
 #define __VELODYNE_CALIBRATION_H
 
 #include <map>
+#include <vector>
 #include <string>
 
 namespace velodyne_pointcloud {
@@ -57,7 +58,8 @@ namespace velodyne_pointcloud {
   public:
 
     float distance_resolution_m;
-    std::map<int, LaserCorrection> laser_corrections;
+    std::map<int,LaserCorrection> laser_corrections_map;
+    std::vector<LaserCorrection> laser_corrections;
     int num_lasers;
     bool initialized;
     bool ros_info;

velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h

@@ -152,6 +152,8 @@ namespace velodyne_rawdata
     void setParameters(double min_range, double max_range, double view_direction,
                        double view_width);
 
+    int scansPerPacket() const;
+
   private:
 
     /** configuration parameters */

velodyne_pointcloud/src/conversions/convert.cc

@@ -65,6 +65,8 @@ namespace velodyne_pointcloud
     outMsg.pc->header.frame_id = scanMsg->header.frame_id;
     outMsg.pc->height = 1;
 
+    outMsg.pc->points.reserve(scanMsg->packets.size() * data_->scansPerPacket());
+
     // process each packet provided by the driver
     for (size_t i = 0; i < scanMsg->packets.size(); ++i)
     {

velodyne_pointcloud/src/lib/calibration.cc

@@ -150,10 +150,17 @@ namespace velodyne_pointcloud
     calibration.laser_corrections.clear();
     calibration.num_lasers = num_lasers;
     calibration.distance_resolution_m = distance_resolution_m;
+    calibration.laser_corrections.resize(num_lasers);
     for (int i = 0; i < num_lasers; i++) {
       std::pair<int, LaserCorrection> correction;
       lasers[i] >> correction;
-      calibration.laser_corrections.insert(correction);
+      const int index = correction.first;
+      if( index >= calibration.laser_corrections.size() )
+      {
+        calibration.laser_corrections.resize( index+1 );
+      }
+      calibration.laser_corrections[index] = (correction.second);
+      calibration.laser_corrections_map.insert(correction);
     }
 
     // For each laser ring, find the next-smallest vertical angle.
@@ -231,8 +238,8 @@ namespace velodyne_pointcloud
       YAML::Value << calibration.distance_resolution_m;
     out << YAML::Key << LASERS << YAML::Value << YAML::BeginSeq;
     for (std::map<int, LaserCorrection>::const_iterator
-           it = calibration.laser_corrections.begin();
-         it != calibration.laser_corrections.end(); it++)
+           it = calibration.laser_corrections_map.begin();
+         it != calibration.laser_corrections_map.end(); it++)
       {
         out << *it; 
       }

velodyne_pointcloud/src/lib/rawdata.cc

@@ -36,6 +36,8 @@
 
 namespace velodyne_rawdata
 {
+inline float SQR(float val) { return val*val; }
+
   ////////////////////////////////////////////////////////////////////////
   //
   // RawData base class implementation
@@ -73,6 +75,18 @@ namespace velodyne_rawdata
     }
   }
 
+  int RawData::scansPerPacket() const
+  {
+    if( calibration_.num_lasers == 16)
+    {
+      return BLOCKS_PER_PACKET * VLP16_FIRINGS_PER_BLOCK *
+          VLP16_SCANS_PER_FIRING;
+    }
+    else{
+      return BLOCKS_PER_PACKET * SCANS_PER_BLOCK;
+    }
+  }
+
   /** Set up for on-line operation. */
   int RawData::setup(ros::NodeHandle private_nh)
   {
@@ -145,6 +159,7 @@ namespace velodyne_rawdata
    */
   void RawData::unpack(const velodyne_msgs::VelodynePacket &pkt, DataContainerBase& data)
   {
+    using velodyne_pointcloud::LaserCorrection;
     ROS_DEBUG_STREAM("Received packet, time: " << pkt.stamp);
 
     /** special parsing for the VLP16 **/
@@ -170,21 +185,24 @@ namespace velodyne_rawdata
 
         float x, y, z;
         float intensity;
-        uint8_t laser_number;       ///< hardware laser number
+        const uint8_t laser_number  = j + bank_origin;
 
-        laser_number = j + bank_origin;
-        velodyne_pointcloud::LaserCorrection &corrections = 
-          calibration_.laser_corrections[laser_number];
+        const LaserCorrection &corrections = calibration_.laser_corrections[laser_number];
 
         /** Position Calculation */
-
+        const raw_block_t &block = raw->blocks[i];
         union two_bytes tmp;
-        tmp.bytes[0] = raw->blocks[i].data[k];
-        tmp.bytes[1] = raw->blocks[i].data[k+1];
+        tmp.bytes[0] = block.data[k];
+        tmp.bytes[1] = block.data[k+1];
+
+        float distance = tmp.uint * calibration_.distance_resolution_m;
+        distance += corrections.dist_correction;
+        if (!pointInRange(distance)) continue;
+
         /*condition added to avoid calculating points which are not
           in the interesting defined area (min_angle < area < max_angle)*/
-        if ((raw->blocks[i].rotation >= config_.min_angle 
-             && raw->blocks[i].rotation <= config_.max_angle 
+        if ((block.rotation >= config_.min_angle
+             && block.rotation <= config_.max_angle
              && config_.min_angle < config_.max_angle)
              ||(config_.min_angle > config_.max_angle 
              && (raw->blocks[i].rotation <= config_.max_angle 
@@ -200,11 +218,11 @@ namespace velodyne_rawdata
           // cos(a-b) = cos(a)*cos(b) + sin(a)*sin(b)
           // sin(a-b) = sin(a)*cos(b) - cos(a)*sin(b)
           float cos_rot_angle = 
-            cos_rot_table_[raw->blocks[i].rotation] * cos_rot_correction + 
-            sin_rot_table_[raw->blocks[i].rotation] * sin_rot_correction;
+            cos_rot_table_[block.rotation] * cos_rot_correction +
+            sin_rot_table_[block.rotation] * sin_rot_correction;
           float sin_rot_angle = 
-            sin_rot_table_[raw->blocks[i].rotation] * cos_rot_correction - 
-            cos_rot_table_[raw->blocks[i].rotation] * sin_rot_correction;
+            sin_rot_table_[block.rotation] * cos_rot_correction -
+            cos_rot_table_[block.rotation] * sin_rot_correction;
 
           float horiz_offset = corrections.horiz_offset_correction;
           float vert_offset = corrections.vert_offset_correction;
@@ -283,13 +301,11 @@ namespace velodyne_rawdata
                              * (1 - corrections.focal_distance / 13100);
           float focal_slope = corrections.focal_slope;
           intensity += focal_slope * (std::abs(focal_offset - 256 * 
-            (1 - static_cast<float>(tmp.uint)/65535)*(1 - static_cast<float>(tmp.uint)/65535)));
+            SQR(1 - static_cast<float>(tmp.uint)/65535)));
           intensity = (intensity < min_intensity) ? min_intensity : intensity;
           intensity = (intensity > max_intensity) ? max_intensity : intensity;
 
-          if (pointInRange(distance)) {
-            data.addPoint(x_coord, y_coord, z_coord, corrections.laser_ring, raw->blocks[i].rotation, distance, intensity);
-          }
+          data.addPoint(x_coord, y_coord, z_coord, corrections.laser_ring, raw->blocks[i].rotation, distance, intensity);
         }
       }
     }
@@ -351,18 +367,23 @@ namespace velodyne_rawdata
 
       for (int firing=0, k=0; firing < VLP16_FIRINGS_PER_BLOCK; firing++){
         for (int dsr=0; dsr < VLP16_SCANS_PER_FIRING; dsr++, k+=RAW_SCAN_SIZE){
-          velodyne_pointcloud::LaserCorrection &corrections = 
-            calibration_.laser_corrections[dsr];
+          velodyne_pointcloud::LaserCorrection &corrections = calibration_.laser_corrections[dsr];
 
           /** Position Calculation */
           union two_bytes tmp;
           tmp.bytes[0] = raw->blocks[block].data[k];
           tmp.bytes[1] = raw->blocks[block].data[k+1];
 
+          float distance = tmp.uint * calibration_.distance_resolution_m;
+          distance += corrections.dist_correction;
+
+          // skip the point if out of range
+          if ( !pointInRange(distance)) continue;
+
           /** correct for the laser rotation as a function of timing during the firings **/
           azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr*VLP16_DSR_TOFFSET) + (firing*VLP16_FIRING_TOFFSET)) / VLP16_BLOCK_TDURATION);
           azimuth_corrected = ((int)round(azimuth_corrected_f)) % 36000;
-
+                 
           /*condition added to avoid calculating points which are not
             in the interesting defined area (min_angle < area < max_angle)*/
           if ((azimuth_corrected >= config_.min_angle 
@@ -461,18 +482,14 @@ namespace velodyne_rawdata
 
             intensity = raw->blocks[block].data[k+2];
 
-            float focal_offset = 256 
-                               * (1 - corrections.focal_distance / 13100) 
-                               * (1 - corrections.focal_distance / 13100);
+            float focal_offset = 256 * SQR(1 - corrections.focal_distance / 13100);
             float focal_slope = corrections.focal_slope;
             intensity += focal_slope * (std::abs(focal_offset - 256 * 
-              (1 - tmp.uint/65535)*(1 - tmp.uint/65535)));
+              SQR(1 - tmp.uint/65535)));
             intensity = (intensity < min_intensity) ? min_intensity : intensity;
             intensity = (intensity > max_intensity) ? max_intensity : intensity;
 
-            if (pointInRange(distance)) {    
-              data.addPoint(x_coord, y_coord, z_coord, corrections.laser_ring, azimuth_corrected, distance, intensity);
-            }
+            data.addPoint(x_coord, y_coord, z_coord, corrections.laser_ring, azimuth_corrected, distance, intensity);
           }
         }
       }