[Personalrobots-commit] SF.net SVN: personalrobots:[2383] pkg/trunk

[<tf...@us...>]

Revision: 2383
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2383&view=rev
Author:   tfoote
Date:     2008-07-31 20:18:03 +0000 (Thu, 31 Jul 2008)

Log Message:
-----------
This is a merge of my work offline last night.  

There is now a laser projection class being used in wavefront player.


 r5861@zug:  tfoote | 2008-07-30 18:03:19 -0700
 creating a copy of pkg trunk for offline use
 
 r5862@zug:  tfoote | 2008-07-30 18:04:24 -0700
 adding in projection functions
 r5863@zug:  tfoote | 2008-07-30 18:32:07 -0700
 the projection is compiling with caching
 r5864@zug:  tfoote | 2008-07-30 18:59:23 -0700
 adding destructor with deallocation and lots of comments
 r5865@zug:  tfoote | 2008-07-31 10:25:53 -0700
 successful test using wavefront

Modified Paths:
--------------
    pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp
    pkg/trunk/nav/wavefront_player/manifest.xml
    pkg/trunk/nav/wavefront_player/wavefront_player.cc
    pkg/trunk/util/laser_scan_utils/include/laser_scan_utils/laser_scan.h
    pkg/trunk/util/laser_scan_utils/src/laser_scan.cc

Property Changed:
----------------
    pkg/trunk/


Property changes on: pkg/trunk
___________________________________________________________________
Added: svk:merge
   + 920d6130-5740-4ec1-bb1a-45963d5fd813:/wgpkgtrunk:5865

Modified: pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp	2008-07-31 20:08:56 UTC (rev 2382)
+++ pkg/trunk/mapping/world_3d_map/src/world_3d_map.cpp	2008-07-31 20:18:03 UTC (rev 2383)
@@ -304,9 +304,9 @@
     
 private:
     
-    PointCloudFloat32              inputCloud;
-    PointCloudFloat32              toProcess;
-    std::deque<PointCloudFloat32*> currentWorld;
+  PointCloudFloat32              inputCloud; //Buffer for recieving cloud
+  PointCloudFloat32              toProcess; //Buffer (size 1) for incoming cloud
+  std::deque<PointCloudFloat32*> currentWorld;// Pointers to saved clouds
     rosTFClient                    tf;
 
     double             maxPublishFrequency;

Modified: pkg/trunk/nav/wavefront_player/manifest.xml
===================================================================
--- pkg/trunk/nav/wavefront_player/manifest.xml	2008-07-31 20:08:56 UTC (rev 2382)
+++ pkg/trunk/nav/wavefront_player/manifest.xml	2008-07-31 20:18:03 UTC (rev 2383)
@@ -4,6 +4,7 @@
   <license>BSD</license>
   <depend package="roscpp" />
   <depend package="player" />
+  <depend package="laser_scan_utils" />
   <depend package="std_srvs" />
   <depend package="rosTF" />
   <export>

Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc	2008-07-31 20:08:56 UTC (rev 2382)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc	2008-07-31 20:18:03 UTC (rev 2383)
@@ -130,6 +130,9 @@
 // For transform support
 #include <rosTF/rosTF.h>
 
+//Laser projection
+#include "laser_scan_utils/laser_scan.h"
+
 // For time support
 #include <ros/time.h>
 
@@ -218,7 +221,8 @@
 
     // Internal helpers
     void sendVelCmd(double vx, double vy, double vth);
-
+  
+    laser_scan::LaserProjection projector_;
   public:
     // Transform client
     rosTFClient tf;
@@ -426,8 +430,12 @@
       continue;
     }
 
+
     // Assemble a point cloud, in the laser's frame
     std_msgs::PointCloudFloat32 local_cloud;
+    projector_.projectLaser(*it, local_cloud);
+    
+    /*  Replaced by above!!
     local_cloud.header.frame_id = it->header.frame_id;
     local_cloud.header.stamp = it->header.stamp;
     local_cloud.set_pts_size(it->get_ranges_size());
@@ -455,7 +463,7 @@
 
     // Resize, because we may have thrown out some points
     local_cloud.set_pts_size(cnt);
-
+*/
     // Convert to a point cloud in the map frame
     std_msgs::PointCloudFloat32 global_cloud;
 

