flirtlib_ros: flirtlib_ros: place_rec

00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  */
00030 
00040 #define BOOST_NO_HASH
00041 #include <flirtlib_ros/flirtlib.h>
00042 
00043 #include <flirtlib_ros/conversions.h>
00044 #include <occupancy_grid_utils/file.h>
00045 #include <occupancy_grid_utils/ray_tracer.h>
00046 #include <visualization_msgs/Marker.h>
00047 #include <mongo_ros/message_collection.h>
00048 #include <ros/ros.h>
00049 #include <tf/transform_listener.h>
00050 #include <boost/thread.hpp>
00051 #include <boost/foreach.hpp>
00052 #include <boost/lexical_cast.hpp>
00053 #include <boost/optional.hpp>
00054 
00055 namespace flirtlib_ros
00056 {
00057 
00058 namespace sm=sensor_msgs;
00059 namespace gu=occupancy_grid_utils;
00060 namespace vm=visualization_msgs;
00061 namespace gm=geometry_msgs;
00062 namespace mr=mongo_ros;
00063 namespace nm=nav_msgs;
00064 
00065 using std::string;
00066 using std::vector;
00067 using boost::shared_ptr;
00068 
00069 typedef boost::mutex::scoped_lock Lock;
00070 typedef vector<InterestPoint*> InterestPointVec;
00071 typedef std::pair<InterestPoint*, InterestPoint*> Correspondence;
00072 typedef vector<Correspondence> Correspondences;
00073 typedef vector<RefScan> RefScans;
00074 
00075 
00076 /************************************************************
00077  * Node class
00078  ***********************************************************/
00079 
00080 class Node
00081 {
00082 public:
00083 
00084   Node ();
00085   void mainLoop (const ros::TimerEvent& e);
00086   void scanCB (sm::LaserScan::ConstPtr scan);
00087 
00088 private:
00089 
00090   RefScans generateRefScans () const;
00091   void initializeRefScans();
00092   gm::Pose getPose();
00093 
00094   // Needed during initialization
00095   boost::mutex mutex_;
00096   ros::NodeHandle nh_;
00097 
00098   // Parameters
00099   const string scan_db_;
00100   const unsigned num_matches_required_;
00101 
00102   // State
00103   sm::LaserScan::ConstPtr scan_;
00104   RefScans ref_scans_;
00105   boost::optional<gm::Pose> last_pose_;
00106 
00108   shared_ptr<SimpleMinMaxPeakFinder> peak_finder_;
00109   shared_ptr<HistogramDistance<double> > histogram_dist_;
00110   shared_ptr<Detector> detector_;
00111   shared_ptr<DescriptorGenerator> descriptor_;
00112   shared_ptr<RansacFeatureSetMatcher> ransac_;
00113  
00114   // Ros objects
00115   tf::TransformListener tf_;
00116   ros::Subscriber scan_sub_;
00117   ros::Publisher marker_pub_;
00118   ros::Timer exec_timer_;
00119 
00120 };
00121 
00122 
00123 /************************************************************
00124  * Initialization
00125  ***********************************************************/
00126 
00127 template <class T>
00128 T getPrivateParam (const string& name)
00129 {
00130   ros::NodeHandle nh("~");
00131   T val;
00132   const bool found = nh.getParam(name, val);
00133   ROS_ASSERT_MSG (found, "Could not find parameter %s", name.c_str());
00134   ROS_DEBUG_STREAM_NAMED ("init", "Initialized " << name << " to " << val);
00135   return val;
00136 }
00137 
00138 template <class T>
00139 T getPrivateParam (const string& name, const T& default_val)
00140 {
00141   ros::NodeHandle nh("~");
00142   T val;
00143   nh.param(name, val, default_val);
00144   ROS_DEBUG_STREAM_NAMED ("init", "Initialized " << name << " to " << val <<
00145                           "(default was " << default_val << ")");
00146   return val;
00147 }
00148 
00149 sm::LaserScan getScannerParams ()
00150 {
00151   sm::LaserScan params;
00152   const double PI = 3.14159265;
00153   params.angle_min = -PI/2;
00154   params.angle_max = PI/2;
00155   params.angle_increment = PI/180;
00156   params.range_max = 10;
00157 
00158   return params;
00159 }
00160 
00161 SimpleMinMaxPeakFinder* createPeakFinder ()
00162 {
00163   return new SimpleMinMaxPeakFinder(0.34, 0.001);
00164 }
00165 
00166 Detector* createDetector (SimpleMinMaxPeakFinder* peak_finder)
00167 {
00168   const double scale = 5.0;
00169   const double dmst = 2.0;
00170   const double base_sigma = 0.2;
00171   const double sigma_step = 1.4;
00172   CurvatureDetector* det = new CurvatureDetector(peak_finder, scale, base_sigma,
00173                                                  sigma_step, dmst);
00174   det->setUseMaxRange(false);
00175   return det;
00176 }
00177 
00178 DescriptorGenerator* createDescriptor (HistogramDistance<double>* dist)
00179 {
00180   const double min_rho = 0.02;
00181   const double max_rho = 0.5;
00182   const double bin_rho = 4;
00183   const double bin_phi = 12;
00184   BetaGridGenerator* gen = new BetaGridGenerator(min_rho, max_rho, bin_rho,
00185                                                  bin_phi);
00186   gen->setDistanceFunction(dist);
00187   return gen;
00188 }
00189 
00190 Node::Node () :
00191   scan_db_(getPrivateParam<string>("scan_db", "")),
00192   num_matches_required_(getPrivateParam<int>("num_matches_required", 10)),
00193 
00194   peak_finder_(createPeakFinder()),
00195   histogram_dist_(new SymmetricChi2Distance<double>()),
00196   detector_(createDetector(peak_finder_.get())),
00197   descriptor_(createDescriptor(histogram_dist_.get())),
00198   ransac_(new RansacFeatureSetMatcher(0.0599, 0.95, 0.4, 0.4,
00199                                            0.0384, false)),
00200 
00201   scan_sub_(nh_.subscribe("scan0", 10, &Node::scanCB, this)),
00202   marker_pub_(nh_.advertise<vm::Marker>("visualization_marker", 10)),
00203   exec_timer_(nh_.createTimer(ros::Duration(0.2), &Node::mainLoop, this))
00204 {
00205   initializeRefScans();
00206 }
00207 
00208 
00209 /************************************************************
00210  * Creation of reference scans
00211  ***********************************************************/
00212 
00213 RefScans Node::generateRefScans() const
00214 {
00215   // Read parameters from server
00216   const string map_file = getPrivateParam<string>("map_file");
00217   const double resolution = getPrivateParam<double>("resolution");
00218   const sm::LaserScan scanner_params = getScannerParams();
00219   const double ref_pos_resolution = getPrivateParam<double>("ref_pos_resolution");
00220   const double ref_angle_resolution = getPrivateParam<double>("ref_angle_resolution");
00221   const double x0 = getPrivateParam<double>("x0", -1e9);
00222   const double x1 = getPrivateParam<double>("x1", 1e9);
00223   const double y0 = getPrivateParam<double>("y0", -1e9);
00224   const double y1 = getPrivateParam<double>("y1", 1e9);
00225 
00226   // Load grid
00227   nm::OccupancyGrid::ConstPtr grid = gu::loadGrid(map_file, resolution);
00228   const unsigned skip = ref_pos_resolution/grid->info.resolution;
00229 
00230   RefScans ref_scans;
00231 
00232   for (unsigned x=0; x<grid->info.width; x+=skip)
00233   {
00234     for (unsigned y=0; y<grid->info.height; y+=skip)
00235     {
00236       for (double theta=0; theta<6.28; theta+=ref_angle_resolution)
00237       {
00238         // Get position and see if it's within limits
00239         gm::Pose pose;
00240         const gu::Cell c(x,y);
00241         pose.position = gu::cellCenter(grid->info, c);
00242         pose.orientation = tf::createQuaternionMsgFromYaw(theta);
00243         if ((pose.position.x > x1) || (pose.position.x < x0) ||
00244             (pose.position.y > y1) || (pose.position.y < y0))
00245           continue;
00246         if (grid->data[cellIndex(grid->info, c)]!=gu::UNOCCUPIED)
00247           continue;
00248         ROS_INFO_THROTTLE (0.3, "Pos: %.2f, %.2f", pose.position.x,
00249                            pose.position.y);
00250         
00251         // Simulate scan
00252         sm::LaserScan::Ptr scan = gu::simulateRangeScan(*grid, pose, scanner_params, true);
00253 
00254         // Convert to flirtlib and extract features
00255         shared_ptr<LaserReading> reading = fromRos(*scan);
00256         InterestPointVec pts;
00257         detector_->detect(*reading, pts);
00258         BOOST_FOREACH (InterestPoint* p, pts)
00259         {
00260           p->setDescriptor(descriptor_->describe(*p, *reading));
00261         }
00262         
00263         // Save
00264         ref_scans.push_back(RefScan(scan, pose, pts));
00265       }
00266     }
00267   }
00268   ROS_INFO_STREAM ("Generated " << ref_scans.size() << " reference scans");
00269   return ref_scans;
00270 }
00271 
00272 
00273 void Node::initializeRefScans ()
00274 {
00275   if (scan_db_.size()>0)
00276   {
00277     mr::MessageCollection<RefScanRos> coll(scan_db_, "scans");
00278     if (coll.count() == 0)
00279     {
00280       ROS_INFO ("Didn't find any messages in %s/scans, so generating features.",
00281                 scan_db_.c_str());
00282       ref_scans_ = generateRefScans();
00283       ROS_INFO ("Saving scans");
00284       BOOST_FOREACH (const RefScan& scan, ref_scans_) 
00285         coll.insert(toRos(scan), mr::Metadata("x", scan.pose.position.x,
00286                                               "y", scan.pose.position.y));
00287     }
00288     else
00289     {
00290       ROS_INFO ("Loading scans from %s/scans", scan_db_.c_str());
00291       ROS_ASSERT(ref_scans_.size()==0);
00292       BOOST_FOREACH (const RefScanRos::ConstPtr m, coll.queryResults(mr::Query(), false))
00293         ref_scans_.push_back(fromRos(*m));
00294     }
00295   }
00296   else
00297   {
00298     ROS_INFO ("No db name provided; generating features from scratch.");
00299     ref_scans_ = generateRefScans();
00300   }
00301 }
00302 
00303 
00304 
00305 /************************************************************
00306  * Visualization
00307  ***********************************************************/
00308 
00309 
00310 /************************************************************
00311  * Callbacks
00312  ***********************************************************/
00313 
00314 
00315 void Node::scanCB (sm::LaserScan::ConstPtr scan)
00316 {
00317   Lock l(mutex_);
00318   scan_ = scan;
00319 }
00320 
00321 
00322 /************************************************************
00323  * Main loop
00324  ***********************************************************/
00325 
00326 
00327 gm::Pose Node::getPose ()
00328 {
00329   tf::StampedTransform trans;
00330   tf_.lookupTransform("/map", "pose0", ros::Time(), trans);
00331   gm::Pose pose;
00332   tf::poseTFToMsg(trans, pose);
00333   return pose;
00334 }
00335 
00336 bool closeTo (const gm::Pose& p1, const gm::Pose& p2)
00337 {
00338   const double TOL=1e-3;
00339   const double dx = p1.position.x - p2.position.x;
00340   const double dy = p1.position.y - p2.position.y;
00341   const double dt = tf::getYaw(p1.orientation) - tf::getYaw(p2.orientation);
00342   return fabs(dx)<TOL && fabs(dy)<TOL && fabs(dt)<TOL;
00343 }
00344 
00345 void Node::mainLoop (const ros::TimerEvent& e)
00346 {
00347   try
00348   {
00349     // Getting pose is the part that can throw exceptions
00350     const gm::Pose current_pose = getPose();
00351     const double theta = tf::getYaw(current_pose.orientation);
00352     const double x=current_pose.position.x;
00353     const double y=current_pose.position.y;
00354 
00355     Lock l(mutex_);
00356     if (last_pose_ && closeTo(*last_pose_, current_pose))
00357       return;
00358     if (!scan_)
00359       return;
00360 
00361     ROS_INFO("Matching scan at %.2f, %.2f, %.2f", x, y, theta);
00362     // Extract features for this scan
00363     InterestPointVec pts;
00364     shared_ptr<LaserReading> reading = fromRos(*scan_);
00365     detector_->detect(*reading, pts);
00366     BOOST_FOREACH (InterestPoint* p, pts) 
00367       p->setDescriptor(descriptor_->describe(*p, *reading));
00368     marker_pub_.publish(interestPointMarkers(pts, current_pose, 0));
00369 
00370     // Match
00371     vector<gm::Pose> match_poses;
00372     BOOST_FOREACH (const RefScan& ref_scan, ref_scans_) 
00373     {
00374       Correspondences matches;
00375       OrientedPoint2D transform;
00376       ransac_->matchSets(ref_scan.raw_pts, pts, transform, matches);
00377       if (matches.size() > num_matches_required_)
00378       {
00379         ROS_INFO ("Found %zu matches with ref scan at %.2f, %.2f, %.2f.  "
00380                   "Current pose is %.2f, %.2f, %.2f.", matches.size(),
00381                   ref_scan.pose.position.x, ref_scan.pose.position.y,
00382                   tf::getYaw(ref_scan.pose.orientation), x, y, theta);
00383         match_poses.push_back(ref_scan.pose);
00384       }
00385     }
00386     ROS_INFO ("Done matching");
00387 
00388     BOOST_FOREACH (const vm::Marker& m, poseMarkers(match_poses))
00389       marker_pub_.publish(m);
00390     last_pose_ = current_pose;
00391   }
00392 
00393   catch (tf::TransformException& e)
00394   {
00395     ROS_INFO ("Skipping because of tf exception");
00396   }
00397 }
00398 
00399 
00400 
00401 
00402 } // namespace
00403 
00404 int main (int argc, char** argv)
00405 {
00406   ros::init(argc, argv, "place_rec_test");
00407   flirtlib_ros::Node node;
00408   ros::spin();
00409   return 0;
00410 }
place_rec_test.cpp
