loop_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/loop_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;

LoopScanMatcher::LoopScanMatcher (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)
{
  ROS_DEBUG_STREAM_NAMED ("init", "Initializing loop scan matcher with max link length " <<
                          max_link_length_ << ", min chain length " << min_chain_length_ <<
                          ", min response " << min_response_);
}

NodeConstraintVector LoopScanMatcher::getConstraints (const PoseGraph& graph, const NodeId ref_node,
                                                      const Pose& init_pose_estimate, const LaserPtr scan) const
{

  // Initialize the matcher based on scan parameters if necessary
  if (!matcher_) {
    ksm::DoubleVector resolutions, sizes;
    
    sizes.push_back(5.0);
    resolutions.push_back(0.05);

    sizes.push_back(0.3);
    resolutions.push_back(0.01);
    
    matcher_.reset(new ksm::KartoScanMatcher(*scan, laser_offset_, sizes, resolutions));
    matcher_->setPenalizeDistance(false); 
  }
    
  const gm::Point b = computeBarycenterInBaseFrame(scan);
  ChainVec candidates = possibleLoopClosures(graph, ref_node, pointInReferenceFrame(b, init_pose_estimate), scan);

  NodeConstraintVector constraints;
  BOOST_FOREACH (const Chain& chain, candidates) {
    NodeSet nodes(chain.begin(), chain.end());
    ksm::ScanMatchResult result = scanMatchNodes(matcher_, init_pose_estimate, scan, nodes);
    ROS_DEBUG_STREAM_NAMED ("loop_closure", "Loop closure response against chain " <<
                            toString(chain) << " was " << result.response);
    if (result.response >= min_response_) {
      Eigen3::Matrix3f prec2d = result.cov.inverse();
      const PrecisionMatrix prec = getPrecisionMatrix(result, use_covariances_);
      const gm::Point opt_barycenter = pointInReferenceFrame(b, convertToPose(result.pose));
      constraints.push_back(scanChainConstraint(opt_barycenter, result.pose, prec, chain));
      ROS_DEBUG_STREAM_NAMED ("loop_closure", "Adding loop constraint with pose " <<
                              toString(result.pose) << " and precision " << prec);
    }
  }

  return constraints;
}

// Either n is further than distance from pose, or it doesn't have an optimized pose
bool LoopScanMatcher::furtherThan (const double max_distance, const Point& p, const NodeId n) const
{
  boost::optional<gm::Point> p2 = nodeBarycenter(n);
  return (!p2 || euclideanDistance(p, *p2) > max_distance);
}

ChainVec LoopScanMatcher::possibleLoopClosures (const PoseGraph& graph, const NodeId ref_node,
                                                const gm::Point& bary, const LaserPtr scan) const
{
  ChainVec chains;

  // We mark all nearby linked nodes as processed, so we'll never consider chains that include them
  NodeSet processed = nearbyNodes(ref_node, bary, graph, max_link_length_);

  // Identify the set of nodes whose barycenter (based on current optimized pose) is within a radius of
  // the current scan's barycenter
  const NodeSet all_nodes = graph.allNodes();
  NodeSet candidate_nodes;
  remove_copy_if(all_nodes.begin(), all_nodes.end(), inserter(candidate_nodes, candidate_nodes.begin()),
                 bind(&LoopScanMatcher::furtherThan, this, max_link_length_, bary, _1));

  ROS_DEBUG_STREAM_NAMED ("loop_closure", "Looking for possible loop closures.  Barycenter is " << 
                          toString(bary) << " and nearby linked nodes are " << toString(processed) <<
                          " and candidate nodes are " << toString(candidate_nodes));

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

  return chains;
}



} // namespace graph_slam