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/MsgConversion.h"
00031 #include <rtabmap/core/util3d.h>
00032 #include <rtabmap/core/Graph.h>
00033 #include <rtabmap/core/Parameters.h>
00034 #include <rtabmap/utilite/ULogger.h>
00035 #include <rtabmap/utilite/UTimer.h>
00036 #include <ros/subscriber.h>
00037 #include <ros/publisher.h>
00038 #include <tf/tf.h>
00039 #include <tf/transform_broadcaster.h>
00040 #include <boost/thread.hpp>
00041
00042 using namespace rtabmap;
00043
00044 class MapOptimizer
00045 {
00046
00047 public:
00048 MapOptimizer() :
00049 mapFrameId_("map"),
00050 odomFrameId_("odom"),
00051 iterations_(100),
00052 ignoreVariance_(false),
00053 globalOptimization_(true),
00054 optimizeFromLastNode_(false),
00055 mapToOdom_(tf::Transform::getIdentity()),
00056 transformThread_(0)
00057 {
00058 ros::NodeHandle nh;
00059 ros::NodeHandle pnh("~");
00060
00061 pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
00062 pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
00063 pnh.param("iterations", iterations_, iterations_);
00064 pnh.param("ignore_variance", ignoreVariance_, ignoreVariance_);
00065 pnh.param("global_optimization", globalOptimization_, globalOptimization_);
00066 pnh.param("optimize_from_last_node", optimizeFromLastNode_, optimizeFromLastNode_);
00067
00068 UASSERT(iterations_ > 0);
00069
00070 double tfDelay = 0.05;
00071 bool publishTf = true;
00072 pnh.param("publish_tf", publishTf, publishTf);
00073 pnh.param("tf_delay", tfDelay, tfDelay);
00074
00075 mapDataTopic_ = nh.subscribe("mapData", 1, &MapOptimizer::mapDataReceivedCallback, this);
00076 mapDataPub_ = nh.advertise<rtabmap_ros::MapData>(nh.resolveName("mapData")+"_optimized", 1);
00077
00078 if(publishTf)
00079 {
00080 ROS_INFO("map_optimizer will publish tf between frames \"%s\" and \"%s\"", mapFrameId_.c_str(), odomFrameId_.c_str());
00081 ROS_INFO("map_optimizer: map_frame_id = %s", mapFrameId_.c_str());
00082 ROS_INFO("map_optimizer: odom_frame_id = %s", odomFrameId_.c_str());
00083 ROS_INFO("map_optimizer: tf_delay = %f", tfDelay);
00084 transformThread_ = new boost::thread(boost::bind(&MapOptimizer::publishLoop, this, tfDelay));
00085 }
00086 }
00087
00088 ~MapOptimizer()
00089 {
00090 if(transformThread_)
00091 {
00092 transformThread_->join();
00093 delete transformThread_;
00094 }
00095 }
00096
00097 void publishLoop(double tfDelay)
00098 {
00099 if(tfDelay == 0)
00100 return;
00101 ros::Rate r(1.0 / tfDelay);
00102 while(ros::ok())
00103 {
00104 mapToOdomMutex_.lock();
00105 ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfDelay);
00106 tfBroadcaster_.sendTransform( tf::StampedTransform (mapToOdom_, tfExpiration, mapFrameId_, odomFrameId_));
00107 mapToOdomMutex_.unlock();
00108 r.sleep();
00109 }
00110 }
00111
00112 void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
00113 {
00114
00115
00116 UASSERT(msg->graph.nodeIds.size() == msg->graph.poses.size());
00117 UASSERT(msg->graph.nodeIds.size() == msg->graph.mapIds.size());
00118 UASSERT(msg->graph.nodeIds.size() == msg->graph.stamps.size());
00119 UASSERT(msg->graph.nodeIds.size() == msg->graph.labels.size());
00120 UASSERT(msg->graph.nodeIds.size() == msg->graph.userDatas.size());
00121
00122 bool dataChanged = false;
00123
00124 std::multimap<int, Link> newConstraints;
00125 for(unsigned int i=0; i<msg->graph.links.size(); ++i)
00126 {
00127 Link link = rtabmap_ros::linkFromROS(msg->graph.links[i]);
00128 newConstraints.insert(std::make_pair(link.from(), link));
00129
00130 bool edgeAlreadyAdded = false;
00131 for(std::multimap<int, Link>::iterator iter = cachedConstraints_.lower_bound(link.from());
00132 iter != cachedConstraints_.end() && iter->first == link.from();
00133 ++iter)
00134 {
00135 if(iter->second.to() == link.to())
00136 {
00137 edgeAlreadyAdded = true;
00138 if(iter->second.transform() != link.transform())
00139 {
00140 dataChanged = true;
00141 }
00142 }
00143 }
00144 if(!edgeAlreadyAdded)
00145 {
00146 cachedConstraints_.insert(std::make_pair(link.from(), link));
00147 }
00148 }
00149
00150 std::map<int, Transform> newPoses;
00151 std::map<int, int> newMapIds;
00152 std::map<int, double> newStamps;
00153 std::map<int, std::string> newLabels;
00154 std::map<int, std::vector<unsigned char> > newUserDatas;
00155
00156 for(unsigned int i=0; i<msg->nodes.size(); ++i)
00157 {
00158 int id = msg->nodes[i].id;
00159 Transform pose = rtabmap_ros::transformFromPoseMsg(msg->nodes[i].pose);
00160 newPoses.insert(std::make_pair(id, pose));
00161 newMapIds.insert(std::make_pair(id, msg->nodes[i].mapId));
00162 newStamps.insert(std::make_pair(id, msg->nodes[i].stamp));
00163 newLabels.insert(std::make_pair(id, msg->nodes[i].label));
00164 newUserDatas.insert(std::make_pair(id, msg->nodes[i].userData.data));
00165
00166 std::pair<std::map<int, Transform>::iterator, bool> p = cachedPoses_.insert(std::make_pair(id, pose));
00167 if(!p.second && pose != cachedPoses_.at(id))
00168 {
00169 dataChanged = true;
00170 }
00171 else if(p.second)
00172 {
00173 cachedMapIds_.insert(std::make_pair(id, msg->nodes[i].mapId));
00174 cachedStamps_.insert(std::make_pair(id, msg->nodes[i].stamp));
00175 cachedLabels_.insert(std::make_pair(id, msg->nodes[i].label));
00176 cachedUserDatas_.insert(std::make_pair(id, msg->nodes[i].userData.data));
00177 }
00178 }
00179
00180 if(dataChanged)
00181 {
00182 ROS_WARN("Graph data has changed! Reset cache...");
00183 cachedPoses_ = newPoses;
00184 cachedMapIds_ = newMapIds;
00185 cachedStamps_ = newStamps;
00186 cachedLabels_ = newLabels;
00187 cachedUserDatas_ = newUserDatas;
00188 cachedConstraints_ = newConstraints;
00189 }
00190
00191
00192 std::map<int, Transform> poses;
00193 std::map<int, int> mapIds;
00194 std::map<int, double> stamps;
00195 std::map<int, std::string> labels;
00196 std::map<int, std::vector<unsigned char> > userDatas;
00197 std::multimap<int, Link> constraints;
00198 if(globalOptimization_)
00199 {
00200 poses = cachedPoses_;
00201 mapIds = cachedMapIds_;
00202 stamps = cachedStamps_;
00203 labels = cachedLabels_;
00204 userDatas = cachedUserDatas_;
00205 constraints = cachedConstraints_;
00206 }
00207 else
00208 {
00209 constraints = newConstraints;
00210 for(unsigned int i=0; i<msg->graph.nodeIds.size(); ++i)
00211 {
00212 std::map<int, Transform>::iterator iter = cachedPoses_.find(msg->graph.nodeIds[i]);
00213 if(iter != cachedPoses_.end())
00214 {
00215 poses.insert(*iter);
00216 mapIds.insert(*cachedMapIds_.find(iter->first));
00217 stamps.insert(*cachedStamps_.find(iter->first));
00218 labels.insert(*cachedLabels_.find(iter->first));
00219 userDatas.insert(*cachedUserDatas_.find(iter->first));
00220 }
00221 else
00222 {
00223 ROS_ERROR("Odometry pose of node %d not found in cache!", msg->graph.nodeIds[i]);
00224 return;
00225 }
00226 }
00227 }
00228
00229
00230 if(mapDataPub_.getNumSubscribers())
00231 {
00232 UTimer timer;
00233 std::map<int, Transform> optimizedPoses;
00234 Transform mapCorrection = Transform::getIdentity();
00235 if(poses.size() > 1 && constraints.size() > 0)
00236 {
00237 graph::TOROOptimizer optimizer(iterations_, false, ignoreVariance_);
00238 int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first;
00239 std::map<int, rtabmap::Transform> posesOut;
00240 std::multimap<int, rtabmap::Link> linksOut;
00241 optimizer.getConnectedGraph(
00242 fromId,
00243 poses,
00244 constraints,
00245 posesOut,
00246 linksOut);
00247 optimizedPoses = optimizer.optimize(fromId, posesOut, linksOut);
00248
00249 mapToOdomMutex_.lock();
00250 mapCorrection = optimizedPoses.at(poses.rbegin()->first) * poses.rbegin()->second.inverse();
00251 rtabmap_ros::transformToTF(mapCorrection, mapToOdom_);
00252 mapToOdomMutex_.unlock();
00253 }
00254 else if(poses.size() == 1 && constraints.size() == 0)
00255 {
00256 optimizedPoses = poses;
00257 }
00258 else if(poses.size() || constraints.size())
00259 {
00260 ROS_ERROR("map_optimizer: Poses=%d and edges=%d (poses must "
00261 "not be null if there are edges, and edges must be null if poses <= 1)",
00262 (int)poses.size(), (int)constraints.size());
00263 mapIds.clear();
00264 labels.clear();
00265 stamps.clear();
00266 userDatas.clear();
00267 }
00268
00269 UASSERT(optimizedPoses.size() == mapIds.size());
00270 UASSERT(optimizedPoses.size() == labels.size());
00271 UASSERT(optimizedPoses.size() == stamps.size());
00272 UASSERT(optimizedPoses.size() == userDatas.size());
00273 rtabmap_ros::MapData outputMsg;
00274 rtabmap_ros::mapGraphToROS(optimizedPoses, mapIds, stamps, labels, userDatas, std::multimap<int, rtabmap::Link>(), mapCorrection, outputMsg.graph);
00275 outputMsg.graph.links = msg->graph.links;
00276 outputMsg.header = msg->header;
00277 outputMsg.nodes = msg->nodes;
00278 mapDataPub_.publish(outputMsg);
00279
00280 ROS_INFO("Time graph optimization = %f s", timer.ticks());
00281 }
00282 }
00283
00284 private:
00285 std::string mapFrameId_;
00286 std::string odomFrameId_;
00287 int iterations_;
00288 bool ignoreVariance_;
00289 bool globalOptimization_;
00290 bool optimizeFromLastNode_;
00291
00292 tf::Transform mapToOdom_;
00293 boost::mutex mapToOdomMutex_;
00294
00295 ros::Subscriber mapDataTopic_;
00296
00297 ros::Publisher mapDataPub_;
00298
00299 std::map<int, Transform> cachedPoses_;
00300 std::map<int, int> cachedMapIds_;
00301 std::map<int, double> cachedStamps_;
00302 std::map<int, std::string> cachedLabels_;
00303 std::map<int, std::vector<unsigned char> > cachedUserDatas_;
00304 std::multimap<int, Link> cachedConstraints_;
00305
00306 tf::TransformBroadcaster tfBroadcaster_;
00307 boost::thread* transformThread_;
00308 };
00309
00310
00311 int main(int argc, char** argv)
00312 {
00313 ros::init(argc, argv, "map_optimizer");
00314 MapOptimizer optimizer;
00315 ros::spin();
00316 return 0;
00317 }