Skip to content

Commit

Permalink
realsense support
Browse files Browse the repository at this point in the history
  • Loading branch information
@raphaelchang
raphaelchang committed Sep 25, 2019
1 parent 253af41 commit 9a03e54
Show file tree
Hide file tree
Showing 3 changed files with 109 additions and 11 deletions.
58 changes: 58 additions & 0 deletions launch/slam_eval_t265.launch
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,58 @@
<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: 287.405, fy: 287.092, cx: 418.432, cy: 399.257, chi: -0.0000588, alpha: 0.675621}" />
<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="/camera/fisheye1/image_raw" />
<param name="stereo_image_topic" value="/camera/fisheye2/image_raw" />
<param name="depth_image_topic" value="" />
<param name="pose_topic" value="/camera/odom/sample" />
<param name="tracked_image_topic" value="/omni_slam/tracked" />
<param name="odometry_estimate_topic" value="/omni_slam/odometry" />
<param name="odometry_optimized_topic" value="/omni_slam/odometry_optimized" />
<param name="odometry_ground_truth_topic" value="/omni_slam/odometry_truth" />
<param name="path_estimate_topic" value="/omni_slam/odometry_path" />
<param name="path_optimized_topic" value="/omni_slam/odometry_path_optimized" />
<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="output_frame" value="map" />
<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
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]
</rosparam>
</node>
</launch>


61 changes: 50 additions & 11 deletions src/ros/eval_base.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,10 @@ void EvalBase<Stereo>::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)
{
Expand All @@ -103,10 +106,20 @@ void EvalBase<Stereo>::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<data::Frame>(new data::Frame(monoImg, posemat, depthFloatImg, pose->header.stamp.toSec(), *cameraModel_)));
if (depth_image != nullptr)
{
ProcessFrame(std::unique_ptr<data::Frame>(new data::Frame(monoImg, posemat, depthFloatImg, pose->header.stamp.toSec(), *cameraModel_)));
}
else
{
ProcessFrame(std::unique_ptr<data::Frame>(new data::Frame(monoImg, posemat, pose->header.stamp.toSec(), *cameraModel_)));
}
#else
if (first_)
{
Expand All @@ -131,7 +144,10 @@ void EvalBase<Stereo>::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)
{
Expand All @@ -147,10 +163,20 @@ void EvalBase<Stereo>::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<data::Frame>(new data::Frame(monoImg, monoImg2, depthFloatImg, posemat, stereoPose_, pose->header.stamp.toSec(), *cameraModel_)));
if (depth_image != nullptr)
{
ProcessFrame(std::unique_ptr<data::Frame>(new data::Frame(monoImg, monoImg2, depthFloatImg, posemat, stereoPose_, pose->header.stamp.toSec(), *cameraModel_)));
}
else
{
ProcessFrame(std::unique_ptr<data::Frame>(new data::Frame(monoImg, monoImg2, posemat, stereoPose_, pose->header.stamp.toSec(), *cameraModel_)));
}
#else
if (first_)
{
Expand Down Expand Up @@ -228,7 +254,9 @@ void EvalBase<Stereo>::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))
Expand All @@ -238,7 +266,7 @@ void EvalBase<Stereo>::Run()
finished = false;
break;
}
if (m.getTopic() == depthImageTopic_)
if (depthImageTopic_ != "" && m.getTopic() == depthImageTopic_)
{
depthMsg = m.instantiate<sensor_msgs::Image>();
runNext++;
Expand All @@ -253,21 +281,32 @@ void EvalBase<Stereo>::Run()
imageMsg = m.instantiate<sensor_msgs::Image>();
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>();
geometry_msgs::PoseStamped::Ptr pose(new geometry_msgs::PoseStamped());
if (poseMsg == nullptr)
{
odomMsg = m.instantiate<nav_msgs::Odometry>();
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);
}
}
}
Expand Down
1 change: 1 addition & 0 deletions src/ros/eval_base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Image.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/synchronizer.h>
Expand Down

0 comments on commit 9a03e54

Please sign in to comment.