From 53798aa9e8fe450d52839b988b8909761dad9579 Mon Sep 17 00:00:00 2001 From: Tom_TKO Date: Sat, 28 Dec 2024 00:10:57 -0500 Subject: [PATCH 1/7] experiment using LBFGSBSolver to estimate yaw --- .vscode/settings.json | 8 +- .../webcam_publisher/CMakeLists.txt | 2 + src/prm_launch/launch/video2detector.py | 4 +- .../opencv_armor_detector/CMakeLists.txt | 6 +- .../opencv_armor_detector/include/LBFGS.h | 192 +++++++ .../opencv_armor_detector/include/LBFGSB.h | 284 ++++++++++ .../include/LBFGSpp/BFGSMat.h | 496 ++++++++++++++++ .../include/LBFGSpp/BKLDLT.h | 536 ++++++++++++++++++ .../include/LBFGSpp/Cauchy.h | 291 ++++++++++ .../include/LBFGSpp/LineSearchBacktracking.h | 125 ++++ .../include/LBFGSpp/LineSearchBracketing.h | 130 +++++ .../include/LBFGSpp/LineSearchMoreThuente.h | 444 +++++++++++++++ .../include/LBFGSpp/LineSearchNocedalWright.h | 281 +++++++++ .../include/LBFGSpp/Param.h | 381 +++++++++++++ .../include/LBFGSpp/SubspaceMin.h | 309 ++++++++++ .../include/OpenCVArmorDetector.h | 2 + .../src/OpenCVArmorDetector.cpp | 303 +++++++++- 17 files changed, 3788 insertions(+), 6 deletions(-) create mode 100644 src/prm_vision/opencv_armor_detector/include/LBFGS.h create mode 100644 src/prm_vision/opencv_armor_detector/include/LBFGSB.h create mode 100644 src/prm_vision/opencv_armor_detector/include/LBFGSpp/BFGSMat.h create mode 100644 src/prm_vision/opencv_armor_detector/include/LBFGSpp/BKLDLT.h create mode 100644 src/prm_vision/opencv_armor_detector/include/LBFGSpp/Cauchy.h create mode 100644 src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchBacktracking.h create mode 100644 src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchBracketing.h create mode 100644 src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchMoreThuente.h create mode 100644 src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchNocedalWright.h create mode 100644 src/prm_vision/opencv_armor_detector/include/LBFGSpp/Param.h create mode 100644 src/prm_vision/opencv_armor_detector/include/LBFGSpp/SubspaceMin.h diff --git a/.vscode/settings.json b/.vscode/settings.json index ea8e196..41b9ed7 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -67,6 +67,12 @@ "typeindex": "cpp", "typeinfo": "cpp", "variant": "cpp", - "bit": "cpp" + "bit": "cpp", + "any": "cpp", + "compare": "cpp", + "concepts": "cpp", + "numbers": "cpp", + "semaphore": "cpp", + "stop_token": "cpp" } } \ No newline at end of file diff --git a/src/prm_camera/webcam_publisher/CMakeLists.txt b/src/prm_camera/webcam_publisher/CMakeLists.txt index efe2c3b..f86d01d 100755 --- a/src/prm_camera/webcam_publisher/CMakeLists.txt +++ b/src/prm_camera/webcam_publisher/CMakeLists.txt @@ -3,6 +3,8 @@ project(webcam_publisher) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + find_package(ament_cmake_auto REQUIRED) diff --git a/src/prm_launch/launch/video2detector.py b/src/prm_launch/launch/video2detector.py index 0c6a70d..031a805 100755 --- a/src/prm_launch/launch/video2detector.py +++ b/src/prm_launch/launch/video2detector.py @@ -7,7 +7,7 @@ def generate_launch_description(): webcam_publisher = get_package_share_path('webcam_publisher') - video_path = "/home/tom/Videos/close.avi" # example, can change to your liking + video_path = "/home/tom/Videos/far_back_spin_and_move.avi" # example, can change to your liking return LaunchDescription([ Node( package='webcam_publisher', @@ -15,7 +15,7 @@ def generate_launch_description(): emulate_tty=True, executable='VideoCaptureNode', parameters=[{'source': str(video_path), - 'fps': 1, + 'fps': 150, 'frame_id': 'video', }] ), diff --git a/src/prm_vision/opencv_armor_detector/CMakeLists.txt b/src/prm_vision/opencv_armor_detector/CMakeLists.txt index 03da33d..a01d07e 100644 --- a/src/prm_vision/opencv_armor_detector/CMakeLists.txt +++ b/src/prm_vision/opencv_armor_detector/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.8) project(opencv_armor_detector CXX) set(CMAKE_CXX_STANDARD 17) # for filesystem support +set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + # Dependencies find_package(ament_cmake REQUIRED) @@ -11,6 +13,7 @@ find_package(OpenCV 4.6.0 REQUIRED) find_package(cv_bridge REQUIRED) find_package(vision_msgs REQUIRED) find_package(ament_index_cpp REQUIRED) # Filesystem search in tests +find_package(Eigen3 REQUIRED) # Eigen3 find_package(ament_cmake_gtest REQUIRED) # GTest # Include directories @@ -19,7 +22,7 @@ include_directories(include) # Build target add_library(OpenCVArmorDetector STATIC src/OpenCVArmorDetector.cpp) -target_link_libraries(OpenCVArmorDetector ${OpenCV_LIBS}) +target_link_libraries(OpenCVArmorDetector ${OpenCV_LIBS} Eigen3::Eigen) add_executable(OpenCVArmorDetectorNode src/OpenCVArmorDetectorNode.cpp) target_link_libraries(OpenCVArmorDetectorNode OpenCVArmorDetector ${OpenCV_LIBS}) @@ -30,6 +33,7 @@ ament_target_dependencies(OpenCVArmorDetectorNode image_transport cv_bridge vision_msgs + Eigen3 ament_index_cpp ) diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGS.h b/src/prm_vision/opencv_armor_detector/include/LBFGS.h new file mode 100644 index 0000000..3d8c132 --- /dev/null +++ b/src/prm_vision/opencv_armor_detector/include/LBFGS.h @@ -0,0 +1,192 @@ +// Copyright (C) 2016-2023 Yixuan Qiu +// Under MIT license + +#ifndef LBFGSPP_LBFGS_H +#define LBFGSPP_LBFGS_H + +#include +#include "LBFGSpp/Param.h" +#include "LBFGSpp/BFGSMat.h" +#include "LBFGSpp/LineSearchBacktracking.h" +#include "LBFGSpp/LineSearchBracketing.h" +#include "LBFGSpp/LineSearchNocedalWright.h" +#include "LBFGSpp/LineSearchMoreThuente.h" + +namespace LBFGSpp { + +/// +/// L-BFGS solver for unconstrained numerical optimization +/// +template class LineSearch = LineSearchNocedalWright> +class LBFGSSolver +{ +private: + using Vector = Eigen::Matrix; + using Matrix = Eigen::Matrix; + using MapVec = Eigen::Map; + + const LBFGSParam& m_param; // Parameters to control the LBFGS algorithm + BFGSMat m_bfgs; // Approximation to the Hessian matrix + Vector m_fx; // History of the objective function values + Vector m_xp; // Old x + Vector m_grad; // New gradient + Scalar m_gnorm; // Norm of the gradient + Vector m_gradp; // Old gradient + Vector m_drt; // Moving direction + + // Reset internal variables + // n: dimension of the vector to be optimized + inline void reset(int n) + { + const int m = m_param.m; + m_bfgs.reset(n, m); + m_xp.resize(n); + m_grad.resize(n); + m_gradp.resize(n); + m_drt.resize(n); + if (m_param.past > 0) + m_fx.resize(m_param.past); + } + +public: + /// + /// Constructor for the L-BFGS solver. + /// + /// \param param An object of \ref LBFGSParam to store parameters for the + /// algorithm + /// + LBFGSSolver(const LBFGSParam& param) : + m_param(param) + { + m_param.check_param(); + } + + /// + /// Minimizing a multivariate function using the L-BFGS algorithm. + /// Exceptions will be thrown if error occurs. + /// + /// \param f A function object such that `f(x, grad)` returns the + /// objective function value at `x`, and overwrites `grad` with + /// the gradient. + /// \param x In: An initial guess of the optimal point. Out: The best point + /// found. + /// \param fx Out: The objective function value at `x`. + /// + /// \return Number of iterations used. + /// + template + inline int minimize(Foo& f, Vector& x, Scalar& fx) + { + using std::abs; + + // Dimension of the vector + const int n = x.size(); + reset(n); + + // The length of lag for objective function value to test convergence + const int fpast = m_param.past; + + // Evaluate function and compute gradient + fx = f(x, m_grad); + m_gnorm = m_grad.norm(); + if (fpast > 0) + m_fx[0] = fx; + + // std::cout << "x0 = " << x.transpose() << std::endl; + // std::cout << "f(x0) = " << fx << ", ||grad|| = " << m_gnorm << std::endl << std::endl; + + // Early exit if the initial x is already a minimizer + if (m_gnorm <= m_param.epsilon || m_gnorm <= m_param.epsilon_rel * x.norm()) + { + return 1; + } + + // Initial direction + m_drt.noalias() = -m_grad; + // Initial step size + Scalar step = Scalar(1) / m_drt.norm(); + // Tolerance for s'y >= eps * (y'y) + constexpr Scalar eps = std::numeric_limits::epsilon(); + // s and y vectors + Vector vecs(n), vecy(n); + + // Number of iterations used + int k = 1; + for (;;) + { + // std::cout << "Iter " << k << " begins" << std::endl << std::endl; + + // Save the curent x and gradient + m_xp.noalias() = x; + m_gradp.noalias() = m_grad; + Scalar dg = m_grad.dot(m_drt); + const Scalar step_max = m_param.max_step; + + // Line search to update x, fx and gradient + LineSearch::LineSearch(f, m_param, m_xp, m_drt, step_max, step, fx, m_grad, dg, x); + + // New gradient norm + m_gnorm = m_grad.norm(); + + // std::cout << "Iter " << k << " finished line search" << std::endl; + // std::cout << " x = " << x.transpose() << std::endl; + // std::cout << " f(x) = " << fx << ", ||grad|| = " << m_gnorm << std::endl << std::endl; + + // Convergence test -- gradient + if (m_gnorm <= m_param.epsilon || m_gnorm <= m_param.epsilon_rel * x.norm()) + { + return k; + } + // Convergence test -- objective function value + if (fpast > 0) + { + const Scalar fxd = m_fx[k % fpast]; + if (k >= fpast && abs(fxd - fx) <= m_param.delta * std::max(std::max(abs(fx), abs(fxd)), Scalar(1))) + return k; + + m_fx[k % fpast] = fx; + } + // Maximum number of iterations + if (m_param.max_iterations != 0 && k >= m_param.max_iterations) + { + return k; + } + + // Update s and y + // s_{k+1} = x_{k+1} - x_k + // y_{k+1} = g_{k+1} - g_k + vecs.noalias() = x - m_xp; + vecy.noalias() = m_grad - m_gradp; + if (vecs.dot(vecy) > eps * vecy.squaredNorm()) + m_bfgs.add_correction(vecs, vecy); + + // Recursive formula to compute d = -H * g + m_bfgs.apply_Hv(m_grad, -Scalar(1), m_drt); + + // Reset step = 1.0 as initial guess for the next line search + step = Scalar(1); + k++; + } + + return k; + } + + /// + /// Returning the gradient vector on the last iterate. + /// Typically used to debug and test convergence. + /// Should only be called after the `minimize()` function. + /// + /// \return A const reference to the gradient vector. + /// + const Vector& final_grad() const { return m_grad; } + + /// + /// Returning the Euclidean norm of the final gradient. + /// + Scalar final_grad_norm() const { return m_gnorm; } +}; + +} // namespace LBFGSpp + +#endif // LBFGSPP_LBFGS_H diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSB.h b/src/prm_vision/opencv_armor_detector/include/LBFGSB.h new file mode 100644 index 0000000..0248f6e --- /dev/null +++ b/src/prm_vision/opencv_armor_detector/include/LBFGSB.h @@ -0,0 +1,284 @@ +// Copyright (C) 2020-2023 Yixuan Qiu +// Under MIT license + +#ifndef LBFGSPP_LBFGSB_H +#define LBFGSPP_LBFGSB_H + +#include // std::invalid_argument +#include +#include +#include "LBFGSpp/Param.h" +#include "LBFGSpp/BFGSMat.h" +#include "LBFGSpp/Cauchy.h" +#include "LBFGSpp/SubspaceMin.h" +#include "LBFGSpp/LineSearchMoreThuente.h" + +namespace LBFGSpp { + +/// +/// L-BFGS-B solver for box-constrained numerical optimization +/// +template class LineSearch = LineSearchMoreThuente> +class LBFGSBSolver +{ +private: + using Vector = Eigen::Matrix; + using Matrix = Eigen::Matrix; + using MapVec = Eigen::Map; + using IndexSet = std::vector; + + const LBFGSBParam& m_param; // Parameters to control the LBFGS algorithm + BFGSMat m_bfgs; // Approximation to the Hessian matrix + Vector m_fx; // History of the objective function values + Vector m_xp; // Old x + Vector m_grad; // New gradient + Scalar m_projgnorm; // Projected gradient norm + Vector m_gradp; // Old gradient + Vector m_drt; // Moving direction + + // Reset internal variables + // n: dimension of the vector to be optimized + inline void reset(int n) + { + const int m = m_param.m; + m_bfgs.reset(n, m); + m_xp.resize(n); + m_grad.resize(n); + m_gradp.resize(n); + m_drt.resize(n); + if (m_param.past > 0) + m_fx.resize(m_param.past); + } + + // Project the vector x to the bound constraint set + static void force_bounds(Vector& x, const Vector& lb, const Vector& ub) + { + x.noalias() = x.cwiseMax(lb).cwiseMin(ub); + } + + // Norm of the projected gradient + // ||P(x-g, l, u) - x||_inf + static Scalar proj_grad_norm(const Vector& x, const Vector& g, const Vector& lb, const Vector& ub) + { + return ((x - g).cwiseMax(lb).cwiseMin(ub) - x).cwiseAbs().maxCoeff(); + } + + // The maximum step size alpha such that x0 + alpha * d stays within the bounds + static Scalar max_step_size(const Vector& x0, const Vector& drt, const Vector& lb, const Vector& ub) + { + const int n = x0.size(); + Scalar step = std::numeric_limits::infinity(); + + for (int i = 0; i < n; i++) + { + if (drt[i] > Scalar(0)) + { + step = std::min(step, (ub[i] - x0[i]) / drt[i]); + } + else if (drt[i] < Scalar(0)) + { + step = std::min(step, (lb[i] - x0[i]) / drt[i]); + } + } + + return step; + } + +public: + /// + /// Constructor for the L-BFGS-B solver. + /// + /// \param param An object of \ref LBFGSParam to store parameters for the + /// algorithm + /// + LBFGSBSolver(const LBFGSBParam& param) : + m_param(param) + { + m_param.check_param(); + } + + /// + /// Minimizing a multivariate function subject to box constraints, using the L-BFGS-B algorithm. + /// Exceptions will be thrown if error occurs. + /// + /// \param f A function object such that `f(x, grad)` returns the + /// objective function value at `x`, and overwrites `grad` with + /// the gradient. + /// \param x In: An initial guess of the optimal point. Out: The best point + /// found. + /// \param fx Out: The objective function value at `x`. + /// \param lb Lower bounds for `x`. + /// \param ub Upper bounds for `x`. + /// + /// \return Number of iterations used. + /// + template + inline int minimize(Foo& f, Vector& x, Scalar& fx, const Vector& lb, const Vector& ub) + { + using std::abs; + + // Dimension of the vector + const int n = x.size(); + if (lb.size() != n || ub.size() != n) + throw std::invalid_argument("'lb' and 'ub' must have the same size as 'x'"); + + // Check whether the initial vector is within the bounds + // If not, project to the feasible set + force_bounds(x, lb, ub); + + // Initialization + reset(n); + + // The length of lag for objective function value to test convergence + const int fpast = m_param.past; + + // Evaluate function and compute gradient + fx = f(x, m_grad); + m_projgnorm = proj_grad_norm(x, m_grad, lb, ub); + if (fpast > 0) + m_fx[0] = fx; + + // std::cout << "x0 = " << x.transpose() << std::endl; + // std::cout << "f(x0) = " << fx << ", ||proj_grad|| = " << m_projgnorm << std::endl << std::endl; + + // Early exit if the initial x is already a minimizer + if (m_projgnorm <= m_param.epsilon || m_projgnorm <= m_param.epsilon_rel * x.norm()) + { + return 1; + } + + // Compute generalized Cauchy point + Vector xcp(n), vecc; + IndexSet newact_set, fv_set; + Cauchy::get_cauchy_point(m_bfgs, x, m_grad, lb, ub, xcp, vecc, newact_set, fv_set); + + /* Vector gcp(n); + Scalar fcp = f(xcp, gcp); + Scalar projgcpnorm = proj_grad_norm(xcp, gcp, lb, ub); + std::cout << "xcp = " << xcp.transpose() << std::endl; + std::cout << "f(xcp) = " << fcp << ", ||proj_grad|| = " << projgcpnorm << std::endl << std::endl; */ + + // Initial direction + m_drt.noalias() = xcp - x; + m_drt.normalize(); + // Tolerance for s'y >= eps * (y'y) + constexpr Scalar eps = std::numeric_limits::epsilon(); + // s and y vectors + Vector vecs(n), vecy(n); + // Number of iterations used + int k = 1; + for (;;) + { + // Save the curent x and gradient + m_xp.noalias() = x; + m_gradp.noalias() = m_grad; + Scalar dg = m_grad.dot(m_drt); + + // Maximum step size to make x feasible + Scalar step_max = max_step_size(x, m_drt, lb, ub); + + // In some cases, the direction returned by the subspace minimization procedure + // in the previous iteration is pathological, leading to issues such as + // step_max~=0 and dg>=0. If this happens, we use xcp-x as the search direction, + // and reset the BFGS matrix. This is because xsm (the subspace minimizer) + // heavily depends on the BFGS matrix. If xsm is corrupted, then we may suspect + // there is something wrong in the BFGS matrix, and it is safer to reset the matrix. + // In contrast, xcp is obtained from a line search, which tends to be more robust + if (dg >= Scalar(0) || step_max <= m_param.min_step) + { + // Reset search direction + m_drt.noalias() = xcp - x; + // Reset BFGS matrix + m_bfgs.reset(n, m_param.m); + // Recompute dg and step_max + dg = m_grad.dot(m_drt); + step_max = max_step_size(x, m_drt, lb, ub); + } + + // Line search to update x, fx and gradient + step_max = std::min(m_param.max_step, step_max); + Scalar step = Scalar(1); + step = std::min(step, step_max); + LineSearch::LineSearch(f, m_param, m_xp, m_drt, step_max, step, fx, m_grad, dg, x); + + // New projected gradient norm + m_projgnorm = proj_grad_norm(x, m_grad, lb, ub); + + /* std::cout << "** Iteration " << k << std::endl; + std::cout << " x = " << x.transpose() << std::endl; + std::cout << " f(x) = " << fx << ", ||proj_grad|| = " << m_projgnorm << std::endl << std::endl; */ + + // Convergence test -- gradient + if (m_projgnorm <= m_param.epsilon || m_projgnorm <= m_param.epsilon_rel * x.norm()) + { + return k; + } + // Convergence test -- objective function value + if (fpast > 0) + { + const Scalar fxd = m_fx[k % fpast]; + if (k >= fpast && abs(fxd - fx) <= m_param.delta * std::max(std::max(abs(fx), abs(fxd)), Scalar(1))) + return k; + + m_fx[k % fpast] = fx; + } + // Maximum number of iterations + if (m_param.max_iterations != 0 && k >= m_param.max_iterations) + { + return k; + } + + // Update s and y + // s_{k+1} = x_{k+1} - x_k + // y_{k+1} = g_{k+1} - g_k + vecs.noalias() = x - m_xp; + vecy.noalias() = m_grad - m_gradp; + if (vecs.dot(vecy) > eps * vecy.squaredNorm()) + m_bfgs.add_correction(vecs, vecy); + + force_bounds(x, lb, ub); + Cauchy::get_cauchy_point(m_bfgs, x, m_grad, lb, ub, xcp, vecc, newact_set, fv_set); + + /*Vector gcp(n); + Scalar fcp = f(xcp, gcp); + Scalar projgcpnorm = proj_grad_norm(xcp, gcp, lb, ub); + std::cout << "xcp = " << xcp.transpose() << std::endl; + std::cout << "f(xcp) = " << fcp << ", ||proj_grad|| = " << projgcpnorm << std::endl << std::endl;*/ + + SubspaceMin::subspace_minimize(m_bfgs, x, xcp, m_grad, lb, ub, + vecc, newact_set, fv_set, m_param.max_submin, m_drt); + + /*Vector gsm(n); + Scalar fsm = f(x + m_drt, gsm); + Scalar projgsmnorm = proj_grad_norm(x + m_drt, gsm, lb, ub); + std::cout << "xsm = " << (x + m_drt).transpose() << std::endl; + std::cout << "f(xsm) = " << fsm << ", ||proj_grad|| = " << projgsmnorm << std::endl << std::endl;*/ + + k++; + } + + return k; + } + + /// + /// Returning the gradient vector on the last iterate. + /// Typically used to debug and test convergence. + /// Should only be called after the `minimize()` function. + /// + /// \return A const reference to the gradient vector. + /// + const Vector& final_grad() const { return m_grad; } + + /// + /// Returning the infinity norm of the final projected gradient. + /// The projected gradient is defined as \f$P(x-g,l,u)-x\f$, where \f$P(v,l,u)\f$ stands for + /// the projection of a vector \f$v\f$ onto the box specified by the lower bound vector \f$l\f$ and + /// upper bound vector \f$u\f$. + /// + Scalar final_grad_norm() const { return m_projgnorm; } +}; + +} // namespace LBFGSpp + +#endif // LBFGSPP_LBFGSB_H diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/BFGSMat.h b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/BFGSMat.h new file mode 100644 index 0000000..01185e1 --- /dev/null +++ b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/BFGSMat.h @@ -0,0 +1,496 @@ +// Copyright (C) 2020-2023 Yixuan Qiu +// Under MIT license + +#ifndef LBFGSPP_BFGS_MAT_H +#define LBFGSPP_BFGS_MAT_H + +#include +#include +#include "BKLDLT.h" + +/// \cond + +namespace LBFGSpp { + +// +// An *implicit* representation of the BFGS approximation to the Hessian matrix B +// +// B = theta * I - W * M * W' +// H = inv(B) +// +// Reference: +// [1] D. C. Liu and J. Nocedal (1989). On the limited memory BFGS method for large scale optimization. +// [2] R. H. Byrd, P. Lu, and J. Nocedal (1995). A limited memory algorithm for bound constrained optimization. +// +template +class BFGSMat +{ +private: + using Vector = Eigen::Matrix; + using Matrix = Eigen::Matrix; + using RefConstVec = Eigen::Ref; + using IndexSet = std::vector; + + int m_m; // Maximum number of correction vectors + Scalar m_theta; // theta * I is the initial approximation to the Hessian matrix + Matrix m_s; // History of the s vectors + Matrix m_y; // History of the y vectors + Vector m_ys; // History of the s'y values + Vector m_alpha; // Temporary values used in computing H * v + int m_ncorr; // Number of correction vectors in the history, m_ncorr <= m + int m_ptr; // A Pointer to locate the most recent history, 1 <= m_ptr <= m + // Details: s and y vectors are stored in cyclic order. + // For example, if the current s-vector is stored in m_s[, m-1], + // then in the next iteration m_s[, 0] will be overwritten. + // m_s[, m_ptr-1] points to the most recent history, + // and m_s[, m_ptr % m] points to the most distant one. + + //========== The following members are only used in L-BFGS-B algorithm ==========// + Matrix m_permMinv; // Permutated M inverse + BKLDLT m_permMsolver; // Represents the permutated M matrix + +public: + // Constructor + BFGSMat() {} + + // Reset internal variables + // n: dimension of the vector to be optimized + // m: maximum number of corrections to approximate the Hessian matrix + inline void reset(int n, int m) + { + m_m = m; + m_theta = Scalar(1); + m_s.resize(n, m); + m_y.resize(n, m); + m_ys.resize(m); + m_alpha.resize(m); + m_ncorr = 0; + m_ptr = m; // This makes sure that m_ptr % m == 0 in the first step + + if (LBFGSB) + { + m_permMinv.resize(2 * m, 2 * m); + m_permMinv.setZero(); + m_permMinv.diagonal().setOnes(); + } + } + + // Add correction vectors to the BFGS matrix + inline void add_correction(const RefConstVec& s, const RefConstVec& y) + { + const int loc = m_ptr % m_m; + + m_s.col(loc).noalias() = s; + m_y.col(loc).noalias() = y; + + // ys = y's = 1/rho + const Scalar ys = m_s.col(loc).dot(m_y.col(loc)); + m_ys[loc] = ys; + + m_theta = m_y.col(loc).squaredNorm() / ys; + + if (m_ncorr < m_m) + m_ncorr++; + + m_ptr = loc + 1; + + if (LBFGSB) + { + // Minv = [-D L'] + // [ L theta*S'S] + + // Copy -D + // Let S=[s[0], ..., s[m-1]], Y=[y[0], ..., y[m-1]] + // D = [s[0]'y[0], ..., s[m-1]'y[m-1]] + m_permMinv(loc, loc) = -ys; + + // Update S'S + // We only store S'S in Minv, and multiply theta when LU decomposition is performed + Vector Ss = m_s.leftCols(m_ncorr).transpose() * m_s.col(loc); + m_permMinv.block(m_m + loc, m_m, 1, m_ncorr).noalias() = Ss.transpose(); + m_permMinv.block(m_m, m_m + loc, m_ncorr, 1).noalias() = Ss; + + // Compute L + // L = [ 0 ] + // [ s[1]'y[0] 0 ] + // [ s[2]'y[0] s[2]'y[1] ] + // ... + // [s[m-1]'y[0] ... ... ... ... ... s[m-1]'y[m-2] 0] + // + // L_next = [ 0 ] + // [s[2]'y[1] 0 ] + // [s[3]'y[1] s[3]'y[2] ] + // ... + // [s[m]'y[1] ... ... ... ... ... s[m]'y[m-1] 0] + const int len = m_ncorr - 1; + // First zero out the column of oldest y + if (m_ncorr >= m_m) + m_permMinv.block(m_m, loc, m_m, 1).setZero(); + // Compute the row associated with new s + // The current row is loc + // End with column (loc + m - 1) % m + // Length is len + int yloc = (loc + m_m - 1) % m_m; + for (int i = 0; i < len; i++) + { + m_permMinv(m_m + loc, yloc) = m_s.col(loc).dot(m_y.col(yloc)); + yloc = (yloc + m_m - 1) % m_m; + } + + // Matrix LDLT factorization + m_permMinv.block(m_m, m_m, m_m, m_m) *= m_theta; + m_permMsolver.compute(m_permMinv); + m_permMinv.block(m_m, m_m, m_m, m_m) /= m_theta; + } + } + + // Recursive formula to compute a * H * v, where a is a scalar, and v is [n x 1] + // H0 = (1/theta) * I is the initial approximation to H + // Algorithm 7.4 of Nocedal, J., & Wright, S. (2006). Numerical optimization. + inline void apply_Hv(const Vector& v, const Scalar& a, Vector& res) + { + res.resize(v.size()); + + // L-BFGS two-loop recursion + + // Loop 1 + res.noalias() = a * v; + int j = m_ptr % m_m; + for (int i = 0; i < m_ncorr; i++) + { + j = (j + m_m - 1) % m_m; + m_alpha[j] = m_s.col(j).dot(res) / m_ys[j]; + res.noalias() -= m_alpha[j] * m_y.col(j); + } + + // Apply initial H0 + res /= m_theta; + + // Loop 2 + for (int i = 0; i < m_ncorr; i++) + { + const Scalar beta = m_y.col(j).dot(res) / m_ys[j]; + res.noalias() += (m_alpha[j] - beta) * m_s.col(j); + j = (j + 1) % m_m; + } + } + + //========== The following functions are only used in L-BFGS-B algorithm ==========// + + // Return the value of theta + inline Scalar theta() const { return m_theta; } + + // Return current number of correction vectors + inline int num_corrections() const { return m_ncorr; } + + // W = [Y, theta * S] + // W [n x (2*ncorr)], v [n x 1], res [(2*ncorr) x 1] + // res preserves the ordering of Y and S columns + inline void apply_Wtv(const Vector& v, Vector& res) const + { + res.resize(2 * m_ncorr); + res.head(m_ncorr).noalias() = m_y.leftCols(m_ncorr).transpose() * v; + res.tail(m_ncorr).noalias() = m_theta * m_s.leftCols(m_ncorr).transpose() * v; + } + + // The b-th row of the W matrix + // Preserves the ordering of Y and S columns + // Return as a column vector + inline Vector Wb(int b) const + { + Vector res(2 * m_ncorr); + for (int j = 0; j < m_ncorr; j++) + { + res[j] = m_y(b, j); + res[m_ncorr + j] = m_s(b, j); + } + res.tail(m_ncorr) *= m_theta; + return res; + } + + // Extract rows of W + inline Matrix Wb(const IndexSet& b) const + { + const int nb = b.size(); + const int* bptr = b.data(); + Matrix res(nb, 2 * m_ncorr); + + for (int j = 0; j < m_ncorr; j++) + { + const Scalar* Yptr = &m_y(0, j); + const Scalar* Sptr = &m_s(0, j); + Scalar* resYptr = res.data() + j * nb; + Scalar* resSptr = resYptr + m_ncorr * nb; + for (int i = 0; i < nb; i++) + { + const int row = bptr[i]; + resYptr[i] = Yptr[row]; + resSptr[i] = Sptr[row]; + } + } + return res; + } + + // M is [(2*ncorr) x (2*ncorr)], v is [(2*ncorr) x 1] + inline void apply_Mv(const Vector& v, Vector& res) const + { + res.resize(2 * m_ncorr); + if (m_ncorr < 1) + return; + + Vector vpadding = Vector::Zero(2 * m_m); + vpadding.head(m_ncorr).noalias() = v.head(m_ncorr); + vpadding.segment(m_m, m_ncorr).noalias() = v.tail(m_ncorr); + + // Solve linear equation + m_permMsolver.solve_inplace(vpadding); + + res.head(m_ncorr).noalias() = vpadding.head(m_ncorr); + res.tail(m_ncorr).noalias() = vpadding.segment(m_m, m_ncorr); + } + + // Compute W'Pv + // W [n x (2*ncorr)], v [nP x 1], res [(2*ncorr) x 1] + // res preserves the ordering of Y and S columns + // Returns false if the result is known to be zero + inline bool apply_WtPv(const IndexSet& P_set, const Vector& v, Vector& res, bool test_zero = false) const + { + const int* Pptr = P_set.data(); + const Scalar* vptr = v.data(); + int nP = P_set.size(); + + // Remove zeros in v to save computation + IndexSet P_reduced; + std::vector v_reduced; + if (test_zero) + { + P_reduced.reserve(nP); + for (int i = 0; i < nP; i++) + { + if (vptr[i] != Scalar(0)) + { + P_reduced.push_back(Pptr[i]); + v_reduced.push_back(vptr[i]); + } + } + Pptr = P_reduced.data(); + vptr = v_reduced.data(); + nP = P_reduced.size(); + } + + res.resize(2 * m_ncorr); + if (m_ncorr < 1 || nP < 1) + { + res.setZero(); + return false; + } + + for (int j = 0; j < m_ncorr; j++) + { + Scalar resy = Scalar(0), ress = Scalar(0); + const Scalar* yptr = &m_y(0, j); + const Scalar* sptr = &m_s(0, j); + for (int i = 0; i < nP; i++) + { + const int row = Pptr[i]; + resy += yptr[row] * vptr[i]; + ress += sptr[row] * vptr[i]; + } + res[j] = resy; + res[m_ncorr + j] = ress; + } + res.tail(m_ncorr) *= m_theta; + return true; + } + + // Compute s * P'WMv + // Assume that v[2*ncorr x 1] has the same ordering (permutation) as W and M + // Returns false if the result is known to be zero + inline bool apply_PtWMv(const IndexSet& P_set, const Vector& v, Vector& res, const Scalar& scale) const + { + const int nP = P_set.size(); + res.resize(nP); + res.setZero(); + if (m_ncorr < 1 || nP < 1) + return false; + + Vector Mv; + apply_Mv(v, Mv); + // WP * Mv + Mv.tail(m_ncorr) *= m_theta; + for (int j = 0; j < m_ncorr; j++) + { + const Scalar* yptr = &m_y(0, j); + const Scalar* sptr = &m_s(0, j); + const Scalar Mvy = Mv[j], Mvs = Mv[m_ncorr + j]; + for (int i = 0; i < nP; i++) + { + const int row = P_set[i]; + res[i] += Mvy * yptr[row] + Mvs * sptr[row]; + } + } + res *= scale; + return true; + } + // If the P'W matrix has been explicitly formed, do a direct matrix multiplication + inline bool apply_PtWMv(const Matrix& WP, const Vector& v, Vector& res, const Scalar& scale) const + { + const int nP = WP.rows(); + res.resize(nP); + if (m_ncorr < 1 || nP < 1) + { + res.setZero(); + return false; + } + + Vector Mv; + apply_Mv(v, Mv); + // WP * Mv + Mv.tail(m_ncorr) *= m_theta; + res.noalias() = scale * (WP * Mv); + return true; + } + + // Compute F'BAb = -(F'W)M(W'AA'd) + // W'd is known, and AA'+FF'=I, so W'AA'd = W'd - W'FF'd + // Usually d contains many zeros, so we fist compute number of nonzero elements in A set and F set, + // denoted as nnz_act and nnz_fv, respectively + // If nnz_act is smaller, compute W'AA'd = WA' (A'd) directly + // If nnz_fv is smaller, compute W'AA'd = W'd - WF' * (F'd) + inline void compute_FtBAb( + const Matrix& WF, const IndexSet& fv_set, const IndexSet& newact_set, const Vector& Wd, const Vector& drt, + Vector& res) const + { + const int nact = newact_set.size(); + const int nfree = WF.rows(); + res.resize(nfree); + if (m_ncorr < 1 || nact < 1 || nfree < 1) + { + res.setZero(); + return; + } + + // W'AA'd + Vector rhs(2 * m_ncorr); + if (nact <= nfree) + { + // Construct A'd + Vector Ad(nfree); + for (int i = 0; i < nact; i++) + Ad[i] = drt[newact_set[i]]; + apply_WtPv(newact_set, Ad, rhs); + } + else + { + // Construct F'd + Vector Fd(nfree); + for (int i = 0; i < nfree; i++) + Fd[i] = drt[fv_set[i]]; + // Compute W'AA'd = W'd - WF' * (F'd) + rhs.noalias() = WF.transpose() * Fd; + rhs.tail(m_ncorr) *= m_theta; + rhs.noalias() = Wd - rhs; + } + + apply_PtWMv(WF, rhs, res, Scalar(-1)); + } + + // Compute inv(P'BP) * v + // P represents an index set + // inv(P'BP) * v = v / theta + WP * inv(inv(M) - WP' * WP / theta) * WP' * v / theta^2 + // + // v is [nP x 1] + inline void solve_PtBP(const Matrix& WP, const Vector& v, Vector& res) const + { + const int nP = WP.rows(); + res.resize(nP); + if (m_ncorr < 1 || nP < 1) + { + res.noalias() = v / m_theta; + return; + } + + // Compute the matrix in the middle (only the lower triangular part is needed) + // Remember that W = [Y, theta * S], but we do not store theta in WP + Matrix mid(2 * m_ncorr, 2 * m_ncorr); + // [0:(ncorr - 1), 0:(ncorr - 1)] + for (int j = 0; j < m_ncorr; j++) + { + mid.col(j).segment(j, m_ncorr - j).noalias() = m_permMinv.col(j).segment(j, m_ncorr - j) - + WP.block(0, j, nP, m_ncorr - j).transpose() * WP.col(j) / m_theta; + } + // [ncorr:(2 * ncorr - 1), 0:(ncorr - 1)] + mid.block(m_ncorr, 0, m_ncorr, m_ncorr).noalias() = m_permMinv.block(m_m, 0, m_ncorr, m_ncorr) - + WP.rightCols(m_ncorr).transpose() * WP.leftCols(m_ncorr); + // [ncorr:(2 * ncorr - 1), ncorr:(2 * ncorr - 1)] + for (int j = 0; j < m_ncorr; j++) + { + mid.col(m_ncorr + j).segment(m_ncorr + j, m_ncorr - j).noalias() = m_theta * + (m_permMinv.col(m_m + j).segment(m_m + j, m_ncorr - j) - WP.rightCols(m_ncorr - j).transpose() * WP.col(m_ncorr + j)); + } + // Factorization + BKLDLT midsolver(mid); + // Compute the final result + Vector WPv = WP.transpose() * v; + WPv.tail(m_ncorr) *= m_theta; + midsolver.solve_inplace(WPv); + WPv.tail(m_ncorr) *= m_theta; + res.noalias() = v / m_theta + (WP * WPv) / (m_theta * m_theta); + } + + // Compute P'BQv, where P and Q are two mutually exclusive index selection operators + // P'BQv = -WP * M * WQ' * v + // Returns false if the result is known to be zero + inline bool apply_PtBQv(const Matrix& WP, const IndexSet& Q_set, const Vector& v, Vector& res, bool test_zero = false) const + { + const int nP = WP.rows(); + const int nQ = Q_set.size(); + res.resize(nP); + if (m_ncorr < 1 || nP < 1 || nQ < 1) + { + res.setZero(); + return false; + } + + Vector WQtv; + bool nonzero = apply_WtPv(Q_set, v, WQtv, test_zero); + if (!nonzero) + { + res.setZero(); + return false; + } + + Vector MWQtv; + apply_Mv(WQtv, MWQtv); + MWQtv.tail(m_ncorr) *= m_theta; + res.noalias() = -WP * MWQtv; + return true; + } + // If the Q'W matrix has been explicitly formed, do a direct matrix multiplication + inline bool apply_PtBQv(const Matrix& WP, const Matrix& WQ, const Vector& v, Vector& res) const + { + const int nP = WP.rows(); + const int nQ = WQ.rows(); + res.resize(nP); + if (m_ncorr < 1 || nP < 1 || nQ < 1) + { + res.setZero(); + return false; + } + + // Remember that W = [Y, theta * S], so we need to multiply theta to the second half + Vector WQtv = WQ.transpose() * v; + WQtv.tail(m_ncorr) *= m_theta; + Vector MWQtv; + apply_Mv(WQtv, MWQtv); + MWQtv.tail(m_ncorr) *= m_theta; + res.noalias() = -WP * MWQtv; + return true; + } +}; + +} // namespace LBFGSpp + +/// \endcond + +#endif // LBFGSPP_BFGS_MAT_H diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/BKLDLT.h b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/BKLDLT.h new file mode 100644 index 0000000..c48bce7 --- /dev/null +++ b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/BKLDLT.h @@ -0,0 +1,536 @@ +// Copyright (C) 2020-2023 Yixuan Qiu +// Under MIT license + +#ifndef LBFGSPP_BK_LDLT_H +#define LBFGSPP_BK_LDLT_H + +#include +#include +#include + +/// \cond + +namespace LBFGSpp { + +enum COMPUTATION_INFO +{ + SUCCESSFUL = 0, + NOT_COMPUTED, + NUMERICAL_ISSUE +}; + +// Bunch-Kaufman LDLT decomposition +// References: +// 1. Bunch, J. R., & Kaufman, L. (1977). Some stable methods for calculating inertia and solving symmetric linear systems. +// Mathematics of computation, 31(137), 163-179. +// 2. Golub, G. H., & Van Loan, C. F. (2012). Matrix computations (Vol. 3). JHU press. Section 4.4. +// 3. Bunch-Parlett diagonal pivoting +// 4. Ashcraft, C., Grimes, R. G., & Lewis, J. G. (1998). Accurate symmetric indefinite linear equation solvers. +// SIAM Journal on Matrix Analysis and Applications, 20(2), 513-561. +template +class BKLDLT +{ +private: + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using MapVec = Eigen::Map; + using MapConstVec = Eigen::Map; + + using IntVector = Eigen::Matrix; + using GenericVector = Eigen::Ref; + using GenericMatrix = Eigen::Ref; + using ConstGenericMatrix = const Eigen::Ref; + using ConstGenericVector = const Eigen::Ref; + + Index m_n; + Vector m_data; // storage for a lower-triangular matrix + std::vector m_colptr; // pointers to columns + IntVector m_perm; // [-2, -1, 3, 1, 4, 5]: 0 <-> 2, 1 <-> 1, 2 <-> 3, 3 <-> 1, 4 <-> 4, 5 <-> 5 + std::vector > m_permc; // compressed version of m_perm: [(0, 2), (2, 3), (3, 1)] + + bool m_computed; + int m_info; + + // Access to elements + // Pointer to the k-th column + Scalar* col_pointer(Index k) { return m_colptr[k]; } + // A[i, j] -> m_colptr[j][i - j], i >= j + Scalar& coeff(Index i, Index j) { return m_colptr[j][i - j]; } + const Scalar& coeff(Index i, Index j) const { return m_colptr[j][i - j]; } + // A[i, i] -> m_colptr[i][0] + Scalar& diag_coeff(Index i) { return m_colptr[i][0]; } + const Scalar& diag_coeff(Index i) const { return m_colptr[i][0]; } + + // Compute column pointers + void compute_pointer() + { + m_colptr.clear(); + m_colptr.reserve(m_n); + Scalar* head = m_data.data(); + + for (Index i = 0; i < m_n; i++) + { + m_colptr.push_back(head); + head += (m_n - i); + } + } + + // Copy mat - shift * I to m_data + void copy_data(ConstGenericMatrix& mat, int uplo, const Scalar& shift) + { + if (uplo == Eigen::Lower) + { + for (Index j = 0; j < m_n; j++) + { + const Scalar* begin = &mat.coeffRef(j, j); + const Index len = m_n - j; + std::copy(begin, begin + len, col_pointer(j)); + diag_coeff(j) -= shift; + } + } + else + { + Scalar* dest = m_data.data(); + for (Index i = 0; i < m_n; i++) + { + for (Index j = i; j < m_n; j++, dest++) + { + *dest = mat.coeff(i, j); + } + diag_coeff(i) -= shift; + } + } + } + + // Compute compressed permutations + void compress_permutation() + { + for (Index i = 0; i < m_n; i++) + { + // Recover the permutation action + const Index perm = (m_perm[i] >= 0) ? (m_perm[i]) : (-m_perm[i] - 1); + if (perm != i) + m_permc.push_back(std::make_pair(i, perm)); + } + } + + // Working on the A[k:end, k:end] submatrix + // Exchange k <-> r + // Assume r >= k + void pivoting_1x1(Index k, Index r) + { + // No permutation + if (k == r) + { + m_perm[k] = r; + return; + } + + // A[k, k] <-> A[r, r] + std::swap(diag_coeff(k), diag_coeff(r)); + + // A[(r+1):end, k] <-> A[(r+1):end, r] + std::swap_ranges(&coeff(r + 1, k), col_pointer(k + 1), &coeff(r + 1, r)); + + // A[(k+1):(r-1), k] <-> A[r, (k+1):(r-1)] + Scalar* src = &coeff(k + 1, k); + for (Index j = k + 1; j < r; j++, src++) + { + std::swap(*src, coeff(r, j)); + } + + m_perm[k] = r; + } + + // Working on the A[k:end, k:end] submatrix + // Exchange [k+1, k] <-> [r, p] + // Assume p >= k, r >= k+1 + void pivoting_2x2(Index k, Index r, Index p) + { + pivoting_1x1(k, p); + pivoting_1x1(k + 1, r); + + // A[k+1, k] <-> A[r, k] + std::swap(coeff(k + 1, k), coeff(r, k)); + + // Use negative signs to indicate a 2x2 block + // Also minus one to distinguish a negative zero from a positive zero + m_perm[k] = -m_perm[k] - 1; + m_perm[k + 1] = -m_perm[k + 1] - 1; + } + + // A[r1, c1:c2] <-> A[r2, c1:c2] + // Assume r2 >= r1 > c2 >= c1 + void interchange_rows(Index r1, Index r2, Index c1, Index c2) + { + if (r1 == r2) + return; + + for (Index j = c1; j <= c2; j++) + { + std::swap(coeff(r1, j), coeff(r2, j)); + } + } + + // lambda = |A[r, k]| = max{|A[k+1, k]|, ..., |A[end, k]|} + // Largest (in magnitude) off-diagonal element in the first column of the current reduced matrix + // r is the row index + // Assume k < end + Scalar find_lambda(Index k, Index& r) + { + using std::abs; + + const Scalar* head = col_pointer(k); // => A[k, k] + const Scalar* end = col_pointer(k + 1); + // Start with r=k+1, lambda=A[k+1, k] + r = k + 1; + Scalar lambda = abs(head[1]); + // Scan remaining elements + for (const Scalar* ptr = head + 2; ptr < end; ptr++) + { + const Scalar abs_elem = abs(*ptr); + if (lambda < abs_elem) + { + lambda = abs_elem; + r = k + (ptr - head); + } + } + + return lambda; + } + + // sigma = |A[p, r]| = max {|A[k, r]|, ..., |A[end, r]|} \ {A[r, r]} + // Largest (in magnitude) off-diagonal element in the r-th column of the current reduced matrix + // p is the row index + // Assume k < r < end + Scalar find_sigma(Index k, Index r, Index& p) + { + using std::abs; + + // First search A[r+1, r], ..., A[end, r], which has the same task as find_lambda() + // If r == end, we skip this search + Scalar sigma = Scalar(-1); + if (r < m_n - 1) + sigma = find_lambda(r, p); + + // Then search A[k, r], ..., A[r-1, r], which maps to A[r, k], ..., A[r, r-1] + for (Index j = k; j < r; j++) + { + const Scalar abs_elem = abs(coeff(r, j)); + if (sigma < abs_elem) + { + sigma = abs_elem; + p = j; + } + } + + return sigma; + } + + // Generate permutations and apply to A + // Return true if the resulting pivoting is 1x1, and false if 2x2 + bool permutate_mat(Index k, const Scalar& alpha) + { + using std::abs; + + Index r = k, p = k; + const Scalar lambda = find_lambda(k, r); + + // If lambda=0, no need to interchange + if (lambda > Scalar(0)) + { + const Scalar abs_akk = abs(diag_coeff(k)); + // If |A[k, k]| >= alpha * lambda, no need to interchange + if (abs_akk < alpha * lambda) + { + const Scalar sigma = find_sigma(k, r, p); + + // If sigma * |A[k, k]| >= alpha * lambda^2, no need to interchange + if (sigma * abs_akk < alpha * lambda * lambda) + { + if (abs_akk >= alpha * sigma) + { + // Permutation on A + pivoting_1x1(k, r); + + // Permutation on L + interchange_rows(k, r, 0, k - 1); + return true; + } + else + { + // There are two versions of permutation here + // 1. A[k+1, k] <-> A[r, k] + // 2. A[k+1, k] <-> A[r, p], where p >= k and r >= k+1 + // + // Version 1 and 2 are used by Ref[1] and Ref[2], respectively + + // Version 1 implementation + p = k; + + // Version 2 implementation + // [r, p] and [p, r] are symmetric, but we need to make sure + // p >= k and r >= k+1, so it is safe to always make r > p + // One exception is when min{r,p} == k+1, in which case we make + // r = k+1, so that only one permutation needs to be performed + /* const Index rp_min = std::min(r, p); + const Index rp_max = std::max(r, p); + if(rp_min == k + 1) + { + r = rp_min; p = rp_max; + } else { + r = rp_max; p = rp_min; + } */ + + // Right now we use Version 1 since it reduces the overhead of interchange + + // Permutation on A + pivoting_2x2(k, r, p); + // Permutation on L + interchange_rows(k, p, 0, k - 1); + interchange_rows(k + 1, r, 0, k - 1); + return false; + } + } + } + } + + return true; + } + + // E = [e11, e12] + // [e21, e22] + // Overwrite E with inv(E) + void inverse_inplace_2x2(Scalar& e11, Scalar& e21, Scalar& e22) const + { + // inv(E) = [d11, d12], d11 = e22/delta, d21 = -e21/delta, d22 = e11/delta + // [d21, d22] + const Scalar delta = e11 * e22 - e21 * e21; + std::swap(e11, e22); + e11 /= delta; + e22 /= delta; + e21 = -e21 / delta; + } + + // Return value is the status, SUCCESSFUL/NUMERICAL_ISSUE + int gaussian_elimination_1x1(Index k) + { + // D = 1 / A[k, k] + const Scalar akk = diag_coeff(k); + // Return NUMERICAL_ISSUE if not invertible + if (akk == Scalar(0)) + return NUMERICAL_ISSUE; + + diag_coeff(k) = Scalar(1) / akk; + + // B -= l * l' / A[k, k], B := A[(k+1):end, (k+1):end], l := L[(k+1):end, k] + Scalar* lptr = col_pointer(k) + 1; + const Index ldim = m_n - k - 1; + MapVec l(lptr, ldim); + for (Index j = 0; j < ldim; j++) + { + MapVec(col_pointer(j + k + 1), ldim - j).noalias() -= (lptr[j] / akk) * l.tail(ldim - j); + } + + // l /= A[k, k] + l /= akk; + + return SUCCESSFUL; + } + + // Return value is the status, SUCCESSFUL/NUMERICAL_ISSUE + int gaussian_elimination_2x2(Index k) + { + // D = inv(E) + Scalar& e11 = diag_coeff(k); + Scalar& e21 = coeff(k + 1, k); + Scalar& e22 = diag_coeff(k + 1); + // Return NUMERICAL_ISSUE if not invertible + if (e11 * e22 - e21 * e21 == Scalar(0)) + return NUMERICAL_ISSUE; + + inverse_inplace_2x2(e11, e21, e22); + + // X = l * inv(E), l := L[(k+2):end, k:(k+1)] + Scalar* l1ptr = &coeff(k + 2, k); + Scalar* l2ptr = &coeff(k + 2, k + 1); + const Index ldim = m_n - k - 2; + MapVec l1(l1ptr, ldim), l2(l2ptr, ldim); + + Eigen::Matrix X(ldim, 2); + X.col(0).noalias() = l1 * e11 + l2 * e21; + X.col(1).noalias() = l1 * e21 + l2 * e22; + + // B -= l * inv(E) * l' = X * l', B = A[(k+2):end, (k+2):end] + for (Index j = 0; j < ldim; j++) + { + MapVec(col_pointer(j + k + 2), ldim - j).noalias() -= (X.col(0).tail(ldim - j) * l1ptr[j] + X.col(1).tail(ldim - j) * l2ptr[j]); + } + + // l = X + l1.noalias() = X.col(0); + l2.noalias() = X.col(1); + + return SUCCESSFUL; + } + +public: + BKLDLT() : + m_n(0), m_computed(false), m_info(NOT_COMPUTED) + {} + + // Factorize mat - shift * I + BKLDLT(ConstGenericMatrix& mat, int uplo = Eigen::Lower, const Scalar& shift = Scalar(0)) : + m_n(mat.rows()), m_computed(false), m_info(NOT_COMPUTED) + { + compute(mat, uplo, shift); + } + + void compute(ConstGenericMatrix& mat, int uplo = Eigen::Lower, const Scalar& shift = Scalar(0)) + { + using std::abs; + + m_n = mat.rows(); + if (m_n != mat.cols()) + throw std::invalid_argument("BKLDLT: matrix must be square"); + + m_perm.setLinSpaced(m_n, 0, m_n - 1); + m_permc.clear(); + + // Copy data + m_data.resize((m_n * (m_n + 1)) / 2); + compute_pointer(); + copy_data(mat, uplo, shift); + + const Scalar alpha = (1.0 + std::sqrt(17.0)) / 8.0; + Index k = 0; + for (k = 0; k < m_n - 1; k++) + { + // 1. Interchange rows and columns of A, and save the result to m_perm + bool is_1x1 = permutate_mat(k, alpha); + + // 2. Gaussian elimination + if (is_1x1) + { + m_info = gaussian_elimination_1x1(k); + } + else + { + m_info = gaussian_elimination_2x2(k); + k++; + } + + // 3. Check status + if (m_info != SUCCESSFUL) + break; + } + // Invert the last 1x1 block if it exists + if (k == m_n - 1) + { + const Scalar akk = diag_coeff(k); + if (akk == Scalar(0)) + m_info = NUMERICAL_ISSUE; + + diag_coeff(k) = Scalar(1) / diag_coeff(k); + } + + compress_permutation(); + + m_computed = true; + } + + // Solve Ax=b + void solve_inplace(GenericVector b) const + { + if (!m_computed) + throw std::logic_error("BKLDLT: need to call compute() first"); + + // PAP' = LDL' + // 1. b -> Pb + Scalar* x = b.data(); + MapVec res(x, m_n); + Index npermc = m_permc.size(); + for (Index i = 0; i < npermc; i++) + { + std::swap(x[m_permc[i].first], x[m_permc[i].second]); + } + + // 2. Lz = Pb + // If m_perm[end] < 0, then end with m_n - 3, otherwise end with m_n - 2 + const Index end = (m_perm[m_n - 1] < 0) ? (m_n - 3) : (m_n - 2); + for (Index i = 0; i <= end; i++) + { + const Index b1size = m_n - i - 1; + const Index b2size = b1size - 1; + if (m_perm[i] >= 0) + { + MapConstVec l(&coeff(i + 1, i), b1size); + res.segment(i + 1, b1size).noalias() -= l * x[i]; + } + else + { + MapConstVec l1(&coeff(i + 2, i), b2size); + MapConstVec l2(&coeff(i + 2, i + 1), b2size); + res.segment(i + 2, b2size).noalias() -= (l1 * x[i] + l2 * x[i + 1]); + i++; + } + } + + // 3. Dw = z + for (Index i = 0; i < m_n; i++) + { + const Scalar e11 = diag_coeff(i); + if (m_perm[i] >= 0) + { + x[i] *= e11; + } + else + { + const Scalar e21 = coeff(i + 1, i), e22 = diag_coeff(i + 1); + const Scalar wi = x[i] * e11 + x[i + 1] * e21; + x[i + 1] = x[i] * e21 + x[i + 1] * e22; + x[i] = wi; + i++; + } + } + + // 4. L'y = w + // If m_perm[end] < 0, then start with m_n - 3, otherwise start with m_n - 2 + Index i = (m_perm[m_n - 1] < 0) ? (m_n - 3) : (m_n - 2); + for (; i >= 0; i--) + { + const Index ldim = m_n - i - 1; + MapConstVec l(&coeff(i + 1, i), ldim); + x[i] -= res.segment(i + 1, ldim).dot(l); + + if (m_perm[i] < 0) + { + MapConstVec l2(&coeff(i + 1, i - 1), ldim); + x[i - 1] -= res.segment(i + 1, ldim).dot(l2); + i--; + } + } + + // 5. x = P'y + for (i = npermc - 1; i >= 0; i--) + { + std::swap(x[m_permc[i].first], x[m_permc[i].second]); + } + } + + Vector solve(ConstGenericVector& b) const + { + Vector res = b; + solve_inplace(res); + return res; + } + + int info() const { return m_info; } +}; + +} // namespace LBFGSpp + +/// \endcond + +#endif // LBFGSPP_BK_LDLT_H diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/Cauchy.h b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/Cauchy.h new file mode 100644 index 0000000..58bf2bf --- /dev/null +++ b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/Cauchy.h @@ -0,0 +1,291 @@ +// Copyright (C) 2020-2023 Yixuan Qiu +// Under MIT license + +#ifndef LBFGSPP_CAUCHY_H +#define LBFGSPP_CAUCHY_H + +#include +#include +#include "BFGSMat.h" + +/// \cond + +namespace LBFGSpp { + +// +// Class to compute the generalized Cauchy point (GCP) for the L-BFGS-B algorithm, +// mainly for internal use. +// +// The target of the GCP procedure is to find a step size t such that +// x(t) = x0 - t * g is a local minimum of the quadratic function m(x), +// where m(x) is a local approximation to the objective function. +// +// First determine a sequence of break points t0=0, t1, t2, ..., tn. +// On each interval [t[i-1], t[i]], x is changing linearly. +// After passing a break point, one or more coordinates of x will be fixed at the bounds. +// We search the first local minimum of m(x) by examining the intervals [t[i-1], t[i]] sequentially. +// +// Reference: +// [1] R. H. Byrd, P. Lu, and J. Nocedal (1995). A limited memory algorithm for bound constrained optimization. +// +template +class ArgSort +{ +private: + using Vector = Eigen::Matrix; + using IndexSet = std::vector; + + const Scalar* values; + +public: + ArgSort(const Vector& value_vec) : + values(value_vec.data()) + {} + + inline bool operator()(int key1, int key2) { return values[key1] < values[key2]; } + inline void sort_key(IndexSet& key_vec) const + { + std::sort(key_vec.begin(), key_vec.end(), *this); + } +}; + +template +class Cauchy +{ +private: + typedef Eigen::Matrix Vector; + typedef Eigen::Matrix IntVector; + typedef Eigen::Matrix Matrix; + typedef std::vector IndexSet; + + // Find the smallest index i such that brk[ord[i]] > t, assuming brk[ord] is already sorted. + // If the return value equals n, then all values are <= t. + static int search_greater(const Vector& brk, const IndexSet& ord, const Scalar& t, int start = 0) + { + const int nord = ord.size(); + int i; + for (i = start; i < nord; i++) + { + if (brk[ord[i]] > t) + break; + } + + return i; + } + +public: + // bfgs: An object that represents the BFGS approximation matrix. + // x0: Current parameter vector. + // g: Gradient at x0. + // lb: Lower bounds for x. + // ub: Upper bounds for x. + // xcp: The output generalized Cauchy point. + // vecc: c = W'(xcp - x0), used in the subspace minimization routine. + // newact_set: Coordinates that newly become active during the GCP procedure. + // fv_set: Free variable set. + static void get_cauchy_point( + const BFGSMat& bfgs, const Vector& x0, const Vector& g, const Vector& lb, const Vector& ub, + Vector& xcp, Vector& vecc, IndexSet& newact_set, IndexSet& fv_set) + { + // std::cout << "========================= Entering GCP search =========================\n\n"; + + // Initialization + const int n = x0.size(); + xcp.resize(n); + xcp.noalias() = x0; + vecc.resize(2 * bfgs.num_corrections()); + vecc.setZero(); + newact_set.clear(); + newact_set.reserve(n); + fv_set.clear(); + fv_set.reserve(n); + + // Construct break points + Vector brk(n), vecd(n); + // If brk[i] == 0, i belongs to active set + // If brk[i] == Inf, i belongs to free variable set + // Others are currently undecided + IndexSet ord; + ord.reserve(n); + const Scalar inf = std::numeric_limits::infinity(); + for (int i = 0; i < n; i++) + { + if (lb[i] == ub[i]) + brk[i] = Scalar(0); + else if (g[i] < Scalar(0)) + brk[i] = (x0[i] - ub[i]) / g[i]; + else if (g[i] > Scalar(0)) + brk[i] = (x0[i] - lb[i]) / g[i]; + else + brk[i] = inf; + + const bool iszero = (brk[i] == Scalar(0)); + vecd[i] = iszero ? Scalar(0) : -g[i]; + + if (brk[i] == inf) + fv_set.push_back(i); + else if (!iszero) + ord.push_back(i); + } + + // Sort indices of break points + ArgSort sorting(brk); + sorting.sort_key(ord); + + // Break points `brko := brk[ord]` are in increasing order + // `ord` contains the coordinates that define the corresponding break points + // brk[i] == 0 <=> The i-th coordinate is on the boundary + const int nord = ord.size(); + const int nfree = fv_set.size(); + if ((nfree < 1) && (nord < 1)) + { + /* std::cout << "** All coordinates at boundary **\n"; + std::cout << "\n========================= Leaving GCP search =========================\n\n"; */ + return; + } + + // First interval: [il=0, iu=brk[ord[0]]] + // In case ord is empty, we take iu=Inf + + // p = W'd, c = 0 + Vector vecp; + bfgs.apply_Wtv(vecd, vecp); + // f' = -d'd + Scalar fp = -vecd.squaredNorm(); + // f'' = -theta * f' - p'Mp + Vector cache; + bfgs.apply_Mv(vecp, cache); // cache = Mp + Scalar fpp = -bfgs.theta() * fp - vecp.dot(cache); + + // Theoretical step size to move + Scalar deltatmin = -fp / fpp; + + // Limit on the current interval + Scalar il = Scalar(0); + // We have excluded the case that max(brk) <= 0 + int b = 0; + Scalar iu = (nord < 1) ? inf : brk[ord[b]]; + Scalar deltat = iu - il; + + /* int iter = 0; + std::cout << "** Iter " << iter << " **\n"; + std::cout << " fp = " << fp << ", fpp = " << fpp << ", deltatmin = " << deltatmin << std::endl; + std::cout << " il = " << il << ", iu = " << iu << ", deltat = " << deltat << std::endl; */ + + // If deltatmin >= deltat, we need to do the following things: + // 1. Update vecc + // 2. Since we are going to cross iu, the coordinates that define iu become active + // 3. Update some quantities on these new active coordinates (xcp, vecd, vecp) + // 4. Move to the next interval and compute the new deltatmin + bool crossed_all = false; + const int ncorr = bfgs.num_corrections(); + Vector wact(2 * ncorr); + while (deltatmin >= deltat) + { + // Step 1 + vecc.noalias() += deltat * vecp; + + // Step 2 + // First check how many coordinates will be active when we cross the previous iu + // b is the smallest number such that brko[b] == iu + // Let bp be the largest number such that brko[bp] == iu + // Then coordinates ord[b] to ord[bp] will be active + const int act_begin = b; + const int act_end = search_greater(brk, ord, iu, b) - 1; + + // If nfree == 0 and act_end == nord-1, then we have crossed all coordinates + // We only need to update xcp from ord[b] to ord[bp], and then exit + if ((nfree == 0) && (act_end == nord - 1)) + { + // std::cout << "** [ "; + for (int i = act_begin; i <= act_end; i++) + { + const int act = ord[i]; + xcp[act] = (vecd[act] > Scalar(0)) ? ub[act] : lb[act]; + newact_set.push_back(act); + // std::cout << act + 1 << " "; + } + // std::cout << "] become active **\n\n"; + // std::cout << "** All break points visited **\n\n"; + + crossed_all = true; + break; + } + + // Step 3 + // Update xcp and d on active coordinates + // std::cout << "** [ "; + fp += deltat * fpp; + for (int i = act_begin; i <= act_end; i++) + { + const int act = ord[i]; + xcp[act] = (vecd[act] > Scalar(0)) ? ub[act] : lb[act]; + // z = xcp - x0 + const Scalar zact = xcp[act] - x0[act]; + const Scalar gact = g[act]; + const Scalar ggact = gact * gact; + wact.noalias() = bfgs.Wb(act); + bfgs.apply_Mv(wact, cache); // cache = Mw + fp += ggact + bfgs.theta() * gact * zact - gact * cache.dot(vecc); + fpp -= (bfgs.theta() * ggact + 2 * gact * cache.dot(vecp) + ggact * cache.dot(wact)); + vecp.noalias() += gact * wact; + vecd[act] = Scalar(0); + newact_set.push_back(act); + // std::cout << act + 1 << " "; + } + // std::cout << "] become active **\n\n"; + + // Step 4 + // Theoretical step size to move + deltatmin = -fp / fpp; + // Update interval bound + il = iu; + b = act_end + 1; + // If we have visited all finite-valued break points, and have not exited earlier, + // then the next iu will be infinity. Simply exit the loop now + if (b >= nord) + break; + iu = brk[ord[b]]; + // Width of the current interval + deltat = iu - il; + + /* iter++; + std::cout << "** Iter " << iter << " **\n"; + std::cout << " fp = " << fp << ", fpp = " << fpp << ", deltatmin = " << deltatmin << std::endl; + std::cout << " il = " << il << ", iu = " << iu << ", deltat = " << deltat << std::endl; */ + } + + // In some rare cases fpp is numerically zero, making deltatmin equal to Inf + // If this happens, force fpp to be the machine precision + const Scalar eps = std::numeric_limits::epsilon(); + if (fpp < eps) + deltatmin = -fp / eps; + + // Last step + if (!crossed_all) + { + deltatmin = std::max(deltatmin, Scalar(0)); + vecc.noalias() += deltatmin * vecp; + const Scalar tfinal = il + deltatmin; + // Update xcp on free variable coordinates + for (int i = 0; i < nfree; i++) + { + const int coord = fv_set[i]; + xcp[coord] = x0[coord] + tfinal * vecd[coord]; + } + for (int i = b; i < nord; i++) + { + const int coord = ord[i]; + xcp[coord] = x0[coord] + tfinal * vecd[coord]; + fv_set.push_back(coord); + } + } + // std::cout << "\n========================= Leaving GCP search =========================\n\n"; + } +}; + +} // namespace LBFGSpp + +/// \endcond + +#endif // LBFGSPP_CAUCHY_H diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchBacktracking.h b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchBacktracking.h new file mode 100644 index 0000000..402cef6 --- /dev/null +++ b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchBacktracking.h @@ -0,0 +1,125 @@ +// Copyright (C) 2016-2023 Yixuan Qiu +// Under MIT license + +#ifndef LBFGSPP_LINE_SEARCH_BACKTRACKING_H +#define LBFGSPP_LINE_SEARCH_BACKTRACKING_H + +#include +#include // std::runtime_error + +namespace LBFGSpp { + +/// +/// The backtracking line search algorithm for L-BFGS. Mainly for internal use. +/// +template +class LineSearchBacktracking +{ +private: + using Vector = Eigen::Matrix; + +public: + /// + /// Line search by backtracking. + /// + /// \param f A function object such that `f(x, grad)` returns the + /// objective function value at `x`, and overwrites `grad` with + /// the gradient. + /// \param param Parameters for the L-BFGS algorithm. + /// \param xp The current point. + /// \param drt The current moving direction. + /// \param step_max The upper bound for the step size that makes x feasible. + /// Can be ignored for the L-BFGS solver. + /// \param step In: The initial step length. + /// Out: The calculated step length. + /// \param fx In: The objective function value at the current point. + /// Out: The function value at the new point. + /// \param grad In: The current gradient vector. + /// Out: The gradient at the new point. + /// \param dg In: The inner product between drt and grad. + /// Out: The inner product between drt and the new gradient. + /// \param x Out: The new point moved to. + /// + template + static void LineSearch(Foo& f, const LBFGSParam& param, + const Vector& xp, const Vector& drt, const Scalar& step_max, + Scalar& step, Scalar& fx, Vector& grad, Scalar& dg, Vector& x) + { + // Decreasing and increasing factors + const Scalar dec = 0.5; + const Scalar inc = 2.1; + + // Check the value of step + if (step <= Scalar(0)) + throw std::invalid_argument("'step' must be positive"); + + // Save the function value at the current x + const Scalar fx_init = fx; + // Projection of gradient on the search direction + const Scalar dg_init = grad.dot(drt); + // Make sure d points to a descent direction + if (dg_init > 0) + throw std::logic_error("the moving direction increases the objective function value"); + + const Scalar test_decr = param.ftol * dg_init; + Scalar width; + + int iter; + for (iter = 0; iter < param.max_linesearch; iter++) + { + // x_{k+1} = x_k + step * d_k + x.noalias() = xp + step * drt; + // Evaluate this candidate + fx = f(x, grad); + + if (fx > fx_init + step * test_decr || (fx != fx)) + { + width = dec; + } + else + { + dg = grad.dot(drt); + + // Armijo condition is met + if (param.linesearch == LBFGS_LINESEARCH_BACKTRACKING_ARMIJO) + break; + + if (dg < param.wolfe * dg_init) + { + width = inc; + } + else + { + // Regular Wolfe condition is met + if (param.linesearch == LBFGS_LINESEARCH_BACKTRACKING_WOLFE) + break; + + if (dg > -param.wolfe * dg_init) + { + width = dec; + } + else + { + // Strong Wolfe condition is met + break; + } + } + } + + if (step < param.min_step) + throw std::runtime_error("the line search step became smaller than the minimum value allowed"); + + if (step > param.max_step) + throw std::runtime_error("the line search step became larger than the maximum value allowed"); + + step *= width; + } + + if (iter >= param.max_linesearch) + throw std::runtime_error("the line search routine reached the maximum number of iterations"); + } +}; + +} // namespace LBFGSpp + +#endif // LBFGSPP_LINE_SEARCH_BACKTRACKING_H diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchBracketing.h b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchBracketing.h new file mode 100644 index 0000000..4d0912f --- /dev/null +++ b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchBracketing.h @@ -0,0 +1,130 @@ +// Copyright (C) 2016-2023 Yixuan Qiu +// Copyright (C) 2016-2023 Dirk Toewe +// Under MIT license + +#ifndef LBFGSPP_LINE_SEARCH_BRACKETING_H +#define LBFGSPP_LINE_SEARCH_BRACKETING_H + +#include +#include // std::runtime_error + +namespace LBFGSpp { + +/// +/// The bracketing line search algorithm for L-BFGS. Mainly for internal use. +/// +template +class LineSearchBracketing +{ +private: + using Vector = Eigen::Matrix; + +public: + /// + /// Line search by bracketing. Similar to the backtracking line search + /// except that it actively maintains an upper and lower bound of the + /// current search range. + /// + /// \param f A function object such that `f(x, grad)` returns the + /// objective function value at `x`, and overwrites `grad` with + /// the gradient. + /// \param param Parameters for the L-BFGS algorithm. + /// \param xp The current point. + /// \param drt The current moving direction. + /// \param step_max The upper bound for the step size that makes x feasible. + /// Can be ignored for the L-BFGS solver. + /// \param step In: The initial step length. + /// Out: The calculated step length. + /// \param fx In: The objective function value at the current point. + /// Out: The function value at the new point. + /// \param grad In: The current gradient vector. + /// Out: The gradient at the new point. + /// \param dg In: The inner product between drt and grad. + /// Out: The inner product between drt and the new gradient. + /// \param x Out: The new point moved to. + /// + template + static void LineSearch(Foo& f, const LBFGSParam& param, + const Vector& xp, const Vector& drt, const Scalar& step_max, + Scalar& step, Scalar& fx, Vector& grad, Scalar& dg, Vector& x) + { + // Check the value of step + if (step <= Scalar(0)) + throw std::invalid_argument("'step' must be positive"); + + // Save the function value at the current x + const Scalar fx_init = fx; + // Projection of gradient on the search direction + const Scalar dg_init = grad.dot(drt); + // Make sure d points to a descent direction + if (dg_init > 0) + throw std::logic_error("the moving direction increases the objective function value"); + + const Scalar test_decr = param.ftol * dg_init; + + // Upper and lower end of the current line search range + Scalar step_lo = 0, + step_hi = std::numeric_limits::infinity(); + + int iter; + for (iter = 0; iter < param.max_linesearch; iter++) + { + // x_{k+1} = x_k + step * d_k + x.noalias() = xp + step * drt; + // Evaluate this candidate + fx = f(x, grad); + + if (fx > fx_init + step * test_decr || (fx != fx)) + { + step_hi = step; + } + else + { + dg = grad.dot(drt); + + // Armijo condition is met + if (param.linesearch == LBFGS_LINESEARCH_BACKTRACKING_ARMIJO) + break; + + if (dg < param.wolfe * dg_init) + { + step_lo = step; + } + else + { + // Regular Wolfe condition is met + if (param.linesearch == LBFGS_LINESEARCH_BACKTRACKING_WOLFE) + break; + + if (dg > -param.wolfe * dg_init) + { + step_hi = step; + } + else + { + // Strong Wolfe condition is met + break; + } + } + } + + assert(step_lo < step_hi); + + if (step < param.min_step) + throw std::runtime_error("the line search step became smaller than the minimum value allowed"); + + if (step > param.max_step) + throw std::runtime_error("the line search step became larger than the maximum value allowed"); + + // continue search in mid of current search range + step = std::isinf(step_hi) ? 2 * step : step_lo / 2 + step_hi / 2; + } + + if (iter >= param.max_linesearch) + throw std::runtime_error("the line search routine reached the maximum number of iterations"); + } +}; + +} // namespace LBFGSpp + +#endif // LBFGSPP_LINE_SEARCH_BRACKETING_H diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchMoreThuente.h b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchMoreThuente.h new file mode 100644 index 0000000..3bf7887 --- /dev/null +++ b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchMoreThuente.h @@ -0,0 +1,444 @@ +// Copyright (C) 2020-2023 Yixuan Qiu +// Under MIT license + +#ifndef LBFGSPP_LINE_SEARCH_MORE_THUENTE_H +#define LBFGSPP_LINE_SEARCH_MORE_THUENTE_H + +#include // std::invalid_argument, std::runtime_error +#include +#include "LBFGSpp/Param.h" + +namespace LBFGSpp { + +/// +/// The line search algorithm by Moré and Thuente (1994), currently used for the L-BFGS-B algorithm. +/// +/// The target of this line search algorithm is to find a step size \f$\alpha\f$ that satisfies the strong Wolfe condition +/// \f$f(x+\alpha d) \le f(x) + \alpha\mu g(x)^T d\f$ and \f$|g(x+\alpha d)^T d| \le \eta|g(x)^T d|\f$. +/// Our implementation is a simplified version of the algorithm in [1]. We assume that \f$0<\mu<\eta<1\f$, while in [1] +/// they do not assume \f$\eta>\mu\f$. As a result, the algorithm in [1] has two stages, but in our implementation we +/// only need the first stage to guarantee the convergence. +/// +/// Reference: +/// [1] Moré, J. J., & Thuente, D. J. (1994). Line search algorithms with guaranteed sufficient decrease. +/// +template +class LineSearchMoreThuente +{ +private: + using Vector = Eigen::Matrix; + + // Minimizer of a quadratic function q(x) = c0 + c1 * x + c2 * x^2 + // that interpolates fa, ga, and fb, assuming the minimizer exists + // For case I: fb >= fa and ga * (b - a) < 0 + static Scalar quadratic_minimizer(const Scalar& a, const Scalar& b, const Scalar& fa, const Scalar& ga, const Scalar& fb) + { + const Scalar ba = b - a; + const Scalar w = Scalar(0.5) * ba * ga / (fa - fb + ba * ga); + return a + w * ba; + } + + // Minimizer of a quadratic function q(x) = c0 + c1 * x + c2 * x^2 + // that interpolates fa, ga and gb, assuming the minimizer exists + // The result actually does not depend on fa + // For case II: ga * (b - a) < 0, ga * gb < 0 + // For case III: ga * (b - a) < 0, ga * ga >= 0, |gb| <= |ga| + static Scalar quadratic_minimizer(const Scalar& a, const Scalar& b, const Scalar& ga, const Scalar& gb) + { + const Scalar w = ga / (ga - gb); + return a + w * (b - a); + } + + // Local minimizer of a cubic function q(x) = c0 + c1 * x + c2 * x^2 + c3 * x^3 + // that interpolates fa, ga, fb and gb, assuming a != b + // Also sets a flag indicating whether the minimizer exists + static Scalar cubic_minimizer(const Scalar& a, const Scalar& b, const Scalar& fa, const Scalar& fb, + const Scalar& ga, const Scalar& gb, bool& exists) + { + using std::abs; + using std::sqrt; + + const Scalar apb = a + b; + const Scalar ba = b - a; + const Scalar ba2 = ba * ba; + const Scalar fba = fb - fa; + const Scalar gba = gb - ga; + // z3 = c3 * (b-a)^3, z2 = c2 * (b-a)^3, z1 = c1 * (b-a)^3 + const Scalar z3 = (ga + gb) * ba - Scalar(2) * fba; + const Scalar z2 = Scalar(0.5) * (gba * ba2 - Scalar(3) * apb * z3); + const Scalar z1 = fba * ba2 - apb * z2 - (a * apb + b * b) * z3; + // std::cout << "z1 = " << z1 << ", z2 = " << z2 << ", z3 = " << z3 << std::endl; + + // If c3 = z/(b-a)^3 == 0, reduce to quadratic problem + const Scalar eps = std::numeric_limits::epsilon(); + if (abs(z3) < eps * abs(z2) || abs(z3) < eps * abs(z1)) + { + // Minimizer exists if c2 > 0 + exists = (z2 * ba > Scalar(0)); + // Return the end point if the minimizer does not exist + return exists ? (-Scalar(0.5) * z1 / z2) : b; + } + + // Now we can assume z3 > 0 + // The minimizer is a solution to the equation c1 + 2*c2 * x + 3*c3 * x^2 = 0 + // roots = -(z2/z3) / 3 (+-) sqrt((z2/z3)^2 - 3 * (z1/z3)) / 3 + // + // Let u = z2/(3z3) and v = z1/z2 + // The minimizer exists if v/u <= 1 + const Scalar u = z2 / (Scalar(3) * z3), v = z1 / z2; + const Scalar vu = v / u; + exists = (vu <= Scalar(1)); + if (!exists) + return b; + + // We need to find a numerically stable way to compute the roots, as z3 may still be small + // + // If |u| >= |v|, let w = 1 + sqrt(1-v/u), and then + // r1 = -u * w, r2 = -v / w, r1 does not need to be the smaller one + // + // If |u| < |v|, we must have uv <= 0, and then + // r = -u (+-) sqrt(delta), where + // sqrt(delta) = sqrt(|u|) * sqrt(|v|) * sqrt(1-u/v) + Scalar r1 = Scalar(0), r2 = Scalar(0); + if (abs(u) >= abs(v)) + { + const Scalar w = Scalar(1) + sqrt(Scalar(1) - vu); + r1 = -u * w; + r2 = -v / w; + } + else + { + const Scalar sqrtd = sqrt(abs(u)) * sqrt(abs(v)) * sqrt(1 - u / v); + r1 = -u - sqrtd; + r2 = -u + sqrtd; + } + return (z3 * ba > Scalar(0)) ? ((std::max)(r1, r2)) : ((std::min)(r1, r2)); + } + + // Select the next step size according to the current step sizes, + // function values, and derivatives + static Scalar step_selection( + const Scalar& al, const Scalar& au, const Scalar& at, + const Scalar& fl, const Scalar& fu, const Scalar& ft, + const Scalar& gl, const Scalar& gu, const Scalar& gt) + { + using std::abs; + + if (al == au) + return al; + + // If ft = Inf or gt = Inf, we return the middle point of al and at + if (!std::isfinite(ft) || !std::isfinite(gt)) + return (al + at) / Scalar(2); + + // ac: cubic interpolation of fl, ft, gl, gt + // aq: quadratic interpolation of fl, gl, ft + bool ac_exists; + // std::cout << "al = " << al << ", at = " << at << ", fl = " << fl << ", ft = " << ft << ", gl = " << gl << ", gt = " << gt << std::endl; + const Scalar ac = cubic_minimizer(al, at, fl, ft, gl, gt, ac_exists); + const Scalar aq = quadratic_minimizer(al, at, fl, gl, ft); + // std::cout << "ac = " << ac << ", aq = " << aq << std::endl; + // Case 1: ft > fl + if (ft > fl) + { + // This should not happen if ft > fl, but just to be safe + if (!ac_exists) + return aq; + // Then use the scheme described in the paper + return (abs(ac - al) < abs(aq - al)) ? ac : ((aq + ac) / Scalar(2)); + } + + // as: quadratic interpolation of gl and gt + const Scalar as = quadratic_minimizer(al, at, gl, gt); + // Case 2: ft <= fl, gt * gl < 0 + if (gt * gl < Scalar(0)) + return (abs(ac - at) >= abs(as - at)) ? ac : as; + + // Case 3: ft <= fl, gt * gl >= 0, |gt| < |gl| + const Scalar deltal = Scalar(1.1), deltau = Scalar(0.66); + if (abs(gt) < abs(gl)) + { + // We choose either ac or as + // The case for ac: 1. It exists, and + // 2. ac is farther than at from al, and + // 3. ac is closer to at than as + // Cases for as: otherwise + const Scalar res = (ac_exists && + (ac - at) * (at - al) > Scalar(0) && + abs(ac - at) < abs(as - at)) ? + ac : + as; + // Postprocessing the chosen step + return (at > al) ? + std::min(at + deltau * (au - at), res) : + std::max(at + deltau * (au - at), res); + } + + // Simple extrapolation if au, fu, or gu is infinity + if ((!std::isfinite(au)) || (!std::isfinite(fu)) || (!std::isfinite(gu))) + return at + deltal * (at - al); + + // ae: cubic interpolation of ft, fu, gt, gu + bool ae_exists; + const Scalar ae = cubic_minimizer(at, au, ft, fu, gt, gu, ae_exists); + // Case 4: ft <= fl, gt * gl >= 0, |gt| >= |gl| + // The following is not used in the paper, but it seems to be a reasonable safeguard + return (at > al) ? + std::min(at + deltau * (au - at), ae) : + std::max(at + deltau * (au - at), ae); + } + +public: + /// + /// Line search by Moré and Thuente (1994). + /// + /// \param f A function object such that `f(x, grad)` returns the + /// objective function value at `x`, and overwrites `grad` with + /// the gradient. + /// \param param An `LBFGSParam` or `LBFGSBParam` object that stores the + /// parameters of the solver. + /// \param xp The current point. + /// \param drt The current moving direction. + /// \param step_max The upper bound for the step size that makes x feasible. + /// \param step In: The initial step length. + /// Out: The calculated step length. + /// \param fx In: The objective function value at the current point. + /// Out: The function value at the new point. + /// \param grad In: The current gradient vector. + /// Out: The gradient at the new point. + /// \param dg In: The inner product between drt and grad. + /// Out: The inner product between drt and the new gradient. + /// \param x Out: The new point moved to. + /// + template + static void LineSearch(Foo& f, const SolverParam& param, + const Vector& xp, const Vector& drt, const Scalar& step_max, + Scalar& step, Scalar& fx, Vector& grad, Scalar& dg, Vector& x) + { + using std::abs; + // std::cout << "========================= Entering line search =========================\n\n"; + + // Check the value of step + if (step <= Scalar(0)) + throw std::invalid_argument("'step' must be positive"); + if (step > step_max) + throw std::invalid_argument("'step' exceeds 'step_max'"); + + // Save the function value at the current x + const Scalar fx_init = fx; + // Projection of gradient on the search direction + const Scalar dg_init = dg; + + // std::cout << "fx_init = " << fx_init << ", dg_init = " << dg_init << std::endl << std::endl; + + // Make sure d points to a descent direction + if (dg_init >= Scalar(0)) + throw std::logic_error("the moving direction does not decrease the objective function value"); + + // Tolerance for convergence test + // Sufficient decrease + const Scalar test_decr = param.ftol * dg_init; + // Curvature + const Scalar test_curv = -param.wolfe * dg_init; + + // The bracketing interval + Scalar I_lo = Scalar(0), I_hi = std::numeric_limits::infinity(); + Scalar fI_lo = Scalar(0), fI_hi = std::numeric_limits::infinity(); + Scalar gI_lo = (Scalar(1) - param.ftol) * dg_init, gI_hi = std::numeric_limits::infinity(); + // We also need to save x and grad for step=I_lo, since we want to return the best + // step size along the path when strong Wolfe condition is not met + Vector x_lo = xp, grad_lo = grad; + Scalar fx_lo = fx_init, dg_lo = dg_init; + + // Function value and gradient at the current step size + x.noalias() = xp + step * drt; + fx = f(x, grad); + dg = grad.dot(drt); + + // std::cout << "max_step = " << step_max << ", step = " << step << ", fx = " << fx << ", dg = " << dg << std::endl; + + // Convergence test + if (fx <= fx_init + step * test_decr && abs(dg) <= test_curv) + { + // std::cout << "** Criteria met\n\n"; + // std::cout << "========================= Leaving line search =========================\n\n"; + return; + } + + // Extrapolation factor + const Scalar delta = Scalar(1.1); + int iter; + for (iter = 0; iter < param.max_linesearch; iter++) + { + // ft = psi(step) = f(xp + step * drt) - f(xp) - step * test_decr + // gt = psi'(step) = dg - mu * dg_init + // mu = param.ftol + const Scalar ft = fx - fx_init - step * test_decr; + const Scalar gt = dg - param.ftol * dg_init; + + // Update step size and bracketing interval + Scalar new_step; + if (ft > fI_lo) + { + // Case 1: ft > fl + new_step = step_selection(I_lo, I_hi, step, fI_lo, fI_hi, ft, gI_lo, gI_hi, gt); + // Sanity check: if the computed new_step is too small, typically due to + // extremely large value of ft, switch to the middle point + if (new_step <= param.min_step) + new_step = (I_lo + step) / Scalar(2); + + I_hi = step; + fI_hi = ft; + gI_hi = gt; + + // std::cout << "Case 1: new step = " << new_step << std::endl; + } + else if (gt * (I_lo - step) > Scalar(0)) + { + // Case 2: ft <= fl, gt * (al - at) > 0 + // + // Page 291 of Moré and Thuente (1994) suggests that + // newat = min(at + delta * (at - al), amax), delta in [1.1, 4] + new_step = std::min(step_max, step + delta * (step - I_lo)); + + // We can also consider the following scheme: + // First let step_selection() decide a value, and then project to the range above + // + // new_step = step_selection(I_lo, I_hi, step, fI_lo, fI_hi, ft, gI_lo, gI_hi, gt); + // const Scalar delta2 = Scalar(4) + // const Scalar t1 = step + delta * (step - I_lo); + // const Scalar t2 = step + delta2 * (step - I_lo); + // const Scalar tl = std::min(t1, t2), tu = std::max(t1, t2); + // new_step = std::min(tu, std::max(tl, new_step)); + // new_step = std::min(step_max, new_step); + + I_lo = step; + fI_lo = ft; + gI_lo = gt; + // Move x and grad to x_lo and grad_lo, respectively + x_lo.swap(x); + grad_lo.swap(grad); + fx_lo = fx; + dg_lo = dg; + + // std::cout << "Case 2: new step = " << new_step << std::endl; + } + else + { + // Case 3: ft <= fl, gt * (al - at) <= 0 + new_step = step_selection(I_lo, I_hi, step, fI_lo, fI_hi, ft, gI_lo, gI_hi, gt); + + I_hi = I_lo; + fI_hi = fI_lo; + gI_hi = gI_lo; + + I_lo = step; + fI_lo = ft; + gI_lo = gt; + // Move x and grad to x_lo and grad_lo, respectively + x_lo.swap(x); + grad_lo.swap(grad); + fx_lo = fx; + dg_lo = dg; + + // std::cout << "Case 3: new step = " << new_step << std::endl; + } + + // Case 1 and 3 are interpolations, whereas Case 2 is extrapolation + // This means that Case 2 may return new_step = step_max, + // and we need to decide whether to accept this value + // 1. If both step and new_step equal to step_max, it means + // step will have no further change, so we accept it + // 2. Otherwise, we need to test the function value and gradient + // on step_max, and decide later + + // In case step, new_step, and step_max are equal, directly return the computed x and fx + if (step == step_max && new_step >= step_max) + { + // std::cout << "** Maximum step size reached\n\n"; + // std::cout << "========================= Leaving line search =========================\n\n"; + + // Move {x, grad}_lo back before returning + x.swap(x_lo); + grad.swap(grad_lo); + return; + } + // Otherwise, recompute x and fx based on new_step + step = new_step; + + if (step < param.min_step) + throw std::runtime_error("the line search step became smaller than the minimum value allowed"); + + if (step > param.max_step) + throw std::runtime_error("the line search step became larger than the maximum value allowed"); + + // Update parameter, function value, and gradient + x.noalias() = xp + step * drt; + fx = f(x, grad); + dg = grad.dot(drt); + + // std::cout << "step = " << step << ", fx = " << fx << ", dg = " << dg << std::endl; + + // Convergence test + if (fx <= fx_init + step * test_decr && abs(dg) <= test_curv) + { + // std::cout << "** Criteria met\n\n"; + // std::cout << "========================= Leaving line search =========================\n\n"; + return; + } + + // Now assume step = step_max, and we need to decide whether to + // exit the line search (see the comments above regarding step_max) + // If we reach here, it means this step size does not pass the convergence + // test, so either the sufficient decrease condition or the curvature + // condition is not met yet + // + // Typically the curvature condition is harder to meet, and it is + // possible that no step size in [0, step_max] satisfies the condition + // + // But we need to make sure that its psi function value is smaller than + // the best one so far. If not, go to the next iteration and find a better one + if (step >= step_max) + { + const Scalar ft_bound = fx - fx_init - step * test_decr; + if (ft_bound <= fI_lo) + { + // std::cout << "** Maximum step size reached\n\n"; + // std::cout << "========================= Leaving line search =========================\n\n"; + return; + } + } + } + + // If we have used up all line search iterations, then the strong Wolfe condition + // is not met. We choose not to raise an exception (unless no step satisfying + // sufficient decrease is found), but to return the best step size so far + if (iter >= param.max_linesearch) + { + // throw std::runtime_error("the line search routine reached the maximum number of iterations"); + + // First test whether the last step is better than I_lo + // If yes, return the last step + const Scalar ft = fx - fx_init - step * test_decr; + if (ft <= fI_lo) + return; + + // If not, then the best step size so far is I_lo, but it needs to be positive + if (I_lo <= Scalar(0)) + throw std::runtime_error("the line search routine is unable to sufficiently decrease the function value"); + + // Return everything with _lo + step = I_lo; + fx = fx_lo; + dg = dg_lo; + // Move {x, grad}_lo back + x.swap(x_lo); + grad.swap(grad_lo); + return; + } + } +}; + +} // namespace LBFGSpp + +#endif // LBFGSPP_LINE_SEARCH_MORE_THUENTE_H diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchNocedalWright.h b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchNocedalWright.h new file mode 100644 index 0000000..9b91ed5 --- /dev/null +++ b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchNocedalWright.h @@ -0,0 +1,281 @@ +// Copyright (C) 2016-2023 Yixuan Qiu +// Copyright (C) 2016-2023 Dirk Toewe +// Under MIT license + +#ifndef LBFGSPP_LINE_SEARCH_NOCEDAL_WRIGHT_H +#define LBFGSPP_LINE_SEARCH_NOCEDAL_WRIGHT_H + +#include +#include + +namespace LBFGSpp { + +/// +/// A line search algorithm for the strong Wolfe condition. Implementation based on: +/// +/// "Numerical Optimization" 2nd Edition, +/// Jorge Nocedal and Stephen J. Wright, +/// Chapter 3. Line Search Methods, page 60. +/// +template +class LineSearchNocedalWright +{ +private: + using Vector = Eigen::Matrix; + + // Use {fx_lo, fx_hi, dg_lo} to make a quadratic interpolation of + // the function, and the fitted quadratic function is used to + // estimate the minimum + static Scalar quad_interp(const Scalar& step_lo, const Scalar& step_hi, + const Scalar& fx_lo, const Scalar& fx_hi, const Scalar& dg_lo) + { + using std::abs; + + // polynomial: p (x) = c0*(x - step)² + c1 + // conditions: p (step_hi) = fx_hi + // p (step_lo) = fx_lo + // p'(step_lo) = dg_lo + + // We allow fx_hi to be Inf, so first compute a candidate for step size, + // and test whether NaN occurs + const Scalar fdiff = fx_hi - fx_lo; + const Scalar sdiff = step_hi - step_lo; + const Scalar smid = (step_hi + step_lo) / Scalar(2); + Scalar step_candid = fdiff * step_lo - smid * sdiff * dg_lo; + step_candid = step_candid / (fdiff - sdiff * dg_lo); + + // In some cases the interpolation is not a good choice + // This includes (a) NaN values; (b) too close to the end points; (c) outside the interval + // In such cases, a bisection search is used + const bool candid_nan = !(std::isfinite(step_candid)); + const Scalar end_dist = std::min(abs(step_candid - step_lo), abs(step_candid - step_hi)); + const bool near_end = end_dist < Scalar(0.01) * abs(sdiff); + const bool bisect = candid_nan || + (step_candid <= std::min(step_lo, step_hi)) || + (step_candid >= std::max(step_lo, step_hi)) || + near_end; + const Scalar step = bisect ? smid : step_candid; + return step; + } + +public: + /// + /// Line search by Nocedal and Wright (2006). + /// + /// \param f A function object such that `f(x, grad)` returns the + /// objective function value at `x`, and overwrites `grad` with + /// the gradient. + /// \param param Parameters for the L-BFGS algorithm. + /// \param xp The current point. + /// \param drt The current moving direction. + /// \param step_max The upper bound for the step size that makes x feasible. + /// Can be ignored for the L-BFGS solver. + /// \param step In: The initial step length. + /// Out: The calculated step length. + /// \param fx In: The objective function value at the current point. + /// Out: The function value at the new point. + /// \param grad In: The current gradient vector. + /// Out: The gradient at the new point. + /// \param dg In: The inner product between drt and grad. + /// Out: The inner product between drt and the new gradient. + /// \param x Out: The new point moved to. + /// + template + static void LineSearch(Foo& f, const LBFGSParam& param, + const Vector& xp, const Vector& drt, const Scalar& step_max, + Scalar& step, Scalar& fx, Vector& grad, Scalar& dg, Vector& x) + { + // Check the value of step + if (step <= Scalar(0)) + throw std::invalid_argument("'step' must be positive"); + + if (param.linesearch != LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE) + throw std::invalid_argument("'param.linesearch' must be 'LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE' for LineSearchNocedalWright"); + + // To make this implementation more similar to the other line search + // methods in LBFGSpp, the symbol names from the literature + // ("Numerical Optimizations") have been changed. + // + // Literature | LBFGSpp + // -----------|-------- + // alpha | step + // phi | fx + // phi' | dg + + // The expansion rate of the step size + const Scalar expansion = Scalar(2); + + // Save the function value at the current x + const Scalar fx_init = fx; + // Projection of gradient on the search direction + const Scalar dg_init = dg; + // Make sure d points to a descent direction + if (dg_init > Scalar(0)) + throw std::logic_error("the moving direction increases the objective function value"); + + const Scalar test_decr = param.ftol * dg_init, // Sufficient decrease + test_curv = -param.wolfe * dg_init; // Curvature + + // Ends of the line search range (step_lo > step_hi is allowed) + // We can also define dg_hi, but it will never be used + Scalar step_hi, fx_hi; + Scalar step_lo = Scalar(0), fx_lo = fx_init, dg_lo = dg_init; + // We also need to save x and grad for step=step_lo, since we want to return the best + // step size along the path when strong Wolfe condition is not met + Vector x_lo = xp, grad_lo = grad; + + // STEP 1: Bracketing Phase + // Find a range guaranteed to contain a step satisfying strong Wolfe. + // The bracketing phase exits if one of the following conditions is satisfied: + // (1) Current step violates the sufficient decrease condition + // (2) Current fx >= previous fx + // (3) Current dg >= 0 + // (4) Strong Wolfe condition is met + // + // (4) terminates the whole line search, and (1)-(3) go to the zoom phase + // + // See also: + // "Numerical Optimization", "Algorithm 3.5 (Line Search Algorithm)". + int iter = 0; + for (;;) + { + // Evaluate the current step size + x.noalias() = xp + step * drt; + fx = f(x, grad); + dg = grad.dot(drt); + + // Test the sufficient decrease condition + if (fx - fx_init > step * test_decr || (Scalar(0) < step_lo && fx >= fx_lo)) + { + // Case (1) and (2) + step_hi = step; + fx_hi = fx; + // dg_hi = dg; + break; + } + // If reaching here, then the sufficient decrease condition is satisfied + + // Test the curvature condition + if (std::abs(dg) <= test_curv) + return; // Case (4) + + step_hi = step_lo; + fx_hi = fx_lo; + // dg_hi = dg_lo; + step_lo = step; + fx_lo = fx; + dg_lo = dg; + // Move x and grad to x_lo and grad_lo, respectively + x_lo.swap(x); + grad_lo.swap(grad); + + if (dg >= Scalar(0)) + break; // Case (3) + + iter++; + // If we have used up all line search iterations in the bracketing phase, + // it means every new step decreases the objective function. Of course, + // the strong Wolfe condition is not met, but we choose not to raise an + // exception; instead, we return the best step size so far. This means that + // we exit the line search with the most recent step size, which has the + // smallest objective function value during the line search + if (iter >= param.max_linesearch) + { + // throw std::runtime_error("the line search routine reached the maximum number of iterations"); + + // At this point we can guarantee that {step, fx, dg}=={step, fx, dg}_lo + // But we need to move {x, grad}_lo back before returning + x.swap(x_lo); + grad.swap(grad_lo); + return; + } + + // If we still stay in the loop, it means we can expand the current step + step *= expansion; + } + + // STEP 2: Zoom Phase + // Given a range (step_lo,step_hi) that is guaranteed to + // contain a valid strong Wolfe step value, this method + // finds such a value. + // + // If step_lo > 0, then step_lo is, among all step sizes generated so far and + // satisfying the sufficient decrease condition, the one giving the smallest + // objective function value. + // + // See also: + // "Numerical Optimization", "Algorithm 3.6 (Zoom)". + for (;;) + { + // Use {fx_lo, fx_hi, dg_lo} to make a quadratic interpolation of + // the function, and the fitted quadratic function is used to + // estimate the minimum + step = quad_interp(step_lo, step_hi, fx_lo, fx_hi, dg_lo); + + // Evaluate the current step size + x.noalias() = xp + step * drt; + fx = f(x, grad); + dg = grad.dot(drt); + + // Test the sufficient decrease condition + if (fx - fx_init > step * test_decr || fx >= fx_lo) + { + if (step == step_hi) + throw std::runtime_error("the line search routine failed, possibly due to insufficient numeric precision"); + + step_hi = step; + fx_hi = fx; + // dg_hi = dg; + } + else + { + // Test the curvature condition + if (std::abs(dg) <= test_curv) + return; + + if (dg * (step_hi - step_lo) >= Scalar(0)) + { + step_hi = step_lo; + fx_hi = fx_lo; + // dg_hi = dg_lo; + } + + if (step == step_lo) + throw std::runtime_error("the line search routine failed, possibly due to insufficient numeric precision"); + + // If reaching here, then the current step satisfies sufficient decrease condition + step_lo = step; + fx_lo = fx; + dg_lo = dg; + // Move x and grad to x_lo and grad_lo, respectively + x_lo.swap(x); + grad_lo.swap(grad); + } + + iter++; + // If we have used up all line search iterations in the zoom phase, + // then the strong Wolfe condition is not met. We choose not to raise an + // exception (unless no step satisfying sufficient decrease is found), + // but to return the best step size so far, i.e., step_lo + if (iter >= param.max_linesearch) + { + // throw std::runtime_error("the line search routine reached the maximum number of iterations"); + if (step_lo <= Scalar(0)) + throw std::runtime_error("the line search routine failed, unable to sufficiently decrease the function value"); + + // Return everything with _lo + step = step_lo; + fx = fx_lo; + dg = dg_lo; + // Move {x, grad}_lo back + x.swap(x_lo); + grad.swap(grad_lo); + return; + } + } + } +}; + +} // namespace LBFGSpp + +#endif // LBFGSPP_LINE_SEARCH_NOCEDAL_WRIGHT_H diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/Param.h b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/Param.h new file mode 100644 index 0000000..d8c6785 --- /dev/null +++ b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/Param.h @@ -0,0 +1,381 @@ +// Copyright (C) 2016-2023 Yixuan Qiu +// Under MIT license + +#ifndef LBFGSPP_PARAM_H +#define LBFGSPP_PARAM_H + +#include +#include // std::invalid_argument + +namespace LBFGSpp { + +/// +/// \defgroup Enumerations +/// +/// Enumeration types for line search. +/// + +/// +/// \ingroup Enumerations +/// +/// The enumeration of line search termination conditions. +/// +enum LINE_SEARCH_TERMINATION_CONDITION +{ + /// + /// Backtracking method with the Armijo condition. + /// The backtracking method finds the step length such that it satisfies + /// the sufficient decrease (Armijo) condition, + /// \f$f(x + a \cdot d) \le f(x) + \beta' \cdot a \cdot g(x)^T d\f$, + /// where \f$x\f$ is the current point, \f$d\f$ is the current search direction, + /// \f$a\f$ is the step length, and \f$\beta'\f$ is the value specified by + /// \ref LBFGSParam::ftol. \f$f\f$ and \f$g\f$ are the function + /// and gradient values respectively. + /// + LBFGS_LINESEARCH_BACKTRACKING_ARMIJO = 1, + + /// + /// The backtracking method with the defualt (regular Wolfe) condition. + /// An alias of `LBFGS_LINESEARCH_BACKTRACKING_WOLFE`. + /// + LBFGS_LINESEARCH_BACKTRACKING = 2, + + /// + /// Backtracking method with regular Wolfe condition. + /// The backtracking method finds the step length such that it satisfies + /// both the Armijo condition (`LBFGS_LINESEARCH_BACKTRACKING_ARMIJO`) + /// and the curvature condition, + /// \f$g(x + a \cdot d)^T d \ge \beta \cdot g(x)^T d\f$, where \f$\beta\f$ + /// is the value specified by \ref LBFGSParam::wolfe. + /// + LBFGS_LINESEARCH_BACKTRACKING_WOLFE = 2, + + /// + /// Backtracking method with strong Wolfe condition. + /// The backtracking method finds the step length such that it satisfies + /// both the Armijo condition (`LBFGS_LINESEARCH_BACKTRACKING_ARMIJO`) + /// and the following condition, + /// \f$\vert g(x + a \cdot d)^T d\vert \le \beta \cdot \vert g(x)^T d\vert\f$, + /// where \f$\beta\f$ is the value specified by \ref LBFGSParam::wolfe. + /// + LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE = 3 +}; + +/// +/// Parameters to control the L-BFGS algorithm. +/// +template +class LBFGSParam +{ +public: + /// + /// The number of corrections to approximate the inverse Hessian matrix. + /// The L-BFGS routine stores the computation results of previous \ref m + /// iterations to approximate the inverse Hessian matrix of the current + /// iteration. This parameter controls the size of the limited memories + /// (corrections). The default value is \c 6. Values less than \c 3 are + /// not recommended. Large values will result in excessive computing time. + /// + int m; + /// + /// Absolute tolerance for convergence test. + /// This parameter determines the absolute accuracy \f$\epsilon_{abs}\f$ + /// with which the solution is to be found. A minimization terminates when + /// \f$||g|| < \max\{\epsilon_{abs}, \epsilon_{rel}||x||\}\f$, + /// where \f$||\cdot||\f$ denotes the Euclidean (L2) norm. The default value is + /// \c 1e-5. + /// + Scalar epsilon; + /// + /// Relative tolerance for convergence test. + /// This parameter determines the relative accuracy \f$\epsilon_{rel}\f$ + /// with which the solution is to be found. A minimization terminates when + /// \f$||g|| < \max\{\epsilon_{abs}, \epsilon_{rel}||x||\}\f$, + /// where \f$||\cdot||\f$ denotes the Euclidean (L2) norm. The default value is + /// \c 1e-5. + /// + Scalar epsilon_rel; + /// + /// Distance for delta-based convergence test. + /// This parameter determines the distance \f$d\f$ to compute the + /// rate of decrease of the objective function, + /// \f$f_{k-d}(x)-f_k(x)\f$, where \f$k\f$ is the current iteration + /// step. If the value of this parameter is zero, the delta-based convergence + /// test will not be performed. The default value is \c 0. + /// + int past; + /// + /// Delta for convergence test. + /// The algorithm stops when the following condition is met, + /// \f$|f_{k-d}(x)-f_k(x)|<\delta\cdot\max(1, |f_k(x)|, |f_{k-d}(x)|)\f$, where \f$f_k(x)\f$ is + /// the current function value, and \f$f_{k-d}(x)\f$ is the function value + /// \f$d\f$ iterations ago (specified by the \ref past parameter). + /// The default value is \c 0. + /// + Scalar delta; + /// + /// The maximum number of iterations. + /// The optimization process is terminated when the iteration count + /// exceeds this parameter. Setting this parameter to zero continues an + /// optimization process until a convergence or error. The default value + /// is \c 0. + /// + int max_iterations; + /// + /// The line search termination condition. + /// This parameter specifies the line search termination condition that will be used + /// by the LBFGS routine. The default value is `LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE`. + /// + int linesearch; + /// + /// The maximum number of trials for the line search. + /// This parameter controls the number of function and gradients evaluations + /// per iteration for the line search routine. The default value is \c 20. + /// + int max_linesearch; + /// + /// The minimum step length allowed in the line search. + /// The default value is \c 1e-20. Usually this value does not need to be + /// modified. + /// + Scalar min_step; + /// + /// The maximum step length allowed in the line search. + /// The default value is \c 1e+20. Usually this value does not need to be + /// modified. + /// + Scalar max_step; + /// + /// A parameter to control the accuracy of the line search routine. + /// The default value is \c 1e-4. This parameter should be greater + /// than zero and smaller than \c 0.5. + /// + Scalar ftol; + /// + /// The coefficient for the Wolfe condition. + /// This parameter is valid only when the line-search + /// algorithm is used with the Wolfe condition. + /// The default value is \c 0.9. This parameter should be greater + /// the \ref ftol parameter and smaller than \c 1.0. + /// + Scalar wolfe; + +public: + /// + /// Constructor for L-BFGS parameters. + /// Default values for parameters will be set when the object is created. + /// + LBFGSParam() + { + // clang-format off + m = 6; + epsilon = Scalar(1e-5); + epsilon_rel = Scalar(1e-5); + past = 0; + delta = Scalar(0); + max_iterations = 0; + linesearch = LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE; + max_linesearch = 20; + min_step = Scalar(1e-20); + max_step = Scalar(1e+20); + ftol = Scalar(1e-4); + wolfe = Scalar(0.9); + // clang-format on + } + + /// + /// Checking the validity of L-BFGS parameters. + /// An `std::invalid_argument` exception will be thrown if some parameter + /// is invalid. + /// + inline void check_param() const + { + if (m <= 0) + throw std::invalid_argument("'m' must be positive"); + if (epsilon < 0) + throw std::invalid_argument("'epsilon' must be non-negative"); + if (epsilon_rel < 0) + throw std::invalid_argument("'epsilon_rel' must be non-negative"); + if (past < 0) + throw std::invalid_argument("'past' must be non-negative"); + if (delta < 0) + throw std::invalid_argument("'delta' must be non-negative"); + if (max_iterations < 0) + throw std::invalid_argument("'max_iterations' must be non-negative"); + if (linesearch < LBFGS_LINESEARCH_BACKTRACKING_ARMIJO || + linesearch > LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE) + throw std::invalid_argument("unsupported line search termination condition"); + if (max_linesearch <= 0) + throw std::invalid_argument("'max_linesearch' must be positive"); + if (min_step < 0) + throw std::invalid_argument("'min_step' must be positive"); + if (max_step < min_step) + throw std::invalid_argument("'max_step' must be greater than 'min_step'"); + if (ftol <= 0 || ftol >= 0.5) + throw std::invalid_argument("'ftol' must satisfy 0 < ftol < 0.5"); + if (wolfe <= ftol || wolfe >= 1) + throw std::invalid_argument("'wolfe' must satisfy ftol < wolfe < 1"); + } +}; + +/// +/// Parameters to control the L-BFGS-B algorithm. +/// +template +class LBFGSBParam +{ +public: + /// + /// The number of corrections to approximate the inverse Hessian matrix. + /// The L-BFGS-B routine stores the computation results of previous \ref m + /// iterations to approximate the inverse Hessian matrix of the current + /// iteration. This parameter controls the size of the limited memories + /// (corrections). The default value is \c 6. Values less than \c 3 are + /// not recommended. Large values will result in excessive computing time. + /// + int m; + /// + /// Absolute tolerance for convergence test. + /// This parameter determines the absolute accuracy \f$\epsilon_{abs}\f$ + /// with which the solution is to be found. A minimization terminates when + /// \f$||Pg||_{\infty} < \max\{\epsilon_{abs}, \epsilon_{rel}||x||\}\f$, + /// where \f$||x||\f$ denotes the Euclidean (L2) norm of \f$x\f$, and + /// \f$Pg=P(x-g,l,u)-x\f$ is the projected gradient. The default value is + /// \c 1e-5. + /// + Scalar epsilon; + /// + /// Relative tolerance for convergence test. + /// This parameter determines the relative accuracy \f$\epsilon_{rel}\f$ + /// with which the solution is to be found. A minimization terminates when + /// \f$||Pg||_{\infty} < \max\{\epsilon_{abs}, \epsilon_{rel}||x||\}\f$, + /// where \f$||x||\f$ denotes the Euclidean (L2) norm of \f$x\f$, and + /// \f$Pg=P(x-g,l,u)-x\f$ is the projected gradient. The default value is + /// \c 1e-5. + /// + Scalar epsilon_rel; + /// + /// Distance for delta-based convergence test. + /// This parameter determines the distance \f$d\f$ to compute the + /// rate of decrease of the objective function, + /// \f$f_{k-d}(x)-f_k(x)\f$, where \f$k\f$ is the current iteration + /// step. If the value of this parameter is zero, the delta-based convergence + /// test will not be performed. The default value is \c 1. + /// + int past; + /// + /// Delta for convergence test. + /// The algorithm stops when the following condition is met, + /// \f$|f_{k-d}(x)-f_k(x)|<\delta\cdot\max(1, |f_k(x)|, |f_{k-d}(x)|)\f$, where \f$f_k(x)\f$ is + /// the current function value, and \f$f_{k-d}(x)\f$ is the function value + /// \f$d\f$ iterations ago (specified by the \ref past parameter). + /// The default value is \c 1e-10. + /// + Scalar delta; + /// + /// The maximum number of iterations. + /// The optimization process is terminated when the iteration count + /// exceeds this parameter. Setting this parameter to zero continues an + /// optimization process until a convergence or error. The default value + /// is \c 0. + /// + int max_iterations; + /// + /// The maximum number of iterations in the subspace minimization. + /// This parameter controls the number of iterations in the subspace + /// minimization routine. The default value is \c 10. + /// + int max_submin; + /// + /// The maximum number of trials for the line search. + /// This parameter controls the number of function and gradients evaluations + /// per iteration for the line search routine. The default value is \c 20. + /// + int max_linesearch; + /// + /// The minimum step length allowed in the line search. + /// The default value is \c 1e-20. Usually this value does not need to be + /// modified. + /// + Scalar min_step; + /// + /// The maximum step length allowed in the line search. + /// The default value is \c 1e+20. Usually this value does not need to be + /// modified. + /// + Scalar max_step; + /// + /// A parameter to control the accuracy of the line search routine. + /// The default value is \c 1e-4. This parameter should be greater + /// than zero and smaller than \c 0.5. + /// + Scalar ftol; + /// + /// The coefficient for the Wolfe condition. + /// This parameter is valid only when the line-search + /// algorithm is used with the Wolfe condition. + /// The default value is \c 0.9. This parameter should be greater + /// the \ref ftol parameter and smaller than \c 1.0. + /// + Scalar wolfe; + +public: + /// + /// Constructor for L-BFGS-B parameters. + /// Default values for parameters will be set when the object is created. + /// + LBFGSBParam() + { + // clang-format off + m = 6; + epsilon = Scalar(1e-5); + epsilon_rel = Scalar(1e-5); + past = 1; + delta = Scalar(1e-10); + max_iterations = 0; + max_submin = 10; + max_linesearch = 20; + min_step = Scalar(1e-20); + max_step = Scalar(1e+20); + ftol = Scalar(1e-4); + wolfe = Scalar(0.9); + // clang-format on + } + + /// + /// Checking the validity of L-BFGS-B parameters. + /// An `std::invalid_argument` exception will be thrown if some parameter + /// is invalid. + /// + inline void check_param() const + { + if (m <= 0) + throw std::invalid_argument("'m' must be positive"); + if (epsilon < 0) + throw std::invalid_argument("'epsilon' must be non-negative"); + if (epsilon_rel < 0) + throw std::invalid_argument("'epsilon_rel' must be non-negative"); + if (past < 0) + throw std::invalid_argument("'past' must be non-negative"); + if (delta < 0) + throw std::invalid_argument("'delta' must be non-negative"); + if (max_iterations < 0) + throw std::invalid_argument("'max_iterations' must be non-negative"); + if (max_submin < 0) + throw std::invalid_argument("'max_submin' must be non-negative"); + if (max_linesearch <= 0) + throw std::invalid_argument("'max_linesearch' must be positive"); + if (min_step < 0) + throw std::invalid_argument("'min_step' must be positive"); + if (max_step < min_step) + throw std::invalid_argument("'max_step' must be greater than 'min_step'"); + if (ftol <= 0 || ftol >= 0.5) + throw std::invalid_argument("'ftol' must satisfy 0 < ftol < 0.5"); + if (wolfe <= ftol || wolfe >= 1) + throw std::invalid_argument("'wolfe' must satisfy ftol < wolfe < 1"); + } +}; + +} // namespace LBFGSpp + +#endif // LBFGSPP_PARAM_H diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/SubspaceMin.h b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/SubspaceMin.h new file mode 100644 index 0000000..e504ea8 --- /dev/null +++ b/src/prm_vision/opencv_armor_detector/include/LBFGSpp/SubspaceMin.h @@ -0,0 +1,309 @@ +// Copyright (C) 2020-2023 Yixuan Qiu +// Under MIT license + +#ifndef LBFGSPP_SUBSPACE_MIN_H +#define LBFGSPP_SUBSPACE_MIN_H + +#include +#include +#include +#include "BFGSMat.h" + +/// \cond + +namespace LBFGSpp { + +// +// Subspace minimization procedure of the L-BFGS-B algorithm, +// mainly for internal use. +// +// The target of subspace minimization is to minimize the quadratic function m(x) +// over the free variables, subject to the bound condition. +// Free variables stand for coordinates that are not at the boundary in xcp, +// the generalized Cauchy point. +// +// In the classical implementation of L-BFGS-B [1], the minimization is done by first +// ignoring the box constraints, followed by a line search. Our implementation is +// an exact minimization subject to the bounds, based on the BOXCQP algorithm [2]. +// +// Reference: +// [1] R. H. Byrd, P. Lu, and J. Nocedal (1995). A limited memory algorithm for bound constrained optimization. +// [2] C. Voglis and I. E. Lagaris (2004). BOXCQP: An algorithm for bound constrained convex quadratic problems. +// +template +class SubspaceMin +{ +private: + using Vector = Eigen::Matrix; + using Matrix = Eigen::Matrix; + using IndexSet = std::vector; + + // v[ind] + static Vector subvec(const Vector& v, const IndexSet& ind) + { + const int nsub = ind.size(); + Vector res(nsub); + for (int i = 0; i < nsub; i++) + res[i] = v[ind[i]]; + return res; + } + + // v[ind] = rhs + static void subvec_assign(Vector& v, const IndexSet& ind, const Vector& rhs) + { + const int nsub = ind.size(); + for (int i = 0; i < nsub; i++) + v[ind[i]] = rhs[i]; + } + + // Check whether the vector is within the bounds + static bool in_bounds(const Vector& x, const Vector& lb, const Vector& ub) + { + const int n = x.size(); + for (int i = 0; i < n; i++) + { + if (x[i] < lb[i] || x[i] > ub[i]) + return false; + } + return true; + } + + // Test convergence of P set + static bool P_converged(const IndexSet& yP_set, const Vector& vecy, const Vector& vecl, const Vector& vecu) + { + const int nP = yP_set.size(); + for (int i = 0; i < nP; i++) + { + const int coord = yP_set[i]; + if (vecy[coord] < vecl[coord] || vecy[coord] > vecu[coord]) + return false; + } + return true; + } + + // Test convergence of L set + static bool L_converged(const IndexSet& yL_set, const Vector& lambda) + { + const int nL = yL_set.size(); + for (int i = 0; i < nL; i++) + { + const int coord = yL_set[i]; + if (lambda[coord] < Scalar(0)) + return false; + } + return true; + } + + // Test convergence of L set + static bool U_converged(const IndexSet& yU_set, const Vector& mu) + { + const int nU = yU_set.size(); + for (int i = 0; i < nU; i++) + { + const int coord = yU_set[i]; + if (mu[coord] < Scalar(0)) + return false; + } + return true; + } + +public: + // bfgs: An object that represents the BFGS approximation matrix. + // x0: Current parameter vector. + // xcp: Computed generalized Cauchy point. + // g: Gradient at x0. + // lb: Lower bounds for x. + // ub: Upper bounds for x. + // Wd: W'(xcp - x0) + // newact_set: Coordinates that newly become active during the GCP procedure. + // fv_set: Free variable set. + // maxit: Maximum number of iterations. + // drt: The output direction vector, drt = xsm - x0. + static void subspace_minimize( + const BFGSMat& bfgs, const Vector& x0, const Vector& xcp, const Vector& g, + const Vector& lb, const Vector& ub, const Vector& Wd, const IndexSet& newact_set, const IndexSet& fv_set, int maxit, + Vector& drt) + { + // std::cout << "========================= Entering subspace minimization =========================\n\n"; + + // d = xcp - x0 + drt.noalias() = xcp - x0; + // Size of free variables + const int nfree = fv_set.size(); + // If there is no free variable, simply return drt + if (nfree < 1) + { + // std::cout << "========================= (Early) leaving subspace minimization =========================\n\n"; + return; + } + + // std::cout << "New active set = [ "; for(std::size_t i = 0; i < newact_set.size(); i++) std::cout << newact_set[i] << " "; std::cout << "]\n"; + // std::cout << "Free variable set = [ "; for(std::size_t i = 0; i < fv_set.size(); i++) std::cout << fv_set[i] << " "; std::cout << "]\n\n"; + + // Extract the rows of W in the free variable set + Matrix WF = bfgs.Wb(fv_set); + // Compute F'BAb = -F'WMW'AA'd + Vector vecc(nfree); + bfgs.compute_FtBAb(WF, fv_set, newact_set, Wd, drt, vecc); + // Set the vector c=F'BAb+F'g for linear term, and vectors l and u for the new bounds + Vector vecl(nfree), vecu(nfree); + for (int i = 0; i < nfree; i++) + { + const int coord = fv_set[i]; + vecl[i] = lb[coord] - x0[coord]; + vecu[i] = ub[coord] - x0[coord]; + vecc[i] += g[coord]; + } + // Solve y = -inv(B[F, F]) * c + Vector vecy(nfree); + bfgs.solve_PtBP(WF, -vecc, vecy); + // Test feasibility + // If yes, then the solution has been found + if (in_bounds(vecy, vecl, vecu)) + { + subvec_assign(drt, fv_set, vecy); + return; + } + // Otherwise, enter the iterations + + // Make a copy of y as a fallback solution + Vector yfallback = vecy; + // Dual variables + Vector lambda = Vector::Zero(nfree), mu = Vector::Zero(nfree); + + // Iterations + IndexSet L_set, U_set, P_set, yL_set, yU_set, yP_set; + L_set.reserve(nfree / 3); + yL_set.reserve(nfree / 3); + U_set.reserve(nfree / 3); + yU_set.reserve(nfree / 3); + P_set.reserve(nfree); + yP_set.reserve(nfree); + int k; + for (k = 0; k < maxit; k++) + { + // Construct the L, U, and P sets, and then update values + // Indices in original drt vector + L_set.clear(); + U_set.clear(); + P_set.clear(); + // Indices in y + yL_set.clear(); + yU_set.clear(); + yP_set.clear(); + for (int i = 0; i < nfree; i++) + { + const int coord = fv_set[i]; + const Scalar li = vecl[i], ui = vecu[i]; + if ((vecy[i] < li) || (vecy[i] == li && lambda[i] >= Scalar(0))) + { + L_set.push_back(coord); + yL_set.push_back(i); + vecy[i] = li; + mu[i] = Scalar(0); + } + else if ((vecy[i] > ui) || (vecy[i] == ui && mu[i] >= Scalar(0))) + { + U_set.push_back(coord); + yU_set.push_back(i); + vecy[i] = ui; + lambda[i] = Scalar(0); + } + else + { + P_set.push_back(coord); + yP_set.push_back(i); + lambda[i] = Scalar(0); + mu[i] = Scalar(0); + } + } + + /* std::cout << "** Iter " << k << " **\n"; + std::cout << " L = [ "; for(std::size_t i = 0; i < L_set.size(); i++) std::cout << L_set[i] << " "; std::cout << "]\n"; + std::cout << " U = [ "; for(std::size_t i = 0; i < U_set.size(); i++) std::cout << U_set[i] << " "; std::cout << "]\n"; + std::cout << " P = [ "; for(std::size_t i = 0; i < P_set.size(); i++) std::cout << P_set[i] << " "; std::cout << "]\n\n"; */ + + // Extract the rows of W in the P set + Matrix WP = bfgs.Wb(P_set); + // Solve y[P] = -inv(B[P, P]) * (B[P, L] * l[L] + B[P, U] * u[U] + c[P]) + const int nP = P_set.size(); + if (nP > 0) + { + Vector rhs = subvec(vecc, yP_set); + Vector lL = subvec(vecl, yL_set); + Vector uU = subvec(vecu, yU_set); + Vector tmp(nP); + bool nonzero = bfgs.apply_PtBQv(WP, L_set, lL, tmp, true); + if (nonzero) + rhs.noalias() += tmp; + nonzero = bfgs.apply_PtBQv(WP, U_set, uU, tmp, true); + if (nonzero) + rhs.noalias() += tmp; + + bfgs.solve_PtBP(WP, -rhs, tmp); + subvec_assign(vecy, yP_set, tmp); + } + + // Solve lambda[L] = B[L, F] * y + c[L] + const int nL = L_set.size(); + const int nU = U_set.size(); + Vector Fy; + if (nL > 0 || nU > 0) + bfgs.apply_WtPv(fv_set, vecy, Fy); + if (nL > 0) + { + Vector res; + bfgs.apply_PtWMv(L_set, Fy, res, Scalar(-1)); + res.noalias() += subvec(vecc, yL_set) + bfgs.theta() * subvec(vecy, yL_set); + subvec_assign(lambda, yL_set, res); + } + + // Solve mu[U] = -B[U, F] * y - c[U] + if (nU > 0) + { + Vector negRes; + bfgs.apply_PtWMv(U_set, Fy, negRes, Scalar(-1)); + negRes.noalias() += subvec(vecc, yU_set) + bfgs.theta() * subvec(vecy, yU_set); + subvec_assign(mu, yU_set, -negRes); + } + + // Test convergence + if (L_converged(yL_set, lambda) && U_converged(yU_set, mu) && P_converged(yP_set, vecy, vecl, vecu)) + break; + } + + // If the iterations do not converge, try the projection + if (k >= maxit) + { + vecy.noalias() = vecy.cwiseMax(vecl).cwiseMin(vecu); + subvec_assign(drt, fv_set, vecy); + // Test whether drt is a descent direction + Scalar dg = drt.dot(g); + // If yes, return the result + if (dg <= -std::numeric_limits::epsilon()) + return; + + // If not, fall back to the projected unconstrained solution + vecy.noalias() = yfallback.cwiseMax(vecl).cwiseMin(vecu); + subvec_assign(drt, fv_set, vecy); + dg = drt.dot(g); + if (dg <= -std::numeric_limits::epsilon()) + return; + + // If still not, fall back to the unconstrained solution + subvec_assign(drt, fv_set, yfallback); + return; + } + + // std::cout << "** Minimization finished in " << k + 1 << " iteration(s) **\n\n"; + // std::cout << "========================= Leaving subspace minimization =========================\n\n"; + + subvec_assign(drt, fv_set, vecy); + } +}; + +} // namespace LBFGSpp + +/// \endcond + +#endif // LBFGSPP_SUBSPACE_MIN_H diff --git a/src/prm_vision/opencv_armor_detector/include/OpenCVArmorDetector.h b/src/prm_vision/opencv_armor_detector/include/OpenCVArmorDetector.h index 74ee99a..7b92ccc 100644 --- a/src/prm_vision/opencv_armor_detector/include/OpenCVArmorDetector.h +++ b/src/prm_vision/opencv_armor_detector/include/OpenCVArmorDetector.h @@ -4,7 +4,9 @@ #include #include "rclcpp/rclcpp.hpp" +#include "LBFGSB.h" #include +#include // Detector Constants #define LIGHT_BAR_ANGLE_LIMIT 10.0 diff --git a/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp b/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp index 6e12112..be799af 100644 --- a/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp +++ b/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp @@ -4,6 +4,17 @@ #include #include "OpenCVArmorDetector.h" +void showDebugWindow(cv::Mat frame, std::vector image_points); +std::vector reproject_points(double theta_x, cv::Mat_ tvec); +cv::Mat rotation_matrix(double theta_x, double theta_y, double theta_z); +double gradient_wrt_yaw_finitediff(double yaw, std::vector image_points, cv::Mat_ tvec); +double compute_loss(double theta_x, std::vector image_points, cv::Mat_ tvec); +double optimize_yaw(double theta_x, std::vector image_points, cv::Mat_ tvec); +cv::Mat rot2euler(const cv::Mat &rotationMatrix); + +using Eigen::VectorXd; +using namespace LBFGSpp; + void OpenCVArmorDetector::setConfig(DetectorConfig config) { _config = config; @@ -66,7 +77,7 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame) std::to_string(_frame_count) + " (" + std::to_string(_detected_frame * 100 / _frame_count) + "%) and missed: " + std::to_string(_missed_frames) + std::string(" frames")); - cv::waitKey(1000); + cv::waitKey(30); #endif // If we didn't find an armor for a few frames (ROS2 param), reset the search area @@ -82,12 +93,17 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame) { // We found an armor, so reset the missed frames and return the keypoints _missed_frames = 0; + std::vector image_points; for (int i = 0; i < 4; i++) { detected_keypoints[i * 2] = points.at(i).x + _search_area[0]; detected_keypoints[i * 2 + 1] = points.at(i).y + _search_area[1]; + + image_points.emplace_back(cv::Point2f(points.at(i).x + _search_area[0], points.at(i).y + _search_area[1])); } + // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), " [*] Detected Armor: (%.2f, %.2f), (%.2f, %.2f), (%.2f, %.2f), (%.2f, %.2f)", image_points.at(0).x, image_points.at(0).y, image_points.at(1).x, image_points.at(1).y, image_points.at(2).x, image_points.at(2).y, image_points.at(3).x, image_points.at(3).y); + if (_reduce_search_area) { // Change the search area to the bounding box of the armor with a 50 pixel buffer @@ -102,6 +118,8 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame) _search_area[3] = std::min(y_max + 50, HEIGHT); } _detected_frame++; + + showDebugWindow(croppedFrame, image_points); } return detected_keypoints; @@ -172,6 +190,8 @@ std::vector OpenCVArmorDetector::detectArmorsInFrame(cv::Mat &frame rect.angle -= 90; } + // cv::RotatedRect rect = cv::fitEllipse(contour); + if (isLightBar(rect)) { light_bar_candidates.push_back(rect); @@ -344,4 +364,283 @@ std::vector OpenCVArmorDetector::rectToPoint(cv::RotatedRect &rect) points.push_back(cv::Point2f(int(rect.center.x + x_offset), int(rect.center.y - y_offset))); points.push_back(cv::Point2f(int(rect.center.x - x_offset), int(rect.center.y + y_offset))); return points; -} \ No newline at end of file +} + +void showDebugWindow(cv::Mat frame, std::vector image_points) +{ + float camera_matrix_[3][3]; + camera_matrix_[0][0] = 1019.108731; + camera_matrix_[0][1] = 0; + camera_matrix_[0][2] = 601.884969; + camera_matrix_[1][0] = 0; + camera_matrix_[1][1] = 1016.784980; + camera_matrix_[1][2] = 521.004587; + camera_matrix_[2][0] = 0; + camera_matrix_[2][1] = 0; + camera_matrix_[2][2] = 1; + auto camera_matrix = cv::Mat(3, 3, CV_32F, &camera_matrix_); + + auto distortion_coefficient = cv::Mat(4, 1, cv::DataType::type); + distortion_coefficient.at(0, 0) = -0.108767; + distortion_coefficient.at(1, 0) = -0.072085; + distortion_coefficient.at(2, 0) = -0.000847; + distortion_coefficient.at(3, 0) = 0.0; + + auto rvec = cv::Mat(3, 1, CV_32F); + auto tvec = cv::Mat(3, 1, CV_32F); + + double SMALL_ARMOR_HALF_WIDTH = 135 / 2.0; + double LIGHTBAR_HALF_HEIGHT = 55 / 2.0; + + std::vector small_armor_object_points; + small_armor_object_points.emplace_back(cv::Point3f(-SMALL_ARMOR_HALF_WIDTH, -LIGHTBAR_HALF_HEIGHT, 0)); // Top Left + small_armor_object_points.emplace_back(cv::Point3f(-SMALL_ARMOR_HALF_WIDTH, LIGHTBAR_HALF_HEIGHT, 0)); // Bot Left + small_armor_object_points.emplace_back(cv::Point3f(SMALL_ARMOR_HALF_WIDTH, -LIGHTBAR_HALF_HEIGHT, 0)); // Top Right + small_armor_object_points.emplace_back(cv::Point3f(SMALL_ARMOR_HALF_WIDTH, LIGHTBAR_HALF_HEIGHT, 0)); // Bot Right + bool PNP_OUT = cv::solvePnP(small_armor_object_points, image_points, camera_matrix, distortion_coefficient, rvec, tvec, false, cv::SOLVEPNP_IPPE); + + std::vector reprojected_points; + cv::projectPoints(small_armor_object_points, rvec, tvec, camera_matrix, distortion_coefficient, reprojected_points); + float error = 0; + for (int i = 0; i < reprojected_points.size(); i++) + { + error += cv::norm(image_points[i] - reprojected_points[i]); + } + + // Draw img and reprojected points centered at 400, 400 + // cv::Mat reprojected_image = cv::Mat::zeros(400, 400, CV_8UC3); + // for (int i = 0; i < reprojected_points.size(); i++) + // { + // cv::circle(reprojected_image, cv::Point(reprojected_points[i].x - 350, reprojected_points[i].y - 500), 1, cv::Scalar(0, 255, 0), -1); + // cv::circle(reprojected_image, cv::Point(image_points[i].x - 350, image_points[i].y - 500), 1, cv::Scalar(0, 0, 255), -1); + // } + + // rvec to rotation matrix + // cv::Mat rotmat; + // cv::Rodrigues(rvec, rotmat); + + // track last yaw + static double optim = 0.0; + + // Guessing yaw + // We use PROJECTED points as ground truth, not image points, since optimizing involves a projection + // So we need to compare error to these + optim = optimize_yaw(optim, reprojected_points, tvec, error); + + // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), " [*] Optimized Yaw: %.2f, Error: %.2f", optim * 180.0 / CV_PI, error); + + // Draw the reprojected points + // cv::Mat reprojected_image_optim = cv::Mat::zeros(400, 400, CV_8UC3); + + // for (int i = 0; i < reprojected_points_optim.size(); i++) + // { + // cv::circle(reprojected_image_optim, cv::Point(reprojected_points_optim[i].x - 350, reprojected_points_optim[i].y - 500), 1, cv::Scalar(0, 255, 0), -1); + // cv::circle(reprojected_image_optim, cv::Point(image_points[i].x - 350, image_points[i].y - 500), 1, cv::Scalar(0, 0, 255), -1); + // } + + // + // Drawing + // + +#ifdef DEBUG + + cv::Mat image = cv::Mat::zeros(800, 800, CV_8UC3); + image.setTo(cv::Scalar(255, 255, 255)); // Set background to white + + // Get Left/Right (x) and Up/Down (y) values from tvec + float x = tvec.at(0, 0); // Left/Right + float y = tvec.at(2, 0); // Forward/back + + // Translate to the center of the image + int centerX = image.cols / 2; + int centerY = image.rows / 2; + + // Scale the tvec values to fit into the image (adjust scale factor as needed) + float scale = 0.45; // Adjust scale to fit the values onto the image + int pointX = centerX + static_cast(x * scale); + int pointY = 400 + centerY - static_cast(y * scale); // Y is inverted for screen coordinates + + // Length of the line + int armor_length = 134; + int length = armor_length * scale; + + // Compute the endpoint using the angle optim(in radians) int x1 = static_cast(pointX + length * cos(optim)); // Calculate x offset + int y1 = static_cast(pointY + length * sin(optim)); // Calculate y offset (invert y) + int x1 = static_cast(pointX + length * cos(optim)); // Calculate x offset + + // Draw the line from (pointX, pointY) to (x1, y1) + cv::line(image, cv::Point(pointX, pointY), cv::Point(x1, y1), cv::Scalar(0, 0, 255), 2); // Red line with thickness 2 + + // draw perpendicular line from the center of the line + // int x2 = static_cast(pointX + length * cos(optim)); // Calculate x offset + // int y2 = static_cast(pointY + length * sin(optim)); // Calculate y offset (invert y) + + // Draw the line from (pointX, pointY) to (x2, y2) + // cv::line(image, cv::Point(pointX, pointY), cv::Point(x2, y2), cv::Scalar(0, 255, 0), 2); // Green line with thickness 2 + + cv::namedWindow("Top-Down View", cv::WINDOW_NORMAL); // Create window with initial title + std::string window_title = "Top-Down View: " + std::to_string(optim * 180.0 / CV_PI); + cv::setWindowTitle("Top-Down View", window_title); // Set the new title + + // Show the image + cv::imshow("Top-Down View", image); + // cv::imshow("Reprojected Image", reprojected_image_optim); + cv::waitKey(1); +#endif +} + +cv::Mat rot2euler(const cv::Mat &rotationMatrix) +{ + cv::Mat euler(3, 1, CV_64F); + + double m00 = rotationMatrix.at(0, 0); + double m02 = rotationMatrix.at(0, 2); + double m10 = rotationMatrix.at(1, 0); + double m11 = rotationMatrix.at(1, 1); + double m12 = rotationMatrix.at(1, 2); + double m20 = rotationMatrix.at(2, 0); + double m22 = rotationMatrix.at(2, 2); + + double bank, attitude, heading; + + // Assuming the angles are in radians. + if (m10 > 0.998) + { // singularity at north pole + bank = 0; + attitude = CV_PI / 2; + heading = atan2(m02, m22); + } + else if (m10 < -0.998) + { // singularity at south pole + bank = 0; + attitude = -CV_PI / 2; + heading = atan2(m02, m22); + } + else + { + bank = atan2(-m12, m11); + attitude = asin(m10); + heading = atan2(-m20, m00); + } + + euler.at(0) = bank; + euler.at(1) = attitude; + euler.at(2) = heading; + + return euler; +} + +// +// YAW OPTIMIZER +// + +double gradient_wrt_yaw_finitediff(double yaw, std::vector image_points, cv::Mat_ tvec) +{ + // Loss is strictly convex in yaw, so finite difference is a good approximation to the gradient + double h = 1e-4; + double loss_plus = compute_loss(yaw + h, image_points, tvec); + double loss_minus = compute_loss(yaw - h, image_points, tvec); + double grad = (loss_plus - loss_minus) / (2 * h); + return grad; +} + +class LossFunction +{ +public: + LossFunction(std::vector image_points, cv::Mat_ tvec) : image_points(image_points), tvec(tvec) {} + + double operator()(const VectorXd &x, VectorXd &grad) + { + double yaw = x(0); + double loss = compute_loss(yaw, image_points, tvec); + grad(0) = gradient_wrt_yaw_finitediff(yaw, image_points, tvec); + return loss; + } + +private: + std::vector image_points; + cv::Mat_ tvec; +}; + +double optimize_yaw(double initial_guess, std::vector image_points, cv::Mat_ tvec) +{ + // Optimize yaw using LBFGS Boxed + const int n = 1; + + LBFGSBParam param; + param.epsilon = 0.4; + param.max_iterations = 100; + + LBFGSBSolver solver(param); + LossFunction loss(image_points, tvec); + + // Bounds + VectorXd lb = VectorXd::Constant(n, -CV_PI / 4); + VectorXd ub = VectorXd::Constant(n, CV_PI / 4); + VectorXd x = VectorXd::Constant(n, initial_guess); + + double fx; + int niter = solver.minimize(loss, x, fx, lb, ub); + + return x(0); +} + +double compute_loss(double yaw, std::vector image_points, cv::Mat_ tvec) +{ + std::vector projected_points = reproject_points(yaw, tvec); + + double loss = 0.0; + for (size_t i = 0; i < image_points.size(); ++i) + { + double dx = projected_points[i].x - image_points[i].x; + double dy = projected_points[i].y - image_points[i].y; + loss += 0.25 * dx * dx + dy * dy; // Squared reprojection error + } + + return pow(loss, 0.5); +} + +std::vector reproject_points(double yaw, cv::Mat_ tvec) +{ + // Camera intrinsics + cv::Mat camera_matrix = (cv::Mat_(3, 3) << 1019.109, 0, 601.885, + 0, 1016.785, 521.005, + 0, 0, 1); + cv::Mat dist_coeffs = (cv::Mat_(1, 4) << -0.1088, -0.0721, -0.000847, 0.0); + + // 3D object points (measured armor dimensions) + const double SMALL_ARMOR = 134.0 / 2.0; + const double LIGHTBAR = 54.0 / 2.0; + std::vector object_points = { + {-SMALL_ARMOR, -LIGHTBAR, 0}, // Top Left + {-SMALL_ARMOR, LIGHTBAR, 0}, // Bot Left + {SMALL_ARMOR, -LIGHTBAR, 0}, // Top Right + {SMALL_ARMOR, LIGHTBAR, 0} // Bot Right + }; + + // Compute rvec + cv::Mat R = rotation_matrix(0.0 * CV_PI / 180.0, yaw, 0.0); + cv::Mat rvec; + cv::Rodrigues(R, rvec); + + // Reproject points into image space + std::vector projected_points; + cv::projectPoints(object_points, rvec, tvec, camera_matrix, dist_coeffs, projected_points); + + return projected_points; +} + +cv::Mat rotation_matrix(double theta_x, double theta_y, double theta_z) +{ + cv::Mat Rz = (cv::Mat_(3, 3) << cos(theta_z), -sin(theta_z), 0, + sin(theta_z), cos(theta_z), 0, + 0, 0, 1); + cv::Mat Ry = (cv::Mat_(3, 3) << cos(theta_y), 0, sin(theta_y), + 0, 1, 0, + -sin(theta_y), 0, cos(theta_y)); + cv::Mat Rx = (cv::Mat_(3, 3) << 1, 0, 0, + 0, cos(theta_x), -sin(theta_x), + 0, sin(theta_x), cos(theta_x)); + cv::Mat R = Rz * Ry * Rx; + return R; +} From bd74b19b761ac794fc62e842e015c56e1317d4ec Mon Sep 17 00:00:00 2001 From: Tom_TKO Date: Mon, 30 Dec 2024 14:44:50 -0500 Subject: [PATCH 2/7] add layout for PoseEstimator node using L-BFGS-B. Reworking ValidityFilter --- src/prm_launch/launch/video2detector.py | 2 +- .../include/OpenCVArmorDetector.h | 6 +- .../src/OpenCVArmorDetector.cpp | 311 +----------------- .../src/OpenCVArmorDetectorNode.cpp | 16 +- src/prm_vision/pose_estimator/CMakeLists.txt | 50 +++ .../include/LBFGS.h | 0 .../include/LBFGSB.h | 0 .../include/LBFGSpp/BFGSMat.h | 0 .../include/LBFGSpp/BKLDLT.h | 0 .../include/LBFGSpp/Cauchy.h | 0 .../include/LBFGSpp/LineSearchBacktracking.h | 0 .../include/LBFGSpp/LineSearchBracketing.h | 0 .../include/LBFGSpp/LineSearchMoreThuente.h | 0 .../include/LBFGSpp/LineSearchNocedalWright.h | 0 .../include/LBFGSpp/Param.h | 0 .../include/LBFGSpp/SubspaceMin.h | 0 .../pose_estimator/include/PoseEstimator.h | 108 ++++++ .../include/PoseEstimatorNode.hpp | 40 +++ .../pose_estimator/include/ValidityFilter.hpp | 56 ++++ src/prm_vision/pose_estimator/package.xml | 22 ++ .../pose_estimator/src/PoseEstimator.cpp | 163 +++++++++ .../pose_estimator/src/PoseEstimatorNode.cpp | 75 +++++ .../pose_estimator/src/ValidityFilter.cpp | 127 +++++++ .../test/test_PoseEstimator.cpp | 0 src/prm_vision/vision_msgs/msg/KeyPoints.msg | 2 +- 25 files changed, 652 insertions(+), 326 deletions(-) create mode 100644 src/prm_vision/pose_estimator/CMakeLists.txt rename src/prm_vision/{opencv_armor_detector => pose_estimator}/include/LBFGS.h (100%) rename src/prm_vision/{opencv_armor_detector => pose_estimator}/include/LBFGSB.h (100%) rename src/prm_vision/{opencv_armor_detector => pose_estimator}/include/LBFGSpp/BFGSMat.h (100%) rename src/prm_vision/{opencv_armor_detector => pose_estimator}/include/LBFGSpp/BKLDLT.h (100%) rename src/prm_vision/{opencv_armor_detector => pose_estimator}/include/LBFGSpp/Cauchy.h (100%) rename src/prm_vision/{opencv_armor_detector => pose_estimator}/include/LBFGSpp/LineSearchBacktracking.h (100%) rename src/prm_vision/{opencv_armor_detector => pose_estimator}/include/LBFGSpp/LineSearchBracketing.h (100%) rename src/prm_vision/{opencv_armor_detector => pose_estimator}/include/LBFGSpp/LineSearchMoreThuente.h (100%) rename src/prm_vision/{opencv_armor_detector => pose_estimator}/include/LBFGSpp/LineSearchNocedalWright.h (100%) rename src/prm_vision/{opencv_armor_detector => pose_estimator}/include/LBFGSpp/Param.h (100%) rename src/prm_vision/{opencv_armor_detector => pose_estimator}/include/LBFGSpp/SubspaceMin.h (100%) create mode 100644 src/prm_vision/pose_estimator/include/PoseEstimator.h create mode 100644 src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp create mode 100644 src/prm_vision/pose_estimator/include/ValidityFilter.hpp create mode 100644 src/prm_vision/pose_estimator/package.xml create mode 100644 src/prm_vision/pose_estimator/src/PoseEstimator.cpp create mode 100644 src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp create mode 100644 src/prm_vision/pose_estimator/src/ValidityFilter.cpp create mode 100644 src/prm_vision/pose_estimator/test/test_PoseEstimator.cpp diff --git a/src/prm_launch/launch/video2detector.py b/src/prm_launch/launch/video2detector.py index 031a805..3a7c090 100755 --- a/src/prm_launch/launch/video2detector.py +++ b/src/prm_launch/launch/video2detector.py @@ -15,7 +15,7 @@ def generate_launch_description(): emulate_tty=True, executable='VideoCaptureNode', parameters=[{'source': str(video_path), - 'fps': 150, + 'fps': 200, 'frame_id': 'video', }] ), diff --git a/src/prm_vision/opencv_armor_detector/include/OpenCVArmorDetector.h b/src/prm_vision/opencv_armor_detector/include/OpenCVArmorDetector.h index 7b92ccc..0e41403 100644 --- a/src/prm_vision/opencv_armor_detector/include/OpenCVArmorDetector.h +++ b/src/prm_vision/opencv_armor_detector/include/OpenCVArmorDetector.h @@ -2,11 +2,9 @@ #define _OPENCVARMORDETECTOR_HPP #include - -#include "rclcpp/rclcpp.hpp" -#include "LBFGSB.h" #include -#include +#include +#include // Detector Constants #define LIGHT_BAR_ANGLE_LIMIT 10.0 diff --git a/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp b/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp index be799af..6555a9d 100644 --- a/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp +++ b/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp @@ -1,20 +1,5 @@ -#include -#include - -#include #include "OpenCVArmorDetector.h" -void showDebugWindow(cv::Mat frame, std::vector image_points); -std::vector reproject_points(double theta_x, cv::Mat_ tvec); -cv::Mat rotation_matrix(double theta_x, double theta_y, double theta_z); -double gradient_wrt_yaw_finitediff(double yaw, std::vector image_points, cv::Mat_ tvec); -double compute_loss(double theta_x, std::vector image_points, cv::Mat_ tvec); -double optimize_yaw(double theta_x, std::vector image_points, cv::Mat_ tvec); -cv::Mat rot2euler(const cv::Mat &rotationMatrix); - -using Eigen::VectorXd; -using namespace LBFGSpp; - void OpenCVArmorDetector::setConfig(DetectorConfig config) { _config = config; @@ -52,17 +37,6 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame) // Detect the armor in the cropped frame std::vector points = detectArmorsInFrame(croppedFrame); - // Print FPS every 500 frames - if (_frame_count % 500 == 0 && _frame_count != 0) - { - // Calculate and print FPS - auto current_time = std::chrono::steady_clock::now(); - double elapsed_time = std::chrono::duration_cast(current_time - last_time).count(); - last_time = current_time; - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), " [*] Detecting Armor (%d) FPS: %.2f", _frame_count, 500.0 / (elapsed_time / 1000.0)); - } - _frame_count++; - // Display the cropped frame for debugging #ifdef DEBUG cv::resize(croppedFrame, croppedFrame, cv::Size(WIDTH / 2, HEIGHT / 2)); @@ -71,7 +45,7 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame) const std::string window_name = "Detection Results"; cv::imshow(window_name, croppedFrame); - // Update the window title (OpenCV >= 4.5) + // Update the window title cv::setWindowTitle(window_name, "detected: " + std::to_string(_detected_frame) + " / " + std::to_string(_frame_count) + " (" + @@ -118,8 +92,6 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame) _search_area[3] = std::min(y_max + 50, HEIGHT); } _detected_frame++; - - showDebugWindow(croppedFrame, image_points); } return detected_keypoints; @@ -364,283 +336,4 @@ std::vector OpenCVArmorDetector::rectToPoint(cv::RotatedRect &rect) points.push_back(cv::Point2f(int(rect.center.x + x_offset), int(rect.center.y - y_offset))); points.push_back(cv::Point2f(int(rect.center.x - x_offset), int(rect.center.y + y_offset))); return points; -} - -void showDebugWindow(cv::Mat frame, std::vector image_points) -{ - float camera_matrix_[3][3]; - camera_matrix_[0][0] = 1019.108731; - camera_matrix_[0][1] = 0; - camera_matrix_[0][2] = 601.884969; - camera_matrix_[1][0] = 0; - camera_matrix_[1][1] = 1016.784980; - camera_matrix_[1][2] = 521.004587; - camera_matrix_[2][0] = 0; - camera_matrix_[2][1] = 0; - camera_matrix_[2][2] = 1; - auto camera_matrix = cv::Mat(3, 3, CV_32F, &camera_matrix_); - - auto distortion_coefficient = cv::Mat(4, 1, cv::DataType::type); - distortion_coefficient.at(0, 0) = -0.108767; - distortion_coefficient.at(1, 0) = -0.072085; - distortion_coefficient.at(2, 0) = -0.000847; - distortion_coefficient.at(3, 0) = 0.0; - - auto rvec = cv::Mat(3, 1, CV_32F); - auto tvec = cv::Mat(3, 1, CV_32F); - - double SMALL_ARMOR_HALF_WIDTH = 135 / 2.0; - double LIGHTBAR_HALF_HEIGHT = 55 / 2.0; - - std::vector small_armor_object_points; - small_armor_object_points.emplace_back(cv::Point3f(-SMALL_ARMOR_HALF_WIDTH, -LIGHTBAR_HALF_HEIGHT, 0)); // Top Left - small_armor_object_points.emplace_back(cv::Point3f(-SMALL_ARMOR_HALF_WIDTH, LIGHTBAR_HALF_HEIGHT, 0)); // Bot Left - small_armor_object_points.emplace_back(cv::Point3f(SMALL_ARMOR_HALF_WIDTH, -LIGHTBAR_HALF_HEIGHT, 0)); // Top Right - small_armor_object_points.emplace_back(cv::Point3f(SMALL_ARMOR_HALF_WIDTH, LIGHTBAR_HALF_HEIGHT, 0)); // Bot Right - bool PNP_OUT = cv::solvePnP(small_armor_object_points, image_points, camera_matrix, distortion_coefficient, rvec, tvec, false, cv::SOLVEPNP_IPPE); - - std::vector reprojected_points; - cv::projectPoints(small_armor_object_points, rvec, tvec, camera_matrix, distortion_coefficient, reprojected_points); - float error = 0; - for (int i = 0; i < reprojected_points.size(); i++) - { - error += cv::norm(image_points[i] - reprojected_points[i]); - } - - // Draw img and reprojected points centered at 400, 400 - // cv::Mat reprojected_image = cv::Mat::zeros(400, 400, CV_8UC3); - // for (int i = 0; i < reprojected_points.size(); i++) - // { - // cv::circle(reprojected_image, cv::Point(reprojected_points[i].x - 350, reprojected_points[i].y - 500), 1, cv::Scalar(0, 255, 0), -1); - // cv::circle(reprojected_image, cv::Point(image_points[i].x - 350, image_points[i].y - 500), 1, cv::Scalar(0, 0, 255), -1); - // } - - // rvec to rotation matrix - // cv::Mat rotmat; - // cv::Rodrigues(rvec, rotmat); - - // track last yaw - static double optim = 0.0; - - // Guessing yaw - // We use PROJECTED points as ground truth, not image points, since optimizing involves a projection - // So we need to compare error to these - optim = optimize_yaw(optim, reprojected_points, tvec, error); - - // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), " [*] Optimized Yaw: %.2f, Error: %.2f", optim * 180.0 / CV_PI, error); - - // Draw the reprojected points - // cv::Mat reprojected_image_optim = cv::Mat::zeros(400, 400, CV_8UC3); - - // for (int i = 0; i < reprojected_points_optim.size(); i++) - // { - // cv::circle(reprojected_image_optim, cv::Point(reprojected_points_optim[i].x - 350, reprojected_points_optim[i].y - 500), 1, cv::Scalar(0, 255, 0), -1); - // cv::circle(reprojected_image_optim, cv::Point(image_points[i].x - 350, image_points[i].y - 500), 1, cv::Scalar(0, 0, 255), -1); - // } - - // - // Drawing - // - -#ifdef DEBUG - - cv::Mat image = cv::Mat::zeros(800, 800, CV_8UC3); - image.setTo(cv::Scalar(255, 255, 255)); // Set background to white - - // Get Left/Right (x) and Up/Down (y) values from tvec - float x = tvec.at(0, 0); // Left/Right - float y = tvec.at(2, 0); // Forward/back - - // Translate to the center of the image - int centerX = image.cols / 2; - int centerY = image.rows / 2; - - // Scale the tvec values to fit into the image (adjust scale factor as needed) - float scale = 0.45; // Adjust scale to fit the values onto the image - int pointX = centerX + static_cast(x * scale); - int pointY = 400 + centerY - static_cast(y * scale); // Y is inverted for screen coordinates - - // Length of the line - int armor_length = 134; - int length = armor_length * scale; - - // Compute the endpoint using the angle optim(in radians) int x1 = static_cast(pointX + length * cos(optim)); // Calculate x offset - int y1 = static_cast(pointY + length * sin(optim)); // Calculate y offset (invert y) - int x1 = static_cast(pointX + length * cos(optim)); // Calculate x offset - - // Draw the line from (pointX, pointY) to (x1, y1) - cv::line(image, cv::Point(pointX, pointY), cv::Point(x1, y1), cv::Scalar(0, 0, 255), 2); // Red line with thickness 2 - - // draw perpendicular line from the center of the line - // int x2 = static_cast(pointX + length * cos(optim)); // Calculate x offset - // int y2 = static_cast(pointY + length * sin(optim)); // Calculate y offset (invert y) - - // Draw the line from (pointX, pointY) to (x2, y2) - // cv::line(image, cv::Point(pointX, pointY), cv::Point(x2, y2), cv::Scalar(0, 255, 0), 2); // Green line with thickness 2 - - cv::namedWindow("Top-Down View", cv::WINDOW_NORMAL); // Create window with initial title - std::string window_title = "Top-Down View: " + std::to_string(optim * 180.0 / CV_PI); - cv::setWindowTitle("Top-Down View", window_title); // Set the new title - - // Show the image - cv::imshow("Top-Down View", image); - // cv::imshow("Reprojected Image", reprojected_image_optim); - cv::waitKey(1); -#endif -} - -cv::Mat rot2euler(const cv::Mat &rotationMatrix) -{ - cv::Mat euler(3, 1, CV_64F); - - double m00 = rotationMatrix.at(0, 0); - double m02 = rotationMatrix.at(0, 2); - double m10 = rotationMatrix.at(1, 0); - double m11 = rotationMatrix.at(1, 1); - double m12 = rotationMatrix.at(1, 2); - double m20 = rotationMatrix.at(2, 0); - double m22 = rotationMatrix.at(2, 2); - - double bank, attitude, heading; - - // Assuming the angles are in radians. - if (m10 > 0.998) - { // singularity at north pole - bank = 0; - attitude = CV_PI / 2; - heading = atan2(m02, m22); - } - else if (m10 < -0.998) - { // singularity at south pole - bank = 0; - attitude = -CV_PI / 2; - heading = atan2(m02, m22); - } - else - { - bank = atan2(-m12, m11); - attitude = asin(m10); - heading = atan2(-m20, m00); - } - - euler.at(0) = bank; - euler.at(1) = attitude; - euler.at(2) = heading; - - return euler; -} - -// -// YAW OPTIMIZER -// - -double gradient_wrt_yaw_finitediff(double yaw, std::vector image_points, cv::Mat_ tvec) -{ - // Loss is strictly convex in yaw, so finite difference is a good approximation to the gradient - double h = 1e-4; - double loss_plus = compute_loss(yaw + h, image_points, tvec); - double loss_minus = compute_loss(yaw - h, image_points, tvec); - double grad = (loss_plus - loss_minus) / (2 * h); - return grad; -} - -class LossFunction -{ -public: - LossFunction(std::vector image_points, cv::Mat_ tvec) : image_points(image_points), tvec(tvec) {} - - double operator()(const VectorXd &x, VectorXd &grad) - { - double yaw = x(0); - double loss = compute_loss(yaw, image_points, tvec); - grad(0) = gradient_wrt_yaw_finitediff(yaw, image_points, tvec); - return loss; - } - -private: - std::vector image_points; - cv::Mat_ tvec; -}; - -double optimize_yaw(double initial_guess, std::vector image_points, cv::Mat_ tvec) -{ - // Optimize yaw using LBFGS Boxed - const int n = 1; - - LBFGSBParam param; - param.epsilon = 0.4; - param.max_iterations = 100; - - LBFGSBSolver solver(param); - LossFunction loss(image_points, tvec); - - // Bounds - VectorXd lb = VectorXd::Constant(n, -CV_PI / 4); - VectorXd ub = VectorXd::Constant(n, CV_PI / 4); - VectorXd x = VectorXd::Constant(n, initial_guess); - - double fx; - int niter = solver.minimize(loss, x, fx, lb, ub); - - return x(0); -} - -double compute_loss(double yaw, std::vector image_points, cv::Mat_ tvec) -{ - std::vector projected_points = reproject_points(yaw, tvec); - - double loss = 0.0; - for (size_t i = 0; i < image_points.size(); ++i) - { - double dx = projected_points[i].x - image_points[i].x; - double dy = projected_points[i].y - image_points[i].y; - loss += 0.25 * dx * dx + dy * dy; // Squared reprojection error - } - - return pow(loss, 0.5); -} - -std::vector reproject_points(double yaw, cv::Mat_ tvec) -{ - // Camera intrinsics - cv::Mat camera_matrix = (cv::Mat_(3, 3) << 1019.109, 0, 601.885, - 0, 1016.785, 521.005, - 0, 0, 1); - cv::Mat dist_coeffs = (cv::Mat_(1, 4) << -0.1088, -0.0721, -0.000847, 0.0); - - // 3D object points (measured armor dimensions) - const double SMALL_ARMOR = 134.0 / 2.0; - const double LIGHTBAR = 54.0 / 2.0; - std::vector object_points = { - {-SMALL_ARMOR, -LIGHTBAR, 0}, // Top Left - {-SMALL_ARMOR, LIGHTBAR, 0}, // Bot Left - {SMALL_ARMOR, -LIGHTBAR, 0}, // Top Right - {SMALL_ARMOR, LIGHTBAR, 0} // Bot Right - }; - - // Compute rvec - cv::Mat R = rotation_matrix(0.0 * CV_PI / 180.0, yaw, 0.0); - cv::Mat rvec; - cv::Rodrigues(R, rvec); - - // Reproject points into image space - std::vector projected_points; - cv::projectPoints(object_points, rvec, tvec, camera_matrix, dist_coeffs, projected_points); - - return projected_points; -} - -cv::Mat rotation_matrix(double theta_x, double theta_y, double theta_z) -{ - cv::Mat Rz = (cv::Mat_(3, 3) << cos(theta_z), -sin(theta_z), 0, - sin(theta_z), cos(theta_z), 0, - 0, 0, 1); - cv::Mat Ry = (cv::Mat_(3, 3) << cos(theta_y), 0, sin(theta_y), - 0, 1, 0, - -sin(theta_y), 0, cos(theta_y)); - cv::Mat Rx = (cv::Mat_(3, 3) << 1, 0, 0, - 0, cos(theta_x), -sin(theta_x), - 0, sin(theta_x), cos(theta_x)); - cv::Mat R = Rz * Ry * Rx; - return R; -} +} \ No newline at end of file diff --git a/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetectorNode.cpp b/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetectorNode.cpp index 68e8f3d..6f85f93 100644 --- a/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetectorNode.cpp +++ b/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetectorNode.cpp @@ -1,8 +1,6 @@ #include "OpenCVArmorDetectorNode.hpp" -OpenCVArmorDetectorNode::OpenCVArmorDetectorNode( - const rclcpp::NodeOptions &options) - : Node("opencv_armor_detector", options) +OpenCVArmorDetectorNode::OpenCVArmorDetectorNode(const rclcpp::NodeOptions &options) : Node("opencv_armor_detector", options) { RCLCPP_INFO(get_logger(), "OpenCVArmorDetectorNode has been started."); @@ -34,9 +32,7 @@ void OpenCVArmorDetectorNode::imageTransportInitilization() std::placeholders::_1)); } -rcl_interfaces::msg::SetParametersResult -OpenCVArmorDetectorNode::parameters_callback( - const std::vector ¶meters) +rcl_interfaces::msg::SetParametersResult OpenCVArmorDetectorNode::parameters_callback(const std::vector ¶meters) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -97,14 +93,12 @@ void OpenCVArmorDetectorNode::imageCallback( vision_msgs::msg::KeyPoints keypoints_msg; std::array points_array; std::copy(points.begin(), points.end(), points_array.begin()); - float h = std::min(cv::norm(points.at(1) - points.at(0)), - cv::norm(points.at(3) - points.at(2))); - float w = cv::norm((points.at(0) + points.at(1)) / 2 - - (points.at(2) + points.at(3)) / 2); + float h = std::min(cv::norm(points.at(1) - points.at(0)), cv::norm(points.at(3) - points.at(2))); + float w = cv::norm((points.at(0) + points.at(1)) / 2 - (points.at(2) + points.at(3)) / 2); keypoints_msg.header = image_msg->header; keypoints_msg.points = points_array; - keypoints_msg.large_armor = (w / h) > 3; // 3 is the width ratio threshold before it is considered a large armor + keypoints_msg.is_large_armor = (w / h) > 3; // 3 is the width ratio threshold before it is considered a large armor // Publish the message keypoints_publisher->publish(keypoints_msg); diff --git a/src/prm_vision/pose_estimator/CMakeLists.txt b/src/prm_vision/pose_estimator/CMakeLists.txt new file mode 100644 index 0000000..7c06725 --- /dev/null +++ b/src/prm_vision/pose_estimator/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.8) +project(pose_estimator CXX) +set(CMAKE_CXX_STANDARD 17) # for filesystem support + +# Dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(vision_msgs REQUIRED) +find_package(OpenCV 4.6.0 REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(ament_cmake_gtest REQUIRED) # GTest +find_package(Eigen3 REQUIRED) # Eigen3 + +# Include directories +include_directories(/usr/include) +include_directories(include) + +# Build target +add_library(PoseEstimator STATIC src/PoseEstimator.cpp) +target_link_libraries(PoseEstimator ${OpenCV_LIBS} Eigen3::Eigen) + +add_library(ValidityFilter STATIC src/ValidityFilter.cpp) + +add_executable(PoseEstimatorNode src/PoseEstimatorNode.cpp) +target_link_libraries(PoseEstimatorNode PoseEstimator ValidityFilter ${OpenCV_LIBS}) + +ament_target_dependencies(PoseEstimatorNode + rclcpp + sensor_msgs + cv_bridge + vision_msgs + Eigen3 +) + +ament_target_dependencies(PoseEstimator + rclcpp +) + +# GTest setup +ament_add_gtest(test_PoseEstimator test/test_PoseEstimator.cpp) +target_include_directories(test_PoseEstimator PRIVATE include) +target_link_libraries(test_PoseEstimator PoseEstimator) + +# Install the node +install(TARGETS PoseEstimatorNode + DESTINATION lib/${PROJECT_NAME}) + +ament_export_include_directories(include) +ament_export_dependencies(rclcpp sensor_msgs cv_bridge vision_msgs Eigen3) +ament_package() \ No newline at end of file diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGS.h b/src/prm_vision/pose_estimator/include/LBFGS.h similarity index 100% rename from src/prm_vision/opencv_armor_detector/include/LBFGS.h rename to src/prm_vision/pose_estimator/include/LBFGS.h diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSB.h b/src/prm_vision/pose_estimator/include/LBFGSB.h similarity index 100% rename from src/prm_vision/opencv_armor_detector/include/LBFGSB.h rename to src/prm_vision/pose_estimator/include/LBFGSB.h diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/BFGSMat.h b/src/prm_vision/pose_estimator/include/LBFGSpp/BFGSMat.h similarity index 100% rename from src/prm_vision/opencv_armor_detector/include/LBFGSpp/BFGSMat.h rename to src/prm_vision/pose_estimator/include/LBFGSpp/BFGSMat.h diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/BKLDLT.h b/src/prm_vision/pose_estimator/include/LBFGSpp/BKLDLT.h similarity index 100% rename from src/prm_vision/opencv_armor_detector/include/LBFGSpp/BKLDLT.h rename to src/prm_vision/pose_estimator/include/LBFGSpp/BKLDLT.h diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/Cauchy.h b/src/prm_vision/pose_estimator/include/LBFGSpp/Cauchy.h similarity index 100% rename from src/prm_vision/opencv_armor_detector/include/LBFGSpp/Cauchy.h rename to src/prm_vision/pose_estimator/include/LBFGSpp/Cauchy.h diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchBacktracking.h b/src/prm_vision/pose_estimator/include/LBFGSpp/LineSearchBacktracking.h similarity index 100% rename from src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchBacktracking.h rename to src/prm_vision/pose_estimator/include/LBFGSpp/LineSearchBacktracking.h diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchBracketing.h b/src/prm_vision/pose_estimator/include/LBFGSpp/LineSearchBracketing.h similarity index 100% rename from src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchBracketing.h rename to src/prm_vision/pose_estimator/include/LBFGSpp/LineSearchBracketing.h diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchMoreThuente.h b/src/prm_vision/pose_estimator/include/LBFGSpp/LineSearchMoreThuente.h similarity index 100% rename from src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchMoreThuente.h rename to src/prm_vision/pose_estimator/include/LBFGSpp/LineSearchMoreThuente.h diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchNocedalWright.h b/src/prm_vision/pose_estimator/include/LBFGSpp/LineSearchNocedalWright.h similarity index 100% rename from src/prm_vision/opencv_armor_detector/include/LBFGSpp/LineSearchNocedalWright.h rename to src/prm_vision/pose_estimator/include/LBFGSpp/LineSearchNocedalWright.h diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/Param.h b/src/prm_vision/pose_estimator/include/LBFGSpp/Param.h similarity index 100% rename from src/prm_vision/opencv_armor_detector/include/LBFGSpp/Param.h rename to src/prm_vision/pose_estimator/include/LBFGSpp/Param.h diff --git a/src/prm_vision/opencv_armor_detector/include/LBFGSpp/SubspaceMin.h b/src/prm_vision/pose_estimator/include/LBFGSpp/SubspaceMin.h similarity index 100% rename from src/prm_vision/opencv_armor_detector/include/LBFGSpp/SubspaceMin.h rename to src/prm_vision/pose_estimator/include/LBFGSpp/SubspaceMin.h diff --git a/src/prm_vision/pose_estimator/include/PoseEstimator.h b/src/prm_vision/pose_estimator/include/PoseEstimator.h new file mode 100644 index 0000000..2f5b496 --- /dev/null +++ b/src/prm_vision/pose_estimator/include/PoseEstimator.h @@ -0,0 +1,108 @@ +#ifndef POSE_ESTIMATOR_HPP +#define POSE_ESTIMATOR_HPP + +#include +#include +#include +#include +#include "LBFGSB.h" // L-BFGS-B optimization library for yaw estimation + +// Classes +#include "ValidityFilter.hpp" + +// Constants for armor plates +#define LIGHTBAR_HALF_HEIGHT 54.f / 2.f +#define SMALL_ARMOR_HALF_WIDTH 134.f / 2.f +#define LARGE_ARMOR_HALF_WIDTH 225.f / 2.f + +using Eigen::VectorXd; +using namespace LBFGSpp; + +class PoseEstimator +{ +public: + PoseEstimator() {} + ~PoseEstimator() {} + + // Class methods + double estimateYaw(const double yaw_guess, const std::vector &image_points, const cv::Mat &tvec); + void estimateTranslation(const std::vector &image_points, bool largeArmor, cv::Mat &tvec, cv::Mat &rvec); + bool isValid(float x, float y, float z, std::string &auto_aim_status); + +private: + // Class methods + ValidityFilter validity_filter_ = ValidityFilter(); + double computeLoss(double yaw_guess, std::vector image_points, cv::Mat tvec); + double gradientWrtYawFinitediff(double yaw, std::vector image_points, cv::Mat tvec); + + // Class variables + double prev_valid_detection_time_ = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + int locked_in_frames = 0; + int num_frames_to_fire_after = 3; + std::chrono::time_point last_fire_time; + + /** + * @brief Functor for the loss function and gradient computation. + * + * Used for the L-BFGS-B optimization library. + * + * @param poseEstimator Reference to the PoseEstimator instance. + * @param image_points The ground truth image points of the detected armor. + * @param tvec The translation vector of the detected armor. + * @param x The input vector containing a guess for the yaw angle. + * @return double The loss value. + * @return VectorXd The gradient vector. + */ + class LossAndGradient + { + public: + // Constructor to accept reference to PoseEstimator instance + LossAndGradient(PoseEstimator &poseEstimator, std::vector image_points, cv::Mat tvec) : poseEstimator(poseEstimator), image_points(image_points), tvec(tvec) {} + + double operator()(const Eigen::VectorXd &x, Eigen::VectorXd &grad) + { + double yaw = x(0); + double loss = poseEstimator.computeLoss(yaw, image_points, tvec); + grad(0) = poseEstimator.gradientWrtYawFinitediff(yaw, image_points, tvec); + return loss; + } + + private: + std::vector image_points; + cv::Mat tvec; + PoseEstimator &poseEstimator; + }; + + // Camera intrinsics + const cv::Mat CAMERA_MATRIX = (cv::Mat_(3, 3) << 1019.109, 0, 601.885, + 0, 1016.785, 521.005, + 0, 0, 1); + const cv::Mat DISTORTION_COEFFS = (cv::Mat_(1, 4) << -0.1088, -0.0721, -0.000847, 0.0); + + /* 3D object points (measured armor dimensions) + * Coordinate system: + * + * +y (yaw) + * ^ + * | +x (pitch) + * +----> + * / + * v + * +z (roll) + */ + const std::vector SMALL_ARMOR_OBJECT_POINTS = { + {-SMALL_ARMOR_HALF_WIDTH, -LIGHTBAR_HALF_HEIGHT, 0}, // Top Left + {-SMALL_ARMOR_HALF_WIDTH, LIGHTBAR_HALF_HEIGHT, 0}, // Bot Left + {SMALL_ARMOR_HALF_WIDTH, -LIGHTBAR_HALF_HEIGHT, 0}, // Top Right + {SMALL_ARMOR_HALF_WIDTH, LIGHTBAR_HALF_HEIGHT, 0} // Bot Right + }; + + const std::vector LARGE_ARMOR_OBJECT_POINTS = { + {-LARGE_ARMOR_HALF_WIDTH, -LIGHTBAR_HALF_HEIGHT, 0}, // Top Left + {-LARGE_ARMOR_HALF_WIDTH, LIGHTBAR_HALF_HEIGHT, 0}, // Bot Left + {LARGE_ARMOR_HALF_WIDTH, -LIGHTBAR_HALF_HEIGHT, 0}, // Top Right + {LARGE_ARMOR_HALF_WIDTH, LIGHTBAR_HALF_HEIGHT, 0} // Bot Right + }; +}; + +#endif // POSE_ESTIMATOR_HPP \ No newline at end of file diff --git a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp new file mode 100644 index 0000000..d53b792 --- /dev/null +++ b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp @@ -0,0 +1,40 @@ +#ifndef POSE_ESTIMATOR_NODE_HPP +#define POSE_ESTIMATOR_NODE_HPP + +// OpenCV and ROS2 includes +#include +#include "rclcpp/logging.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rcutils/time.h" +#include "vision_msgs/msg/key_points.hpp" +#include "vision_msgs/msg/predicted_armor.hpp" + +// tf publishing +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Matrix3x3.h" + +// Pose Estimator classes +#include "PoseEstimator.h" +#include "ValidityFilter.hpp" + +class PoseEstimatorNode : public rclcpp::Node +{ +public: + PoseEstimatorNode(const rclcpp::NodeOptions &options); + ~PoseEstimatorNode(); + +private: + PoseEstimator *pose_estimator; + + // Class methods + void publishZeroPredictedArmor(std_msgs::msg::Header header); + + // Callbacks and publishers/subscribers + rclcpp::Subscription::SharedPtr key_points_subscriber; + std::shared_ptr> predicted_armor_publisher; + void keyPointsCallback(const vision_msgs::msg::KeyPoints::SharedPtr msg); +}; + +#endif // POSE_ESTIMATOR_NODE_HPP \ No newline at end of file diff --git a/src/prm_vision/pose_estimator/include/ValidityFilter.hpp b/src/prm_vision/pose_estimator/include/ValidityFilter.hpp new file mode 100644 index 0000000..293b7ec --- /dev/null +++ b/src/prm_vision/pose_estimator/include/ValidityFilter.hpp @@ -0,0 +1,56 @@ +#ifndef _RESULT_FILTER_HPP_ +#define _RESULT_FILTER_HPP_ + +#include +#include +#include +#include + +enum ValidityFilterState +{ + TRACKING, + IDLING, + STOPPING, +}; + +class ValidityFilter +{ +public: + ValidityFilter(int lock_in_after, float max_distance, float min_distance, float max_shift_distance, float prev_len); + + ValidityFilter(); + ~ValidityFilter(); + + bool isValid(float x, float y, float z, double dt); + int get_lock_in_counter(); + + ValidityFilterState state; + +private: + int lock_in_after = 3; // lock in after n frames + int lock_in_counter = 0; + + int last_valid_time = 0; // sec + + float max_distance = 10000.f; // mm + float min_distance = 10.f; // mm + + double max_dt = 2000; // ms + int prev_len = 5; // check the back n frams for max shift distance vilolation + + float prev_x[20]; + float prev_y[20]; + float prev_z[20]; + int prev_idx = 0; + + float max_shift_distance = 150.f; // mm + + void update_prev(float, float, float); + bool position_validity(float, float, float); + bool distance_validity(float, float, float); + void increment_lock_in_counter(); + void decrement_lock_in_counter(); + void reset_lock_in_counter(); +}; + +#endif // _RESULT_FILTER_HPP_ \ No newline at end of file diff --git a/src/prm_vision/pose_estimator/package.xml b/src/prm_vision/pose_estimator/package.xml new file mode 100644 index 0000000..f9eb13c --- /dev/null +++ b/src/prm_vision/pose_estimator/package.xml @@ -0,0 +1,22 @@ + + + pose_estimator + 0.0.1 + Module written for Purdue Robomaster to estimate the 3D pose (translation and rotation) of armor plates of RoboMaster Robots. + + Tom O'Donnell + Apache-2.0 + + ament_cmake + sensor_msgs + rclcpp + cv_bridge + vision_msgs + + + ament_cmake_gtest + + + ament_cmake + + diff --git a/src/prm_vision/pose_estimator/src/PoseEstimator.cpp b/src/prm_vision/pose_estimator/src/PoseEstimator.cpp new file mode 100644 index 0000000..ace95fd --- /dev/null +++ b/src/prm_vision/pose_estimator/src/PoseEstimator.cpp @@ -0,0 +1,163 @@ +#include "PoseEstimator.h" + +void PoseEstimator::estimateTranslation(const std::vector &image_points, bool largeArmor, cv::Mat &tvec, cv::Mat &rvec) +{ + // SolvePnP requires 3D object points + std::vector object_points; + if (largeArmor) + { + object_points = LARGE_ARMOR_OBJECT_POINTS; + } + else + { + object_points = SMALL_ARMOR_OBJECT_POINTS; + } + + cv::solvePnP(object_points, image_points, CAMERA_MATRIX, DISTORTION_COEFFS, rvec, tvec, false, cv::SOLVEPNP_IPPE); +} + +/** + * @brief Check if the estimated pose is valid based on the validity filter. + * + * @param x The x-coordinate of the detected armor. + * @param y The y-coordinate of the detected armor. + * @param z The z-coordinate of the detected armor. + * @param auto_aim_status Reference to the status of the auto-aiming system. Set by this function. + * @return true If the pose is valid based on the validity filter. + */ +bool PoseEstimator::isValid(float x, float y, float z, std::string &auto_aim_status) +{ + // dt = current time - previous valid detection time + double curr_time_ = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + double dt = curr_time_ - prev_valid_detection_time_; + bool invalid = validity_filter_.validation(x, y, z, dt); + + // Stopping motor, we publish 0s for predicted armor + if (validity_filter_.state == STOPPING) + { + auto_aim_status = "STOPPING"; + locked_in_frames = 0; + return false; + } + + // Idling, dont send anything + else if (validity_filter_.state == IDLING) + { + auto_aim_status = "IDLING"; + locked_in_frames = 0; + return false; + } + + // Tracking, update kalman filter + else if (validity_filter_.state == TRACKING) + { + // if tracking and we have locked in for enough frames, fire at the target + if (locked_in_frames > num_frames_to_fire_after) + { + auto_aim_status = "FIRE"; + last_fire_time = std::chrono::system_clock::now(); + } + else + { + auto_aim_status = "TRACKING"; + } + + prev_valid_detection_time_ = curr_time_; + + // if validity filter valid for last 3 frames, increment locked_in_frames + if (validity_filter_.get_lock_in_counter() == 3) + { + locked_in_frames++; + } + else + { + // We are not locked in, reset locked_in_frames + locked_in_frames = 0; + } + } + + return !invalid; +} + +/** + * @brief Estimate the yaw angle of the detected armor. + * + * Uses L-BFGS-B optimization to minimize the reprojection error given a guess for the yaw angle. + * L-BFGS-B is a limited-memory quasi-Newton bounded optimization algorithm that works well for this one-dimensional problem. + * + * NOTE: This function assumes that pitch and roll are zero. + * This is valid due to robot design constraints. Pitch actually is ~15 degrees, but 0 seems to work well. + * + * @param yaw_guess The initial guess for the yaw angle in radians. + * @param image_points The image points of the detected armor. + * @param tvec The translation vector of the detected armor. + * @return double The estimated yaw angle in radians. + */ +double PoseEstimator::estimateYaw(const double yaw_guess, const std::vector &image_points, const cv::Mat &tvec) +{ + // We only have one parameter to optimize: yaw + const int n = 1; + double fx; + + // L-BFGS-B parameters (experimentally tuned) + LBFGSBParam param; + param.epsilon = 0.4; + param.max_iterations = 50; + + LBFGSBSolver solver(param); + LossAndGradient loss(*this, image_points, tvec); + + // Bound yaw to -45 to 45 degrees + VectorXd lb = VectorXd::Constant(n, -CV_PI / 4); + VectorXd ub = VectorXd::Constant(n, CV_PI / 4); + VectorXd x = VectorXd::Constant(n, yaw_guess); + + solver.minimize(loss, x, fx, lb, ub); + return x(0); +} + +/** + * @brief Compute the gradient of the loss function with respect to yaw using finite differences. + * + * @param yaw The yaw angle in radians. + * @param image_points The image points of the detected armor. + * @param tvec The translation vector of the detected armor. + * @return double The gradient value. + */ +double PoseEstimator::gradientWrtYawFinitediff(double yaw, std::vector image_points, cv::Mat tvec) +{ + double h = 1e-4; + double loss_plus = computeLoss(yaw + h, image_points, tvec); + double loss_minus = computeLoss(yaw - h, image_points, tvec); + double grad = (loss_plus - loss_minus) / (2 * h); + return grad; +} + +/** + * @brief Compute the loss between image points and reprojected points based on the yaw guess. + * + * @param yaw_guess The yaw angle guess in radians. + * @param image_points The image points of the detected armor. + * @param tvec The translation vector of the detected armor. + * @return double The loss value. + */ +double PoseEstimator::computeLoss(double yaw_guess, std::vector image_points, cv::Mat tvec) +{ + // We assume 0 pitch and roll, so they do not contribute to the rotation matrix + cv::Mat R = (cv::Mat_(3, 3) << cos(yaw_guess), 0, sin(yaw_guess), + 0, 1, 0, + -sin(yaw_guess), 0, cos(yaw_guess)); + + // Reproject the 3D object points using predicted yaw to the image plane + std::vector projected_points; + cv::projectPoints(SMALL_ARMOR_OBJECT_POINTS, R, tvec, CAMERA_MATRIX, DISTORTION_COEFFS, projected_points); + + // Loss = sqrt(0.25 * sum((x - x')^2 + (y - y')^2)) + double loss = 0; + for (int i = 0; i < image_points.size(); i++) + { + cv::Point2d diff = image_points[i] - projected_points[i]; + loss += 0.25 * (diff.x * diff.x + diff.y * diff.y); // Squared reprojection error + } + return pow(loss, 0.5); +} \ No newline at end of file diff --git a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp new file mode 100644 index 0000000..3dc945f --- /dev/null +++ b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp @@ -0,0 +1,75 @@ +#include "PoseEstimatorNode.hpp" + +PoseEstimatorNode::PoseEstimatorNode(const rclcpp::NodeOptions &options) : Node("pose_estimator", options) +{ + RCLCPP_INFO(get_logger(), "PoseEstimatorNode has been started."); + + // Initialize the pose estimator + pose_estimator = new PoseEstimator(); + + // Callbacks and pub/sub + key_points_subscriber = this->create_subscription("key_points", 10, std::bind(&PoseEstimatorNode::keyPointsCallback, this, std::placeholders::_1)); + predicted_armor_publisher = this->create_publisher("predicted_armor", 10); +} + +PoseEstimatorNode::~PoseEstimatorNode() { delete pose_estimator; } + +void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::SharedPtr key_points_msg) +{ + if (key_points_msg->points.size() != 8 || key_points_msg->points[4] == 0) + { + // No armor detected + publishZeroPredictedArmor(key_points_msg->header); + return; + } + + // Convert the message to a vector of cv::Point2f + std::vector image_points; + image_points.push_back(cv::Point2f(key_points_msg->points[0], key_points_msg->points[1])); + image_points.push_back(cv::Point2f(key_points_msg->points[2], key_points_msg->points[3])); + image_points.push_back(cv::Point2f(key_points_msg->points[4], key_points_msg->points[5])); + image_points.push_back(cv::Point2f(key_points_msg->points[6], key_points_msg->points[7])); + + // Compute armor's pose and validate it + cv::Mat tvec, rvec; + pose_estimator->estimateTranslation(image_points, key_points_msg->is_large_armor, tvec, rvec); + std::string auto_aim_status; + bool valid = pose_estimator->isValid(tvec.at(0), tvec.at(1), tvec.at(2), auto_aim_status); + + // Publish the predicted armor if it is valid +} + +void PoseEstimatorNode::publishZeroPredictedArmor(std_msgs::msg::Header header) +{ + vision_msgs::msg::PredictedArmor predicted_armor_msg; + predicted_armor_msg.header = header; + predicted_armor_msg.x = 0.0; + predicted_armor_msg.y = 0.0; + predicted_armor_msg.z = 0.0; + predicted_armor_msg.rvec_x = 0; + predicted_armor_msg.rvec_y = 0; + predicted_armor_msg.rvec_z = 0; + predicted_armor_msg.x_vel = 0; + predicted_armor_msg.y_vel = 0; + predicted_armor_msg.z_vel = 0; + + // We may still fire for a short time after losing the target + predicted_armor_msg.fire = false; + predicted_armor_publisher->publish(predicted_armor_msg); +} + +int main(int argc, char *argv[]) +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor exec; + + const rclcpp::NodeOptions options; + auto pose_estimator_node = std::make_shared(options); + + exec.add_node(pose_estimator_node); + exec.spin(); + + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/prm_vision/pose_estimator/src/ValidityFilter.cpp b/src/prm_vision/pose_estimator/src/ValidityFilter.cpp new file mode 100644 index 0000000..c3c668f --- /dev/null +++ b/src/prm_vision/pose_estimator/src/ValidityFilter.cpp @@ -0,0 +1,127 @@ +#include "ValidityFilter.hpp" +#include +#include + +ValidityFilter::ValidityFilter(int lock_in_after, float max_distance, float min_distance, float max_shift_distance, float prev_len) +{ + this->lock_in_after = lock_in_after; + this->max_distance = max_distance; + this->min_distance = min_distance; + this->max_shift_distance = max_shift_distance; + this->prev_len = std::min(prev_len, 20.0f); + + last_valid_time = 0; + lock_in_counter = 0; + prev_idx = 0; + state = STOPPING; + + // Initialize prev arrays + for (int i = 0; i < this->prev_len; i++) + { + prev_x[i] = 0; + prev_y[i] = 0; + prev_z[i] = 0; + } +} + +ValidityFilter::ValidityFilter() {} +ValidityFilter::~ValidityFilter() {} + +bool ValidityFilter::isValid(float x, float y, float z, double dt) +{ + // Check distance validity + if (!distance_validity(x, y, z)) + { + decrement_lock_in_counter(); + return state == STOPPING; + } + + // Check time since last valid detection + if (dt > max_dt) + { + reset_lock_in_counter(); + update_prev(x, y, z); + return false; // Invalid because of time lapse + } + + // Check position validity + int num_valid = position_validity(x, y, z); + if (num_valid == 0) + { + decrement_lock_in_counter(); + update_prev(x, y, z); + return state == STOPPING; + } + + // Update state based on detection confidence + increment_lock_in_counter(); + update_prev(x, y, z); + + return state == TRACKING; +} + +void ValidityFilter::update_prev(float x, float y, float z) +{ + prev_x[prev_idx] = x; + prev_y[prev_idx] = y; + prev_z[prev_idx] = z; + prev_idx = (prev_idx + 1) % prev_len; +} + +bool ValidityFilter::distance_validity(float x, float y, float z) +{ + float dst = std::sqrt(x * x + y * y + z * z); + return (dst <= max_distance && dst >= min_distance); +} + +bool ValidityFilter::position_validity(float x, float y, float z) +{ + for (int i = 0; i < prev_len; i++) + { + float dx = x - prev_x[i]; + float dy = y - prev_y[i]; + float dz = z - prev_z[i]; + if (std::sqrt(dx * dx + dy * dy + dz * dz) < max_shift_distance) + { + return true; // Valid as soon as one match is found + } + } + return false; // No valid matches found +} + +void ValidityFilter::increment_lock_in_counter() +{ + if (++lock_in_counter >= lock_in_after) + { + state = TRACKING; + lock_in_counter = lock_in_after; // Cap at max + } + else + { + state = IDLING; + } +} + +void ValidityFilter::decrement_lock_in_counter() +{ + if (--lock_in_counter <= 0) + { + state = STOPPING; + lock_in_counter = 0; // Ensure no negative values + } + else + { + state = IDLING; + } +} + +void ValidityFilter::reset_lock_in_counter() +{ + lock_in_counter = 0; + state = STOPPING; +} + +int ValidityFilter::get_lock_in_counter() +{ + return lock_in_counter; +} diff --git a/src/prm_vision/pose_estimator/test/test_PoseEstimator.cpp b/src/prm_vision/pose_estimator/test/test_PoseEstimator.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/prm_vision/vision_msgs/msg/KeyPoints.msg b/src/prm_vision/vision_msgs/msg/KeyPoints.msg index 13a5669..3944549 100644 --- a/src/prm_vision/vision_msgs/msg/KeyPoints.msg +++ b/src/prm_vision/vision_msgs/msg/KeyPoints.msg @@ -1,3 +1,3 @@ std_msgs/Header header float32[8] points # top_left, bot_left, top_right, bot_right -bool large_armor +bool is_large_armor From 15fecd53d92d8f3be2480ee18144ac300326929f Mon Sep 17 00:00:00 2001 From: Tom_TKO Date: Mon, 30 Dec 2024 15:59:19 -0500 Subject: [PATCH 3/7] continue pose estimation integration into poseestimateornode. change predictedarmor.msg to use PYR. Builds, need to test --- src/prm_vision/pose_estimator/CMakeLists.txt | 6 +- .../pose_estimator/include/PoseEstimator.h | 6 +- .../include/PoseEstimatorNode.hpp | 1 + .../pose_estimator/include/ValidityFilter.hpp | 5 +- .../pose_estimator/src/PoseEstimator.cpp | 116 +++++++++--------- .../pose_estimator/src/PoseEstimatorNode.cpp | 49 +++++--- .../pose_estimator/src/ValidityFilter.cpp | 31 +++-- .../vision_msgs/msg/PredictedArmor.msg | 20 +-- 8 files changed, 136 insertions(+), 98 deletions(-) diff --git a/src/prm_vision/pose_estimator/CMakeLists.txt b/src/prm_vision/pose_estimator/CMakeLists.txt index 7c06725..5d3e638 100644 --- a/src/prm_vision/pose_estimator/CMakeLists.txt +++ b/src/prm_vision/pose_estimator/CMakeLists.txt @@ -8,6 +8,8 @@ find_package(rclcpp REQUIRED) find_package(vision_msgs REQUIRED) find_package(OpenCV 4.6.0 REQUIRED) find_package(cv_bridge REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) find_package(ament_cmake_gtest REQUIRED) # GTest find_package(Eigen3 REQUIRED) # Eigen3 @@ -28,6 +30,8 @@ ament_target_dependencies(PoseEstimatorNode rclcpp sensor_msgs cv_bridge + tf2 + tf2_ros vision_msgs Eigen3 ) @@ -46,5 +50,5 @@ install(TARGETS PoseEstimatorNode DESTINATION lib/${PROJECT_NAME}) ament_export_include_directories(include) -ament_export_dependencies(rclcpp sensor_msgs cv_bridge vision_msgs Eigen3) +ament_export_dependencies(rclcpp sensor_msgs cv_bridge vision_msgs Eigen3 tf2) ament_package() \ No newline at end of file diff --git a/src/prm_vision/pose_estimator/include/PoseEstimator.h b/src/prm_vision/pose_estimator/include/PoseEstimator.h index 2f5b496..b919cf1 100644 --- a/src/prm_vision/pose_estimator/include/PoseEstimator.h +++ b/src/prm_vision/pose_estimator/include/PoseEstimator.h @@ -2,7 +2,8 @@ #define POSE_ESTIMATOR_HPP #include -#include +#include +#include #include #include #include "LBFGSB.h" // L-BFGS-B optimization library for yaw estimation @@ -36,8 +37,7 @@ class PoseEstimator double gradientWrtYawFinitediff(double yaw, std::vector image_points, cv::Mat tvec); // Class variables - double prev_valid_detection_time_ = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); - int locked_in_frames = 0; + int consecutive_tracking_frames_ctr = 0; int num_frames_to_fire_after = 3; std::chrono::time_point last_fire_time; diff --git a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp index d53b792..2d057a9 100644 --- a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp +++ b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp @@ -27,6 +27,7 @@ class PoseEstimatorNode : public rclcpp::Node private: PoseEstimator *pose_estimator; + double last_yaw_estimate = 0.0; // Class methods void publishZeroPredictedArmor(std_msgs::msg::Header header); diff --git a/src/prm_vision/pose_estimator/include/ValidityFilter.hpp b/src/prm_vision/pose_estimator/include/ValidityFilter.hpp index 293b7ec..3e7b921 100644 --- a/src/prm_vision/pose_estimator/include/ValidityFilter.hpp +++ b/src/prm_vision/pose_estimator/include/ValidityFilter.hpp @@ -2,6 +2,7 @@ #define _RESULT_FILTER_HPP_ #include +#include #include #include #include @@ -21,7 +22,7 @@ class ValidityFilter ValidityFilter(); ~ValidityFilter(); - bool isValid(float x, float y, float z, double dt); + bool isValid(float x, float y, float z); int get_lock_in_counter(); ValidityFilterState state; @@ -30,7 +31,7 @@ class ValidityFilter int lock_in_after = 3; // lock in after n frames int lock_in_counter = 0; - int last_valid_time = 0; // sec + std::chrono::steady_clock::time_point last_valid_time = std::chrono::steady_clock::now(); float max_distance = 10000.f; // mm float min_distance = 10.f; // mm diff --git a/src/prm_vision/pose_estimator/src/PoseEstimator.cpp b/src/prm_vision/pose_estimator/src/PoseEstimator.cpp index ace95fd..599d046 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimator.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimator.cpp @@ -13,9 +13,47 @@ void PoseEstimator::estimateTranslation(const std::vector &image_po object_points = SMALL_ARMOR_OBJECT_POINTS; } + std::string auto_aim_status; cv::solvePnP(object_points, image_points, CAMERA_MATRIX, DISTORTION_COEFFS, rvec, tvec, false, cv::SOLVEPNP_IPPE); } +/** + * @brief Estimate the yaw angle of the detected armor. + * + * Uses L-BFGS-B optimization to minimize the reprojection error given a guess for the yaw angle. + * L-BFGS-B is a limited-memory quasi-Newton bounded optimization algorithm that works well for this one-dimensional problem. + * + * NOTE: This function assumes that pitch and roll are zero. + * This is valid due to robot design constraints. Pitch actually is ~15 degrees, but 0 seems to work well. + * + * @param yaw_guess The initial guess for the yaw angle in radians. + * @param image_points The image points of the detected armor. + * @param tvec The translation vector of the detected armor. + * @return double The estimated yaw angle in radians. + */ +double PoseEstimator::estimateYaw(const double yaw_guess, const std::vector &image_points, const cv::Mat &tvec) +{ + // We only have one parameter to optimize: yaw + const int n = 1; + double fx; + + // L-BFGS-B parameters (experimentally tuned) + LBFGSBParam param; + param.epsilon = 0.4; + param.max_iterations = 50; + + LBFGSBSolver solver(param); + LossAndGradient loss(*this, image_points, tvec); + + // Bound yaw to -45 to 45 degrees + VectorXd lb = VectorXd::Constant(n, -CV_PI / 4); + VectorXd ub = VectorXd::Constant(n, CV_PI / 4); + VectorXd x = VectorXd::Constant(n, yaw_guess); + + solver.minimize(loss, x, fx, lb, ub); + return x(0); +} + /** * @brief Check if the estimated pose is valid based on the validity filter. * @@ -27,93 +65,53 @@ void PoseEstimator::estimateTranslation(const std::vector &image_po */ bool PoseEstimator::isValid(float x, float y, float z, std::string &auto_aim_status) { - // dt = current time - previous valid detection time - double curr_time_ = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); - double dt = curr_time_ - prev_valid_detection_time_; - bool invalid = validity_filter_.validation(x, y, z, dt); + // Check if the pose is valid based on the validity filter + // NOTE: The return value is ONLY used to determine if we should reset the Kalman filter. + // We primarily use the state variable to determine the action to take (idle, track, stop). + bool isValid = validity_filter_.isValid(x, y, z); - // Stopping motor, we publish 0s for predicted armor + // Stopping motor, we want to publish 0s for predicted armor (no auto aiming) if (validity_filter_.state == STOPPING) { auto_aim_status = "STOPPING"; - locked_in_frames = 0; + consecutive_tracking_frames_ctr = 0; return false; } - // Idling, dont send anything + // We have a detection but not yet tracking else if (validity_filter_.state == IDLING) { auto_aim_status = "IDLING"; - locked_in_frames = 0; + consecutive_tracking_frames_ctr = 0; return false; } - // Tracking, update kalman filter + // We have enough valid detections to track the target else if (validity_filter_.state == TRACKING) { - // if tracking and we have locked in for enough frames, fire at the target - if (locked_in_frames > num_frames_to_fire_after) + // if validity filter valid for last 3 frames, increment locked_in_frames + if (validity_filter_.get_lock_in_counter() == 3) { - auto_aim_status = "FIRE"; - last_fire_time = std::chrono::system_clock::now(); + consecutive_tracking_frames_ctr++; } else { - auto_aim_status = "TRACKING"; + // We are not locked in, reset locked_in_frames + consecutive_tracking_frames_ctr = 0; } - prev_valid_detection_time_ = curr_time_; - - // if validity filter valid for last 3 frames, increment locked_in_frames - if (validity_filter_.get_lock_in_counter() == 3) + // if tracking and we have locked in for enough frames, fire at the target + if (consecutive_tracking_frames_ctr > num_frames_to_fire_after) { - locked_in_frames++; + auto_aim_status = "FIRE"; } else { - // We are not locked in, reset locked_in_frames - locked_in_frames = 0; + auto_aim_status = "TRACKING"; } } - return !invalid; -} - -/** - * @brief Estimate the yaw angle of the detected armor. - * - * Uses L-BFGS-B optimization to minimize the reprojection error given a guess for the yaw angle. - * L-BFGS-B is a limited-memory quasi-Newton bounded optimization algorithm that works well for this one-dimensional problem. - * - * NOTE: This function assumes that pitch and roll are zero. - * This is valid due to robot design constraints. Pitch actually is ~15 degrees, but 0 seems to work well. - * - * @param yaw_guess The initial guess for the yaw angle in radians. - * @param image_points The image points of the detected armor. - * @param tvec The translation vector of the detected armor. - * @return double The estimated yaw angle in radians. - */ -double PoseEstimator::estimateYaw(const double yaw_guess, const std::vector &image_points, const cv::Mat &tvec) -{ - // We only have one parameter to optimize: yaw - const int n = 1; - double fx; - - // L-BFGS-B parameters (experimentally tuned) - LBFGSBParam param; - param.epsilon = 0.4; - param.max_iterations = 50; - - LBFGSBSolver solver(param); - LossAndGradient loss(*this, image_points, tvec); - - // Bound yaw to -45 to 45 degrees - VectorXd lb = VectorXd::Constant(n, -CV_PI / 4); - VectorXd ub = VectorXd::Constant(n, CV_PI / 4); - VectorXd x = VectorXd::Constant(n, yaw_guess); - - solver.minimize(loss, x, fx, lb, ub); - return x(0); + return true; } /** diff --git a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp index 3dc945f..0e71fa1 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp @@ -23,38 +23,59 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha return; } - // Convert the message to a vector of cv::Point2f + cv::Mat tvec, rvec; + std::string new_auto_aim_status; std::vector image_points; + + // Convert the message to a vector of cv::Point2f image_points.push_back(cv::Point2f(key_points_msg->points[0], key_points_msg->points[1])); image_points.push_back(cv::Point2f(key_points_msg->points[2], key_points_msg->points[3])); image_points.push_back(cv::Point2f(key_points_msg->points[4], key_points_msg->points[5])); image_points.push_back(cv::Point2f(key_points_msg->points[6], key_points_msg->points[7])); - // Compute armor's pose and validate it - cv::Mat tvec, rvec; + // Compute armor's pose and validate it. Compute new yaw estimate based on the last yaw estimate. pose_estimator->estimateTranslation(image_points, key_points_msg->is_large_armor, tvec, rvec); - std::string auto_aim_status; - bool valid = pose_estimator->isValid(tvec.at(0), tvec.at(1), tvec.at(2), auto_aim_status); + last_yaw_estimate = pose_estimator->estimateYaw(last_yaw_estimate, image_points, tvec); + bool valid_pose_estimate = pose_estimator->isValid(tvec.at(0), tvec.at(1), tvec.at(2), new_auto_aim_status); - // Publish the predicted armor if it is valid + // Publish the predicted armor + if (valid_pose_estimate) + { + vision_msgs::msg::PredictedArmor predicted_armor_msg; + predicted_armor_msg.header = key_points_msg->header; + predicted_armor_msg.x = tvec.at(0); + predicted_armor_msg.y = tvec.at(1); + predicted_armor_msg.z = tvec.at(2); + predicted_armor_msg.pitch = 15 * CV_PI / 180; // 15 degrees in radians + predicted_armor_msg.yaw = last_yaw_estimate; // Use the latest yaw estimate + predicted_armor_msg.roll = 0; // Roll is always 0 + predicted_armor_msg.x_vel = 0; + predicted_armor_msg.y_vel = 0; // TODO: compute yaw rate + predicted_armor_msg.z_vel = 0; + predicted_armor_msg.fire = new_auto_aim_status == "FIRE"; + predicted_armor_publisher->publish(predicted_armor_msg); + } + else + { + publishZeroPredictedArmor(key_points_msg->header); + } } void PoseEstimatorNode::publishZeroPredictedArmor(std_msgs::msg::Header header) { vision_msgs::msg::PredictedArmor predicted_armor_msg; predicted_armor_msg.header = header; - predicted_armor_msg.x = 0.0; - predicted_armor_msg.y = 0.0; - predicted_armor_msg.z = 0.0; - predicted_armor_msg.rvec_x = 0; - predicted_armor_msg.rvec_y = 0; - predicted_armor_msg.rvec_z = 0; + predicted_armor_msg.x = 0; + predicted_armor_msg.y = 0; + predicted_armor_msg.z = 0; + predicted_armor_msg.pitch = 0; + predicted_armor_msg.yaw = 0; + predicted_armor_msg.roll = 0; predicted_armor_msg.x_vel = 0; predicted_armor_msg.y_vel = 0; predicted_armor_msg.z_vel = 0; + predicted_armor_msg.fire = false; // TODO: Still fire after a few frames - // We may still fire for a short time after losing the target - predicted_armor_msg.fire = false; predicted_armor_publisher->publish(predicted_armor_msg); } diff --git a/src/prm_vision/pose_estimator/src/ValidityFilter.cpp b/src/prm_vision/pose_estimator/src/ValidityFilter.cpp index c3c668f..5d417ff 100644 --- a/src/prm_vision/pose_estimator/src/ValidityFilter.cpp +++ b/src/prm_vision/pose_estimator/src/ValidityFilter.cpp @@ -1,6 +1,4 @@ #include "ValidityFilter.hpp" -#include -#include ValidityFilter::ValidityFilter(int lock_in_after, float max_distance, float min_distance, float max_shift_distance, float prev_len) { @@ -10,12 +8,12 @@ ValidityFilter::ValidityFilter(int lock_in_after, float max_distance, float min_ this->max_shift_distance = max_shift_distance; this->prev_len = std::min(prev_len, 20.0f); - last_valid_time = 0; + // Initial state of the filter lock_in_counter = 0; prev_idx = 0; state = STOPPING; - // Initialize prev arrays + // Initialize arrays to track previous predictions for (int i = 0; i < this->prev_len; i++) { prev_x[i] = 0; @@ -27,13 +25,28 @@ ValidityFilter::ValidityFilter(int lock_in_after, float max_distance, float min_ ValidityFilter::ValidityFilter() {} ValidityFilter::~ValidityFilter() {} -bool ValidityFilter::isValid(float x, float y, float z, double dt) +/** + * @brief Check if the estimated pose is valid based on a validity filter. + * + * @param x The x-coordinate of the detected armor. + * @param y The y-coordinate of the detected armor. + * @param z The z-coordinate of the detected armor. + * @return true If the pose is valid based on the validity filter and current aiming state. + * + * The return value is ONLY used to determine if we should reset the Kalman filter inside PoseEstimator. + * We primarily use the state variable to determine the action to take (idle, track, stop). + */ +bool ValidityFilter::isValid(float x, float y, float z) { + // Calculate time difference since last valid detection + auto now = std::chrono::steady_clock::now(); + double dt = std::chrono::duration(now - last_valid_time).count(); + // Check distance validity if (!distance_validity(x, y, z)) { decrement_lock_in_counter(); - return state == STOPPING; + return state != STOPPING; } // Check time since last valid detection @@ -45,17 +58,17 @@ bool ValidityFilter::isValid(float x, float y, float z, double dt) } // Check position validity - int num_valid = position_validity(x, y, z); - if (num_valid == 0) + if (!position_validity(x, y, z)) { decrement_lock_in_counter(); update_prev(x, y, z); return state == STOPPING; } - // Update state based on detection confidence + // Valid detection increment_lock_in_counter(); update_prev(x, y, z); + last_valid_time = now; // Update the last valid time return state == TRACKING; } diff --git a/src/prm_vision/vision_msgs/msg/PredictedArmor.msg b/src/prm_vision/vision_msgs/msg/PredictedArmor.msg index 45ba419..9ce55d5 100644 --- a/src/prm_vision/vision_msgs/msg/PredictedArmor.msg +++ b/src/prm_vision/vision_msgs/msg/PredictedArmor.msg @@ -1,11 +1,11 @@ std_msgs/Header header -float64 x # x position [mm] -float64 y # y position [mm] -float64 z # z position [mm] -float64 rvec_x # x component of rotation vector [rad, axis-angle] -float64 rvec_y # y component of rotation vector [rad, axis-angle] -float64 rvec_z # z component of rotation vector [rad, axis-angle] -float64 x_vel # x velocity [mm/s] -float64 y_vel # y velocity [mm/s] -float64 z_vel # z velocity [mm/s] -bool fire # true if we have locked on to a target and will fire \ No newline at end of file +float64 x # x position [mm] +float64 y # y position [mm] +float64 z # z position [mm] +float64 pitch # pitch [rad] +float64 yaw # yaw [rad] +float64 roll # roll [rad] +float64 x_vel # x velocity [mm/s] +float64 y_vel # y velocity [mm/s] +float64 z_vel # z velocity [mm/s] +bool fire # true if we have locked on to a target and will fire \ No newline at end of file From 15684d06a628665f22eb89e4504097bf3d5d7643 Mon Sep 17 00:00:00 2001 From: Tom_TKO Date: Wed, 1 Jan 2025 18:23:08 -0500 Subject: [PATCH 4/7] fix frame count bug, fix naming conventions, update pose estimation validity check. need to write tests now --- .../src/OpenCVArmorDetector.cpp | 1 + src/prm_vision/pose_estimator/CMakeLists.txt | 3 + .../pose_estimator/include/PoseEstimator.h | 8 +- .../include/PoseEstimatorNode.hpp | 3 +- .../pose_estimator/include/ValidityFilter.hpp | 42 +++---- .../pose_estimator/src/PoseEstimator.cpp | 32 +++--- .../pose_estimator/src/PoseEstimatorNode.cpp | 19 +-- .../pose_estimator/src/ValidityFilter.cpp | 108 +++++++++--------- .../test/test_ValidityFilter.cpp | 18 +++ 9 files changed, 130 insertions(+), 104 deletions(-) create mode 100644 src/prm_vision/pose_estimator/test/test_ValidityFilter.cpp diff --git a/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp b/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp index 6555a9d..3ec33d5 100644 --- a/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp +++ b/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp @@ -36,6 +36,7 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame) // Detect the armor in the cropped frame std::vector points = detectArmorsInFrame(croppedFrame); + _frame_count++; // Display the cropped frame for debugging #ifdef DEBUG diff --git a/src/prm_vision/pose_estimator/CMakeLists.txt b/src/prm_vision/pose_estimator/CMakeLists.txt index 5d3e638..aa4ce38 100644 --- a/src/prm_vision/pose_estimator/CMakeLists.txt +++ b/src/prm_vision/pose_estimator/CMakeLists.txt @@ -42,8 +42,11 @@ ament_target_dependencies(PoseEstimator # GTest setup ament_add_gtest(test_PoseEstimator test/test_PoseEstimator.cpp) +ament_add_gtest(test_ValidityFilter test/test_ValidityFilter.cpp) target_include_directories(test_PoseEstimator PRIVATE include) +target_include_directories(test_ValidityFilter PRIVATE include) target_link_libraries(test_PoseEstimator PoseEstimator) +target_link_libraries(test_ValidityFilter ValidityFilter) # Install the node install(TARGETS PoseEstimatorNode diff --git a/src/prm_vision/pose_estimator/include/PoseEstimator.h b/src/prm_vision/pose_estimator/include/PoseEstimator.h index b919cf1..5a89be5 100644 --- a/src/prm_vision/pose_estimator/include/PoseEstimator.h +++ b/src/prm_vision/pose_estimator/include/PoseEstimator.h @@ -28,7 +28,7 @@ class PoseEstimator // Class methods double estimateYaw(const double yaw_guess, const std::vector &image_points, const cv::Mat &tvec); void estimateTranslation(const std::vector &image_points, bool largeArmor, cv::Mat &tvec, cv::Mat &rvec); - bool isValid(float x, float y, float z, std::string &auto_aim_status); + bool isValid(float x, float y, float z, std::string &auto_aim_status, bool &reset_kalman); private: // Class methods @@ -37,9 +37,9 @@ class PoseEstimator double gradientWrtYawFinitediff(double yaw, std::vector image_points, cv::Mat tvec); // Class variables - int consecutive_tracking_frames_ctr = 0; - int num_frames_to_fire_after = 3; - std::chrono::time_point last_fire_time; + int _consecutive_tracking_frames_ctr = 0; + int _num_frames_to_fire_after = 3; + std::chrono::time_point _last_fire_time; /** * @brief Functor for the loss function and gradient computation. diff --git a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp index 2d057a9..3016be5 100644 --- a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp +++ b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp @@ -27,7 +27,8 @@ class PoseEstimatorNode : public rclcpp::Node private: PoseEstimator *pose_estimator; - double last_yaw_estimate = 0.0; + double _last_yaw_estimate = 0.0; + int _allowed_missed_frames_before_no_fire = 0; // Gets set to 5 when we have a valid pose estimate // Class methods void publishZeroPredictedArmor(std_msgs::msg::Header header); diff --git a/src/prm_vision/pose_estimator/include/ValidityFilter.hpp b/src/prm_vision/pose_estimator/include/ValidityFilter.hpp index 3e7b921..6b4746a 100644 --- a/src/prm_vision/pose_estimator/include/ValidityFilter.hpp +++ b/src/prm_vision/pose_estimator/include/ValidityFilter.hpp @@ -22,36 +22,36 @@ class ValidityFilter ValidityFilter(); ~ValidityFilter(); - bool isValid(float x, float y, float z); - int get_lock_in_counter(); + bool resetKalman(float x, float y, float z); + int getLockInCounter(); - ValidityFilterState state; + ValidityFilterState state = STOPPING; private: - int lock_in_after = 3; // lock in after n frames - int lock_in_counter = 0; + int _lock_in_after = 3; // lock in after n frames + int _lock_in_counter = 0; - std::chrono::steady_clock::time_point last_valid_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point _last_valid_time = std::chrono::steady_clock::now(); - float max_distance = 10000.f; // mm - float min_distance = 10.f; // mm + float _max_distance = 10000.f; // mm + float _min_distance = 10.f; // mm - double max_dt = 2000; // ms - int prev_len = 5; // check the back n frams for max shift distance vilolation + double _max_dt = 2000; // ms + int _prev_len = 5; // check the back n frames for max shift distance vilolation - float prev_x[20]; - float prev_y[20]; - float prev_z[20]; - int prev_idx = 0; + float _prev_x[20]; + float _prev_y[20]; + float _prev_z[20]; + int _prev_idx = 0; - float max_shift_distance = 150.f; // mm + float _max_shift_distance = 150.f; // mm - void update_prev(float, float, float); - bool position_validity(float, float, float); - bool distance_validity(float, float, float); - void increment_lock_in_counter(); - void decrement_lock_in_counter(); - void reset_lock_in_counter(); + void updatePrev(float, float, float); + bool positionValidity(float, float, float); + bool distanceValidity(float, float, float); + void incrementLockInCounter(); + void decrementLockInCounter(); + void resetLockInCounter(); }; #endif // _RESULT_FILTER_HPP_ \ No newline at end of file diff --git a/src/prm_vision/pose_estimator/src/PoseEstimator.cpp b/src/prm_vision/pose_estimator/src/PoseEstimator.cpp index 599d046..337ffdf 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimator.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimator.cpp @@ -55,26 +55,30 @@ double PoseEstimator::estimateYaw(const double yaw_guess, const std::vector num_frames_to_fire_after) + if (_consecutive_tracking_frames_ctr > _num_frames_to_fire_after) { auto_aim_status = "FIRE"; } diff --git a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp index 0e71fa1..27061fe 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp @@ -16,7 +16,7 @@ PoseEstimatorNode::~PoseEstimatorNode() { delete pose_estimator; } void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::SharedPtr key_points_msg) { - if (key_points_msg->points.size() != 8 || key_points_msg->points[4] == 0) + if (key_points_msg->points.size() != 8 || key_points_msg->points[4] == 0) // idx 4 is x-coord of top right corner, so if it's 0, we know there's no armor { // No armor detected publishZeroPredictedArmor(key_points_msg->header); @@ -24,6 +24,7 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha } cv::Mat tvec, rvec; + bool reset_kalman = false; std::string new_auto_aim_status; std::vector image_points; @@ -33,26 +34,28 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha image_points.push_back(cv::Point2f(key_points_msg->points[4], key_points_msg->points[5])); image_points.push_back(cv::Point2f(key_points_msg->points[6], key_points_msg->points[7])); - // Compute armor's pose and validate it. Compute new yaw estimate based on the last yaw estimate. + // Compute armor's pose and validate it. We compute new yaw estimate based on the last yaw estimate. pose_estimator->estimateTranslation(image_points, key_points_msg->is_large_armor, tvec, rvec); - last_yaw_estimate = pose_estimator->estimateYaw(last_yaw_estimate, image_points, tvec); - bool valid_pose_estimate = pose_estimator->isValid(tvec.at(0), tvec.at(1), tvec.at(2), new_auto_aim_status); + _last_yaw_estimate = pose_estimator->estimateYaw(_last_yaw_estimate, image_points, tvec); + bool valid_pose_estimate = pose_estimator->isValid(tvec.at(0), tvec.at(1), tvec.at(2), new_auto_aim_status, reset_kalman); - // Publish the predicted armor + // Publish the predicted armor if the pose is valid (we are tracking or firing) if (valid_pose_estimate) { + _allowed_missed_frames_before_no_fire = 5; + vision_msgs::msg::PredictedArmor predicted_armor_msg; predicted_armor_msg.header = key_points_msg->header; predicted_armor_msg.x = tvec.at(0); predicted_armor_msg.y = tvec.at(1); predicted_armor_msg.z = tvec.at(2); predicted_armor_msg.pitch = 15 * CV_PI / 180; // 15 degrees in radians - predicted_armor_msg.yaw = last_yaw_estimate; // Use the latest yaw estimate + predicted_armor_msg.yaw = _last_yaw_estimate; // Use the latest yaw estimate predicted_armor_msg.roll = 0; // Roll is always 0 predicted_armor_msg.x_vel = 0; predicted_armor_msg.y_vel = 0; // TODO: compute yaw rate predicted_armor_msg.z_vel = 0; - predicted_armor_msg.fire = new_auto_aim_status == "FIRE"; + predicted_armor_msg.fire = (new_auto_aim_status == "FIRE"); predicted_armor_publisher->publish(predicted_armor_msg); } else @@ -74,7 +77,7 @@ void PoseEstimatorNode::publishZeroPredictedArmor(std_msgs::msg::Header header) predicted_armor_msg.x_vel = 0; predicted_armor_msg.y_vel = 0; predicted_armor_msg.z_vel = 0; - predicted_armor_msg.fire = false; // TODO: Still fire after a few frames + predicted_armor_msg.fire = (_allowed_missed_frames_before_no_fire-- > 0); predicted_armor_publisher->publish(predicted_armor_msg); } diff --git a/src/prm_vision/pose_estimator/src/ValidityFilter.cpp b/src/prm_vision/pose_estimator/src/ValidityFilter.cpp index 5d417ff..b63563e 100644 --- a/src/prm_vision/pose_estimator/src/ValidityFilter.cpp +++ b/src/prm_vision/pose_estimator/src/ValidityFilter.cpp @@ -2,23 +2,18 @@ ValidityFilter::ValidityFilter(int lock_in_after, float max_distance, float min_distance, float max_shift_distance, float prev_len) { - this->lock_in_after = lock_in_after; - this->max_distance = max_distance; - this->min_distance = min_distance; - this->max_shift_distance = max_shift_distance; - this->prev_len = std::min(prev_len, 20.0f); - - // Initial state of the filter - lock_in_counter = 0; - prev_idx = 0; - state = STOPPING; + this->_lock_in_after = lock_in_after; + this->_max_distance = max_distance; + this->_min_distance = min_distance; + this->_max_shift_distance = max_shift_distance; + this->_prev_len = std::min(prev_len, 20.0f); // Initialize arrays to track previous predictions - for (int i = 0; i < this->prev_len; i++) + for (int i = 0; i < this->_prev_len; i++) { - prev_x[i] = 0; - prev_y[i] = 0; - prev_z[i] = 0; + _prev_x[i] = 0; + _prev_y[i] = 0; + _prev_z[i] = 0; } } @@ -26,75 +21,76 @@ ValidityFilter::ValidityFilter() {} ValidityFilter::~ValidityFilter() {} /** - * @brief Check if the estimated pose is valid based on a validity filter. + * @brief Check if we should reset kalman filter based on a validity filter. * * @param x The x-coordinate of the detected armor. * @param y The y-coordinate of the detected armor. * @param z The z-coordinate of the detected armor. * @return true If the pose is valid based on the validity filter and current aiming state. * - * The return value is ONLY used to determine if we should reset the Kalman filter inside PoseEstimator. + * NOTE: The return value is ONLY used to determine if we should reset the Kalman filter inside PoseEstimator. * We primarily use the state variable to determine the action to take (idle, track, stop). */ -bool ValidityFilter::isValid(float x, float y, float z) +bool ValidityFilter::resetKalman(float x, float y, float z) { // Calculate time difference since last valid detection auto now = std::chrono::steady_clock::now(); - double dt = std::chrono::duration(now - last_valid_time).count(); + double dt = std::chrono::duration(now - _last_valid_time).count(); // Check distance validity - if (!distance_validity(x, y, z)) + if (!distanceValidity(x, y, z)) { - decrement_lock_in_counter(); - return state != STOPPING; + decrementLockInCounter(); + updatePrev(x, y, z); // We still add to prior detections, this is a design choice + return state != STOPPING; // not in range, so invalid if we aren't stopping } // Check time since last valid detection - if (dt > max_dt) + if (dt > _max_dt) { - reset_lock_in_counter(); - update_prev(x, y, z); - return false; // Invalid because of time lapse + resetLockInCounter(); + updatePrev(x, y, z); + return state == STOPPING; // too much time has passed, so invalid if we are stopping } // Check position validity - if (!position_validity(x, y, z)) + if (!positionValidity(x, y, z)) { - decrement_lock_in_counter(); - update_prev(x, y, z); - return state == STOPPING; + decrementLockInCounter(); + updatePrev(x, y, z); + return state == STOPPING; // too much shift, so invalid if we are stopping } // Valid detection - increment_lock_in_counter(); - update_prev(x, y, z); - last_valid_time = now; // Update the last valid time + incrementLockInCounter(); + updatePrev(x, y, z); + _last_valid_time = now; // Update the last valid time - return state == TRACKING; + return true; } -void ValidityFilter::update_prev(float x, float y, float z) +void ValidityFilter::updatePrev(float x, float y, float z) { - prev_x[prev_idx] = x; - prev_y[prev_idx] = y; - prev_z[prev_idx] = z; - prev_idx = (prev_idx + 1) % prev_len; + _prev_x[_prev_idx] = x; + _prev_y[_prev_idx] = y; + _prev_z[_prev_idx] = z; + _prev_idx = (_prev_idx + 1) % _prev_len; } -bool ValidityFilter::distance_validity(float x, float y, float z) +bool ValidityFilter::distanceValidity(float x, float y, float z) { float dst = std::sqrt(x * x + y * y + z * z); - return (dst <= max_distance && dst >= min_distance); + return (dst <= _max_distance && dst >= _min_distance); } -bool ValidityFilter::position_validity(float x, float y, float z) +bool ValidityFilter::positionValidity(float x, float y, float z) { - for (int i = 0; i < prev_len; i++) + for (int i = 0; i < _prev_len; i++) { - float dx = x - prev_x[i]; - float dy = y - prev_y[i]; - float dz = z - prev_z[i]; - if (std::sqrt(dx * dx + dy * dy + dz * dz) < max_shift_distance) + float dx = x - _prev_x[i]; + float dy = y - _prev_y[i]; + float dz = z - _prev_z[i]; + if (std::sqrt(dx * dx + dy * dy + dz * dz) < _max_shift_distance) { return true; // Valid as soon as one match is found } @@ -102,12 +98,12 @@ bool ValidityFilter::position_validity(float x, float y, float z) return false; // No valid matches found } -void ValidityFilter::increment_lock_in_counter() +void ValidityFilter::incrementLockInCounter() { - if (++lock_in_counter >= lock_in_after) + if (++_lock_in_counter >= _lock_in_after) { state = TRACKING; - lock_in_counter = lock_in_after; // Cap at max + _lock_in_counter = _lock_in_after; // Cap at max } else { @@ -115,12 +111,12 @@ void ValidityFilter::increment_lock_in_counter() } } -void ValidityFilter::decrement_lock_in_counter() +void ValidityFilter::decrementLockInCounter() { - if (--lock_in_counter <= 0) + if (--_lock_in_counter <= 0) { state = STOPPING; - lock_in_counter = 0; // Ensure no negative values + _lock_in_counter = 0; // Ensure no negative values } else { @@ -128,13 +124,13 @@ void ValidityFilter::decrement_lock_in_counter() } } -void ValidityFilter::reset_lock_in_counter() +void ValidityFilter::resetLockInCounter() { - lock_in_counter = 0; + _lock_in_counter = 0; state = STOPPING; } -int ValidityFilter::get_lock_in_counter() +int ValidityFilter::getLockInCounter() { - return lock_in_counter; + return _lock_in_counter; } diff --git a/src/prm_vision/pose_estimator/test/test_ValidityFilter.cpp b/src/prm_vision/pose_estimator/test/test_ValidityFilter.cpp new file mode 100644 index 0000000..1b132c4 --- /dev/null +++ b/src/prm_vision/pose_estimator/test/test_ValidityFilter.cpp @@ -0,0 +1,18 @@ +#include "ValidityFilter.hpp" +#include "gtest/gtest.h" + +class ValidityFilterTest : public ::testing::Test +{ +protected: + ValidityFilter *filter; + + void SetUp() override + { + filter = new ValidityFilter(); + } + + void TearDown() override + { + delete filter; + } +}; \ No newline at end of file From 35ed2680aac3b9cfe10dc57c3ad9b02d5a3277d1 Mon Sep 17 00:00:00 2001 From: Tom_TKO Date: Thu, 2 Jan 2025 02:40:42 -0500 Subject: [PATCH 5/7] tests for validity filter anf fix functionality. add option to run just one node's tests in run script --- run | 30 ++- src/prm_launch/launch/video2detector.py | 8 +- .../pose_estimator/include/ValidityFilter.hpp | 22 +- .../pose_estimator/src/PoseEstimator.cpp | 4 +- .../pose_estimator/src/PoseEstimatorNode.cpp | 2 +- .../pose_estimator/src/ValidityFilter.cpp | 11 +- .../test/test_ValidityFilter.cpp | 215 +++++++++++++++++- 7 files changed, 265 insertions(+), 27 deletions(-) diff --git a/run b/run index 2400b1f..7461b9f 100755 --- a/run +++ b/run @@ -57,18 +57,33 @@ build() { # Function to run tests for each ROS module (non-recursively at first) test() { - print_green "Running tests for each ROS module..." + print_green "Running tests for ROS modules..." # Run build first build - # Run tests quietly for both - print_green "[*] Running tests..." + # Check if a package name is provided + local package_name=$1 + + # Run tests + if [[ -z "$package_name" ]]; then + print_green "Running all tests..." + else + print_green "Running tests for package: $package_name" + fi if [[ "$quiet" == "true" ]]; then - colcon test >> "$LOG_FILE" 2>&1 + if [[ -n "$package_name" ]]; then + colcon test --packages-select "$package_name" >> "$LOG_FILE" 2>&1 + else + colcon test >> "$LOG_FILE" 2>&1 + fi colcon test-result --verbose >> "$LOG_FILE" 2>&1 else - colcon test + if [[ -n "$package_name" ]]; then + colcon test --packages-select "$package_name" + else + colcon test + fi colcon test-result --verbose fi @@ -122,7 +137,8 @@ while [[ $# -gt 0 ]]; do shift ;; test) - test + shift + test "$1" # Pass the next argument (package name) to the test function shift ;; run) @@ -131,7 +147,7 @@ while [[ $# -gt 0 ]]; do shift $# ;; *) - echo "Usage: $0 [--quiet] [--debug] {build|clean|test|run }" + echo "Usage: $0 [--quiet] [--debug] {build|clean|test [package_name]|run }" exit 1 ;; esac diff --git a/src/prm_launch/launch/video2detector.py b/src/prm_launch/launch/video2detector.py index 3a7c090..23ec677 100755 --- a/src/prm_launch/launch/video2detector.py +++ b/src/prm_launch/launch/video2detector.py @@ -15,12 +15,16 @@ def generate_launch_description(): emulate_tty=True, executable='VideoCaptureNode', parameters=[{'source': str(video_path), - 'fps': 200, + 'fps': 1, 'frame_id': 'video', }] ), Node( package='opencv_armor_detector', executable='OpenCVArmorDetectorNode', - ) + ), + Node( + package='pose_estimator', + executable='PoseEstimatorNode', + ), ]) diff --git a/src/prm_vision/pose_estimator/include/ValidityFilter.hpp b/src/prm_vision/pose_estimator/include/ValidityFilter.hpp index 6b4746a..edca90c 100644 --- a/src/prm_vision/pose_estimator/include/ValidityFilter.hpp +++ b/src/prm_vision/pose_estimator/include/ValidityFilter.hpp @@ -22,22 +22,31 @@ class ValidityFilter ValidityFilter(); ~ValidityFilter(); - bool resetKalman(float x, float y, float z); + bool shouldResetKalman(float x, float y, float z); + bool positionValidity(float, float, float); + bool distanceValidity(float, float, float); + void updatePrev(float, float, float); int getLockInCounter(); ValidityFilterState state = STOPPING; + double _max_dt = 2000; // ms + + // Getters + float *getPrevX() { return _prev_x; } + float *getPrevY() { return _prev_y; } + float *getPrevZ() { return _prev_z; } -private: +protected: int _lock_in_after = 3; // lock in after n frames int _lock_in_counter = 0; - std::chrono::steady_clock::time_point _last_valid_time = std::chrono::steady_clock::now(); + // zero time point + std::chrono::steady_clock::time_point _last_valid_time = std::chrono::steady_clock::time_point::min(); float _max_distance = 10000.f; // mm float _min_distance = 10.f; // mm - double _max_dt = 2000; // ms - int _prev_len = 5; // check the back n frames for max shift distance vilolation + int _prev_len = 5; // check the back n frames for max shift distance vilolation float _prev_x[20]; float _prev_y[20]; @@ -46,9 +55,6 @@ class ValidityFilter float _max_shift_distance = 150.f; // mm - void updatePrev(float, float, float); - bool positionValidity(float, float, float); - bool distanceValidity(float, float, float); void incrementLockInCounter(); void decrementLockInCounter(); void resetLockInCounter(); diff --git a/src/prm_vision/pose_estimator/src/PoseEstimator.cpp b/src/prm_vision/pose_estimator/src/PoseEstimator.cpp index 337ffdf..c7e16b3 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimator.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimator.cpp @@ -72,7 +72,7 @@ double PoseEstimator::estimateYaw(const double yaw_guess, const std::vector _num_frames_to_fire_after) + if (_consecutive_tracking_frames_ctr >= _num_frames_to_fire_after) { auto_aim_status = "FIRE"; } diff --git a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp index 27061fe..96a6832 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp @@ -37,7 +37,7 @@ void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::Sha // Compute armor's pose and validate it. We compute new yaw estimate based on the last yaw estimate. pose_estimator->estimateTranslation(image_points, key_points_msg->is_large_armor, tvec, rvec); _last_yaw_estimate = pose_estimator->estimateYaw(_last_yaw_estimate, image_points, tvec); - bool valid_pose_estimate = pose_estimator->isValid(tvec.at(0), tvec.at(1), tvec.at(2), new_auto_aim_status, reset_kalman); + bool valid_pose_estimate = pose_estimator->isValid(tvec.at(0), tvec.at(1), tvec.at(2), new_auto_aim_status, reset_kalman); // Publish the predicted armor if the pose is valid (we are tracking or firing) if (valid_pose_estimate) diff --git a/src/prm_vision/pose_estimator/src/ValidityFilter.cpp b/src/prm_vision/pose_estimator/src/ValidityFilter.cpp index b63563e..5417c59 100644 --- a/src/prm_vision/pose_estimator/src/ValidityFilter.cpp +++ b/src/prm_vision/pose_estimator/src/ValidityFilter.cpp @@ -31,11 +31,11 @@ ValidityFilter::~ValidityFilter() {} * NOTE: The return value is ONLY used to determine if we should reset the Kalman filter inside PoseEstimator. * We primarily use the state variable to determine the action to take (idle, track, stop). */ -bool ValidityFilter::resetKalman(float x, float y, float z) +bool ValidityFilter::shouldResetKalman(float x, float y, float z) { // Calculate time difference since last valid detection auto now = std::chrono::steady_clock::now(); - double dt = std::chrono::duration(now - _last_valid_time).count(); + double dt = std::chrono::duration(now - _last_valid_time).count(); // Check distance validity if (!distanceValidity(x, y, z)) @@ -49,8 +49,9 @@ bool ValidityFilter::resetKalman(float x, float y, float z) if (dt > _max_dt) { resetLockInCounter(); + _last_valid_time = now; // Reset the timer even after a timeout, prevents this check from always failing if we fail after succeeding for _max_dt updatePrev(x, y, z); - return state == STOPPING; // too much time has passed, so invalid if we are stopping + return state != STOPPING; // too much time has passed, so invalid if we aren't stopping } // Check position validity @@ -58,7 +59,7 @@ bool ValidityFilter::resetKalman(float x, float y, float z) { decrementLockInCounter(); updatePrev(x, y, z); - return state == STOPPING; // too much shift, so invalid if we are stopping + return state != STOPPING; // too much shift, so invalid if we aren't stopping } // Valid detection @@ -66,7 +67,7 @@ bool ValidityFilter::resetKalman(float x, float y, float z) updatePrev(x, y, z); _last_valid_time = now; // Update the last valid time - return true; + return false; } void ValidityFilter::updatePrev(float x, float y, float z) diff --git a/src/prm_vision/pose_estimator/test/test_ValidityFilter.cpp b/src/prm_vision/pose_estimator/test/test_ValidityFilter.cpp index 1b132c4..efaf2e3 100644 --- a/src/prm_vision/pose_estimator/test/test_ValidityFilter.cpp +++ b/src/prm_vision/pose_estimator/test/test_ValidityFilter.cpp @@ -1,5 +1,7 @@ #include "ValidityFilter.hpp" #include "gtest/gtest.h" +#include +#include class ValidityFilterTest : public ::testing::Test { @@ -8,11 +10,220 @@ class ValidityFilterTest : public ::testing::Test void SetUp() override { - filter = new ValidityFilter(); + // Explicitly set the parameters, in case a default param changes + filter = new ValidityFilter(3, 10000, 10, 150, 5); } void TearDown() override { delete filter; } -}; \ No newline at end of file +}; + +TEST_F(ValidityFilterTest, test_updatePrev) +{ + ValidityFilter *filter_temp = new ValidityFilter(3, 10000, 10, 150, 5); + + // write 6 values to the filter, should wrap around + for (int i = 0; i < 6; i++) + { + filter_temp->updatePrev(i, i, i); + } + + // check the values, first should be overwritten as 5 + // 0 -> 01 -> 012 -> 0123 -> 01234 -> 51234 + float *prev_x = filter_temp->getPrevX(); + float *prev_y = filter_temp->getPrevY(); + float *prev_z = filter_temp->getPrevZ(); + + EXPECT_EQ(prev_x[0], 5); + EXPECT_EQ(prev_y[0], 5); + EXPECT_EQ(prev_z[0], 5); + + for (int i = 1; i < 5; i++) + { + EXPECT_EQ(prev_x[i], i); + EXPECT_EQ(prev_y[i], i); + EXPECT_EQ(prev_z[i], i); + } +} + +TEST_F(ValidityFilterTest, test_distanceValidity) +{ + // Valid values + EXPECT_TRUE(filter->distanceValidity(10, 0, 0)); + EXPECT_TRUE(filter->distanceValidity(0, 10, 0)); + EXPECT_TRUE(filter->distanceValidity(0, 0, 10)); + EXPECT_TRUE(filter->distanceValidity(10, 10, 10)); + EXPECT_TRUE(filter->distanceValidity(10000, 0, 0)); + EXPECT_TRUE(filter->distanceValidity(0, 10000, 0)); + EXPECT_TRUE(filter->distanceValidity(0, 0, 10000)); + + // Right at the edge + // dist = sqrt(x^2 + x^2 + x^2) -> x = sqrt(dist^2 / 3) + double lower = std::sqrt(pow(10000, 2) / 3); + EXPECT_TRUE(filter->distanceValidity(lower, lower, lower)); + + // Too high values + EXPECT_FALSE(filter->distanceValidity(10001, 0, 0)); + EXPECT_FALSE(filter->distanceValidity(0, 10001, 0)); + EXPECT_FALSE(filter->distanceValidity(0, 0, 10001)); + EXPECT_FALSE(filter->distanceValidity(10001, 10001, 10001)); + EXPECT_FALSE(filter->distanceValidity(lower + 0.01, lower, lower)); + + // Too low values + EXPECT_FALSE(filter->distanceValidity(0, 0, 0)); + EXPECT_FALSE(filter->distanceValidity(0, 0, 9.99)); + EXPECT_FALSE(filter->distanceValidity(0, 9, 0.5)); +} + +TEST_F(ValidityFilterTest, test_positionValidity) +{ + // Valid values + filter->updatePrev(0, 0, 0); + EXPECT_TRUE(filter->positionValidity(0, 0, 0)); + EXPECT_TRUE(filter->positionValidity(0, 149.0, 0.5)); + EXPECT_TRUE(filter->positionValidity(0, 0, 0)); + EXPECT_TRUE(filter->positionValidity(0, 0, 0)); + EXPECT_TRUE(filter->positionValidity(0, 0, 0)); + + // Too high values + EXPECT_FALSE(filter->positionValidity(150.1, 0, 0)); + EXPECT_FALSE(filter->positionValidity(0, 150.1, 0)); + EXPECT_FALSE(filter->positionValidity(0, 0, 150.1)); + EXPECT_FALSE(filter->positionValidity(150.1, 150.1, 150.1)); + + // Initially invalid, then valid after a "detection" with values closer + EXPECT_FALSE(filter->positionValidity(150.1, 0, 0)); + filter->updatePrev(100, 0, 0); + EXPECT_TRUE(filter->positionValidity(150.1, 0, 0)); + + // Initially invalid, then still invalid, then valid after a "detection" with values closer + EXPECT_FALSE(filter->positionValidity(450.1, 0, 0)); + filter->updatePrev(100, 0, 0); + EXPECT_FALSE(filter->positionValidity(450.1, 0, 0)); + filter->updatePrev(400, 0, 0); + EXPECT_TRUE(filter->positionValidity(450.1, 0, 0)); +} + +TEST_F(ValidityFilterTest, test_shouldResetKalman_states) +{ + ValidityFilter *filter_temp = new ValidityFilter(3, 10000, 10, 150, 5); + + // Initial detection will always be INVALID since we have no prior detections for positionValidity, and dt is too high + EXPECT_EQ(filter_temp->state, STOPPING); + EXPECT_EQ(filter_temp->getLockInCounter(), 0); + EXPECT_EQ(filter_temp->getPrevX()[0], 0); + EXPECT_EQ(filter_temp->getPrevY()[0], 0); + EXPECT_EQ(filter_temp->getPrevZ()[0], 0); + EXPECT_FALSE(filter_temp->shouldResetKalman(100, 200, 100)); // But do not reset KF since we are in STOPPING + EXPECT_EQ(filter_temp->state, STOPPING); + EXPECT_EQ(filter_temp->getLockInCounter(), 0); + // However we still cache the values + EXPECT_EQ(filter_temp->getPrevX()[0], 100); + EXPECT_EQ(filter_temp->getPrevY()[0], 200); + EXPECT_EQ(filter_temp->getPrevZ()[0], 100); + // Now let's get another detection, this time it should be valid and put us in IDLING + EXPECT_FALSE(filter_temp->shouldResetKalman(123, 210, 123)); + EXPECT_EQ(filter_temp->state, IDLING); + EXPECT_EQ(filter_temp->getLockInCounter(), 1); + EXPECT_EQ(filter_temp->getPrevX()[1], 123); + EXPECT_EQ(filter_temp->getPrevY()[1], 210); + EXPECT_EQ(filter_temp->getPrevZ()[1], 123); + // Another valid detection, still in IDLING + EXPECT_FALSE(filter_temp->shouldResetKalman(124, 211, 124)); + EXPECT_EQ(filter_temp->state, IDLING); + EXPECT_EQ(filter_temp->getLockInCounter(), 2); + EXPECT_EQ(filter_temp->getPrevX()[2], 124); + EXPECT_EQ(filter_temp->getPrevY()[2], 211); + EXPECT_EQ(filter_temp->getPrevZ()[2], 124); + // Let's do an invalid detection, we should still be in IDLING but counter should decrement + EXPECT_TRUE(filter_temp->shouldResetKalman(999, 999, 999)); // Reset KF since we are too far + EXPECT_EQ(filter_temp->state, IDLING); + EXPECT_EQ(filter_temp->getLockInCounter(), 1); + EXPECT_EQ(filter_temp->getPrevX()[3], 999); + EXPECT_EQ(filter_temp->getPrevY()[3], 999); + EXPECT_EQ(filter_temp->getPrevZ()[3], 999); + // Another invalid detection, we will go to STOPPING since lock in counter decrements to 0 + EXPECT_FALSE(filter_temp->shouldResetKalman(1999, 1999, 1999)); // This puts us in STOPPING, so we actually don't reset KF + EXPECT_EQ(filter_temp->state, STOPPING); + EXPECT_EQ(filter_temp->getLockInCounter(), 0); + EXPECT_FALSE(filter_temp->shouldResetKalman(2999, 2999, 2999)); // This is invalid, but we are already in STOPPING + EXPECT_EQ(filter_temp->state, STOPPING); + EXPECT_EQ(filter_temp->getLockInCounter(), 0); + // Should advance to IDLING after a valid detection + EXPECT_FALSE(filter_temp->shouldResetKalman(125, 212, 125)); + EXPECT_EQ(filter_temp->state, IDLING); + EXPECT_EQ(filter_temp->getLockInCounter(), 1); + // Should advance to TRACKING after 2 more valid detections + EXPECT_FALSE(filter_temp->shouldResetKalman(126, 213, 126)); + EXPECT_EQ(filter_temp->state, IDLING); + EXPECT_EQ(filter_temp->getLockInCounter(), 2); + EXPECT_FALSE(filter_temp->shouldResetKalman(127, 214, 127)); + EXPECT_EQ(filter_temp->state, TRACKING); + EXPECT_EQ(filter_temp->getLockInCounter(), 3); + // Should stay in TRACKING after 3 more valid detections + EXPECT_FALSE(filter_temp->shouldResetKalman(128, 215, 128)); + EXPECT_EQ(filter_temp->state, TRACKING); + EXPECT_EQ(filter_temp->getLockInCounter(), 3); + EXPECT_FALSE(filter_temp->shouldResetKalman(129, 216, 129)); + EXPECT_EQ(filter_temp->state, TRACKING); + EXPECT_EQ(filter_temp->getLockInCounter(), 3); + EXPECT_FALSE(filter_temp->shouldResetKalman(130, 217, 130)); + EXPECT_EQ(filter_temp->state, TRACKING); + EXPECT_EQ(filter_temp->getLockInCounter(), 3); +} + +TEST_F(ValidityFilterTest, test_shouldResetKalman_failures) +{ + ValidityFilter *filter_temp = new ValidityFilter(3, 10000, 10, 150, 5); + EXPECT_EQ(filter_temp->state, STOPPING); + EXPECT_EQ(filter_temp->getLockInCounter(), 0); + EXPECT_EQ(filter_temp->getPrevX()[0], 0); + EXPECT_EQ(filter_temp->getPrevY()[0], 0); + EXPECT_EQ(filter_temp->getPrevZ()[0], 0); + + // Two valid to get two into IDLING, then one to fail due to distance + EXPECT_FALSE(filter_temp->shouldResetKalman(100, 200, 100)); + EXPECT_EQ(filter_temp->state, STOPPING); + EXPECT_EQ(filter_temp->getLockInCounter(), 0); + EXPECT_FALSE(filter_temp->shouldResetKalman(123, 210, 123)); + EXPECT_FALSE(filter_temp->shouldResetKalman(123, 210, 123)); + EXPECT_EQ(filter_temp->state, IDLING); + EXPECT_EQ(filter_temp->getLockInCounter(), 2); + EXPECT_TRUE(filter_temp->shouldResetKalman(12400, 21100, 12400)); // Not in stopping, so we reset KF + EXPECT_EQ(filter_temp->state, IDLING); + EXPECT_EQ(filter_temp->getLockInCounter(), 1); + EXPECT_FALSE(filter_temp->shouldResetKalman(22400, 21100, 12400)); // In stopping, so we don't reset KF + EXPECT_EQ(filter_temp->state, STOPPING); + EXPECT_EQ(filter_temp->getLockInCounter(), 0); + + // From here, two valids to get two into IDLING, then one to fail due to dt + EXPECT_FALSE(filter_temp->shouldResetKalman(100, 200, 100)); + EXPECT_FALSE(filter_temp->shouldResetKalman(123, 210, 123)); + EXPECT_EQ(filter_temp->state, IDLING); + EXPECT_EQ(filter_temp->getLockInCounter(), 2); + // sleep for _max_dt and then invalid. Will reset lock in counter + std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(filter_temp->_max_dt) + 500)); + EXPECT_FALSE(filter_temp->shouldResetKalman(123, 210, 123)); // We reset lock in counter, so ALWAYS regress to STOPPING + EXPECT_EQ(filter_temp->state, STOPPING); + EXPECT_EQ(filter_temp->getLockInCounter(), 0); + + // From here, two valids to get two into IDLING, then one to fail due to shift + EXPECT_FALSE(filter_temp->shouldResetKalman(123, 210, 123)); + EXPECT_FALSE(filter_temp->shouldResetKalman(123, 210, 123)); + EXPECT_EQ(filter_temp->state, IDLING); + EXPECT_EQ(filter_temp->getLockInCounter(), 2); + // Invalid due to shift, should reset KF + EXPECT_TRUE(filter_temp->shouldResetKalman(400, 400, 400)); // Not in stopping, so we reset KF + EXPECT_EQ(filter_temp->state, IDLING); + EXPECT_EQ(filter_temp->getLockInCounter(), 1); + EXPECT_FALSE(filter_temp->shouldResetKalman(600, 400, 400)); // Not in stopping, so we reset KF + EXPECT_EQ(filter_temp->state, STOPPING); + EXPECT_EQ(filter_temp->getLockInCounter(), 0); + // Back to tracking and we call it a day + EXPECT_FALSE(filter_temp->shouldResetKalman(100, 200, 100)); + EXPECT_FALSE(filter_temp->shouldResetKalman(123, 210, 123)); + EXPECT_EQ(filter_temp->state, IDLING); + EXPECT_EQ(filter_temp->getLockInCounter(), 2); +} \ No newline at end of file From 080f9bf5427f296da6c237d1e11a2308d7a538d8 Mon Sep 17 00:00:00 2001 From: Tom_TKO Date: Thu, 2 Jan 2025 23:50:12 -0500 Subject: [PATCH 6/7] validityfilter and pose estimator variables as ros2 params --- src/prm_launch/launch/video2detector.py | 2 +- .../src/OpenCVArmorDetector.cpp | 6 ++ .../src/OpenCVArmorDetectorNode.cpp | 8 ++- .../pose_estimator/include/PoseEstimator.h | 13 +++- .../include/PoseEstimatorNode.hpp | 7 +- .../pose_estimator/include/ValidityFilter.hpp | 26 ++++--- .../pose_estimator/src/PoseEstimator.cpp | 4 +- .../pose_estimator/src/PoseEstimatorNode.cpp | 69 ++++++++++++++++++- .../pose_estimator/src/ValidityFilter.cpp | 5 +- 9 files changed, 117 insertions(+), 23 deletions(-) diff --git a/src/prm_launch/launch/video2detector.py b/src/prm_launch/launch/video2detector.py index 23ec677..e50a200 100755 --- a/src/prm_launch/launch/video2detector.py +++ b/src/prm_launch/launch/video2detector.py @@ -15,7 +15,7 @@ def generate_launch_description(): emulate_tty=True, executable='VideoCaptureNode', parameters=[{'source': str(video_path), - 'fps': 1, + 'fps': 4, 'frame_id': 'video', }] ), diff --git a/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp b/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp index 3ec33d5..656421a 100644 --- a/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp +++ b/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetector.cpp @@ -92,6 +92,12 @@ std::vector<_Float32> OpenCVArmorDetector::search(cv::Mat &frame) _search_area[2] = std::min(x_max + 50, WIDTH); _search_area[3] = std::min(y_max + 50, HEIGHT); } + else + { + // Reset the search area to the full frame + _reset_search_area = true; + } + _detected_frame++; } diff --git a/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetectorNode.cpp b/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetectorNode.cpp index 6f85f93..704d3d6 100644 --- a/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetectorNode.cpp +++ b/src/prm_vision/opencv_armor_detector/src/OpenCVArmorDetectorNode.cpp @@ -68,6 +68,12 @@ rcl_interfaces::msg::SetParametersResult OpenCVArmorDetectorNode::parameters_cal RCLCPP_INFO(get_logger(), "New max missed frames: %d", _max_missed_frames); } + else if (parameter.get_name() == "_reduce_search_area") + { + _reduce_search_area = parameter.as_bool(); + RCLCPP_INFO(get_logger(), "New reduce search area: %s", + _reduce_search_area ? "true" : "false"); + } else { result.successful = false; @@ -76,7 +82,7 @@ rcl_interfaces::msg::SetParametersResult OpenCVArmorDetectorNode::parameters_cal } // Update the detector's config - detector->setConfig({_target_color, _hue_range_limit, _saturation_lower_limit, _value_lower_limit, _max_missed_frames}); + detector->setConfig({_target_color, _hue_range_limit, _saturation_lower_limit, _value_lower_limit, _max_missed_frames, _reduce_search_area}); return result; } diff --git a/src/prm_vision/pose_estimator/include/PoseEstimator.h b/src/prm_vision/pose_estimator/include/PoseEstimator.h index 5a89be5..32e0e4b 100644 --- a/src/prm_vision/pose_estimator/include/PoseEstimator.h +++ b/src/prm_vision/pose_estimator/include/PoseEstimator.h @@ -25,14 +25,18 @@ class PoseEstimator PoseEstimator() {} ~PoseEstimator() {} + ValidityFilter validity_filter_ = ValidityFilter(); + // Class methods double estimateYaw(const double yaw_guess, const std::vector &image_points, const cv::Mat &tvec); void estimateTranslation(const std::vector &image_points, bool largeArmor, cv::Mat &tvec, cv::Mat &rvec); bool isValid(float x, float y, float z, std::string &auto_aim_status, bool &reset_kalman); + // Setters + void setNumFramesToFireAfter(int num_frames_to_fire_after) { _num_frames_to_fire_after = num_frames_to_fire_after; } + private: // Class methods - ValidityFilter validity_filter_ = ValidityFilter(); double computeLoss(double yaw_guess, std::vector image_points, cv::Mat tvec); double gradientWrtYawFinitediff(double yaw, std::vector image_points, cv::Mat tvec); @@ -41,6 +45,13 @@ class PoseEstimator int _num_frames_to_fire_after = 3; std::chrono::time_point _last_fire_time; + // Validity filter parameters + int _lock_in_after = 3; + float _max_distance = 10000; + float _min_distance = 10; + float _max_shift_distance = 150; + int _prev_len = 5; + /** * @brief Functor for the loss function and gradient computation. * diff --git a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp index 3016be5..9d7b9c5 100644 --- a/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp +++ b/src/prm_vision/pose_estimator/include/PoseEstimatorNode.hpp @@ -17,7 +17,6 @@ // Pose Estimator classes #include "PoseEstimator.h" -#include "ValidityFilter.hpp" class PoseEstimatorNode : public rclcpp::Node { @@ -25,8 +24,10 @@ class PoseEstimatorNode : public rclcpp::Node PoseEstimatorNode(const rclcpp::NodeOptions &options); ~PoseEstimatorNode(); + PoseEstimator *pose_estimator = new PoseEstimator(); + ValidityFilter &validity_filter_ = pose_estimator->validity_filter_; // Reference to the validity filter + private: - PoseEstimator *pose_estimator; double _last_yaw_estimate = 0.0; int _allowed_missed_frames_before_no_fire = 0; // Gets set to 5 when we have a valid pose estimate @@ -37,6 +38,8 @@ class PoseEstimatorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr key_points_subscriber; std::shared_ptr> predicted_armor_publisher; void keyPointsCallback(const vision_msgs::msg::KeyPoints::SharedPtr msg); + OnSetParametersCallbackHandle::SharedPtr params_callback_handle_; + rcl_interfaces::msg::SetParametersResult parameters_callback(const std::vector ¶meters); }; #endif // POSE_ESTIMATOR_NODE_HPP \ No newline at end of file diff --git a/src/prm_vision/pose_estimator/include/ValidityFilter.hpp b/src/prm_vision/pose_estimator/include/ValidityFilter.hpp index edca90c..6402932 100644 --- a/src/prm_vision/pose_estimator/include/ValidityFilter.hpp +++ b/src/prm_vision/pose_estimator/include/ValidityFilter.hpp @@ -17,9 +17,8 @@ enum ValidityFilterState class ValidityFilter { public: - ValidityFilter(int lock_in_after, float max_distance, float min_distance, float max_shift_distance, float prev_len); + ValidityFilter(int lock_in_after = 3, float max_distance = 10000, float min_distance = 10, float max_shift_distance = 150, int prev_len = 5); - ValidityFilter(); ~ValidityFilter(); bool shouldResetKalman(float x, float y, float z); @@ -35,26 +34,33 @@ class ValidityFilter float *getPrevX() { return _prev_x; } float *getPrevY() { return _prev_y; } float *getPrevZ() { return _prev_z; } + int getLockInAfter() { return _lock_in_after; } + + // Setters + void setLockInAfter(int lock_in_after) { _lock_in_after = lock_in_after; } + void setMaxDistance(float max_distance) { _max_distance = max_distance; } + void setMinDistance(float min_distance) { _min_distance = min_distance; } + void setMaxShiftDistance(float max_shift_distance) { _max_shift_distance = max_shift_distance; } + void setPrevLen(int prev_len) { _prev_len = prev_len; } + void setMaxDt(double max_dt) { _max_dt = max_dt; } protected: - int _lock_in_after = 3; // lock in after n frames + int _lock_in_after; // lock in after n frames + float _max_distance; // mm + float _min_distance; // mm + float _max_shift_distance; // mm + int _prev_len; // check the back n frames for max shift distance vilolation + int _lock_in_counter = 0; // zero time point std::chrono::steady_clock::time_point _last_valid_time = std::chrono::steady_clock::time_point::min(); - float _max_distance = 10000.f; // mm - float _min_distance = 10.f; // mm - - int _prev_len = 5; // check the back n frames for max shift distance vilolation - float _prev_x[20]; float _prev_y[20]; float _prev_z[20]; int _prev_idx = 0; - float _max_shift_distance = 150.f; // mm - void incrementLockInCounter(); void decrementLockInCounter(); void resetLockInCounter(); diff --git a/src/prm_vision/pose_estimator/src/PoseEstimator.cpp b/src/prm_vision/pose_estimator/src/PoseEstimator.cpp index c7e16b3..905cefa 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimator.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimator.cpp @@ -93,8 +93,8 @@ bool PoseEstimator::isValid(float x, float y, float z, std::string &auto_aim_sta // We have enough valid detections to track the target else if (validity_filter_.state == TRACKING) { - // if validity filter valid for last 3 frames, increment locked_in_frames - if (validity_filter_.getLockInCounter() == 3) + // if validity filter valid for last number of frames, increment locked_in_frames + if (validity_filter_.getLockInCounter() == validity_filter_.getLockInAfter()) { _consecutive_tracking_frames_ctr++; } diff --git a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp index 96a6832..99d3af7 100644 --- a/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp +++ b/src/prm_vision/pose_estimator/src/PoseEstimatorNode.cpp @@ -4,16 +4,79 @@ PoseEstimatorNode::PoseEstimatorNode(const rclcpp::NodeOptions &options) : Node( { RCLCPP_INFO(get_logger(), "PoseEstimatorNode has been started."); - // Initialize the pose estimator - pose_estimator = new PoseEstimator(); - // Callbacks and pub/sub key_points_subscriber = this->create_subscription("key_points", 10, std::bind(&PoseEstimatorNode::keyPointsCallback, this, std::placeholders::_1)); predicted_armor_publisher = this->create_publisher("predicted_armor", 10); + + // Dynamic parameters + pose_estimator->setNumFramesToFireAfter(this->declare_parameter("_num_frames_to_fire_after", 3)); + validity_filter_.setLockInAfter(this->declare_parameter("_lock_in_after", 3)); + validity_filter_.setMaxDistance(this->declare_parameter("_max_distance", 10000)); + validity_filter_.setMinDistance(this->declare_parameter("_min_distance", 10)); + validity_filter_.setMaxShiftDistance(this->declare_parameter("_max_shift_distance", 150)); + validity_filter_.setPrevLen(this->declare_parameter("_prev_len", 5)); + validity_filter_.setMaxDt(this->declare_parameter("_max_dt", 2000.0)); + params_callback_handle_ = this->add_on_set_parameters_callback(std::bind(&PoseEstimatorNode::parameters_callback, this, std::placeholders::_1)); } PoseEstimatorNode::~PoseEstimatorNode() { delete pose_estimator; } +rcl_interfaces::msg::SetParametersResult PoseEstimatorNode::parameters_callback( + const std::vector ¶meters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "Parameters successfully updated."; + + for (const auto ¶m : parameters) + { + if (param.get_name() == "_num_frames_to_fire_after" && param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) + { + pose_estimator->setNumFramesToFireAfter(param.as_int()); + RCLCPP_INFO(this->get_logger(), "Parameter '_num_frames_to_fire_after' updated to: %d", param.as_int()); + } + else if (param.get_name() == "_lock_in_after" && param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) + { + validity_filter_.setLockInAfter(param.as_int()); + RCLCPP_INFO(this->get_logger(), "Parameter '_lock_in_after' updated to: %d", param.as_int()); + } + else if (param.get_name() == "_max_distance" && param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) + { + validity_filter_.setMaxDistance(param.as_int()); + RCLCPP_INFO(this->get_logger(), "Parameter '_max_distance' updated to: %d", param.as_int()); + } + else if (param.get_name() == "_min_distance" && param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) + { + validity_filter_.setMinDistance(param.as_int()); + RCLCPP_INFO(this->get_logger(), "Parameter '_min_distance' updated to: %d", param.as_int()); + } + else if (param.get_name() == "_max_shift_distance" && param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) + { + validity_filter_.setMaxShiftDistance(param.as_int()); + RCLCPP_INFO(this->get_logger(), "Parameter '_max_shift_distance' updated to: %d", param.as_int()); + } + else if (param.get_name() == "_prev_len" && param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) + { + validity_filter_.setPrevLen(param.as_int()); + RCLCPP_INFO(this->get_logger(), "Parameter '_prev_len' updated to: %d", param.as_int()); + } + else if (param.get_name() == "_max_dt" && param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE || param.get_type()) + { + validity_filter_.setMaxDt(param.as_double()); + RCLCPP_INFO(this->get_logger(), "Parameter '_max_dt' updated to: %f", param.as_double()); + RCLCPP_INFO(this->get_logger(), "max dt is %f", validity_filter_._max_dt); + } + else + { + result.successful = false; + result.reason = "Invalid parameter or type."; + RCLCPP_WARN(this->get_logger(), "Failed to update parameter: %s", param.get_name().c_str()); + } + } + + return result; +} + void PoseEstimatorNode::keyPointsCallback(const vision_msgs::msg::KeyPoints::SharedPtr key_points_msg) { if (key_points_msg->points.size() != 8 || key_points_msg->points[4] == 0) // idx 4 is x-coord of top right corner, so if it's 0, we know there's no armor diff --git a/src/prm_vision/pose_estimator/src/ValidityFilter.cpp b/src/prm_vision/pose_estimator/src/ValidityFilter.cpp index 5417c59..daefee7 100644 --- a/src/prm_vision/pose_estimator/src/ValidityFilter.cpp +++ b/src/prm_vision/pose_estimator/src/ValidityFilter.cpp @@ -1,12 +1,12 @@ #include "ValidityFilter.hpp" -ValidityFilter::ValidityFilter(int lock_in_after, float max_distance, float min_distance, float max_shift_distance, float prev_len) +ValidityFilter::ValidityFilter(int lock_in_after, float max_distance, float min_distance, float max_shift_distance, int prev_len) { this->_lock_in_after = lock_in_after; this->_max_distance = max_distance; this->_min_distance = min_distance; this->_max_shift_distance = max_shift_distance; - this->_prev_len = std::min(prev_len, 20.0f); + this->_prev_len = std::min(prev_len, 20); // Initialize arrays to track previous predictions for (int i = 0; i < this->_prev_len; i++) @@ -17,7 +17,6 @@ ValidityFilter::ValidityFilter(int lock_in_after, float max_distance, float min_ } } -ValidityFilter::ValidityFilter() {} ValidityFilter::~ValidityFilter() {} /** From 7002b5a28ffa36de7609a07525b7bb10550c9058 Mon Sep 17 00:00:00 2001 From: Tom_TKO Date: Fri, 3 Jan 2025 00:43:16 -0500 Subject: [PATCH 7/7] return 0 upon success --- run | 2 ++ 1 file changed, 2 insertions(+) diff --git a/run b/run index 7461b9f..5afd975 100755 --- a/run +++ b/run @@ -152,3 +152,5 @@ while [[ $# -gt 0 ]]; do ;; esac done + +exit 0 \ No newline at end of file