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"
31 #include "rtabmap_ros/MsgConversion.h"
32 #include <rtabmap/core/util3d.h>
33 #include <rtabmap/core/Graph.h>
34 #include <rtabmap/core/Optimizer.h>
35 #include <rtabmap/core/Parameters.h>
36 #include <rtabmap/utilite/ULogger.h>
37 #include <rtabmap/utilite/UTimer.h>
38 #include <rtabmap/utilite/UConversion.h>
39 #include <ros/subscriber.h>
40 #include <ros/publisher.h>
41 #include <tf2_ros/transform_broadcaster.h>
42 #include <boost/thread.hpp>
43 
44 using namespace rtabmap;
45 
46 class MapOptimizer
47 {
48 
49 public:
50  MapOptimizer() :
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 
110  ~MapOptimizer()
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);
128  geometry_msgs::TransformStamped msg;
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_;
319  bool globalOptimization_;
320  bool optimizeFromLastNode_;
321  Optimizer * optimizer_;
322 
323  rtabmap::Transform mapToOdom_;
324  boost::mutex mapToOdomMutex_;
325 
326  ros::Subscriber mapDataTopic_;
327 
328  ros::Publisher mapDataPub_;
329  ros::Publisher mapGraphPub_;
330 
331  std::multimap<int, Link> cachedConstraints_;
332  std::map<int, Signature> cachedNodeInfos_;
333 
334  tf2_ros::TransformBroadcaster tfBroadcaster_;
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 }
ros::NodeHandle
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
MapOptimizer::mapDataReceivedCallback
void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr &msg)
Definition: MapOptimizerNode.cpp:139
MapOptimizer::mapToOdom_
rtabmap::Transform mapToOdom_
Definition: MapOptimizerNode.cpp:323
rtabmap::Transform::getDistanceSquared
float getDistanceSquared(const Transform &t) const
rtabmap_ros::mapGraphToROS
void mapGraphToROS(const std::map< int, rtabmap::Transform > &poses, const std::multimap< int, rtabmap::Link > &links, const rtabmap::Transform &mapToOdom, rtabmap_ros::MapGraph &msg)
Definition: MsgConversion.cpp:985
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
ros::NodeHandle::resolveName
std::string resolveName(const std::string &name, bool remap=true) const
rtabmap::Transform::getIdentity
static Transform getIdentity()
MapOptimizer::mapToOdomMutex_
boost::mutex mapToOdomMutex_
Definition: MapOptimizerNode.cpp:324
s
XmlRpcServer s
rtabmap_ros::transformToGeometryMsg
void transformToGeometryMsg(const rtabmap::Transform &transform, geometry_msgs::Transform &msg)
Definition: MsgConversion.cpp:69
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
rtabmap_ros::nodeInfoFromROS
rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData &msg)
Definition: MsgConversion.cpp:1271
rtabmap_ros::transformFromPoseMsg
rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose &msg, bool ignoreRotationIfNotSet=false)
Definition: MsgConversion.cpp:116
transform_broadcaster.h
ROS_WARN
#define ROS_WARN(...)
uBool2Str
std::string uBool2Str(bool boolean)
ros::spin
ROSCPP_DECL void spin(Spinner &spinner)
UASSERT
#define UASSERT(condition)
MapOptimizer::transformThread_
boost::thread * transformThread_
Definition: MapOptimizerNode.cpp:335
uNumber2Str
std::string uNumber2Str(unsigned int number)
true
#define true
MapOptimizer::mapGraphPub_
ros::Publisher mapGraphPub_
Definition: MapOptimizerNode.cpp:329
MapOptimizer::cachedConstraints_
std::multimap< int, Link > cachedConstraints_
Definition: MapOptimizerNode.cpp:331
epsilon
GLM_FUNC_DECL genType epsilon()
rtabmap_ros::linkFromROS
rtabmap::Link linkFromROS(const rtabmap_ros::Link &msg)
Definition: MsgConversion.cpp:534
ROS_INFO
#define ROS_INFO(...)
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
main
int main(int argc, char **argv)
Definition: MapOptimizerNode.cpp:339
MapOptimizer::mapDataTopic_
ros::Subscriber mapDataTopic_
Definition: MapOptimizerNode.cpp:326
ros::ok
ROSCPP_DECL bool ok()
ParametersMap
QMap< QString, QVariant > ParametersMap
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
MapOptimizer::mapFrameId_
std::string mapFrameId_
Definition: MapOptimizerNode.cpp:317
ros::Rate::sleep
bool sleep()
MapOptimizer::tfBroadcaster_
tf2_ros::TransformBroadcaster tfBroadcaster_
Definition: MapOptimizerNode.cpp:334
false
#define false
MapOptimizer::odomFrameId_
std::string odomFrameId_
Definition: MapOptimizerNode.cpp:318
transform_to_tf.TransformStamped
TransformStamped
Definition: transform_to_tf.py:31
MapOptimizer::publishLoop
void publishLoop(double tfDelay)
Definition: MapOptimizerNode.cpp:119
tf2_ros::TransformBroadcaster
ros::Time::now
static Time now()
UTimer::ticks
double ticks()
MapOptimizer::mapDataPub_
ros::Publisher mapDataPub_
Definition: MapOptimizerNode.cpp:328
rtabmap::Transform::inverse
Transform inverse() const
r
r
ROS_ERROR
#define ROS_ERROR(...)
MapOptimizer::optimizer_
Optimizer * optimizer_
Definition: MapOptimizerNode.cpp:321
rtabmap_ros::nodeDataToROS
void nodeDataToROS(const rtabmap::Signature &signature, rtabmap_ros::NodeData &msg)
Definition: MsgConversion.cpp:1150
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
MapOptimizer::cachedNodeInfos_
std::map< int, Signature > cachedNodeInfos_
Definition: MapOptimizerNode.cpp:332

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:42:19