MapOptimizerNode.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <ros/ros.h>
29 #include "rtabmap_ros/MapData.h"
30 #include "rtabmap_ros/MapGraph.h"
32 #include <rtabmap/core/util3d.h>
33 #include <rtabmap/core/Graph.h>
34 #include <rtabmap/core/Optimizer.h>
37 #include <rtabmap/utilite/UTimer.h>
39 #include <ros/subscriber.h>
40 #include <ros/publisher.h>
42 #include <boost/thread.hpp>
43 
44 using namespace rtabmap;
45 
47 {
48 
49 public:
51  mapFrameId_("map"),
52  odomFrameId_("odom"),
53  globalOptimization_(true),
54  optimizeFromLastNode_(false),
55  mapToOdom_(rtabmap::Transform::getIdentity()),
56  transformThread_(0)
57  {
58  ros::NodeHandle nh;
59  ros::NodeHandle pnh("~");
60 
61  double epsilon = 0.0;
62  bool robust = true;
63  bool slam2d =false;
64  int strategy = 0; // 0=TORO, 1=g2o, 2=GTSAM
65  int iterations = 100;
66  bool ignoreVariance = false;
67 
68  pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
69  pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
70  pnh.param("iterations", iterations, iterations);
71  pnh.param("ignore_variance", ignoreVariance, ignoreVariance);
72  pnh.param("global_optimization", globalOptimization_, globalOptimization_);
73  pnh.param("optimize_from_last_node", optimizeFromLastNode_, optimizeFromLastNode_);
74  pnh.param("epsilon", epsilon, epsilon);
75  pnh.param("robust", robust, robust);
76  pnh.param("slam_2d", slam2d, slam2d);
77  pnh.param("strategy", strategy, strategy);
78 
79 
80  UASSERT(iterations > 0);
81 
82  ParametersMap parameters;
83  parameters.insert(ParametersPair(Parameters::kOptimizerStrategy(), uNumber2Str(strategy)));
84  parameters.insert(ParametersPair(Parameters::kOptimizerEpsilon(), uNumber2Str(epsilon)));
85  parameters.insert(ParametersPair(Parameters::kOptimizerIterations(), uNumber2Str(iterations)));
86  parameters.insert(ParametersPair(Parameters::kOptimizerRobust(), uBool2Str(robust)));
87  parameters.insert(ParametersPair(Parameters::kRegForce3DoF(), uBool2Str(slam2d)));
88  parameters.insert(ParametersPair(Parameters::kOptimizerVarianceIgnored(), uBool2Str(ignoreVariance)));
89  optimizer_ = Optimizer::create(parameters);
90 
91  double tfDelay = 0.05; // 20 Hz
92  bool publishTf = true;
93  pnh.param("publish_tf", publishTf, publishTf);
94  pnh.param("tf_delay", tfDelay, tfDelay);
95 
96  mapDataTopic_ = nh.subscribe("mapData", 1, &MapOptimizer::mapDataReceivedCallback, this);
97  mapDataPub_ = nh.advertise<rtabmap_ros::MapData>(nh.resolveName("mapData")+"_optimized", 1);
98  mapGraphPub_ = nh.advertise<rtabmap_ros::MapGraph>(nh.resolveName("mapData")+"Graph_optimized", 1);
99 
100  if(publishTf)
101  {
102  ROS_INFO("map_optimizer will publish tf between frames \"%s\" and \"%s\"", mapFrameId_.c_str(), odomFrameId_.c_str());
103  ROS_INFO("map_optimizer: map_frame_id = %s", mapFrameId_.c_str());
104  ROS_INFO("map_optimizer: odom_frame_id = %s", odomFrameId_.c_str());
105  ROS_INFO("map_optimizer: tf_delay = %f", tfDelay);
106  transformThread_ = new boost::thread(boost::bind(&MapOptimizer::publishLoop, this, tfDelay));
107  }
108  }
109 
111  {
112  if(transformThread_)
113  {
114  transformThread_->join();
115  delete transformThread_;
116  }
117  }
118 
119  void publishLoop(double tfDelay)
120  {
121  if(tfDelay == 0)
122  return;
123  ros::Rate r(1.0 / tfDelay);
124  while(ros::ok())
125  {
126  mapToOdomMutex_.lock();
127  ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfDelay);
129  msg.child_frame_id = odomFrameId_;
130  msg.header.frame_id = mapFrameId_;
131  msg.header.stamp = tfExpiration;
132  rtabmap_ros::transformToGeometryMsg(mapToOdom_, msg.transform);
133  tfBroadcaster_.sendTransform(msg);
134  mapToOdomMutex_.unlock();
135  r.sleep();
136  }
137  }
138 
139  void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
140  {
141  // save new poses and constraints
142  // Assuming that nodes/constraints are all linked together
143  UASSERT(msg->graph.posesId.size() == msg->graph.poses.size());
144 
145  bool dataChanged = false;
146 
147  std::multimap<int, Link> newConstraints;
148  for(unsigned int i=0; i<msg->graph.links.size(); ++i)
149  {
150  Link link = rtabmap_ros::linkFromROS(msg->graph.links[i]);
151  newConstraints.insert(std::make_pair(link.from(), link));
152 
153  bool edgeAlreadyAdded = false;
154  for(std::multimap<int, Link>::iterator iter = cachedConstraints_.lower_bound(link.from());
155  iter != cachedConstraints_.end() && iter->first == link.from();
156  ++iter)
157  {
158  if(iter->second.to() == link.to())
159  {
160  edgeAlreadyAdded = true;
161  if(iter->second.transform().getDistanceSquared(link.transform()) > 0.0001)
162  {
163  ROS_WARN("%d ->%d (%s vs %s)",iter->second.from(), iter->second.to(), iter->second.transform().prettyPrint().c_str(),
164  link.transform().prettyPrint().c_str());
165  dataChanged = true;
166  }
167  }
168  }
169  if(!edgeAlreadyAdded)
170  {
171  cachedConstraints_.insert(std::make_pair(link.from(), link));
172  }
173  }
174 
175  std::map<int, Signature> newNodeInfos;
176  // add new odometry poses
177  for(unsigned int i=0; i<msg->nodes.size(); ++i)
178  {
179  int id = msg->nodes[i].id;
180  Transform pose = rtabmap_ros::transformFromPoseMsg(msg->nodes[i].pose);
181  Signature s = rtabmap_ros::nodeInfoFromROS(msg->nodes[i]);
182  newNodeInfos.insert(std::make_pair(id, s));
183 
184  std::pair<std::map<int, Signature>::iterator, bool> p = cachedNodeInfos_.insert(std::make_pair(id, s));
185  if(!p.second && pose.getDistanceSquared(cachedNodeInfos_.at(id).getPose()) > 0.0001)
186  {
187  dataChanged = true;
188  }
189  }
190 
191  if(dataChanged)
192  {
193  ROS_WARN("Graph data has changed! Reset cache...");
194  cachedConstraints_ = newConstraints;
195  cachedNodeInfos_ = newNodeInfos;
196  }
197 
198  //match poses in the graph
199  std::multimap<int, Link> constraints;
200  std::map<int, Signature> nodeInfos;
201  if(globalOptimization_)
202  {
203  constraints = cachedConstraints_;
204  nodeInfos = cachedNodeInfos_;
205  }
206  else
207  {
208  constraints = newConstraints;
209  for(unsigned int i=0; i<msg->graph.posesId.size(); ++i)
210  {
211  std::map<int, Signature>::iterator iter = cachedNodeInfos_.find(msg->graph.posesId[i]);
212  if(iter != cachedNodeInfos_.end())
213  {
214  nodeInfos.insert(*iter);
215  }
216  else
217  {
218  ROS_ERROR("Odometry pose of node %d not found in cache!", msg->graph.posesId[i]);
219  return;
220  }
221  }
222  }
223 
224  std::map<int, Transform> poses;
225  for(std::map<int, Signature>::iterator iter=nodeInfos.begin(); iter!=nodeInfos.end(); ++iter)
226  {
227  poses.insert(std::make_pair(iter->first, iter->second.getPose()));
228  }
229 
230  // Optimize only if there is a subscriber
231  if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
232  {
233  UTimer timer;
234  std::map<int, Transform> optimizedPoses;
235  Transform mapCorrection = Transform::getIdentity();
236  std::map<int, rtabmap::Transform> posesOut;
237  std::multimap<int, rtabmap::Link> linksOut;
238  if(poses.size() > 1 && constraints.size() > 0)
239  {
240  int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first;
241  optimizer_->getConnectedGraph(
242  fromId,
243  poses,
244  constraints,
245  posesOut,
246  linksOut);
247  optimizedPoses = optimizer_->optimize(fromId, posesOut, linksOut);
248  mapToOdomMutex_.lock();
249  mapCorrection = optimizedPoses.at(posesOut.rbegin()->first) * posesOut.rbegin()->second.inverse();
250  mapToOdom_ = mapCorrection;
251  mapToOdomMutex_.unlock();
252  }
253  else if(poses.size() == 1 && constraints.size() == 0)
254  {
255  optimizedPoses = poses;
256  }
257  else if(poses.size() == 0 && constraints.size())
258  {
259  ROS_ERROR("map_optimizer: Poses=%d and edges=%d: poses must "
260  "not be null if there are edges.",
261  (int)poses.size(), (int)constraints.size());
262  }
263 
264  rtabmap_ros::MapData outputDataMsg;
265  rtabmap_ros::MapGraph outputGraphMsg;
266  rtabmap_ros::mapGraphToROS(optimizedPoses,
267  linksOut,
268  mapCorrection,
269  outputGraphMsg);
270 
271  if(mapGraphPub_.getNumSubscribers())
272  {
273  outputGraphMsg.header = msg->header;
274  mapGraphPub_.publish(outputGraphMsg);
275  }
276 
277  if(mapDataPub_.getNumSubscribers())
278  {
279  outputDataMsg.header = msg->header;
280  outputDataMsg.graph = outputGraphMsg;
281  outputDataMsg.nodes = msg->nodes;
282  if(posesOut.size() > msg->nodes.size())
283  {
284  std::set<int> addedNodes;
285  for(unsigned int i=0; i<msg->nodes.size(); ++i)
286  {
287  addedNodes.insert(msg->nodes[i].id);
288  }
289  std::list<int> toAdd;
290  for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
291  {
292  if(addedNodes.find(iter->first) == addedNodes.end())
293  {
294  toAdd.push_back(iter->first);
295  }
296  }
297  if(toAdd.size())
298  {
299  int oi = outputDataMsg.nodes.size();
300  outputDataMsg.nodes.resize(outputDataMsg.nodes.size()+toAdd.size());
301  for(std::list<int>::iterator iter=toAdd.begin(); iter!=toAdd.end(); ++iter)
302  {
303  UASSERT(cachedNodeInfos_.find(*iter) != cachedNodeInfos_.end());
304  rtabmap_ros::nodeDataToROS(cachedNodeInfos_.at(*iter), outputDataMsg.nodes[oi]);
305  ++oi;
306  }
307  }
308  }
309  mapDataPub_.publish(outputDataMsg);
310  }
311 
312  ROS_INFO("Time graph optimization = %f s", timer.ticks());
313  }
314  }
315 
316 private:
317  std::string mapFrameId_;
318  std::string odomFrameId_;
322 
324  boost::mutex mapToOdomMutex_;
325 
327 
330 
331  std::multimap<int, Link> cachedConstraints_;
332  std::map<int, Signature> cachedNodeInfos_;
333 
335  boost::thread* transformThread_;
336 };
337 
338 
339 int main(int argc, char** argv)
340 {
341  ros::init(argc, argv, "map_optimizer");
342  MapOptimizer optimizer;
343  ros::spin();
344  return 0;
345 }
std::string prettyPrint() const
void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr &msg)
rtabmap::Transform mapToOdom_
float getDistanceSquared(const Transform &t) const
void mapGraphToROS(const std::map< int, rtabmap::Transform > &poses, const std::multimap< int, rtabmap::Link > &links, const rtabmap::Transform &mapToOdom, rtabmap_ros::MapGraph &msg)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::pair< std::string, std::string > ParametersPair
std::string resolveName(const std::string &name, bool remap=true) const
static Transform getIdentity()
boost::mutex mapToOdomMutex_
XmlRpcServer s
void transformToGeometryMsg(const rtabmap::Transform &transform, geometry_msgs::Transform &msg)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData &msg)
#define ROS_WARN(...)
std::string uBool2Str(bool boolean)
ROSCPP_DECL void spin(Spinner &spinner)
#define UASSERT(condition)
boost::thread * transformThread_
std::string uNumber2Str(unsigned int number)
#define true
ros::Publisher mapGraphPub_
std::multimap< int, Link > cachedConstraints_
GLM_FUNC_DECL genType epsilon()
rtabmap::Link linkFromROS(const rtabmap_ros::Link &msg)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
int main(int argc, char **argv)
ros::Subscriber mapDataTopic_
ROSCPP_DECL bool ok()
QMap< QString, QVariant > ParametersMap
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::string mapFrameId_
rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose &msg)
bool sleep()
tf2_ros::TransformBroadcaster tfBroadcaster_
#define false
std::string odomFrameId_
void publishLoop(double tfDelay)
static Time now()
double ticks()
ros::Publisher mapDataPub_
Transform inverse() const
r
#define ROS_ERROR(...)
Optimizer * optimizer_
void nodeDataToROS(const rtabmap::Signature &signature, rtabmap_ros::NodeData &msg)
static Optimizer * create(const ParametersMap &parameters)
std::map< int, Signature > cachedNodeInfos_


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Fri Jun 7 2019 21:55:03