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 "rtabmap_ros/MsgConversion.h"
00029
00030 #include <opencv2/highgui/highgui.hpp>
00031 #include <zlib.h>
00032 #include <ros/ros.h>
00033 #include <rtabmap/core/util3d.h>
00034 #include <rtabmap/utilite/UStl.h>
00035 #include <tf_conversions/tf_eigen.h>
00036 #include <pcl_conversions/pcl_conversions.h>
00037
00038 namespace rtabmap_ros {
00039
00040 void transformToTF(const rtabmap::Transform & transform, tf::Transform & tfTransform)
00041 {
00042 if(!transform.isNull())
00043 {
00044 tf::transformEigenToTF(transform.toEigen3d(), tfTransform);
00045 }
00046 else
00047 {
00048 tfTransform = tf::Transform();
00049 }
00050 }
00051
00052 rtabmap::Transform transformFromTF(const tf::Transform & transform)
00053 {
00054 Eigen::Affine3d eigenTf;
00055 tf::transformTFToEigen(transform, eigenTf);
00056 return rtabmap::Transform::fromEigen3d(eigenTf);
00057 }
00058
00059 void transformToGeometryMsg(const rtabmap::Transform & transform, geometry_msgs::Transform & msg)
00060 {
00061 if(!transform.isNull())
00062 {
00063 tf::Transform tfTransform;
00064 transformToTF(transform, tfTransform);
00065 tf::transformTFToMsg(tfTransform, msg);
00066 }
00067 else
00068 {
00069 msg = geometry_msgs::Transform();
00070 }
00071 }
00072
00073
00074 rtabmap::Transform transformFromGeometryMsg(const geometry_msgs::Transform & msg)
00075 {
00076 tf::Transform tfTransform;
00077 tf::transformMsgToTF(msg, tfTransform);
00078 return transformFromTF(tfTransform);
00079 }
00080
00081 void transformToPoseMsg(const rtabmap::Transform & transform, geometry_msgs::Pose & msg)
00082 {
00083 if(!transform.isNull())
00084 {
00085 tf::Transform tfTransform;
00086 transformToTF(transform, tfTransform);
00087 tf::poseTFToMsg(tfTransform, msg);
00088 }
00089 else
00090 {
00091 msg = geometry_msgs::Pose();
00092 }
00093 }
00094
00095 rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg)
00096 {
00097 tf::Pose tfTransform;
00098 tf::poseMsgToTF(msg, tfTransform);
00099 return transformFromTF(tfTransform);
00100 }
00101
00102 void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes)
00103 {
00104 UASSERT(compressed.empty() || compressed.type() == CV_8UC1);
00105 bytes.clear();
00106 if(!compressed.empty())
00107 {
00108 bytes.resize(compressed.cols * compressed.rows);
00109 memcpy(bytes.data(), compressed.data, bytes.size());
00110 }
00111 }
00112
00113 cv::Mat compressedMatFromBytes(const std::vector<unsigned char> & bytes, bool copy)
00114 {
00115 cv::Mat out;
00116 if(bytes.size())
00117 {
00118 out = cv::Mat(1, bytes.size(), CV_8UC1, (void*)bytes.data());
00119 if(copy)
00120 {
00121 out = out.clone();
00122 }
00123 }
00124 return out;
00125 }
00126
00127 void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat)
00128 {
00129 stat.setExtended(true);
00130
00131
00132 stat.setRefImageId(info.refId);
00133 stat.setLoopClosureId(info.loopClosureId);
00134 stat.setLocalLoopClosureId(info.localLoopClosureId);
00135
00136 stat.setLoopClosureTransform(rtabmap_ros::transformFromGeometryMsg(info.loopClosureTransform));
00137
00138
00139 std::map<int, float> mapIntFloat;
00140 for(unsigned int i=0; i<info.posteriorKeys.size() && i<info.posteriorValues.size(); ++i)
00141 {
00142 mapIntFloat.insert(std::pair<int, float>(info.posteriorKeys.at(i), info.posteriorValues.at(i)));
00143 }
00144 stat.setPosterior(mapIntFloat);
00145 mapIntFloat.clear();
00146 for(unsigned int i=0; i<info.likelihoodKeys.size() && i<info.likelihoodValues.size(); ++i)
00147 {
00148 mapIntFloat.insert(std::pair<int, float>(info.likelihoodKeys.at(i), info.likelihoodValues.at(i)));
00149 }
00150 stat.setLikelihood(mapIntFloat);
00151 mapIntFloat.clear();
00152 for(unsigned int i=0; i<info.rawLikelihoodKeys.size() && i<info.rawLikelihoodValues.size(); ++i)
00153 {
00154 mapIntFloat.insert(std::pair<int, float>(info.rawLikelihoodKeys.at(i), info.rawLikelihoodValues.at(i)));
00155 }
00156 stat.setRawLikelihood(mapIntFloat);
00157 std::map<int, int> mapIntInt;
00158 for(unsigned int i=0; i<info.weightsKeys.size() && i<info.weightsValues.size(); ++i)
00159 {
00160 mapIntInt.insert(std::pair<int, int>(info.weightsKeys.at(i), info.weightsValues.at(i)));
00161 }
00162 stat.setWeights(mapIntInt);
00163
00164 stat.setLocalPath(info.localPath);
00165
00166
00167 for(unsigned int i=0; i<info.statsKeys.size() && i<info.statsValues.size(); i++)
00168 {
00169 stat.addStatistic(info.statsKeys.at(i), info.statsValues.at(i));
00170 }
00171 }
00172
00173 void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info)
00174 {
00175 info.refId = stats.refImageId();
00176 info.loopClosureId = stats.loopClosureId();
00177 info.localLoopClosureId = stats.localLoopClosureId();
00178
00179 rtabmap_ros::transformToGeometryMsg(stats.loopClosureTransform(), info.loopClosureTransform);
00180
00181
00182 if(stats.extended())
00183 {
00184
00185 info.posteriorKeys = uKeys(stats.posterior());
00186 info.posteriorValues = uValues(stats.posterior());
00187 info.likelihoodKeys = uKeys(stats.likelihood());
00188 info.likelihoodValues = uValues(stats.likelihood());
00189 info.rawLikelihoodKeys = uKeys(stats.rawLikelihood());
00190 info.rawLikelihoodValues = uValues(stats.rawLikelihood());
00191 info.weightsKeys = uKeys(stats.weights());
00192 info.weightsValues = uValues(stats.weights());
00193 info.localPath = stats.localPath();
00194
00195
00196 info.statsKeys = uKeys(stats.data());
00197 info.statsValues = uValues(stats.data());
00198 }
00199 }
00200
00201 rtabmap::Link linkFromROS(const rtabmap_ros::Link & msg)
00202 {
00203 return rtabmap::Link(msg.fromId, msg.toId, (rtabmap::Link::Type)msg.type, transformFromGeometryMsg(msg.transform), msg.rotVariance, msg.transVariance);
00204 }
00205
00206 void linkToROS(const rtabmap::Link & link, rtabmap_ros::Link & msg)
00207 {
00208 msg.fromId = link.from();
00209 msg.toId = link.to();
00210 msg.type = link.type();
00211 msg.rotVariance = link.rotVariance();
00212 msg.transVariance = link.transVariance();
00213 transformToGeometryMsg(link.transform(), msg.transform);
00214 }
00215
00216 cv::KeyPoint keypointFromROS(const rtabmap_ros::KeyPoint & msg)
00217 {
00218 return cv::KeyPoint(msg.pt.x, msg.pt.y, msg.size, msg.angle, msg.response, msg.octave, msg.class_id);
00219 }
00220
00221 void keypointToROS(const cv::KeyPoint & kpt, rtabmap_ros::KeyPoint & msg)
00222 {
00223 msg.angle = kpt.angle;
00224 msg.class_id = kpt.class_id;
00225 msg.octave = kpt.octave;
00226 msg.pt.x = kpt.pt.x;
00227 msg.pt.y = kpt.pt.y;
00228 msg.response = kpt.response;
00229 msg.size = kpt.size;
00230 }
00231
00232 std::vector<cv::KeyPoint> keypointsFromROS(const std::vector<rtabmap_ros::KeyPoint> & msg)
00233 {
00234 std::vector<cv::KeyPoint> v(msg.size());
00235 for(unsigned int i=0; i<msg.size(); ++i)
00236 {
00237 v[i] = keypointFromROS(msg[i]);
00238 }
00239 return v;
00240 }
00241
00242 void keypointsToROS(const std::vector<cv::KeyPoint> & kpts, std::vector<rtabmap_ros::KeyPoint> & msg)
00243 {
00244 msg.resize(kpts.size());
00245 for(unsigned int i=0; i<msg.size(); ++i)
00246 {
00247 keypointToROS(kpts[i], msg[i]);
00248 }
00249 }
00250
00251 cv::Point2f point2fFromROS(const rtabmap_ros::Point2f & msg)
00252 {
00253 return cv::Point2f(msg.x, msg.y);
00254 }
00255
00256 void point2fToROS(const cv::Point2f & kpt, rtabmap_ros::Point2f & msg)
00257 {
00258 msg.x = kpt.x;
00259 msg.y = kpt.y;
00260 }
00261
00262 std::vector<cv::Point2f> points2fFromROS(const std::vector<rtabmap_ros::Point2f> & msg)
00263 {
00264 std::vector<cv::Point2f> v(msg.size());
00265 for(unsigned int i=0; i<msg.size(); ++i)
00266 {
00267 v[i] = point2fFromROS(msg[i]);
00268 }
00269 return v;
00270 }
00271
00272 void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ros::Point2f> & msg)
00273 {
00274 msg.resize(kpts.size());
00275 for(unsigned int i=0; i<msg.size(); ++i)
00276 {
00277 point2fToROS(kpts[i], msg[i]);
00278 }
00279 }
00280
00281 void mapGraphFromROS(
00282 const rtabmap_ros::Graph & msg,
00283 std::map<int, rtabmap::Transform> & poses,
00284 std::map<int, int> & mapIds,
00285 std::map<int, double> & stamps,
00286 std::map<int, std::string> & labels,
00287 std::map<int, std::vector<unsigned char> > & userDatas,
00288 std::multimap<int, rtabmap::Link> & links,
00289 rtabmap::Transform & mapToOdom)
00290 {
00291 mapToOdom = transformFromGeometryMsg(msg.mapToOdom);
00292
00293 UASSERT(msg.nodeIds.size() == msg.mapIds.size());
00294 UASSERT(msg.nodeIds.size() == msg.poses.size());
00295 UASSERT(msg.nodeIds.size() == msg.stamps.size());
00296 UASSERT(msg.nodeIds.size() == msg.labels.size());
00297 UASSERT(msg.nodeIds.size() == msg.userDatas.size());
00298
00299 for(unsigned int i=0; i<msg.nodeIds.size(); ++i)
00300 {
00301 poses.insert(std::make_pair(msg.nodeIds[i], rtabmap_ros::transformFromPoseMsg(msg.poses[i])));
00302 mapIds.insert(std::make_pair(msg.nodeIds[i], msg.mapIds[i]));
00303 stamps.insert(std::make_pair(msg.nodeIds[i], msg.stamps[i]));
00304 labels.insert(std::make_pair(msg.nodeIds[i], msg.labels[i]));
00305 userDatas.insert(std::make_pair(msg.nodeIds[i], msg.userDatas[i].data));
00306 }
00307
00308 for(unsigned int i=0; i<msg.links.size(); ++i)
00309 {
00310 rtabmap::Transform t = rtabmap_ros::transformFromGeometryMsg(msg.links[i].transform);
00311 links.insert(std::make_pair(msg.links[i].fromId, linkFromROS(msg.links[i])));
00312 }
00313 }
00314 void mapGraphToROS(
00315 const std::map<int, rtabmap::Transform> & poses,
00316 const std::map<int, int> & mapIds,
00317 const std::map<int, double> & stamps,
00318 const std::map<int, std::string> & labels,
00319 const std::map<int, std::vector<unsigned char> > & userDatas,
00320 const std::multimap<int, rtabmap::Link> & links,
00321 const rtabmap::Transform & mapToOdom,
00322 rtabmap_ros::Graph & msg)
00323 {
00324 UASSERT(poses.size() == 0 ||
00325 (poses.size() == mapIds.size() &&
00326 poses.size() == labels.size() &&
00327 poses.size() == stamps.size() &&
00328 poses.size() == userDatas.size()));
00329
00330 transformToGeometryMsg(mapToOdom, msg.mapToOdom);
00331
00332 msg.nodeIds.resize(poses.size());
00333 msg.poses.resize(poses.size());
00334 msg.mapIds.resize(poses.size());
00335 msg.stamps.resize(poses.size());
00336 msg.labels.resize(poses.size());
00337 msg.userDatas.resize(poses.size());
00338 int index = 0;
00339 std::map<int, rtabmap::Transform>::const_iterator iterPoses = poses.begin();
00340 std::map<int, int>::const_iterator iterMapIds = mapIds.begin();
00341 std::map<int, double>::const_iterator iterStamps = stamps.begin();
00342 std::map<int, std::string>::const_iterator iterLabels = labels.begin();
00343 std::map<int, std::vector<unsigned char> >::const_iterator iterUserDatas = userDatas.begin();
00344 while(iterPoses != poses.end())
00345 {
00346 msg.nodeIds[index] = iterPoses->first;
00347 msg.mapIds[index] = iterMapIds->second;
00348 msg.stamps[index] = iterStamps->second;
00349 msg.labels[index] = iterLabels->second;
00350 msg.userDatas[index].data = iterUserDatas->second;
00351 transformToPoseMsg(iterPoses->second, msg.poses[index]);
00352 ++iterPoses;
00353 ++iterMapIds;
00354 ++iterStamps;
00355 ++iterLabels;
00356 ++iterUserDatas;
00357 ++index;
00358 }
00359
00360 msg.links.resize(links.size());
00361 index=0;
00362 for(std::multimap<int, rtabmap::Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
00363 {
00364 linkToROS(iter->second, msg.links[index++]);
00365 }
00366 }
00367
00368 rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
00369 {
00370
00371 std::multimap<int, cv::KeyPoint> words;
00372 std::multimap<int, pcl::PointXYZ> words3D;
00373 pcl::PointCloud<pcl::PointXYZ> cloud;
00374 if(msg.wordPts.data.size() &&
00375 msg.wordPts.data.size() == msg.wordIds.size())
00376 {
00377 pcl::fromROSMsg(msg.wordPts, cloud);
00378 }
00379 for(unsigned int i=0; i<msg.wordIds.size() && i<msg.wordKpts.size(); ++i)
00380 {
00381 cv::KeyPoint pt = keypointFromROS(msg.wordKpts.at(i));
00382 int wordId = msg.wordIds.at(i);
00383 words.insert(std::make_pair(wordId, pt));
00384 if(i< cloud.size())
00385 {
00386 words3D.insert(std::make_pair(wordId, cloud[i]));
00387 }
00388 }
00389
00390 if(words3D.size() && words3D.size() != words.size())
00391 {
00392 ROS_ERROR("Words 2D and 3D should be the same size (%d, %d)!", (int)words.size(), (int)words3D.size());
00393 }
00394
00395 return rtabmap::Signature(
00396 msg.id,
00397 msg.mapId,
00398 msg.weight,
00399 msg.stamp,
00400 msg.label,
00401 words,
00402 words3D,
00403 transformFromPoseMsg(msg.pose),
00404 msg.userData.data,
00405 compressedMatFromBytes(msg.laserScan),
00406 compressedMatFromBytes(msg.image),
00407 compressedMatFromBytes(msg.depth),
00408 msg.fx,
00409 msg.fy,
00410 msg.cx,
00411 msg.cy,
00412 transformFromGeometryMsg(msg.localTransform));
00413 }
00414 void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg)
00415 {
00416
00417 msg.id = signature.id();
00418 msg.mapId = signature.mapId();
00419 msg.weight = signature.getWeight();
00420 msg.stamp = signature.getStamp();
00421 msg.label = signature.getLabel();
00422 msg.userData.data = signature.getUserData();
00423 transformToPoseMsg(signature.getPose(), msg.pose);
00424 compressedMatToBytes(signature.getImageCompressed(), msg.image);
00425 compressedMatToBytes(signature.getDepthCompressed(), msg.depth);
00426 compressedMatToBytes(signature.getLaserScanCompressed(), msg.laserScan);
00427 msg.fx = signature.getFx();
00428 msg.fy = signature.getFy();
00429 msg.cx = signature.getCx();
00430 msg.cy = signature.getCy();
00431 transformToGeometryMsg(signature.getLocalTransform(), msg.localTransform);
00432
00433
00434 msg.wordIds = uKeys(signature.getWords());
00435 msg.wordKpts.resize(signature.getWords().size());
00436 int index = 0;
00437 for(std::multimap<int, cv::KeyPoint>::const_iterator jter=signature.getWords().begin();
00438 jter!=signature.getWords().end();
00439 ++jter)
00440 {
00441 keypointToROS(jter->second, msg.wordKpts.at(index++));
00442 }
00443
00444 if(signature.getWords3().size() && signature.getWords3().size() == signature.getWords().size())
00445 {
00446 pcl::PointCloud<pcl::PointXYZ> cloud;
00447 cloud.resize(signature.getWords3().size());
00448 index = 0;
00449 for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=signature.getWords3().begin();
00450 jter!=signature.getWords3().end();
00451 ++jter)
00452 {
00453 cloud[index++] = jter->second;
00454 }
00455 pcl::toROSMsg(cloud, msg.wordPts);
00456 }
00457 else if(signature.getWords3().size())
00458 {
00459 ROS_ERROR("Words 2D and words 3D must have the same size (%d vs %d)!",
00460 (int)signature.getWords().size(),
00461 (int)signature.getWords3().size());
00462 }
00463 }
00464
00465 rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
00466 {
00467 rtabmap::OdometryInfo info;
00468 info.lost = msg.lost;
00469 info.matches = msg.matches;
00470 info.features = msg.features;
00471 info.inliers = msg.inliers;
00472 info.localMapSize = msg.localMapSize;
00473 info.time = msg.time;
00474 info.variance = msg.variance;
00475
00476 info.type = msg.type;
00477
00478 UASSERT(msg.wordsKeys.size() == msg.wordsValues.size());
00479 for(unsigned int i=0; i<msg.wordsKeys.size(); ++i)
00480 {
00481 info.words.insert(std::make_pair(msg.wordsKeys[i], keypointFromROS(msg.wordsValues[i])));
00482 }
00483
00484 info.wordMatches = msg.wordMatches;
00485 info.wordInliers = msg.wordInliers;
00486
00487 info.refCorners = points2fFromROS(msg.refCorners);
00488 info.newCorners = points2fFromROS(msg.newCorners);
00489 info.cornerInliers = msg.cornerInliers;
00490
00491 return info;
00492 }
00493
00494 void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg)
00495 {
00496 msg.lost = info.lost;
00497 msg.matches = info.matches;
00498 msg.features = info.features;
00499 msg.inliers = info.inliers;
00500 msg.localMapSize = info.localMapSize;
00501 msg.time = info.time;
00502 msg.variance = info.variance;
00503
00504 msg.type = info.type;
00505
00506 msg.wordsKeys = uKeys(info.words);
00507 keypointsToROS(uValues(info.words), msg.wordsValues);
00508
00509 msg.wordMatches = info.wordMatches;
00510 msg.wordInliers = info.wordInliers;
00511
00512 points2fToROS(info.refCorners, msg.refCorners);
00513 points2fToROS(info.newCorners, msg.newCorners);
00514 msg.cornerInliers = info.cornerInliers;
00515
00516 }
00517
00518 }