diff --git a/launch/slam_eval.launch b/launch/slam_eval.launch index 07d0fd4..ae2f1a1 100644 --- a/launch/slam_eval.launch +++ b/launch/slam_eval.launch @@ -3,6 +3,7 @@ + @@ -25,6 +26,7 @@ camera_model: '$(arg camera_model)' camera_parameters: $(arg camera_params) + stereo_camera_parameters: $(arg stereo_camera_params) detector_type: 'GFTT' detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5} tracker_window_size: 128 diff --git a/launch/slam_eval_kitti.launch b/launch/slam_eval_kitti.launch index 1ab2039..887aaad 100644 --- a/launch/slam_eval_kitti.launch +++ b/launch/slam_eval_kitti.launch @@ -3,6 +3,7 @@ + @@ -25,6 +26,7 @@ camera_model: '$(arg camera_model)' camera_parameters: $(arg camera_params) + stereo_camera_parameters: $(arg stereo_camera_params) detector_type: 'GFTT' detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5} tracker_window_size: 128 diff --git a/launch/slam_eval_t265.launch b/launch/slam_eval_t265.launch index e816e2a..4f0e253 100644 --- a/launch/slam_eval_t265.launch +++ b/launch/slam_eval_t265.launch @@ -3,6 +3,7 @@ + @@ -25,6 +26,7 @@ camera_model: '$(arg camera_model)' camera_parameters: $(arg camera_params) + stereo_camera_parameters: $(arg stereo_camera_params) detector_type: 'GFTT' detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5} tracker_window_size: 128 diff --git a/launch/slam_eval_tum.launch b/launch/slam_eval_tum.launch new file mode 100644 index 0000000..5f031eb --- /dev/null +++ b/launch/slam_eval_tum.launch @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + camera_model: '$(arg camera_model)' + camera_parameters: $(arg camera_params) + stereo_camera_parameters: $(arg stereo_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: 1000000.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.1010976, 0.001878, 0.00117] + stereo_tf_r: [-0.0237542, 0.0002876, -0.0004555, 0.9997177] + + + + + diff --git a/src/camera/double_sphere.h b/src/camera/double_sphere.h index 1e2347e..204538d 100644 --- a/src/camera/double_sphere.h +++ b/src/camera/double_sphere.h @@ -122,7 +122,8 @@ class DoubleSphere : public CameraModel T r2; if (alpha_ > 0.5) { - r2 = std::min(mx * mx, 1. / (2. * alpha_ - 1.)); + //r2 = std::min(mx * mx, 1. / (2. * alpha_ - 1.)); + r2 = 1. / (2. * alpha_ - 1.); } else { diff --git a/src/data/feature.cc b/src/data/feature.cc index 483014f..01d2a31 100644 --- a/src/data/feature.cc +++ b/src/data/feature.cc @@ -7,16 +7,18 @@ namespace omni_slam namespace data { -Feature::Feature(Frame &frame, cv::KeyPoint kpt, cv::Mat descriptor) +Feature::Feature(Frame &frame, cv::KeyPoint kpt, cv::Mat descriptor, bool stereo) : frame_(frame), kpt_(kpt), - descriptor_(descriptor) + descriptor_(descriptor), + stereo_(stereo) { } -Feature::Feature(Frame &frame, cv::KeyPoint kpt) +Feature::Feature(Frame &frame, cv::KeyPoint kpt, bool stereo) : frame_(frame), - kpt_(kpt) + kpt_(kpt), + stereo_(stereo) { } @@ -40,7 +42,14 @@ Vector3d Feature::GetBearing() const Vector3d cameraFramePt; Vector2d pixelPt; pixelPt << kpt_.pt.x + 0.5, kpt_.pt.y + 0.5; - frame_.GetCameraModel().UnprojectToBearing(pixelPt, cameraFramePt); + if (stereo_) + { + frame_.GetStereoCameraModel().UnprojectToBearing(pixelPt, cameraFramePt); + } + else + { + frame_.GetCameraModel().UnprojectToBearing(pixelPt, cameraFramePt); + } return util::TFUtil::CameraFrameToWorldFrame(cameraFramePt); } diff --git a/src/data/feature.h b/src/data/feature.h index 5953d74..f049f97 100644 --- a/src/data/feature.h +++ b/src/data/feature.h @@ -15,8 +15,8 @@ namespace data class Feature { public: - Feature(Frame &frame, cv::KeyPoint kpt, cv::Mat descriptor); - Feature(Frame &frame, cv::KeyPoint kpt); + Feature(Frame &frame, cv::KeyPoint kpt, cv::Mat descriptor, bool stereo = false); + Feature(Frame &frame, cv::KeyPoint kpt, bool stereo = false); const Frame& GetFrame() const; const cv::KeyPoint& GetKeypoint() const; @@ -34,6 +34,7 @@ class Feature cv::Mat descriptor_; Vector3d worldPoint_; Vector3d worldPointEstimate_; + bool stereo_; bool worldPointCached_{false}; bool worldPointEstimateCached_{false}; diff --git a/src/data/frame.cc b/src/data/frame.cc index 495386e..4234cb4 100644 --- a/src/data/frame.cc +++ b/src/data/frame.cc @@ -8,7 +8,7 @@ namespace data int Frame::lastFrameId_ = 0; -Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix &pose, Matrix &stereo_pose, double time, camera::CameraModel<> &camera_model) +Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix &pose, Matrix &stereo_pose, double time, camera::CameraModel<> &camera_model, camera::CameraModel<> &stereo_camera_model) : id_(lastFrameId_++), image_(image.clone()), stereoImage_(stereo_image.clone()), @@ -17,7 +17,8 @@ Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix invPose_(util::TFUtil::InversePoseMatrix(pose)), stereoPose_(util::TFUtil::InversePoseMatrix(stereo_pose)), timeSec_(time), - cameraModel_(camera_model) + cameraModel_(camera_model), + stereoCameraModel_(&stereo_camera_model) { hasPose_ = true; hasDepth_ = true; @@ -25,13 +26,14 @@ Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix(); } -Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix &stereo_pose, double time, camera::CameraModel<> &camera_model) +Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix &stereo_pose, double time, camera::CameraModel<> &camera_model, camera::CameraModel<> &stereo_camera_model) : id_(lastFrameId_++), image_(image.clone()), stereoImage_(stereo_image.clone()), stereoPose_(util::TFUtil::InversePoseMatrix(stereo_pose)), timeSec_(time), - cameraModel_(camera_model) + cameraModel_(camera_model), + stereoCameraModel_(&stereo_camera_model) { hasPose_ = false; hasDepth_ = false; @@ -52,14 +54,15 @@ Frame::Frame(cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraMo pose_ = invPose_ = poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix(); } -Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix &stereo_pose, double time, camera::CameraModel<> &camera_model) +Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix &stereo_pose, double time, camera::CameraModel<> &camera_model, camera::CameraModel<> &stereo_camera_model) : id_(lastFrameId_++), image_(image.clone()), stereoImage_(stereo_image.clone()), depthImage_(depth_image.clone()), stereoPose_(util::TFUtil::InversePoseMatrix(stereo_pose)), timeSec_(time), - cameraModel_(camera_model) + cameraModel_(camera_model), + stereoCameraModel_(&stereo_camera_model) { hasPose_ = false; hasDepth_ = true; @@ -81,7 +84,7 @@ Frame::Frame(cv::Mat &image, Matrix &pose, double time, camera::C poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix(); } -Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix &pose, Matrix &stereo_pose, double time, camera::CameraModel<> &camera_model) +Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix &pose, Matrix &stereo_pose, double time, camera::CameraModel<> &camera_model, camera::CameraModel<> &stereo_camera_model) : id_(lastFrameId_++), image_(image.clone()), stereoImage_(stereo_image.clone()), @@ -89,7 +92,8 @@ Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix &pose, invPose_(util::TFUtil::InversePoseMatrix(pose)), stereoPose_(util::TFUtil::InversePoseMatrix(stereo_pose)), timeSec_(time), - cameraModel_(camera_model) + cameraModel_(camera_model), + stereoCameraModel_(&stereo_camera_model) { hasPose_ = true; hasDepth_ = false; @@ -97,7 +101,7 @@ Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix &pose, poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix(); } -Frame::Frame(cv::Mat &image, Matrix &pose, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model) +Frame::Frame(cv::Mat &image, cv::Mat &depth_image, Matrix &pose, double time, camera::CameraModel<> &camera_model) : id_(lastFrameId_++), image_(image.clone()), depthImage_(depth_image.clone()), @@ -194,6 +198,11 @@ const camera::CameraModel<>& Frame::GetCameraModel() const return cameraModel_; } +const camera::CameraModel<>& Frame::GetStereoCameraModel() const +{ + return *stereoCameraModel_; +} + const Matrix& Frame::GetEstimatedPose() const { return poseEstimate_; diff --git a/src/data/frame.h b/src/data/frame.h index cc83be8..a678272 100644 --- a/src/data/frame.h +++ b/src/data/frame.h @@ -16,13 +16,13 @@ namespace data class Frame { public: - Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix &pose, Matrix &stereo_pose, double time, camera::CameraModel<> &camera_model); - Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix &stereo_pose, double time, camera::CameraModel<> &camera_model); + Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix &pose, Matrix &stereo_pose, double time, camera::CameraModel<> &camera_model, camera::CameraModel<> &stereo_camera_model); + Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix &stereo_pose, double time, camera::CameraModel<> &camera_model, camera::CameraModel<> &stereo_camera_model); Frame(cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model); - Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix &stereo_pose, double time, camera::CameraModel<> &camera_model); + Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix &stereo_pose, double time, camera::CameraModel<> &camera_model, camera::CameraModel<> &stereo_camera_model); Frame(cv::Mat &image, Matrix &pose, double time, camera::CameraModel<> &camera_model); - Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix &pose, Matrix &stereo_pose, double time, camera::CameraModel<> &camera_model); - Frame(cv::Mat &image, Matrix &pose, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model); + Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix &pose, Matrix &stereo_pose, double time, camera::CameraModel<> &camera_model, camera::CameraModel<> &stereo_camera_model); + Frame(cv::Mat &image, cv::Mat &depth_image, Matrix &pose, double time, camera::CameraModel<> &camera_model); Frame(cv::Mat &image, double time, camera::CameraModel<> &camera_model); Frame(const Frame &other); @@ -33,6 +33,7 @@ class Frame const cv::Mat& GetStereoImage(); const Matrix& GetStereoPose() const; const camera::CameraModel<>& GetCameraModel() const; + const camera::CameraModel<>& GetStereoCameraModel() const; const Matrix& GetEstimatedPose() const; const Matrix& GetEstimatedInversePose() const; const int GetID() const; @@ -72,6 +73,7 @@ class Frame Matrix invPoseEstimate_; double timeSec_; camera::CameraModel<> &cameraModel_; + camera::CameraModel<> *stereoCameraModel_{nullptr}; std::unordered_set estLandmarkIds_; diff --git a/src/module/tracking_module.cc b/src/module/tracking_module.cc index 0127aa9..49cf6ac 100644 --- a/src/module/tracking_module.cc +++ b/src/module/tracking_module.cc @@ -80,21 +80,24 @@ void TrackingModule::Update(std::unique_ptr &frame) const data::Feature *obsPrev = landmark.GetObservationByFrameID((*next(frames_.rbegin()))->GetID()); Vector2d pixelGnd; - if (frames_.back()->GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(frames_.back()->GetInversePose(), landmark.GetGroundTruth())), pixelGnd)) + if (frames_.back()->HasPose() && landmark.HasGroundTruth()) { - Vector2d pixel; - pixel << obs->GetKeypoint().pt.x, obs->GetKeypoint().pt.y; - double error = (pixel - pixelGnd).norm(); + if (frames_.back()->GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(frames_.back()->GetInversePose(), landmark.GetGroundTruth())), pixelGnd)) + { + Vector2d pixel; + pixel << obs->GetKeypoint().pt.x, obs->GetKeypoint().pt.y; + double error = (pixel - pixelGnd).norm(); - visualization_.AddTrack(cv::Point2f(pixelGnd(0), pixelGnd(1)), obsPrev->GetKeypoint().pt, obs->GetKeypoint().pt, error, i); + visualization_.AddTrack(cv::Point2f(pixelGnd(0), pixelGnd(1)), obsPrev->GetKeypoint().pt, obs->GetKeypoint().pt, error, i); - double xg = pixelGnd(0) - frames_.back()->GetImage().cols / 2. + 0.5; - double yg = pixelGnd(1) - frames_.back()->GetImage().rows / 2. + 0.5; - double rg = sqrt(xg * xg + yg * yg) / imsize; - stats_.radialErrors.emplace_back(vector{rg, error}); - stats_.frameErrors.emplace_back(vector{(double)landmark.GetNumObservations() - 1, (double)i, rg, error}); + double xg = pixelGnd(0) - frames_.back()->GetImage().cols / 2. + 0.5; + double yg = pixelGnd(1) - frames_.back()->GetImage().rows / 2. + 0.5; + double rg = sqrt(xg * xg + yg * yg) / imsize; + stats_.radialErrors.emplace_back(vector{rg, error}); + stats_.frameErrors.emplace_back(vector{(double)landmark.GetNumObservations() - 1, (double)i, rg, error}); + } } - else if (!frames_.back()->HasPose()) + else { visualization_.AddTrack(obsPrev->GetKeypoint().pt, obs->GetKeypoint().pt, i); } diff --git a/src/optimization/reprojection_error.h b/src/optimization/reprojection_error.h index 2c5e2fc..0337c87 100644 --- a/src/optimization/reprojection_error.h +++ b/src/optimization/reprojection_error.h @@ -45,7 +45,8 @@ class ReprojectionError { Matrix reprojPoint2; Matrix stereoPose = stereoFeature_.GetFrame().GetStereoPose().cast(); - camera.ProjectToImage(util::TFUtil::TransformPoint(stereoPose, camPt), reprojPoint2); + C stereoCamera(feature_.GetFrame().GetStereoCameraModel()); + stereoCamera.ProjectToImage(util::TFUtil::TransformPoint(stereoPose, camPt), reprojPoint2); reproj_error[0] += reprojPoint(0) - T(stereoFeature_.GetKeypoint().pt.x); reproj_error[1] += reprojPoint(1) - T(stereoFeature_.GetKeypoint().pt.y); } diff --git a/src/ros/eval_base.cc b/src/ros/eval_base.cc index 0f0eff3..8ec16bc 100644 --- a/src/ros/eval_base.cc +++ b/src/ros/eval_base.cc @@ -17,8 +17,8 @@ namespace omni_slam namespace ros { -template -EvalBase::EvalBase(const ::ros::NodeHandle &nh, const ::ros::NodeHandle &nh_private) +template <> +EvalBase::EvalBase(const ::ros::NodeHandle &nh, const ::ros::NodeHandle &nh_private) : nh_(nh), nhp_(nh_private), imageTransport_(nh) { std::string cameraModel; @@ -27,27 +27,53 @@ EvalBase::EvalBase(const ::ros::NodeHandle &nh, const ::ros::NodeHandle nhp_.param("image_topic", imageTopic_, std::string("/camera/image_raw")); nhp_.param("depth_image_topic", depthImageTopic_, std::string("/depth_camera/image_raw")); nhp_.param("pose_topic", poseTopic_, std::string("/pose")); - if (Stereo) + + if (cameraModel == "double_sphere") { - nhp_.param("stereo_image_topic", stereoImageTopic_, std::string("/camera2/image_raw")); - std::vector stereoT; - stereoT.reserve(3); - std::vector stereoR; - stereoR.reserve(4); - nhp_.getParam("stereo_tf_t", stereoT); - nhp_.getParam("stereo_tf_r", stereoR); - Quaterniond q(stereoR[3], stereoR[0], stereoR[1], stereoR[2]); - Vector3d t(stereoT[0], stereoT[1], stereoT[2]); - stereoPose_ = util::TFUtil::QuaternionTranslationToPoseMatrix(q, t); + cameraModel_.reset(new camera::DoubleSphere<>(cameraParams_["fx"], cameraParams_["fy"], cameraParams_["cx"], cameraParams_["cy"], cameraParams_["chi"], cameraParams_["alpha"])); } + else if (cameraModel == "perspective") + { + cameraModel_.reset(new camera::Perspective<>(cameraParams_["fx"], cameraParams_["fy"], cameraParams_["cx"], cameraParams_["cy"])); + } + else + { + ROS_ERROR("Invalid camera model specified"); + } +} + +template <> +EvalBase::EvalBase(const ::ros::NodeHandle &nh, const ::ros::NodeHandle &nh_private) + : nh_(nh), nhp_(nh_private), imageTransport_(nh) +{ + std::string cameraModel; + std::map stereoCameraParams; + nhp_.param("camera_model", cameraModel, std::string("double_sphere")); + nhp_.getParam("camera_parameters", cameraParams_); + nhp_.getParam("stereo_camera_parameters", stereoCameraParams); + nhp_.param("image_topic", imageTopic_, std::string("/camera/image_raw")); + nhp_.param("depth_image_topic", depthImageTopic_, std::string("/depth_camera/image_raw")); + nhp_.param("pose_topic", poseTopic_, std::string("/pose")); + nhp_.param("stereo_image_topic", stereoImageTopic_, std::string("/camera2/image_raw")); + std::vector stereoT; + stereoT.reserve(3); + std::vector stereoR; + stereoR.reserve(4); + nhp_.getParam("stereo_tf_t", stereoT); + nhp_.getParam("stereo_tf_r", stereoR); + Quaterniond q(stereoR[3], stereoR[0], stereoR[1], stereoR[2]); + Vector3d t(stereoT[0], stereoT[1], stereoT[2]); + stereoPose_ = util::TFUtil::QuaternionTranslationToPoseMatrix(q, t); if (cameraModel == "double_sphere") { cameraModel_.reset(new camera::DoubleSphere<>(cameraParams_["fx"], cameraParams_["fy"], cameraParams_["cx"], cameraParams_["cy"], cameraParams_["chi"], cameraParams_["alpha"])); + stereoCameraModel_.reset(new camera::DoubleSphere<>(stereoCameraParams["fx"], stereoCameraParams["fy"], stereoCameraParams["cx"], stereoCameraParams["cy"], stereoCameraParams["chi"], stereoCameraParams["alpha"])); } else if (cameraModel == "perspective") { cameraModel_.reset(new camera::Perspective<>(cameraParams_["fx"], cameraParams_["fy"], cameraParams_["cx"], cameraParams_["cy"])); + stereoCameraModel_.reset(new camera::Perspective<>(stereoCameraParams["fx"], stereoCameraParams["fy"], stereoCameraParams["cx"], stereoCameraParams["cy"])); } else { @@ -114,7 +140,7 @@ void EvalBase::FrameCallback(const sensor_msgs::ImageConstPtr &image, co #ifdef USE_GROUND_TRUTH if (depth_image != nullptr) { - ProcessFrame(std::unique_ptr(new data::Frame(monoImg, posemat, depthFloatImg, pose->header.stamp.toSec(), *cameraModel_))); + ProcessFrame(std::unique_ptr(new data::Frame(monoImg, depthFloatImg, posemat, pose->header.stamp.toSec(), *cameraModel_))); } else { @@ -171,21 +197,21 @@ void EvalBase::FrameCallback(const sensor_msgs::ImageConstPtr &image, co #ifdef USE_GROUND_TRUTH if (depth_image != nullptr) { - ProcessFrame(std::unique_ptr(new data::Frame(monoImg, monoImg2, depthFloatImg, posemat, stereoPose_, pose->header.stamp.toSec(), *cameraModel_))); + ProcessFrame(std::unique_ptr(new data::Frame(monoImg, monoImg2, depthFloatImg, posemat, stereoPose_, pose->header.stamp.toSec(), *cameraModel_, *stereoCameraModel_))); } else { - ProcessFrame(std::unique_ptr(new data::Frame(monoImg, monoImg2, posemat, stereoPose_, pose->header.stamp.toSec(), *cameraModel_))); + ProcessFrame(std::unique_ptr(new data::Frame(monoImg, monoImg2, posemat, stereoPose_, pose->header.stamp.toSec(), *cameraModel_, *stereoCameraModel_))); } #else if (first_) { - ProcessFrame(std::unique_ptr(new data::Frame(monoImg, monoImg2, posemat, stereoPose_, pose->header.stamp.toSec(), *cameraModel_))); + ProcessFrame(std::unique_ptr(new data::Frame(monoImg, monoImg2, posemat, stereoPose_, pose->header.stamp.toSec(), *cameraModel_, *stereoCameraModel_))); first_ = false; } else { - ProcessFrame(std::unique_ptr(new data::Frame(monoImg, monoImg2, stereoPose_, pose->header.stamp.toSec(), *cameraModel_))); + ProcessFrame(std::unique_ptr(new data::Frame(monoImg, monoImg2, stereoPose_, pose->header.stamp.toSec(), *cameraModel_, *stereoCameraModel_))); } #endif @@ -255,6 +281,7 @@ void EvalBase::Run() sensor_msgs::ImageConstPtr depthMsg = nullptr; geometry_msgs::PoseStamped::ConstPtr poseMsg = nullptr; nav_msgs::Odometry::ConstPtr odomMsg = nullptr; + geometry_msgs::TransformStamped::ConstPtr tfMsg = nullptr; int runNext = 0; int numMsgs = depthImageTopic_ == "" ? 1 : 2; int skip = 0; @@ -288,8 +315,23 @@ void EvalBase::Run() if (poseMsg == nullptr) { odomMsg = m.instantiate(); - pose->pose = odomMsg->pose.pose; - pose->header = odomMsg->header; + if (odomMsg == nullptr) + { + tfMsg = m.instantiate(); + if (tfMsg != nullptr) + { + pose->header = tfMsg->header; + pose->pose.position.x = -tfMsg->transform.translation.y; + pose->pose.position.y = tfMsg->transform.translation.x; + pose->pose.position.z = tfMsg->transform.translation.z; + pose->pose.orientation = tfMsg->transform.rotation; + } + } + else + { + pose->pose = odomMsg->pose.pose; + pose->header = odomMsg->header; + } } else { @@ -298,7 +340,7 @@ void EvalBase::Run() runNext = 0; if (skip == 0) { - if (imageMsg != nullptr && (depthImageTopic_ == "" || depthMsg != nullptr) && (poseMsg != nullptr || odomMsg != nullptr) && (!Stereo || stereoMsg != nullptr)) + if (imageMsg != nullptr && (depthImageTopic_ == "" || depthMsg != nullptr) && (poseMsg != nullptr || odomMsg != nullptr || tfMsg != nullptr) && (!Stereo || stereoMsg != nullptr)) { if (Stereo) { diff --git a/src/ros/eval_base.h b/src/ros/eval_base.h index e75847b..62524f9 100644 --- a/src/ros/eval_base.h +++ b/src/ros/eval_base.h @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -40,8 +41,6 @@ class EvalBase image_transport::ImageTransport imageTransport_; - std::unique_ptr> cameraModel_; - private: void FrameCallback(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::ImageConstPtr &depth_image, const geometry_msgs::PoseStamped::ConstPtr &pose); void FrameCallback(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::ImageConstPtr &stereo_image, const sensor_msgs::ImageConstPtr &depth_image, const geometry_msgs::PoseStamped::ConstPtr &pose); @@ -67,6 +66,8 @@ class EvalBase std::string poseTopic_; std::map cameraParams_; Matrix stereoPose_; + std::unique_ptr> cameraModel_; + std::unique_ptr> stereoCameraModel_; bool first_{true}; }; diff --git a/src/ros/odometry_eval.cc b/src/ros/odometry_eval.cc index 8d481a6..3927fd6 100644 --- a/src/ros/odometry_eval.cc +++ b/src/ros/odometry_eval.cc @@ -53,7 +53,7 @@ void OdometryEval::InitPublishers() this->nhp_.param("odometry_optimized_topic", outputOptTopic, string("/omni_slam/odometry_optimized")); this->nhp_.param("odometry_ground_truth_topic", outputGndTopic, string("/omni_slam/odometry_truth")); this->nhp_.param("path_estimate_topic", outputPathTopic, string("/omni_slam/odometry_path")); - this->nhp_.param("path_optimized_topic", outputOptTopic, string("/omni_slam/odometry_path_optimized")); + this->nhp_.param("path_optimized_topic", outputPathOptTopic, string("/omni_slam/odometry_path_optimized")); this->nhp_.param("path_ground_truth_topic", outputPathGndTopic, string("/omni_slam/odometry_path_truth")); odometryPublisher_ = this->nh_.template advertise(outputTopic, 2); odometryOptPublisher_ = this->nh_.template advertise(outputOptTopic, 2); diff --git a/src/stereo/stereo_matcher.cc b/src/stereo/stereo_matcher.cc index 5264d2b..facabb4 100644 --- a/src/stereo/stereo_matcher.cc +++ b/src/stereo/stereo_matcher.cc @@ -66,7 +66,7 @@ int StereoMatcher::Match(data::Frame &frame, std::vector &landma { int inx = *it; Vector3d &bearing1 = bearings1[inx]; - data::Feature feat(frame, matchedPoints[inx]); + data::Feature feat(frame, matchedPoints[inx], true); Vector3d bearing2 = util::TFUtil::WorldFrameToCameraFrame(feat.GetBearing().normalized()); RowVector3d epiplane1 = bearing2.transpose() * E;