i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged ros2->galactic-devel

Browse Source
This commit is contained in:
matlabbe
2021-12-26 15:02:16 -05:00
parent 761da79561 6cbd26a8a6
commit d071de4d80
4 changed files with 69 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -53,7 +53,7 @@ find_package(octomap_msgs)
#find_package(apriltag_msgs)
#find_package(find_object_2d)
find_package(move_base_msgs)
find_package(fiducial_msgs)
#find_package(fiducial_msgs)
## System dependencies are found with CMake's conventions
find_package(RTABMap 0.20.15 REQUIRED)
msg/NodeData.msg
+5 -3
View File
@@ -54,9 +54,11 @@ uint8[] grid_empty_cells
float32 grid_cell_size
Point3f grid_view_point
# std::multimap<wordId, cv::Keypoint>
# std::multimap<wordId, cv::Point3f>
int32[] word_ids
# std::multimap<wordId, index>
# std::vector<cv::Keypoint>
# std::vector<cv::Point3f>
int32[] word_id_keys
int32[] word_id_values
KeyPoint[] word_kpts
Point3f[] word_pts
# compressed descriptors
package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>rtabmap_ros</name>
<version>0.20.15</version>
<version>0.20.16</version>
<description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
src/MsgConversion.cpp
+62 -44
View File
@@ -1074,31 +1074,45 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::msg::NodeData & msg)
cv::Mat wordsDescriptors = rtabmap::uncompressData(msg.word_descriptors);
if(!msg.word_kpts.empty() && msg.word_kpts.size() != msg.word_ids.size())
if(msg.word_id_keys.size() != msg.word_id_values.size())
{
UERROR("Word IDs and 2D keypoints should be the same size (%d, %d)!", (int)msg.word_ids.size(), (int)msg.word_kpts.size());
UERROR("Word ID keys and values should be the same size (%d, %d)!", (int)msg.word_id_keys.size(), (int)msg.word_id_values.size());
}
if(!msg.word_pts.empty() && msg.word_pts.size() != msg.word_ids.size())
if(!msg.word_kpts.empty() && msg.word_kpts.size() != msg.word_id_keys.size())
{
UERROR("Word IDs and 3D points should be the same size (%d, %d)!", (int)msg.word_ids.size(), (int)msg.word_pts.size());
UERROR("Word IDs and 2D keypoints should be the same size (%d, %d)!", (int)msg.word_id_keys.size(), (int)msg.word_kpts.size());
}
if(!wordsDescriptors.empty() && wordsDescriptors.rows != (int)msg.word_ids.size())
if(!msg.word_pts.empty() && msg.word_pts.size() != msg.word_id_keys.size())
{
UERROR("Word IDs and descriptors should be the same size (%d, %d)!", (int)msg.word_ids.size(), wordsDescriptors.rows);
UERROR("Word IDs and 3D points should be the same size (%d, %d)!", (int)msg.word_id_keys.size(), (int)msg.word_pts.size());
}
if(!wordsDescriptors.empty() && wordsDescriptors.rows != (int)msg.word_id_keys.size())
{
UERROR("Word IDs and descriptors should be the same size (%d, %d)!", (int)msg.word_id_keys.size(), wordsDescriptors.rows);
wordsDescriptors = cv::Mat();
}
for(size_t i=0; i<msg.word_ids.size(); ++i)
if(msg.word_id_keys.size() == msg.word_id_values.size())
{
words.insert(std::make_pair(msg.word_ids.at(i), words.size()));
if(msg.word_ids.size() == msg.word_kpts.size())
for(unsigned int i=0; i<msg.word_id_keys.size(); ++i)
{
cv::KeyPoint pt = keypointFromROS(msg.word_kpts.at(i));
wordsKpts.push_back(pt);
}
if(msg.word_ids.size() == msg.word_pts.size())
{
words3D.push_back(point3fFromROS(msg.word_pts[i]));
words.insert(std::make_pair(msg.word_id_keys.at(i), msg.word_id_values.at(i))); // ID to index
if(msg.word_id_keys.size() == msg.word_kpts.size())
{
if(wordsKpts.empty())
{
wordsKpts.reserve(msg.word_kpts.size());
}
wordsKpts.push_back(keypointFromROS(msg.word_kpts.at(i)));
}
if(msg.word_id_keys.size() == msg.word_pts.size())
{
if(words3D.empty())
{
words3D.reserve(msg.word_pts.size());
}
words3D.push_back(point3fFromROS(msg.word_pts[i]));
}
}
}
@@ -1264,43 +1278,47 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::msg::NodeD
}
//Features stuff...
msg.word_ids = uKeys(signature.getWords());
if(!signature.getWordsKpts().empty())
if(!signature.getWordsKpts().empty() &&
signature.getWords().size() != signature.getWordsKpts().size())
{
if(msg.word_ids.size() == signature.getWordsKpts().size())
{
msg.word_kpts.resize(signature.getWordsKpts().size());
}
else
{
UERROR("Word IDs and 2D keypoints must have the same size (%d vs %d)!",
(int)signature.getWords().size(),
(int)signature.getWordsKpts().size());
}
UERROR("Word IDs and 2D keypoints must have the same size (%d vs %d)!",
(int)signature.getWords().size(),
(int)signature.getWordsKpts().size());
}
if(!signature.getWords3().empty())
if(!signature.getWords3().empty() &&
signature.getWords().size() != signature.getWords3().size())
{
if(msg.word_ids.size() == signature.getWords3().size())
{
msg.word_pts.resize(signature.getWords3().size());
}
else
{
UERROR("Word IDs and 3D points must have the same size (%d vs %d)!",
(int)signature.getWords().size(),
(int)signature.getWords3().size());
}
UERROR("Word IDs and 3D points must have the same size (%d vs %d)!",
(int)signature.getWords().size(),
(int)signature.getWords3().size());
}
if(!msg.word_kpts.empty() || !msg.word_pts.empty())
int i=0;
msg.word_id_keys.resize(signature.getWords().size());
msg.word_id_values.resize(signature.getWords().size());
for(std::multimap<int, int>::const_iterator iter=signature.getWords().begin();
iter!=signature.getWords().end();
++iter)
{
for(size_t i=0; i<msg.word_ids.size(); ++i)
msg.word_id_keys.at(i) = iter->first;
msg.word_id_values.at(i) = iter->second;
if(signature.getWordsKpts().size() == signature.getWords().size())
{
if(!msg.word_kpts.empty())
keypointToROS(signature.getWordsKpts().at(i), msg.word_kpts.at(i));
if(!msg.word_pts.empty())
point3fToROS(signature.getWords3().at(i), msg.word_pts[i]);
if(msg.word_kpts.empty())
{
msg.word_kpts.resize(signature.getWords().size());
}
keypointToROS(signature.getWordsKpts().at(i), msg.word_kpts.at(i));
}
if(signature.getWords3().size() == signature.getWords().size())
{
if(msg.word_pts.empty())
{
msg.word_pts.resize(signature.getWords().size());
}
point3fToROS(signature.getWords3().at(i), msg.word_pts.at(i));
}
++i;
}
if(!signature.getWordsDescriptors().empty())