00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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;
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;
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
00142
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
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
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
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 }