karto_scan_matcher.cpp
Go to the documentation of this file.
00001 /*********************************************************/
00009 #include <karto_scan_matcher/karto_scan_matcher.h>
00010 #include <tf/transform_datatypes.h>
00011 #include <string>
00012 #include <vector>
00013 #include <boost/bind.hpp>
00014 #include <boost/foreach.hpp>
00015 #include <boost/lexical_cast.hpp>
00016 #include <boost/thread.hpp>
00017 /*
00018 #include <pcl_ros/point_cloud.h>
00019 #include <pcl/point_types.h>
00020 */
00021 
00022 namespace karto_scan_matcher
00023 {
00024 
00025 namespace sm=sensor_msgs;
00026 namespace gm=geometry_msgs;
00027 using std::string;
00028 using std::vector;
00029 using boost::bind;
00030 using boost::lexical_cast;
00031 
00032 //typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
00033 
00034 // Since Karto has a global namespace for sensors, we need to autogenerate unique ones of these
00035 static boost::mutex static_name_mutex;
00036 static unsigned name_suffix;
00037 
00038 
00039 /************************************************************
00040  * Conversion
00041  ************************************************************/
00042 
00043 karto::LocalizedRangeScan* convertLaserScan (karto::LaserRangeFinder* laser, const sm::LaserScan& scan, 
00044                                              const gm::Pose2D& pose)
00045 {
00046   // Convert to Karto
00047   karto::Pose2 karto_pose(pose.x, pose.y, pose.theta);
00048   vector<kt_double> readings(scan.ranges.begin(), scan.ranges.end());
00049 
00050   karto::LocalizedRangeScan* localized = new karto::LocalizedRangeScan(laser->GetName(), readings);
00051   localized->SetOdometricPose(karto_pose);
00052   localized->SetCorrectedPose(karto_pose);
00053   return localized;
00054 }
00055 
00056 karto::LocalizedRangeScan* convertLaserScan(karto::LaserRangeFinder* laser, const ScanWithPose& scan)
00057 {
00058   return convertLaserScan(laser, scan.scan, scan.pose);
00059 }
00060 
00061 
00062 /************************************************************
00063  * Initialization 
00064  ************************************************************/
00065 
00066 KartoScanMatcher::KartoScanMatcher (const sm::LaserScan& init_scan, const tf::TransformListener& tf,
00067                                     const double search_space_size, const double search_grid_resolution) 
00068 {
00069   // Get the laser's pose, relative to base.
00070   tf::Stamped<tf::Pose> ident;
00071   tf::Stamped<tf::Pose> laser_to_base;
00072   ident.setIdentity();
00073   ident.frame_id_ = init_scan.header.frame_id;
00074   ident.stamp_ = ros::Time();
00075   
00076   if (!tf.waitForTransform("base_link", ident.frame_id_, ros::Time::now(), ros::Duration(10.0))) {
00077     ROS_FATAL_STREAM ("Didn't get a transform between base_link and " << ident.frame_id_ <<
00078                       " in time during KartoScanMatcher initialization");
00079     ROS_BREAK();
00080   }
00081   
00082   tf.transformPose("base_link", ident, laser_to_base);
00083 
00084   gm::Pose2D laser_pose;
00085   laser_pose.x = laser_to_base.getOrigin().x();
00086   laser_pose.y = laser_to_base.getOrigin().y();
00087   laser_pose.theta = tf::getYaw(laser_to_base.getRotation());
00088 
00089   vector<double> search_sizes;
00090   search_sizes.push_back(search_space_size);
00091   vector<double> search_resolutions;
00092   search_resolutions.push_back(search_grid_resolution);  
00093   initialize(init_scan, laser_pose, search_sizes, search_resolutions);
00094 }
00095 
00096 KartoScanMatcher::KartoScanMatcher (const sm::LaserScan& init_scan, const gm::Pose2D& laser_pose,
00097                                     const double search_space_size, const double search_grid_resolution)
00098 {
00099   DoubleVector search_sizes;
00100   search_sizes.push_back(search_space_size);
00101   DoubleVector search_resolutions;
00102   search_resolutions.push_back(search_grid_resolution);  
00103   initialize(init_scan, laser_pose, search_sizes, search_resolutions);
00104 }
00105 
00106 KartoScanMatcher::KartoScanMatcher (const sensor_msgs::LaserScan& init_scan, const geometry_msgs::Pose2D& laser_pose,
00107                                     const DoubleVector& search_sizes, const DoubleVector& search_resolutions)
00108 {
00109   initialize(init_scan, laser_pose, search_sizes, search_resolutions);
00110 }
00111 
00112 void KartoScanMatcher::initialize (const sm::LaserScan& init_scan, const gm::Pose2D& laser_pose,
00113                                    const DoubleVector& search_sizes, const DoubleVector& search_resolutions)
00114 {
00115   penalize_distance_ = true;
00116   laser_to_base_ = btTransform(tf::createQuaternionFromYaw(laser_pose.theta).asBt(),
00117                                btVector3(laser_pose.x, laser_pose.y, 0.0));
00118   string suffix;
00119   {
00120     boost::mutex::scoped_lock l(static_name_mutex);
00121     suffix =  boost::lexical_cast<string>(name_suffix++);
00122   }
00123   
00124   ROS_DEBUG_NAMED ("karto", "Initializing scan matcher(s) using scan %u", init_scan.header.seq);
00125   dataset_.reset(new karto::Dataset());
00126   const size_t num_matchers = search_sizes.size();
00127   if (num_matchers == 0)
00128     ROS_WARN_NAMED ("karto", "Zero sets of matcher parameters given to KartoScanMatcher: almost surely an error");
00129   ROS_ASSERT (search_resolutions.size() == num_matchers);
00130   matchers_.resize(num_matchers);
00131   mappers_.resize(num_matchers);
00132   for (size_t i=0; i<num_matchers; i++) {
00133     if (i>0) {
00134       if (search_sizes[i] >= search_sizes[i-1])
00135         ROS_WARN_NAMED ("karto", "Matcher %zu search size was %f and %zu was %f, which look out of order",
00136                         i-1, search_sizes[i-1], i, search_sizes[i]);
00137       if (search_resolutions[i] >= search_resolutions[i-1])
00138         ROS_WARN_NAMED ("karto", "Matcher %zu search resolution was %f and %zu was %f, which look out of order",
00139                         i-1, search_resolutions[i-1], i, search_sizes[i]);
00140     }
00141     mappers_[i].reset(new karto::Mapper());
00142     matchers_[i].reset(karto::ScanMatcher::Create(mappers_[i].get(), search_sizes[i], search_resolutions[i],
00143                                                   DEFAULT_SMEAR_DEVIATION, DEFAULT_RANGE_THRESHOLD));
00144     ROS_DEBUG_NAMED ("karto", "Search space size for matcher %zu is %.2f and resolution is %.2f",
00145                      i, search_sizes[i], search_resolutions[i]);
00146   }
00147 
00148 
00149   // This is copied over from slam_karto.cpp
00150   
00151   // Create a laser range finder device and copy in data from the first scan
00152   string name = init_scan.header.frame_id;
00153   laser_ = karto::LaserRangeFinder::CreateLaserRangeFinder
00154     (karto::LaserRangeFinder_Custom, karto::Name(name+suffix));
00155   laser_->SetOffsetPose(karto::Pose2(laser_pose.x, laser_pose.y,
00156                                      laser_pose.theta));
00157   laser_->SetMinimumRange(init_scan.range_min);
00158   laser_->SetMaximumRange(init_scan.range_max);
00159   laser_->SetMinimumAngle(init_scan.angle_min);
00160   laser_->SetMaximumAngle(init_scan.angle_max);
00161   laser_->SetAngularResolution(init_scan.angle_increment);
00162   // TODO: expose this, and many other parameters
00163   //laser_->SetRangeThreshold(12.0);
00164   dataset_->Add(laser_);
00165 }
00166 
00167 void KartoScanMatcher::setPenalizeDistance (const bool penalize)
00168 {
00169   penalize_distance_ = penalize;
00170 }
00171   
00172 /************************************************************
00173  * Scan matching
00174  ************************************************************/
00175 
00176 gm::Pose2D subtractLaserOffset (const karto::Pose2& pose, const karto::Pose2& offset)
00177 {
00178   btTransform laser_to_base(tf::createQuaternionFromYaw(offset.GetHeading()).asBt(),
00179                             btVector3(offset.GetX(), offset.GetY(), 0.0));
00180   btTransform laser_to_map(tf::createQuaternionFromYaw(pose.GetHeading()).asBt(),
00181                            btVector3(pose.GetX(), pose.GetY(), 0.0));
00182   btTransform base_to_map = laser_to_map*laser_to_base.inverse();
00183   gm::Pose2D result;
00184   result.x = base_to_map.getOrigin().x();
00185   result.y = base_to_map.getOrigin().y();
00186   result.theta = tf::getYaw(base_to_map.getRotation());
00187   return result;
00188 }
00189 
00190 typedef boost::shared_ptr<karto::LocalizedRangeScan> ScanPtr;
00191 ScanPtr wrapBareScanPtr (karto::LocalizedRangeScan* scan)
00192 {
00193   return ScanPtr(scan);
00194 }
00195 
00196 
00197 ScanMatchResult KartoScanMatcher::scanMatch (const sm::LaserScan& scan, const gm::Pose2D& pose, 
00198                                         const vector<ScanWithPose>& reference_scans) const
00199 {
00200   ROS_DEBUG_NAMED ("karto", "Converting to Karto and matching");
00201   
00202   karto::LocalizedRangeScanVector localized_reference_scans;
00203   transform(reference_scans.begin(), reference_scans.end(), back_inserter(localized_reference_scans),
00204             bind(convertLaserScan, laser_, _1));
00205 
00206   // RAII objects to ensure the above pointers get freed at end of this scope
00207   vector<ScanPtr> scan_ptrs(localized_reference_scans.size());
00208   transform(localized_reference_scans.begin(), localized_reference_scans.end(),
00209             scan_ptrs.begin(), wrapBareScanPtr);
00210 
00211   gm::Pose2D current_estimate = pose;
00212   double last_response = -42.42;
00213   Eigen::Matrix3f covariance = Eigen::Matrix3f::Zero();
00214 
00215   // Repeatedly match with each matcher, using the previous matcher's estimate to initialize
00216   BOOST_FOREACH (const MatcherPtr matcher, matchers_) {
00217 
00218     karto::LocalizedRangeScan* localized_scan = convertLaserScan(laser_, scan, current_estimate);
00219 
00220     // To make sure it gets freed we wrap it in a shared pointer
00221     // In later versions of Karto, the 'dataset' object is being used to memory-manage such pointers
00222     // I'm keeping it this way for now because the dataset only frees things when it goes out of scope
00223     // I could make it local to this function, but then it'd have to reinitialize the laser scan object
00224     // every time.  There seems to be no other effect of adding a laser scan to a dataset, so this should
00225     // be fine.
00226     ScanPtr scan_ptr(localized_scan); 
00227     karto::Pose2 mean;
00228     karto::Matrix3 cov;
00229 
00230     const bool refine_match = true;
00231     ROS_DEBUG_NAMED ("karto", "  Current estimate is %.2f, %.2f, %.2f, Calling scan matcher",
00232                      current_estimate.x, current_estimate.y, current_estimate.theta);
00233     last_response = matcher->MatchScan(localized_scan, localized_reference_scans, mean,
00234                                        cov, penalize_distance_, refine_match);
00235     current_estimate = subtractLaserOffset(mean, laser_->GetOffsetPose());
00236     for (unsigned i=0; i<3; i++) {
00237       for (unsigned j=0; j<3; j++) {
00238         covariance(i,j) = cov(i,j);
00239       }
00240     }
00241     ROS_DEBUG_NAMED ("karto", "  Response was %.4f.", last_response);
00242   }
00243 
00244   if (vis_pub_)
00245     visualizeResult(scan, pose, reference_scans, current_estimate);
00246   
00247   ROS_DEBUG_NAMED ("karto", "Returning result %.2f, %.2f, %.2f with covariances (x-x: %.2f, y-y: %.2f, x-y: %.2f, th-th: %.2f)",
00248                    current_estimate.x, current_estimate.y, current_estimate.theta, covariance(0,0),
00249                    covariance(1,1), covariance(0,1), covariance(2,2));
00250 
00251   return ScanMatchResult(current_estimate, covariance, last_response);
00252 }
00253 
00254 /************************************************************
00255  * Visualization
00256  ***********************************************************/
00257 
00258 void KartoScanMatcher::setVisualizationPublisher (const std::string& topic,
00259                                                   const std::string& frame)
00260 {
00261 
00262   //nh_ = ros::NodeHandle();
00263   /*
00264   vis_pub_ = nh_->advertise<PointCloud>(topic, 10);
00265   vis_frame_ = frame;
00266   */
00267 }
00268 /*
00269 void addPoints (PointCloud* cloud, const gm::Pose2D& pose,
00270                 const sm::LaserScan& scan, const char r,
00271                 const char g, const char b,
00272                 const btTransform& laser_to_base)
00273 {
00274   double theta = scan.angle_min;
00275   BOOST_FOREACH (const double range, scan.ranges)
00276   {
00277     if ((scan.range_min < range) && (range < scan.range_max))
00278     {
00279       // Point in laser frame
00280       btVector3 p(range*cos(theta), range*sin(theta), 0);
00281       btVector3 p2 = laser_to_base*p;
00282       btVector3 p3 = btTransform(tf::createQuaternionFromYaw(pose.theta),
00283                                  btVector3(pose.x, pose.y, 0))*p2;
00284       pcl::PointXYZRGB pt;
00285       pt.x = p3.x();
00286       pt.y = p3.y();
00287       pt.z = p3.z();
00288       int rgb=0;
00289       rgb |= (r<<16);
00290       rgb |= (g<<8);
00291       rgb |= b;
00292       pt.rgb = *(float*)&rgb;
00293       cloud->points.push_back(pt);
00294     }
00295     theta += scan.angle_increment;
00296   }
00297 }
00298 */
00299 
00300 void KartoScanMatcher::visualizeResult (const sm::LaserScan& scan,
00301                                         const gm::Pose2D& pose, 
00302                                         const vector<ScanWithPose>& ref,
00303                                         const gm::Pose2D& corrected) const
00304 {
00305   /*
00306   PointCloud cloud;
00307 
00308   // Input initial estimate
00309   addPoints(&cloud, pose, scan, 1, 0, 0, laser_to_base_);
00310 
00311   // Corrected estimate
00312   addPoints(&cloud, corrected, scan, 0, 1, 0, laser_to_base_);
00313 
00314   // Reference scans
00315   BOOST_FOREACH (const ScanWithPose& ref_scan, ref)
00316     addPoints(&cloud, ref_scan.pose, ref_scan.scan, 0, 0, 1, laser_to_base_);
00317 
00318   // Set cloud params and publish
00319   cloud.header.frame_id = vis_frame_;
00320   cloud.header.stamp = ros::Time::now();
00321   cloud.height = 1;
00322   cloud.width = cloud.points.size();
00323   vis_pub_->publish(cloud);
00324   */
00325 }
00326 
00327 
00328 } // namespace karto_scan_matcher


karto_scan_matcher
Author(s): Bhaskara Marthi
autogenerated on Thu Jan 2 2014 12:02:42