From 9a03e54231a91ca7a3638ca1d7867e6a3e5811c7 Mon Sep 17 00:00:00 2001 From: raphaelchang Date: Wed, 25 Sep 2019 17:51:57 -0400 Subject: [PATCH] realsense support --- launch/slam_eval_t265.launch | 58 ++++++++++++++++++++++++++++++++++ src/ros/eval_base.cc | 61 +++++++++++++++++++++++++++++------- src/ros/eval_base.h | 1 + 3 files changed, 109 insertions(+), 11 deletions(-) create mode 100644 launch/slam_eval_t265.launch diff --git a/launch/slam_eval_t265.launch b/launch/slam_eval_t265.launch new file mode 100644 index 0000000..e816e2a --- /dev/null +++ b/launch/slam_eval_t265.launch @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + max_features_per_region: 1000 + 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 + local_bundle_adjustment_window: 0 + local_bundle_adjustment_interval: 0 + stereo_matcher_window_size: 256 + stereo_matcher_num_scales: 5 + stereo_matcher_error_threshold: 20 + stereo_matcher_epipolar_threshold: 0.008 + stereo_tf_t: [0.06410918, 0.0, 0.0] + stereo_tf_r: [0.0, 0.0, 0.0, 1.0] + + + + + diff --git a/src/ros/eval_base.cc b/src/ros/eval_base.cc index f71d7ee..0f0eff3 100644 --- a/src/ros/eval_base.cc +++ b/src/ros/eval_base.cc @@ -89,7 +89,10 @@ void EvalBase::FrameCallback(const sensor_msgs::ImageConstPtr &image, co try { cvImage = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8); - cvDepthImage = cv_bridge::toCvCopy(depth_image, sensor_msgs::image_encodings::TYPE_16UC1); + if (depth_image != nullptr) + { + cvDepthImage = cv_bridge::toCvCopy(depth_image, sensor_msgs::image_encodings::TYPE_16UC1); + } } catch (cv_bridge::Exception &e) { @@ -103,10 +106,20 @@ void EvalBase::FrameCallback(const sensor_msgs::ImageConstPtr &image, co cv::Mat monoImg; cv::cvtColor(cvImage->image, monoImg, CV_BGR2GRAY); cv::Mat depthFloatImg; - cvDepthImage->image.convertTo(depthFloatImg, CV_64FC1, 500. / 65535); + if (depth_image != nullptr) + { + cvDepthImage->image.convertTo(depthFloatImg, CV_64FC1, 500. / 65535); + } #ifdef USE_GROUND_TRUTH - ProcessFrame(std::unique_ptr(new data::Frame(monoImg, posemat, depthFloatImg, pose->header.stamp.toSec(), *cameraModel_))); + if (depth_image != nullptr) + { + ProcessFrame(std::unique_ptr(new data::Frame(monoImg, posemat, depthFloatImg, pose->header.stamp.toSec(), *cameraModel_))); + } + else + { + ProcessFrame(std::unique_ptr(new data::Frame(monoImg, posemat, pose->header.stamp.toSec(), *cameraModel_))); + } #else if (first_) { @@ -131,7 +144,10 @@ void EvalBase::FrameCallback(const sensor_msgs::ImageConstPtr &image, co { cvImage = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::BGR8); cvStereoImage = cv_bridge::toCvCopy(stereo_image, sensor_msgs::image_encodings::BGR8); - cvDepthImage = cv_bridge::toCvCopy(depth_image, sensor_msgs::image_encodings::TYPE_16UC1); + if (depth_image != nullptr) + { + cvDepthImage = cv_bridge::toCvCopy(depth_image, sensor_msgs::image_encodings::TYPE_16UC1); + } } catch (cv_bridge::Exception &e) { @@ -147,10 +163,20 @@ void EvalBase::FrameCallback(const sensor_msgs::ImageConstPtr &image, co cv::Mat monoImg2; cv::cvtColor(cvStereoImage->image, monoImg2, CV_BGR2GRAY); cv::Mat depthFloatImg; - cvDepthImage->image.convertTo(depthFloatImg, CV_64FC1, 500. / 65535); + if (depth_image != nullptr) + { + cvDepthImage->image.convertTo(depthFloatImg, CV_64FC1, 500. / 65535); + } #ifdef USE_GROUND_TRUTH - ProcessFrame(std::unique_ptr(new data::Frame(monoImg, monoImg2, depthFloatImg, posemat, stereoPose_, pose->header.stamp.toSec(), *cameraModel_))); + if (depth_image != nullptr) + { + ProcessFrame(std::unique_ptr(new data::Frame(monoImg, monoImg2, depthFloatImg, posemat, stereoPose_, pose->header.stamp.toSec(), *cameraModel_))); + } + else + { + ProcessFrame(std::unique_ptr(new data::Frame(monoImg, monoImg2, posemat, stereoPose_, pose->header.stamp.toSec(), *cameraModel_))); + } #else if (first_) { @@ -228,7 +254,9 @@ void EvalBase::Run() sensor_msgs::ImageConstPtr stereoMsg = nullptr; sensor_msgs::ImageConstPtr depthMsg = nullptr; geometry_msgs::PoseStamped::ConstPtr poseMsg = nullptr; + nav_msgs::Odometry::ConstPtr odomMsg = nullptr; int runNext = 0; + int numMsgs = depthImageTopic_ == "" ? 1 : 2; int skip = 0; int finished = true; for (rosbag::MessageInstance const m : rosbag::View(bag)) @@ -238,7 +266,7 @@ void EvalBase::Run() finished = false; break; } - if (m.getTopic() == depthImageTopic_) + if (depthImageTopic_ != "" && m.getTopic() == depthImageTopic_) { depthMsg = m.instantiate(); runNext++; @@ -253,21 +281,32 @@ void EvalBase::Run() imageMsg = m.instantiate(); runNext++; } - else if (runNext >= (Stereo ? 3 : 2) && m.getTopic() == poseTopic_) + else if (runNext >= (Stereo ? numMsgs + 1 : numMsgs) && m.getTopic() == poseTopic_) { poseMsg = m.instantiate(); + geometry_msgs::PoseStamped::Ptr pose(new geometry_msgs::PoseStamped()); + if (poseMsg == nullptr) + { + odomMsg = m.instantiate(); + pose->pose = odomMsg->pose.pose; + pose->header = odomMsg->header; + } + else + { + *pose = *poseMsg; + } runNext = 0; if (skip == 0) { - if (imageMsg != nullptr && depthMsg != nullptr && poseMsg != nullptr && (!Stereo || stereoMsg != nullptr)) + if (imageMsg != nullptr && (depthImageTopic_ == "" || depthMsg != nullptr) && (poseMsg != nullptr || odomMsg != nullptr) && (!Stereo || stereoMsg != nullptr)) { if (Stereo) { - FrameCallback(imageMsg, stereoMsg, depthMsg, poseMsg); + FrameCallback(imageMsg, stereoMsg, depthMsg, pose); } else { - FrameCallback(imageMsg, depthMsg, poseMsg); + FrameCallback(imageMsg, depthMsg, pose); } } } diff --git a/src/ros/eval_base.h b/src/ros/eval_base.h index c007515..e75847b 100644 --- a/src/ros/eval_base.h +++ b/src/ros/eval_base.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include