i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-08 02:37:45 +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
include/rtabmap_ros/CommonDataSubscriber.h
+3 -3
View File
@@ -103,9 +103,9 @@ protected:
const sensor_msgs::PointCloud2& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::GlobalDescriptor>(),
const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_ros::KeyPoint> >(),
const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_ros::Point3f> >(),
const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>()) = 0;
const std::vector<rtabmap_ros::KeyPoint> & localKeyPoints = std::vector<rtabmap_ros::KeyPoint>(),
const std::vector<rtabmap_ros::Point3f> & localPoints3d = std::vector<rtabmap_ros::Point3f>(),
const cv::Mat & localDescriptors = cv::Mat()) = 0;
virtual void commonLaserScanCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
include/rtabmap_ros/CoreWrapper.h
+3 -3
View File
@@ -137,9 +137,9 @@ private:
const sensor_msgs::PointCloud2& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::GlobalDescriptor>(),
const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_ros::KeyPoint> >(),
const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_ros::Point3f> >(),
const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>());
const std::vector<rtabmap_ros::KeyPoint> & localKeyPoints = std::vector<rtabmap_ros::KeyPoint>(),
const std::vector<rtabmap_ros::Point3f> & localPoints3d = std::vector<rtabmap_ros::Point3f>(),
const cv::Mat & localDescriptors = cv::Mat());
virtual void commonLaserScanCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
include/rtabmap_ros/GuiWrapper.h
+3 -3
View File
@@ -91,9 +91,9 @@ private:
const sensor_msgs::PointCloud2& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg,
const std::vector<rtabmap_ros::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_ros::GlobalDescriptor>(),
const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_ros::KeyPoint> >(),
const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_ros::Point3f> >(),
const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>());
const std::vector<rtabmap_ros::KeyPoint> & localKeyPoints = std::vector<rtabmap_ros::KeyPoint>(),
const std::vector<rtabmap_ros::Point3f> & localPoints3d = std::vector<rtabmap_ros::Point3f>(),
const cv::Mat & localDescriptors = cv::Mat());
virtual void commonLaserScanCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
include/rtabmap_ros/MsgConversion.h
+12 -3
View File
@@ -74,6 +74,7 @@ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg, bool ig
void toCvCopy(const rtabmap_ros::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth);
void toCvShare(const rtabmap_ros::RGBDImageConstPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth);
void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_ros::RGBDImage & msg, const std::string & sensorFrameId);
rtabmap::SensorData rgbdImageFromROS(const rtabmap_ros::RGBDImageConstPtr & image);
// copy data
@@ -90,6 +91,7 @@ cv::KeyPoint keypointFromROS(const rtabmap_ros::KeyPoint & msg);
void keypointToROS(const cv::KeyPoint & kpt, rtabmap_ros::KeyPoint & msg);
std::vector<cv::KeyPoint> keypointsFromROS(const std::vector<rtabmap_ros::KeyPoint> & msg);
void keypointsFromROS(const std::vector<rtabmap_ros::KeyPoint> & msg, std::vector<cv::KeyPoint> & kpts, int xShift=0);
void keypointsToROS(const std::vector<cv::KeyPoint> & kpts, std::vector<rtabmap_ros::KeyPoint> & msg);
rtabmap::GlobalDescriptor globalDescriptorFromROS(const rtabmap_ros::GlobalDescriptor & msg);
@@ -112,8 +114,9 @@ void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ro
cv::Point3f point3fFromROS(const rtabmap_ros::Point3f & msg);
void point3fToROS(const cv::Point3f & pt, rtabmap_ros::Point3f & msg);
std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_ros::Point3f> & msg);
void points3fToROS(const std::vector<cv::Point3f> & pts, std::vector<rtabmap_ros::Point3f> & msg);
std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_ros::Point3f> & msg, const rtabmap::Transform & transform = rtabmap::Transform());
void points3fFromROS(const std::vector<rtabmap_ros::Point3f> & msg, std::vector<cv::Point3f> & points3, const rtabmap::Transform & transform = rtabmap::Transform());
void points3fToROS(const std::vector<cv::Point3f> & pts, std::vector<rtabmap_ros::Point3f> & msg, const rtabmap::Transform & transform = rtabmap::Transform());
rtabmap::CameraModel cameraModelFromROS(
const sensor_msgs::CameraInfo & camInfo,
@@ -213,7 +216,13 @@ bool convertRGBDMsgs(
cv::Mat & depth,
std::vector<rtabmap::CameraModel> & cameraModels,
tf::TransformListener & listener,
double waitForTransform);
double waitForTransform,
const std::vector<std::vector<rtabmap_ros::KeyPoint> > & localKeyPointsMsgs = std::vector<std::vector<rtabmap_ros::KeyPoint> >(),
const std::vector<std::vector<rtabmap_ros::Point3f> > & localPoints3dMsgs = std::vector<std::vector<rtabmap_ros::Point3f> >(),
const std::vector<cv::Mat> & localDescriptorsMsgs = std::vector<cv::Mat>(),
std::vector<cv::KeyPoint> * localKeyPoints = 0,
std::vector<cv::Point3f> * localPoints3d = 0,
cv::Mat * localDescriptors = 0);
bool convertStereoMsg(
const cv_bridge::CvImageConstPtr& leftImageMsg,
include/rtabmap_ros/OdometryROS.h
+3 -2
View File
@@ -57,7 +57,7 @@ public:
OdometryROS(bool stereoParams, bool visParams, bool icpParams);
virtual ~OdometryROS();
void processData(const rtabmap::SensorData & data, const ros::Time & stamp);
void processData(const rtabmap::SensorData & data, const ros::Time & stamp, const std::string & sensorFrameId);
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resetToPose(rtabmap_ros::ResetPose::Request&, rtabmap_ros::ResetPose::Response&);
@@ -115,6 +115,7 @@ private:
ros::Publisher odomLocalMap_;
ros::Publisher odomLocalScanMap_;
ros::Publisher odomLastFrame_;
ros::Publisher odomRgbdImagePub_;
ros::ServiceServer resetSrv_;
ros::ServiceServer resetToPoseSrv_;
ros::ServiceServer pauseSrv_;
@@ -142,7 +143,7 @@ private:
bool waitIMUToinit_;
bool imuProcessed_;
std::map<double, rtabmap::IMU> imus_;
std::pair<rtabmap::SensorData, ros::Time> bufferedData_;
std::pair<rtabmap::SensorData, std::pair<ros::Time, std::string> > bufferedData_;
};
}