scan_manager.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/scan_manager.h>
#include <pose_graph/geometry.h>
#include <pose_graph/utils.h>
#include <pose_graph/transforms.h>
#include <boost/foreach.hpp>
#include <queue>

namespace graph_slam
{

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

using std::vector;
using boost::optional;

typedef NodeSequenceMap::value_type SequenceLink;
typedef NodeSequenceMap::left_map::const_iterator LeftIter;
typedef NodeSequenceMap::right_map::const_iterator RightIter;

const double RADIAN_TO_METER_MULT = 1.0;

ScanManager::ScanManager (const gm::Pose2D& offset) : laser_offset_(offset)
{}

void ScanManager::addNode (const NodeId n, const LaserPtr scan)
{
  ROS_ASSERT (!contains(scans_, n) && !contains(barycenters_, n));
  ROS_DEBUG_NAMED ("scan_manager", "Adding node %lu", n.getId());
  scans_[n] = scan;

  barycenters_[n] = computeBarycenterInBaseFrame(scan);
  
  if (last_node_) {
    node_sequence_.insert(SequenceLink(*last_node_, n));
    ROS_DEBUG_NAMED ("scan_manager", " added sequence link from %lu", last_node_->getId());
  }
  last_node_ = n;
}
    
void ScanManager::resetLastNode ()
{
  last_node_ = optional<NodeId>();
  ROS_DEBUG_STREAM_NAMED ("scan_manager", "Resetting last node");
}


optional<NodeId> ScanManager::lastNode () const
{
  return last_node_;
}

ksm::ScanWithPose makeScanWithPose (const NodePoseMap& optimized_poses, const NodeScanMap scans_,
                                    const NodeId n)
{
  return ksm::ScanWithPose(*keyValue(scans_, n), projectToPose2D(keyValue(optimized_poses, n)));
}


template <typename NodeContainer>
vector<ksm::ScanWithPose> ScanManager::makeRefScans (const NodeContainer& nodes) const
{
  vector<ksm::ScanWithPose> loc_scans(nodes.size());
  transform(nodes.begin(), nodes.end(), loc_scans.begin(),
            bind(makeScanWithPose, *optimized_poses_, scans_, _1));
  return loc_scans;
}




Chain ScanManager::getChain (const NodeId seed, const NodeSet& nodes, const NodeSet& processed) const
{
  ROS_ASSERT_MSG (contains(nodes, seed), "Node set does not contain %lu", seed.getId());
  ROS_ASSERT_MSG (!contains(processed, seed), "%lu was already processed", seed.getId());
  const Chain forward_chain = onewayChain(node_sequence_.left, seed, nodes, processed);
  const Chain backward_chain = onewayChain(node_sequence_.right, seed, nodes, processed);

  Chain chain(forward_chain.size()+backward_chain.size()-1);
  copy(backward_chain.rbegin(), backward_chain.rend(), chain.begin());
  copy(forward_chain.begin()+1, forward_chain.end(), chain.begin() + backward_chain.size());

  return chain;
}

gm::Point ScanManager::computeBarycenterInBaseFrame (const LaserPtr scan) const
{
  const gm::Point offset_center = computeBarycenter(*scan);
  const Affine3d offset_transform = poseToWorldTransform(convertToPose(laser_offset_));
  return transformPoint(offset_transform, offset_center);
}


optional<gm::Point> ScanManager::nodeBarycenter (const NodeId n) const
{
  if (contains(*optimized_poses_, n) && contains(barycenters_, n)) 
    return optional<gm::Point>(nodeBarycenter(n, keyValue(*optimized_poses_, n)));
  else
    return optional<gm::Point>();
}

gm::Point ScanManager::nodeBarycenter (const NodeId n, const gm::Pose& pose) const
{
  return pointInReferenceFrame(keyValue(barycenters_, n), pose);
}


NodeSet ScanManager::nearbyNodes (const NodeId n, const Point& center, const PoseGraph& g,
    const double max_distance) const
{
  NodeSet nodes;
  
  std::queue<NodeId> q;
  q.push(n);
  ROS_DEBUG_STREAM_NAMED ("scan_manager_nearby_nodes", "Looking for nodes within " << max_distance
                          << " of " << n << " at " << toString(center));

  while (!q.empty()) {
    const NodeId current = q.front();
    optional<gm::Point> current_center = nodeBarycenter(current);
    q.pop();
    if (!current_center) {
      ROS_DEBUG_STREAM_NAMED ("scan_manager_nearby_nodes", "No barycenter found for " << current);
      continue;
    }

    if (!contains(nodes, current) && 
        euclideanDistance(*current_center, center) < max_distance) {
      nodes.insert(current);
      ROS_DEBUG_STREAM_NAMED ("scan_manager_nearby_nodes", "Adding nearby node " <<
                              current << " at " << toString(*current_center));
      BOOST_FOREACH (const NodeId n2, g.neighbors(current))
        q.push(n2);
    }
  }

  return nodes;
}

NodeConstraint ScanManager::scanChainConstraint (const gm::Point& barycenter, const gm::Pose2D& pose,
                                                 const pg::PrecisionMatrix& prec,
                                                 const Chain& chain) const
{
  const NodeId closest = closestNode(barycenter, chain);
  const Pose2D closest_pose = projectToPose2D(keyValue(*optimized_poses_, closest));
  return NodeConstraint(closest, makeConstraint(relativeTransform(pose, closest_pose), prec));
}


// Used for finding closest node to a pose
// Note that we just ignore nodes without an optimized pose (rather than asserting)
bool closerTo (const gm::Pose2D& pose, const NodePoseMap& optimized_poses,
               const NodeId n1, const NodeId n2)
{
  if (!contains(optimized_poses, n1)) {
    ROS_WARN_STREAM ("Node " << n1 << " didn't have an optimized pose; ignoring.");
    return false;
  }
  else if (!contains(optimized_poses, n2)) {
    ROS_WARN_STREAM ("Node " << n2 << " didn't have an optimized pose; ignoring.");
    return true;
  }
  else {
    const gm::Pose2D p1 = projectToPose2D(keyValue(optimized_poses, n1));
    const gm::Pose2D p2 = projectToPose2D(keyValue(optimized_poses, n2));
    const double d1=euclideanDistance(pose, p1, RADIAN_TO_METER_MULT);
    const double d2=euclideanDistance(pose, p2, RADIAN_TO_METER_MULT);
    return d1<d2;
  }
}


NodeId ScanManager::closestNode (const gm::Point& barycenter, const Chain& chain) const
{
  optional<NodeId> closest;
  double min_distance = -42;

  BOOST_FOREACH (const NodeId n, chain) {
    if (contains(*optimized_poses_, n) && contains(barycenters_, n)) {
      const double dist = euclideanDistance(*nodeBarycenter(n), barycenter);
      if (!closest || min_distance > dist) {
        closest = n;
        min_distance = dist;
      }
    }
  }
  ROS_ASSERT_MSG(min_distance > -0.1, "Couldn't find closest node to point");
  return *closest;
}

NodeId ScanManager::closestNode (const gm::Pose2D& pose, const NodeSet& nodes) const
{
  NodeSet::const_iterator pos = min_element (nodes.begin(), nodes.end(), 
                                             bind(closerTo, pose, *optimized_poses_, _1, _2));
  ROS_ASSERT (pos!=nodes.end());
  return *pos;
}

geometry_msgs::Pose ScanManager::optimizedPose (const NodeId n) const
{
  return keyValue(*optimized_poses_, n);
}

ksm::ScanMatchResult ScanManager::scanMatchNodes (KartoMatcherPtr matcher,
                                                  const gm::Pose& init_pose_estimate, 
                                                  const LaserPtr scan, const NodeSet& nodes) const
{
  
  return matcher->scanMatch(*scan, projectToPose2D(init_pose_estimate), makeRefScans(nodes));
}

PrecisionMatrix getPrecisionMatrix (const ksm::ScanMatchResult& res, const bool use_covariances)
{
  PrecisionMatrix prec;
  const Eigen3::Matrix3f prec2D = res.cov.inverse();
  ROS_ASSERT (prec2D(0,1) == prec2D(1,0)); // validate my assumption
  if (use_covariances)
    prec = makePrecisionMatrix(prec2D(0,0), prec2D(1,1), prec2D(0,1), prec2D(2,2));
  else
    prec = makePrecisionMatrix(1, 1, 1);
  return prec;
}

void addChain (const Chain& chain, vector<Chain>* chains, NodeSet* processed)
{
  ROS_DEBUG_NAMED ("scan_odom_chains", "Adding chain starting at %lu with %zu nodes",
                   chain[0].getId(), chain.size());
  chains->push_back(chain);
  BOOST_FOREACH (const NodeId n, chain) {
    processed->insert(n);
  }
}



void ScanManager::setOptimizedPoses (const NodePoseMap& optimized_poses) const 
{
  optimized_poses_ = optimized_poses;
}

void ScanManager::unsetOptimizedPoses () const
{
  optimized_poses_ = optional<NodePoseMap>();
}

WithOptimizedPoses::WithOptimizedPoses (const ScanManager* matcher, const NodePoseMap& poses) :
  matcher(matcher)
{
  matcher->setOptimizedPoses(poses);
}

WithOptimizedPoses::~WithOptimizedPoses ()
{
  matcher->unsetOptimizedPoses();
}

  


}