laser_scan_matcher.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2011, Ivan Dryanovski, William Morris
 * 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 CCNY Robotics Lab 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.
 */

/*  This package uses Canonical Scan Matcher [1], written by
 *  Andrea Censi
 *
 *  [1] A. Censi, "An ICP variant using a point-to-line metric"
 *  Proceedings of the IEEE International Conference
 *  on Robotics and Automation (ICRA), 2008
 */

#include "laser_scan_matcher/laser_scan_matcher.h"

namespace scan_tools
{

LaserScanMatcher::LaserScanMatcher(ros::NodeHandle nh, ros::NodeHandle nh_private):
  nh_(nh),
  nh_private_(nh_private)
{
  ROS_INFO("Starting LaserScanMatcher");

  // **** init parameters
  initParams();

  // **** state variables

  initialized_   = false;
  received_imu_  = false;
  received_odom_ = false;

  w2b_.setIdentity();

  v_x_ = 0;
  v_y_ = 0;
  v_a_ = 0;

  pose2d_msg_ = boost::make_shared<geometry_msgs::Pose2D>();
  pose_msg_ = boost::make_shared<geometry_msgs::PoseStamped>();


  input_.laser[0] = 0.0;
  input_.laser[1] = 0.0;
  input_.laser[2] = 0.0;

  // *** subscribers

  if (use_cloud_input_)
  {
    cloud_subscriber_ = nh_.subscribe(
      cloud_topic_, 1, &LaserScanMatcher::cloudCallback, this);

  }
  else
  {
    scan_subscriber_ = nh_.subscribe(
      scan_topic_, 1, &LaserScanMatcher::scanCallback, this);
  }

  if (use_imu_)
  {
    imu_subscriber_ = nh_.subscribe(
      imu_topic_, 1, &LaserScanMatcher::imuCallback, this);
  }

  if (use_odom_)
  {
    odom_subscriber_ = nh_.subscribe(
      odom_topic_, 1, &LaserScanMatcher::odomCallback, this);
  }

  // **** pose publisher
  if (publish_pose_)
  {
    pose_publisher_  = nh_.advertise<geometry_msgs::PoseStamped>(
      pose_topic_, 5);
  }
  if (publish_pose2d_)
  {
    pose2d_publisher_  = nh_.advertise<geometry_msgs::Pose2D>(
      pose2d_topic_, 5);
  }
}

LaserScanMatcher::~LaserScanMatcher()
{

}

void LaserScanMatcher::initParams()
{
  // inputs
  if (!nh_private_.getParam ("scan_topic", scan_topic_))
    scan_topic_  = "scan";
  if (!nh_private_.getParam ("cloud_topic", cloud_topic_))
    cloud_topic_ = "cloud";
  if (!nh_private_.getParam ("odom_topic", odom_topic_))
    odom_topic_  = "odom";
  if (!nh_private_.getParam ("imu_topic", imu_topic_))
    imu_topic_   = "imu";
  // outputs
  if (!nh_private_.getParam ("pose2d_topic", pose2d_topic_))
    pose2d_topic_ = "pose2D";
  if (!nh_private_.getParam ("pose_topic", pose_topic_))
    pose_topic_ = "poseICP";
  // tfs
  if (!nh_private_.getParam ("base_frame", base_frame_))
    base_frame_ = "base_link";
  if (!nh_private_.getParam ("fixed_frame", fixed_frame_))
    fixed_frame_ = "world";
  if (!nh_private_.getParam ("frameid", frameid_))
    frameid_ = "icp_pose";

  ROS_INFO("scan_topic_ %s",scan_topic_.c_str());
  ROS_INFO("cloud_topic_ %s",cloud_topic_.c_str());
  ROS_INFO("odom_topic_ %s",odom_topic_.c_str());
  ROS_INFO("imu_topic_ %s",imu_topic_.c_str());
  ROS_INFO("pose2d_topic_ %s",pose2d_topic_.c_str());
  ROS_INFO("pose_topic_ %s",pose_topic_.c_str());
  ROS_INFO("base_frame_ %s",base_frame_.c_str());
  ROS_INFO("fixed_frame_ %s",fixed_frame_.c_str());
  ROS_INFO("frameid_ %s",frameid_.c_str());



  // **** input type - laser scan, or point clouds?
  // if false, will subscrive to LaserScan msgs on /scan.
  // if true, will subscrive to PointCloud2 msgs on /cloud

  if (!nh_private_.getParam ("use_cloud_input", use_cloud_input_))
    use_cloud_input_= false;

  if (use_cloud_input_)
  {
    if (!nh_private_.getParam ("cloud_range_min", cloud_range_min_))
      cloud_range_min_ = 0.1;
    if (!nh_private_.getParam ("cloud_range_max", cloud_range_max_))
      cloud_range_max_ = 50.0;

    input_.min_reading = cloud_range_min_;
    input_.max_reading = cloud_range_max_;
  }

  // **** What predictions are available to speed up the ICP?
  // 1) imu - [theta] from imu yaw angle - /odom topic
  // 2) odom - [x, y, theta] from wheel odometry - /imu topic
  // 3) alpha_beta - [x, y, theta] from simple tracking filter - no topic req.
  // If more than one is enabled, priority is imu > odom > alpha_beta

  if (!nh_private_.getParam ("use_imu", use_imu_))
    use_imu_ = true;
  if (!nh_private_.getParam ("use_odom", use_odom_))
    use_odom_ = true;
  if (!nh_private_.getParam ("use_alpha_beta", use_alpha_beta_))
    use_alpha_beta_ = false;

  // **** How to publish the output?
  // tf (fixed_frame->base_frame),
  // pose message (pose of base frame in the fixed frame)

  if (!nh_private_.getParam ("publish_tf", publish_tf_))
    publish_tf_ = true;
  if (!nh_private_.getParam ("publish_pose", publish_pose_))
    publish_pose_ = true;
  if (!nh_private_.getParam ("publish_pose2d", publish_pose2d_))
    publish_pose2d_ = true;

  if (!nh_private_.getParam ("alpha", alpha_))
    alpha_ = 1.0;
  if (!nh_private_.getParam ("beta", beta_))
    beta_ = 0.8;

 // **** CSM parameters - comments copied from algos.h (by Andrea Censi)

  // Maximum angular displacement between scans
  if (!nh_private_.getParam ("max_angular_correction_deg", input_.max_angular_correction_deg))
    input_.max_angular_correction_deg = 45.0;

  // Maximum translation between scans (m)
  if (!nh_private_.getParam ("max_linear_correction", input_.max_linear_correction))
    input_.max_linear_correction = 0.50;

  // Maximum ICP cycle iterations
  if (!nh_private_.getParam ("max_iterations", input_.max_iterations))
    input_.max_iterations = 10;

  // A threshold for stopping (m)
  if (!nh_private_.getParam ("epsilon_xy", input_.epsilon_xy))
    input_.epsilon_xy = 0.000001;

  // A threshold for stopping (rad)
  if (!nh_private_.getParam ("epsilon_theta", input_.epsilon_theta))
    input_.epsilon_theta = 0.000001;

  // Maximum distance for a correspondence to be valid
  if (!nh_private_.getParam ("max_correspondence_dist", input_.max_correspondence_dist))
    input_.max_correspondence_dist = 0.3;

  // Noise in the scan (m)
  if (!nh_private_.getParam ("sigma", input_.sigma))
    input_.sigma = 0.010;

  // Use smart tricks for finding correspondences.
  if (!nh_private_.getParam ("use_corr_tricks", input_.use_corr_tricks))
    input_.use_corr_tricks = 1;

  // Restart: Restart if error is over threshold
  if (!nh_private_.getParam ("restart", input_.restart))
    input_.restart = 0;

  // Restart: Threshold for restarting
  if (!nh_private_.getParam ("restart_threshold_mean_error", input_.restart_threshold_mean_error))
    input_.restart_threshold_mean_error = 0.01;

  // Restart: displacement for restarting. (m)
  if (!nh_private_.getParam ("restart_dt", input_.restart_dt))
    input_.restart_dt = 1.0;

  // Restart: displacement for restarting. (rad)
  if (!nh_private_.getParam ("restart_dtheta", input_.restart_dtheta))
    input_.restart_dtheta = 0.1;

  // Max distance for staying in the same clustering
  if (!nh_private_.getParam ("clustering_threshold", input_.clustering_threshold))
    input_.clustering_threshold = 0.25;

  // Number of neighbour rays used to estimate the orientation
  if (!nh_private_.getParam ("orientation_neighbourhood", input_.orientation_neighbourhood))
    input_.orientation_neighbourhood = 20;

  // If 0, it's vanilla ICP
  if (!nh_private_.getParam ("use_point_to_line_distance", input_.use_point_to_line_distance))
    input_.use_point_to_line_distance = 1;

  // Discard correspondences based on the angles
  if (!nh_private_.getParam ("do_alpha_test", input_.do_alpha_test))
    input_.do_alpha_test = 0;

  // Discard correspondences based on the angles - threshold angle, in degrees
  if (!nh_private_.getParam ("do_alpha_test_thresholdDeg", input_.do_alpha_test_thresholdDeg))
    input_.do_alpha_test_thresholdDeg = 20.0;

  // Percentage of correspondences to consider: if 0.9,
        // always discard the top 10% of correspondences with more error
  if (!nh_private_.getParam ("outliers_maxPerc", input_.outliers_maxPerc))
    input_.outliers_maxPerc = 0.90;

  // Parameters describing a simple adaptive algorithm for discarding.
        //  1) Order the errors.
        //      2) Choose the percentile according to outliers_adaptive_order.
        //         (if it is 0.7, get the 70% percentile)
        //      3) Define an adaptive threshold multiplying outliers_adaptive_mult
        //         with the value of the error at the chosen percentile.
        //      4) Discard correspondences over the threshold.
        //      This is useful to be conservative; yet remove the biggest errors.
  if (!nh_private_.getParam ("outliers_adaptive_order", input_.outliers_adaptive_order))
    input_.outliers_adaptive_order = 0.7;

  if (!nh_private_.getParam ("outliers_adaptive_mult", input_.outliers_adaptive_mult))
    input_.outliers_adaptive_mult = 2.0;

  //If you already have a guess of the solution, you can compute the polar angle
        //      of the points of one scan in the new position. If the polar angle is not a monotone
        //      function of the readings index, it means that the surface is not visible in the
        //      next position. If it is not visible, then we don't use it for matching.
  if (!nh_private_.getParam ("do_visibility_test", input_.do_visibility_test))
    input_.do_visibility_test = 0;

  // no two points in laser_sens can have the same corr.
  if (!nh_private_.getParam ("outliers_remove_doubles", input_.outliers_remove_doubles))
    input_.outliers_remove_doubles = 1;

  // If 1, computes the covariance of ICP using the method http://purl.org/censi/2006/icpcov
  if (!nh_private_.getParam ("do_compute_covariance", input_.do_compute_covariance))
    input_.do_compute_covariance = 0;

  // Checks that find_correspondences_tricks gives the right answer
  if (!nh_private_.getParam ("debug_verify_tricks", input_.debug_verify_tricks))
    input_.debug_verify_tricks = 0;

  // If 1, the field 'true_alpha' (or 'alpha') in the first scan is used to compute the
  // incidence beta, and the factor (1/cos^2(beta)) used to weight the correspondence.");
  if (!nh_private_.getParam ("use_ml_weights", input_.use_ml_weights))
    input_.use_ml_weights = 0;

  // If 1, the field 'readings_sigma' in the second scan is used to weight the
  // correspondence by 1/sigma^2
  if (!nh_private_.getParam ("use_sigma_weights", input_.use_sigma_weights))
    input_.use_sigma_weights = 0;
}

void LaserScanMatcher::imuCallback (const sensor_msgs::ImuPtr& imu_msg)
{
  boost::mutex::scoped_lock(mutex_);
  latest_imu_yaw_ = getYawFromQuaternion(imu_msg->orientation);
  if (!received_imu_)
  {
    last_imu_yaw_ = getYawFromQuaternion(imu_msg->orientation);
    received_imu_ = true;
  }
}

void LaserScanMatcher::odomCallback (const nav_msgs::Odometry::ConstPtr& odom_msg)
{
  boost::mutex::scoped_lock(mutex_);
  latest_odom_ = *odom_msg;
  if (!received_odom_)
  {
    last_odom_ = *odom_msg;
    received_odom_ = true;
  }
}

void LaserScanMatcher::cloudCallback (const PointCloudT::ConstPtr& cloud)
{
  // **** if first scan, cache the tf from base to the scanner

  if (!initialized_)
  {
    // cache the static tf from base to laser
    if (!getBaseToLaserTf(cloud->header.frame_id))
    {
      ROS_WARN("ScanMatcher: Skipping scan");
      return;
    }

    PointCloudToLDP(cloud, prev_ldp_scan_);
    last_icp_time_ = cloud->header.stamp;
    last_imu_yaw_ = latest_imu_yaw_;
    last_odom_ = latest_odom_;
    initialized_ = true;
  }

  LDP curr_ldp_scan;
  PointCloudToLDP(cloud, curr_ldp_scan);
  processScan(curr_ldp_scan, cloud->header.stamp);
}

void LaserScanMatcher::scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
{
  // **** if first scan, cache the tf from base to the scanner

  if (!initialized_)
  {
    createCache(scan_msg);    // caches the sin and cos of all angles

    // cache the static tf from base to laser
    if (!getBaseToLaserTf(scan_msg->header.frame_id))
    {
      ROS_WARN("ScanMatcher: Skipping scan");
      return;
    }

    laserScanToLDP(scan_msg, prev_ldp_scan_);
    last_icp_time_ = scan_msg->header.stamp;
    last_imu_yaw_ = latest_imu_yaw_;
    last_odom_ = latest_odom_;
    initialized_ = true;
  }

  LDP curr_ldp_scan;
  laserScanToLDP(scan_msg, curr_ldp_scan);
  processScan(curr_ldp_scan, scan_msg->header.stamp);
}

void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time)
{
  struct timeval start, end;    // used for timing
  gettimeofday(&start, NULL);

  // CSM is used in the following way:
  // The scans are always in the laser frame
  // The reference scan (prevLDPcan_) has a pose of 0
  // The new scan (currLDPScan) has a pose equal to the movement
  // of the laser in the laser frame since the last scan
  // The computed correction is then propagated using the tf machinery

  prev_ldp_scan_->odometry[0] = 0;
  prev_ldp_scan_->odometry[1] = 0;
  prev_ldp_scan_->odometry[2] = 0;

  prev_ldp_scan_->estimate[0] = 0;
  prev_ldp_scan_->estimate[1] = 0;
  prev_ldp_scan_->estimate[2] = 0;

  prev_ldp_scan_->true_pose[0] = 0;
  prev_ldp_scan_->true_pose[1] = 0;
  prev_ldp_scan_->true_pose[2] = 0;

  input_.laser_ref  = prev_ldp_scan_;
  input_.laser_sens = curr_ldp_scan;

  // **** estimated change since last scan

  ros::Time new_icp_time = ros::Time::now();
  ros::Duration dur = new_icp_time - last_icp_time_;
  double dt = dur.toSec();

  double pr_ch_x, pr_ch_y, pr_ch_a;
  getPrediction(pr_ch_x, pr_ch_y, pr_ch_a, dt);

  // the predicted change of the laser's position, in the base frame

  tf::Transform pr_ch;
  createTfFromXYTheta(pr_ch_x, pr_ch_y, pr_ch_a, pr_ch);

  // the predicted change of the laser's position, in the laser frame

  tf::Transform pr_ch_l;
  pr_ch_l = laser_to_base_ * pr_ch * base_to_laser_;

  input_.first_guess[0] = pr_ch_l.getOrigin().getX();
  input_.first_guess[1] = pr_ch_l.getOrigin().getY();
  input_.first_guess[2] = getYawFromQuaternion(pr_ch_l.getRotation());

/*
  printf("%f, %f, %f\n", input_.first_guess[0],
                         input_.first_guess[1],
                         input_.first_guess[2]);
*/
  // *** scan match - using point to line icp from CSM

  sm_icp(&input_, &output_);

  if (output_.valid)
  {
    // the correction of the laser's position, in the laser frame

    //printf("%f, %f, %f\n", output_.x[0], output_.x[1], output_.x[2]);

    float dx = output_.x[0] - pr_ch_l.getOrigin().getX();
    float dy = output_.x[1] - pr_ch_l.getOrigin().getY();
    float da = output_.x[2] - getYawFromQuaternion(pr_ch_l.getRotation());

    tf::Transform corr_ch_l;
    float epsilon_dist = 2.25;
    float epsilon_angle = 3.14159;
    float dist = dx*dx + dy*dy;

    if( dist <= epsilon_dist && abs(da) <= epsilon_angle )
    {
      createTfFromXYTheta(output_.x[0], output_.x[1], output_.x[2], corr_ch_l);
    }else
    {
      ROS_WARN("dist: %f, angle %f",dist,da);
      corr_ch_l = pr_ch_l;
    }

    // the correction of the base's position, in the world frame

    tf::Transform corr_ch = base_to_laser_ * corr_ch_l * laser_to_base_;

    if(use_alpha_beta_)
    {
      tf::Transform w2b_new = w2b_ * corr_ch;

      double dx = w2b_new.getOrigin().getX() - w2b_.getOrigin().getX();
      double dy = w2b_new.getOrigin().getY() - w2b_.getOrigin().getY();
      double da = getYawFromQuaternion(w2b_new.getRotation()) -
                  getYawFromQuaternion(w2b_.getRotation());

      double r_x = dx - pr_ch_x;
      double r_y = dy - pr_ch_y;
      double r_a = da - pr_ch_a;

      double x = w2b_.getOrigin().getX();
      double y = w2b_.getOrigin().getY();
      double a = getYawFromQuaternion(w2b_.getRotation());

      double x_new  = (x + pr_ch_x) + alpha_ * r_x;
      double y_new  = (y + pr_ch_y) + alpha_ * r_y;
      double a_new  = (a + pr_ch_a) + alpha_ * r_a;

      createTfFromXYTheta(x_new, y_new, a_new, w2b_);

      if (dt != 0.0)
      {
        v_x_ = v_x_ + (beta_ / dt) * r_x;
        v_y_ = v_y_ + (beta_ / dt) * r_y;
        v_a_ = v_a_ + (beta_ / dt) * r_a;
      }
    }
    else
    {
      w2b_ = w2b_ * corr_ch;
    }

    // **** publish

    if (publish_pose2d_)
    {
      pose2d_msg_->x = w2b_.getOrigin().getX();
      pose2d_msg_->y = w2b_.getOrigin().getY();
      pose2d_msg_->theta = getYawFromQuaternion(w2b_.getRotation());
      pose2d_publisher_.publish(pose2d_msg_);
    }
    if (publish_tf_)
    {
      tf::StampedTransform transform_msg (w2b_, time, fixed_frame_, base_frame_);
      tf_broadcaster_.sendTransform (transform_msg);
    }
    if(publish_pose_)
    {
      pose_msg_->header.stamp = time;
      pose_msg_->header.frame_id  = frameid_;
      pose_msg_->pose.position.x = w2b_.getOrigin().getX();
      pose_msg_->pose.position.y = w2b_.getOrigin().getY();
      pose_msg_->pose.position.z = 0.0;
      pose_msg_->pose.orientation.x = w2b_.getRotation().x();
      pose_msg_->pose.orientation.y = w2b_.getRotation().y();
      pose_msg_->pose.orientation.z = w2b_.getRotation().z();
      pose_msg_->pose.orientation.w = w2b_.getRotation().w();
      pose_publisher_.publish(pose_msg_);
    }

  }
  else
  {
    ROS_WARN("Error in scan matching");

    v_x_ = 0.0;
    v_y_ = 0.0;
    v_a_ = 0.0;
  }

  // **** swap old and new

  ld_free(prev_ldp_scan_);
  prev_ldp_scan_ = curr_ldp_scan;
  last_icp_time_ = new_icp_time;

  // **** statistics

  gettimeofday(&end, NULL);
  double dur_total = ((end.tv_sec   * 1000000 + end.tv_usec  ) -
                      (start.tv_sec * 1000000 + start.tv_usec)) / 1000.0;
  ROS_DEBUG("scan matcher total duration: %.1f ms", dur_total);
}

