constraint_generator.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/constraints/fake_loop_closure.h>
#include <graph_slam/constraints/odom.h>
#include <graph_slam/constraints/karto_laser_constraints.h>
#include <pose_graph/exception.h>
#include <ros/ros.h>


namespace graph_slam
{

using std::string;
using ros::NodeHandle;
using XmlRpc::XmlRpcValue;
using boost::format;

template <class T>
T getPrivateParam (const ros::NodeHandle& nh, const string& name, const T& default_val)
{
  T val;
  nh.param(name, val, default_val);
  ROS_DEBUG_STREAM_NAMED ("init", "Looked up param " << name << " with value " 
                          << val << " (default was " << default_val << ")");
  return val;
}


ConstraintGenPtr createConstraintGenerator (const ros::NodeHandle& nh)
{
  std::string gen_type;
  if (!nh.getParam("type", gen_type))
    throw pg::NonexistentParamException(nh.getNamespace() + "/type");
  ROS_DEBUG_STREAM_NAMED ("init", "Initializing constraint of type " << gen_type);
  if (gen_type == "odometry") {
    return ConstraintGenPtr(new OdomConstraintGenerator());
  }
  else if (gen_type == "fake_loop_closure") {
    const double proximity_threshold = getPrivateParam(nh, "proximity_threshold", 0.5);
    
    return ConstraintGenPtr(new FakeLoopClosureGenerator(proximity_threshold));
  }
  else if (gen_type == "scan_odometry") {
    const double max_link_length = getPrivateParam(nh, "max_link_length", 10.0);
    const unsigned min_chain_length = getPrivateParam(nh, "min_chain_length", 5);
    const double min_response = getPrivateParam(nh, "min_response", 0.8);
    const double min_loop_closure_response = getPrivateParam(nh, "min_loop_closure_response", 0.7);
    const double max_loop_link_length = getPrivateParam(nh, "max_loop_link_length", 5.0);
    const double min_loop_chain_length = getPrivateParam(nh, "min_loop_chain_length", 10);
    const bool use_covariances = getPrivateParam(nh, "use_covariances", true);
    return ConstraintGenPtr(new KartoLaserConstraints(max_link_length, min_chain_length,
                                                      min_response, max_loop_link_length,
                                                      min_loop_chain_length, min_loop_closure_response,
                                                      use_covariances));
  }
  else {
    throw pg::PoseGraphException(format("Constraint type %1% not known") % gen_type);
  }
}
    

} // namespace graph_slam