From 74ce966180fc901820a66c36dc1538cbb7fd7194 Mon Sep 17 00:00:00 2001 From: weihaoysgs Date: Mon, 1 Apr 2024 22:23:49 +0800 Subject: [PATCH] success vo --- .../impl/reprojection_se3_factor_impl.hpp | 8 +- geometry/CMakeLists.txt | 4 + geometry/example/triangulate_test.cpp | 8 + geometry/motion_ba/motion_ba.cpp | 198 ++++++++++++++++ geometry/motion_ba/motion_ba.hpp | 37 +++ geometry/triangulate/triangulate_cv.hpp | 39 ++++ geometry/triangulate/triangulate_impl.hpp | 4 + vio_hw/CMakeLists.txt | 1 + vio_hw/internal/frame.hpp | 14 +- vio_hw/internal/keyframe.hpp | 9 +- vio_hw/internal/map_manager.hpp | 10 +- vio_hw/internal/map_point.hpp | 8 + vio_hw/internal/mapping.hpp | 8 +- vio_hw/internal/visual_frontend.hpp | 1 + vio_hw/internal/viz/rviz_visualization.hpp | 2 + vio_hw/params/rviz/hw-vio.rviz | 30 ++- vio_hw/src/frame.cpp | 122 +++++++++- vio_hw/src/map_manager.cpp | 110 +++++++++ vio_hw/src/map_point.cpp | 72 ++++++ vio_hw/src/mapping.cpp | 218 +++++++++++++++++- vio_hw/src/visual_frontend.cpp | 62 ++++- vio_hw/src/viz/rviz_visualization.cpp | 30 ++- vio_hw/src/world_manager.cpp | 3 +- 23 files changed, 961 insertions(+), 37 deletions(-) create mode 100644 geometry/example/triangulate_test.cpp create mode 100644 geometry/motion_ba/motion_ba.cpp create mode 100644 geometry/motion_ba/motion_ba.hpp create mode 100644 geometry/triangulate/triangulate_cv.hpp create mode 100644 geometry/triangulate/triangulate_impl.hpp diff --git a/backend/cost_function/impl/reprojection_se3_factor_impl.hpp b/backend/cost_function/impl/reprojection_se3_factor_impl.hpp index 2ca64ec..da27f3b 100644 --- a/backend/cost_function/impl/reprojection_se3_factor_impl.hpp +++ b/backend/cost_function/impl/reprojection_se3_factor_impl.hpp @@ -2,9 +2,8 @@ namespace backend { namespace DirectLeftSE3 { -bool ReprojectionErrorSE3::Evaluate(double const* const* parameters, - double* residuals, - double** jacobians) const { +inline bool ReprojectionErrorSE3::Evaluate(double const* const* parameters, double* residuals, + double** jacobians) const { // [tx, ty, tz, qw, qx, qy, qz] Eigen::Map twc(parameters[0]); Eigen::Map qwc(parameters[0] + 3); @@ -43,8 +42,7 @@ bool ReprojectionErrorSE3::Evaluate(double const* const* parameters, J_lRcw.noalias() = J_lcam * Tcw.rotationMatrix(); if (jacobians[0] != NULL) { - Eigen::Map > J_se3pose( - jacobians[0]); + Eigen::Map > J_se3pose(jacobians[0]); J_se3pose.setZero(); J_se3pose.block<2, 3>(0, 0).noalias() = -1. * J_lRcw; diff --git a/geometry/CMakeLists.txt b/geometry/CMakeLists.txt index e69de29..0a9d414 100644 --- a/geometry/CMakeLists.txt +++ b/geometry/CMakeLists.txt @@ -0,0 +1,4 @@ +add_library(geometry motion_ba/motion_ba.cpp) +target_link_libraries(geometry tiny_ceres) +add_executable(triangulate_test example/triangulate_test.cpp) +target_link_libraries(triangulate_test ${OpenCV_LIBS}) \ No newline at end of file diff --git a/geometry/example/triangulate_test.cpp b/geometry/example/triangulate_test.cpp new file mode 100644 index 0000000..6c8cfd4 --- /dev/null +++ b/geometry/example/triangulate_test.cpp @@ -0,0 +1,8 @@ +#include +#include +#include "geometry/triangulate/triangulate_cv.hpp" + +int main(int argc, char **argv) { + std::cout << "hello world"; + return 0; +} \ No newline at end of file diff --git a/geometry/motion_ba/motion_ba.cpp b/geometry/motion_ba/motion_ba.cpp new file mode 100644 index 0000000..d4ca4df --- /dev/null +++ b/geometry/motion_ba/motion_ba.cpp @@ -0,0 +1,198 @@ +#include "geometry/motion_ba/motion_ba.hpp" + +namespace geometry { + +bool tceresMotionOnlyBA( + const std::vector > &vunkps, + const std::vector > &vwpts, + const std::vector &vscales, Sophus::SE3d &Twc, const int nmaxiter, const float chi2th, + const bool buse_robust, const bool bapply_l2_after_robust, const Eigen::Matrix3d &K, + std::vector &voutliersidx) { + float fx = K(0, 0); + float fy = K(1, 1); + float cx = K(0, 2); + float cy = K(1, 2); + return tceresMotionOnlyBA(vunkps, vwpts, vscales, Twc, nmaxiter, chi2th, buse_robust, + bapply_l2_after_robust, fx, fy, cx, cy, voutliersidx); +} + +bool tceresMotionOnlyBA( + const std::vector > &vunkps, + const std::vector > &vwpts, + const std::vector &vscales, Sophus::SE3d &Twc, const int nmaxiter, const float chi2th, + const bool buse_robust, const bool bapply_l2_after_robust, const float fx, const float fy, + const float cx, const float cy, std::vector &voutliersidx) { + assert(vunkps.size() == vwpts.size()); + + tceres::Problem problem; + + double chi2thrtsq = std::sqrt(chi2th); + + tceres::LossFunctionWrapper *loss_function; + loss_function = + new tceres::LossFunctionWrapper(new tceres::HuberLoss(chi2thrtsq), tceres::TAKE_OWNERSHIP); + + if (!buse_robust) { + loss_function->Reset(NULL, tceres::TAKE_OWNERSHIP); + } + + size_t nbkps = vunkps.size(); + + tceres::LocalParameterization *local_parameterization = new backend::SE3LeftParameterization(); + + backend::PoseParametersBlock posepar = backend::PoseParametersBlock(0, Twc); + + problem.AddParameterBlock(posepar.values(), 7, local_parameterization); + + std::vector verrors_; + std::vector vrids_; + + for (size_t i = 0; i < nbkps; i++) { + auto *f = new backend::DirectLeftSE3::ReprojectionErrorSE3( + vunkps[i].x(), vunkps[i].y(), fx, fy, cx, cy, vwpts.at(i), std::pow(2., vscales[i])); + tceres::ResidualBlockId rid = problem.AddResidualBlock(f, loss_function, posepar.values()); + + verrors_.push_back(f); + vrids_.push_back(rid); + } + + tceres::Solver::Options options; + options.linear_solver_type = tceres::DENSE_SCHUR; + options.trust_region_strategy_type = tceres::LEVENBERG_MARQUARDT; + + options.num_threads = 1; + options.max_num_iterations = nmaxiter; + options.max_solver_time_in_seconds = 0.005; + options.function_tolerance = 1.e-3; + + options.minimizer_progress_to_stdout = false; + + tceres::Solver::Summary summary; + tceres::Solve(options, &problem, &summary); + + size_t bad = 0; + + for (size_t i = 0; i < nbkps; i++) { + auto err = verrors_.at(i); + if (err->chi2err_ > chi2th || !err->isdepthpositive_) { + if (bapply_l2_after_robust) { + auto rid = vrids_.at(i); + problem.RemoveResidualBlock(rid); + } + voutliersidx.push_back(i); + bad++; + } + } + + if (bad == nbkps) { + return false; + } + + if (bapply_l2_after_robust && !voutliersidx.empty()) { + loss_function->Reset(nullptr, tceres::TAKE_OWNERSHIP); + tceres::Solve(options, &problem, &summary); + } + + Twc = posepar.getPose(); + + return summary.IsSolutionUsable(); +} + +bool opencvP3PRansac( + const std::vector > &bvs, + const std::vector > &vwpts, + const int nmaxiter, const float errth, const float fx, const float fy, const bool boptimize, + Sophus::SE3d &Twc, std::vector &voutliersidx) { + assert(bvs.size() == vwpts.size()); + + size_t nb3dpts = bvs.size(); + + if (nb3dpts < 4) { + return false; + } + + voutliersidx.reserve(nb3dpts); + + std::vector cvbvs; + cvbvs.reserve(nb3dpts); + + std::vector cvwpts; + cvwpts.reserve(nb3dpts); + + for (size_t i = 0; i < nb3dpts; i++) { + cvbvs.push_back(cv::Point2f(bvs.at(i).x() / bvs.at(i).z(), bvs.at(i).y() / bvs.at(i).z())); + + cvwpts.push_back(cv::Point3f(vwpts.at(i).x(), vwpts.at(i).y(), vwpts.at(i).z())); + } + + // Using homoegeneous pts here so no dist or calib. + cv::Mat D; + cv::Mat K = cv::Mat::eye(3, 3, CV_32F); + + cv::Mat tvec, rvec; + cv::Mat inliers; + + bool use_extrinsic_guess = false; + float confidence = 0.99; + + float focal = (fx + fy) / 2.; + + cv::solvePnPRansac(cvwpts, cvbvs, K, D, rvec, tvec, use_extrinsic_guess, nmaxiter, errth / focal, + confidence, inliers, cv::SOLVEPNP_ITERATIVE); + + if (inliers.rows == 0) { + return false; + } + + int k = 0; + for (size_t i = 0; i < nb3dpts; i++) { + if (inliers.at(k) == (int)i) { + k++; + if (k == inliers.rows) { + k = 0; + } + } else { + voutliersidx.push_back(i); + } + } + + if (voutliersidx.size() >= nb3dpts - 5) { + return false; + } + + if (boptimize) { + use_extrinsic_guess = true; + + // Filter outliers + std::vector in_cvbvs; + in_cvbvs.reserve(inliers.rows); + + std::vector in_cvwpts; + in_cvwpts.reserve(inliers.rows); + + for (int i = 0; i < inliers.rows; i++) { + in_cvbvs.push_back(cvbvs.at(inliers.at(i))); + in_cvwpts.push_back(cvwpts.at(inliers.at(i))); + } + + cv::solvePnP(in_cvwpts, in_cvbvs, K, D, rvec, tvec, use_extrinsic_guess, + cv::SOLVEPNP_ITERATIVE); + } + + // Store the resulting pose + cv::Mat cvR; + cv::Rodrigues(rvec, cvR); + + Eigen::Vector3d tcw; + Eigen::Matrix3d Rcw; + + cv::cv2eigen(cvR, Rcw); + cv::cv2eigen(tvec, tcw); + + Twc.translation() = -1. * Rcw.transpose() * tcw; + Twc.setRotationMatrix(Rcw.transpose()); + + return true; +} + +} // namespace geometry \ No newline at end of file diff --git a/geometry/motion_ba/motion_ba.hpp b/geometry/motion_ba/motion_ba.hpp new file mode 100644 index 0000000..b5e7a8b --- /dev/null +++ b/geometry/motion_ba/motion_ba.hpp @@ -0,0 +1,37 @@ +#ifndef VIO_HELLO_WORLD_MOTION_BA_HPP +#define VIO_HELLO_WORLD_MOTION_BA_HPP + +#include "backend/cost_function/reprojection_se3_factor.hpp" +#include "backend/parameter_block/point_parameter_block.hpp" +#include "backend/parameter_block/se3_parameter_block.hpp" +#include "backend/parameterization/se3left_parametrization.hpp" +#include "opencv2/core/eigen.hpp" +#include "opencv2/opencv.hpp" +#include "tceres/loss_function.h" +#include "tceres/problem.h" +#include "tceres/solver.h" + +namespace geometry { + +bool tceresMotionOnlyBA( + const std::vector > &vunkps, + const std::vector > &vwpts, + const std::vector &vscales, Sophus::SE3d &Twc, const int nmaxiter, const float chi2th, + const bool buse_robust, const bool bapply_l2_after_robust, const float fx, const float fy, + const float cx, const float cy, std::vector &voutliersidx); + +bool tceresMotionOnlyBA( + const std::vector > &vunkps, + const std::vector > &vwpts, + const std::vector &vscales, Sophus::SE3d &Twc, const int nmaxiter, const float chi2th, + const bool buse_robust, const bool bapply_l2_after_robust, const Eigen::Matrix3d &K, + std::vector &voutliersidx); + +bool opencvP3PRansac( + const std::vector > &bvs, + const std::vector > &vwpts, + const int nmaxiter, const float errth, const float fx, const float fy, const bool boptimize, + Sophus::SE3d &Twc, std::vector &voutliersidx); +} // namespace geometry + +#endif // VIO_HELLO_WORLD_MOTION_BA_HPP diff --git a/geometry/triangulate/triangulate_cv.hpp b/geometry/triangulate/triangulate_cv.hpp new file mode 100644 index 0000000..4ff4414 --- /dev/null +++ b/geometry/triangulate/triangulate_cv.hpp @@ -0,0 +1,39 @@ +#ifndef VIO_HELLO_WORLD_TRIANGULATE_CV_HPP +#define VIO_HELLO_WORLD_TRIANGULATE_CV_HPP +#include +#include + +#include "sophus/se3.hpp" + +namespace geometry { +inline Eigen::Vector3d OpencvTriangulate(const Sophus::SE3d &Tlr, const Eigen::Vector3d &bvl, + const Eigen::Vector3d &bvr) { + std::vector lpt, rpt; + lpt.push_back(cv::Point2f(bvl.x() / bvl.z(), bvl.y() / bvl.z())); + rpt.push_back(cv::Point2f(bvr.x() / bvr.z(), bvr.y() / bvr.z())); + + cv::Matx34f P0 = cv::Matx34f(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0); + + Sophus::SE3d Tcw = Tlr.inverse(); + Eigen::Matrix3d R = Tcw.rotationMatrix(); + Eigen::Vector3d t = Tcw.translation(); + + cv::Matx34f P1 = cv::Matx34f(R(0, 0), R(0, 1), R(0, 2), t(0), R(1, 0), R(1, 1), R(1, 2), t(1), + R(2, 0), R(2, 1), R(2, 2), t(2)); + + cv::Mat campt; + cv::triangulatePoints(P0, P1, lpt, rpt, campt); + + if (campt.col(0).at(3) != 1.) { + campt.col(0) /= campt.col(0).at(3); + } + + Eigen::Vector3d pt(campt.col(0).at(0), campt.col(0).at(1), + campt.col(0).at(2)); + + return pt; +} + +} // namespace geometry + +#endif // VIO_HELLO_WORLD_TRIANGULATE_CV_HPP diff --git a/geometry/triangulate/triangulate_impl.hpp b/geometry/triangulate/triangulate_impl.hpp new file mode 100644 index 0000000..8164812 --- /dev/null +++ b/geometry/triangulate/triangulate_impl.hpp @@ -0,0 +1,4 @@ +#ifndef VIO_HELLO_WORLD_TRIANGULATE_IMPL_HPP +#define VIO_HELLO_WORLD_TRIANGULATE_IMPL_HPP + +#endif // VIO_HELLO_WORLD_TRIANGULATE_IMPL_HPP diff --git a/vio_hw/CMakeLists.txt b/vio_hw/CMakeLists.txt index 2c76407..3edadba 100644 --- a/vio_hw/CMakeLists.txt +++ b/vio_hw/CMakeLists.txt @@ -43,6 +43,7 @@ add_library(vio_hw ${SOURCE_FILES}) target_link_libraries(vio_hw feature_extraction + geometry tiny_ceres feature_tracking ${OpenCV_LIBS} diff --git a/vio_hw/internal/frame.hpp b/vio_hw/internal/frame.hpp index d8b54cd..0693b0d 100644 --- a/vio_hw/internal/frame.hpp +++ b/vio_hw/internal/frame.hpp @@ -33,6 +33,18 @@ class Frame void UpdateKeypoint(const int lmid, const cv::Point2f &pt); void RemoveKeypointById(const int lmid); Keypoint GetKeypointById(const int lmid) const; + void ComputeStereoKeypoint(const cv::Point2f &pt, Keypoint &kp); + std::vector getKeypointsStereo() const; + void RemoveStereoKeypointById(const int lmid); + std::vector GetKeypoints2d() const; + void UpdateKeypointStereo(const int lmid, const cv::Point2f &pt); + cv::Point2f ProjCamToRightImage(const Eigen::Vector3d &pt) const; + Eigen::Vector3d ProjCamToWorld(const Eigen::Vector3d &pt) const; + Sophus::SE3d GetTwc() const; + Sophus::SE3d GetTcw() const; + void SetTwc(const Sophus::SE3d &Twc); + cv::Point2f ProjCamToImage(const Eigen::Vector3d &pt) const; + void TurnKeypoint3d(const int lmid); // For using frame in ordered containers bool operator<(const Frame &f) const { return id_ < f.id_; } @@ -53,7 +65,7 @@ class Frame // Pose (T cam -> world), (T world -> cam) Sophus::SE3d Twc_, Tcw_; - + Sophus::SE3d Twb_, Tbw_; /* TODO Set a vector of calib ptrs to handle any multicam system. Each calib ptr should contain an extrinsic parametrization with a common diff --git a/vio_hw/internal/keyframe.hpp b/vio_hw/internal/keyframe.hpp index 3bb32b7..d8f5485 100644 --- a/vio_hw/internal/keyframe.hpp +++ b/vio_hw/internal/keyframe.hpp @@ -18,8 +18,10 @@ struct Keyframe Keyframe(int kfid, const cv::Mat &imleftraw) : kfid_(kfid), imleftraw_(imleftraw.clone()), is_stereo_(false) {} - Keyframe(int kfid, const cv::Mat &imleftraw, - const std::vector &vpyrleft, + Keyframe(int kfid, const cv::Mat &imleftraw, const cv::Mat &imright_raw) + : kfid_(kfid), imleftraw_(imleftraw.clone()), imrightraw_(imright_raw.clone()), is_stereo_(true) {} + + Keyframe(int kfid, const cv::Mat &imleftraw, const std::vector &vpyrleft, const std::vector &vpyrright) : kfid_(kfid), imleftraw_(imleftraw.clone()), @@ -37,8 +39,7 @@ struct Keyframe void displayInfo() { std::cout << "\n\n Keyframe struct object ! Info : id #" << kfid_ << " - is stereo : " << is_stereo_; - std::cout << " - imleft size : " << imleft_.size - << " - imright size : " << imright_.size; + std::cout << " - imleft size : " << imleft_.size << " - imright size : " << imright_.size; std::cout << " - pyr left size : " << vpyr_imleft_.size() << " - pyr right size : " << vpyr_imright_.size() << "\n\n"; } diff --git a/vio_hw/internal/map_manager.hpp b/vio_hw/internal/map_manager.hpp index 5f3448e..06ba4d1 100644 --- a/vio_hw/internal/map_manager.hpp +++ b/vio_hw/internal/map_manager.hpp @@ -23,6 +23,8 @@ class MapManager void PrepareFrame(); void AddKeyframe(); void ExtractKeypoints(const cv::Mat &im, const cv::Mat &im_raw); + void StereoMatching(Frame &frame, const std::vector &vleftpyr, + const std::vector &vrightpyr); void AddMapPoint(); void AddMapPoint(const cv::Mat &desc); void AddKeypointsToFrame(const std::vector &vpts, Frame &frame); @@ -31,6 +33,13 @@ class MapManager void RemoveObsFromCurFrameById(const int lmid); std::shared_ptr GetKeyframe(const int kfid) const; std::shared_ptr GetMapPoint(const int lmid) const; + void UpdateMapPoint(const int lmid, const Eigen::Vector3d &wpt, const double kfanch_invdepth); + void RemoveMapPointObs(const int lmid, const int kfid); + + public: + mutable std::mutex kf_mutex_, lm_mutex_; + mutable std::mutex map_mutex_; + private: int lm_id_, kf_id_; int num_lms_, num_kfs_; @@ -42,7 +51,6 @@ class MapManager std::unordered_map> map_kfs_; std::unordered_map> map_lms_; - mutable std::mutex kf_mutex_, lm_mutex_; }; typedef std::shared_ptr MapManagerPtr; diff --git a/vio_hw/internal/map_point.hpp b/vio_hw/internal/map_point.hpp index 2549e96..fd6d428 100644 --- a/vio_hw/internal/map_point.hpp +++ b/vio_hw/internal/map_point.hpp @@ -20,6 +20,14 @@ class MapPoint MapPoint(const int lmid, const int kfid, const bool bobs = true); MapPoint(const int lmid, const int kfid, const cv::Mat &desc, const bool bobs = true); + std::set GetKfObsSet() const; + + void RemoveKfObs(const int kfid); + + Eigen::Vector3d GetPoint() const; + + void SetPoint(const Eigen::Vector3d &ptxyz, const double invdepth = -1.); + // For using MapPoint in ordered containers bool operator<(const MapPoint &mp) const { return lmid_ < mp.lmid_; } diff --git a/vio_hw/internal/mapping.hpp b/vio_hw/internal/mapping.hpp index 2e74ab1..fe4a3ff 100644 --- a/vio_hw/internal/mapping.hpp +++ b/vio_hw/internal/mapping.hpp @@ -11,6 +11,7 @@ #include "vio_hw/internal/keyframe.hpp" #include "vio_hw/internal/map_manager.hpp" #include "vio_hw/internal/setting.hpp" +#include "geometry/triangulate/triangulate_cv.hpp" namespace viohw { @@ -32,10 +33,10 @@ class Mapping void run(); // get new keyframe, return true meaning successful - bool getNewKf(Keyframe &kf); + bool GetNewKf(Keyframe &kf); // add new kf to kf queen wait for process - void addNewKf(const Keyframe &kf); + void AddNewKf(const Keyframe &kf); void TriangulateTemporal(Frame &frame); @@ -46,6 +47,9 @@ class Mapping private: bool is_new_kf_available_ = false; + bool stereo_mode_; + bool use_clahe_; + cv::Ptr clahe_; FramePtr current_frame_; SettingPtr params_; diff --git a/vio_hw/internal/visual_frontend.hpp b/vio_hw/internal/visual_frontend.hpp index 9b8513c..fe6792f 100644 --- a/vio_hw/internal/visual_frontend.hpp +++ b/vio_hw/internal/visual_frontend.hpp @@ -7,6 +7,7 @@ #include "vio_hw/internal/setting.hpp" #include "vio_hw/internal/tracker/tracker_base.hpp" #include "vio_hw/internal/viz/visualization_base.hpp" +#include "geometry/motion_ba/motion_ba.hpp" namespace viohw { class VisualFrontEnd diff --git a/vio_hw/internal/viz/rviz_visualization.hpp b/vio_hw/internal/viz/rviz_visualization.hpp index 649d61a..c206025 100644 --- a/vio_hw/internal/viz/rviz_visualization.hpp +++ b/vio_hw/internal/viz/rviz_visualization.hpp @@ -34,6 +34,8 @@ class RvizVisualization : public VisualizationBase CameraPoseVisualization camera_pose_visual_; ros::Publisher pub_kfs_traj_, pub_vo_traj_; ros::Publisher pub_img_tracker_result_; + nav_msgs::Path vo_traj_path_; + const std::string world_frame_ = "map"; }; } // namespace viohw diff --git a/vio_hw/params/rviz/hw-vio.rviz b/vio_hw/params/rviz/hw-vio.rviz index 2ef85e3..bfe10c1 100644 --- a/vio_hw/params/rviz/hw-vio.rviz +++ b/vio_hw/params/rviz/hw-vio.rviz @@ -23,7 +23,7 @@ Panels: - Class: rviz/Time Name: Time SyncMode: 0 - SyncSource: Image + SyncSource: tracker_result Preferences: PromptSaveOnExit: true Toolbars: @@ -61,6 +61,30 @@ Visualization Manager: Transport Hint: raw Unreliable: false Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: vo_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /hello_world_vio/vo_traj + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -89,7 +113,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 10 + Distance: 31.058481216430664 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -107,7 +131,7 @@ Visualization Manager: Near Clip Distance: 0.009999999776482582 Pitch: 0.9953978061676025 Target Frame: - Yaw: 1.5403972864151 + Yaw: 1.5353972911834717 Saved: ~ Window Geometry: Displays: diff --git a/vio_hw/src/frame.cpp b/vio_hw/src/frame.cpp index 6dfc7ba..72aa5dc 100644 --- a/vio_hw/src/frame.cpp +++ b/vio_hw/src/frame.cpp @@ -186,15 +186,131 @@ void Frame::RemoveKeypointById(const int lmid) { mapkps_.erase(lmid); } -Keypoint Frame::GetKeypointById(const int lmid) const -{ +Keypoint Frame::GetKeypointById(const int lmid) const { std::lock_guard lock(kps_mutex_); auto it = mapkps_.find(lmid); - if( it == mapkps_.end() ) { + if (it == mapkps_.end()) { return Keypoint(); } return it->second; } + +void Frame::UpdateKeypointStereo(const int lmid, const cv::Point2f &pt) { + std::lock_guard lock(kps_mutex_); + + auto it = mapkps_.find(lmid); + if (it == mapkps_.end()) { + return; + } + + ComputeStereoKeypoint(pt, it->second); +} + +// Compute stereo keypoint from raw pixel position +void Frame::ComputeStereoKeypoint(const cv::Point2f &pt, Keypoint &kp) { + kp.rpx_ = pt; + kp.runpx_ = pcalib_rightcam_->undistortImagePoint(pt); + + Eigen::Vector3d bv(kp.runpx_.x, kp.runpx_.y, 1.); + bv = pcalib_rightcam_->iK_ * bv.eval(); + bv.normalize(); + + kp.rbv_ = bv; + + if (!kp.is_stereo_) { + kp.is_stereo_ = true; + nb_stereo_kps_++; + } +} +// Return vector of stereo keypoint objects +std::vector Frame::getKeypointsStereo() const { + std::lock_guard lock(kps_mutex_); + + std::vector v; + v.reserve(nb_stereo_kps_); + for (const auto &kp : mapkps_) { + if (kp.second.is_stereo_) { + v.push_back(kp.second); + } + } + return v; +} +// Return vector of 2D keypoint objects +std::vector Frame::GetKeypoints2d() const +{ + std::lock_guard lock(kps_mutex_); + + std::vector v; + v.reserve(nb2dkps_); + for( const auto & kp : mapkps_ ) { + if( !kp.second.is3d_ ) { + v.push_back(kp.second); + } + } + return v; +} + +void Frame::RemoveStereoKeypointById(const int lmid) { + std::lock_guard lock(kps_mutex_); + + auto it = mapkps_.find(lmid); + if (it == mapkps_.end()) { + return; + } + + if (it->second.is_stereo_) { + it->second.is_stereo_ = false; + nb_stereo_kps_--; + } +} + +void Frame::TurnKeypoint3d(const int lmid) { + std::lock_guard lock(kps_mutex_); + + auto it = mapkps_.find(lmid); + if (it == mapkps_.end()) { + return; + } + + if (!it->second.is3d_) { + it->second.is3d_ = true; + nb3dkps_++; + nb2dkps_--; + } +} + +cv::Point2f Frame::ProjCamToImage(const Eigen::Vector3d &pt) const { + return pcalib_leftcam_->projectCamToImage(pt); +} + +cv::Point2f Frame::ProjCamToRightImage(const Eigen::Vector3d &pt) const { + return pcalib_rightcam_->projectCamToImage(pcalib_rightcam_->Tcic0_ * pt); +} + +Eigen::Vector3d Frame::ProjCamToWorld(const Eigen::Vector3d &pt) const { + std::lock_guard lock(pose_mutex_); + + Eigen::Vector3d wpt = Twc_ * pt; + + return wpt; +} +Sophus::SE3d Frame::GetTwc() const +{ + std::lock_guard lock(pose_mutex_); + return Twc_; +} +Sophus::SE3d Frame::GetTcw() const +{ + std::lock_guard lock(pose_mutex_); + return Tcw_; +} +void Frame::SetTwc(const Sophus::SE3d &Twc) +{ + std::lock_guard lock(pose_mutex_); + + Twc_ = Twc; + Tcw_ = Twc.inverse(); +} } // namespace viohw diff --git a/vio_hw/src/map_manager.cpp b/vio_hw/src/map_manager.cpp index 2780ebc..cb8265e 100644 --- a/vio_hw/src/map_manager.cpp +++ b/vio_hw/src/map_manager.cpp @@ -164,4 +164,114 @@ std::shared_ptr MapManager::GetMapPoint(const int lmid) const { return it->second; } +void MapManager::StereoMatching(Frame& frame, const std::vector& vleftpyr, + const std::vector& vrightpyr) { + // Find stereo correspondances with left kps + auto vleftkps = frame.getKeypoints(); + size_t nbkps = vleftkps.size(); + + std::vector v3dkpids, vkpids, voutkpids, vpriorids; + std::vector v3dkps, v3dpriors, vkps, vpriors; + for (size_t i = 0; i < nbkps; i++) { + // Set left kp + auto& kp = vleftkps.at(i); + + // Set prior right kp + cv::Point2f priorpt = kp.px_; + + vkpids.push_back(kp.lmid_); + vkps.push_back(kp.px_); + vpriors.push_back(priorpt); + } + + std::vector good_right_kps; + std::vector good_ids; + size_t num_good = 0, tracker_good = 0, inliner_good = 0; + if (!vkps.empty()) { + // Good / bad kps vector + std::vector vkpstatus; + std::vector inliers; + tracker_->trackerAndMatcher( + vleftpyr, vrightpyr, param_->feat_tracker_setting_.klt_win_size_, + param_->feat_tracker_setting_.klt_pyr_level_, param_->feat_tracker_setting_.klt_err_, + param_->feat_tracker_setting_.klt_max_fb_dist_, vkps, vpriors, vkpstatus); + + + for (size_t i = 0; i < vkpstatus.size(); i++) { + if (vkpstatus.at(i)) { + frame.UpdateKeypointStereo(vkpids.at(i), vpriors.at(i)); + num_good++; + } + } + // TODO + } + LOG(INFO) << "kp num: " << vkps.size() << ",Good Stereo Matching Num: " << num_good + << ",tracker good:" << tracker_good << ", inliner good:" << inliner_good; +} + +void MapManager::UpdateMapPoint(const int lmid, const Eigen::Vector3d& wpt, + const double inv_depth) { + std::lock_guard lock(lm_mutex_); + std::lock_guard lock_kf(kf_mutex_); + auto plmit = map_lms_.find(lmid); + + if (plmit == map_lms_.end()) { + return; + } + if (plmit->second == nullptr) { + return; + } + // If MP 2D -> 3D => Notif. KFs + if (!plmit->second->is3d_) { + for (const auto& kfid : plmit->second->GetKfObsSet()) { + auto pkfit = map_kfs_.find(kfid); + if (pkfit != map_kfs_.end()) { + pkfit->second->TurnKeypoint3d(lmid); + } else { + plmit->second->RemoveKfObs(kfid); + } + } + if (plmit->second->isobs_) { + current_frame_->TurnKeypoint3d(lmid); + } + } + + // Update MP world pos. + if( inv_depth >= 0. ) { + plmit->second->SetPoint(wpt, inv_depth); + } else { + plmit->second->SetPoint(wpt); + } +} +// Remove a KF obs from a MP +void MapManager::RemoveMapPointObs(const int lmid, const int kfid) { + std::lock_guard lock(lm_mutex_); + std::lock_guard lockkf(kf_mutex_); + + // Remove MP obs from KF + auto pkfit = map_kfs_.find(kfid); + if (pkfit != map_kfs_.end()) { + pkfit->second->RemoveKeypointById(lmid); + } + + // Remove KF obs from MP + auto plmit = map_lms_.find(lmid); + + // Skip if MP does not exist + if (plmit == map_lms_.end()) { + return; + } + plmit->second->RemoveKfObs(kfid); + + // TODO + // if( pkfit != map_pkfs_.end() ) { + // for( const auto &cokfid : plmit->second->getKfObsSet() ) { + // auto pcokfit = map_pkfs_.find(cokfid); + // if( pcokfit != map_pkfs_.end() ) { + // pkfit->second->decreaseCovisibleKf(cokfid); + // pcokfit->second->decreaseCovisibleKf(kfid); + // } + // } + // } +} } // namespace viohw diff --git a/vio_hw/src/map_point.cpp b/vio_hw/src/map_point.cpp index 2d7c5b7..2ce8f09 100644 --- a/vio_hw/src/map_point.cpp +++ b/vio_hw/src/map_point.cpp @@ -19,4 +19,76 @@ MapPoint::MapPoint(const int lmid, const int kfid, const cv::Mat &desc, const bo is3d_ = false; ptxyz_.setZero(); } + +std::set MapPoint::GetKfObsSet() const { + std::lock_guard lock(pt_mutex); + return set_kfids_; +} +void MapPoint::RemoveKfObs(const int kfid) { + std::lock_guard lock(pt_mutex); + + if (!set_kfids_.count(kfid)) { + return; + } + + // First remove the related id + set_kfids_.erase(kfid); + + if (set_kfids_.empty()) { + desc_.release(); + map_kf_desc_.clear(); + map_desc_dist_.clear(); + return; + } + + // Set new KF anchor if removed + if (kfid == kfid_) { + kfid_ = *set_kfids_.begin(); + } + + // Then figure out the most representative one + // (we could also use the last one to save time) + float mindist = desc_.cols * 8.; + int minid = -1; + + auto itdesc = map_kf_desc_.find(kfid); + if (itdesc != map_kf_desc_.end()) { + for (const auto &kf_d : map_kf_desc_) { + if (kf_d.first != kfid) { + float dist = cv::norm(itdesc->second, kf_d.second, cv::NORM_HAMMING); + float &descdist = map_desc_dist_.find(kf_d.first)->second; + descdist -= dist; + + // Get the lowest one + if (descdist < mindist) { + mindist = descdist; + minid = kf_d.first; + } + } + } + + itdesc->second.release(); + map_kf_desc_.erase(kfid); + map_desc_dist_.erase(kfid); + + // Remove desc / update mean desc + if (minid > 0) { + desc_ = map_kf_desc_.at(minid); + } + } +} + +Eigen::Vector3d MapPoint::GetPoint() const { + std::lock_guard lock(pt_mutex); + return ptxyz_; +} + +void MapPoint::SetPoint(const Eigen::Vector3d &ptxyz, const double kfanch_invdepth) { + std::lock_guard lock(pt_mutex); + ptxyz_ = ptxyz; + is3d_ = true; + if (kfanch_invdepth >= 0.) { + invdepth_ = kfanch_invdepth; + } +} } // namespace viohw \ No newline at end of file diff --git a/vio_hw/src/mapping.cpp b/vio_hw/src/mapping.cpp index e482f0b..af1a517 100644 --- a/vio_hw/src/mapping.cpp +++ b/vio_hw/src/mapping.cpp @@ -4,15 +4,51 @@ namespace viohw { Mapping::Mapping(viohw::SettingPtr param, viohw::MapManagerPtr map_manager, viohw::FramePtr frame) : params_(param), map_manager_(map_manager), current_frame_(frame) { + stereo_mode_ = params_->slam_setting_.stereo_mode_; + use_clahe_ = params_->feat_tracker_setting_.use_clahe_; + + if (use_clahe_) { + int tilesize = 50; + cv::Size clahe_tiles(params_->cam_setting_.left_resolution_[0] / tilesize, + params_->cam_setting_.left_resolution_[1] / tilesize); + clahe_ = cv::createCLAHE(params_->feat_tracker_setting_.clahe_val_, clahe_tiles); + } std::thread mapper_thread(&Mapping::run, this); mapper_thread.detach(); } void Mapping::run() { LOG(INFO) << "Mapper is ready to process Keyframes!"; + int klt_win_size = params_->feat_tracker_setting_.klt_win_size_; + cv::Size cv_klt_win_size(klt_win_size, klt_win_size); + int klt_pyra_level = params_->feat_tracker_setting_.klt_pyr_level_; Keyframe kf; while (true) { - if (getNewKf(kf)) { + if (GetNewKf(kf)) { + // Get new KF ptr + std::shared_ptr new_kf = map_manager_->GetKeyframe(kf.kfid_); + assert(new_kf); + if (stereo_mode_) { + cv::Mat img_right, img_left = kf.imleftraw_; + if (use_clahe_) { + clahe_->apply(kf.imrightraw_, img_right); + } else { + img_right = kf.imrightraw_; + } + std::vector vpyr_img_right, vpyr_img_left; + cv::buildOpticalFlowPyramid(img_right, vpyr_img_right, cv_klt_win_size, klt_pyra_level); + cv::buildOpticalFlowPyramid(img_left, vpyr_img_left, cv_klt_win_size, klt_pyra_level); + map_manager_->StereoMatching(*new_kf, vpyr_img_left, vpyr_img_right); + if (new_kf->nb2dkps_ > 0 && new_kf->nb_stereo_kps_ > 0) { + std::lock_guard lock(map_manager_->map_mutex_); + TriangulateStereo(*new_kf); + } + } + if (new_kf->nb2dkps_ > 0 && new_kf->kfid_ > 0) { + std::lock_guard lock(map_manager_->map_mutex_); + TriangulateTemporal(*new_kf); + } + } else { std::chrono::microseconds dura(100); std::this_thread::sleep_for(dura); @@ -20,16 +56,186 @@ void Mapping::run() { } } -void Mapping::TriangulateTemporal(Frame& frame) {} +void Mapping::TriangulateTemporal(Frame& frame) { + // Get New KF kps / pose + std::vector vkps = frame.GetKeypoints2d(); + size_t nbkps = vkps.size(); + + Sophus::SE3d Twcj = frame.GetTwc(); + if (vkps.empty()) { + LOG(WARNING) << ">>> No kps to temporal triangulate..."; + return; + } + + std::vector > vleftbvs, vrightbvs; + // Init a pkf object that will point to the prev KF to use for triangulation + std::shared_ptr pkf; + pkf.reset(new Frame()); + pkf->kfid_ = -1; + + // Relative motions between new KF and prev. KFs + int relkfid = -1; + Sophus::SE3d Tcicj, Tcjci; + Eigen::Matrix3d Rcicj; + + std::vector > vwpts; + std::vector vlmids; + int good = 0; + + for (size_t i = 0; i < nbkps; i++) { + // Get the related MP and check if it is ready to be triangulated + std::shared_ptr plm = map_manager_->GetMapPoint(vkps.at(i).lmid_); + if (plm == nullptr) { + map_manager_->RemoveMapPointObs(vkps.at(i).lmid_, frame.kfid_); + continue; + } + // If MP is already 3D continue (should not happen) + if (plm->is3d_) { + continue; + } + // Get the set of KFs sharing observation of this 2D MP + std::set co_kf_ids = plm->GetKfObsSet(); + // Continue if new KF is the only one observing it + if (co_kf_ids.size() < 2) { + continue; + } + + int kfid = *co_kf_ids.begin(); + if (frame.kfid_ == kfid) { + continue; + } + + // Get the 1st KF observation of the related MP + pkf = map_manager_->GetKeyframe(kfid); + + if (pkf == nullptr) { + continue; + } -void Mapping::TriangulateStereo(Frame& frame) {} + // Compute relative motion between new KF and selected KF (only if req.) + if (relkfid != kfid) { + Sophus::SE3d Tciw = pkf->GetTcw(); + Tcicj = Tciw * Twcj; + Tcjci = Tcicj.inverse(); + Rcicj = Tcicj.rotationMatrix(); + + relkfid = kfid; + } + // If no motion between both KF, skip + if (params_->slam_setting_.stereo_mode_ && Tcicj.translation().norm() < 0.01) { + continue; + } + // Get obs kp + Keypoint kfkp = pkf->GetKeypointById(vkps.at(i).lmid_); + if (kfkp.lmid_ != vkps.at(i).lmid_) { + continue; + } + + // Check rotation-compensated parallax + cv::Point2f rotpx = frame.ProjCamToImage(Rcicj * vkps.at(i).bv_); + double parallax = cv::norm(kfkp.unpx_ - rotpx); + // Compute 3D pos and check if its good or not + Eigen::Vector3d left_pt = ComputeTriangulation(Tcicj, kfkp.bv_, vkps.at(i).bv_); + // Project into right cam (new KF) + Eigen::Vector3d right_pt = Tcjci * left_pt; + // Ensure that the 3D MP is in front of both camera + if (left_pt.z() < 0.1 || right_pt.z() < 0.1) { + if (parallax > 20.) { + map_manager_->RemoveMapPointObs(kfkp.lmid_, frame.kfid_); + } + continue; + } + + // Remove MP with high reprojection error + cv::Point2f left_px_proj = pkf->ProjCamToImage(left_pt); + cv::Point2f right_px_proj = frame.ProjCamToImage(right_pt); + double ldist = cv::norm(left_px_proj - kfkp.unpx_); + double rdist = cv::norm(right_px_proj - vkps.at(i).unpx_); + + if (ldist > 30. || rdist > 30.) { + if (parallax > 20.) { + map_manager_->RemoveMapPointObs(kfkp.lmid_, frame.kfid_); + } + continue; + } + + // The 3D pos is good, update SLAM MP and related KF / Frame + Eigen::Vector3d wpt = pkf->ProjCamToWorld(left_pt); + map_manager_->UpdateMapPoint(vkps.at(i).lmid_, wpt, 1./left_pt.z()); + + good++; + } +} + +void Mapping::TriangulateStereo(Frame& frame) { + std::vector vkps = frame.getKeypointsStereo(); + size_t num_kps = vkps.size(); + + if (vkps.empty()) { + LOG(WARNING) << ">>> No kps to stereo triangulate..."; + return; + } + + // Store the stereo kps along with their idx + std::vector stereo_idx; + std::vector lm_ids; + + std::vector > left_bvs, right_bvs; + std::vector > world_pts; + + Sophus::SE3d Tlr = frame.pcalib_rightcam_->getExtrinsic(); + Sophus::SE3d Trl = Tlr.inverse(); + + for (size_t i = 0; i < num_kps; i++) { + if (!vkps.at(i).is3d_ && vkps.at(i).is_stereo_) { + stereo_idx.push_back(i); + left_bvs.push_back(vkps.at(i).bv_); + right_bvs.push_back(vkps.at(i).rbv_); + } + } + + if (stereo_idx.empty()) { + LOG(WARNING) << "stereo feature is empty"; + return; + } + + int good; + + for (size_t i = 0; i < stereo_idx.size(); i++) { + int kpidx = stereo_idx.at(i); + Eigen::Vector3d left_pt = ComputeTriangulation(Tlr, left_bvs.at(i), right_bvs.at(i)); + // Project into right cam frame + Eigen::Vector3d right_pt = Trl * left_pt; + if (left_pt.z() < 0.1 || right_pt.z() < 0.1) { + frame.RemoveStereoKeypointById(vkps.at(kpidx).lmid_); + continue; + } + + cv::Point2f left_px_proj = frame.ProjCamToImage(left_pt); + cv::Point2f right_px_proj = frame.ProjCamToRightImage(left_pt); + double ldist = cv::norm(left_px_proj - vkps.at(kpidx).unpx_); + double rdist = cv::norm(right_px_proj - vkps.at(kpidx).runpx_); + // TODO max_reprojection_error in params + if (ldist > 3. || rdist > 3.) { + frame.RemoveStereoKeypointById(vkps.at(kpidx).lmid_); + continue; + } + + // Project MP in world frame + Eigen::Vector3d wpt = frame.ProjCamToWorld(left_pt); + + map_manager_->UpdateMapPoint(vkps.at(kpidx).lmid_, wpt, 1. / left_pt.z()); + + good++; + } +} Eigen::Vector3d Mapping::ComputeTriangulation(const Sophus::SE3d& T, const Eigen::Vector3d& bvl, const Eigen::Vector3d& bvr) { - return Eigen::Vector3d(); + return geometry::OpencvTriangulate(T, bvl, bvr); } -bool Mapping::getNewKf(Keyframe& kf) { +bool Mapping::GetNewKf(Keyframe& kf) { std::lock_guard lock(kf_queen_mutex_); // Check if new KF is available @@ -55,7 +261,7 @@ bool Mapping::getNewKf(Keyframe& kf) { return true; } -void Mapping::addNewKf(const Keyframe& kf) { +void Mapping::AddNewKf(const Keyframe& kf) { std::lock_guard lock(kf_queen_mutex_); kfs_queen_.push(kf); diff --git a/vio_hw/src/visual_frontend.cpp b/vio_hw/src/visual_frontend.cpp index e8ba75e..37bb301 100644 --- a/vio_hw/src/visual_frontend.cpp +++ b/vio_hw/src/visual_frontend.cpp @@ -198,14 +198,70 @@ void VisualFrontEnd::ShowTrackingResult() { cv::arrowedLine(draw_tracker_image, kp.px_, kf_kp.px_, cv::Scalar(0, 255, 0), 2, 8, 0, 0.3); cv::circle(draw_tracker_image, kf_kp.px_, 2, cv::Scalar(0, 255, 0), -1); } - // cv::imshow("tracker", draw_tracker_image); - // cv::waitKey(1); + viz_->showTrackerResultImage(draw_tracker_image); } bool VisualFrontEnd::CheckIsNewKeyframe() { return true; } -void VisualFrontEnd::ComputePose() {} +void VisualFrontEnd::ComputePose() { + // Get cur nb of 3D kps + size_t nb3dkps = current_frame_->nb3dkps_; + LOG(INFO) << "3d kps num: " << nb3dkps; + if (nb3dkps < 4) { + LOG(WARNING) << ">>> Not enough kps to compute P3P / PnP"; + return; + } + // Setup P3P-Ransac computation for OpenGV-based Pose estimation + motion-only BA with Ceres + std::vector > vwpts, vbvs; + std::vector > vkps; + std::vector vkpids, voutliersidx; + + for (const auto& it : current_frame_->mapkps_) { + if (!it.second.is3d_) { + continue; + } + auto& kp = it.second; + auto plm = map_manager_->GetMapPoint(kp.lmid_); + if (plm == nullptr) { + continue; + } + // for P3P + vbvs.push_back(kp.bv_); + + vkps.push_back(Eigen::Vector2d(kp.unpx_.x, kp.unpx_.y)); + vwpts.push_back(plm->GetPoint()); + vkpids.push_back(kp.lmid_); + } + + std::vector vscales(vkps.size(), 0); + Sophus::SE3d Twc = current_frame_->GetTwc(); + bool success = false; + int max_iters = 5; + float robust_mono_th = 5.9915; + bool use_robust = true; + Eigen::Matrix3d K = current_frame_->pcalib_leftcam_->K_; + // success = geometry::tceresMotionOnlyBA(vkps, vwpts, vscales, Twc, max_iters, robust_mono_th, use_robust, + // true, K, voutliersidx); + + success = geometry::opencvP3PRansac(vbvs, vwpts, 100, 3., K(0, 0), K(1, 1), true, Twc, + voutliersidx); + + // Check that pose estim. was good enough + size_t nbinliers = vwpts.size() - voutliersidx.size(); + if (!success || nbinliers < 5 || voutliersidx.size() > 0.5 * vwpts.size() || + Twc.translation().array().isInf().any() || Twc.translation().array().isNaN().any()) { + LOG(WARNING) << "ceres PNP calculate failed"; + } + // Update frame pose + current_frame_->SetTwc(Twc); + // Remove outliers + for (const auto& idx : voutliersidx) { + // MapManager is responsible for all removing operations + map_manager_->RemoveObsFromCurFrameById(vkpids.at(idx)); + } + viz_->addTrajectory(Twc.rotationMatrix(), Twc.translation()); +} void VisualFrontEnd::UpdateMotionModel() {} diff --git a/vio_hw/src/viz/rviz_visualization.cpp b/vio_hw/src/viz/rviz_visualization.cpp index 68cce17..eb76a52 100644 --- a/vio_hw/src/viz/rviz_visualization.cpp +++ b/vio_hw/src/viz/rviz_visualization.cpp @@ -2,8 +2,28 @@ namespace viohw { -bool RvizVisualization::addTrajectory(const Eigen::Matrix3d& Q, const Eigen::Vector3d& t) { - std::cout << "rviz addTrajectory\n"; +RvizVisualization::RvizVisualization() : camera_pose_visual_(1, 0, 0, 1) { + LOG(INFO) << "Create RvizVisualization"; + ros::NodeHandle n("~"); + pub_img_tracker_result_ = n.advertise("image_track", 1000); + pub_vo_traj_ = n.advertise("vo_traj", 100); + vo_traj_path_.header.stamp = ros::Time::now(); + vo_traj_path_.header.frame_id = world_frame_; +} + +bool RvizVisualization::addTrajectory(const Eigen::Matrix3d& R, const Eigen::Vector3d& t) { + geometry_msgs::PoseStamped pose; + pose.header.stamp = ros::Time::now(); + Eigen::Quaterniond Q(R); + pose.pose.orientation.x = Q.x(); + pose.pose.orientation.y = Q.y(); + pose.pose.orientation.z = Q.z(); + pose.pose.orientation.w = Q.w(); + pose.pose.position.x = t.x(); + pose.pose.position.y = t.y(); + pose.pose.position.z = t.z(); + vo_traj_path_.poses.push_back(pose); + pub_vo_traj_.publish(vo_traj_path_); return true; } @@ -30,10 +50,4 @@ bool RvizVisualization::addPoint(const Eigen::Vector3d& t, const Eigen::Vector3d return false; } -RvizVisualization::RvizVisualization() : camera_pose_visual_(1, 0, 0, 1) { - LOG(INFO) << "Create RvizVisualization"; - ros::NodeHandle n("~"); - pub_img_tracker_result_ = n.advertise("image_track", 1000); -} - } // namespace viohw diff --git a/vio_hw/src/world_manager.cpp b/vio_hw/src/world_manager.cpp index e3bcb10..591ecf7 100644 --- a/vio_hw/src/world_manager.cpp +++ b/vio_hw/src/world_manager.cpp @@ -53,7 +53,8 @@ void WorldManager::run() { bool is_kf = visual_frontend_->VisualTracking(img_left, cur_time); if (is_kf) { - LOG(INFO) << "Keyframe create"; + Keyframe kf(current_frame_->kfid_, img_left, img_right); + mapping_->AddNewKf(kf); } } std::chrono::milliseconds dura(1);