diff --git a/CMakeLists.txt b/CMakeLists.txt index 4d5f0121057eda48060b18b4e625f3ed5fe56f10..01bb44f1c27e19e25777a4a6978c47a6c835bd04 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -134,11 +134,15 @@ SET(HDRS_FACTOR include/${PROJECT_NAME}/factor/factor_pose_2d.h include/${PROJECT_NAME}/factor/factor_pose_3d.h include/${PROJECT_NAME}/factor/factor_quaternion_absolute.h + include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.h include/${PROJECT_NAME}/factor/factor_relative_pose_2d.h include/${PROJECT_NAME}/factor/factor_relative_pose_2d_with_extrinsics.h include/${PROJECT_NAME}/factor/factor_relative_pose_3d.h - include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.h include/${PROJECT_NAME}/factor/factor_relative_pose_3d_with_extrinsics.h + include/${PROJECT_NAME}/factor/factor_relative_position_2d.h + include/${PROJECT_NAME}/factor/factor_relative_position_2d_with_extrinsics.h + include/${PROJECT_NAME}/factor/factor_relative_position_3d.h + include/${PROJECT_NAME}/factor/factor_relative_position_3d_with_extrinsics.h include/${PROJECT_NAME}/factor/factor_velocity_local_direction_3d.h ) SET(HDRS_FEATURE diff --git a/include/core/factor/factor_relative_pose_3d.h b/include/core/factor/factor_relative_pose_3d.h index 3e6ebceb5354aed9f5ac67c0c5fb561ade536c89..4ea300d5c13d35aed9ff1c60ba57c822cd4386c6 100644 --- a/include/core/factor/factor_relative_pose_3d.h +++ b/include/core/factor/factor_relative_pose_3d.h @@ -66,7 +66,7 @@ class FactorRelativePose3d : public FactorAutodiff<FactorRelativePose3d,6,3,4,3, T* _expectation_dp, T* _expectation_dq) const; - Eigen::VectorXd expectation() const; + Eigen::Vector7d expectation() const; private: template<typename T> @@ -189,9 +189,9 @@ inline bool FactorRelativePose3d::expectation(const T* const _p_target, return true; } -inline Eigen::VectorXd FactorRelativePose3d::expectation() const +inline Eigen::Vector7d FactorRelativePose3d::expectation() const { - Eigen::VectorXd exp(7); + Eigen::Vector7d exp; auto frm_current = getFeature()->getFrame(); auto frm_past = getFrameOther(); auto lmk = getLandmarkOther(); @@ -201,8 +201,10 @@ inline Eigen::VectorXd FactorRelativePose3d::expectation() const const Eigen::VectorXd ref_pos = ( frm_past ? frm_past->getP()->getState() : frm_current->getP()->getState()); const Eigen::VectorXd ref_ori = ( frm_past ? frm_past->getO()->getState() : frm_current->getO()->getState()); -// std::cout << "target_pos: " << frm_current->getP()->getState().transpose() << std::endl; -// std::cout << "ref_pos: " << frm_past->getP()->getState().transpose() << std::endl; +// std::cout << "target_pos: " << target_pos.transpose() << std::endl; +// std::cout << "target_ori: " << target_ori.coeffs().transpose() << std::endl; +// std::cout << "ref_pos: " << ref_pos.transpose() << std::endl; +// std::cout << "ref_ori: " << ref_ori.coeffs().transpose() << std::endl; expectation(target_pos.data(), target_ori.data(), diff --git a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h index 680c619c3622092e4e0631fb6c74c594794ce70d..66468bbcafb2b4480df5077ca6b2a16d6ff61839 100644 --- a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h +++ b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h @@ -58,7 +58,7 @@ class FactorRelativePose3dWithExtrinsics : public FactorAutodiff<FactorRelativeP /** \brief Class Destructor */ - ~FactorRelativePose3dWithExtrinsics() override; + ~FactorRelativePose3dWithExtrinsics() override = default; /** brief : compute the residual from the state blocks being iterated by the solver. **/ @@ -116,6 +116,8 @@ inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(co _feature_ptr->getCapture()->getSensorP(), _feature_ptr->getCapture()->getSensorO()) { + assert(_feature_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!"); + assert(_feature_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!"); } inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(const FeatureBasePtr& _feature_ptr, @@ -143,11 +145,6 @@ inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(co { } -inline FactorRelativePose3dWithExtrinsics::~FactorRelativePose3dWithExtrinsics() -{ - // -} - template<typename T> inline bool FactorRelativePose3dWithExtrinsics::operator ()(const T* const _p_ref, const T* const _o_ref, diff --git a/include/core/factor/factor_relative_position_3d.h b/include/core/factor/factor_relative_position_3d.h new file mode 100644 index 0000000000000000000000000000000000000000..8d76dcb3406059f37218d0ace894ec1b3f9bc474 --- /dev/null +++ b/include/core/factor/factor_relative_position_3d.h @@ -0,0 +1,154 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see <http://www.gnu.org/licenses/>. +// +//--------LICENSE_END-------- + +#pragma once + +#include "core/factor/factor_autodiff.h" + +namespace wolf +{ + +WOLF_PTR_TYPEDEFS(FactorRelativePosition3d); + +//class +class FactorRelativePosition3d : public FactorAutodiff<FactorRelativePosition3d,3,3,4,3> +{ + public: + FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr, + const FrameBasePtr& _frame_past_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + const FactorTopology& _top, + FactorStatus _status = FAC_ACTIVE); + + FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr, + const LandmarkBasePtr& _landmark_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + const FactorTopology& _top, + FactorStatus _status = FAC_ACTIVE); + + ~FactorRelativePosition3d() override = default; + + template<typename T> + bool operator ()(const T* const _p_ref, + const T* const _q_ref, + const T* const _p_target, + T* _residuals) const; + + private: + template<typename T> + void printRes(const Eigen::Matrix<T, 3, 1>& r) const; +}; + +template<typename T> +inline void FactorRelativePosition3d::printRes(const Eigen::Matrix<T, 3, 1>& r) const +{ + std::cout << "Jet residual = " << std::endl; + std::cout << r(0) << std::endl; + std::cout << r(1) << std::endl; + std::cout << r(2) << std::endl; +} + +template<> +inline void FactorRelativePosition3d::printRes (const Eigen::Matrix<double,3,1> & r) const +{ + std::cout << "Scalar residual = " << std::endl; + std::cout << r.transpose() << std::endl; +} + +inline FactorRelativePosition3d::FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr, + const FrameBasePtr& _frame_past_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + const FactorTopology& _top, + FactorStatus _status) : + FactorAutodiff("FactorRelativePosition3d", // type + _top, // topology + _ftr_current_ptr, // feature + _frame_past_ptr, // frame other + nullptr, // capture other + nullptr, // feature other + nullptr, // landmark other + _processor_ptr, // processor + _apply_loss_function, + _status, + _frame_past_ptr->getP(), // past frame P + _frame_past_ptr->getO(), // past frame Q + _ftr_current_ptr->getFrame()->getP()) // current frame P +{ + MatrixSizeCheck<3,1>::check(_ftr_current_ptr->getMeasurement()); + MatrixSizeCheck<3,3>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper()); +} + +inline FactorRelativePosition3d::FactorRelativePosition3d(const FeatureBasePtr& _ftr_current_ptr, + const LandmarkBasePtr& _lmk_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + const FactorTopology& _top, + FactorStatus _status) : + FactorAutodiff("FactorRelativePosition3d", // type + _top, // topology + _ftr_current_ptr, // feature + nullptr, // frame other + nullptr, // capture other + nullptr, // feature other + _lmk_ptr, // landmark other + _processor_ptr, // processor + _apply_loss_function, + _status, + _ftr_current_ptr->getFrame()->getP(), // frame P + _ftr_current_ptr->getFrame()->getO(), // frame Q + _lmk_ptr->getP()) // landmark P +{ + MatrixSizeCheck<3,1>::check(_ftr_current_ptr->getMeasurement()); + MatrixSizeCheck<3,3>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper()); +} + +template<typename T> +inline bool FactorRelativePosition3d::operator ()(const T* const _p_ref, + const T* const _q_ref, + const T* const _p_target, + T* _residuals) const +{ + Eigen::Map<const Eigen::Matrix<T,3,1> > p_target(_p_target); + Eigen::Map<const Eigen::Matrix<T,3,1> > p_ref(_p_ref); + Eigen::Map<const Eigen::Quaternion<T> > q_ref(_q_ref); + Eigen::Map<Eigen::Matrix<T,3,1> > residuals(_residuals); + +// std::cout << "p_ref: " << p_ref(0) << " " +// << p_ref(1) << " " +// << p_ref(2) << "\n"; +// std::cout << "q_ref: " << q_ref.coeffs()(0) << " " +// << q_ref.coeffs()(1) << " " +// << q_ref.coeffs()(2) << " " +// << q_ref.coeffs()(3) << "\n"; +// std::cout << "p_target: " << p_target(0) << " " +// << p_target(1) << " " +// << p_target(2) << "\n"; + + residuals = getMeasurementSquareRootInformationUpper() * (getMeasurement() - (q_ref.conjugate() * (p_target - p_ref))); + + return true; +} + +} /* namespace wolf */ \ No newline at end of file diff --git a/include/core/factor/factor_relative_position_3d_with_extrinsics.h b/include/core/factor/factor_relative_position_3d_with_extrinsics.h new file mode 100644 index 0000000000000000000000000000000000000000..f892ac25a4adcb6c31f43f76b4fc3022a62b56ae --- /dev/null +++ b/include/core/factor/factor_relative_position_3d_with_extrinsics.h @@ -0,0 +1,149 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see <http://www.gnu.org/licenses/>. +// +//--------LICENSE_END-------- +#pragma once + +//Wolf includes +#include "core/factor/factor_autodiff.h" +#include "core/frame/frame_base.h" +#include "core/math/rotations.h" + +//#include "ceres/jet.h" + +namespace wolf { + +WOLF_PTR_TYPEDEFS(FactorRelativePosition3dWithExtrinsics); + +//class +class FactorRelativePosition3dWithExtrinsics : public FactorAutodiff<FactorRelativePosition3dWithExtrinsics, 3, 3, 4, 3, 3, 4> +{ + public: + + /** \brief Class constructor Frame-Landmark + */ + FactorRelativePosition3dWithExtrinsics(const FeatureBasePtr& _ftr_ptr, + const LandmarkBasePtr& _lmk_other_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + const FactorTopology& _top = TOP_LMK, + FactorStatus _status = FAC_ACTIVE); + + ~FactorRelativePosition3dWithExtrinsics() override = default; + + template<typename T> + bool operator ()(const T* const _p_ref, + const T* const _o_ref, + const T* const _p_target, + const T* const _p_sensor, + const T* const _o_sensor, + T* _residuals) const; + + Eigen::Vector3d residual() const; + double cost() const; +}; + +inline FactorRelativePosition3dWithExtrinsics::FactorRelativePosition3dWithExtrinsics(const FeatureBasePtr& _ftr_ptr, + const LandmarkBasePtr& _lmk_other_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + const FactorTopology& _top, + FactorStatus _status) : + FactorAutodiff("FactorRelativePosition3dWithExtrinsics", + _top, + _ftr_ptr, + nullptr, nullptr, nullptr, _lmk_other_ptr, + _processor_ptr, + _apply_loss_function, + _status, + _ftr_ptr->getFrame()->getP(), + _ftr_ptr->getFrame()->getO(), + _lmk_other_ptr->getP(), + _ftr_ptr->getCapture()->getSensorP(), + _ftr_ptr->getCapture()->getSensorO()) +{ + assert(_ftr_ptr->getCapture()->getSensorP() != nullptr && "No extrinsics found!"); + assert(_ftr_ptr->getCapture()->getSensorO() != nullptr && "No extrinsics found!"); +} + +template<typename T> +inline bool FactorRelativePosition3dWithExtrinsics::operator ()(const T* const _p_ref, + const T* const _o_ref, + const T* const _p_target, + const T* const _p_sensor, + const T* const _o_sensor, + T* _residuals) const +{ + // Maps + Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_s(_p_sensor); + Eigen::Map<const Eigen::Quaternion<T>> q_r_s(_o_sensor); + Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_r(_p_ref); + Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_ref); + Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_t(_p_target); + Eigen::Map<Eigen::Matrix<T,3,1>> residuals(_residuals); + + // WOLF_INFO("p_sensor: ", p_r_s.transpose()); + // WOLF_INFO("o_sensor: ", q_r_s.coeffs().transpose()); + // WOLF_INFO("p_ref: ", p_w_r.transpose()); + // WOLF_INFO("o_ref: ", q_w_r.coeffs().transpose()); + // WOLF_INFO("p_target: ", p_w_t.transpose()); + + // Expected measurement + Eigen::Matrix<T,3,1> expected_p; + expected_p = q_r_s.conjugate() * (q_w_r.conjugate() * (p_w_t - p_w_r) - p_r_s); + + // Residual + residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (getMeasurement() - expected_p); + + return true; +} + +inline Eigen::Vector3d FactorRelativePosition3dWithExtrinsics::residual() const +{ + Eigen::Vector3d res; + Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target; + p_sensor = getCapture()->getSensorP()->getState(); + o_sensor = getCapture()->getSensorO()->getState(); + // FRAME CASE + if (not getFrameOtherList().empty()) + { + p_ref = getFrameOther()->getP()->getState(); + o_ref = getFrameOther()->getO()->getState(); + p_target = getCapture()->getFrame()->getP()->getState(); + } + // LANDMARK CASE + else if (not getLandmarkOtherList().empty()) + { + p_ref = getCapture()->getFrame()->getP()->getState(); + o_ref = getCapture()->getFrame()->getO()->getState(); + p_target = getLandmarkOther()->getP()->getState(); + } + + operator() (p_ref.data(), o_ref.data(), p_target.data(), p_sensor.data(), o_sensor.data(), res.data()); + + return res; +} + +inline double FactorRelativePosition3dWithExtrinsics::cost() const +{ + return residual().squaredNorm(); +} + +} // namespace wolf \ No newline at end of file diff --git a/src/processor/processor_tracker_feature_landmark_external.cpp b/src/processor/processor_tracker_feature_landmark_external.cpp index 5b47399e2445a33c3af4b8bde838fc9019ae05e9..50a1730c2d3bf08c7020022448913ca7c857cf3c 100644 --- a/src/processor/processor_tracker_feature_landmark_external.cpp +++ b/src/processor/processor_tracker_feature_landmark_external.cpp @@ -26,7 +26,7 @@ #include "core/factor/factor_relative_pose_2d_with_extrinsics.h" #include "core/factor/factor_relative_position_2d_with_extrinsics.h" #include "core/factor/factor_relative_pose_3d_with_extrinsics.h" -//#include "core/factor/factor_relative_position_3d_with_extrinsics.h" +#include "core/factor/factor_relative_position_3d_with_extrinsics.h" #include "core/landmark/landmark_base.h" #include "core/state_block/state_block_derived.h" #include "core/state_block/state_quaternion.h" @@ -182,16 +182,19 @@ bool ProcessorTrackerFeatureLandmarkExternal::voteForKeyFrame() const if (track_matrix_.track(match.first).size() >= params_tfle_->filter_track_length_th) n_tracks++; } + WOLF_INFO("Nbr. of active feature tracks: " , n_tracks ); if (n_tracks == 0) return false; - WOLF_INFO("Nbr. of active feature tracks: " , n_tracks ); - - bool vote = n_tracks < params_tracker_feature_->min_features_for_keyframe; + bool vote = false;// = n_tracks < params_tracker_feature_->min_features_for_keyframe; if (origin_ptr_) { - WOLF_INFO("Time span since last frame: " , last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() ); + WOLF_INFO("Time span since last frame: " , + last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() , + " (should be greater than ", + params_tfle_->time_span, + ")"); vote = vote or last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_tfle_->time_span; } @@ -213,6 +216,8 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase { assert(_feature_ptr); assert(_feature_other_ptr); + assert(_feature_ptr->getCapture()); + assert(_feature_ptr->getCapture()->getFrame()); assert(getProblem()); assert(getProblem()->getMap()); @@ -241,9 +246,9 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase if (not lmk) { Vector2d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState(); - Vector2d sen_p = _feature_ptr->getCapture()->getP()->getState(); + Vector2d sen_p = _feature_ptr->getCapture()->getSensorP()->getState(); double frm_o = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0); - double sen_o = _feature_ptr->getCapture()->getO()->getState()(0); + double sen_o = _feature_ptr->getCapture()->getSensorO()->getState()(0); Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature_ptr->getMeasurement()); @@ -268,9 +273,9 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase if (not lmk) { Vector2d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState(); - Vector2d sen_p = _feature_ptr->getCapture()->getP()->getState(); + Vector2d sen_p = _feature_ptr->getCapture()->getSensorP()->getState(); double frm_o = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0); - double sen_o = _feature_ptr->getCapture()->getO()->getState()(0); + double sen_o = _feature_ptr->getCapture()->getSensorO()->getState()(0); Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature_ptr->getMeasurement().head<2>()); double lmk_o = frm_o + sen_o + _feature_ptr->getMeasurement()(2); @@ -285,7 +290,7 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase else if (not lmk->getO()) { double frm_o = _feature_ptr->getCapture()->getFrame()->getO()->getState()(0); - double sen_o = _feature_ptr->getCapture()->getO()->getState()(0); + double sen_o = _feature_ptr->getCapture()->getSensorO()->getState()(0); double lmk_o = frm_o + sen_o + _feature_ptr->getMeasurement()(2); @@ -302,15 +307,13 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase // 3D - Relative Position else if (getProblem()->getDim() == 3 and _feature_ptr->getMeasurement().size() == 3) { - throw std::runtime_error("FactorRelativePosition3dWithExtrinsics is not implemented yet"); - // no landmark, create it if (not lmk) { Vector3d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState(); - Vector3d sen_p = _feature_ptr->getCapture()->getP()->getState(); + Vector3d sen_p = _feature_ptr->getCapture()->getSensorP()->getState(); Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState())); - Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getO()->getState())); + Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getSensorO()->getState())); Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature_ptr->getMeasurement()); @@ -322,11 +325,11 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase } // emplace factor - // return FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(_feature_ptr, - // _feature_ptr, - // lmk, - // shared_from_this(), - // params_tfle_->apply_loss_function); + return FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(_feature_ptr, + _feature_ptr, + lmk, + shared_from_this(), + params_tfle_->apply_loss_function); } // 3D - Relative Pose else if (getProblem()->getDim() == 3 and _feature_ptr->getMeasurement().size() == 7) @@ -335,9 +338,9 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase if (not lmk) { Vector3d frm_p = _feature_ptr->getCapture()->getFrame()->getP()->getState(); - Vector3d sen_p = _feature_ptr->getCapture()->getP()->getState(); + Vector3d sen_p = _feature_ptr->getCapture()->getSensorP()->getState(); Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState())); - Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getO()->getState())); + Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getSensorO()->getState())); Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature_ptr->getMeasurement().head<3>()); Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature_ptr->getMeasurement().tail<4>()); @@ -352,7 +355,7 @@ FactorBasePtr ProcessorTrackerFeatureLandmarkExternal::emplaceFactor(FeatureBase else if (not lmk->getO()) { Quaterniond frm_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getFrame()->getO()->getState())); - Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getO()->getState())); + Quaterniond sen_o = Quaterniond(Vector4d(_feature_ptr->getCapture()->getSensorO()->getState())); Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature_ptr->getMeasurement().tail<4>()); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 73af9e2ed1d3f9fbbdc32240eb2ef83d9b07ff46..261438924ee686d0799f738278b5f1186630a48e 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -179,6 +179,12 @@ wolf_add_gtest(gtest_factor_relative_position_2d gtest_factor_relative_position_ # FactorRelativePosition2dWithExtrinsics class test wolf_add_gtest(gtest_factor_relative_position_2d_with_extrinsics gtest_factor_relative_position_2d_with_extrinsics.cpp) +# FactorRelativePosition3d class test +wolf_add_gtest(gtest_factor_relative_position_3d gtest_factor_relative_position_3d.cpp) + +# FactorRelativePosition3dWithExtrinsics class test +wolf_add_gtest(gtest_factor_relative_position_3d_with_extrinsics gtest_factor_relative_position_3d_with_extrinsics.cpp) + # FactorVelocityLocalDirection3d class test wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp) diff --git a/test/gtest_factor_relative_pose_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp index 00c75bec4aeec1df11a17fbd39eea2f06ca8577e..3b7f25c8fba43f0d56f5146fd2d5168e83733631 100644 --- a/test/gtest_factor_relative_pose_3d.cpp +++ b/test/gtest_factor_relative_pose_3d.cpp @@ -19,12 +19,6 @@ // along with this program. If not, see <http://www.gnu.org/licenses/>. // //--------LICENSE_END-------- -/** - * \file gtest_factor_relative_pose_3d.cpp - * - * Created on: Mar 30, 2017 - * \author: jsola - */ #include "core/utils/utils_gtest.h" diff --git a/test/gtest_factor_relative_position_3d.cpp b/test/gtest_factor_relative_position_3d.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e928a49baf2f61b7c23eb663830a7a0d63c78a47 --- /dev/null +++ b/test/gtest_factor_relative_position_3d.cpp @@ -0,0 +1,280 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see <http://www.gnu.org/licenses/>. +// +//--------LICENSE_END-------- + +#include "core/utils/utils_gtest.h" + +#include "core/ceres_wrapper/solver_ceres.h" +#include "core/factor/factor_relative_position_3d.h" +#include "core/capture/capture_odom_3d.h" +#include "core/state_block/state_block_derived.h" +#include "core/state_block/state_quaternion.h" + + +using namespace Eigen; +using namespace wolf; +using std::cout; +using std::endl; + +std::string wolf_root = _WOLF_ROOT_DIR; + +int N = 1e2; + +// Vectors +Vector7d x_f; +Vector3d delta1, delta2, delta3, x_1, x_2, x_3; + +// Input odometry data and covariance +Matrix3d data_cov = (Vector3d() << 1e-3, 1e-3, 1e-3).finished().asDiagonal(); + +// Problem and solver +ProblemPtr problem_ptr = Problem::create("PO", 3); + +SolverCeres solver(problem_ptr); + +// Two frames +FrameBasePtr frm = problem_ptr->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() ); +FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, (Vector7d() << 0,0,0,0,0,0,1).finished() ); +FrameBasePtr frm2 = problem_ptr->emplaceFrame(2, (Vector7d() << 0,0,0,0,0,0,1).finished() ); +FrameBasePtr frm3 = problem_ptr->emplaceFrame(3, (Vector7d() << 0,0,0,0,0,0,1).finished() ); + +// Landmarks +LandmarkBasePtr lmk1 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), + "LandmarkPosition3d", + std::make_shared<StatePoint3d>(Vector3d::Zero()), + nullptr); + +LandmarkBasePtr lmk2 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), + "LandmarkPosition3d", + std::make_shared<StatePoint3d>(Vector3d::Zero()), + nullptr); + +LandmarkBasePtr lmk3 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), + "LandmarkPosition3d", + std::make_shared<StatePoint3d>(Vector3d::Zero()), + nullptr); + +// Captures +CaptureBasePtr cap1, cap2, cap3; +// Features +FeatureBasePtr fea1, fea2, fea3; +// Factors +FactorRelativePosition3dPtr fac1, fac2, fac3; + +//////////////////////////////////////////////////////// +void generateRandomProblemFrame() +{ + // Random delta + delta1 = Vector3d::Random() * 1e2; + delta2 = Vector3d::Random() * 1e2; + delta3 = Vector3d::Random() * 1e2; + + // Random frame pose + x_f = Vector7d::Random() * 1e2; + x_f.tail<4>().normalize(); + auto q_f = Quaterniond(x_f.tail<4>()); + + // Frames position + x_1 = x_f.head<3>() + q_f * delta1; + x_2 = x_f.head<3>() + q_f * delta2; + x_3 = x_f.head<3>() + q_f * delta3; + + // Set states + frm->setState(x_f); + frm1->getP()->setState(x_1); + frm2->getP()->setState(x_2); + frm3->getP()->setState(x_3); + + // capture & feature & factor with delta measurementCaptureBase(const std::string& _type, + cap1 = CaptureBase::emplace<CaptureBase>(frm1, "CaptureFramePosition3d", 1); + cap2 = CaptureBase::emplace<CaptureBase>(frm2, "CaptureFramePosition3d", 2); + cap3 = CaptureBase::emplace<CaptureBase>(frm3, "CaptureFramePosition3d", 3); + fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureFramePosition3d", delta1, data_cov); + fea2 = FeatureBase::emplace<FeatureBase>(cap2, "FeatureFramePosition3d", delta2, data_cov); + fea3 = FeatureBase::emplace<FeatureBase>(cap3, "FeatureFramePosition3d", delta3, data_cov); + fac1 = FactorBase::emplace<FactorRelativePosition3d>(fea1, fea1, frm, nullptr, false, TOP_MOTION); + fac2 = FactorBase::emplace<FactorRelativePosition3d>(fea2, fea2, frm, nullptr, false, TOP_MOTION); + fac3 = FactorBase::emplace<FactorRelativePosition3d>(fea3, fea3, frm, nullptr, false, TOP_MOTION); +} + +void generateRandomProblemLandmark() +{ + // Random delta + delta1 = Vector3d::Random() * 1e2; + delta2 = Vector3d::Random() * 1e2; + delta3 = Vector3d::Random() * 1e2; + + // Random frame pose + x_f = Vector7d::Random() * 1e2; + x_f.tail<4>().normalize(); + auto q_f = Quaterniond(x_f.tail<4>()); + + // Landmarks positions + x_1 = x_f.head<3>() + q_f * delta1; + x_2 = x_f.head<3>() + q_f * delta2; + x_3 = x_f.head<3>() + q_f * delta3; + + // Set states + frm->setState(x_f); + lmk1->setState(x_1); + lmk2->setState(x_2); + lmk3->setState(x_3); + + // feature & factor with delta measurement + cap1 = CaptureBase::emplace<CaptureBase>(frm, "CaptureLandmarkPosition3d", 1); + cap2 = CaptureBase::emplace<CaptureBase>(frm, "CaptureLandmarkPosition3d", 1); + cap3 = CaptureBase::emplace<CaptureBase>(frm, "CaptureLandmarkPosition3d", 1); + fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPosition3d", delta1, data_cov); + fea2 = FeatureBase::emplace<FeatureBase>(cap2, "FeatureLandmarkPosition3d", delta2, data_cov); + fea3 = FeatureBase::emplace<FeatureBase>(cap3, "FeatureLandmarkPosition3d", delta3, data_cov); + fac1 = FactorBase::emplace<FactorRelativePosition3d>(fea1, fea1, lmk1, nullptr, false, TOP_LMK); + fac2 = FactorBase::emplace<FactorRelativePosition3d>(fea2, fea2, lmk2, nullptr, false, TOP_LMK); + fac3 = FactorBase::emplace<FactorRelativePosition3d>(fea3, fea3, lmk3, nullptr, false, TOP_LMK); +} + +TEST(FactorRelativePosition3d, check_tree) +{ + ASSERT_TRUE(problem_ptr->check(0)); +} + +// FRAME TO FRAME ========================================================================= +TEST(FactorRelativePosition3d, frame_solve_frame) +{ + for (int i = 0; i < N; i++) + { + // random setup + generateRandomProblemFrame(); + + // Perturb frm, fix the rest + frm->unfix(); + frm1->fix(); + frm2->fix(); + frm3->fix(); + frm->perturb(1); + + // solve for frm1 + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO(report); + + ASSERT_POSE3d_APPROX(frm->getStateVector(), x_f, 1e-3); + + // remove captures (and features and factors) for the next loop + cap1->remove(); + cap2->remove(); + cap3->remove(); + } +} + +TEST(FactorRelativePosition3d, frame_solve_frames) +{ + for (int i = 0; i < N; i++) + { + // random setup + generateRandomProblemFrame(); + + // Perturb frm0, fix the rest + frm->fix(); + frm1->unfix(); + frm2->unfix(); + frm3->unfix(); + frm1->getP()->perturb(1); + frm2->getP()->perturb(1); + frm3->getP()->perturb(1); + + // solve for frm0 + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO(report); + + ASSERT_MATRIX_APPROX(frm1->getStateVector("P"), x_1, 1e-3); + ASSERT_MATRIX_APPROX(frm2->getStateVector("P"), x_2, 1e-3); + ASSERT_MATRIX_APPROX(frm3->getStateVector("P"), x_3, 1e-3); + + // remove captures (and features and factors) for the next loop + cap1->remove(); + cap2->remove(); + cap3->remove(); + } +} + +// FRAME TO LANDMARK ========================================================================= +TEST(FactorRelativePosition3d, landmark_solve_frame) +{ + for (int i = 0; i < N; i++) + { + // random setup + generateRandomProblemLandmark(); + + // Perturb frm0, fix the rest + frm->unfix(); + lmk1->fix(); + lmk2->fix(); + lmk3->fix(); + frm->perturb(1); + + // solve for frm0 + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO(report); + + ASSERT_POSE3d_APPROX(frm->getStateVector(), x_f, 1e-3); + + // remove captures (and features and factors) for the next loop + cap1->remove(); + cap2->remove(); + cap3->remove(); + } +} + +TEST(FactorRelativePosition3d, landmark_solve_lmks) +{ + for (int i = 0; i < N; i++) + { + // random setup + generateRandomProblemLandmark(); + + // Perturb landmark, fix the rest + frm->fix(); + lmk1->unfix(); + lmk2->unfix(); + lmk3->unfix(); + lmk1->perturb(1); + lmk2->perturb(1); + lmk3->perturb(1); + + // solve for landmark + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO(report); + + ASSERT_MATRIX_APPROX(lmk1->getStateVector("P"), x_1, 1e-3); + ASSERT_MATRIX_APPROX(lmk2->getStateVector("P"), x_2, 1e-3); + ASSERT_MATRIX_APPROX(lmk3->getStateVector("P"), x_3, 1e-3); + + // remove captures (and features and factors) for the next loop + cap1->remove(); + cap2->remove(); + cap3->remove(); + } +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/gtest_factor_relative_position_3d_with_extrinsics.cpp b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp new file mode 100644 index 0000000000000000000000000000000000000000..3917e5d85f021c0f950b18fa7ab4d191fb835379 --- /dev/null +++ b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp @@ -0,0 +1,267 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see <http://www.gnu.org/licenses/>. +// +//--------LICENSE_END-------- +#include "core/utils/utils_gtest.h" + +#include "core/ceres_wrapper/solver_ceres.h" +#include "core/factor/factor_relative_position_3d_with_extrinsics.h" +#include "core/capture/capture_odom_3d.h" +#include "core/sensor/sensor_odom_3d.h" +#include "core/math/rotations.h" +#include "core/state_block/state_block_derived.h" +#include "core/state_block/state_quaternion.h" + + +using namespace Eigen; +using namespace wolf; +using std::cout; +using std::endl; + +std::string wolf_root = _WOLF_ROOT_DIR; + +int N = 1e2; + +// Vectors +Vector7d x_f, x_s; +Vector3d delta1, delta2, delta3, x_l1, x_l2, x_l3; + +// Input odometry data and covariance +Matrix3d data_cov = (Vector3d() << 1e-3, 1e-3, 1e-3).finished().asDiagonal(); + +// Problem and solver +ProblemPtr problem_ptr = Problem::create("PO", 3); + +SolverCeres solver(problem_ptr); + +// Sensor +auto sensor = problem_ptr->installSensor("SensorOdom3d", "odom", (Vector7d() << 0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_odom_3d.yaml"); + +// Frame +FrameBasePtr frm = problem_ptr->emplaceFrame(0, (Vector7d() << 0,0,0,0,0,0,1).finished() ); + +// Landmarks +LandmarkBasePtr lmk1 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), + "LandmarkPosition3d", + std::make_shared<StatePoint3d>(Vector3d::Zero()), + nullptr); + +LandmarkBasePtr lmk2 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), + "LandmarkPosition3d", + std::make_shared<StatePoint3d>(Vector3d::Zero()), + nullptr); + +LandmarkBasePtr lmk3 = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(), + "LandmarkPosition3d", + std::make_shared<StatePoint3d>(Vector3d::Zero()), + nullptr); + +// Capture +auto cap = CaptureBase::emplace<CaptureOdom3d>(frm, 1, sensor, Vector3d::Zero(), data_cov); +// Features +FeatureBasePtr fea1, fea2, fea3; +// Factors +FactorRelativePosition3dWithExtrinsicsPtr fac1, fac2, fac3; + +void generateRandomProblemLandmark() +{ + // Random deltas + delta1 = Vector3d::Random() * 10; + delta2 = Vector3d::Random() * 10; + delta3 = Vector3d::Random() * 10; + + // Random frame pose + x_f = Vector7d::Random() * 10; + x_f.tail<4>().normalize(); + auto q_f = Quaterniond(x_f.tail<4>()); + + // Random extrinsics + x_s = Vector7d::Random() * 10; + x_s.tail<4>().normalize(); + auto q_s = Quaterniond(x_s.tail<4>()); + + // Landmarks poses + x_l1 = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta1); + x_l2 = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta2); + x_l3 = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta3); + + // Set states + frm->setState(x_f); + lmk1->setState(x_l1); + lmk2->setState(x_l2); + lmk3->setState(x_l3); + sensor->setState(x_s); + + // features & factors with deltas measurements + fea1 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta1, data_cov); + fea2 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta2, data_cov); + fea3 = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPose", delta3, data_cov); + fac1 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea1, fea1, lmk1, nullptr, false, TOP_LMK); + fac2 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea2, fea2, lmk2, nullptr, false, TOP_LMK); + fac3 = FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(fea3, fea3, lmk3, nullptr, false, TOP_LMK); +} + +TEST(FactorRelativePosition3dWithExtrinsics, check_tree) +{ + ASSERT_TRUE(problem_ptr->check(0)); +} + +// FRAME TO LANDMARK ========================================================================= +TEST(FactorRelativePosition3dWithExtrinsics, landmark_solve_lmks) +{ + for (int i = 0; i < N; i++) + { + // random setup + generateRandomProblemLandmark(); + + ASSERT_MATRIX_APPROX(fac1->residual(), VectorXd::Zero(3), Constants::EPS); + ASSERT_MATRIX_APPROX(fac2->residual(), VectorXd::Zero(3), Constants::EPS); + ASSERT_MATRIX_APPROX(fac3->residual(), VectorXd::Zero(3), Constants::EPS); + + // Perturb landmark, fix the rest + frm->fix(); + lmk1->unfix(); + lmk2->unfix(); + lmk3->unfix(); + sensor->getP()->fix(); + sensor->getO()->fix(); + lmk1->perturb(1); + lmk2->perturb(1); + lmk3->perturb(1); + + // solve for landmark + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO(report); + + ASSERT_MATRIX_APPROX(lmk1->getStateVector(), x_l1, 1e-3); + ASSERT_MATRIX_APPROX(lmk2->getStateVector(), x_l2, 1e-3); + ASSERT_MATRIX_APPROX(lmk3->getStateVector(), x_l3, 1e-3); + + // remove features (and factors) for the next loop + fea1->remove(); + fea2->remove(); + fea3->remove(); + } +} + +TEST(FactorRelativePosition3dWithExtrinsics, landmark_solve_frame) +{ + for (int i = 0; i < N; i++) + { + // random setup + generateRandomProblemLandmark(); + + ASSERT_MATRIX_APPROX(fac1->residual(), VectorXd::Zero(3), Constants::EPS); + ASSERT_MATRIX_APPROX(fac2->residual(), VectorXd::Zero(3), Constants::EPS); + ASSERT_MATRIX_APPROX(fac3->residual(), VectorXd::Zero(3), Constants::EPS); + + // Perturb frm0, fix the rest + frm->unfix(); + lmk1->fix(); + lmk2->fix(); + lmk3->fix(); + sensor->getP()->fix(); + sensor->getO()->fix(); + frm->perturb(1); + + // solve for frm0 + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO(report); + + ASSERT_POSE3d_APPROX(frm->getStateVector(), x_f, 1e-3); + + // remove feature (and factor) for the next loop + fea1->remove(); + fea2->remove(); + fea3->remove(); + } +} + +TEST(FactorRelativePosition3dWithExtrinsics, landmark_extrinsics_p_solve) +{ + for (int i = 0; i < N; i++) + { + // random setup + generateRandomProblemLandmark(); + + ASSERT_MATRIX_APPROX(fac1->residual(), VectorXd::Zero(3), Constants::EPS); + ASSERT_MATRIX_APPROX(fac2->residual(), VectorXd::Zero(3), Constants::EPS); + ASSERT_MATRIX_APPROX(fac3->residual(), VectorXd::Zero(3), Constants::EPS); + + // Perturb sensor P, fix the rest + frm->fix(); + lmk1->fix(); + lmk2->fix(); + lmk3->fix(); + sensor->getP()->unfix(); + sensor->getO()->fix(); + sensor->getP()->perturb(1); + + // solve for frm0 + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO(report); + + ASSERT_POSE3d_APPROX(sensor->getStateVector(), x_s, 1e-3); + + // remove feature (and factor) for the next loop + fea1->remove(); + fea2->remove(); + fea3->remove(); + } +} + +TEST(FactorRelativePosition3dWithExtrinsics, landmark_extrinsics_o_solve) +{ + for (int i = 0; i < N; i++) + { + // random setup + generateRandomProblemLandmark(); + + ASSERT_MATRIX_APPROX(fac1->residual(), VectorXd::Zero(3), Constants::EPS); + ASSERT_MATRIX_APPROX(fac2->residual(), VectorXd::Zero(3), Constants::EPS); + ASSERT_MATRIX_APPROX(fac3->residual(), VectorXd::Zero(3), Constants::EPS); + + // Perturb sensor O, fix the rest + frm->fix(); + lmk1->fix(); + lmk2->fix(); + lmk3->fix(); + sensor->getP()->fix(); + sensor->getO()->unfix(); + sensor->getO()->perturb(.2); + + // solve for frm0 + std::string report = solver.solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO(report); + + ASSERT_POSE3d_APPROX(sensor->getStateVector(), x_s, 1e-3); + + // remove feature (and factor) for the next loop + fea1->remove(); + fea2->remove(); + fea3->remove(); + } +} + +int main(int argc, char **argv) +{ +testing::InitGoogleTest(&argc, argv); +return RUN_ALL_TESTS(); +} diff --git a/test/gtest_processor_tracker_feature_landmark_external.cpp b/test/gtest_processor_tracker_feature_landmark_external.cpp index 5ec2e7998ec1dd40259db5e3bb16ffc257b868e4..5010597787c5d715592c5f5a81338460af79877e 100644 --- a/test/gtest_processor_tracker_feature_landmark_external.cpp +++ b/test/gtest_processor_tracker_feature_landmark_external.cpp @@ -31,6 +31,7 @@ #include "core/state_block/state_block_derived.h" #include "core/state_block/state_angle.h" #include "core/state_block/state_quaternion.h" +#include "core/ceres_wrapper/solver_ceres.h" #include "core/processor/processor_tracker_feature_landmark_external.h" @@ -50,6 +51,7 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test double dt; ProblemPtr problem; + SolverManagerPtr solver; SensorBasePtr sensor, sensor_odom; ProcessorTrackerFeatureLandmarkExternalPtr processor; ProcessorMotionPtr processor_motion; @@ -64,10 +66,17 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test double _quality_th, double _dist_th, unsigned int _track_length_th, - double _time_span); + double _time_span, + bool _add_landmarks); void randomStep(); CaptureLandmarksExternalPtr computeCaptureLandmarks() const; - void addRandomDetection(CaptureLandmarksExternalPtr _cap, int _id, double _quality) const; + void testConfiguration(int dim, + bool orientation, + double quality_th, + double dist_th, + int track_length, + double time_span, + bool add_landmarks); }; void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim, @@ -75,7 +84,8 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim, double _quality_th, double _dist_th, unsigned int _track_length_th, - double _time_span) + double _time_span, + bool _add_landmarks) { dim = _dim; orientation = _orientation; @@ -83,6 +93,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim, dt = 0.1; problem = Problem::create("PO", dim); + solver = std::make_shared<SolverCeres>(problem); // Sensors if (dim == 2) @@ -152,7 +163,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim, { LandmarkBasePtr lmk; if (dim == 2) - lmk = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), + lmk = LandmarkBase::emplace<LandmarkBase>(_add_landmarks ? problem->getMap() : nullptr, "LandmarkExternal", std::make_shared<StatePoint2d>(Vector2d::Random() * 10), (orientation ? @@ -160,7 +171,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim, nullptr)); else - lmk = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), + lmk = LandmarkBase::emplace<LandmarkBase>(_add_landmarks ? problem->getMap() : nullptr, "LandmarkExternal", std::make_shared<StatePoint3d>(Vector3d::Random() * 10), (orientation ? @@ -181,18 +192,21 @@ void ProcessorTrackerFeatureLandmarkExternalTest::randomStep() { // compute delta VectorXd delta; + MatrixXd delta_cov; if (dim == 2) { delta = Vector2d::Random() * 0.1; + delta_cov = Matrix2d::Identity() * 0.1; } else { delta = Vector7d::Random() * 0.1; delta.tail<4>().normalize(); + delta_cov = Matrix6d::Identity() * 0.1; } // Process odometry - auto cap_odom = std::make_shared<CaptureMotion>("CaptureOdom", t, sensor_odom, delta, nullptr); + auto cap_odom = std::make_shared<CaptureMotion>("CaptureOdom", t, sensor_odom, delta, delta_cov, nullptr); cap_odom->process(); // update groundtruth pose with random movement @@ -218,7 +232,7 @@ CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::compute { meas.head(2) = Rotation2Dd(-o_sensor(0))*(Rotation2Dd(-o_robot(0))*(p_lmk - p_robot) - p_sensor); if (orientation) - meas(2) = o_lmk(0) - p_robot(0); + meas(2) = o_lmk(0) - o_robot(0) - o_sensor(0); } else { @@ -229,69 +243,30 @@ CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::compute if (orientation) meas.tail(4) = (q_sensor.conjugate() * q_robot.conjugate() * Quaterniond(Vector4d(o_lmk))).coeffs(); } + // cov MatrixXd cov = MatrixXd::Identity(meas.size(), meas.size()); if (orientation and dim != 2) cov = MatrixXd::Identity(6, 6); + // quality + double quality = (double) i / (double) (landmarks.size()-1); // increasing from 0 to 1 + // add detection - cap->addDetection(landmarks.at(i)->id(), meas, cov, (double) i / (double) landmarks.size()); + cap->addDetection(landmarks.at(i)->id(), meas, cov, quality ); } return cap; } -void ProcessorTrackerFeatureLandmarkExternalTest::addRandomDetection(CaptureLandmarksExternalPtr _cap, int _id, double _quality) const -{ - VectorXd detection; - MatrixXd cov; - if (orientation) - { - if (dim == 2) - { - detection = Vector3d::Random()*5; - detection(2) = pi2pi(detection(2)); - cov = Matrix3d::Identity()*0.1; - } - else - { - detection = Vector7d::Random()*5; - detection.tail<4>().normalize(); - cov = Matrix6d::Identity()*0.1; - } - } - else - { - if (dim == 2) - { - detection = Vector2d::Random()*5; - cov = Matrix2d::Identity()*0.1; - } - else - { - detection = Vector3d::Random()*5; - cov = Matrix3d::Identity()*0.1; - } - } - _cap->addDetection(_id, detection, cov, _quality); -} - -// TESTS ///////////////////////////////////////////////////////////////////////////////// - -TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d) +void ProcessorTrackerFeatureLandmarkExternalTest::testConfiguration(int dim, + bool orientation, + double quality_th, + double dist_th, + int track_length, + double time_span, + bool add_landmarks) { - int dim = 2; - bool orientation = false; - double quality_th = 0; - double dist_th = 1e6; - int track_length = 5; - double time_span = 3*dt; - bool remove_landmarks = true; - - initProblem(dim, orientation, quality_th, dist_th, track_length, time_span); - - if (remove_landmarks) - for (auto lmk : landmarks) - problem->getMap()->removeLanlmk->remove(); + initProblem(dim, orientation, quality_th, dist_th, track_length, time_span, add_landmarks); ASSERT_TRUE(problem->check()); @@ -306,12 +281,16 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d) // process detections cap->process(); ASSERT_TRUE(problem->check()); - //problem->print(4,1,1,1); + problem->print(4,1,1,1); // CHECKS FactorBasePtrList fac_list; problem->getTrajectory()->getFactorList(fac_list); - bool should_emplace_KF = t.get() >= time_span and i >= track_length-1; + // should emplace KF in last? + bool any_active_track = quality_th <=1 and i >= track_length-1; + bool should_emplace_KF = t-dt > time_span and any_active_track; + WOLF_INFO("should emplace KF: ", should_emplace_KF, " t-dt = ", t-dt, " time_span = ", time_span, " track_length-1 = ", track_length-1); + if (should_emplace_KF) { // voted for keyframe @@ -327,34 +306,37 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d) if (fac->getProcessor() == processor) { ASSERT_EQ(fac->getType(), std::string("FactorRelative") + - (orientation ? "Pose" : "Position") + - (dim == 2 ? "2d" : "3d") + - "WithExtrinsiscs"); + (orientation ? "Pose" : "Position") + + (dim == 2 ? "2d" : "3d") + + "WithExtrinsics"); } } - // landmarks - auto landmarks_map = problem->getMap()->getLandmarkList(); - ASSERT_EQ(landmarks_map.size(), landmarks.size()); - for (auto lmk_map : landmarks_map) + // landmarks created by processor + if (not add_landmarks) { - ASSERT_TRUE(lmk_map->id() < landmarks.size()); - auto lmk_gt = landmarks.at(lmk_map->id()); - ASSERT_EQ(lmk_map->id(), lmk_gt->id()); - if (dim == 2 and orientation) - { - ASSERT_POSE2d_APPROX(lmk_map->getState().vector("PO"), lmk_gt->getState().vector("PO"), Constants::EPS); - } - else if (dim == 2 and not orientation) - { - ASSERT_MATRIX_APPROX(lmk_map->getState().vector("P"), lmk_gt->getState().vector("P"), Constants::EPS); - } - else if (dim == 3 and orientation) - { - ASSERT_POSE3d_APPROX(lmk_map->getState().vector("PO"), lmk_gt->getState().vector("PO"), Constants::EPS); - } - else if (dim == 3 and not orientation) + auto landmarks_map = problem->getMap()->getLandmarkList(); + // amount of landmarks due to quality filtering of detections + auto n_landmarks = (quality_th <= 0 ? 3 : (quality_th <= 0.5 ? 2 : (quality_th <= 1 ? 1 : 0))); + ASSERT_EQ(landmarks_map.size(), n_landmarks); + + // check state correctly initialized + for (auto lmk_map : landmarks_map) { - ASSERT_MATRIX_APPROX(lmk_map->getState().vector("P"), lmk_gt->getState().vector("P"), Constants::EPS); + ASSERT_TRUE(lmk_map->id() < landmarks.size()); + auto lmk_gt = landmarks.at(lmk_map->id()); + ASSERT_EQ(lmk_map->id(), lmk_gt->id()); + if (not orientation) + { + ASSERT_MATRIX_APPROX(lmk_map->getState().vector("P"), lmk_gt->getState().vector("P"), Constants::EPS); + } + else if (dim == 2) + { + ASSERT_POSE2d_APPROX(lmk_map->getState().vector("PO"), lmk_gt->getState().vector("PO"), Constants::EPS); + } + else if (dim == 3) + { + ASSERT_POSE3d_APPROX(lmk_map->getState().vector("PO"), lmk_gt->getState().vector("PO"), Constants::EPS); + } } } } @@ -364,10 +346,23 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d) ASSERT_FALSE(problem->getTrajectory()->size() > 1); // no factors emplaced ASSERT_TRUE(fac_list.empty()); - // landmarks - ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), remove_landmarks); + // no landmarks created yet (if not added by the test) + ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), not add_landmarks); } + // solve + if (not problem->getMap()->getLandmarkList().empty()) + { + // TODO store gt values + + // TODO perturb + + // solve + auto report = solver->solve(SolverManager::ReportVerbosity::FULL); + WOLF_INFO(report); + + // TODO check values + } // step with random movement t+=dt; @@ -375,42 +370,138 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d) } } -// TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_po_2d) -// { -// initProblem(2, true); -// ASSERT_TRUE(problem->check()); +// / TESTS ////////////////////////////////////////// +TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_existing_lmks) +{ + testConfiguration(2, //int dim + false, //bool orientation + 0, //double quality_th + 1e6, //double dist_th + 6, //int track_length + 4.5*dt, //double time_span + true); //bool add_landmarks +} + +TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_no_lmks) +{ + testConfiguration(2, //int dim + false, //bool orientation + 0, //double quality_th + 1e6, //double dist_th + 2, //int track_length + 4.5*dt, //double time_span + false); //bool add_landmarks +} -// auto cap = randomStep(); -// ASSERT_TRUE(problem->check()); +TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_2d_no_lmks_quality) +{ + testConfiguration(2, //int dim + false, //bool orientation + 0.3, //double quality_th + 1e6, //double dist_th + 6, //int track_length + 4.5*dt, //double time_span + false); //bool add_landmarks +} + +TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_existing_lmks) +{ + testConfiguration(2, //int dim + true, //bool orientation + 0, //double quality_th + 1e6, //double dist_th + 3, //int track_length + 4.5*dt, //double time_span + true); //bool add_landmarks +} -// cap->process(); -// ASSERT_TRUE(problem->check()); -// } +TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_no_lmks) +{ + testConfiguration(2, //int dim + true, //bool orientation + 0, //double quality_th + 1e6, //double dist_th + 1, //int track_length + 4.5*dt, //double time_span + false); //bool add_landmarks +} -// TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_3d) -// { -// initProblem(3, false); -// ASSERT_TRUE(problem->check()); +TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_2d_no_lmks_quality) +{ + testConfiguration(2, //int dim + true, //bool orientation + 0.3, //double quality_th + 1e6, //double dist_th + 0, //int track_length + 4.5*dt, //double time_span + false); //bool add_landmarks +} -// auto cap = randomStep(); -// ASSERT_TRUE(problem->check()); +TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_existing_lmks) +{ + testConfiguration(3, //int dim + false, //bool orientation + 0, //double quality_th + 1e6, //double dist_th + 7, //int track_length + 4.5*dt, //double time_span + true); //bool add_landmarks +} -// cap->process(); -// ASSERT_TRUE(problem->check()); -// } +TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_no_lmks) +{ + testConfiguration(3, //int dim + false, //bool orientation + 0, //double quality_th + 1e6, //double dist_th + 53, //int track_length + 4.5*dt, //double time_span + false); //bool add_landmarks +} -// TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_po_3d) -// { -// initProblem(3, true); -// ASSERT_TRUE(problem->check()); +TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, P_3d_no_lmks_quality) +{ + testConfiguration(3, //int dim + false, //bool orientation + 0.3, //double quality_th + 1e6, //double dist_th + 2, //int track_length + 4.5*dt, //double time_span + false); //bool add_landmarks +} -// auto cap = randomStep(); -// ASSERT_TRUE(problem->check()); +TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_existing_lmks) +{ + testConfiguration(3, //int dim + true, //bool orientation + 0, //double quality_th + 1e6, //double dist_th + 1, //int track_length + 4.5*dt, //double time_span + true); //bool add_landmarks +} -// cap->process(); -// ASSERT_TRUE(problem->check()); -// } +TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_no_lmks) +{ + testConfiguration(3, //int dim + true, //bool orientation + 0, //double quality_th + 1e6, //double dist_th + 4, //int track_length + 4.5*dt, //double time_span + false); //bool add_landmarks +} +TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, PO_3d_no_lmks_quality) +{ + testConfiguration(3, //int dim + true, //bool orientation + 0.2, //double quality_th + 1e6, //double dist_th + 5, //int track_length + 4.5*dt, //double time_span + false); //bool add_landmarks +} int main(int argc, char **argv)