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

Added support to use local keypoints and descriptors from RGBDImage. OdometryROS: added new output topic odom_rgbd_image with features extracted.

Browse Source
This commit is contained in:
matlabbe
2020-11-05 16:38:08 -05:00
parent 646ad6ee7b
commit c20e38b11e
15 changed files with 353 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/rgbd_odometry.cpp
+14 -3
View File
@@ -69,7 +69,8 @@ public:
exactSync3_(0),
approxSync4_(0),
exactSync4_(0),
queueSize_(5)
queueSize_(5),
keepColor_(false)
{
}
@@ -136,11 +137,13 @@ private:
{
NODELET_FATAL("Only 4 cameras maximum supported yet.");
}
pnh.param("keep_color", keepColor_, keepColor_);
NODELET_INFO("RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
NODELET_INFO("RGBDOdometry: queue_size = %d", queueSize_);
NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
NODELET_INFO("RGBDOdometry: rgbd_cameras = %d", rgbdCameras);
NODELET_INFO("RGBDOdometry: keep_color = %s", keepColor_?"true":"false");
std::string subscribedTopicsMsg;
if(subscribeRGBD)
@@ -391,7 +394,14 @@ private:
if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
{
ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
if(keepColor_ && rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
{
ptrImage = cv_bridge::cvtColor(rgbImages[i], "bgr8");
}
else
{
ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
}
}
cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
@@ -437,7 +447,7 @@ private:
0,
rtabmap_ros::timestampFromROS(higherStamp));
this->processData(data, higherStamp);
this->processData(data, higherStamp, rgbImages.size()==1?rgbImages[0]->header.frame_id:"");
}
void callback(
@@ -647,6 +657,7 @@ private:
typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync4Policy;
message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
int queueSize_;
bool keepColor_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDOdometry, nodelet::Nodelet);