-
Notifications
You must be signed in to change notification settings - Fork 5
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
3a62f35
commit 13e38e0
Showing
15 changed files
with
238 additions
and
20 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,52 @@ | ||
| <launch> | ||
| <arg name="bag_file" default="" /> | ||
| <arg name="results_file" default="$(arg bag_file).slam.hdf5" /> | ||
| <arg name="camera_model" default="double_sphere" /> | ||
| <arg name="camera_params" default="{fx: 295.936, fy: 295.936, cx: 512, cy: 512, chi: 0.3, alpha: 0.6666667}" /> | ||
| <arg name="rate" default="1" /> | ||
| <node pkg="omni_slam_eval" type="omni_slam_slam_eval_node" name="omni_slam_slam_eval_node" required="true" output="screen"> | ||
| <param name="bag_file" value="$(arg bag_file)" /> | ||
| <param name="results_file" value="$(arg results_file)" /> | ||
| <param name="image_topic" value="/unity_ros/Sphere/FisheyeCamera/image_raw" /> | ||
| <param name="stereo_image_topic" value="/unity_ros/Sphere/FisheyeCamera2/image_raw" /> | ||
| <param name="depth_image_topic" value="/unity_ros/Sphere/FisheyeDepthCamera/image_raw" /> | ||
| <param name="pose_topic" value="/unity_ros/Sphere/TrueState/pose" /> | ||
| <param name="tracked_image_topic" value="/omni_slam/tracked" /> | ||
| <param name="odometry_estimate_topic" value="/omni_slam/odometry" /> | ||
| <param name="odometry_ground_truth_topic" value="/omni_slam/odometry_truth" /> | ||
| <param name="path_estimate_topic" value="/omni_slam/odometry_path" /> | ||
| <param name="path_ground_truth_topic" value="/omni_slam/odometry_path_truth" /> | ||
| <param name="point_cloud_topic" value="/omni_slam/reconstructed" /> | ||
| <param name="stereo_matched_topic" value="/omni_slam/stereo_matched" /> | ||
| <param name="rate" value="$(arg rate)" /> | ||
| <rosparam subst_value="true"> | ||
| camera_model: '$(arg camera_model)' | ||
| camera_parameters: $(arg camera_params) | ||
| detector_type: 'GFTT' | ||
| detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5} | ||
| tracker_window_size: 128 | ||
| tracker_num_scales: 4 | ||
| tracker_checker_epipolar_threshold: 0.008 | ||
| tracker_checker_iterations: 1000 | ||
| tracker_delta_pixel_error_threshold: 0.0 | ||
| tracker_error_threshold: 20.0 | ||
| min_features_per_region: 100 | ||
| pnp_inlier_threshold: 3.0 | ||
| pnp_iterations: 3000 | ||
| max_reprojection_error: 5.0 | ||
| min_triangulation_angle: 3.0 | ||
| bundle_adjustment_max_iterations: 1000 | ||
| bundle_adjustment_loss_coefficient: 0.05 | ||
| bundle_adjustment_logging: true | ||
| bundle_adjustment_num_threads: 20 | ||
| stereo_matcher_window_size: 256 | ||
| stereo_matcher_num_scales: 5 | ||
| stereo_matcher_error_threshold: 20 | ||
| stereo_matcher_epipolar_threshold: 0.008 | ||
| stereo_tf_t: [2.0, 0.0, 0.0] | ||
| stereo_tf_r: [0.0, 0.0, 0.0, 1.0] | ||
| </rosparam> | ||
| </node> | ||
| </launch> | ||
|
|
||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,13 @@ | ||
| #include "ros/slam_eval.h" | ||
|
|
||
| int main(int argc, char *argv[]) | ||
| { | ||
| ros::init(argc, argv, "omni_slam_slam_eval_node"); | ||
|
|
||
| omni_slam::ros::SLAMEval slam_eval; | ||
| slam_eval.Run(); | ||
|
|
||
| return 0; | ||
| } | ||
|
|
||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,63 @@ | ||
| #include "slam_eval.h" | ||
|
|
||
| #include "odometry/pnp.h" | ||
| #include "optimization/bundle_adjuster.h" | ||
| #include "module/tracking_module.h" | ||
|
|
||
| #include "geometry_msgs/PoseStamped.h" | ||
| #include "nav_msgs/Path.h" | ||
|
|
||
| using namespace std; | ||
|
|
||
| namespace omni_slam | ||
| { | ||
| namespace ros | ||
| { | ||
|
|
||
| SLAMEval::SLAMEval(const ::ros::NodeHandle &nh, const ::ros::NodeHandle &nh_private) | ||
| : OdometryEval<true>(nh, nh_private), ReconstructionEval<true>(nh, nh_private), StereoEval(nh, nh_private) | ||
| { | ||
| } | ||
|
|
||
| void SLAMEval::InitPublishers() | ||
| { | ||
| OdometryEval<true>::InitPublishers(); | ||
| ReconstructionEval<true>::InitPublishers(); | ||
| StereoEval::InitPublishers(); | ||
| } | ||
|
|
||
| void SLAMEval::ProcessFrame(unique_ptr<data::Frame> &&frame) | ||
| { | ||
| trackingModule_->Update(frame); | ||
| odometryModule_->Update(trackingModule_->GetLandmarks(), *trackingModule_->GetFrames().back()); | ||
| reconstructionModule_->Update(trackingModule_->GetLandmarks()); | ||
| trackingModule_->Redetect(); | ||
| stereoModule_->Update(*trackingModule_->GetFrames().back(), trackingModule_->GetLandmarks()); | ||
| } | ||
|
|
||
| void SLAMEval::Finish() | ||
| { | ||
| ReconstructionEval<true>::Finish(); | ||
| PublishOdometry(); | ||
| } | ||
|
|
||
| void SLAMEval::GetResultsData(std::map<std::string, std::vector<std::vector<double>>> &data) | ||
| { | ||
| } | ||
|
|
||
| void SLAMEval::Visualize(cv_bridge::CvImagePtr &base_img) | ||
| { | ||
| visualized_ = false; | ||
| ReconstructionEval<true>::Visualize(base_img); | ||
| OdometryEval<true>::Visualize(base_img); | ||
| } | ||
|
|
||
| void SLAMEval::Visualize(cv_bridge::CvImagePtr &base_img, cv_bridge::CvImagePtr &base_stereo_img) | ||
| { | ||
| Visualize(base_img); | ||
| StereoEval::Visualize(base_img, base_stereo_img); | ||
| } | ||
|
|
||
| } | ||
| } | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,34 @@ | ||
| #ifndef _SLAM_EVAL_H_ | ||
| #define _SLAM_EVAL_H_ | ||
|
|
||
| #include "odometry_eval.h" | ||
| #include "reconstruction_eval.h" | ||
| #include "stereo_eval.h" | ||
| #include <ros/ros.h> | ||
| #include <vector> | ||
|
|
||
| namespace omni_slam | ||
| { | ||
| namespace ros | ||
| { | ||
|
|
||
| class SLAMEval : public OdometryEval<true>, ReconstructionEval<true>, StereoEval | ||
| { | ||
| public: | ||
| SLAMEval(const ::ros::NodeHandle &nh, const ::ros::NodeHandle &nh_private); | ||
| SLAMEval() : SLAMEval(::ros::NodeHandle(), ::ros::NodeHandle("~")) {} | ||
|
|
||
| private: | ||
| void InitPublishers(); | ||
|
|
||
| void ProcessFrame(std::unique_ptr<data::Frame> &&frame); | ||
| void Finish(); | ||
| void GetResultsData(std::map<std::string, std::vector<std::vector<double>>> &data); | ||
| void Visualize(cv_bridge::CvImagePtr &base_img); | ||
| void Visualize(cv_bridge::CvImagePtr &base_img, cv_bridge::CvImagePtr &base_stereo_img); | ||
| }; | ||
|
|
||
| } | ||
| } | ||
|
|
||
| #endif /* _SLAM_EVAL_H_ */ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.