ExternalLoopDetectionExample.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 <ros/publisher.h>
30 #include <ros/subscriber.h>
31 
32 #include <rtabmap_ros/MapData.h>
33 #include <rtabmap_ros/Info.h>
35 #include <rtabmap_ros/AddLink.h>
36 #include <rtabmap_ros/GetMap.h>
37 
38 #include <rtabmap/core/Rtabmap.h>
39 #include <rtabmap/core/Memory.h>
41 #include <rtabmap/core/util3d.h>
43 #include <rtabmap/utilite/UStl.h>
44 
48 
49 /*
50  * Test:
51  * $ roslaunch rtabmap_ros demo_robot_mapping.launch
52  * Disable internal loop closure detection, in rtabmapviz->Preferences:
53  * ->Vocabulary, set Max words to -1 (loop closure detection disabled)
54  * ->Proximity Detection, uncheck proximity detection by space
55  * $ rosrun rtabmap_ros external_loop_detection_example
56  * $ rosbag play --clock demo_mapping.bag
57  */
58 
60 
63 
64 // This is used to keep in cache the old data of the map
65 std::map<int, rtabmap::SensorData> localData;
66 
67 // In this example, we use rtabmap for our loop closure detector
69 
71 
72 bool g_localizationMode = false;
73 
74 void mapDataCallback(const rtabmap_ros::MapDataConstPtr & mapDataMsg, const rtabmap_ros::InfoConstPtr & infoMsg)
75 {
76  ROS_INFO("Received map data!");
77 
78  rtabmap::Statistics stats;
79  rtabmap_ros::infoFromROS(*infoMsg, stats);
80 
81  bool smallMovement = (bool)uValue(stats.data(), rtabmap::Statistics::kMemorySmall_movement(), 0.0f);
82  bool fastMovement = (bool)uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
83 
84  if(smallMovement || fastMovement)
85  {
86  // The signature has been ignored from rtabmap, don't process it
87  return;
88  }
89 
90  rtabmap::Transform mapToOdom;
91  std::map<int, rtabmap::Transform> poses;
92  std::multimap<int, rtabmap::Link> links;
93  std::map<int, rtabmap::Signature> signatures;
94  rtabmap_ros::mapDataFromROS(*mapDataMsg, poses, links, signatures, mapToOdom);
95 
96  if(!signatures.empty() &&
97  signatures.rbegin()->second.sensorData().isValid() &&
98  localData.find(signatures.rbegin()->first) == localData.end())
99  {
100  int id = signatures.rbegin()->first;
101  const rtabmap::SensorData & s = signatures.rbegin()->second.sensorData();
102  cv::Mat rgb;
103  //rtabmap::LaserScan scan;
104  s.uncompressDataConst(&rgb, 0/*, &scan*/);
105  //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(scan, scan.localTransform());
106 
107  if(loopClosureDetector.process(rgb, id))
108  {
109  if(loopClosureDetector.getLoopClosureId()>0)
110  {
111  int fromId = id;
112  int toId = loopClosureDetector.getLoopClosureId();
113  ROS_INFO("Detected loop closure between %d and %d", fromId, toId);
114  if(localData.find(toId) != localData.end())
115  {
116  //Compute transformation
118  rtabmap::SensorData tmpFrom = s;
119  rtabmap::SensorData tmpTo = localData.at(toId);
120  tmpFrom.uncompressData();
121  tmpTo.uncompressData();
122  rtabmap::Transform t = reg.computeTransformation(tmpFrom, tmpTo, rtabmap::Transform(), &regInfo);
123 
124  if(!t.isNull())
125  {
126  rtabmap::Link link(fromId, toId, rtabmap::Link::kUserClosure, t, regInfo.covariance.inv());
127  rtabmap_ros::AddLinkRequest req;
128  rtabmap_ros::linkToROS(link, req.link);
129  rtabmap_ros::AddLinkResponse res;
130  if(!addLinkSrv.call(req, res))
131  {
132  ROS_ERROR("Failed to call %s service", addLinkSrv.getService().c_str());
133  }
134  }
135  else
136  {
137  ROS_WARN("Could not compute transformation between %d and %d: %s", fromId, toId, regInfo.rejectedMsg.c_str());
138  }
139  }
140  else
141  {
142  ROS_WARN("Could not compute transformation between %d and %d because node data %d is not in cache.", fromId, toId, toId);
143  }
144  }
145  if(!g_localizationMode)
146  {
147  localData.insert(std::make_pair(id, s));
148  }
149  }
150  }
151 }
152 
153 int main(int argc, char** argv)
154 {
155  ros::init(argc, argv, "external_loop_detection_example");
156 
159 
160  ros::NodeHandle nh;
161  ros::NodeHandle pnh("~");
162 
164  params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemGenerateIds(), "false")); // use provided ids
165  params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDEnabled(), "false")); // BOW-only mode
166  loopClosureDetector.init(params, "");
167 
168  pnh.param("localization", g_localizationMode, g_localizationMode);
169 
170  // service to add link
171  addLinkSrv = nh.serviceClient<rtabmap_ros::AddLink>("/rtabmap/add_link");
172 
173  // service to get map for initialization
174  getMapSrv = nh.serviceClient<rtabmap_ros::GetMap>("/rtabmap/get_map_data");
175 
176  rtabmap_ros::GetMap::Request mapReq;
177  mapReq.global = true;
178  mapReq.graphOnly = false;
179  mapReq.optimized = false;
180  rtabmap_ros::GetMap::Response mapRes;
181  ros::Rate rate(1);
182  while(!getMapSrv.exists() && nh.ok())
183  {
184  ROS_INFO("Waiting for service \"%s\" to be available. If rtabmap is already started, "
185  "make sure the service name is the same or remap it.", getMapSrv.getService().c_str());
186  rate.sleep();
187  ros::spinOnce();
188  }
189  if(!nh.ok())
190  {
191  return 0;
192  }
193  ROS_INFO("Calling \"%s\" service to get data already in the map.", getMapSrv.getService().c_str());
194  if(getMapSrv.call(mapReq, mapRes) && !mapRes.data.nodes.empty())
195  {
196  ROS_INFO("Adding %d nodes to memory...", (int)mapRes.data.nodes.size());
197  std::map<int, rtabmap::Transform> poses;
198  std::multimap<int, rtabmap::Link> links;
199  rtabmap::Transform mapToOdom;
200  rtabmap_ros::mapGraphFromROS(mapRes.data.graph, poses, links, mapToOdom);
201  int addedNodes = 0;
202  for(size_t i=0; i<mapRes.data.nodes.size(); ++i)
203  {
204  rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(mapRes.data.nodes.at(i));
205  rtabmap::SensorData compressedData = s.sensorData();
207  if(loopClosureDetector.process(s.sensorData(), rtabmap::Transform()))
208  {
209  localData.insert(std::make_pair(
210  loopClosureDetector.getStatistics().getLastSignatureData().id(),
211  compressedData));
212  ++addedNodes;
213  }
214  }
215  ROS_INFO("Added %d/%d nodes to memory! Vocabulary size=%d",
216  addedNodes,
217  (int)mapRes.data.nodes.size(),
218  (int)loopClosureDetector.getMemory()->getVWDictionary()->getVisualWords().size());
219 
220  }
221 
223  {
224  params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false")); // localization mode
225  loopClosureDetector.parseParameters(params);
226  ROS_INFO("Set detector in localization mode");
227  }
228 
229  // subscription
232  infoTopic.subscribe(nh, "/rtabmap/info", 1);
233  mapDataTopic.subscribe(nh, "/rtabmap/mapData", 1);
236  mapDataTopic,
237  infoTopic);
238  infoMapSync.registerCallback(&mapDataCallback);
239  ROS_INFO("Subscribed to %s and %s", mapDataTopic.getTopic().c_str(), infoTopic.getTopic().c_str());
240 
241  ros::spin();
242 
243  return 0;
244 }
ros::ServiceClient getMapSrv
message_filters::sync_policies::ExactTime< rtabmap_ros::MapData, rtabmap_ros::Info > MyInfoMapSyncPolicy
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
int main(int argc, char **argv)
rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData &msg)
rtabmap::RegistrationVis reg
std::string getTopic() const
const Statistics & getStatistics() const
std::pair< std::string, std::string > ParametersPair
XmlRpcServer s
std::map< int, rtabmap::SensorData > localData
Transform computeTransformation(const Signature &from, const Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
std::map< std::string, std::string > ParametersMap
void init(const ParametersMap &parameters, const std::string &databasePath="", bool loadDatabaseParameters=false)
const std::map< int, VisualWord *> & getVisualWords() const
static void setLevel(ULogger::Level level)
#define ROS_WARN(...)
geometry_msgs::TransformStamped t
const std::map< std::string, float > & data() const
rtabmap::Rtabmap loopClosureDetector
void mapDataCallback(const rtabmap_ros::MapDataConstPtr &mapDataMsg, const rtabmap_ros::InfoConstPtr &infoMsg)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void mapDataFromROS(const rtabmap_ros::MapData &msg, std::map< int, rtabmap::Transform > &poses, std::multimap< int, rtabmap::Link > &links, std::map< int, rtabmap::Signature > &signatures, rtabmap::Transform &mapToOdom)
Connection registerCallback(C &callback)
void linkToROS(const rtabmap::Link &link, rtabmap_ros::Link &msg)
const Memory * getMemory() const
#define ROS_INFO(...)
void infoFromROS(const rtabmap_ros::Info &info, rtabmap::Statistics &stat)
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
void mapGraphFromROS(const rtabmap_ros::MapGraph &msg, std::map< int, rtabmap::Transform > &poses, std::multimap< int, rtabmap::Link > &links, rtabmap::Transform &mapToOdom)
bool process(const SensorData &data, Transform odomPose, const cv::Mat &odomCovariance=cv::Mat::eye(6, 6, CV_64FC1), const std::vector< float > &odomVelocity=std::vector< float >(), const std::map< std::string, float > &externalStats=std::map< std::string, float >())
ROSCPP_DECL void spin()
params
void parseParameters(const ParametersMap &parameters)
ros::ServiceClient addLinkSrv
bool sleep()
bool isNull() const
SensorData & sensorData()
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
const Signature & getLastSignatureData() const
std::string getService()
ROSCPP_DECL void spinOnce()
bool ok() const
#define ROS_ERROR(...)
const VWDictionary * getVWDictionary() const
void uncompressDataConst(cv::Mat *imageRaw, cv::Mat *depthOrRightRaw, LaserScan *laserScanRaw=0, cv::Mat *userDataRaw=0, cv::Mat *groundCellsRaw=0, cv::Mat *obstacleCellsRaw=0, cv::Mat *emptyCellsRaw=0) const
res
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
int getLoopClosureId() const


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Tue Jan 24 2023 04:04:40