From 510778d5e411b7401a6076b7638b7f72f3f1c258 Mon Sep 17 00:00:00 2001 From: raphaelchang Date: Sun, 29 Sep 2019 02:58:42 -0400 Subject: [PATCH] try essential matrix odometry --- CMakeLists.txt | 3 +- src/feature/{tracker.cc => lk_tracker.cc} | 8 +- src/feature/lk_tracker.h | 34 ++++ src/feature/tracker.h | 17 +- src/module/odometry_module.cc | 12 +- src/module/odometry_module.h | 10 +- src/module/tracking_module.cc | 2 +- src/odometry/five_point.cc | 197 +++++++++++++++++++++- src/odometry/five_point.h | 14 +- src/odometry/pnp.cc | 26 ++- src/odometry/pnp.h | 6 +- src/odometry/pose_estimator.cc | 15 ++ src/odometry/pose_estimator.h | 22 +++ src/ros/odometry_eval.cc | 14 +- src/ros/slam_eval.cc | 2 +- src/ros/tracking_eval.cc | 4 +- src/util/tf_util.h | 16 +- 17 files changed, 330 insertions(+), 72 deletions(-) rename src/feature/{tracker.cc => lk_tracker.cc} (90%) create mode 100644 src/feature/lk_tracker.h create mode 100644 src/odometry/pose_estimator.cc create mode 100644 src/odometry/pose_estimator.h diff --git a/CMakeLists.txt b/CMakeLists.txt index b7c53a0..b9bc989 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,10 +63,11 @@ add_library(omni_slam_eval_lib src/data/frame.cc src/data/feature.cc src/data/landmark.cc - src/feature/tracker.cc + src/feature/lk_tracker.cc src/feature/detector.cc src/feature/matcher.cc src/reconstruction/triangulator.cc + src/odometry/pose_estimator.cc src/odometry/pnp.cc src/odometry/five_point.cc src/optimization/bundle_adjuster.cc diff --git a/src/feature/tracker.cc b/src/feature/lk_tracker.cc similarity index 90% rename from src/feature/tracker.cc rename to src/feature/lk_tracker.cc index 421bdb7..2000403 100644 --- a/src/feature/tracker.cc +++ b/src/feature/lk_tracker.cc @@ -1,4 +1,4 @@ -#include "tracker.h" +#include "lk_tracker.h" #include #include "util/tf_util.h" @@ -7,7 +7,7 @@ namespace omni_slam namespace feature { -Tracker::Tracker(const int window_size, const int num_scales, const float delta_pix_err_thresh, const float err_thresh, const int term_count, const double term_eps) +LKTracker::LKTracker(const int window_size, const int num_scales, const float delta_pix_err_thresh, const float err_thresh, const int term_count, const double term_eps) : windowSize_(window_size / pow(2, num_scales), window_size / pow(2, num_scales)), numScales_(num_scales), errThresh_(err_thresh), @@ -17,12 +17,12 @@ Tracker::Tracker(const int window_size, const int num_scales, const float delta_ { } -void Tracker::Init(data::Frame &init_frame) +void LKTracker::Init(data::Frame &init_frame) { prevFrame_ = &init_frame; } -int Tracker::Track(std::vector &landmarks, data::Frame &cur_frame, std::vector &errors) +int LKTracker::Track(std::vector &landmarks, data::Frame &cur_frame, std::vector &errors) { if (prevFrame_ == nullptr) { diff --git a/src/feature/lk_tracker.h b/src/feature/lk_tracker.h new file mode 100644 index 0000000..b60a7b5 --- /dev/null +++ b/src/feature/lk_tracker.h @@ -0,0 +1,34 @@ +#ifndef _LK_TRACKER_H_ +#define _LK_TRACKER_H_ + +#include "tracker.h" +#include +#include "data/frame.h" +#include "data/landmark.h" +#include "odometry/five_point.h" +#include + +namespace omni_slam +{ +namespace feature +{ + +class LKTracker : public Tracker +{ +public: + LKTracker(const int window_size, const int num_scales, const float delta_pix_err_thresh = 5., const float err_thresh = 20., const int term_count = 50, const double term_eps = 0.01); + void Init(data::Frame &init_frame); + int Track(std::vector &landmarks, data::Frame &cur_frame, std::vector &errors); + +private: + cv::TermCriteria termCrit_; + const cv::Size windowSize_; + const int numScales_; + const float errThresh_; + const float deltaPixErrThresh_; + data::Frame *prevFrame_; +}; + +} +} +#endif /* _LK_TRACKER_H_ */ diff --git a/src/feature/tracker.h b/src/feature/tracker.h index f9ad316..c2d109f 100644 --- a/src/feature/tracker.h +++ b/src/feature/tracker.h @@ -1,11 +1,8 @@ #ifndef _TRACKER_H_ #define _TRACKER_H_ -#include #include "data/frame.h" #include "data/landmark.h" -#include "odometry/five_point.h" -#include namespace omni_slam { @@ -15,19 +12,11 @@ namespace feature class Tracker { public: - Tracker(const int window_size, const int num_scales, const float delta_pix_err_thresh = 5., const float err_thresh = 20., const int term_count = 50, const double term_eps = 0.01); - void Init(data::Frame &init_frame); - int Track(std::vector &landmarks, data::Frame &cur_frame, std::vector &errors); - -private: - cv::TermCriteria termCrit_; - const cv::Size windowSize_; - const int numScales_; - const float errThresh_; - const float deltaPixErrThresh_; - data::Frame *prevFrame_; + virtual void Init(data::Frame &init_frame) = 0; + virtual int Track(std::vector &landmarks, data::Frame &cur_frame, std::vector &errors) = 0; }; } } + #endif /* _TRACKER_H_ */ diff --git a/src/module/odometry_module.cc b/src/module/odometry_module.cc index ee465cc..2e6bd84 100644 --- a/src/module/odometry_module.cc +++ b/src/module/odometry_module.cc @@ -7,24 +7,24 @@ namespace omni_slam namespace module { -OdometryModule::OdometryModule(std::unique_ptr &pnp, std::unique_ptr &bundle_adjuster) - : pnp_(std::move(pnp)), +OdometryModule::OdometryModule(std::unique_ptr &pose_estimator, std::unique_ptr &bundle_adjuster) + : poseEstimator_(std::move(pose_estimator)), bundleAdjuster_(std::move(bundle_adjuster)) { } -OdometryModule::OdometryModule(std::unique_ptr &&pnp, std::unique_ptr &&bundle_adjuster) - : OdometryModule(pnp, bundle_adjuster) +OdometryModule::OdometryModule(std::unique_ptr &&pose_estimator, std::unique_ptr &&bundle_adjuster) + : OdometryModule(pose_estimator, bundle_adjuster) { } -void OdometryModule::Update(std::vector &landmarks, data::Frame &frame) +void OdometryModule::Update(std::vector &landmarks, std::vector> &frames) { if (landmarks.size() == 0) { return; } - pnp_->Compute(landmarks, frame); + poseEstimator_->Compute(landmarks, *frames.back(), **next(frames.rbegin())); } void OdometryModule::BundleAdjust(std::vector &landmarks) diff --git a/src/module/odometry_module.h b/src/module/odometry_module.h index 93d7189..1e1248f 100644 --- a/src/module/odometry_module.h +++ b/src/module/odometry_module.h @@ -5,7 +5,7 @@ #include #include -#include "odometry/pnp.h" +#include "odometry/pose_estimator.h" #include "optimization/bundle_adjuster.h" #include "data/landmark.h" @@ -26,16 +26,16 @@ class OdometryModule { }; - OdometryModule(std::unique_ptr &pnp, std::unique_ptr &bundle_adjuster); - OdometryModule(std::unique_ptr &&pnp, std::unique_ptr &&bundle_adjuster); + OdometryModule(std::unique_ptr &pose_estimator, std::unique_ptr &bundle_adjuster); + OdometryModule(std::unique_ptr &&pose_estimator, std::unique_ptr &&bundle_adjuster); - void Update(std::vector &landmarks, data::Frame &frame); + void Update(std::vector &landmarks, std::vector> &frames); void BundleAdjust(std::vector &landmarks); Stats& GetStats(); private: - std::shared_ptr pnp_; + std::shared_ptr poseEstimator_; std::shared_ptr bundleAdjuster_; Stats stats_; diff --git a/src/module/tracking_module.cc b/src/module/tracking_module.cc index 49cf6ac..838a8b6 100644 --- a/src/module/tracking_module.cc +++ b/src/module/tracking_module.cc @@ -46,7 +46,7 @@ void TrackingModule::Update(std::unique_ptr &frame) { Matrix3d E; std::vector inlierIndices; - fivePointChecker_->Compute(landmarks_, **next(frames_.rbegin()), *frames_.back(), E, inlierIndices); + fivePointChecker_->ComputeE(landmarks_, **next(frames_.rbegin()), *frames_.back(), E, inlierIndices); std::unordered_set inlierSet(inlierIndices.begin(), inlierIndices.end()); for (int i = 0; i < landmarks_.size(); i++) { diff --git a/src/odometry/five_point.cc b/src/odometry/five_point.cc index e682939..595ed3d 100644 --- a/src/odometry/five_point.cc +++ b/src/odometry/five_point.cc @@ -1,21 +1,113 @@ #include "five_point.h" #include "util/math_util.h" +#include "util/tf_util.h" #include #include +#include +#include "optimization/reprojection_error.h" +#include "camera/double_sphere.h" +#include "camera/perspective.h" namespace omni_slam { namespace odometry { -FivePoint::FivePoint(int ransac_iterations, double epipolar_threshold) +FivePoint::FivePoint(int ransac_iterations, double epipolar_threshold, int num_ceres_threads) : ransacIterations_(ransac_iterations), - epipolarThreshold_(epipolar_threshold) + epipolarThreshold_(epipolar_threshold), + numCeresThreads_(num_ceres_threads) { } -int FivePoint::Compute(const std::vector &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E, std::vector &inlier_indices) const +int FivePoint::Compute(const std::vector &landmarks, data::Frame &cur_frame, data::Frame &prev_frame, std::vector &inlier_indices) const +{ + Matrix3d E; + int inliers = ComputeE(landmarks, prev_frame, cur_frame, E, inlier_indices); + std::vector rs; + std::vector ts; + EssentialToPoses(E, rs, ts); + Matrix I = util::TFUtil::IdentityPoseMatrix(); + int bestPoints = 0; + Matrix bestPose; + std::vector bestXs; + std::vector bestFeats; + std::vector bestIds; + for (int i = 0; i < rs.size(); i++) + { + Matrix P; + P.block<3, 3>(0, 0) = rs[i]; + P.block<3, 1>(0, 3) = ts[i]; + int goodPoints = 0; + std::vector xs; + std::vector feats; + std::vector ids; + for (int inx : inlier_indices) + { + const data::Landmark &landmark = landmarks[inx]; + const data::Feature *feat1 = landmark.GetObservationByFrameID(prev_frame.GetID()); + const data::Feature *feat2 = landmark.GetObservationByFrameID(cur_frame.GetID()); + Vector3d X = TriangulateDLT(feat1->GetBearing().normalized(), feat2->GetBearing().normalized(), I, P); + + Vector2d pix; + if (cur_frame.GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(X), pix) && prev_frame.GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(P, X)), pix)) + { + goodPoints++; + if (prev_frame.HasStereoImage() && landmark.HasEstimatedPosition() && landmark.GetStereoObservationByFrameID(landmark.GetFirstFrameID()) != nullptr) + { + xs.push_back(landmark.GetEstimatedPosition()); + feats.push_back(feat2); + ids.push_back(landmark.GetID()); + } + else if (!prev_frame.HasStereoImage() && landmark.HasEstimatedPosition()) + { + xs.push_back(landmark.GetEstimatedPosition()); + feats.push_back(feat2); + ids.push_back(landmark.GetID()); + } + else if (!prev_frame.HasStereoImage() && landmark.HasGroundTruth()) + { + xs.push_back(landmark.GetGroundTruth()); + feats.push_back(feat2); + ids.push_back(landmark.GetID()); + } + } + } + if (goodPoints > bestPoints) + { + bestPoints = goodPoints; + bestPose = P; + bestXs = xs; + bestFeats = feats; + bestIds = ids; + } + } + Matrix estPose; + Vector3d t; + if (prev_frame.HasEstimatedPose()) + { + estPose = util::TFUtil::CombineTransforms(bestPose, prev_frame.GetEstimatedInversePose()); + t = prev_frame.GetEstimatedInversePose().block<3, 1>(0, 3); + } + else if (prev_frame.HasPose()) + { + estPose = util::TFUtil::CombineTransforms(bestPose, prev_frame.GetInversePose()); + t = prev_frame.GetInversePose().block<3, 1>(0, 3); + } + else + { + estPose = bestPose; + t << 0, 0, 0; + } + ComputeTranslation(bestXs, bestFeats, util::TFUtil::GetRotationFromPoseMatrix(estPose), t); + estPose.block<3, 1>(0, 3) = t; + cur_frame.SetEstimatedInversePose(estPose, bestIds); + + return inliers; +} + +int FivePoint::ComputeE(const std::vector &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E, std::vector &inlier_indices) const { std::vector x1; std::vector x2; @@ -48,12 +140,6 @@ int FivePoint::Compute(const std::vector &landmarks, const data: return inliers; } -int FivePoint::Compute(const std::vector &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E) const -{ - std::vector temp; - return Compute(landmarks, frame1, frame2, E, temp); -} - int FivePoint::RANSAC(const std::vector &x1, const std::vector &x2, Matrix3d &E) const { int maxInliers = 0; @@ -185,6 +271,99 @@ void FivePoint::FivePointRelativePose(const std::vector &x1, const std } } +void FivePoint::EssentialToPoses(const Matrix3d &E, std::vector &rs, std::vector &ts) const +{ + Eigen::JacobiSVD USV(E, Eigen::ComputeFullU | Eigen::ComputeFullV); + Matrix3d U = USV.matrixU(); + Matrix3d Vt = USV.matrixV().transpose(); + + if (U.determinant() < 0) + { + U.col(2) *= -1; + } + if (Vt.determinant() < 0) + { + Vt.row(2) *= -1; + } + Matrix3d W; + W << 0, -1, 0, 1, 0, 0, 0, 0, 1; + + Matrix3d UWVt = U * W * Vt; + Matrix3d UWtVt = U * W.transpose() * Vt; + + rs.clear(); + rs.reserve(4); + ts.clear(); + ts.reserve(4); + rs.emplace_back(UWVt); + ts.emplace_back(-UWVt.transpose() * U.col(2)); + rs.emplace_back(UWtVt); + ts.emplace_back(-UWtVt.transpose() * -U.col(2)); + rs.emplace_back(UWVt); + ts.emplace_back(-UWVt.transpose() * -U.col(2)); + rs.emplace_back(UWtVt); + ts.emplace_back(-UWtVt.transpose() * U.col(2)); +} + +void FivePoint::ComputeTranslation(const std::vector &xs, const std::vector &ys, const Matrix3d &R, Vector3d &t) const +{ + ceres::Problem problem; + ceres::LossFunction *loss_function = new ceres::HuberLoss(0.001); + std::vector landmarks; + landmarks.reserve(3 * xs.size()); + Quaterniond quat(R); + quat.normalize(); + std::vector quatData(quat.coeffs().data(), quat.coeffs().data() + 4); + std::vector tData(t.data(), t.data() + 3); + for (int i = 0; i < xs.size(); i++) + { + const Vector3d &x = xs[i]; + landmarks.push_back(x(0)); + landmarks.push_back(x(1)); + landmarks.push_back(x(2)); + ceres::CostFunction *cost_function = nullptr; + if (ys[i]->GetFrame().GetCameraModel().GetType() == camera::CameraModel<>::kPerspective) + { + cost_function = optimization::ReprojectionError::Create(*ys[i]); + } + else if (ys[i]->GetFrame().GetCameraModel().GetType() == camera::CameraModel<>::kDoubleSphere) + { + cost_function = optimization::ReprojectionError::Create(*ys[i]); + } + if (cost_function != nullptr) + { + problem.AddResidualBlock(cost_function, loss_function, &quatData[0], &tData[0], &landmarks[landmarks.size() - 3]); + problem.SetParameterBlockConstant(&landmarks[landmarks.size() - 3]); + problem.SetParameterBlockConstant(&quatData[0]); + } + } + ceres::Solver::Options options; + options.max_num_iterations = 100; + options.linear_solver_type = ceres::DENSE_SCHUR; + options.function_tolerance = 1e-6; + options.gradient_tolerance = 1e-6; + options.num_threads = numCeresThreads_; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + if (!summary.IsSolutionUsable()) + { + return; + } + const Vector3d tRes = Map(&tData[0]); + t = tRes; +} + +Vector3d FivePoint::TriangulateDLT(const Vector3d &x1, const Vector3d &x2, const Matrix &pose1, const Matrix &pose2) const +{ + Matrix4d design; + design.row(0) = x1(0) * pose1.row(2) - x1(2) * pose1.row(0); + design.row(1) = x1(1) * pose1.row(2) - x1(2) * pose1.row(1); + design.row(2) = x2(0) * pose2.row(2) - x2(2) * pose2.row(0); + design.row(3) = x2(1) * pose2.row(2) - x2(2) * pose2.row(1); + + Eigen::JacobiSVD svd(design, Eigen::ComputeFullV); + return svd.matrixV().col(3).hnormalized(); +} } } diff --git a/src/odometry/five_point.h b/src/odometry/five_point.h index 86f1532..7137b7a 100644 --- a/src/odometry/five_point.h +++ b/src/odometry/five_point.h @@ -1,6 +1,7 @@ #ifndef _FIVE_POINT_H_ #define _FIVE_POINT_H_ +#include "pose_estimator.h" #include #include "data/landmark.h" @@ -11,22 +12,25 @@ namespace omni_slam namespace odometry { -class FivePoint +class FivePoint : public PoseEstimator { public: - FivePoint(int ransac_iterations, double epipolar_threshold); + FivePoint(int ransac_iterations, double epipolar_threshold, int num_ceres_threads = 1); - int Compute(const std::vector &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E, std::vector &inlier_indices) const; - int Compute(const std::vector &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E) const; + int Compute(const std::vector &landmarks, data::Frame &cur_frame, data::Frame &prev_frame, std::vector &inlier_indices) const; + int ComputeE(const std::vector &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E, std::vector &inlier_indices) const; private: int RANSAC(const std::vector &x1, const std::vector &x2, Matrix3d &E) const; void FivePointRelativePose(const std::vector &x1, const std::vector &x2, std::vector &Es) const; std::vector GetInlierIndices(const std::vector &x1, const std::vector &x2, const Matrix3d &E) const; + void EssentialToPoses(const Matrix3d &E, std::vector &rs, std::vector &ts) const; + void ComputeTranslation(const std::vector &xs, const std::vector &ys, const Matrix3d &R, Vector3d &t) const; + Vector3d TriangulateDLT(const Vector3d &x1, const Vector3d &x2, const Matrix &pose1, const Matrix &pose2) const; int ransacIterations_; double epipolarThreshold_; - + int numCeresThreads_; }; } diff --git a/src/odometry/pnp.cc b/src/odometry/pnp.cc index e5968bf..e965ad4 100644 --- a/src/odometry/pnp.cc +++ b/src/odometry/pnp.cc @@ -23,7 +23,7 @@ PNP::PNP(int ransac_iterations, double reprojection_threshold, int num_refine_th { } -int PNP::Compute(const std::vector &landmarks, data::Frame &frame, std::vector &inlier_indices) const +int PNP::Compute(const std::vector &landmarks, data::Frame &cur_frame, data::Frame &prev_frame, std::vector &inlier_indices) const { std::vector xs; std::vector yns; @@ -34,17 +34,17 @@ int PNP::Compute(const std::vector &landmarks, data::Frame &fram int i = 0; for (const data::Landmark &landmark : landmarks) { - if (landmark.IsObservedInFrame(frame.GetID())) + if (landmark.IsObservedInFrame(cur_frame.GetID())) { - if (frame.HasStereoImage() && landmark.HasEstimatedPosition() && landmark.GetStereoObservationByFrameID(landmark.GetFirstFrameID()) != nullptr) + if (cur_frame.HasStereoImage() && landmark.HasEstimatedPosition() && landmark.GetStereoObservationByFrameID(landmark.GetFirstFrameID()) != nullptr) { xs.push_back(landmark.GetEstimatedPosition()); } - else if (!frame.HasStereoImage() && landmark.HasEstimatedPosition()) + else if (!cur_frame.HasStereoImage() && landmark.HasEstimatedPosition()) { xs.push_back(landmark.GetEstimatedPosition()); } - else if (!frame.HasStereoImage() && landmark.HasGroundTruth()) + else if (!cur_frame.HasStereoImage() && landmark.HasGroundTruth()) { xs.push_back(landmark.GetGroundTruth()); } @@ -53,7 +53,7 @@ int PNP::Compute(const std::vector &landmarks, data::Frame &fram i++; continue; } - const data::Feature *feat = landmark.GetObservationByFrameID(frame.GetID()); + const data::Feature *feat = landmark.GetObservationByFrameID(cur_frame.GetID()); Vector2d pix; pix << feat->GetKeypoint().pt.x, feat->GetKeypoint().pt.y; yns.push_back(pix); @@ -69,8 +69,8 @@ int PNP::Compute(const std::vector &landmarks, data::Frame &fram return 0; } Matrix pose; - int inliers = RANSAC(xs, ys, yns, frame.GetCameraModel(), pose); - std::vector indices = GetInlierIndices(xs, yns, pose, frame.GetCameraModel()); + int inliers = RANSAC(xs, ys, yns, cur_frame.GetCameraModel(), pose); + std::vector indices = GetInlierIndices(xs, yns, pose, cur_frame.GetCameraModel()); if (inliers > 3) { Refine(xs, features, indices, pose); @@ -84,16 +84,10 @@ int PNP::Compute(const std::vector &landmarks, data::Frame &fram inlierIds.push_back(indexToId[inx]); inlier_indices.push_back(indexToLandmarkIndex[inx]); } - frame.SetEstimatedInversePose(pose, inlierIds); + cur_frame.SetEstimatedInversePose(pose, inlierIds); return inliers; } -int PNP::Compute(const std::vector &landmarks, data::Frame &frame) const -{ - std::vector temp; - return Compute(landmarks, frame, temp); -} - int PNP::RANSAC(const std::vector &xs, const std::vector &ys, const std::vector &yns, const camera::CameraModel<> &camera_model, Matrix &pose) const { int maxInliers = 0; @@ -137,7 +131,7 @@ bool PNP::Refine(const std::vector &xs, const std::vector landmarks; landmarks.reserve(3 * indices.size()); - Quaterniond quat(pose.block<3, 3>(0, 0)); + Quaterniond quat(util::TFUtil::GetRotationFromPoseMatrix(pose)); quat.normalize(); Vector3d t(pose.block<3, 1>(0, 3)); std::vector quatData(quat.coeffs().data(), quat.coeffs().data() + 4); diff --git a/src/odometry/pnp.h b/src/odometry/pnp.h index 4c3f2aa..939fba4 100644 --- a/src/odometry/pnp.h +++ b/src/odometry/pnp.h @@ -1,6 +1,7 @@ #ifndef _PNP_H_ #define _PNP_H_ +#include "pose_estimator.h" #include #include "data/landmark.h" #include "data/frame.h" @@ -14,12 +15,11 @@ namespace omni_slam namespace odometry { -class PNP +class PNP : public PoseEstimator { public: PNP(int ransac_iterations, double reprojection_threshold, int num_refine_threads = 1); - int Compute(const std::vector &landmarks, data::Frame &frame, std::vector &inlier_indices) const; - int Compute(const std::vector &landmarks, data::Frame &frame) const; + int Compute(const std::vector &landmarks, data::Frame &cur_frame, data::Frame &prev_frame, std::vector &inlier_indices) const; private: int RANSAC(const std::vector &xs, const std::vector &ys, const std::vector &yns, const camera::CameraModel<> &camera_model, Matrix &pose) const; diff --git a/src/odometry/pose_estimator.cc b/src/odometry/pose_estimator.cc new file mode 100644 index 0000000..a5c87d7 --- /dev/null +++ b/src/odometry/pose_estimator.cc @@ -0,0 +1,15 @@ +#include "pose_estimator.h" + +namespace omni_slam +{ +namespace odometry +{ + +int PoseEstimator::Compute(const std::vector &landmarks, data::Frame &cur_frame, data::Frame &prev_frame) const +{ + std::vector temp; + return Compute(landmarks, cur_frame, prev_frame, temp); +} + +} +} diff --git a/src/odometry/pose_estimator.h b/src/odometry/pose_estimator.h new file mode 100644 index 0000000..339589d --- /dev/null +++ b/src/odometry/pose_estimator.h @@ -0,0 +1,22 @@ +#ifndef _POSE_ESTIMATOR_H_ +#define _POSE_ESTIMATOR_H_ + +#include +#include "data/landmark.h" + +namespace omni_slam +{ +namespace odometry +{ + +class PoseEstimator +{ +public: + virtual int Compute(const std::vector &landmarks, data::Frame &cur_frame, data::Frame &prev_frame, std::vector &inlier_indices) const = 0; + int Compute(const std::vector &landmarks, data::Frame &cur_frame, data::Frame &prev_frame) const; +}; + +} +} + +#endif /* _POSE_ESTIMATOR_H_ */ diff --git a/src/ros/odometry_eval.cc b/src/ros/odometry_eval.cc index 3927fd6..7813aab 100644 --- a/src/ros/odometry_eval.cc +++ b/src/ros/odometry_eval.cc @@ -1,6 +1,7 @@ #include "odometry_eval.h" #include "odometry/pnp.h" +#include "odometry/five_point.h" #include "optimization/bundle_adjuster.h" #include "module/tracking_module.h" @@ -25,6 +26,9 @@ OdometryEval::OdometryEval(const ::ros::NodeHandle &nh, const ::ros::Nod bool logCeres; int numCeresThreads; + double fivePointThreshold; + int fivePointRansacIterations; + this->nhp_.param("output_frame", cameraFrame_, std::string("map")); this->nhp_.param("pnp_inlier_threshold", reprojThresh, 10.); this->nhp_.param("pnp_iterations", iterations, 1000); @@ -33,10 +37,14 @@ OdometryEval::OdometryEval(const ::ros::NodeHandle &nh, const ::ros::Nod this->nhp_.param("bundle_adjustment_logging", logCeres, false); this->nhp_.param("bundle_adjustment_num_threads", numCeresThreads, 1); - unique_ptr pnp(new odometry::PNP(iterations, reprojThresh, numCeresThreads)); + this->nhp_.param("tracker_checker_epipolar_threshold", fivePointThreshold, 0.01745240643); + this->nhp_.param("tracker_checker_iterations", fivePointRansacIterations, 1000); + + unique_ptr poseEstimator(new odometry::PNP(iterations, reprojThresh, numCeresThreads)); + //unique_ptr poseEstimator(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold, numCeresThreads)); unique_ptr bundleAdjuster(new optimization::BundleAdjuster(baMaxIter, baLossCoeff, numCeresThreads, logCeres)); - odometryModule_.reset(new module::OdometryModule(pnp, bundleAdjuster)); + odometryModule_.reset(new module::OdometryModule(poseEstimator, bundleAdjuster)); } template @@ -67,7 +75,7 @@ template void OdometryEval::ProcessFrame(unique_ptr &&frame) { this->trackingModule_->Update(frame); - odometryModule_->Update(this->trackingModule_->GetLandmarks(), *this->trackingModule_->GetFrames().back()); + odometryModule_->Update(this->trackingModule_->GetLandmarks(), this->trackingModule_->GetFrames()); this->trackingModule_->Redetect(); this->visualized_ = false; diff --git a/src/ros/slam_eval.cc b/src/ros/slam_eval.cc index 54d5a37..3316780 100644 --- a/src/ros/slam_eval.cc +++ b/src/ros/slam_eval.cc @@ -31,7 +31,7 @@ void SLAMEval::InitPublishers() void SLAMEval::ProcessFrame(unique_ptr &&frame) { trackingModule_->Update(frame); - odometryModule_->Update(trackingModule_->GetLandmarks(), *trackingModule_->GetFrames().back()); + odometryModule_->Update(trackingModule_->GetLandmarks(), trackingModule_->GetFrames()); reconstructionModule_->Update(trackingModule_->GetLandmarks()); if (baSlidingWindow_ > 0 && baSlidingInterval_ > 0 && (frameNum_ + 1) % baSlidingInterval_ == 0) { diff --git a/src/ros/tracking_eval.cc b/src/ros/tracking_eval.cc index f08a808..bcd1198 100644 --- a/src/ros/tracking_eval.cc +++ b/src/ros/tracking_eval.cc @@ -1,6 +1,6 @@ #include "tracking_eval.h" -#include "feature/tracker.h" +#include "feature/lk_tracker.h" #include "feature/detector.h" #include "odometry/five_point.h" @@ -47,7 +47,7 @@ TrackingEval::TrackingEval(const ::ros::NodeHandle &nh, const ::ros::Nod ROS_ERROR("Invalid feature detector specified"); } - unique_ptr tracker(new feature::Tracker(trackerWindowSize, trackerNumScales, trackerDeltaPixelErrorThresh, trackerErrorThresh)); + unique_ptr tracker(new feature::LKTracker(trackerWindowSize, trackerNumScales, trackerDeltaPixelErrorThresh, trackerErrorThresh)); unique_ptr checker(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold)); diff --git a/src/util/tf_util.h b/src/util/tf_util.h index 449f228..62fc028 100644 --- a/src/util/tf_util.h +++ b/src/util/tf_util.h @@ -67,6 +67,18 @@ inline Matrix QuaternionTranslationToPoseMatrix(Quaternion quat, Mat return pose; } +template +inline Matrix CombineTransforms(Matrix pose1, Matrix pose2) +{ + Matrix pose1H = Matrix::Zero(); + Matrix pose2H = Matrix::Zero(); + pose1H.template block<3, 4>(0, 0) = pose1; + pose1H(3, 3) = 1; + pose2H.template block<3, 4>(0, 0) = pose2; + pose2H(3, 3) = 1; + return (pose1H * pose2H).template block<3, 4>(0, 0); +} + template inline Matrix GetRelativeTransform(Matrix pose1, Matrix pose2) { @@ -80,8 +92,8 @@ template inline Matrix GetEssentialMatrixFromPoses(Matrix pose1, Matrix pose2) { Matrix rel = GetRelativeTransform(pose1, pose2); - Vector3d t = -GetRotationFromPoseMatrix(rel).transpose() * rel.template block<3, 1>(0, 3); - Matrix3d tx; + Matrix t = -GetRotationFromPoseMatrix(rel).transpose() * rel.template block<3, 1>(0, 3); + Matrix tx; tx << 0, -t(2), t(1), t(2), 0, -t(0), -t(1), t(0), 0;