Modified: pkg/trunk/util/laser_scan_utils/include/laser_scan_utils/laser_scan.h
===================================================================
--- pkg/trunk/util/laser_scan_utils/include/laser_scan_utils/laser_scan.h	2008-07-31 20:08:56 UTC (rev 2382)
+++ pkg/trunk/util/laser_scan_utils/include/laser_scan_utils/laser_scan.h	2008-07-31 20:18:03 UTC (rev 2383)
@@ -27,30 +27,56 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#ifndef LASER_SCAN_H
-#define LASER_SCAN_H
+#ifndef LASER_SCAN_UTILS_LASERSCAN_H
+#define LASER_SCAN_UTILS_LASERSCAN_H
 
+#include <map>
 #include <iostream>
+#include <sstream>
+
 #include <newmat10/newmat.h>
 #include <newmat10/newmatio.h>
 #include <newmat10/newmatap.h>
 
 #include "math_utils/math_utils.h"
 #include "std_msgs/LaserScan.h"
+#include "std_msgs/PointCloudFloat32.h"
 
 /* \mainpage 
  * This is a class for laser scan utilities.  
- * The first goal will be to project laser scans into point clouds efficiently.  
+ * \todo The first goal will be to project laser scans into point clouds efficiently.  
  * The second goal is to provide median filtering.  
+ * \todo Other potential additions are upsampling and downsampling algorithms for the scans.
  */
 
 namespace laser_scan{
-  
-  /** \brief Project Laser Scan
-   * This will project a laser scan from a linear array into a 3D point cloud
+  /** \brief A Class to Project Laser Scan
+   * This class will project laser scans into point clouds, and caches unit vectors 
+   * between runs so as not to need to recalculate.  
    */
-  //  void projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloudFloat32 & cloud_out);
+  class LaserProjection
+    {
+    public:
+      /** \brief Destructor to deallocate stored unit vectors */
+      ~LaserProjection(); 
+      
+      /** \brief Project Laser Scan
+       * This will project a laser scan from a linear array into a 3D point cloud
+       */
+      void projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloudFloat32 & cloud_out);
+      
+      /** \brief Return the unit vectors for this configuration
+       * Return the unit vectors for this configuration.
+       * if they have not been calculated yet, calculate them and store them
+       * Otherwise it will return them from memory. */
+      NEWMAT::Matrix& getUnitVectors(float angle_max, float angle_min, float angle_increment);
+    private:
+      ///The map of pointers to stored values
+      std::map<std::string,NEWMAT::Matrix*> unit_vector_map_;
+      
+    };
   
+  /** \brief A class to provide median filtering of laser scans */
   class LaserMedianFilter
     {
     public:
@@ -58,7 +84,8 @@
       
       /** \brief Constructor
        * \param averaging_length How many scans to average over.
-       * \param Whether to downsample and return or compute a rolling median over the last n scans
+       * \param num_ranges Whether to downsample and return or compute a rolling median over the last n scans
+       * \param mode What mode to operate in Trailing or Downsampling (Effectively changes returning true every time or every 3)
        */
       LaserMedianFilter(unsigned int averaging_length, unsigned int num_ranges, MedianMode_t mode = MEDIAN_DOWNSAMPLE);
       /** \brief Add a scan to the filter
@@ -66,22 +93,22 @@
        * return whether there is a new output to get */
       bool addScan(const std_msgs::LaserScan& scan_in);
       /** \brief get the Filtered results
-       * \param The scan to fill with the median results */
+       * \param scan_result The scan to fill with the median results */
       void getMedian(std_msgs::LaserScan& scan_result);
-
-
+      
+      
     private:
-      unsigned int current_packet_num_;
-      NEWMAT::Matrix range_data_;
-      NEWMAT::Matrix intensity_data_;
-      unsigned int filter_length_;
-      unsigned int num_ranges_;
-      MedianMode_t mode_;
-        
+      unsigned int current_packet_num_; ///The number of scans recieved
+      NEWMAT::Matrix range_data_; ///Storage for range_data
+      NEWMAT::Matrix intensity_data_; ///Storage for intensity data
+      unsigned int filter_length_; ///How many scans to average over
+      unsigned int num_ranges_; /// How many data point are in each row
+      MedianMode_t mode_; ///Whether to return true every time or once every 3
+      
       std_msgs::LaserScan temp_scan_; /** \todo cache only shallow info not full scan */
       
     };
   
   
 }