void LaserScanMatcher::PointCloudToLDP(const PointCloudT::ConstPtr& cloud,
                                             LDP& ldp)
{
  unsigned int n = cloud->width * cloud->height ;
  ldp = ld_alloc_new(n);

  for (unsigned int i = 0; i < n; i++)
  {
    // calculate position in laser frame

    if (is_nan(cloud->points[i].x) || is_nan(cloud->points[i].y))
    {
      ROS_WARN("Laser Scan Matcher: Cloud input contains NaN values. \
                Please use a filtered cloud input.");
    }
    else
    {
      double r = sqrt(cloud->points[i].x * cloud->points[i].x +
                      cloud->points[i].y * cloud->points[i].y);

      if (r > cloud_range_min_ && r < cloud_range_max_)
      {
        ldp->valid[i] = 1;
        ldp->readings[i] = r;
      }
      else
      {
        ldp->valid[i] = 0;
        ldp->readings[i] = -1;  // for invalid range
      }
    }

    ldp->theta[i] = atan2(cloud->points[i].y, cloud->points[i].x);
    ldp->cluster[i]  = -1;
  }

  ldp->min_theta = ldp->theta[0];
  ldp->max_theta = ldp->theta[n-1];

  ldp->odometry[0] = 0.0;
  ldp->odometry[1] = 0.0;
  ldp->odometry[2] = 0.0;

  ldp->true_pose[0] = 0.0;
  ldp->true_pose[1] = 0.0;
  ldp->true_pose[2] = 0.0;
}

