MapOptimizerNode.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
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     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <ros/ros.h>
00029 #include "rtabmap_ros/MapData.h"
00030 #include "rtabmap_ros/MapGraph.h"
00031 #include "rtabmap_ros/MsgConversion.h"
00032 #include <rtabmap/core/util3d.h>
00033 #include <rtabmap/core/Graph.h>
00034 #include <rtabmap/core/Optimizer.h>
00035 #include <rtabmap/core/Parameters.h>
00036 #include <rtabmap/utilite/ULogger.h>
00037 #include <rtabmap/utilite/UTimer.h>
00038 #include <rtabmap/utilite/UConversion.h>
00039 #include <ros/subscriber.h>
00040 #include <ros/publisher.h>
00041 #include <tf2_ros/transform_broadcaster.h>
00042 #include <boost/thread.hpp>
00043 
00044 using namespace rtabmap;
00045 
00046 class MapOptimizer
00047 {
00048 
00049 public:
00050         MapOptimizer() :
00051                 mapFrameId_("map"),
00052                 odomFrameId_("odom"),
00053                 globalOptimization_(true),
00054                 optimizeFromLastNode_(false),
00055                 mapToOdom_(rtabmap::Transform::getIdentity()),
00056                 transformThread_(0)
00057         {
00058                 ros::NodeHandle nh;
00059                 ros::NodeHandle pnh("~");
00060 
00061                 double epsilon = 0.0;
00062                 bool robust = true;
00063                 bool slam2d =false;
00064                 int strategy = 0; // 0=TORO, 1=g2o, 2=GTSAM
00065                 int iterations = 100;
00066                 bool ignoreVariance = false;
00067 
00068                 pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
00069                 pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
00070                 pnh.param("iterations", iterations, iterations);
00071                 pnh.param("ignore_variance", ignoreVariance, ignoreVariance);
00072                 pnh.param("global_optimization", globalOptimization_, globalOptimization_);
00073                 pnh.param("optimize_from_last_node", optimizeFromLastNode_, optimizeFromLastNode_);
00074                 pnh.param("epsilon", epsilon, epsilon);
00075                 pnh.param("robust", robust, robust);
00076                 pnh.param("slam_2d", slam2d, slam2d);
00077                 pnh.param("strategy", strategy, strategy);
00078 
00079 
00080                 UASSERT(iterations > 0);
00081 
00082                 ParametersMap parameters;
00083                 parameters.insert(ParametersPair(Parameters::kOptimizerStrategy(), uNumber2Str(strategy)));
00084                 parameters.insert(ParametersPair(Parameters::kOptimizerEpsilon(), uNumber2Str(epsilon)));
00085                 parameters.insert(ParametersPair(Parameters::kOptimizerIterations(), uNumber2Str(iterations)));
00086                 parameters.insert(ParametersPair(Parameters::kOptimizerRobust(), uBool2Str(robust)));
00087                 parameters.insert(ParametersPair(Parameters::kOptimizerSlam2D(), uBool2Str(slam2d)));
00088                 parameters.insert(ParametersPair(Parameters::kOptimizerVarianceIgnored(), uBool2Str(ignoreVariance)));
00089                 optimizer_ = Optimizer::create(parameters);
00090 
00091                 double tfDelay = 0.05; // 20 Hz
00092                 bool publishTf = true;
00093                 pnh.param("publish_tf", publishTf, publishTf);
00094                 pnh.param("tf_delay", tfDelay, tfDelay);
00095 
00096                 mapDataTopic_ = nh.subscribe("mapData", 1, &MapOptimizer::mapDataReceivedCallback, this);
00097                 mapDataPub_ = nh.advertise<rtabmap_ros::MapData>(nh.resolveName("mapData")+"_optimized", 1);
00098                 mapGraphPub_ = nh.advertise<rtabmap_ros::MapGraph>(nh.resolveName("mapData")+"Graph_optimized", 1);
00099 
00100                 if(publishTf)
00101                 {
00102                         ROS_INFO("map_optimizer will publish tf between frames \"%s\" and \"%s\"", mapFrameId_.c_str(), odomFrameId_.c_str());
00103                         ROS_INFO("map_optimizer: map_frame_id = %s", mapFrameId_.c_str());
00104                         ROS_INFO("map_optimizer: odom_frame_id = %s", odomFrameId_.c_str());
00105                         ROS_INFO("map_optimizer: tf_delay = %f", tfDelay);
00106                         transformThread_ = new boost::thread(boost::bind(&MapOptimizer::publishLoop, this, tfDelay));
00107                 }
00108         }
00109 
00110         ~MapOptimizer()
00111         {
00112                 if(transformThread_)
00113                 {
00114                         transformThread_->join();
00115                         delete transformThread_;
00116                 }
00117         }
00118 
00119         void publishLoop(double tfDelay)
00120         {
00121                 if(tfDelay == 0)
00122                         return;
00123                 ros::Rate r(1.0 / tfDelay);
00124                 while(ros::ok())
00125                 {
00126                         mapToOdomMutex_.lock();
00127                         ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfDelay);
00128                         geometry_msgs::TransformStamped msg;
00129                         msg.child_frame_id = odomFrameId_;
00130                         msg.header.frame_id = mapFrameId_;
00131                         msg.header.stamp = tfExpiration;
00132                         rtabmap_ros::transformToGeometryMsg(mapToOdom_, msg.transform);
00133                         tfBroadcaster_.sendTransform(msg);
00134                         mapToOdomMutex_.unlock();
00135                         r.sleep();
00136                 }
00137         }
00138 
00139         void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
00140         {
00141                 // save new poses and constraints
00142                 // Assuming that nodes/constraints are all linked together
00143                 UASSERT(msg->graph.posesId.size() == msg->graph.poses.size());
00144 
00145                 bool dataChanged = false;
00146 
00147                 std::multimap<int, Link> newConstraints;
00148                 for(unsigned int i=0; i<msg->graph.links.size(); ++i)
00149                 {
00150                         Link link = rtabmap_ros::linkFromROS(msg->graph.links[i]);
00151                         newConstraints.insert(std::make_pair(link.from(), link));
00152 
00153                         bool edgeAlreadyAdded = false;
00154                         for(std::multimap<int, Link>::iterator iter = cachedConstraints_.lower_bound(link.from());
00155                                         iter != cachedConstraints_.end() && iter->first == link.from();
00156                                         ++iter)
00157                         {
00158                                 if(iter->second.to() == link.to())
00159                                 {
00160                                         edgeAlreadyAdded = true;
00161                                         if(iter->second.transform().getDistanceSquared(link.transform()) > 0.0001)
00162                                         {
00163                                                 ROS_WARN("%d ->%d (%s vs %s)",iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str(),
00164                                                                 link.transform().prettyPrint().c_str());
00165                                                 dataChanged = true;
00166                                         }
00167                                 }
00168                         }
00169                         if(!edgeAlreadyAdded)
00170                         {
00171                                 cachedConstraints_.insert(std::make_pair(link.from(), link));
00172                         }
00173                 }
00174 
00175                 std::map<int, Signature> newNodeInfos;
00176                 // add new odometry poses
00177                 for(unsigned int i=0; i<msg->nodes.size(); ++i)
00178                 {
00179                         int id = msg->nodes[i].id;
00180                         Transform pose = rtabmap_ros::transformFromPoseMsg(msg->nodes[i].pose);
00181                         Signature s = rtabmap_ros::nodeInfoFromROS(msg->nodes[i]);
00182                         newNodeInfos.insert(std::make_pair(id, s));
00183 
00184                         std::pair<std::map<int, Signature>::iterator, bool> p = cachedNodeInfos_.insert(std::make_pair(id, s));
00185                         if(!p.second && pose.getDistanceSquared(cachedNodeInfos_.at(id).getPose()) > 0.0001)
00186                         {
00187                                 dataChanged = true;
00188                         }
00189                 }
00190 
00191                 if(dataChanged)
00192                 {
00193                         ROS_WARN("Graph data has changed! Reset cache...");
00194                         cachedConstraints_ = newConstraints;
00195                         cachedNodeInfos_ = newNodeInfos;
00196                 }
00197 
00198                 //match poses in the graph
00199                 std::multimap<int, Link> constraints;
00200                 std::map<int, Signature> nodeInfos;
00201                 if(globalOptimization_)
00202                 {
00203                         constraints = cachedConstraints_;
00204                         nodeInfos = cachedNodeInfos_;
00205                 }
00206                 else
00207                 {
00208                         constraints = newConstraints;
00209                         for(unsigned int i=0; i<msg->graph.posesId.size(); ++i)
00210                         {
00211                                 std::map<int, Signature>::iterator iter = cachedNodeInfos_.find(msg->graph.posesId[i]);
00212                                 if(iter != cachedNodeInfos_.end())
00213                                 {
00214                                         nodeInfos.insert(*iter);
00215                                 }
00216                                 else
00217                                 {
00218                                         ROS_ERROR("Odometry pose of node %d not found in cache!", msg->graph.posesId[i]);
00219                                         return;
00220                                 }
00221                         }
00222                 }
00223 
00224                 std::map<int, Transform> poses;
00225                 for(std::map<int, Signature>::iterator iter=nodeInfos.begin(); iter!=nodeInfos.end(); ++iter)
00226                 {
00227                         poses.insert(std::make_pair(iter->first, iter->second.getPose()));
00228                 }
00229 
00230                 // Optimize only if there is a subscriber
00231                 if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
00232                 {
00233                         UTimer timer;
00234                         std::map<int, Transform> optimizedPoses;
00235                         Transform mapCorrection = Transform::getIdentity();
00236                         std::map<int, rtabmap::Transform> posesOut;
00237                         std::multimap<int, rtabmap::Link> linksOut;
00238                         if(poses.size() > 1 && constraints.size() > 0)
00239                         {
00240                                 int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first;
00241                                 optimizer_->getConnectedGraph(
00242                                                 fromId,
00243                                                 poses,
00244                                                 constraints,
00245                                                 posesOut,
00246                                                 linksOut);
00247                                 optimizedPoses = optimizer_->optimize(fromId, posesOut, linksOut);
00248                                 mapToOdomMutex_.lock();
00249                                 mapCorrection = optimizedPoses.at(posesOut.rbegin()->first) * posesOut.rbegin()->second.inverse();
00250                                 mapToOdom_ = mapCorrection;
00251                                 mapToOdomMutex_.unlock();
00252                         }
00253                         else if(poses.size() == 1 && constraints.size() == 0)
00254                         {
00255                                 optimizedPoses = poses;
00256                         }
00257                         else if(poses.size() || constraints.size())
00258                         {
00259                                 ROS_ERROR("map_optimizer: Poses=%d and edges=%d (poses must "
00260                                            "not be null if there are edges, and edges must be null if poses <= 1)",
00261                                           (int)poses.size(), (int)constraints.size());
00262                         }
00263 
00264                         rtabmap_ros::MapData outputDataMsg;
00265                         rtabmap_ros::MapGraph outputGraphMsg;
00266                         rtabmap_ros::mapGraphToROS(optimizedPoses,
00267                                         linksOut,
00268                                         mapCorrection,
00269                                         outputGraphMsg);
00270 
00271                         if(mapGraphPub_.getNumSubscribers())
00272                         {
00273                                 outputGraphMsg.header = msg->header;
00274                                 mapGraphPub_.publish(outputGraphMsg);
00275                         }
00276 
00277                         if(mapDataPub_.getNumSubscribers())
00278                         {
00279                                 outputDataMsg.header = msg->header;
00280                                 outputDataMsg.graph = outputGraphMsg;
00281                                 outputDataMsg.nodes = msg->nodes;
00282                                 if(posesOut.size() > msg->nodes.size())
00283                                 {
00284                                         std::set<int> addedNodes;
00285                                         for(unsigned int i=0; i<msg->nodes.size(); ++i)
00286                                         {
00287                                                 addedNodes.insert(msg->nodes[i].id);
00288                                         }
00289                                         std::list<int> toAdd;
00290                                         for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
00291                                         {
00292                                                 if(addedNodes.find(iter->first) == addedNodes.end())
00293                                                 {
00294                                                         toAdd.push_back(iter->first);
00295                                                 }
00296                                         }
00297                                         if(toAdd.size())
00298                                         {
00299                                                 int oi = outputDataMsg.nodes.size();
00300                                                 outputDataMsg.nodes.resize(outputDataMsg.nodes.size()+toAdd.size());
00301                                                 for(std::list<int>::iterator iter=toAdd.begin(); iter!=toAdd.end(); ++iter)
00302                                                 {
00303                                                         UASSERT(cachedNodeInfos_.find(*iter) != cachedNodeInfos_.end());
00304                                                         rtabmap_ros::nodeDataToROS(cachedNodeInfos_.at(*iter), outputDataMsg.nodes[oi]);
00305                                                         ++oi;
00306                                                 }
00307                                         }
00308                                 }
00309                                 mapDataPub_.publish(outputDataMsg);
00310                         }
00311 
00312                         ROS_INFO("Time graph optimization = %f s", timer.ticks());
00313                 }
00314         }
00315 
00316 private:
00317         std::string mapFrameId_;
00318         std::string odomFrameId_;
00319         bool globalOptimization_;
00320         bool optimizeFromLastNode_;
00321         Optimizer * optimizer_;
00322 
00323         rtabmap::Transform mapToOdom_;
00324         boost::mutex mapToOdomMutex_;
00325 
00326         ros::Subscriber mapDataTopic_;
00327 
00328         ros::Publisher mapDataPub_;
00329         ros::Publisher mapGraphPub_;
00330 
00331         std::multimap<int, Link> cachedConstraints_;
00332         std::map<int, Signature> cachedNodeInfos_;
00333 
00334         tf2_ros::TransformBroadcaster tfBroadcaster_;
00335         boost::thread* transformThread_;
00336 };
00337 
00338 
00339 int main(int argc, char** argv)
00340 {
00341         ros::init(argc, argv, "map_optimizer");
00342         MapOptimizer optimizer;
00343         ros::spin();
00344         return 0;
00345 }


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Sun Jul 24 2016 03:49:08