Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[12226] pkg/trunk
|
From: <tf...@us...> - 2009-03-06 02:16:36
|
Revision: 12226
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12226&view=rev
Author: tfoote
Date: 2009-03-06 02:16:26 +0000 (Fri, 06 Mar 2009)
Log Message:
-----------
absorbing laser_processor into laser_scan
Modified Paths:
--------------
pkg/trunk/util/laser_scan/CMakeLists.txt
pkg/trunk/util/laser_scan/manifest.xml
pkg/trunk/vision/people/manifest.xml
pkg/trunk/vision/people/src/calc_leg_features.cpp
pkg/trunk/vision/people/src/calc_leg_features.h
pkg/trunk/vision/people/src/leg_detector.cpp
pkg/trunk/vision/people/src/train_leg_detector.cpp
Added Paths:
-----------
pkg/trunk/util/laser_scan/include/laser_scan/laser_processor.h
pkg/trunk/util/laser_scan/src/laser_processor.cpp
Removed Paths:
-------------
pkg/trunk/vision/laser_processor/
Modified: pkg/trunk/util/laser_scan/CMakeLists.txt
===================================================================
--- pkg/trunk/util/laser_scan/CMakeLists.txt 2009-03-06 01:52:19 UTC (rev 12225)
+++ pkg/trunk/util/laser_scan/CMakeLists.txt 2009-03-06 02:16:26 UTC (rev 12226)
@@ -6,6 +6,9 @@
rospack_add_boost_directories()
+#todo integrate this
+rospack_add_library(laser_processor src/laser_processor.cpp)
+
rospack_add_library(laser_scan src/laser_scan.cc )
rospack_link_boost(laser_scan thread)
Copied: pkg/trunk/util/laser_scan/include/laser_scan/laser_processor.h (from rev 10961, pkg/trunk/vision/laser_processor/laser_processor.h)
===================================================================
--- pkg/trunk/util/laser_scan/include/laser_scan/laser_processor.h (rev 0)
+++ pkg/trunk/util/laser_scan/include/laser_scan/laser_processor.h 2009-03-06 02:16:26 UTC (rev 12226)
@@ -0,0 +1,145 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, 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 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.
+*********************************************************************/
+
+/*! \mainpage
+ * \htmlinclude manifest.html
+ */
+
+//! A namespace containing the laser processor helper classes
+
+
+#ifndef LASER_SCAN_LASERPROCESSOR_HH
+#define LASER_SCAN_LASERPROCESSOR_HH
+
+#include <unistd.h>
+#include <math.h>
+#include "ros/node.h"
+#include "laser_scan/LaserScan.h"
+#include "robot_msgs/PointCloud.h"
+#include "robot_msgs/Point.h"
+
+#include <list>
+#include <set>
+#include <vector>
+#include <map>
+#include <utility>
+#include <algorithm>
+
+#include "tf/transform_datatypes.h"
+
+namespace laser_scan
+{
+ //! A struct representing a single sample from the laser.
+ class Sample
+ {
+ public:
+ int index;
+ float range;
+ float intensity;
+ float x;
+ float y;
+
+ static Sample* Extract(int ind, laser_scan::LaserScan& scan);
+
+ private:
+ Sample() {};
+ };
+
+ //! The comparator allowing the creation of an ordered "SampleSet"
+ struct CompareSample
+ {
+ inline bool operator() (const Sample* a, const Sample* b)
+ {
+ return (a->index < b->index);
+ }
+ };
+
+
+ //! An ordered set of Samples
+ class SampleSet : public std::set<Sample*, CompareSample>
+ {
+ public:
+
+ ~SampleSet() { clear(); }
+
+ void clear();
+
+ void appendToCloud(robot_msgs::PointCloud& cloud, int r = 0, int g = 0, int b = 0);
+
+ tf::Point center();
+ };
+
+ //! A mask for filtering out Samples based on range
+ class ScanMask
+ {
+ SampleSet mask_;
+
+ bool filled;
+ float angle_min;
+ float angle_max;
+ uint32_t size;
+
+ public:
+
+ ScanMask() : filled(false), angle_min(0), angle_max(0), size(0) { }
+
+ inline void clear() { mask_.clear(); filled = false; }
+
+ void addScan(laser_scan::LaserScan& scan);
+
+ bool hasSample(Sample* s, float thresh);
+ };
+
+
+
+ class ScanProcessor
+ {
+ std::list<SampleSet*> clusters_;
+ laser_scan::LaserScan scan_;
+
+ public:
+
+ std::list<SampleSet*>& getClusters() { return clusters_; }
+
+ ScanProcessor(laser_scan::LaserScan& scan, ScanMask& mask_, float mask_threshold = 0.03);
+
+ ~ScanProcessor();
+
+ void removeLessThan(uint32_t num);
+
+ void splitConnected(float thresh);
+ };
+};
+
+#endif
Modified: pkg/trunk/util/laser_scan/manifest.xml
===================================================================
--- pkg/trunk/util/laser_scan/manifest.xml 2009-03-06 01:52:19 UTC (rev 12225)
+++ pkg/trunk/util/laser_scan/manifest.xml 2009-03-06 02:16:26 UTC (rev 12226)
@@ -13,6 +13,6 @@
<depend package="tf"/>
<depend package="filters"/>
<export>
-<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -llaser_scan"/>
+<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -llaser_scan -llaser_processor"/>
</export>
</package>
Copied: pkg/trunk/util/laser_scan/src/laser_processor.cpp (from rev 10961, pkg/trunk/vision/laser_processor/laser_processor.cpp)
===================================================================
--- pkg/trunk/util/laser_scan/src/laser_processor.cpp (rev 0)
+++ pkg/trunk/util/laser_scan/src/laser_processor.cpp 2009-03-06 02:16:26 UTC (rev 12226)
@@ -0,0 +1,288 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, 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 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.
+*********************************************************************/
+
+#include "laser_scan/laser_processor.h"
+
+#include <stdexcept>
+
+using namespace ros;
+using namespace std;
+using namespace laser_scan;
+
+Sample* Sample::Extract(int ind, laser_scan::LaserScan& scan)
+{
+ Sample* s = new Sample;
+
+ s->index = ind;
+ s->range = scan.ranges[ind];
+ s->intensity = scan.intensities[ind];
+ s->x = cos( scan.angle_min + ind*scan.angle_increment ) * s->range;
+ s->y = sin( scan.angle_min + ind*scan.angle_increment ) * s->range;
+ if (s->range > scan.range_min && s->range < scan.range_max)
+ return s;
+ else
+ {
+ delete s;
+ return NULL;
+ }
+}
+
+void SampleSet::clear()
+{
+ for (SampleSet::iterator i = begin();
+ i != end();
+ i++)
+ {
+ delete (*i);
+ }
+ set<Sample*, CompareSample>::clear();
+}
+
+void SampleSet::appendToCloud(robot_msgs::PointCloud& cloud, int r, int g, int b)
+{
+ float color_val = 0;
+
+ int rgb = (r << 16) | (g << 8) | b;
+ color_val = *(float*)&(rgb);
+
+ for (iterator sample_iter = begin();
+ sample_iter != end();
+ sample_iter++)
+ {
+ robot_msgs::Point32 point;
+ point.x = (*sample_iter)->x;
+ point.y = (*sample_iter)->y;
+ point.z = 0;
+
+ cloud.pts.push_back(point);
+
+ if (cloud.chan[0].name == "rgb")
+ cloud.chan[0].vals.push_back(color_val);
+
+ if (cloud.chan[0].name == "intensity")
+ cloud.chan[0].vals.push_back((*sample_iter)->intensity);
+ }
+}
+
+tf::Point SampleSet::center()
+{
+ float x_mean = 0.0;
+ float y_mean = 0.0;
+ for (iterator i = begin();
+ i != end();
+ i++)
+
+ {
+ x_mean += ((*i)->x)/size();
+ y_mean += ((*i)->y)/size();
+ }
+
+ return tf::Point (x_mean, y_mean, 0.0);
+}
+
+
+void ScanMask::addScan(laser_scan::LaserScan& scan)
+{
+ if (!filled)
+ {
+ angle_min = scan.angle_min;
+ angle_max = scan.angle_max;
+ size = scan.ranges.size();
+ filled = true;
+ } else if (angle_min != scan.angle_min ||
+ angle_max != scan.angle_max ||
+ size != scan.ranges.size())
+ {
+ throw std::runtime_error("laser_scan::ScanMask::addScan: inconsistantly sized scans added to mask");
+ }
+
+ for (uint32_t i = 0; i < scan.ranges.size(); i++)
+ {
+ Sample* s = Sample::Extract(i, scan);
+
+ if (s != NULL)
+ {
+ SampleSet::iterator m = mask_.find(s);
+
+ if (m != mask_.end())
+ {
+ if ((*m)->range > s->range)
+ {
+ delete (*m);
+ mask_.erase(m);
+ mask_.insert(s);
+ } else {
+ delete s;
+ }
+ }
+ else
+ {
+ mask_.insert(s);
+ }
+ }
+ }
+}
+
+
+bool ScanMask::hasSample(Sample* s, float thresh)
+{
+ if (s != NULL)
+ {
+ SampleSet::iterator m = mask_.find(s);
+ if ( m != mask_.end())
+ if (((*m)->range - thresh) < s->range)
+ return true;
+ }
+ return false;
+}
+
+
+
+ScanProcessor::ScanProcessor(laser_scan::LaserScan& scan, ScanMask& mask_, float mask_threshold)
+{
+ scan_ = scan;
+
+ SampleSet* cluster = new SampleSet;
+
+ for (uint32_t i = 0; i < scan.ranges.size(); i++)
+ {
+ Sample* s = Sample::Extract(i, scan);
+
+ if (s != NULL)
+ {
+ if (!mask_.hasSample(s, mask_threshold))
+ {
+ cluster->insert(s);
+ } else {
+ delete s;
+ }
+ }
+ }
+
+ clusters_.push_back(cluster);
+
+}
+
+ScanProcessor::~ScanProcessor()
+{
+ for ( list<SampleSet*>::iterator c = clusters_.begin();
+ c != clusters_.end();
+ c++)
+ delete (*c);
+}
+
+void
+ScanProcessor::removeLessThan(uint32_t num)
+{
+ list<SampleSet*>::iterator c_iter = clusters_.begin();
+ while (c_iter != clusters_.end())
+ {
+ if ( (*c_iter)->size() < num )
+ {
+ delete (*c_iter);
+ clusters_.erase(c_iter++);
+ } else {
+ ++c_iter;
+ }
+ }
+}
+
+
+void
+ScanProcessor::splitConnected(float thresh)
+{
+ list<SampleSet*> tmp_clusters;
+
+ list<SampleSet*>::iterator c_iter = clusters_.begin();
+
+ // For each cluster
+ while (c_iter != clusters_.end())
+ {
+ // Go through the entire list
+ while ((*c_iter)->size() > 0 )
+ {
+ // Take the first element
+ SampleSet::iterator s_first = (*c_iter)->begin();
+
+ // Start a new queue
+ list<Sample*> sample_queue;
+ sample_queue.push_back(*s_first);
+
+ (*c_iter)->erase(s_first);
+
+ // Grow until we get to the end of the queue
+ list<Sample*>::iterator s_q = sample_queue.begin();
+ while (s_q != sample_queue.end())
+ {
+ int expand = (int)(asin( thresh / (*s_q)->range ) / scan_.angle_increment);
+
+ SampleSet::iterator s_rest = (*c_iter)->begin();
+
+ while ( (s_rest != (*c_iter)->end() &&
+ (*s_rest)->index < (*s_q)->index + expand ) )
+ {
+ if ( (*s_rest)->range - (*s_q)->range > thresh)
+ {
+ break;
+ }
+ else if (sqrt( pow( (*s_q)->x - (*s_rest)->x, 2.0f) + pow( (*s_q)->y - (*s_rest)->y, 2.0f)) < thresh)
+ {
+ sample_queue.push_back(*s_rest);
+ (*c_iter)->erase(s_rest++);
+ break;
+ } else {
+ ++s_rest;
+ }
+ }
+ s_q++;
+ }
+
+ // Move all the samples into the new cluster
+ SampleSet* c = new SampleSet;
+ for (s_q = sample_queue.begin(); s_q != sample_queue.end(); s_q++)
+ c->insert(*s_q);
+
+ // Store the temporary clusters
+ tmp_clusters.push_back(c);
+ }
+
+ //Now that c_iter is empty, we can delete
+ delete (*c_iter);
+
+ //And remove from the map
+ clusters_.erase(c_iter++);
+ }
+
+ clusters_.insert(clusters_.begin(), tmp_clusters.begin(), tmp_clusters.end());
+}
Modified: pkg/trunk/vision/people/manifest.xml
===================================================================
--- pkg/trunk/vision/people/manifest.xml 2009-03-06 01:52:19 UTC (rev 12225)
+++ pkg/trunk/vision/people/manifest.xml 2009-03-06 02:16:26 UTC (rev 12226)
@@ -19,7 +19,7 @@
<depend package="rospy"/>
<depend package="image_msgs"/>
<depend package="topic_synchronizer"/>
-<depend package="laser_processor"/>
+<depend package="laser_scan"/>
<depend package="tf"/>
<depend package="color_calib"/>
<depend package="bfl"/>
Modified: pkg/trunk/vision/people/src/calc_leg_features.cpp
===================================================================
--- pkg/trunk/vision/people/src/calc_leg_features.cpp 2009-03-06 01:52:19 UTC (rev 12225)
+++ pkg/trunk/vision/people/src/calc_leg_features.cpp 2009-03-06 02:16:26 UTC (rev 12226)
@@ -37,7 +37,7 @@
#include "opencv/cxcore.h"
#include "opencv/cv.h"
-using namespace laser_processor;
+using namespace laser_scan;
using namespace std;
vector<float> calcLegFeatures(SampleSet* cluster, laser_scan::LaserScan& scan)
Modified: pkg/trunk/vision/people/src/calc_leg_features.h
===================================================================
--- pkg/trunk/vision/people/src/calc_leg_features.h 2009-03-06 01:52:19 UTC (rev 12225)
+++ pkg/trunk/vision/people/src/calc_leg_features.h 2009-03-06 02:16:26 UTC (rev 12226)
@@ -35,11 +35,11 @@
#ifndef CALCLEGFEATURES_HH
#define CALCLEGFEATURES_HH
-#include "laser_processor.h"
+#include "laser_scan/laser_processor.h"
#include "laser_scan/LaserScan.h"
// TODO: Should remove scan dependency from here.
// Only used for jump distance
-std::vector<float> calcLegFeatures(laser_processor::SampleSet* cluster, laser_scan::LaserScan& scan);
+std::vector<float> calcLegFeatures(laser_scan::SampleSet* cluster, laser_scan::LaserScan& scan);
#endif
Modified: pkg/trunk/vision/people/src/leg_detector.cpp
===================================================================
--- pkg/trunk/vision/people/src/leg_detector.cpp 2009-03-06 01:52:19 UTC (rev 12225)
+++ pkg/trunk/vision/people/src/leg_detector.cpp 2009-03-06 02:16:26 UTC (rev 12226)
@@ -37,7 +37,7 @@
-#include "laser_processor.h"
+#include "laser_scan/laser_processor.h"
#include "calc_leg_features.h"
#include "opencv/cxcore.h"
@@ -60,7 +60,7 @@
#include <algorithm>
using namespace std;
-using namespace laser_processor;
+using namespace laser_scan;
using namespace ros;
using namespace tf;
using namespace estimation;
Modified: pkg/trunk/vision/people/src/train_leg_detector.cpp
===================================================================
--- pkg/trunk/vision/people/src/train_leg_detector.cpp 2009-03-06 01:52:19 UTC (rev 12225)
+++ pkg/trunk/vision/people/src/train_leg_detector.cpp 2009-03-06 02:16:26 UTC (rev 12226)
@@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#include "laser_processor.h"
+#include "laser_scan/laser_processor.h"
#include "calc_leg_features.h"
#include "opencv/cxcore.h"
@@ -45,7 +45,7 @@
#include "laser_scan/LaserScan.h"
using namespace std;
-using namespace laser_processor;
+using namespace laser_scan;
using namespace ros;
enum LoadType {LOADING_NONE, LOADING_POS, LOADING_NEG, LOADING_TEST};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