void LaserScanMatcher::laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan_msg,
                                            LDP& ldp)
{
  unsigned int n = scan_msg->ranges.size();
  ldp = ld_alloc_new(n);

  for (unsigned int i = 0; i < n; i++)
  {
    // calculate position in laser frame

    double r = scan_msg->ranges[i];

    if (r > scan_msg->range_min && r < scan_msg->range_max)
    {
      // fill in laser scan data

      ldp->valid[i] = 1;
      ldp->readings[i] = r;
    }
    else
    {
      ldp->valid[i] = 0;
      ldp->readings[i] = -1;  // for invalid range
    }

    ldp->theta[i]    = scan_msg->angle_min + i * scan_msg->angle_increment;

    ldp->cluster[i]  = -1;
  }

  ldp->min_theta = ldp->theta[0];
  ldp->max_theta = ldp->theta[n-1];

  ldp->odometry[0] = 0.0;
  ldp->odometry[1] = 0.0;
  ldp->odometry[2] = 0.0;

  ldp->true_pose[0] = 0.0;
  ldp->true_pose[1] = 0.0;
  ldp->true_pose[2] = 0.0;
}

void LaserScanMatcher::createCache (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
{
  a_cos_.clear();
  a_sin_.clear();

  for (unsigned int i = 0; i < scan_msg->ranges.size(); ++i)
  {
    double angle = scan_msg->angle_min + i * scan_msg->angle_increment;
    a_cos_.push_back(cos(angle));
    a_sin_.push_back(sin(angle));
  }

  input_.min_reading = scan_msg->range_min;
  input_.max_reading = scan_msg->range_max;
}

bool LaserScanMatcher::getBaseToLaserTf (const std::string& frame_id)
{
  ros::Time t = ros::Time::now();

  tf::StampedTransform base_to_laser_tf;
  try
  {
    tf_listener_.waitForTransform(
      base_frame_, frame_id, t, ros::Duration(1.0));
    tf_listener_.lookupTransform (
      base_frame_, frame_id, t, base_to_laser_tf);
  }
  catch (tf::TransformException ex)
  {
    ROS_WARN("ScanMatcher: Could not get initial laser transform(%s)", ex.what());
    return false;
  }
  base_to_laser_ = base_to_laser_tf;
  laser_to_base_ = base_to_laser_.inverse();

  return true;
}

// returns the predicted change in pose (in fixed frame)
// since the last time we did icp
void LaserScanMatcher::getPrediction(double& pr_ch_x, double& pr_ch_y,
                                     double& pr_ch_a, double dt)
{
  boost::mutex::scoped_lock(mutex_);

  // **** base case - no input available, use zero-motion model
  pr_ch_x = 0.0;
  pr_ch_y = 0.0;
  pr_ch_a = 0.0;

  // **** use alpha-beta tracking (const. vel. model)
  if (use_alpha_beta_)
  {
    // estmate change in fixed frame, using fixed velocity
    pr_ch_x = v_x_ * dt;     // in fixed frame
    pr_ch_y = v_y_ * dt;
    pr_ch_a = v_a_ * dt;
  }

  // **** use wheel odometry
  if (use_odom_ && received_odom_)
  {
    pr_ch_x = latest_odom_.pose.pose.position.x -
              last_odom_.pose.pose.position.x;

    pr_ch_y = latest_odom_.pose.pose.position.y -
              last_odom_.pose.pose.position.y;

    pr_ch_a = getYawFromQuaternion(latest_odom_.pose.pose.orientation) -
              getYawFromQuaternion(last_odom_.pose.pose.orientation);

    last_odom_ = latest_odom_;
  }

  // **** use imu
  if (use_imu_ && received_imu_)
  {
    pr_ch_a = latest_imu_yaw_ - last_imu_yaw_;
    last_imu_yaw_ = latest_imu_yaw_;
  }
}

double LaserScanMatcher::getYawFromQuaternion(
  const tf::Quaternion& quaternion)
{
  double temp, yaw;
  btMatrix3x3 m(quaternion);
  m.getRPY(temp, temp, yaw);
  return yaw;
}

double LaserScanMatcher::getYawFromQuaternion(
  const geometry_msgs::Quaternion& quaternion)
{
  tf::Quaternion q;
  tf::quaternionMsgToTF(quaternion, q);
  return getYawFromQuaternion(q);
}

void LaserScanMatcher::createTfFromXYTheta(
  double x, double y, double theta, tf::Transform& t)
{
  t.setOrigin(btVector3(x, y, 0.0));
  tf::Quaternion q;
  q.setRPY(0.0, 0.0, theta);
  t.setRotation(q);
}

} //namespace