-#endif //LASER_SCAN_H
+#endif //LASER_SCAN_UTILS_LASERSCAN_H

Modified: pkg/trunk/util/laser_scan_utils/src/laser_scan.cc
===================================================================
--- pkg/trunk/util/laser_scan_utils/src/laser_scan.cc	2008-07-31 20:08:56 UTC (rev 2382)
+++ pkg/trunk/util/laser_scan_utils/src/laser_scan.cc	2008-07-31 20:18:03 UTC (rev 2383)
@@ -30,9 +30,86 @@
 #include "laser_scan_utils/laser_scan.h"
 
 
-/** \todo add other channel pass throughs */
 namespace laser_scan{
 
+  
+  void LaserProjection::projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloudFloat32 & cloud_out)
+  {
+    NEWMAT::Matrix ranges(2, scan_in.ranges_size);
+    double * matPointer = ranges.Store();
+    // Fill the ranges matrix
+    for (unsigned int index = 0; index < scan_in.ranges_size; index++)
+      {
+        matPointer[index] = (double) scan_in.ranges[index];
+        matPointer[index+scan_in.ranges_size] = (double) scan_in.ranges[index];
+      }
+    
+    //Do the projection
+    NEWMAT::Matrix output = NEWMAT::SP(ranges, getUnitVectors(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment));
+    
+
+    //Stuff the output cloud
+    cloud_out.header = scan_in.header;
+    cloud_out.set_pts_size(scan_in.ranges_size);
+
+    double* outputMat = output.Store();
+
+    unsigned int count = 0;
+    for (unsigned int index = 0; index< scan_in.ranges_size; index++)
+      {
+        if (matPointer[index] <= scan_in.range_max 
+            && matPointer[index] >= scan_in.range_min) //only valid
+          {
+            cloud_out.pts[count].x = outputMat[index];
+            cloud_out.pts[count].y = outputMat[index + scan_in.ranges_size];
+            cloud_out.pts[count].z = 0.0;
+            count++;
+          }
+      }
+
+    //downsize if necessary
+    cloud_out.set_pts_size(count);
+
+    /** \todo Add intensity to channel 1 */
+
+  };
+  
+  NEWMAT::Matrix& LaserProjection::getUnitVectors(float angle_min, float angle_max, float angle_increment)
+  {
+    //construct string for lookup in the map
+    std::stringstream anglestring;
+    anglestring <<angle_min<<","<<angle_max<<","<<angle_increment;
+    std::map<string, NEWMAT::Matrix*>::iterator it;
+    it = unit_vector_map_.find(anglestring.str());
+    //check the map for presense
+    if (it != unit_vector_map_.end())
+      return *((*it).second);     //if present return
+    //else calculate
+    unsigned int length = (unsigned int) ((angle_max - angle_min)/angle_increment) + 1;//fixme assuming it's calculated right
+    NEWMAT::Matrix * tempPtr = new NEWMAT::Matrix(2,length);
+    for (unsigned int index = 0;index < length; index++)
+      {
+        (*tempPtr)(1,index+1) = cos(angle_min + (double) index * angle_increment);
+        (*tempPtr)(2,index+1) = sin(angle_min + (double) index * angle_increment);
+      }
+    //store 
+    unit_vector_map_[anglestring.str()] = tempPtr;
+    //and return
+    return *tempPtr;
+  };
+
+
+  LaserProjection::~LaserProjection()
+  {
+    std::map<string, NEWMAT::Matrix*>::iterator it;
+    it = unit_vector_map_.begin();
+    while (it != unit_vector_map_.end())
+      {
+        delete (*it).second;
+        it++;
+      }
+  };
+
   LaserMedianFilter::LaserMedianFilter(unsigned int filter_length, unsigned int num_ranges, MedianMode_t mode):
     current_packet_num_(0),
     range_data_(filter_length,num_ranges),


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.