diff --git a/msg/NodeData.msg b/msg/NodeData.msg index 2265fd71d..f9ce1d0d2 100644 --- a/msg/NodeData.msg +++ b/msg/NodeData.msg @@ -42,4 +42,8 @@ uint8[] userData # std::multimap int32[] wordIds KeyPoint[] wordKpts -sensor_msgs/PointCloud2 wordPts \ No newline at end of file +sensor_msgs/PointCloud2 wordPts + +# compressed descriptors +# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" +uint8[] descriptors diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index ec2bec023..9d8f84313 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -505,11 +505,14 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg) //Features stuff... std::multimap words; std::multimap words3D; + std::multimap wordsDescriptors; pcl::PointCloud cloud; + cv::Mat descriptors; if(msg.wordPts.data.size() && msg.wordPts.height*msg.wordPts.width == msg.wordIds.size()) { pcl::fromROSMsg(msg.wordPts, cloud); + descriptors = rtabmap::uncompressData(msg.descriptors); } for(unsigned int i=0; isecond.cols, + signature.getWordsDescriptors().begin()->second.type()); + index = 0; + bool valid = true; + for(std::multimap::const_iterator jter=signature.getWordsDescriptors().begin(); + jter!=signature.getWordsDescriptors().end() && valid; + ++jter) + { + if(jter->second.cols == descriptors.cols && + jter->second.type() == descriptors.type()) + { + jter->second.copyTo(descriptors.row(index++)); + } + else + { + valid = false; + ROS_ERROR("Some descriptors have different type/size! Cannot copy them..."); + } + } + + if(valid) + { + msg.descriptors = rtabmap::compressData(descriptors); + } + } + else if(signature.getWordsDescriptors().size()) + { + ROS_ERROR("Words and descriptors must have the same size (%d vs %d)!", + (int)signature.getWords().size(), + (int)signature.getWordsDescriptors().size()); + } } rtabmap::Signature nodeInfoFromROS(const rtabmap_ros::NodeData & msg)