Skip to content

Commit

Permalink
Merge pull request #17 from bomiquel/melodic
Browse files Browse the repository at this point in the history 
Melodic
  • Loading branch information
@bomiquel
bomiquel authored Jun 30, 2023
2 parents 53c8d8b + 87c590d commit 3497699
Show file tree
Hide file tree
Showing 18 changed files with 404 additions and 461 deletions.
92 changes: 63 additions & 29 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,7 @@ CITATION:
```
Installation (Ubuntu + ROS Indigo)
Installation (Ubuntu + ROS)
-------
1) Install the dependencies
Expand All @@ -61,12 +60,48 @@ rosmake
Parameters
-------
* `odom_topic` - Visual odometry topic (type nav_msgs::Odometry).
* `camera_topic` - The namespace of your stereo camera.
* `refine` - Refine odometry (true/false).
* `distance_between_keyframes` - Minimum distance (m) between keyframes.
* `working_directory` - Directory where all output files will be stored.
* `feature_detector_selection` - Name of the feature detector to be used (ORB or SIFT).
* `lc_min_inliers` - Minimum number of inliers to close a loop.
* `lc_epipolar_thresh` - Maximum reprojection error allowed.
* `map_frame_name` - Frame of the slam output
* `lc_neighbors` - Number of neighbours to recover in order to increase the possibility of closing the loop.
* `lc_discard_window` - Window size of discarded vertices.
* `ransac_iterations` - Number of RANSAC iterations for the solvePnPRansac
Subscribed topics
-------
Other (hard-coded) parameters
* `odom` - Input odometry (type nav_msgs::Odometry).
* `left_image_rect_color` - Left image in color and rectified (type sensor_msgs::Image).
* `right_image_rect_color`- Right image in color and rectified (type sensor_msgs::Image).
* `left_camera_info` - Extrinsic and intrinsic camera parameters (type sensor_msgs::CameraInfo).
* `right_camera_info`- Extrinsic and intrinsic camera parameters (type sensor_msgs::CameraInfo).
* `include/constants.h` - Contains the set of node parameters. Default parameters should work.
Published Topics
-------
* `/stereo_slam/odometry` - The odometry of the robot (type nav_msgs::Odometry).
* `/stereo_slam/graph_poses` - The updated graph poses (type stereo_slam::GraphPoses).
* `/stereo_slam/graph_camera_odometry` - Absolute pose of the camera (type nav_msgs::Odometry).
* `/stereo_slam/graph_robot_odometry` - Absolute pose of the vehicle (type nav_msgs::Odometry).
* `/stereo_slam/keyframes` - Number of inserted keyframes (type std_msgs::Int32).
* `/stereo_slam/keypoints_clustering` - Image containing the keypoint clusters (type sensor_msgs::Image).
* `/stereo_slam/stereo_matches_num` - Number of correspondences between the stereo pair (type std_msgs::Int32).
* `/stereo_slam/stereo_matches_img` - Correspondences between the stereo pair (type sensor_msgs::Image).
* `/stereo_slam/num_clusters` - Number of clusters (type std_msgs::Int32).
* `/stereo_slam/loop_closing_inliers_img` - Image of the loop closing correspondences. Correspondences are keyframe-to-multi-keyframe (type sensor_msgs::Image).
* `/stereo_slam/loop_closing_matches_num` - Number of matches of each loop closing (type std_msgs::Int32).
* `/stereo_slam/loop_closing_inliers_num` - Number of inliers of each loop closing (type std_msgs::Int32).
* `/stereo_slam/loop_closing_queue` - Number of keyframes waiting on the loop closing queue. Please monitor this topic, to check the real-time performance: if this number grows indefinitely it means that your system is not able to process all the keyframes, then, scale your images (type std_msgs::String).
* `/stereo_slam/loop_closings_num` - Number of loop closings found (type std_msgs::Int32).
* `/robot_0/stereo_slam/time_tracking` - The elapsed time of each iteration of the tracking thread (type stereo_slam::TimeTracking).
* `/robot_0/stereo_slam/time_graph` - The elapsed time of each iteration of the graph thread (type stereo_slam::TimeGraph).
* `/robot_0/stereo_slam/time_loop_closing` - The elapsed time of each iteration of the loop closing thread (type stereo_slam::TimeLoopClosing).
* `/robot_0/stereo_slam/sub_time_loop_closing` - The elapsed time in certain processes of the loop closing thread (type stereo_slam::SubTimeLoopClosing).
Run the node
Expand All @@ -76,44 +111,43 @@ You can run the node using the following launch file (please, for a better perfo
```bash
<launch>
<arg name="camera" default="/stereo"/>
<arg name = "camera" default = "/stereo"/>
<arg name = "robot_name" default = "robot_0"/>
<!-- Run the stereo image proc -->
<node ns="$(arg camera)" pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc" />
<node ns = "$(arg camera)" pkg = "stereo_image_proc" type = "stereo_image_proc" name = "stereo_image_proc" />
<node pkg="viso2_ros" type="stereo_odometer" name="stereo_odometer">
<remap from="stereo" to="$(arg camera)"/>
<remap from="image" to="image_rect"/>
<node pkg = "viso2_ros" type = "stereo_odometer" name = "stereo_odometer">
<remap from = "stereo" to = "$(arg camera)"/>
<remap from = "image" to = "image_rect"/>
</node>
<node pkg="stereo_slam" type="localization" name="stereo_slam" output="screen">
<param name="odom_topic" value="/stereo_odometer/odometry"/>
<param name="camera_topic" value="$(arg camera)"/>
<node pkg="stereo_slam" type="localization" name="stereo_slam" output="screen" if = "$(eval enable_decimate_x1 == true)">
<remap from = "odom" to = "stereo_odometer/odometry"/>
<remap from = "left_camera_info" to = "/$(arg camera)/left/camera_info"/>
<remap from = "right_camera_info" to = "/$(arg camera)/right/camera_info"/>
<remap from = "left_image_rect_color" to = "/$(arg camera)/left/image_rect_color"/>
<remap from = "right_image_rect_color" to = "/$(arg camera)/right/image_rect_color"/>
<param name = "refine" value = "false"/>
<param name = "distance_between_keyframes" value = "0.5"/>
<param name = "feature_detector_selection" value = "ORB"/>
<param name = "lc_min_inliers" value = "30"/>
<param name = "lc_epipolar_thresh" value = "1.0"/>
<param name = "map_frame_name" value = "/$(arg robot_name)/map"/>
<param name = "lc_neighbors" value = "5"/>
<param name = "lc_discard_window" value = "20"/>
<param name = "ransac_iterations" value = "150"/>
</node>
</launch>
```
Published Topics
-------
* `/stereo_slam/odometry` - The vehicle pose (type nav_msgs::Odometry).
* `/stereo_slam/graph_poses` - The updated graph poses (type stereo_slam::GraphPoses).
* `/stereo_slam/keyframes` - Number of inserted keyframes (type std_msgs::String).
* `/stereo_slam/keypoints_clustering` - Image containing the keypoint clusters (type sensor_msgs::Image).
* `/stereo_slam/loop_closing_matchings` - Image of the loop closing correspondences. Correspondences are keyframe-to-multi-keyframe (type sensor_msgs::Image).
* `/stereo_slam/loop_closing_queue` - Number of keyframes waiting on the loop closing queue. Please monitor this topic, to check the real-time performance: if this number grows indefinitely it means that your system is not able to process all the keyframes, then, scale your images. (type std_msgs::String).
* `/stereo_slam/loop_closings` - Number of loop closings found (type std_msgs::String).
* `/stereo_slam/pointcloud` - The pointcloud for every keyframe (type sensor_msgs::PointCloud2).
* `/stereo_slam/tracking_overlap` - Image containing a representation of the traking overlap. Used to decide when to insert a new keyframe into the graph (type sensor_msgs::Image).
* `/stereo_slam/camera_params` - The optimized (calibrated) camera parameters after every loop closure (type stereo_slam::CameraParams).
Saved data
-------
The node stores some data into the stereo_slam directory during the execution:
* `haloc` - A folder containing all the files needed for the libhaloc library, which is responsible for loop closing detection. You do not need this folder at all.
* `keyframes` - Stores the left stereo image for every keyframe (with the possibility of drawing the keypoint clustering over the image).
* `loop_closures` - Stores all the images published in the topic `/stereo_slam/loop_closing_matchings`.
* `pointclouds` - Stores all the pointclouds published in the topic `/stereo_slam/pointcloud`.
* `loop_closures` - Stores all the images published in the topic `/stereo_slam/loop_closing_inliers_img`.
Online graph viewer
Expand Down
18 changes: 8 additions & 10 deletions include/cluster.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,6 @@
#include <opencv2/opencv.hpp>
#include <opencv2/features2d/features2d.hpp>

using namespace std;

namespace slam
{

Expand All @@ -28,12 +26,12 @@ class Cluster

/** \brief Class constructor
*/
Cluster(int id, int frame_id, tf::Transform camera_pose, vector<cv::KeyPoint> kp_l, vector<cv::KeyPoint> kp_r, cv::Mat orb_desc, cv::Mat sift_desc, vector<cv::Point3f> points);
Cluster(int id, int frame_id, tf::Transform camera_pose, std::vector<cv::KeyPoint> kp_l, std::vector<cv::KeyPoint> kp_r, cv::Mat orb_desc, cv::Mat sift_desc, std::vector<cv::Point3f> points);

/** \brief Computes and returns the 3D points in world coordinates
* @return the 3D points in world coordinates
*/
vector<cv::Point3f> getWorldPoints();
std::vector<cv::Point3f> getWorldPoints();

/** \brief Get the cluster id
*/
Expand All @@ -45,11 +43,11 @@ class Cluster

/** \brief Get left cv::KeyPoints
*/
inline vector<cv::KeyPoint> getLeftKp() const {return kp_l_;}
inline std::vector<cv::KeyPoint> getLeftKp() const {return kp_l_;}

/** \brief Get right cv::KeyPoints
*/
inline vector<cv::KeyPoint> getRightKp() const {return kp_r_;}
inline std::vector<cv::KeyPoint> getRightKp() const {return kp_r_;}

/** \brief Get orb descriptors
*/
Expand All @@ -61,7 +59,7 @@ class Cluster

/** \brief Get 3D camera points
*/
inline vector<cv::Point3f> getPoints() const {return points_;}
inline std::vector<cv::Point3f> getPoints() const {return points_;}

/** \brief Get camera pose
*/
Expand All @@ -76,14 +74,14 @@ class Cluster

tf::Transform camera_pose_; //!> Camera world position

vector<cv::KeyPoint> kp_l_; //!> left cv::KeyPoints.
std::vector<cv::KeyPoint> kp_l_; //!> left cv::KeyPoints.

vector<cv::KeyPoint> kp_r_; //!> left cv::KeyPoints.
std::vector<cv::KeyPoint> kp_r_; //!> left cv::KeyPoints.

cv::Mat orb_desc_; //!> ORB descriptors
cv::Mat sift_desc_; //!> Sift descriptors

vector<cv::Point3f> points_; //!> Stereo 3D points in camera frame
std::vector<cv::Point3f> points_; //!> Stereo 3D points in camera frame

};

Expand Down
47 changes: 0 additions & 47 deletions include/constants.h
View file

This file was deleted.

55 changes: 26 additions & 29 deletions include/frame.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,13 +18,10 @@

#include <pcl/point_types.h>

using namespace std;
using namespace pcl;

typedef PointXYZ PointXYZ;
typedef PointXYZRGB PointRGB;
typedef PointCloud<PointXYZ> PointCloudXYZ;
typedef PointCloud<PointRGB> PointCloudRGB;
typedef pcl::PointXYZ PointXYZ;
typedef pcl::PointXYZRGB PointRGB;
typedef pcl::PointCloud<PointXYZ> PointCloudXYZ;
typedef pcl::PointCloud<PointRGB> PointCloudRGB;

namespace slam
{
Expand All @@ -40,7 +37,7 @@ class Frame

/** \brief Class constructor
*/
Frame(cv::Mat l_img, cv::Mat r_img, image_geometry::StereoCameraModel camera_model, double timestamp, string feature_detector);
Frame(cv::Mat l_img, cv::Mat r_img, image_geometry::StereoCameraModel camera_model, double timestamp, std::string feature_detector);

/** \brief Get left image
*/
Expand All @@ -52,24 +49,24 @@ class Frame

/** \brief Get left keypoints
*/
inline vector<cv::KeyPoint> getLeftKp() const {return l_kp_;}
inline std::vector<cv::KeyPoint> getLeftKp() const {return l_kp_;}

/** \brief Set left keypoints
* \param vector of keypoints
*/
inline void setLeftKp(const vector<cv::KeyPoint>& l_kp){l_kp_ = l_kp;}
inline void setLeftKp(const std::vector<cv::KeyPoint>& l_kp){l_kp_ = l_kp;}

/** \brief Get right keypoints
*/
inline vector<cv::KeyPoint> getRightKp() const {return r_kp_;}
inline std::vector<cv::KeyPoint> getRightKp() const {return r_kp_;}

/** \brief Get left non-filtered keypoints
*/
inline vector<cv::KeyPoint> getNonFilteredLeftKp() const {return l_nonfiltered_kp_;}
inline std::vector<cv::KeyPoint> getNonFilteredLeftKp() const {return l_nonfiltered_kp_;}

/** \brief Get right non-filtered keypoints
*/
inline vector<cv::KeyPoint> getNonFilteredRightKp() const {return r_nonfiltered_kp_;}
inline std::vector<cv::KeyPoint> getNonFilteredRightKp() const {return r_nonfiltered_kp_;}

/** \brief Get left descriptors
*/
Expand All @@ -82,11 +79,11 @@ class Frame

/** \brief Get stereo matches
*/
inline vector<cv::DMatch> getMatches() const {return matches_filtered_;}
inline std::vector<cv::DMatch> getMatches() const {return matches_filtered_;}

/** \brief Get 3D in camera frame
*/
inline vector<cv::Point3f> getCameraPoints() const {return camera_points_;}
inline std::vector<cv::Point3f> getCameraPoints() const {return camera_points_;}

/** \brief Set frame id
* \param vector of 3D points
Expand All @@ -96,7 +93,7 @@ class Frame
/** \brief Set 3D
* \param vector of 3D points
*/
inline void setCameraPoints(const vector<cv::Point3f>& points_3d){camera_points_ = points_3d;}
inline void setCameraPoints(const std::vector<cv::Point3f>& points_3d){camera_points_ = points_3d;}

/** \brief Set camera pose
* \param camera pose
Expand Down Expand Up @@ -128,11 +125,11 @@ class Frame

/** \brief Return the clustering for the current frame
*/
inline vector< vector<int> > getClusters() const {return clusters_;}
inline std::vector< std::vector<int> > getClusters() const {return clusters_;}

/** \brief Return the clustering for the current frame
*/
inline vector<Eigen::Vector4f> getClusterCentroids() const {return cluster_centroids_;}
inline std::vector<Eigen::Vector4f> getClusterCentroids() const {return cluster_centroids_;}

/** \brief Get frame timestamp
*/
Expand Down Expand Up @@ -179,7 +176,7 @@ class Frame
* \param query keypoint
* \param maximum distance to considerate a keypoint into the region
*/
vector<int> regionQuery(vector<cv::KeyPoint> *keypoints, cv::KeyPoint *keypoint, float eps);
std::vector<int> regionQuery(std::vector<cv::KeyPoint> *keypoints, cv::KeyPoint *keypoint, float eps);

private:

Expand All @@ -188,24 +185,24 @@ class Frame
cv::Mat l_img_; //!> Left image
cv::Mat r_img_; //!> Right image

string feature_detector_;
std::string feature_detector_;

vector<cv::KeyPoint> l_kp_; //!> Left keypoints.
vector<cv::KeyPoint> r_kp_; //!> Right keypoints.
//!
vector<cv::KeyPoint> l_nonfiltered_kp_; //!> Left non-filtered keypoints.
vector<cv::KeyPoint> r_nonfiltered_kp_; //!> Right non-filtered keypoints.
std::vector<cv::KeyPoint> l_kp_; //!> Left keypoints.
std::vector<cv::KeyPoint> r_kp_; //!> Right keypoints.

std::vector<cv::KeyPoint> l_nonfiltered_kp_; //!> Left non-filtered keypoints.
std::vector<cv::KeyPoint> r_nonfiltered_kp_; //!> Right non-filtered keypoints.

cv::Mat l_desc_; //!> Left descriptors.
cv::Mat r_desc_; //!> Right descriptors.

vector<cv::DMatch> matches_filtered_; //!> Filtered stereo matches
std::vector<cv::DMatch> matches_filtered_; //!> Filtered stereo matches

vector<cv::Point3f> camera_points_; //!> Stereo 3D points in camera frame
std::vector<cv::Point3f> camera_points_; //!> Stereo 3D points in camera frame

vector< vector<int> > clusters_; //!> Keypoints clustering
std::vector< std::vector<int> > clusters_; //!> Keypoints clustering

vector<Eigen::Vector4f> cluster_centroids_; //!> Central point for every cluster
std::vector<Eigen::Vector4f> cluster_centroids_; //!> Central point for every cluster

tf::Transform camera_pose_; //!> Camera world position for this frame

Expand Down
Loading

0 comments on commit 3497699

Please sign in to comment.