laser_scan_matcher.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Ivan Dryanovski, William Morris
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 CCNY Robotics Lab 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 /*  This package uses Canonical Scan Matcher [1], written by
00031  *  Andrea Censi
00032  *
00033  *  [1] A. Censi, "An ICP variant using a point-to-line metric"
00034  *  Proceedings of the IEEE International Conference
00035  *  on Robotics and Automation (ICRA), 2008
00036  */
00037 
00038 #include <laser_scan_matcher/laser_scan_matcher.h>
00039 #include <pcl_conversions/pcl_conversions.h>
00040 #include <boost/assign.hpp>
00041 
00042 namespace scan_tools
00043 {
00044 
00045 LaserScanMatcher::LaserScanMatcher(ros::NodeHandle nh, ros::NodeHandle nh_private):
00046   nh_(nh),
00047   nh_private_(nh_private),
00048   initialized_(false),
00049   received_imu_(false),
00050   received_odom_(false),
00051   received_vel_(false)
00052 {
00053   ROS_INFO("Starting LaserScanMatcher");
00054 
00055   // **** init parameters
00056 
00057   initParams();
00058 
00059   // **** state variables
00060 
00061   f2b_.setIdentity();
00062   f2b_kf_.setIdentity();
00063   input_.laser[0] = 0.0;
00064   input_.laser[1] = 0.0;
00065   input_.laser[2] = 0.0;
00066 
00067   // Initialize output_ vectors as Null for error-checking
00068   output_.cov_x_m = 0;
00069   output_.dx_dy1_m = 0;
00070   output_.dx_dy2_m = 0;
00071 
00072   // **** publishers
00073 
00074   if (publish_pose_)
00075   {
00076     pose_publisher_  = nh_.advertise<geometry_msgs::Pose2D>(
00077       "pose2D", 5);
00078   }
00079 
00080   if (publish_pose_stamped_)
00081   {
00082     pose_stamped_publisher_ = nh_.advertise<geometry_msgs::PoseStamped>(
00083       "pose_stamped", 5);
00084   }
00085 
00086   if (publish_pose_with_covariance_)
00087   {
00088     pose_with_covariance_publisher_  = nh_.advertise<geometry_msgs::PoseWithCovariance>(
00089       "pose_with_covariance", 5);
00090   }
00091 
00092   if (publish_pose_with_covariance_stamped_)
00093   {
00094     pose_with_covariance_stamped_publisher_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(
00095       "pose_with_covariance_stamped", 5);
00096   }
00097 
00098   // *** subscribers
00099 
00100   if (use_cloud_input_)
00101   {
00102     cloud_subscriber_ = nh_.subscribe(
00103       "cloud", 1, &LaserScanMatcher::cloudCallback, this);
00104   }
00105   else
00106   {
00107     scan_subscriber_ = nh_.subscribe(
00108       "scan", 1, &LaserScanMatcher::scanCallback, this);
00109   }
00110 
00111   if (use_imu_)
00112   {
00113     imu_subscriber_ = nh_.subscribe(
00114       "imu/data", 1, &LaserScanMatcher::imuCallback, this);
00115   }
00116   if (use_odom_)
00117   {
00118     odom_subscriber_ = nh_.subscribe(
00119       "odom", 1, &LaserScanMatcher::odomCallback, this);
00120   }
00121   if (use_vel_)
00122   {
00123     if (stamped_vel_)
00124       vel_subscriber_ = nh_.subscribe(
00125         "vel", 1, &LaserScanMatcher::velStmpCallback, this);
00126     else
00127       vel_subscriber_ = nh_.subscribe(
00128         "vel", 1, &LaserScanMatcher::velCallback, this);
00129   }
00130 }
00131 
00132 LaserScanMatcher::~LaserScanMatcher()
00133 {
00134   ROS_INFO("Destroying LaserScanMatcher");
00135 }
00136 
00137 void LaserScanMatcher::initParams()
00138 {
00139   if (!nh_private_.getParam ("base_frame", base_frame_))
00140     base_frame_ = "base_link";
00141   if (!nh_private_.getParam ("fixed_frame", fixed_frame_))
00142     fixed_frame_ = "world";
00143 
00144   // **** input type - laser scan, or point clouds?
00145   // if false, will subscribe to LaserScan msgs on /scan.
00146   // if true, will subscribe to PointCloud2 msgs on /cloud
00147 
00148   if (!nh_private_.getParam ("use_cloud_input", use_cloud_input_))
00149     use_cloud_input_= false;
00150 
00151   if (use_cloud_input_)
00152   {
00153     if (!nh_private_.getParam ("cloud_range_min", cloud_range_min_))
00154       cloud_range_min_ = 0.1;
00155     if (!nh_private_.getParam ("cloud_range_max", cloud_range_max_))
00156       cloud_range_max_ = 50.0;
00157     if (!nh_private_.getParam ("cloud_res", cloud_res_))
00158       cloud_res_ = 0.05;
00159 
00160     input_.min_reading = cloud_range_min_;
00161     input_.max_reading = cloud_range_max_;
00162   }
00163 
00164   // **** keyframe params: when to generate the keyframe scan
00165   // if either is set to 0, reduces to frame-to-frame matching
00166 
00167   if (!nh_private_.getParam ("kf_dist_linear", kf_dist_linear_))
00168     kf_dist_linear_ = 0.10;
00169   if (!nh_private_.getParam ("kf_dist_angular", kf_dist_angular_))
00170     kf_dist_angular_ = 10.0 * (M_PI / 180.0);
00171 
00172   kf_dist_linear_sq_ = kf_dist_linear_ * kf_dist_linear_;
00173 
00174   // **** What predictions are available to speed up the ICP?
00175   // 1) imu - [theta] from imu yaw angle - /imu topic
00176   // 2) odom - [x, y, theta] from wheel odometry - /odom topic
00177   // 3) vel - [x, y, theta] from velocity predictor - see alpha-beta predictors - /vel topic
00178   // If more than one is enabled, priority is imu > odom > vel
00179 
00180   if (!nh_private_.getParam ("use_imu", use_imu_))
00181     use_imu_ = true;
00182   if (!nh_private_.getParam ("use_odom", use_odom_))
00183     use_odom_ = true;
00184   if (!nh_private_.getParam ("use_vel", use_vel_))
00185     use_vel_ = false;
00186 
00187   // **** Are velocity input messages stamped?
00188   // if false, will subscribe to Twist msgs on /vel
00189   // if true, will subscribe to TwistStamped msgs on /vel
00190   if (!nh_private_.getParam ("stamped_vel", stamped_vel_))
00191     stamped_vel_ = false;
00192 
00193   // **** How to publish the output?
00194   // tf (fixed_frame->base_frame),
00195   // pose message (pose of base frame in the fixed frame)
00196 
00197   if (!nh_private_.getParam ("publish_tf", publish_tf_))
00198     publish_tf_ = true;
00199   if (!nh_private_.getParam ("publish_pose", publish_pose_))
00200     publish_pose_ = true;
00201   if (!nh_private_.getParam ("publish_pose_stamped", publish_pose_stamped_))
00202     publish_pose_stamped_ = false;
00203   if (!nh_private_.getParam ("publish_pose_with_covariance", publish_pose_with_covariance_))
00204     publish_pose_with_covariance_ = false;
00205   if (!nh_private_.getParam ("publish_pose_with_covariance_stamped", publish_pose_with_covariance_stamped_))
00206     publish_pose_with_covariance_stamped_ = false;
00207 
00208   if (!nh_private_.getParam("position_covariance", position_covariance_))
00209   {
00210     position_covariance_.resize(3);
00211     std::fill(position_covariance_.begin(), position_covariance_.end(), 1e-9);
00212   }
00213 
00214   if (!nh_private_.getParam("orientation_covariance", orientation_covariance_))
00215   {
00216     orientation_covariance_.resize(3);
00217     std::fill(orientation_covariance_.begin(), orientation_covariance_.end(), 1e-9);
00218   }
00219   // **** CSM parameters - comments copied from algos.h (by Andrea Censi)
00220 
00221   // Maximum angular displacement between scans
00222   if (!nh_private_.getParam ("max_angular_correction_deg", input_.max_angular_correction_deg))
00223     input_.max_angular_correction_deg = 45.0;
00224 
00225   // Maximum translation between scans (m)
00226   if (!nh_private_.getParam ("max_linear_correction", input_.max_linear_correction))
00227     input_.max_linear_correction = 0.50;
00228 
00229   // Maximum ICP cycle iterations
00230   if (!nh_private_.getParam ("max_iterations", input_.max_iterations))
00231     input_.max_iterations = 10;
00232 
00233   // A threshold for stopping (m)
00234   if (!nh_private_.getParam ("epsilon_xy", input_.epsilon_xy))
00235     input_.epsilon_xy = 0.000001;
00236 
00237   // A threshold for stopping (rad)
00238   if (!nh_private_.getParam ("epsilon_theta", input_.epsilon_theta))
00239     input_.epsilon_theta = 0.000001;
00240 
00241   // Maximum distance for a correspondence to be valid
00242   if (!nh_private_.getParam ("max_correspondence_dist", input_.max_correspondence_dist))
00243     input_.max_correspondence_dist = 0.3;
00244 
00245   // Noise in the scan (m)
00246   if (!nh_private_.getParam ("sigma", input_.sigma))
00247     input_.sigma = 0.010;
00248 
00249   // Use smart tricks for finding correspondences.
00250   if (!nh_private_.getParam ("use_corr_tricks", input_.use_corr_tricks))
00251     input_.use_corr_tricks = 1;
00252 
00253   // Restart: Restart if error is over threshold
00254   if (!nh_private_.getParam ("restart", input_.restart))
00255     input_.restart = 0;
00256 
00257   // Restart: Threshold for restarting
00258   if (!nh_private_.getParam ("restart_threshold_mean_error", input_.restart_threshold_mean_error))
00259     input_.restart_threshold_mean_error = 0.01;
00260 
00261   // Restart: displacement for restarting. (m)
00262   if (!nh_private_.getParam ("restart_dt", input_.restart_dt))
00263     input_.restart_dt = 1.0;
00264 
00265   // Restart: displacement for restarting. (rad)
00266   if (!nh_private_.getParam ("restart_dtheta", input_.restart_dtheta))
00267     input_.restart_dtheta = 0.1;
00268 
00269   // Max distance for staying in the same clustering
00270   if (!nh_private_.getParam ("clustering_threshold", input_.clustering_threshold))
00271     input_.clustering_threshold = 0.25;
00272 
00273   // Number of neighbour rays used to estimate the orientation
00274   if (!nh_private_.getParam ("orientation_neighbourhood", input_.orientation_neighbourhood))
00275     input_.orientation_neighbourhood = 20;
00276 
00277   // If 0, it's vanilla ICP
00278   if (!nh_private_.getParam ("use_point_to_line_distance", input_.use_point_to_line_distance))
00279     input_.use_point_to_line_distance = 1;
00280 
00281   // Discard correspondences based on the angles
00282   if (!nh_private_.getParam ("do_alpha_test", input_.do_alpha_test))
00283     input_.do_alpha_test = 0;
00284 
00285   // Discard correspondences based on the angles - threshold angle, in degrees
00286   if (!nh_private_.getParam ("do_alpha_test_thresholdDeg", input_.do_alpha_test_thresholdDeg))
00287     input_.do_alpha_test_thresholdDeg = 20.0;
00288 
00289   // Percentage of correspondences to consider: if 0.9,
00290   // always discard the top 10% of correspondences with more error
00291   if (!nh_private_.getParam ("outliers_maxPerc", input_.outliers_maxPerc))
00292     input_.outliers_maxPerc = 0.90;
00293 
00294   // Parameters describing a simple adaptive algorithm for discarding.
00295   //  1) Order the errors.
00296   //  2) Choose the percentile according to outliers_adaptive_order.
00297   //     (if it is 0.7, get the 70% percentile)
00298   //  3) Define an adaptive threshold multiplying outliers_adaptive_mult
00299   //     with the value of the error at the chosen percentile.
00300   //  4) Discard correspondences over the threshold.
00301   //  This is useful to be conservative; yet remove the biggest errors.
00302   if (!nh_private_.getParam ("outliers_adaptive_order", input_.outliers_adaptive_order))
00303     input_.outliers_adaptive_order = 0.7;
00304 
00305   if (!nh_private_.getParam ("outliers_adaptive_mult", input_.outliers_adaptive_mult))
00306     input_.outliers_adaptive_mult = 2.0;
00307 
00308   // If you already have a guess of the solution, you can compute the polar angle
00309   // of the points of one scan in the new position. If the polar angle is not a monotone
00310   // function of the readings index, it means that the surface is not visible in the
00311   // next position. If it is not visible, then we don't use it for matching.
00312   if (!nh_private_.getParam ("do_visibility_test", input_.do_visibility_test))
00313     input_.do_visibility_test = 0;
00314 
00315   // no two points in laser_sens can have the same corr.
00316   if (!nh_private_.getParam ("outliers_remove_doubles", input_.outliers_remove_doubles))
00317     input_.outliers_remove_doubles = 1;
00318 
00319   // If 1, computes the covariance of ICP using the method http://purl.org/censi/2006/icpcov
00320   if (!nh_private_.getParam ("do_compute_covariance", input_.do_compute_covariance))
00321     input_.do_compute_covariance = 0;
00322 
00323   // Checks that find_correspondences_tricks gives the right answer
00324   if (!nh_private_.getParam ("debug_verify_tricks", input_.debug_verify_tricks))
00325     input_.debug_verify_tricks = 0;
00326 
00327   // If 1, the field 'true_alpha' (or 'alpha') in the first scan is used to compute the
00328   // incidence beta, and the factor (1/cos^2(beta)) used to weight the correspondence.");
00329   if (!nh_private_.getParam ("use_ml_weights", input_.use_ml_weights))
00330     input_.use_ml_weights = 0;
00331 
00332   // If 1, the field 'readings_sigma' in the second scan is used to weight the
00333   // correspondence by 1/sigma^2
00334   if (!nh_private_.getParam ("use_sigma_weights", input_.use_sigma_weights))
00335     input_.use_sigma_weights = 0;
00336 }
00337 
00338 void LaserScanMatcher::imuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg)
00339 {
00340   boost::mutex::scoped_lock(mutex_);
00341   latest_imu_msg_ = *imu_msg;
00342   if (!received_imu_)
00343   {
00344     last_used_imu_msg_ = *imu_msg;
00345     received_imu_ = true;
00346   }
00347 }
00348 
00349 void LaserScanMatcher::odomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg)
00350 {
00351   boost::mutex::scoped_lock(mutex_);
00352   latest_odom_msg_ = *odom_msg;
00353   if (!received_odom_)
00354   {
00355     last_used_odom_msg_ = *odom_msg;
00356     received_odom_ = true;
00357   }
00358 }
00359 
00360 void LaserScanMatcher::velCallback(const geometry_msgs::Twist::ConstPtr& twist_msg)
00361 {
00362   boost::mutex::scoped_lock(mutex_);
00363   latest_vel_msg_ = *twist_msg;
00364 
00365   received_vel_ = true;
00366 }
00367 
00368 void LaserScanMatcher::velStmpCallback(const geometry_msgs::TwistStamped::ConstPtr& twist_msg)
00369 {
00370   boost::mutex::scoped_lock(mutex_);
00371   latest_vel_msg_ = twist_msg->twist;
00372 
00373   received_vel_ = true;
00374 }
00375 
00376 void LaserScanMatcher::cloudCallback (const PointCloudT::ConstPtr& cloud)
00377 {
00378   // **** if first scan, cache the tf from base to the scanner
00379 
00380   std_msgs::Header cloud_header = pcl_conversions::fromPCL(cloud->header);
00381 
00382   if (!initialized_)
00383   {
00384     // cache the static tf from base to laser
00385     if (!getBaseToLaserTf(cloud_header.frame_id))
00386     {
00387       ROS_WARN("Skipping scan");
00388       return;
00389     }
00390 
00391     PointCloudToLDP(cloud, prev_ldp_scan_);
00392     last_icp_time_ = cloud_header.stamp;
00393     initialized_ = true;
00394   }
00395 
00396   LDP curr_ldp_scan;
00397   PointCloudToLDP(cloud, curr_ldp_scan);
00398   processScan(curr_ldp_scan, cloud_header.stamp);
00399 }
00400 
00401 void LaserScanMatcher::scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
00402 {
00403   // **** if first scan, cache the tf from base to the scanner
00404 
00405   if (!initialized_)
00406   {
00407     createCache(scan_msg);    // caches the sin and cos of all angles
00408 
00409     // cache the static tf from base to laser
00410     if (!getBaseToLaserTf(scan_msg->header.frame_id))
00411     {
00412       ROS_WARN("Skipping scan");
00413       return;
00414     }
00415 
00416     laserScanToLDP(scan_msg, prev_ldp_scan_);
00417     last_icp_time_ = scan_msg->header.stamp;
00418     initialized_ = true;
00419   }
00420 
00421   LDP curr_ldp_scan;
00422   laserScanToLDP(scan_msg, curr_ldp_scan);
00423   processScan(curr_ldp_scan, scan_msg->header.stamp);
00424 }
00425 
00426 void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time)
00427 {
00428   ros::WallTime start = ros::WallTime::now();
00429 
00430   // CSM is used in the following way:
00431   // The scans are always in the laser frame
00432   // The reference scan (prevLDPcan_) has a pose of [0, 0, 0]
00433   // The new scan (currLDPScan) has a pose equal to the movement
00434   // of the laser in the laser frame since the last scan
00435   // The computed correction is then propagated using the tf machinery
00436 
00437   prev_ldp_scan_->odometry[0] = 0.0;
00438   prev_ldp_scan_->odometry[1] = 0.0;
00439   prev_ldp_scan_->odometry[2] = 0.0;
00440 
00441   prev_ldp_scan_->estimate[0] = 0.0;
00442   prev_ldp_scan_->estimate[1] = 0.0;
00443   prev_ldp_scan_->estimate[2] = 0.0;
00444 
00445   prev_ldp_scan_->true_pose[0] = 0.0;
00446   prev_ldp_scan_->true_pose[1] = 0.0;
00447   prev_ldp_scan_->true_pose[2] = 0.0;
00448 
00449   input_.laser_ref  = prev_ldp_scan_;
00450   input_.laser_sens = curr_ldp_scan;
00451 
00452   // **** estimated change since last scan
00453 
00454   double dt = (time - last_icp_time_).toSec();
00455   double pr_ch_x, pr_ch_y, pr_ch_a;
00456   getPrediction(pr_ch_x, pr_ch_y, pr_ch_a, dt);
00457 
00458   // the predicted change of the laser's position, in the fixed frame
00459 
00460   tf::Transform pr_ch;
00461   createTfFromXYTheta(pr_ch_x, pr_ch_y, pr_ch_a, pr_ch);
00462 
00463   // account for the change since the last kf, in the fixed frame
00464 
00465   pr_ch = pr_ch * (f2b_ * f2b_kf_.inverse());
00466 
00467   // the predicted change of the laser's position, in the laser frame
00468 
00469   tf::Transform pr_ch_l;
00470   pr_ch_l = laser_to_base_ * f2b_.inverse() * pr_ch * f2b_ * base_to_laser_ ;
00471 
00472   input_.first_guess[0] = pr_ch_l.getOrigin().getX();
00473   input_.first_guess[1] = pr_ch_l.getOrigin().getY();
00474   input_.first_guess[2] = tf::getYaw(pr_ch_l.getRotation());
00475 
00476   // If they are non-Null, free covariance gsl matrices to avoid leaking memory
00477   if (output_.cov_x_m)
00478   {
00479     gsl_matrix_free(output_.cov_x_m);
00480     output_.cov_x_m = 0;
00481   }
00482   if (output_.dx_dy1_m)
00483   {
00484     gsl_matrix_free(output_.dx_dy1_m);
00485     output_.dx_dy1_m = 0;
00486   }
00487   if (output_.dx_dy2_m)
00488   {
00489     gsl_matrix_free(output_.dx_dy2_m);
00490     output_.dx_dy2_m = 0;
00491   }
00492 
00493   // *** scan match - using point to line icp from CSM
00494 
00495   sm_icp(&input_, &output_);
00496   tf::Transform corr_ch;
00497 
00498   if (output_.valid)
00499   {
00500 
00501     // the correction of the laser's position, in the laser frame
00502     tf::Transform corr_ch_l;
00503     createTfFromXYTheta(output_.x[0], output_.x[1], output_.x[2], corr_ch_l);
00504 
00505     // the correction of the base's position, in the base frame
00506     corr_ch = base_to_laser_ * corr_ch_l * laser_to_base_;
00507 
00508     // update the pose in the world frame
00509     f2b_ = f2b_kf_ * corr_ch;
00510 
00511     // **** publish
00512 
00513     if (publish_pose_)
00514     {
00515       // unstamped Pose2D message
00516       geometry_msgs::Pose2D::Ptr pose_msg;
00517       pose_msg = boost::make_shared<geometry_msgs::Pose2D>();
00518       pose_msg->x = f2b_.getOrigin().getX();
00519       pose_msg->y = f2b_.getOrigin().getY();
00520       pose_msg->theta = tf::getYaw(f2b_.getRotation());
00521       pose_publisher_.publish(pose_msg);
00522     }
00523     if (publish_pose_stamped_)
00524     {
00525       // stamped Pose message
00526       geometry_msgs::PoseStamped::Ptr pose_stamped_msg;
00527       pose_stamped_msg = boost::make_shared<geometry_msgs::PoseStamped>();
00528 
00529       pose_stamped_msg->header.stamp    = time;
00530       pose_stamped_msg->header.frame_id = fixed_frame_;
00531 
00532       tf::poseTFToMsg(f2b_, pose_stamped_msg->pose);
00533 
00534       pose_stamped_publisher_.publish(pose_stamped_msg);
00535     }
00536     if (publish_pose_with_covariance_)
00537     {
00538       // unstamped PoseWithCovariance message
00539       geometry_msgs::PoseWithCovariance::Ptr pose_with_covariance_msg;
00540       pose_with_covariance_msg = boost::make_shared<geometry_msgs::PoseWithCovariance>();
00541       tf::poseTFToMsg(f2b_, pose_with_covariance_msg->pose);
00542 
00543       if (input_.do_compute_covariance)
00544       {
00545         pose_with_covariance_msg->covariance = boost::assign::list_of
00546           (gsl_matrix_get(output_.cov_x_m, 0, 0)) (0)  (0)  (0)  (0)  (0)
00547           (0)  (gsl_matrix_get(output_.cov_x_m, 0, 1)) (0)  (0)  (0)  (0)
00548           (0)  (0)  (static_cast<double>(position_covariance_[2])) (0)  (0)  (0)
00549           (0)  (0)  (0)  (static_cast<double>(orientation_covariance_[0])) (0)  (0)
00550           (0)  (0)  (0)  (0)  (static_cast<double>(orientation_covariance_[1])) (0)
00551           (0)  (0)  (0)  (0)  (0)  (gsl_matrix_get(output_.cov_x_m, 0, 2));
00552       }
00553       else
00554       {
00555         pose_with_covariance_msg->covariance = boost::assign::list_of
00556           (static_cast<double>(position_covariance_[0])) (0)  (0)  (0)  (0)  (0)
00557           (0)  (static_cast<double>(position_covariance_[1])) (0)  (0)  (0)  (0)
00558           (0)  (0)  (static_cast<double>(position_covariance_[2])) (0)  (0)  (0)
00559           (0)  (0)  (0)  (static_cast<double>(orientation_covariance_[0])) (0)  (0)
00560           (0)  (0)  (0)  (0)  (static_cast<double>(orientation_covariance_[1])) (0)
00561           (0)  (0)  (0)  (0)  (0)  (static_cast<double>(orientation_covariance_[2]));
00562       }
00563 
00564       pose_with_covariance_publisher_.publish(pose_with_covariance_msg);
00565     }
00566     if (publish_pose_with_covariance_stamped_)
00567     {
00568       // stamped Pose message
00569       geometry_msgs::PoseWithCovarianceStamped::Ptr pose_with_covariance_stamped_msg;
00570       pose_with_covariance_stamped_msg = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
00571 
00572       pose_with_covariance_stamped_msg->header.stamp    = time;
00573       pose_with_covariance_stamped_msg->header.frame_id = fixed_frame_;
00574 
00575       tf::poseTFToMsg(f2b_, pose_with_covariance_stamped_msg->pose.pose);
00576 
00577       if (input_.do_compute_covariance)
00578       {
00579         pose_with_covariance_stamped_msg->pose.covariance = boost::assign::list_of
00580           (gsl_matrix_get(output_.cov_x_m, 0, 0)) (0)  (0)  (0)  (0)  (0)
00581           (0)  (gsl_matrix_get(output_.cov_x_m, 0, 1)) (0)  (0)  (0)  (0)
00582           (0)  (0)  (static_cast<double>(position_covariance_[2])) (0)  (0)  (0)
00583           (0)  (0)  (0)  (static_cast<double>(orientation_covariance_[0])) (0)  (0)
00584           (0)  (0)  (0)  (0)  (static_cast<double>(orientation_covariance_[1])) (0)
00585           (0)  (0)  (0)  (0)  (0)  (gsl_matrix_get(output_.cov_x_m, 0, 2));
00586       }
00587       else
00588       {
00589         pose_with_covariance_stamped_msg->pose.covariance = boost::assign::list_of
00590           (static_cast<double>(position_covariance_[0])) (0)  (0)  (0)  (0)  (0)
00591           (0)  (static_cast<double>(position_covariance_[1])) (0)  (0)  (0)  (0)
00592           (0)  (0)  (static_cast<double>(position_covariance_[2])) (0)  (0)  (0)
00593           (0)  (0)  (0)  (static_cast<double>(orientation_covariance_[0])) (0)  (0)
00594           (0)  (0)  (0)  (0)  (static_cast<double>(orientation_covariance_[1])) (0)
00595           (0)  (0)  (0)  (0)  (0)  (static_cast<double>(orientation_covariance_[2]));
00596       }
00597 
00598       pose_with_covariance_stamped_publisher_.publish(pose_with_covariance_stamped_msg);
00599     }
00600 
00601     if (publish_tf_)
00602     {
00603       tf::StampedTransform transform_msg (f2b_, time, fixed_frame_, base_frame_);
00604       tf_broadcaster_.sendTransform (transform_msg);
00605     }
00606   }
00607   else
00608   {
00609     corr_ch.setIdentity();
00610     ROS_WARN("Error in scan matching");
00611   }
00612 
00613   // **** swap old and new
00614 
00615   if (newKeyframeNeeded(corr_ch))
00616   {
00617     // generate a keyframe
00618     ld_free(prev_ldp_scan_);
00619     prev_ldp_scan_ = curr_ldp_scan;
00620     f2b_kf_ = f2b_;
00621   }
00622   else
00623   {
00624     ld_free(curr_ldp_scan);
00625   }
00626 
00627   last_icp_time_ = time;
00628 
00629   // **** statistics
00630 
00631   double dur = (ros::WallTime::now() - start).toSec() * 1e3;
00632   ROS_DEBUG("Scan matcher total duration: %.1f ms", dur);
00633 }
00634 
00635 bool LaserScanMatcher::newKeyframeNeeded(const tf::Transform& d)
00636 {
00637   if (fabs(tf::getYaw(d.getRotation())) > kf_dist_angular_) return true;
00638 
00639   double x = d.getOrigin().getX();
00640   double y = d.getOrigin().getY();
00641   if (x*x + y*y > kf_dist_linear_sq_) return true;
00642 
00643   return false;
00644 }
00645 
00646 void LaserScanMatcher::PointCloudToLDP(const PointCloudT::ConstPtr& cloud,
00647                                              LDP& ldp)
00648 {
00649   double max_d2 = cloud_res_ * cloud_res_;
00650 
00651   PointCloudT cloud_f;
00652 
00653   cloud_f.points.push_back(cloud->points[0]);
00654 
00655   for (unsigned int i = 1; i < cloud->points.size(); ++i)
00656   {
00657     const PointT& pa = cloud_f.points[cloud_f.points.size() - 1];
00658     const PointT& pb = cloud->points[i];
00659 
00660     double dx = pa.x - pb.x;
00661     double dy = pa.y - pb.y;
00662     double d2 = dx*dx + dy*dy;
00663 
00664     if (d2 > max_d2)
00665     {
00666       cloud_f.points.push_back(pb);
00667     }
00668   }
00669 
00670   unsigned int n = cloud_f.points.size();
00671 
00672   ldp = ld_alloc_new(n);
00673 
00674   for (unsigned int i = 0; i < n; i++)
00675   {
00676     // calculate position in laser frame
00677     if (is_nan(cloud_f.points[i].x) || is_nan(cloud_f.points[i].y))
00678     {
00679       ROS_WARN("Laser Scan Matcher: Cloud input contains NaN values. \
00680                 Please use a filtered cloud input.");
00681     }
00682     else
00683     {
00684       double r = sqrt(cloud_f.points[i].x * cloud_f.points[i].x +
00685                       cloud_f.points[i].y * cloud_f.points[i].y);
00686 
00687       if (r > cloud_range_min_ && r < cloud_range_max_)
00688       {
00689         ldp->valid[i] = 1;
00690         ldp->readings[i] = r;
00691       }
00692       else
00693       {
00694         ldp->valid[i] = 0;
00695         ldp->readings[i] = -1;  // for invalid range
00696       }
00697     }
00698 
00699     ldp->theta[i] = atan2(cloud_f.points[i].y, cloud_f.points[i].x);
00700     ldp->cluster[i]  = -1;
00701   }
00702 
00703   ldp->min_theta = ldp->theta[0];
00704   ldp->max_theta = ldp->theta[n-1];
00705 
00706   ldp->odometry[0] = 0.0;
00707   ldp->odometry[1] = 0.0;
00708   ldp->odometry[2] = 0.0;
00709 
00710   ldp->true_pose[0] = 0.0;
00711   ldp->true_pose[1] = 0.0;
00712   ldp->true_pose[2] = 0.0;
00713 }
00714 
00715 void LaserScanMatcher::laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan_msg,
00716                                             LDP& ldp)
00717 {
00718   unsigned int n = scan_msg->ranges.size();
00719   ldp = ld_alloc_new(n);
00720 
00721   for (unsigned int i = 0; i < n; i++)
00722   {
00723     // calculate position in laser frame
00724 
00725     double r = scan_msg->ranges[i];
00726 
00727     if (r > scan_msg->range_min && r < scan_msg->range_max)
00728     {
00729       // fill in laser scan data
00730 
00731       ldp->valid[i] = 1;
00732       ldp->readings[i] = r;
00733     }
00734     else
00735     {
00736       ldp->valid[i] = 0;
00737       ldp->readings[i] = -1;  // for invalid range
00738     }
00739 
00740     ldp->theta[i]    = scan_msg->angle_min + i * scan_msg->angle_increment;
00741 
00742     ldp->cluster[i]  = -1;
00743   }
00744 
00745   ldp->min_theta = ldp->theta[0];
00746   ldp->max_theta = ldp->theta[n-1];
00747 
00748   ldp->odometry[0] = 0.0;
00749   ldp->odometry[1] = 0.0;
00750   ldp->odometry[2] = 0.0;
00751 
00752   ldp->true_pose[0] = 0.0;
00753   ldp->true_pose[1] = 0.0;
00754   ldp->true_pose[2] = 0.0;
00755 }
00756 
00757 void LaserScanMatcher::createCache (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
00758 {
00759   a_cos_.clear();
00760   a_sin_.clear();
00761 
00762   for (unsigned int i = 0; i < scan_msg->ranges.size(); ++i)
00763   {
00764     double angle = scan_msg->angle_min + i * scan_msg->angle_increment;
00765     a_cos_.push_back(cos(angle));
00766     a_sin_.push_back(sin(angle));
00767   }
00768 
00769   input_.min_reading = scan_msg->range_min;
00770   input_.max_reading = scan_msg->range_max;
00771 }
00772 
00773 bool LaserScanMatcher::getBaseToLaserTf (const std::string& frame_id)
00774 {
00775   ros::Time t = ros::Time::now();
00776 
00777   tf::StampedTransform base_to_laser_tf;
00778   try
00779   {
00780     tf_listener_.waitForTransform(
00781       base_frame_, frame_id, t, ros::Duration(1.0));
00782     tf_listener_.lookupTransform (
00783       base_frame_, frame_id, t, base_to_laser_tf);
00784   }
00785   catch (tf::TransformException ex)
00786   {
00787     ROS_WARN("Could not get initial transform from base to laser frame, %s", ex.what());
00788     return false;
00789   }
00790   base_to_laser_ = base_to_laser_tf;
00791   laser_to_base_ = base_to_laser_.inverse();
00792 
00793   return true;
00794 }
00795 
00796 // returns the predicted change in pose (in fixed frame)
00797 // since the last time we did icp
00798 void LaserScanMatcher::getPrediction(double& pr_ch_x, double& pr_ch_y,
00799                                      double& pr_ch_a, double dt)
00800 {
00801   boost::mutex::scoped_lock(mutex_);
00802 
00803   // **** base case - no input available, use zero-motion model
00804   pr_ch_x = 0.0;
00805   pr_ch_y = 0.0;
00806   pr_ch_a = 0.0;
00807 
00808   // **** use velocity (for example from ab-filter)
00809   if (use_vel_)
00810   {
00811     pr_ch_x = dt * latest_vel_msg_.linear.x;
00812     pr_ch_y = dt * latest_vel_msg_.linear.y;
00813     pr_ch_a = dt * latest_vel_msg_.angular.z;
00814 
00815     if      (pr_ch_a >= M_PI) pr_ch_a -= 2.0 * M_PI;
00816     else if (pr_ch_a < -M_PI) pr_ch_a += 2.0 * M_PI;
00817   }
00818 
00819   // **** use wheel odometry
00820   if (use_odom_ && received_odom_)
00821   {
00822     pr_ch_x = latest_odom_msg_.pose.pose.position.x -
00823               last_used_odom_msg_.pose.pose.position.x;
00824 
00825     pr_ch_y = latest_odom_msg_.pose.pose.position.y -
00826               last_used_odom_msg_.pose.pose.position.y;
00827 
00828     pr_ch_a = tf::getYaw(latest_odom_msg_.pose.pose.orientation) -
00829               tf::getYaw(last_used_odom_msg_.pose.pose.orientation);
00830 
00831     if      (pr_ch_a >= M_PI) pr_ch_a -= 2.0 * M_PI;
00832     else if (pr_ch_a < -M_PI) pr_ch_a += 2.0 * M_PI;
00833 
00834     last_used_odom_msg_ = latest_odom_msg_;
00835   }
00836 
00837   // **** use imu
00838   if (use_imu_ && received_imu_)
00839   {
00840     pr_ch_a = tf::getYaw(latest_imu_msg_.orientation) -
00841               tf::getYaw(last_used_imu_msg_.orientation);
00842 
00843     if      (pr_ch_a >= M_PI) pr_ch_a -= 2.0 * M_PI;
00844     else if (pr_ch_a < -M_PI) pr_ch_a += 2.0 * M_PI;
00845 
00846     last_used_imu_msg_ = latest_imu_msg_;
00847   }
00848 }
00849 
00850 void LaserScanMatcher::createTfFromXYTheta(
00851   double x, double y, double theta, tf::Transform& t)
00852 {
00853   t.setOrigin(tf::Vector3(x, y, 0.0));
00854   tf::Quaternion q;
00855   q.setRPY(0.0, 0.0, theta);
00856   t.setRotation(q);
00857 }
00858 
00859 } // namespace scan_tools


laser_scan_matcher
Author(s): Ivan Dryanovski, William Morris, Andrea Censi
autogenerated on Thu Mar 23 2017 03:34:32