sequential_scan_matcher.cpp

Go to the documentation of this file.

/*
 * 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, Inc. 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 <graph_slam/sequential_scan_matcher.h>
#include <pose_graph/geometry.h>
#include <pose_graph/utils.h>
#include <pose_graph/transforms.h>
#include <boost/foreach.hpp>
#include <ros/ros.h>

namespace graph_slam
{

namespace gm=geometry_msgs;
namespace ksm=karto_scan_matcher;
using namespace pose_graph;

using std::vector;

SequentialScanMatcher::SequentialScanMatcher (const gm::Pose2D& laser_offset, const double max_link_length,
                                              const unsigned min_chain_length, const double min_response,
                                              const bool use_covariances) :
  ScanManager(laser_offset), max_link_length_(max_link_length),
  min_chain_length_(min_chain_length), min_response_(min_response),
  use_covariances_(use_covariances), running_nodes_(50)
{
  ROS_DEBUG_STREAM_NAMED ("init", "Initializing sequential scan matcher with max link length " <<
                          max_link_length_ << ", min chain length " << min_chain_length_ <<
                          ", min response " << min_response_);
}

bool alreadyHaveConstraintToTarget (const NodeConstraint& constraint, const NodeConstraintVector& constraints)
{
  bool found = false;
  BOOST_FOREACH (const NodeConstraint& c, constraints) {
    if (c.target == constraint.target) {
      found = true;
      break;
    }
  }
  return found;
}



NodeConstraintVector SequentialScanMatcher::getConstraints (const PoseGraph& graph, const NodeId ref_node,
                                                            const Pose& initial_pose_estimate,
                                                            const LaserPtr scan) const
{
  // Initialize the matcher based on the parameters of \a scan if necessary
  if (!matcher_)
    matcher_.reset(new ksm::KartoScanMatcher(*scan, laser_offset_, 0.3, 0.01));

  const gm::Point b = computeBarycenterInBaseFrame(scan);
  NodeConstraintVector constraints;
  
  if (running_nodes_.size() > 0) {
    ROS_ASSERT (*(--running_nodes_.end()) == ref_node);

    // Update the initial estimate based on scan matching
    NodeSet nodes;
    BOOST_FOREACH (const NodeId& n, running_nodes_)
      nodes.insert(n);
    const ksm::ScanMatchResult res = scanMatchNodes(matcher_, initial_pose_estimate, scan, nodes);
    const Pose corrected_pose = convertToPose(res.pose);
    const PrecisionMatrix prec = getPrecisionMatrix(res, use_covariances_);
    const Eigen3::Affine3d trans = relativeTransform(corrected_pose, optimizedPose(ref_node));
    const NodeConstraint constraint(ref_node, makeConstraint(trans, prec));
    ROS_DEBUG_STREAM_NAMED ("running_nodes", "Updating localization based on " << running_nodes_.size() <<
                            " nodes from " << toString2D(initial_pose_estimate) << " to " <<
                            toString2D(corrected_pose) << "; added constraint to " << constraint.target <<
                            " with precision " << toString(constraint.constraint.precision));
    constraints.push_back(constraint);

    vector<Chain> chains = candidateChains(graph, ref_node, pointInReferenceFrame(b, corrected_pose), scan);

    for (unsigned i=0; i<chains.size(); i++) {
      NodeSet nodes(chains[i].begin(), chains[i].end());
      ksm::ScanMatchResult result = scanMatchNodes(matcher_, corrected_pose, scan, nodes);
      if (result.response >= min_response_ || i==0) {
        const PrecisionMatrix prec = getPrecisionMatrix(result, use_covariances_);
        const gm::Point opt_frame_barycenter = pointInReferenceFrame(b, convertToPose(result.pose));
        NodeConstraint constraint = scanChainConstraint(opt_frame_barycenter, result.pose, prec, chains[i]);
        if (!alreadyHaveConstraintToTarget(constraint, constraints))
          constraints.push_back(constraint);
      
        ROS_DEBUG_STREAM_NAMED ("scan_odom", "Initial corrected estimate to scan matching was " <<
                                toString2D(corrected_pose) << " and scan-matched estimate is "
                                << toString(result.pose) << " to node " << constraint.target <<
                                " with precision " << toString(constraint.constraint.precision));
                     
      }
    }
  }
  return constraints;
}


ChainVec SequentialScanMatcher::candidateChains (const PoseGraph& g, const NodeId ref_node, 
                                                 const gm::Point& barycenter, LaserPtr scan) const
{
  vector<Chain> chains;
  NodeSet processed;
  
  ROS_DEBUG_STREAM_NAMED ("scan_odom_chains", "Getting chains near " << barycenter 
                          << " starting at " << ref_node);

  NodeSet nodes = nearbyNodes(ref_node, barycenter, g, max_link_length_);

  Chain running_chain(running_nodes_.begin(), running_nodes_.end());
  addChain(running_chain, &chains, &processed);

  BOOST_FOREACH (const NodeId seed, nodes) {
    if (!contains(processed, seed)) {
      Chain candidate = getChain(seed, nodes, processed);
      if (candidate.size() >= min_chain_length_) 
        addChain(candidate, &chains, &processed);
    }
  }
  return chains;
}


void SequentialScanMatcher::addRunningNode (const NodeId& n)
{
  running_nodes_.push_back(n);
}


} // namespace graph_slam