scan_manager.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  *
00029  */
00030 
00031 
00040 #ifndef POSE_GRAPH_SCAN_MANAGER_H
00041 #define POSE_GRAPH_SCAN_MANAGER_H
00042 
00043 #include <graph_slam/constraint_generator.h>
00044 #include <karto_scan_matcher/karto_scan_matcher.h>
00045 #include <ros/assert.h>
00046 #include <boost/shared_ptr.hpp>
00047 #include <boost/bimap.hpp>
00048 #include <boost/optional.hpp>
00049 
00050 namespace graph_slam
00051 {
00052 
00053 namespace pg=pose_graph;
00054 
00055 typedef boost::bimap<pg::NodeId, pg::NodeId> NodeSequenceMap;
00056 typedef std::vector<pg::NodeId> Chain;
00057 typedef boost::shared_ptr<karto_scan_matcher::KartoScanMatcher> KartoMatcherPtr;
00058 typedef sensor_msgs::LaserScan::ConstPtr LaserPtr;
00059 typedef std::map<pg::NodeId, LaserPtr> NodeScanMap;
00060 typedef std::set<pg::NodeId> NodeSet;                         
00061 typedef std::vector<Chain> ChainVec;
00062 
00063 
00067 class ScanManager
00068 {
00069 public:
00070 
00071   ScanManager (const geometry_msgs::Pose2D& laser_offset);
00072   
00075   void addNode (pg::NodeId n, LaserPtr scan);
00076 
00078   boost::optional<pg::NodeId> lastNode () const;
00079 
00081   void resetLastNode ();
00082 
00086   pg::NodeId closestNode (const geometry_msgs::Pose2D& pose, const NodeSet& nodes) const;
00087 
00090   geometry_msgs::Point computeBarycenterInBaseFrame (const LaserPtr scan) const;
00091 
00095   NodeSet nearbyNodes (pg::NodeId n, const geometry_msgs::Point& center, const pg::PoseGraph& graph,
00096         const double max_distance) const;
00097 
00098 protected:
00099 
00102   NodeConstraint scanChainConstraint (const geometry_msgs::Point& barycenter,
00103                                       const geometry_msgs::Pose2D& pose, const pg::PrecisionMatrix& prec,
00104                                       const Chain& chain) const;
00105 
00106 
00108   template <typename NodeContainer>
00109   std::vector<karto_scan_matcher::ScanWithPose> makeRefScans (const NodeContainer& nodes) const;
00110 
00114   Chain getChain (pg::NodeId n, const NodeSet& nodes, const NodeSet& processed) const;
00115 
00117   karto_scan_matcher::ScanMatchResult scanMatchNodes (KartoMatcherPtr matcher, 
00118                                                       const geometry_msgs::Pose& init_pose_estimate, 
00119                                                       const LaserPtr scan, const NodeSet& nodes) const;
00120 
00122   const geometry_msgs::Pose2D laser_offset_;
00123 
00125   NodeSequenceMap node_sequence_;
00126 
00129   boost::optional<geometry_msgs::Point> nodeBarycenter (const pg::NodeId n) const;
00130 
00132   geometry_msgs::Pose optimizedPose (const pg::NodeId n) const;
00133 
00134 private:
00135 
00136   NodeScanMap scans_;
00137   std::map<pg::NodeId, geometry_msgs::Point> barycenters_; // In the base frame
00138   boost::optional<pg::NodeId> last_node_; // The node that was last added, if it exists
00139   mutable boost::optional<NodePoseMap> optimized_poses_; // Temporary storage of optimized poses
00140 
00142   geometry_msgs::Point nodeBarycenter (const pg::NodeId n, const geometry_msgs::Pose& pose) const;
00143 
00145   pg::NodeId closestNode (const geometry_msgs::Point& barycenter, const Chain& chain) const;
00146 
00147 
00149   void setOptimizedPoses (const NodePoseMap& optimized_poses) const;
00150   void unsetOptimizedPoses () const;
00151   friend class WithOptimizedPoses;
00152 
00153 
00154 };
00155 
00156 
00158 struct WithOptimizedPoses
00159 {
00160   WithOptimizedPoses (const ScanManager* matcher, const NodePoseMap& poses);
00161   ~WithOptimizedPoses ();
00162   const ScanManager* matcher;
00163 };
00164 
00165 
00166 // Utility function for walking along a chain
00167 // Templated so it can take either side of a bimap
00168 template <typename Map>
00169 Chain onewayChain (const Map& m, const pg::NodeId seed, const NodeSet& allowed, const NodeSet& forbidden)
00170 {
00171   Chain chain;
00172   pg::NodeId current=seed;
00173   while (true) {
00174     if (allowed.find(current)==allowed.end() ||
00175         forbidden.find(current)!=forbidden.end())
00176       break;
00177     chain.push_back(current);
00178     typename Map::const_iterator pos=m.find(current);
00179     if (pos==m.end())
00180       break;
00181     current = pos->second;
00182   }
00183   return chain;
00184 }
00185 
00187 pg::PrecisionMatrix getPrecisionMatrix (const karto_scan_matcher::ScanMatchResult& res,
00188                                         const bool use_covariances);
00189 
00190 // Add chain to chains, and add all its nodes to processed
00191 void addChain (const Chain& chain, ChainVec* chains, NodeSet* processed);
00192 
00193 
00194 } // namespace graph_slam
00195 
00196 #endif // include guard


graph_slam
Author(s): Bhaskara Marthi
autogenerated on Tue Jan 7 2014 11:17:21