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