diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 8e9b97fcd99909926fcb12ddbcab4309bc3cc62f..d41fdb9eedb59fcd7b3d6750b4373c843b51ef52 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,3 +1,13 @@
+workflow:
+ rules:
+ - if: '$CI_PIPELINE_SOURCE == "web"'
+ - if: $CI_COMMIT_BRANCH && $CI_OPEN_MERGE_REQUESTS && $CI_PIPELINE_SOURCE == "push"
+ when: never
+ - if: '$CI_PIPELINE_SOURCE == "merge_request_event"'
+ - if: '$CI_COMMIT_BRANCH && $CI_OPEN_MERGE_REQUESTS'
+ when: never
+ - if: '$CI_COMMIT_BRANCH'
+
stages:
- license
- build_and_test
@@ -110,7 +120,6 @@ stages:
- cd build
- cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=ON -DBUILD_TESTS=ON ..
- make -j$(nproc)
- - ldconfig
- ctest -j$(nproc) --output-on-failure
# run demos
- ../bin/hello_wolf
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b6a68b80313507ee548b1426881b1bf14d95a21d..6e2dbbe0a63578c80e3f9716a416ccae852fe46f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -49,6 +49,10 @@ IF(NOT BUILD_TESTS)
OPTION(BUILD_TESTS "Build Unit tests" ON)
ENDIF(NOT BUILD_TESTS)
+IF(NOT BUILD_GMOCK)
+ OPTION(BUILD_GMOCK "Build GMock" OFF)
+ENDIF(NOT BUILD_GMOCK)
+
IF(NOT BUILD_DEMOS)
OPTION(BUILD_DEMOS "Build Demos" ON)
ENDIF(NOT BUILD_DEMOS)
@@ -109,12 +113,13 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_D
# ============ HEADERS ============
SET(HDRS_CAPTURE
include/${PROJECT_NAME}/capture/capture_base.h
+ include/${PROJECT_NAME}/capture/capture_diff_drive.h
+ include/${PROJECT_NAME}/capture/capture_landmarks_external.h
include/${PROJECT_NAME}/capture/capture_motion.h
include/${PROJECT_NAME}/capture/capture_odom_2d.h
include/${PROJECT_NAME}/capture/capture_odom_3d.h
include/${PROJECT_NAME}/capture/capture_pose.h
include/${PROJECT_NAME}/capture/capture_void.h
- include/${PROJECT_NAME}/capture/capture_diff_drive.h
)
SET(HDRS_COMMON
include/${PROJECT_NAME}/common/factory.h
@@ -136,10 +141,15 @@ SET(HDRS_FACTOR
include/${PROJECT_NAME}/factor/factor_pose_3d.h
include/${PROJECT_NAME}/factor/factor_pose_3d_with_extrinsics.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_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
@@ -181,6 +191,7 @@ SET(HDRS_PROCESSOR
include/${PROJECT_NAME}/processor/processor_base.h
include/${PROJECT_NAME}/processor/processor_diff_drive.h
include/${PROJECT_NAME}/processor/processor_fixed_wing_model.h
+ include/${PROJECT_NAME}/processor/processor_landmark_external.h
include/${PROJECT_NAME}/processor/processor_loop_closure.h
include/${PROJECT_NAME}/processor/processor_motion.h
include/${PROJECT_NAME}/processor/processor_odom_2d.h
@@ -244,12 +255,13 @@ SET(HDRS_UTILS
# ============ SOURCES ============
SET(SRCS_CAPTURE
src/capture/capture_base.cpp
+ src/capture/capture_diff_drive.cpp
+ src/capture/capture_landmarks_external.cpp
src/capture/capture_motion.cpp
src/capture/capture_odom_2d.cpp
src/capture/capture_odom_3d.cpp
src/capture/capture_pose.cpp
src/capture/capture_void.cpp
- src/capture/capture_diff_drive.cpp
)
SET(SRCS_COMMON
src/common/node_base.cpp
@@ -287,6 +299,7 @@ SET(SRCS_PROCESSOR
src/processor/processor_base.cpp
src/processor/processor_diff_drive.cpp
src/processor/processor_fixed_wing_model.cpp
+ src/processor/processor_landmark_external.cpp
src/processor/processor_loop_closure.cpp
src/processor/processor_motion.cpp
src/processor/processor_odom_2d.cpp
@@ -338,6 +351,7 @@ IF (Ceres_FOUND)
include/${PROJECT_NAME}/ceres_wrapper/local_parametrization_wrapper.h
include/${PROJECT_NAME}/ceres_wrapper/iteration_update_callback.h
include/${PROJECT_NAME}/ceres_wrapper/solver_ceres.h
+ include/${PROJECT_NAME}/ceres_wrapper/wolf_jet.h
include/${PROJECT_NAME}/solver/solver_manager.h
include/${PROJECT_NAME}/solver_suitesparse/sparse_utils.h
)
diff --git a/include/core/capture/capture_landmarks_external.h b/include/core/capture/capture_landmarks_external.h
new file mode 100644
index 0000000000000000000000000000000000000000..27af193e1f73de46ec3f934c0f89b0ff0f2a9750
--- /dev/null
+++ b/include/core/capture/capture_landmarks_external.h
@@ -0,0 +1,61 @@
+//--------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/capture/capture_base.h"
+
+namespace wolf {
+
+struct LandmarkDetection
+{
+ int id; // id of landmark
+ Eigen::VectorXd measure; // either pose or position
+ Eigen::MatrixXd covariance; // covariance of the measure
+ double quality; // [0, 1] quality of the detection
+};
+
+WOLF_PTR_TYPEDEFS(CaptureLandmarksExternal);
+
+//class CaptureLandmarksExternal
+class CaptureLandmarksExternal : public CaptureBase
+{
+ protected:
+ std::vector<LandmarkDetection> detections_;
+
+ public:
+
+ CaptureLandmarksExternal(const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
+ const std::vector<int>& _ids = {},
+ const std::vector<Eigen::VectorXd>& _detections = {},
+ const std::vector<Eigen::MatrixXd>& _covs = {},
+ const std::vector<double>& _qualities = {});
+
+ ~CaptureLandmarksExternal() override;
+
+ std::vector<LandmarkDetection> getDetections() const {return detections_;};
+
+ void addDetection(const int& _id, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& quality);
+};
+
+} //namespace wolf
\ No newline at end of file
diff --git a/include/core/ceres_wrapper/wolf_jet.h b/include/core/ceres_wrapper/wolf_jet.h
new file mode 100644
index 0000000000000000000000000000000000000000..6300dcf6b96aa9dcec262c5c30cb3f0e5cad2f07
--- /dev/null
+++ b/include/core/ceres_wrapper/wolf_jet.h
@@ -0,0 +1,51 @@
+//--------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
+
+// CERES JET
+#include "ceres/jet.h"
+
+#include "core/math/rotations.h"
+
+namespace ceres{
+
+// // efficient implementation of pi2pi for jets (without while)
+// template<int N>
+// inline ceres::Jet<double, N> pi2pi(const ceres::Jet<double, N>& _angle)
+// {
+// ceres::Jet<double, N> angle = _angle;
+// angle.a = pi2pi(_angle.a);
+// return angle;
+// }
+// }
+
+using std::remainder;
+
+template<int N>
+inline Jet<double, N> remainder(const Jet<double, N>& _x, long double _y)
+{
+ Jet<double, N> res = _x;
+ res.a = remainder(_x.a, _y);
+ return res;
+}
+}
\ No newline at end of file
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index befbe579d48c44f15c321a50675b9f280921ce40..b414400cf4b4749625df8c2a6414d04e80ef2110 100644
--- a/include/core/factor/factor_autodiff.h
+++ b/include/core/factor/factor_autodiff.h
@@ -25,8 +25,8 @@
#include "core/factor/factor_base.h"
#include "core/state_block/state_block.h"
-// CERES
-#include "ceres/jet.h"
+// jet
+#include "core/ceres_wrapper/wolf_jet.h"
// GENERAL
#include <array>
@@ -34,37 +34,1043 @@
namespace wolf {
//template class FactorAutodiff
-template <class FacT, unsigned int RES, unsigned int B0, unsigned int B1 = 0, unsigned int B2 = 0, unsigned int B3 = 0, unsigned int B4 = 0, unsigned int B5 = 0, unsigned int B6 = 0, unsigned int B7 = 0, unsigned int B8 = 0, unsigned int B9 = 0, unsigned int B10 = 0, unsigned int B11 = 0>
+template <class FacT, unsigned int RES, unsigned int B0, unsigned int B1 = 0, unsigned int B2 = 0, unsigned int B3 = 0, unsigned int B4 = 0, unsigned int B5 = 0, unsigned int B6 = 0, unsigned int B7 = 0, unsigned int B8 = 0, unsigned int B9 = 0, unsigned int B10 = 0, unsigned int B11 = 0, unsigned int B12 = 0, unsigned int B13 = 0, unsigned int B14 = 0>
class FactorAutodiff : public FactorBase
{
public:
+ static const unsigned int n_blocks = 15;
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = B8;
- static const unsigned int block9Size = B9;
- static const unsigned int block10Size = B10;
- static const unsigned int block11Size = B11;
- static const unsigned int n_blocks = 12;
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
+ B5 + B6 + B7 + B8 + B9 +
+ B10 + B11 + B12 + B13 + B14> WolfJet;
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+ mutable std::array<WolfJet, B10> jets_10_;
+ mutable std::array<WolfJet, B11> jets_11_;
+ mutable std::array<WolfJet, B12> jets_12_;
+ mutable std::array<WolfJet, B13> jets_13_;
+ mutable std::array<WolfJet, B14> jets_14_;
+
+ public:
+ /** \brief Constructor valid for all categories (ABSOLUTE, FRAME, FEATURE, LANDMARK)
+ **/
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr,
+ StateBlockPtr _state12Ptr,
+ StateBlockPtr _state13Ptr,
+ StateBlockPtr _state14Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr,
+ _state7Ptr,
+ _state8Ptr,
+ _state9Ptr,
+ _state10Ptr,
+ _state11Ptr,
+ _state12Ptr,
+ _state13Ptr,
+ _state14Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr,
+ StateBlockPtr _state12Ptr,
+ StateBlockPtr _state13Ptr,
+ StateBlockPtr _state14Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr,_state12Ptr,_state13Ptr,_state14Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr,_state12Ptr,_state13Ptr,_state14Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+ assert(_state9Ptr != nullptr && "nullptr state block");
+ assert(_state10Ptr != nullptr && "nullptr state block");
+ assert(_state11Ptr != nullptr && "nullptr state block");
+ assert(_state12Ptr != nullptr && "nullptr state block");
+ assert(_state13Ptr != nullptr && "nullptr state block");
+ assert(_state14Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B11; i++)
+ jets_11_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B12; i++)
+ jets_12_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B13; i++)
+ jets_13_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B14; i++)
+ jets_14_[i] = WolfJet(0, last_jet_idx++);
+ }
+
+ ~FactorAutodiff() override = default;
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ /** \brief Returns the residual and jacobians given the state values
+ *
+ * Returns the residual and jacobians given the state values
+ *
+ **/
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ parameters[9],
+ parameters[10],
+ parameters[11],
+ parameters[12],
+ parameters[13],
+ parameters[14],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ jets_13_.data(),
+ jets_14_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ /** \brief Updates all jets real part with values of parameters
+ *
+ **/
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i].a = parameters[8][i];
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i].a = parameters[9][i];
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i].a = parameters[10][i];
+ for (unsigned int i = 0; i < B11; i++)
+ jets_11_[i].a = parameters[11][i];
+ for (unsigned int i = 0; i < B12; i++)
+ jets_12_[i].a = parameters[12][i];
+ for (unsigned int i = 0; i < B13; i++)
+ jets_13_[i].a = parameters[13][i];
+ for (unsigned int i = 0; i < B14; i++)
+ jets_14_[i].a = parameters[14][i];
+ }
+
+ /** \brief Returns a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
+ *
+ **/
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ jets_13_.data(),
+ jets_14_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+ }
+
+ /** \brief Returns a vector of pointers to the states
+ *
+ * Returns a vector of pointers to the state in which this factor depends
+ *
+ **/
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ /** \brief Returns a vector of the states sizes
+ *
+ **/
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ /** \brief Returns the residual size
+ *
+ * Returns the residual size
+ *
+ **/
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 14 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11,unsigned int B12,unsigned int B13>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,B13,0> : public FactorBase
+{
+ public:
+ static const unsigned int n_blocks = 14;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 + B5 + B6 +
+ B7 + B8 + B9 + B10 + B11 + B12 + B13> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+ mutable std::array<WolfJet, B10> jets_10_;
+ mutable std::array<WolfJet, B11> jets_11_;
+ mutable std::array<WolfJet, B12> jets_12_;
+ mutable std::array<WolfJet, B13> jets_13_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr,
+ StateBlockPtr _state12Ptr,
+ StateBlockPtr _state13Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr,
+ _state7Ptr,
+ _state8Ptr,
+ _state9Ptr,
+ _state10Ptr,
+ _state11Ptr,
+ _state12Ptr,
+ _state13Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr,
+ StateBlockPtr _state12Ptr,
+ StateBlockPtr _state13Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr, _state12Ptr, _state13Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr,_state12Ptr, _state13Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+ assert(_state9Ptr != nullptr && "nullptr state block");
+ assert(_state10Ptr != nullptr && "nullptr state block");
+ assert(_state11Ptr != nullptr && "nullptr state block");
+ assert(_state12Ptr != nullptr && "nullptr state block");
+ assert(_state13Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B11; i++)
+ jets_11_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B12; i++)
+ jets_12_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B13; i++)
+ jets_13_[i] = WolfJet(0, last_jet_idx++);
+ }
+
+ ~FactorAutodiff() override = default;
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ parameters[9],
+ parameters[10],
+ parameters[11],
+ parameters[12],
+ parameters[13],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ jets_13_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i].a = parameters[8][i];
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i].a = parameters[9][i];
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i].a = parameters[10][i];
+ for (unsigned int i = 0; i < B11; i++)
+ jets_11_[i].a = parameters[11][i];
+ for (unsigned int i = 0; i < B12; i++)
+ jets_12_[i].a = parameters[12][i];
+ for (unsigned int i = 0; i < B13; i++)
+ jets_13_[i].a = parameters[13][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ jets_13_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+ }
+
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
+
+////////////////// SPECIALIZATION 13 BLOCKS ////////////////////////////////////////////////////////////////////////
+
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11,unsigned int B12>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,0,0> : public FactorBase
+{
+ public:
+ static const unsigned int n_blocks = 13;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 + B5 + B6 +
+ B7 + B8 + B9 + B10 + B11 + B12> WolfJet;
+
+ protected:
+
+ std::vector<StateBlockPtr> state_ptrs_;
+ std::vector<StateBlockConstPtr> state_ptrs_const_;
+
+ static const std::vector<unsigned int> jacobian_locations_;
+ mutable std::array<WolfJet, RES> residuals_jets_;
+ mutable std::array<WolfJet, B0> jets_0_;
+ mutable std::array<WolfJet, B1> jets_1_;
+ mutable std::array<WolfJet, B2> jets_2_;
+ mutable std::array<WolfJet, B3> jets_3_;
+ mutable std::array<WolfJet, B4> jets_4_;
+ mutable std::array<WolfJet, B5> jets_5_;
+ mutable std::array<WolfJet, B6> jets_6_;
+ mutable std::array<WolfJet, B7> jets_7_;
+ mutable std::array<WolfJet, B8> jets_8_;
+ mutable std::array<WolfJet, B9> jets_9_;
+ mutable std::array<WolfJet, B10> jets_10_;
+ mutable std::array<WolfJet, B11> jets_11_;
+ mutable std::array<WolfJet, B12> jets_12_;
+
+ public:
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr,
+ StateBlockPtr _state12Ptr) :
+ FactorAutodiff(_tp,
+ _top,
+ _feature_ptr,
+ _frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
+ _capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
+ _feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
+ _landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _state0Ptr,
+ _state1Ptr,
+ _state2Ptr,
+ _state3Ptr,
+ _state4Ptr,
+ _state5Ptr,
+ _state6Ptr,
+ _state7Ptr,
+ _state8Ptr,
+ _state9Ptr,
+ _state10Ptr,
+ _state11Ptr,
+ _state12Ptr){}
+
+ FactorAutodiff(const std::string& _tp,
+ const FactorTopology& _top,
+ const FeatureBasePtr& _feature_ptr,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr,
+ StateBlockPtr _state10Ptr,
+ StateBlockPtr _state11Ptr,
+ StateBlockPtr _state12Ptr) :
+ FactorBase(_tp, _top, _feature_ptr, _frame_other_list, _capture_other_list, _feature_other_list, _landmark_other_list, _processor_ptr, _apply_loss_function, _status),
+ state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr,_state12Ptr}),
+ state_ptrs_const_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr,_state12Ptr})
+ {
+ // asserts for null states
+ assert(state_ptrs_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(state_ptrs_const_.size() == n_blocks && "state_ptrs wrongly initialized");
+ assert(_state0Ptr != nullptr && "nullptr state block");
+ assert(_state1Ptr != nullptr && "nullptr state block");
+ assert(_state2Ptr != nullptr && "nullptr state block");
+ assert(_state3Ptr != nullptr && "nullptr state block");
+ assert(_state4Ptr != nullptr && "nullptr state block");
+ assert(_state5Ptr != nullptr && "nullptr state block");
+ assert(_state6Ptr != nullptr && "nullptr state block");
+ assert(_state7Ptr != nullptr && "nullptr state block");
+ assert(_state8Ptr != nullptr && "nullptr state block");
+ assert(_state9Ptr != nullptr && "nullptr state block");
+ assert(_state10Ptr != nullptr && "nullptr state block");
+ assert(_state11Ptr != nullptr && "nullptr state block");
+ assert(_state12Ptr != nullptr && "nullptr state block");
+
+ // initialize jets
+ unsigned int last_jet_idx = 0;
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B11; i++)
+ jets_11_[i] = WolfJet(0, last_jet_idx++);
+ for (unsigned int i = 0; i < B12; i++)
+ jets_12_[i] = WolfJet(0, last_jet_idx++);
+ }
+
+ ~FactorAutodiff() override = default;
+
+ JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+ bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
+ {
+ // only residuals
+ if (jacobians == nullptr)
+ {
+ (*static_cast<FacT const*>(this))(parameters[0],
+ parameters[1],
+ parameters[2],
+ parameters[3],
+ parameters[4],
+ parameters[5],
+ parameters[6],
+ parameters[7],
+ parameters[8],
+ parameters[9],
+ parameters[10],
+ parameters[11],
+ parameters[12],
+ residuals);
+ }
+ // also compute jacobians
+ else
+ {
+ // update jets real part
+ std::vector<double const*> param_vec;
+ param_vec.assign(parameters,parameters+n_blocks);
+ updateJetsRealPart(param_vec);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ residuals_jets_.data());
+
+ // fill the residual array
+ for (unsigned int i = 0; i < RES; i++)
+ residuals[i] = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ if (jacobians[i] != nullptr)
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ jacobians[i] + row * state_block_sizes_.at(i));
+ }
+ return true;
+ }
+
+ void updateJetsRealPart(const std::vector<double const*>& parameters) const
+ {
+ // update jets real part
+ for (unsigned int i = 0; i < B0; i++)
+ jets_0_[i].a = parameters[0][i];
+ for (unsigned int i = 0; i < B1; i++)
+ jets_1_[i].a = parameters[1][i];
+ for (unsigned int i = 0; i < B2; i++)
+ jets_2_[i].a = parameters[2][i];
+ for (unsigned int i = 0; i < B3; i++)
+ jets_3_[i].a = parameters[3][i];
+ for (unsigned int i = 0; i < B4; i++)
+ jets_4_[i].a = parameters[4][i];
+ for (unsigned int i = 0; i < B5; i++)
+ jets_5_[i].a = parameters[5][i];
+ for (unsigned int i = 0; i < B6; i++)
+ jets_6_[i].a = parameters[6][i];
+ for (unsigned int i = 0; i < B7; i++)
+ jets_7_[i].a = parameters[7][i];
+ for (unsigned int i = 0; i < B8; i++)
+ jets_8_[i].a = parameters[8][i];
+ for (unsigned int i = 0; i < B9; i++)
+ jets_9_[i].a = parameters[9][i];
+ for (unsigned int i = 0; i < B10; i++)
+ jets_10_[i].a = parameters[10][i];
+ for (unsigned int i = 0; i < B11; i++)
+ jets_11_[i].a = parameters[11][i];
+ for (unsigned int i = 0; i < B12; i++)
+ jets_12_[i].a = parameters[12][i];
+ }
+
+ void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
+ {
+ assert(residual_.size() == RES);
+ jacobians_.clear();
+
+ assert(_states_ptr.size() == n_blocks);
+
+ // update jets real part
+ updateJetsRealPart(_states_ptr);
+
+ // call functor
+ (*static_cast<FacT const*>(this))(jets_0_.data(),
+ jets_1_.data(),
+ jets_2_.data(),
+ jets_3_.data(),
+ jets_4_.data(),
+ jets_5_.data(),
+ jets_6_.data(),
+ jets_7_.data(),
+ jets_8_.data(),
+ jets_9_.data(),
+ jets_10_.data(),
+ jets_11_.data(),
+ jets_12_.data(),
+ residuals_jets_.data());
+
+ // fill the residual vector
+ for (unsigned int i = 0; i < RES; i++)
+ residual_(i) = residuals_jets_[i].a;
+
+ // fill the jacobian matrices
+ for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
+ if (!state_ptrs_[i]->isFixed())
+ for (unsigned int row = 0; row < RES; row++)
+ std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+ residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+ Ji.data() + row * state_block_sizes_.at(i));
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
+ }
- static const std::vector<unsigned int> state_block_sizes_;
+ std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+ {
+ return state_ptrs_const_;
+ }
+ std::vector<StateBlockPtr> getStateBlockPtrVector() override
+ {
+ return state_ptrs_;
+ }
+
+ std::vector<unsigned int> getStateSizes() const override
+ {
+ return state_block_sizes_;
+ }
+
+ unsigned int getSize() const override
+ {
+ return RES;
+ }
+};
- typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 +
- B5 + B6 + B7 + B8 + B9 +
- B10 + B11> WolfJet;
+////////////////// SPECIALIZATION 12 BLOCKS ////////////////////////////////////////////////////////////////////////
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,0,0,0> : public FactorBase
+{
+ public:
+ static const unsigned int n_blocks = 12;
+
+ static const std::vector<unsigned int> state_block_sizes_;
+
+ typedef ceres::Jet<double, B0 + B1 + B2 + B3 + B4 + B5 +
+ B6 + B7 + B8 + B9 + B10 + B11> WolfJet;
+
protected:
-
+
std::vector<StateBlockPtr> state_ptrs_;
std::vector<StateBlockConstPtr> state_ptrs_const_;
-
+
static const std::vector<unsigned int> jacobian_locations_;
mutable std::array<WolfJet, RES> residuals_jets_;
mutable std::array<WolfJet, B0> jets_0_;
@@ -79,10 +1085,9 @@ class FactorAutodiff : public FactorBase
mutable std::array<WolfJet, B9> jets_9_;
mutable std::array<WolfJet, B10> jets_10_;
mutable std::array<WolfJet, B11> jets_11_;
-
+
public:
- /** \brief Constructor valid for all categories (ABSOLUTE, FRAME, FEATURE, LANDMARK)
- **/
+
FactorAutodiff(const std::string& _tp,
const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
@@ -169,7 +1174,7 @@ class FactorAutodiff : public FactorBase
assert(_state9Ptr != nullptr && "nullptr state block");
assert(_state10Ptr != nullptr && "nullptr state block");
assert(_state11Ptr != nullptr && "nullptr state block");
-
+
// initialize jets
unsigned int last_jet_idx = 0;
for (unsigned int i = 0; i < B0; i++)
@@ -196,22 +1201,16 @@ class FactorAutodiff : public FactorBase
jets_10_[i] = WolfJet(0, last_jet_idx++);
for (unsigned int i = 0; i < B11; i++)
jets_11_[i] = WolfJet(0, last_jet_idx++);
+
}
-
- ~FactorAutodiff() override
- {
- }
-
+
+ ~FactorAutodiff() override = default;
+
JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
}
-
- /** \brief Returns the residual and jacobians given the state values
- *
- * Returns the residual and jacobians given the state values
- *
- **/
+
bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
{
// only residuals
@@ -238,7 +1237,7 @@ class FactorAutodiff : public FactorBase
std::vector<double const*> param_vec;
param_vec.assign(parameters,parameters+n_blocks);
updateJetsRealPart(param_vec);
-
+
// call functor
(*static_cast<FacT const*>(this))(jets_0_.data(),
jets_1_.data(),
@@ -253,11 +1252,11 @@ class FactorAutodiff : public FactorBase
jets_10_.data(),
jets_11_.data(),
residuals_jets_.data());
-
+
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
residuals[i] = residuals_jets_[i].a;
-
+
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
if (jacobians[i] != nullptr)
@@ -268,10 +1267,7 @@ class FactorAutodiff : public FactorBase
}
return true;
}
-
- /** \brief Updates all jets real part with values of parameters
- *
- **/
+
void updateJetsRealPart(const std::vector<double const*>& parameters) const
{
// update jets real part
@@ -300,20 +1296,17 @@ class FactorAutodiff : public FactorBase
for (unsigned int i = 0; i < B11; i++)
jets_11_[i].a = parameters[11][i];
}
-
- /** \brief Returns a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
- *
- **/
+
void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
{
assert(residual_.size() == RES);
jacobians_.clear();
-
+
assert(_states_ptr.size() == n_blocks);
-
+
// update jets real part
updateJetsRealPart(_states_ptr);
-
+
// call functor
(*static_cast<FacT const*>(this))(jets_0_.data(),
jets_1_.data(),
@@ -328,11 +1321,11 @@ class FactorAutodiff : public FactorBase
jets_10_.data(),
jets_11_.data(),
residuals_jets_.data());
-
+
// fill the residual vector
for (unsigned int i = 0; i < RES; i++)
residual_(i) = residuals_jets_[i].a;
-
+
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
{
@@ -347,17 +1340,8 @@ class FactorAutodiff : public FactorBase
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- // for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
- /** \brief Returns a vector of pointers to the states
- *
- * Returns a vector of pointers to the state in which this factor depends
- *
- **/
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
{
return state_ptrs_const_;
@@ -366,47 +1350,24 @@ class FactorAutodiff : public FactorBase
{
return state_ptrs_;
}
-
- /** \brief Returns a vector of the states sizes
- *
- **/
+
std::vector<unsigned int> getStateSizes() const override
{
return state_block_sizes_;
}
-
- /** \brief Returns the residual size
- *
- * Returns the residual size
- *
- **/
+
unsigned int getSize() const override
{
return RES;
}
};
-
////////////////// SPECIALIZATION 11 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = B8;
- static const unsigned int block9Size = B9;
- static const unsigned int block10Size = B10;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 11;
static const std::vector<unsigned int> state_block_sizes_;
@@ -545,9 +1506,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -678,10 +1637,6 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- // for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -707,23 +1662,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
////////////////// SPECIALIZATION 10 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = B8;
- static const unsigned int block9Size = B9;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 10;
static const std::vector<unsigned int> state_block_sizes_;
@@ -855,9 +1796,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -983,10 +1922,6 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- // for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -1012,23 +1947,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
////////////////// SPECIALIZATION 9 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = B8;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 9;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1153,10 +2074,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
}
- ~FactorAutodiff() override
- {
- }
-
+ ~FactorAutodiff() override = default;
+
JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
@@ -1276,10 +2195,6 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- // for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -1305,23 +2220,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
////////////////// SPECIALIZATION 8 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = B7;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 8;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1439,9 +2340,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -1557,10 +2456,6 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- // for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -1585,23 +2480,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
////////////////// SPECIALIZATION 7 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = B6;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 7;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1712,9 +2593,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -1825,10 +2704,6 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- //for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -1853,23 +2728,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
////////////////// SPECIALIZATION 6 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = B5;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 6;
static const std::vector<unsigned int> state_block_sizes_;
@@ -1973,9 +2834,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -2105,23 +2964,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 5 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = B4;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 5;
static const std::vector<unsigned int> state_block_sizes_;
@@ -2217,9 +3062,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -2344,23 +3187,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 4 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = B3;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 4;
static const std::vector<unsigned int> state_block_sizes_;
@@ -2449,9 +3278,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -2547,10 +3374,6 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- //for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -2575,23 +3398,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 3 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = B2;
- static const unsigned int block3Size = 0;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 3;
static const std::vector<unsigned int> state_block_sizes_;
@@ -2673,9 +3482,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -2766,10 +3573,6 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- //for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -2794,23 +3597,9 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 2 BLOCKS ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
-class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = B1;
- static const unsigned int block2Size = 0;
- static const unsigned int block3Size = 0;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 2;
static const std::vector<unsigned int> state_block_sizes_;
@@ -2885,9 +3674,7 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -2973,10 +3760,6 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- //for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -3000,23 +3783,9 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
////////////////// SPECIALIZATION 1 BLOCK ////////////////////////////////////////////////////////////////////////
template <class FacT,unsigned int RES,unsigned int B0>
-class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
+class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
-
- static const unsigned int residualSize = RES;
- static const unsigned int block0Size = B0;
- static const unsigned int block1Size = 0;
- static const unsigned int block2Size = 0;
- static const unsigned int block3Size = 0;
- static const unsigned int block4Size = 0;
- static const unsigned int block5Size = 0;
- static const unsigned int block6Size = 0;
- static const unsigned int block7Size = 0;
- static const unsigned int block8Size = 0;
- static const unsigned int block9Size = 0;
- static const unsigned int block10Size = 0;
- static const unsigned int block11Size = 0;
static const unsigned int n_blocks = 1;
static const std::vector<unsigned int> state_block_sizes_;
@@ -3084,9 +3853,7 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
}
- ~FactorAutodiff() override
- {
- }
+ ~FactorAutodiff() override = default;
JacobianMethod getJacobianMethod() const override
{
@@ -3167,10 +3934,6 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
// add to the Jacobians vector
jacobians_.push_back(Ji);
}
-
- // print jacobian matrices
- //for (unsigned int i = 0; i < n_blocks; i++)
- // std::cout << jacobians_[i] << std::endl << std::endl;
}
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
@@ -3197,6 +3960,15 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
// STATIC CONST VECTORS INITIALIZATION
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// state_block_sizes_
+// 15 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11,unsigned int B12,unsigned int B13,unsigned int B14>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,B13,B14>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,B13,B14};
+// 14 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11,unsigned int B12,unsigned int B13>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,B13>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,B13};
+// 13 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11,unsigned int B12>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12};
// 12 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11>
const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11};
@@ -3235,107 +4007,155 @@ template <class FacT,unsigned int RES,unsigned int B0>
const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0};
// jacobian_locations_
+// 15 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11,unsigned int B12,unsigned int B13,unsigned int B14>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,B13,B14>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10+B11,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10+B11+B12,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10+B11+B12+B13};
+// 14 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11,unsigned int B12,unsigned int B13>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,B13,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10+B11,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10+B11+B12};
+// 13 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11,unsigned int B12>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,B12,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10+B11};
// 12 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10,unsigned int B11>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7,
- B0+B1+B2+B3+B4+B5+B6+B7+B8,
- B0+B1+B2+B3+B4+B5+B6+B7+B8+B9,
- B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,B11,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9+B10};
// 10 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9,unsigned int B10>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7,
- B0+B1+B2+B3+B4+B5+B6+B7+B8,
- B0+B1+B2+B3+B4+B5+B6+B7+B8+B9};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8+B9};
// 10 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7,
- B0+B1+B2+B3+B4+B5+B6+B7+B8};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8};
// 9 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7};
// 8 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6};
// 7 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5};
// 6 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4};
// 5 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3};
// 4 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2};
// 3 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1};
// 2 BLOCKS
template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0};
// 1 BLOCK
template <class FacT,unsigned int RES,unsigned int B0>
-const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0};
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0};
} // namespace wolf
\ No newline at end of file
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index d35617be28a0999ddeadaa54b05597b7fd7d9015..b5529a667335850a5c5dd09f0d64ab843d08d9d5 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -122,6 +122,31 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
return topology_;
}
+ std::string getTopologyString() const
+ {
+ switch (topology_)
+ {
+ case TOP_GEOM:
+ return "GEOM";
+ break;
+ case TOP_ABS:
+ return "ABS";
+ break;
+ case TOP_LMK:
+ return "LMK";
+ break;
+ case TOP_LOOP:
+ return "LOOP";
+ break;
+ case TOP_OTHER:
+ return "OTHER";
+ break;
+ default:
+ return "UNDEFINED";
+ }
+ }
+
+
/** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
virtual bool evaluate(double const* const* _parameters, double* _residuals, double** _jacobians) const = 0;
diff --git a/include/core/factor/factor_relative_pose_2d.h b/include/core/factor/factor_relative_pose_2d.h
index 3271c97d3cc8fd8b66512532e68afd00ab33339e..5230d88abd7be872cd3e5ac7e7a0443eb3e0352a 100644
--- a/include/core/factor/factor_relative_pose_2d.h
+++ b/include/core/factor/factor_relative_pose_2d.h
@@ -141,11 +141,11 @@ class FactorRelativePose2d : public FactorAnalytic
*
**/
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
- std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
- const std::vector<bool>& _compute_jacobian) const override;
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
- std::vector<Eigen::MatrixXd>& jacobians,
- const std::vector<bool>& _compute_jacobian) const override;
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
@@ -174,8 +174,8 @@ inline Eigen::VectorXd FactorRelativePose2d::evaluateResiduals(
}
inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
- std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
- const std::vector<bool>& _compute_jacobian) const
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const
{
assert(jacobians.size() == 4);
assert(_st_vector.size() == 4);
@@ -210,8 +210,8 @@ inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map
}
inline void FactorRelativePose2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
- std::vector<Eigen::MatrixXd>& jacobians,
- const std::vector<bool>& _compute_jacobian) const
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const
{
assert(jacobians.size() == 4);
assert(_st_vector.size() == 4);
diff --git a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
index 67260654b58004a61e4a3d6ee612255e1861e615..6989352149574bc3724e1443eb220d36755f4457 100644
--- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
@@ -64,6 +64,8 @@ class FactorRelativePose2dWithExtrinsics : public FactorAutodiff<FactorRelativeP
const T* const _p_sensor,
const T* const _o_sensor,
T* _residuals) const;
+
+ Eigen::Vector3d residual() const;
};
inline FactorRelativePose2dWithExtrinsics::FactorRelativePose2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
@@ -131,31 +133,49 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_re
Eigen::Map<const Eigen::Matrix<T,2,1> > p_ref(_p_ref);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_target(_p_target);
Eigen::Map<const Eigen::Matrix<T,2,1> > p_sensor(_p_sensor);
- T o_ref = _o_ref[0];
- T o_target = _o_target[0];
- T o_sensor = _o_sensor[0];
+ const T& o_ref = _o_ref[0];
+ const T& o_target = _o_target[0];
+ const T& o_sensor = _o_sensor[0];
Eigen::Matrix<T, 3, 1> expected_measurement;
Eigen::Matrix<T, 3, 1> er; // error
- // Expected measurement
- // r1_p_r1_s1 = ps
- // r2_p_r2_s2 = ps
- // r1_R_s1 = R(os)
- // r2_R_s2 = R(os)
- // w_R_r1 = R(o1)
- // w_R_r2 = R(o2)
- // ----------------------------------------
-
- // s1_p_s1_s2 = s1_R_r1*r1_p_s1_r1 +
- // s1_R_r1*r1_R_w*w_p_r1_w +
- // s1_R_r1*r1_R_w*w_p_w_r2 +
- // s1_R_r1*r1_R_w*w_R_r2*r2_p_r2_s2
-
- // s1_p_s1_s2 = s1_R_r1*(r1_p_s1_r1 + r1_R_w*(w_p_r1_w + w_p_w_r2 + w_R_r2*r2_p_r2_s2))
-
- expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target + Eigen::Rotation2D<T>(o_target)*p_sensor));
- expected_measurement(2) = o_target - o_ref;
+ // FRAME CASE
+ if (not getFrameOtherList().empty())
+ {
+ // Expected measurement
+ // r1_p_s1 = p_sensor
+ // r2_p_s2 = p_sensor
+ // r1_R_s1 = R(o_sensor)
+ // r2_R_s2 = R(o_sensor)
+ // w_p_r1 = p_ref
+ // w_p_r2 = p_target
+ // w_R_r1 = R(o_ref)
+ // w_R_r2 = R(o_target)
+ // ----------------------------------------
+ // s1_p_s2 = s1_R_r1*(r1_R_w*(w_p_r2 + w_R_r2*r2_p_s2 - w_p_r1) - r1_p_s1)
+
+ expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target + Eigen::Rotation2D<T>(o_target)*p_sensor));
+ expected_measurement(2) = o_target - o_ref;
+ }
+ // LANDMARK CASE
+ else if (not getLandmarkOtherList().empty())
+ {
+ // Expected measurement
+ // r1_p_s1 = p_sensor
+ // r2_p_s2 = p_sensor
+ // r1_R_s1 = R(o_sensor)
+ // r2_R_s2 = R(o_sensor)
+ // w_p_r1 = p_ref
+ // w_p_r2 = p_target
+ // w_R_r1 = R(o_ref)
+ // w_R_r2 = R(o_target)
+ // ----------------------------------------
+ // s1_p_s2 = s1_R_r1*(r1_R_w*(w_p_r2 - w_p_r1) - r1_p_s1)
+
+ expected_measurement.head(2) = Eigen::Rotation2D<T>(-o_sensor)*(-p_sensor + Eigen::Rotation2D<T>(-o_ref)*(-p_ref + p_target));
+ expected_measurement(2) = o_target - o_ref - o_sensor;
+ }
// Error
er = expected_measurement - getMeasurement().cast<T>();
@@ -185,4 +205,32 @@ inline bool FactorRelativePose2dWithExtrinsics::operator ()(const T* const _p_re
return true;
}
+inline Eigen::Vector3d FactorRelativePose2dWithExtrinsics::residual() const
+{
+ Eigen::Vector3d res;
+ Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target, o_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();
+ o_target = getCapture()->getFrame()->getO()->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();
+ o_target = getLandmarkOther()->getO()->getState();
+ }
+
+ operator() (p_ref.data(), o_ref.data(), p_target.data(), o_target.data(), p_sensor.data(), o_sensor.data(), res.data());
+
+ return res;
+}
+
} // namespace wolf
\ No newline at end of file
diff --git a/include/core/factor/factor_relative_pose_3d.h b/include/core/factor/factor_relative_pose_3d.h
index e73a3fc86ec32475cb770ea9b49fa2454b0bc0f2..08e3d487342a436c02bb041935616369c36773df 100644
--- a/include/core/factor/factor_relative_pose_3d.h
+++ b/include/core/factor/factor_relative_pose_3d.h
@@ -40,24 +40,31 @@ class FactorRelativePose3d : public FactorAutodiff<FactorRelativePose3d,6,3,4,3,
const FactorTopology& _top,
FactorStatus _status = FAC_ACTIVE);
+ FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
+ const LandmarkBasePtr& _landmark_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE);
+
~FactorRelativePose3d() override = default;
template<typename T>
- bool operator ()(const T* const _p_current,
- const T* const _q_current,
- const T* const _p_past,
- const T* const _q_past,
+ bool operator ()(const T* const _p_target,
+ const T* const _q_target,
+ const T* const _p_ref,
+ const T* const _q_ref,
T* _residuals) const;
template<typename T>
- bool expectation(const T* const _p_current,
- const T* const _q_current,
- const T* const _p_past,
- const T* const _q_past,
+ bool expectation(const T* const _p_target,
+ const T* const _q_target,
+ const T* const _p_ref,
+ const T* const _q_ref,
T* _expectation_dp,
T* _expectation_dq) const;
- Eigen::VectorXd expectation() const;
+ Eigen::Vector7d expectation() const;
private:
template<typename T>
@@ -110,39 +117,64 @@ inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_cur
MatrixSizeCheck<6,6>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
}
+inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_current_ptr,
+ const LandmarkBasePtr& _lmk_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff<FactorRelativePose3d, 6, 3, 4, 3, 4>("FactorRelativePose3d", // 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,
+ _lmk_ptr->getP(), // landmark P
+ _lmk_ptr->getO(), // landmark Q
+ _ftr_current_ptr->getFrame()->getP(), // current frame P
+ _ftr_current_ptr->getFrame()->getO()) // current frame Q
+{
+ MatrixSizeCheck<7,1>::check(_ftr_current_ptr->getMeasurement());
+ MatrixSizeCheck<6,6>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
+}
+
template<typename T>
-inline bool FactorRelativePose3d::expectation(const T* const _p_current,
- const T* const _q_current,
- const T* const _p_past,
- const T* const _q_past,
- T* _expectation_dp,
- T* _expectation_dq) const
+inline bool FactorRelativePose3d::expectation(const T* const _p_target,
+ const T* const _q_target,
+ const T* const _p_ref,
+ const T* const _q_ref,
+ T* _expectation_dp,
+ T* _expectation_dq) const
{
- Eigen::Map<const Eigen::Matrix<T,3,1> > p_current(_p_current);
- Eigen::Map<const Eigen::Quaternion<T> > q_current(_q_current);
- Eigen::Map<const Eigen::Matrix<T,3,1> > p_past(_p_past);
- Eigen::Map<const Eigen::Quaternion<T> > q_past(_q_past);
+ Eigen::Map<const Eigen::Matrix<T,3,1> > p_target(_p_target);
+ Eigen::Map<const Eigen::Quaternion<T> > q_target(_q_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> > expectation_dp(_expectation_dp);
Eigen::Map<Eigen::Quaternion<T> > expectation_dq(_expectation_dq);
-// std::cout << "p_current: " << p_current(0) << " "
-// << p_current(1) << " "
-// << p_current(2) << "\n";
-// std::cout << "q_current: " << q_current.coeffs()(0) << " "
-// << q_current.coeffs()(1) << " "
-// << q_current.coeffs()(2) << " "
-// << q_current.coeffs()(3) << "\n";
-// std::cout << "p_past: " << p_past(0) << " "
-// << p_past(1) << " "
-// << p_past(2) << "\n";
-// std::cout << "q_past: " << q_past.coeffs()(0) << " "
-// << q_past.coeffs()(1) << " "
-// << q_past.coeffs()(2) << " "
-// << q_past.coeffs()(3) << "\n";
+// std::cout << "p_target: " << p_target(0) << " "
+// << p_target(1) << " "
+// << p_target(2) << "\n";
+// std::cout << "q_target: " << q_target.coeffs()(0) << " "
+// << q_target.coeffs()(1) << " "
+// << q_target.coeffs()(2) << " "
+// << q_target.coeffs()(3) << "\n";
+// 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";
// estimate motion increment, dp, dq;
- expectation_dp = q_past.conjugate() * (p_current - p_past);
- expectation_dq = q_past.conjugate() * q_current;
+ expectation_dp = q_ref.conjugate() * (p_target - p_ref);
+ expectation_dq = q_ref.conjugate() * q_target;
// std::cout << "exp_p: " << expectation_dp(0) << " "
// << expectation_dp(1) << " "
@@ -155,34 +187,38 @@ inline bool FactorRelativePose3d::expectation(const T* const _p_current,
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();
- const Eigen::VectorXd frame_current_pos = frm_current->getP()->getState();
- const Eigen::VectorXd frame_current_ori = frm_current->getO()->getState();
- const Eigen::VectorXd frame_past_pos = frm_past->getP()->getState();
- const Eigen::VectorXd frame_past_ori = frm_past->getO()->getState();
-
-// std::cout << "frame_current_pos: " << frm_current->getP()->getState().transpose() << std::endl;
-// std::cout << "frame_past_pos: " << frm_past->getP()->getState().transpose() << std::endl;
-
- expectation(frame_current_pos.data(),
- frame_current_ori.data(),
- frame_past_pos.data(),
- frame_past_ori.data(),
+ auto lmk = getLandmarkOther();
+
+ const Eigen::VectorXd target_pos = ( frm_past ? frm_current->getP()->getState() : lmk->getP()->getState());
+ const Eigen::VectorXd target_ori = ( frm_past ? frm_current->getO()->getState() : lmk->getO()->getState());
+ 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: " << 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(),
+ ref_pos.data(),
+ ref_ori.data(),
exp.data(),
exp.data()+3);
return exp;
}
template<typename T>
-inline bool FactorRelativePose3d::operator ()(const T* const _p_current,
- const T* const _q_current,
- const T* const _p_past,
- const T* const _q_past,
- T* _residuals) const
+inline bool FactorRelativePose3d::operator ()(const T* const _p_target,
+ const T* const _q_target,
+ const T* const _p_ref,
+ const T* const _q_ref,
+ T* _residuals) const
{
/* Residual expression
@@ -224,7 +260,7 @@ inline bool FactorRelativePose3d::operator ()(const T* const _p_current,
Eigen::Map<Eigen::Matrix<T,6,1> > residuals(_residuals);
Eigen::Matrix<T, Eigen::Dynamic, 1> expected(7) ;
- expectation(_p_current, _q_current, _p_past, _q_past, expected.data(), expected.data()+3);
+ expectation(_p_target, _q_target, _p_ref, _q_ref, expected.data(), expected.data()+3);
// measured motion increment, dp_m, dq_m
// Eigen::Matrix<T,3,1> dp_m = getMeasurement().head<3>().cast<T>();
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 5c463ff979bcec3e254959d42024f8d85e280f12..3bbcc4bcc3899b2e2cfb8d4e5a29535f5a1f33a2 100644
--- a/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_3d_with_extrinsics.h
@@ -57,7 +57,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.
**/
@@ -115,6 +115,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,
@@ -142,45 +144,59 @@ inline FactorRelativePose3dWithExtrinsics::FactorRelativePose3dWithExtrinsics(co
{
}
-inline FactorRelativePose3dWithExtrinsics::~FactorRelativePose3dWithExtrinsics()
-{
- //
-}
-
template<typename T>
inline bool FactorRelativePose3dWithExtrinsics::operator ()(const T* const _p_ref,
- const T* const _o_ref,
- const T* const _p_target,
- const T* const _o_target,
- const T* const _p_sensor,
- const T* const _o_sensor,
- T* _residuals) const
+ const T* const _o_ref,
+ const T* const _p_target,
+ const T* const _o_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_c(_p_sensor);
- Eigen::Map<const Eigen::Quaternion<T>> q_r_c(_o_sensor);
+ 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_l(_p_target);
- Eigen::Map<const Eigen::Quaternion<T>> q_w_l(_o_target);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_t(_p_target);
+ Eigen::Map<const Eigen::Quaternion<T>> q_w_t(_o_target);
Eigen::Map<Eigen::Matrix<T,6,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());
+ // WOLF_INFO("o_target: ", q_w_t.coeffs().transpose());
+
// Expected measurement
- Eigen::Quaternion<T> q_c_w = (q_w_r * q_r_c).conjugate();
- Eigen::Quaternion<T> q_c_l = q_c_w * q_w_l;
- Eigen::Matrix<T,3,1> p_c_l = q_c_w * (-(p_w_r + q_w_r * p_r_c) + p_w_l);
+ Eigen::Quaternion<T> expected_q;
+ Eigen::Matrix<T,3,1> expected_p;
+ // FRAME CASE
+ if (not getFrameOtherList().empty())
+ {
+ expected_p = q_r_s.conjugate() * (q_w_r.conjugate() * (p_w_t + q_w_t * p_r_s - p_w_r) - p_r_s);
+ expected_q = q_r_s.conjugate() * q_w_r.conjugate() * q_w_t * q_r_s;
+ }
+ // LANDMARK CASE
+ else if (not getLandmarkOtherList().empty())
+ {
+ expected_p = q_r_s.conjugate() * (q_w_r.conjugate() * (p_w_t - p_w_r) - p_r_s);
+ expected_q = q_r_s.conjugate() * q_w_r.conjugate() * q_w_t;
+ }
// Measurement
- Eigen::Vector3d p_c_l_meas(getMeasurement().head<3>());
+ Eigen::Vector3d p_meas(getMeasurement().head<3>());
Eigen::Quaterniond q_c_l_meas(getMeasurement().data() + 3 );
Eigen::Quaternion<T> q_l_c_meas = q_c_l_meas.conjugate().cast<T>();
//Eigen::Matrix<T,3,1> p_l_c_meas = -q_l_c_meas * p_c_l_meas.cast<T>();
// Error
Eigen::Matrix<T, 6, 1> err;
- err.head(3) = q_l_c_meas * (p_c_l_meas.cast<T>() - p_c_l);
- //err.tail(3) = wolf::log_q(q_l_c_meas * q_c_l); // true error function for which the covariance should be computed
- err.tail(3) = T(2)*(q_l_c_meas * q_c_l).vec(); // 1rst order approximation of sin function ( 2*sin(aa/2) ~ aa )
+ // err.head(3) = q_l_c_meas * (p_meas.cast<T>() - expected_p);
+ err.head(3) = (p_meas - expected_p);
+ err.tail(3) = wolf::log_q(q_l_c_meas * expected_q); // true error function for which the covariance should be computed
+ //err.tail(3) = T(2)*(q_l_c_meas * expected_q).vec(); // 1rst order approximation of sin function ( 2*sin(aa/2) ~ aa )
// Residual
residuals = getMeasurementSquareRootInformationUpper().cast<T>() * err;
@@ -191,15 +207,27 @@ inline bool FactorRelativePose3dWithExtrinsics::operator ()(const T* const _p_re
inline Eigen::Vector6d FactorRelativePose3dWithExtrinsics::residual() const
{
Eigen::Vector6d res;
- double * p_camera, * o_camera, * p_ref, * o_ref, * p_target, * o_target;
- p_camera = getCapture()->getSensorP()->getState().data();
- o_camera = getCapture()->getSensorO()->getState().data();
- p_ref = getCapture()->getFrame()->getP()->getState().data();
- o_ref = getCapture()->getFrame()->getO()->getState().data();
- p_target = getLandmarkOther()->getP()->getState().data();
- o_target = getLandmarkOther()->getO()->getState().data();
-
- operator() (p_camera, o_camera, p_ref, o_ref, p_target, o_target, res.data());
+ Eigen::VectorXd p_sensor, o_sensor, p_ref, o_ref, p_target, o_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();
+ o_target = getCapture()->getFrame()->getO()->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();
+ o_target = getLandmarkOther()->getO()->getState();
+ }
+
+ operator() (p_ref.data(), o_ref.data(), p_target.data(), o_target.data(), p_sensor.data(), o_sensor.data(), res.data());
return res;
}
diff --git a/include/core/factor/factor_relative_position_2d.h b/include/core/factor/factor_relative_position_2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..8a2c7a0e52a7b58f8cda42586c56abff31793e79
--- /dev/null
+++ b/include/core/factor/factor_relative_position_2d.h
@@ -0,0 +1,258 @@
+//--------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_analytic.h"
+#include "core/landmark/landmark_base.h"
+#include "core/math/rotations.h"
+#include <Eigen/StdVector>
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorRelativePosition2d);
+
+//class
+class FactorRelativePosition2d : public FactorAnalytic
+{
+ public:
+
+ /** \brief Constructor of category FAC_FRAME
+ **/
+ FactorRelativePosition2d(const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorRelativePosition2d",
+ _top,
+ _ftr_ptr,
+ _frame_other_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP())
+ {
+ //
+ }
+
+ /** \brief Constructor of category FAC_FEATURE
+ **/
+ FactorRelativePosition2d(const FeatureBasePtr& _ftr_ptr,
+ const FeatureBasePtr& _ftr_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorRelativePosition2d",
+ _top,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ _ftr_other_ptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_other_ptr->getFrame()->getP() )
+ {
+ //
+ }
+
+ /** \brief Constructor of category FAC_LANDMARK
+ **/
+ FactorRelativePosition2d(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("FactorRelativePosition2d",
+ _top,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _landmark_other_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _landmark_other_ptr->getP())
+ {
+ //
+ }
+
+ ~FactorRelativePosition2d() override = default;
+
+ /** \brief Returns the factor residual size
+ **/
+ unsigned int getSize() const override
+ {
+ return 2;
+ }
+
+ /** \brief Returns the residual evaluated in the states provided
+ *
+ * Returns the residual evaluated in the states provided in a std::vector of mapped Eigen::VectorXd
+ **/
+ Eigen::VectorXd evaluateResiduals(
+ const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const override;
+
+ /** \brief Returns the jacobians evaluated in the states provided
+ *
+ * Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
+ * IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
+ *
+ * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
+ * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
+ * \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
+ *
+ **/
+ void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
+ void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const override;
+
+ /** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
+ * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
+ *
+ **/
+ void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
+
+};
+
+/// IMPLEMENTATION ///
+
+inline Eigen::VectorXd FactorRelativePosition2d::evaluateResiduals(
+ const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
+{
+ Eigen::VectorXd residual(2);
+ Eigen::VectorXd expected_measurement(2);
+ // Expected measurement
+ Eigen::Matrix2d R = Eigen::Rotation2D<double>(-_st_vector[1](0)).matrix();
+ expected_measurement = R * (_st_vector[2] - _st_vector[0]); // rotar menys l'angle de primer (-_o1)
+ // Residual
+ residual = expected_measurement - getMeasurement();
+ residual = getMeasurementSquareRootInformationUpper() * residual;
+ return residual;
+}
+
+inline void FactorRelativePosition2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixRowXd> >& jacobians,
+ const std::vector<bool>& _compute_jacobian) const
+{
+ assert(jacobians.size() == 3);
+ assert(_st_vector.size() == 3);
+ assert(_compute_jacobian.size() == 3);
+
+ if (_compute_jacobian[0])
+ {
+ jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
+ -cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
+ sin(_st_vector[1](0)), -cos(_st_vector[1](0))).finished();
+ }
+ if (_compute_jacobian[1])
+ {
+ jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,1) <<
+ -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) +
+ (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
+ -(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) -
+ (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0))).finished();
+ }
+ if (_compute_jacobian[2])
+ {
+ jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
+ cos(_st_vector[1](0)), sin(_st_vector[1](0)),
+ -sin(_st_vector[1](0)), cos(_st_vector[1](0))).finished();
+ }
+}
+
+inline void FactorRelativePosition2d::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
+ std::vector<Eigen::MatrixXd>& jacobians,
+ const std::vector<bool>& _compute_jacobian) const
+{
+ assert(jacobians.size() == 3);
+ assert(_st_vector.size() == 3);
+ assert(_compute_jacobian.size() == 3);
+
+ if (_compute_jacobian[0])
+ {
+ jacobians[0] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
+ -cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
+ sin(_st_vector[1](0)), -cos(_st_vector[1](0))).finished();
+ }
+ if (_compute_jacobian[1])
+ {
+ jacobians[1] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,1) <<
+ -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) +
+ (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
+ -(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) -
+ (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0))).finished();
+ }
+ if (_compute_jacobian[2])
+ {
+ jacobians[2] = getMeasurementSquareRootInformationUpper() * (Eigen::MatrixRowXd(2,2) <<
+ cos(_st_vector[1](0)), sin(_st_vector[1](0)),
+ -sin(_st_vector[1](0)), cos(_st_vector[1](0))).finished();
+ }
+}
+
+inline void FactorRelativePosition2d::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
+{
+ assert(jacobians.size() == 3);
+
+ jacobians[0] = (Eigen::MatrixXd(2,2) <<
+ -cos(getStateBlockPtrVector()[1]->getState()(0)),
+ -sin(getStateBlockPtrVector()[1]->getState()(0)),
+ sin(getStateBlockPtrVector()[1]->getState()(0)),
+ -cos(getStateBlockPtrVector()[1]->getState()(0))).finished();
+
+ jacobians[1] = (Eigen::MatrixXd(2,1) <<
+ -(getStateBlockPtrVector()[2]->getState()(0)
+ - getStateBlockPtrVector()[0]->getState()(0)) * sin(getStateBlockPtrVector()[1]->getState()(0))
+ + (getStateBlockPtrVector()[2]->getState()(1)
+ - getStateBlockPtrVector()[0]->getState()(1)) * cos(getStateBlockPtrVector()[1]->getState()(0)),
+ -(getStateBlockPtrVector()[2]->getState()(0)
+ - getStateBlockPtrVector()[0]->getState()(0)) * cos(getStateBlockPtrVector()[1]->getState()(0))
+ - (getStateBlockPtrVector()[2]->getState()(1)
+ - getStateBlockPtrVector()[0]->getState()(1)) * sin(getStateBlockPtrVector()[1]->getState()(0))).finished();
+
+ jacobians[2] = (Eigen::MatrixXd(2,2) <<
+ cos(getStateBlockPtrVector()[1]->getState()(0)),
+ sin(getStateBlockPtrVector()[1]->getState()(0)),
+ -sin(getStateBlockPtrVector()[1]->getState()(0)),
+ cos(getStateBlockPtrVector()[1]->getState()(0))).finished();
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/core/factor/factor_relative_position_2d_with_extrinsics.h b/include/core/factor/factor_relative_position_2d_with_extrinsics.h
new file mode 100644
index 0000000000000000000000000000000000000000..fd36867f4a93e08fe671f0491a07276ed00b6624
--- /dev/null
+++ b/include/core/factor/factor_relative_position_2d_with_extrinsics.h
@@ -0,0 +1,128 @@
+//--------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(FactorRelativePosition2dWithExtrinsics);
+
+//class
+class FactorRelativePosition2dWithExtrinsics : public FactorAutodiff<FactorRelativePosition2dWithExtrinsics, 2, 2, 1, 2, 2, 1>
+{
+ public:
+
+ /** \brief Class constructor Frame-Landmark
+ */
+ FactorRelativePosition2dWithExtrinsics(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);
+
+ ~FactorRelativePosition2dWithExtrinsics() 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;
+};
+
+inline FactorRelativePosition2dWithExtrinsics::FactorRelativePosition2dWithExtrinsics(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkBasePtr& _lmk_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ const FactorTopology& _top,
+ FactorStatus _status) :
+ FactorAutodiff("FactorRelativePosition2dWithExtrinsics",
+ _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 FactorRelativePosition2dWithExtrinsics::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<Eigen::Matrix<T,2,1> > res(_residuals);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > p_ref(_p_ref);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > p_target(_p_target);
+ Eigen::Map<const Eigen::Matrix<T,2,1> > p_sensor(_p_sensor);
+ T o_ref = _o_ref[0];
+ T o_sensor = _o_sensor[0];
+
+ // Expected measurement
+ Eigen::Matrix<T, 2, 1> expected_measurement = Eigen::Rotation2D<T>(-o_sensor) *
+ (-p_sensor + Eigen::Rotation2D<T>(-o_ref) * (p_target - p_ref));
+
+ // Residuals
+ res = getMeasurementSquareRootInformationUpper() * (expected_measurement - getMeasurement());
+
+ ////////////////////////////////////////////////////////
+ // print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
+// using ceres::Jet;
+// Eigen::MatrixXs J(3,6);
+// J.row(0) = ((Jet<Scalar, 6>)(er(0))).v;
+// J.row(1) = ((Jet<Scalar, 6>)(er(1))).v;
+// J.row(2) = ((Jet<Scalar, 6>)(er(2))).v;
+// J.row(0) = ((Jet<Scalar, 6>)(res(0))).v;
+// J.row(1) = ((Jet<Scalar, 6>)(res(1))).v;
+// J.row(2) = ((Jet<Scalar, 6>)(res(2))).v;
+// if (sizeof(er(0)) != sizeof(double))
+// {
+// std::cout << "FactorRelativePosition2dWithExtrinsics::Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorRelativePosition2dWithExtrinsics::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorRelativePosition2dWithExtrinsics::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
+// }
+ ////////////////////////////////////////////////////////
+
+ return true;
+}
+
+} // namespace wolf
\ No newline at end of file
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/include/core/map/map_base.h b/include/core/map/map_base.h
index d1078082e587fe300123c84b8d1d97d498c8679d..764464c43ef96b01f1bf20ce18bca3f7d29c310a 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -102,9 +102,11 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
public:
LandmarkBaseConstPtrList getLandmarkList() const;
LandmarkBasePtrList getLandmarkList();
-
- void load(const std::string& _map_file_yaml);
- void save(const std::string& _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf");
+ LandmarkBaseConstPtr getLandmark(const unsigned int& _id) const;
+ LandmarkBasePtr getLandmark(const unsigned int& _id);
+
+ void load(std::string _map_file_yaml);
+ void save(std::string _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf") const;
virtual void printHeader(int depth, //
bool constr_by, //
@@ -123,7 +125,7 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
- std::string dateTimeNow();
+ std::string dateTimeNow() const;
};
inline LandmarkBaseConstPtrList MapBase::getLandmarkList() const
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index fbbc8276df890fc7982bdd7cdf8d6c361b7ff387..e9586cab58a356caf6c9c91f32501ad66bf471bf 100644
--- a/include/core/math/SE2.h
+++ b/include/core/math/SE2.h
@@ -106,6 +106,18 @@ inline void inverse(const MatrixBase<D1>& p, const typename D1::Scalar& o,
io = -o;
}
+template<typename D1, typename D2, typename D3, typename D4>
+inline void inverse(const MatrixBase<D1>& p, const MatrixBase<D2>& o,
+ MatrixBase<D3>& ip, MatrixBase<D4>& io)
+{
+ MatrixSizeCheck<2, 1>::check(p);
+ MatrixSizeCheck<1, 1>::check(o);
+ MatrixSizeCheck<2, 1>::check(ip);
+ MatrixSizeCheck<1, 1>::check(io);
+
+ inverse(p, o(0), ip, io(0));
+}
+
template<typename D1, typename D2>
inline void inverse(const MatrixBase<D1>& d,
MatrixBase<D2>& id)
@@ -127,6 +139,18 @@ inline Matrix<typename D::Scalar, 3, 1> inverse(const MatrixBase<D>& d)
return id;
}
+inline void inverse(const VectorComposite& v, VectorComposite& c)
+{
+ inverse(v.at('P'), v.at('O'), c['P'], c['O']);
+}
+
+inline VectorComposite inverse(const VectorComposite& v)
+{
+ VectorComposite c = identity();
+ inverse(v, c);
+ return c;
+}
+
template<class D1, class D2>
inline void exp(const Eigen::MatrixBase<D1>& _tau, Eigen::MatrixBase<D2>& _delta)
{
@@ -200,7 +224,7 @@ inline void exp(const VectorComposite& _tau, VectorComposite& _delta)
inline VectorComposite exp(const VectorComposite& tau)
{
- VectorComposite res("PO", Vector3d::Zero(), {2,1});
+ VectorComposite res = identity();
exp(tau, res);
@@ -229,7 +253,7 @@ inline void exp(const VectorComposite& _tau, VectorComposite& _delta, MatrixComp
}
template<class D1, class D2, class D3>
-inline void compose(const MatrixBase<D1> &_delta1, MatrixBase<D2> &_delta2, MatrixBase<D3> &_delta1_compose_delta2)
+inline void compose(const MatrixBase<D1> &_delta1, const MatrixBase<D2> &_delta2, MatrixBase<D3> &_delta1_compose_delta2)
{
MatrixSizeCheck<3, 1>::check(_delta1);
MatrixSizeCheck<3, 1>::check(_delta2);
@@ -240,12 +264,20 @@ inline void compose(const MatrixBase<D1> &_delta1, MatrixBase<D2> &_delta2, Matr
_delta1_compose_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
}
+template<class D1, class D2>
+inline Matrix<typename D1::Scalar,3,1> compose(const MatrixBase<D1> &_delta1, const MatrixBase<D2> &_delta2)
+{
+ Matrix<typename D1::Scalar,3,1> delta1_compose_delta2;
+ compose(_delta1, _delta2, delta1_compose_delta2);
+ return delta1_compose_delta2;
+}
+
template<class D1, class D2, class D3, class D4, class D5>
inline void compose(const MatrixBase<D1>& _delta1,
- const MatrixBase<D2>& _delta2,
- MatrixBase<D3>& _delta1_compose_delta2,
- MatrixBase<D4>& _J_compose_delta1,
- MatrixBase<D5>& _J_compose_delta2)
+ const MatrixBase<D2>& _delta2,
+ MatrixBase<D3>& _delta1_compose_delta2,
+ MatrixBase<D4>& _J_compose_delta1,
+ MatrixBase<D5>& _J_compose_delta2)
{
MatrixSizeCheck<3, 1>::check(_delta1);
MatrixSizeCheck<3, 1>::check(_delta2);
@@ -313,11 +345,18 @@ inline void compose(const VectorComposite& _x1,
_c['O'] = Matrix1d( pi2pi(a1 + a2) ) ;
}
+inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2)
+{
+ VectorComposite c = identity();
+ compose(x1, x2, c);
+ return c;
+}
+
inline void compose(const VectorComposite& _x1,
- const VectorComposite& _x2,
- VectorComposite& _c,
- MatrixComposite& _J_c_x1,
- MatrixComposite& _J_c_x2)
+ const VectorComposite& _x2,
+ VectorComposite& _c,
+ MatrixComposite& _J_c_x1,
+ MatrixComposite& _J_c_x2)
{
const auto& p1 = _x1.at('P');
const auto& a1 = _x1.at('O')(0); // angle
@@ -423,10 +462,7 @@ inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorCom
inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
{
- VectorComposite res;
-
- res['P'] = Vector2d();
- res['O'] = Vector1d();
+ VectorComposite res = identity();
plus(x, tau, res);
diff --git a/include/core/math/rotations.h b/include/core/math/rotations.h
index b1cdcc26c5f0a3ce8766426b07cb187e824a0d7c..4fce1da660032ee10d526a955003220c89c56fae 100644
--- a/include/core/math/rotations.h
+++ b/include/core/math/rotations.h
@@ -29,21 +29,14 @@ namespace wolf
//////////////////////////////////////////////////////////////
/** \brief Return angle between -pi and pi
- *
+ * This implementation uses std::remainder which makes it incompatible for ceres::jet (this implementation was included in ceres_wrapper/wolf_jet.h)
* @param angle
* @return formatted angle
*/
template<typename T>
-inline T
-pi2pi(const T _angle)
+inline T pi2pi(const T& _angle)
{
- T angle = _angle;
- while (angle > T( M_PI ))
- angle -= T( 2 * M_PI );
- while (angle <= T( -M_PI ))
- angle += T( 2 * M_PI );
-
- return angle;
+ return remainder(_angle,2*M_PI);
}
/** \brief Conversion to radians
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index a751022705f891fdaf1d8bbdb3419732c6e0019d..1d2dc002ab9d5aa47f0cf6aebf9891937a727732 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -338,7 +338,10 @@ class Problem : public std::enable_shared_from_this<Problem>
// All branches -------------------------------------------
- // perturb states
+ /**
+ * \brief perturb all states states with random noise
+ * following an uniform distribution in [ -amplitude, amplitude ]
+ */
void perturb(double amplitude = 0.01);
// transform states
void transform(const VectorComposite& _transformation);
diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h
index ae25dbf4cc08e45de3a241d0764d8bee5135fa0b..3475e6fd787122525eb1d2b71268bb8089b625b2 100644
--- a/include/core/processor/motion_provider.h
+++ b/include/core/processor/motion_provider.h
@@ -82,8 +82,12 @@ class MotionProvider
protected:
TypeComposite state_types_;
- VectorComposite odometry_;
ParamsMotionProviderPtr params_motion_provider_;
+
+ private:
+ VectorComposite odometry_;
+ mutable std::mutex mut_odometry_;
+
};
// inline MotionProvider::MotionProvider(const StateKeys& _structure, ParamsMotionProviderPtr _params) :
@@ -94,11 +98,6 @@ inline MotionProvider::MotionProvider(const TypeComposite& _state_types, ParamsM
checkTypeComposite(_state_types);
}
-inline wolf::VectorComposite MotionProvider::getOdometry ( ) const
-{
- return odometry_;
-}
-
}
///// IMPLEMENTATION ///////
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 0d2f18f2d984f1616c42168202abcab09740e556..e33ff4d726e6eb3071f04f1bba9c4846c0934b07 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -352,10 +352,10 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
void setTimeTolerance(double _time_tolerance);
double getTimeTolerance() const;
- bool checkTimeTolerance (const TimeStamp& _ts1, const TimeStamp& _ts2);
- bool checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp& _ts);
- bool checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp& _ts);
- bool checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap);
+ bool checkTimeTolerance (const TimeStamp& _ts1, const TimeStamp& _ts2) const;
+ bool checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp& _ts) const;
+ bool checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp& _ts) const;
+ bool checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap) const;
void link(SensorBasePtr);
diff --git a/include/core/processor/processor_landmark_external.h b/include/core/processor/processor_landmark_external.h
new file mode 100644
index 0000000000000000000000000000000000000000..14745c09f8ac53fcee0b8d2ca2078aec43bc67fe
--- /dev/null
+++ b/include/core/processor/processor_landmark_external.h
@@ -0,0 +1,165 @@
+//--------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/common/wolf.h"
+#include "core/processor/processor_tracker.h"
+#include "core/processor/track_matrix.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLandmarkExternal);
+
+struct ParamsProcessorLandmarkExternal : public ParamsProcessorTracker
+{
+ bool use_orientation; ///< use orientation measure or not when emplacing factors
+ double match_dist_th; ///< for considering tracked detection: distance threshold to previous detection
+ unsigned int new_features_for_keyframe; ///< for keyframe voting: amount of new features with respect to origin (sufficient condition if more than min_features_for_keyframe)
+ double filter_quality_th; ///< min quality to consider the detection
+ unsigned int filter_track_length_th; ///< length of the track necessary to consider the detection
+ double time_span; ///< for keyframe voting: time span since last frame (sufficient condition if more than min_features_for_keyframe)
+
+ ParamsProcessorLandmarkExternal() = default;
+ ParamsProcessorLandmarkExternal(const YAML::Node& _n):
+ ParamsProcessorTracker(_n)
+ {
+ use_orientation = _n["use_orientation"].as<bool>();
+ filter_quality_th = _n["filter"]["quality_th"].as<double>();
+ filter_track_length_th = _n["filter"]["track_length_th"].as<unsigned int>();
+ match_dist_th = _n["match_dist_th"].as<double>();
+ time_span = _n["keyframe_vote"]["time_span"].as<double>();
+ new_features_for_keyframe = _n["keyframe_vote"]["new_features_for_keyframe"].as<unsigned int>();
+ }
+
+ std::string print() const override
+ {
+ return ParamsProcessorTracker::print() + "\n"
+ + "use_orientation" + toString(use_orientation) + "\n"
+ + "filter/quality_th" + toString(filter_quality_th) + "\n"
+ + "filter/track_length_th" + toString(filter_track_length_th) + "\n"
+ + "match_dist_th" + toString(match_dist_th) + "\n"
+ + "keyframe_vote/time_span" + toString(time_span) + "\n"
+ + "keyframe_vote/new_features_for_keyframe" + toString(new_features_for_keyframe) + "\n";
+ }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorLandmarkExternal);
+
+//Class
+class ProcessorLandmarkExternal : public ProcessorTracker
+{
+ public:
+ ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tracker_feature);
+ ~ProcessorLandmarkExternal() override = default;
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorLandmarkExternal, ParamsProcessorLandmarkExternal);
+
+ void configure(SensorBasePtr _sensor) override { };
+
+ protected:
+
+ ParamsProcessorLandmarkExternalPtr params_tfle_;
+ TrackMatrix track_matrix_;
+ std::set<SizeStd> lmks_ids_origin_;
+
+ /** Pre-process incoming Capture
+ *
+ * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
+ *
+ * Extract the detections and fill unmatched_detections_incoming_ (FeaturePtrList)
+ */
+ void preProcess() override;
+
+
+ /** \brief Process known Features
+ * \return The number of successful matches.
+ *
+ * This function tracks features from last to incoming and starts new tracks for new (not tracked) features in incoming
+ */
+ unsigned int processKnown() override;
+
+ /**\brief Process new Features
+ *
+ */
+ unsigned int processNew(const int& _max_features) override { return 0;};
+
+ /** \brief Vote for KeyFrame generation
+ *
+ * If a KeyFrame criterion is validated, this function returns true,
+ * meaning that it wants to create a KeyFrame at the \b last Capture.
+ *
+ * WARNING! This function only votes! It does not create KeyFrames!
+ */
+ bool voteForKeyFrame() const override;
+
+ /** \brief Emplaces a new factor for each known feature in Capture \b last
+ */
+ void establishFactors() override;
+
+ /** \brief Emplaces a new factor
+ * \param _feature feature pointer
+ * \param _landmark pointer to the landmark
+ */
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature, LandmarkBasePtr _landmark);
+
+ /** \brief Emplaces a landmark or modifies (if needed) a landmark
+ * \param _feature_ptr pointer to the Feature
+ */
+ LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr);
+
+ /** \brief Modifies (if needed) a landmark
+ * \param _landmark pointer to the landmark
+ * \param _feature pointer to the Feature.
+ */
+ void modifyLandmark(LandmarkBasePtr _landmark,
+ FeatureBasePtr _feature);
+
+ /** Post-process
+ *
+ * This is called by process() after finishing the processing algorithm.
+ *
+ * It does nothing for now
+ */
+ void postProcess() override {};
+
+ void advanceDerived() override;
+ void resetDerived() override;
+
+ double detectionDistance(FeatureBasePtr _ftr1,
+ FeatureBasePtr _ftr2,
+ const VectorComposite& _pose1,
+ const VectorComposite& _pose2,
+ const VectorComposite& _pose_sen) const;
+};
+
+inline ProcessorLandmarkExternal::ProcessorLandmarkExternal(ParamsProcessorLandmarkExternalPtr _params_tfle) :
+ ProcessorTracker("ProcessorLandmarkExternal", "PO", 0, _params_tfle),
+ params_tfle_(_params_tfle),
+ lmks_ids_origin_()
+{
+ //
+}
+
+} // namespace wolf
\ No newline at end of file
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 0d68a216211c1169e62c0daedf8056952524bb24..ac5afed6703c154924bcdb2f9a3cde443dc73b43 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -22,7 +22,7 @@
#pragma once
// Wolf
-#include <core/processor/motion_provider.h>
+#include "core/processor/motion_provider.h"
#include "core/capture/capture_motion.h"
#include "core/processor/processor_base.h"
#include "core/common/time_stamp.h"
@@ -262,6 +262,10 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
bool voteForKeyFrame() const override;
double updateDt();
+ /** \brief Make one step of motion integration
+ *
+ * Integrate motiondata in incoming_ptr_ and store all results in the MotionBuffer in last_ptr_
+ */
void integrateOneStep();
void reintegrateBuffer(CaptureMotionPtr _capture_ptr) const;
void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index f1b04fe357035477f51f51c26b39b5f9eb88b599..afdd11c67cb7914448f9255a6d373a08e5c3654b 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -45,7 +45,7 @@ struct ParamsProcessorOdom2d : public ParamsProcessorMotion
std::string print() const override
{
return ParamsProcessorMotion::print() + "\n"
- + "cov_det: " + toString(cov_det) + "\n";
+ + "keyframe_vote/cov_det: " + toString(cov_det) + "\n";
}
};
diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h
index c4563d754f01253be8bcf386d59a68a35341029f..b1f7a619e9a786b25fbbd5a0b2806ef83c8fa59a 100644
--- a/include/core/processor/processor_tracker_landmark.h
+++ b/include/core/processor/processor_tracker_landmark.h
@@ -80,7 +80,7 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
* - createLandmark() : creates a Landmark using a new Feature <=== IMPLEMENT
* - findLandmarks() : find the new Landmarks again in \b incoming <=== IMPLEMENT
* - establishFactors() : which calls the pure virtual:
- * - createFactor() : create a Feature-Landmark factor of the correct derived type <=== IMPLEMENT
+ * - emplaceFactor() : create a Feature-Landmark factor of the correct derived type <=== IMPLEMENT
*
* Should you need extra functionality for your derived types, you can overload these two methods,
*
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 06341d5a8d24fb790be63ddc5aeb2c59567b4559..7d3473678c65126015cab891c67382eca0eb1193 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -90,7 +90,10 @@ class HasStateBlocks
unsigned int getSize(const StateKeys& _keys="") const;
unsigned int getLocalSize(const StateKeys& _keys="") const;
- // Perturb state
+ /**
+ * \brief perturb all states states with random noise
+ * following an uniform distribution in [ -amplitude, amplitude ]
+ */
void perturb(double amplitude = 0.01);
protected:
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 6a65d6447c1a7591cd0158405cc3cfde86473c4c..38e06149cb9e28b0fdf778ff26c31d00f616f253 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -204,7 +204,9 @@ public:
virtual Eigen::VectorXd identity() const;
Eigen::VectorXd zero() const;
- /** \brief perturb state
+ /**
+ * \brief perturb all states states with random noise
+ * following an uniform distribution in [ -amplitude, amplitude ]
*/
void perturb(double amplitude = 0.1);
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index f6d851b488b1147514a3a7f76da68d1c325cae58..b7b8571e6ae1640b5cc1e69fcdec1b61ef3b0f92 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -253,7 +253,10 @@ class StateBlockComposite
// Plus operator
void plus(const VectorComposite& _dx);
- // Perturb state with random noise
+ /**
+ * \brief perturb all states states with random noise
+ * following an uniform distribution in [ -amplitude, amplitude ]
+ */
void perturb(double amplitude = 0.01);
// These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
diff --git a/include/core/utils/utils_gtest.h b/include/core/utils/utils_gtest.h
index 905f25c8b26a3fa1caa79cb6e72be4f6d07cfcc1..9c46dd20c96535308e5f851e218641498787a631 100644
--- a/include/core/utils/utils_gtest.h
+++ b/include/core/utils/utils_gtest.h
@@ -135,15 +135,15 @@ TEST(Test, Foo)
}, \
C_expect, C_actual);
-#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::Quaterniond lhs, const Eigen::Quaterniond rhs) { \
- return lhs.angularDistance(rhs) < precision; \
+#define EXPECT_QUATERNION_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
}, \
- C_expect, C_actual);
+ Quaterniond(C_expect).coeffs(), Quaterniond(C_actual).coeffs());
-#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::Quaterniond lhs, const Eigen::Quaterniond rhs) { \
- return lhs.angularDistance(rhs) < precision; \
+#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
+ return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
}, \
- C_expect, C_actual);
+ Quaterniond(C_expect).coeffs(), Quaterniond(C_actual).coeffs());
#define EXPECT_QUATERNION_VECTOR_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::VectorXd lhs, const Eigen::VectorXd rhs) { \
return Eigen::Quaterniond(Eigen::Vector4d(lhs)).angularDistance(Eigen::Quaterniond(Eigen::Vector4d(rhs))) < precision; \
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 4257c9f834eb0c0bf4ac9d339500a96ff224f4cc..6919fc4eb6bc3ef5017d38ef661b26f8848198f4 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -158,13 +158,15 @@ FactorBasePtrList CaptureBase::getFactorList()
void CaptureBase::getFactorList(FactorBaseConstPtrList& _fac_list) const
{
for (auto f_ptr : getFeatureList())
- f_ptr->getFactorList(_fac_list);
+ if (not f_ptr->isRemoving())
+ f_ptr->getFactorList(_fac_list);
}
void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
{
for (auto f_ptr : getFeatureList())
- f_ptr->getFactorList(_fac_list);
+ if (not f_ptr->isRemoving())
+ f_ptr->getFactorList(_fac_list);
}
FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
diff --git a/src/capture/capture_landmarks_external.cpp b/src/capture/capture_landmarks_external.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..154dcc83f488b39a91c69f0906a0f59e2fdd56d7
--- /dev/null
+++ b/src/capture/capture_landmarks_external.cpp
@@ -0,0 +1,53 @@
+//--------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/capture/capture_landmarks_external.h"
+
+namespace wolf{
+
+CaptureLandmarksExternal::CaptureLandmarksExternal(const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
+ const std::vector<int>& _ids,
+ const std::vector<Eigen::VectorXd>& _detections,
+ const std::vector<Eigen::MatrixXd>& _covs,
+ const std::vector<double>& _qualities) :
+ CaptureBase("CaptureLandmarksExternal", _ts, _sensor_ptr)
+{
+ if (_ids.size() != _detections.size() or
+ _ids.size() != _covs.size() or
+ _ids.size() != _qualities.size())
+ throw std::runtime_error("CaptureLandmarksExternal constructor: '_ids', '_detections', '_covs', '_qualities' should have the same size.");
+
+ for (auto i = 0; i < _detections.size(); i++)
+ addDetection(_ids.at(i), _detections.at(i), _covs.at(i), _qualities.at(i));
+}
+
+CaptureLandmarksExternal::~CaptureLandmarksExternal()
+{
+ //
+}
+
+void CaptureLandmarksExternal::addDetection(const int& _id, const Eigen::VectorXd& _detection, const Eigen::MatrixXd& _cov, const double& _quality)
+{
+ detections_.push_back(LandmarkDetection{_id, _detection, _cov, _quality});
+}
+
+} // namespace wolf
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 9ac1675b8b9ddc388f85b7fd6d8d7e34a274e65b..9610063417bd9a283b01d90068abf51e655e2f68 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -141,42 +141,44 @@ void FactorBase::remove(bool viral_remove_empty_parent)
CaptureBaseConstPtr FactorBase::getCapture() const
{
auto ftr = getFeature();
- if (ftr != nullptr) return ftr->getCapture();
+ if (ftr != nullptr and not ftr->isRemoving())
+ return ftr->getCapture();
else return nullptr;
}
CaptureBasePtr FactorBase::getCapture()
{
auto ftr = getFeature();
- if (ftr != nullptr) return ftr->getCapture();
+ if (ftr != nullptr and not ftr->isRemoving())
+ return ftr->getCapture();
else return nullptr;
}
FrameBaseConstPtr FactorBase::getFrame() const
{
auto cpt = getCapture();
- if(cpt != nullptr) return cpt->getFrame();
+ if(cpt != nullptr and not cpt->isRemoving()) return cpt->getFrame();
else return nullptr;
}
FrameBasePtr FactorBase::getFrame()
{
auto cpt = getCapture();
- if(cpt != nullptr) return cpt->getFrame();
+ if(cpt != nullptr and not cpt->isRemoving()) return cpt->getFrame();
else return nullptr;
}
SensorBaseConstPtr FactorBase::getSensor() const
{
auto cpt = getCapture();
- if(cpt != nullptr) return cpt->getSensor();
+ if(cpt != nullptr and not cpt->isRemoving()) return cpt->getSensor();
else return nullptr;
}
SensorBasePtr FactorBase::getSensor()
{
auto cpt = getCapture();
- if(cpt != nullptr) return cpt->getSensor();
+ if(cpt != nullptr and not cpt->isRemoving()) return cpt->getSensor();
else return nullptr;
}
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 9819bce8c6d484368d78581b58f3aefa3ac72ba9..293aa5ad97e2c82714f9d13cde47aa99db1be256 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -330,13 +330,15 @@ FactorBasePtrList FrameBase::getFactorList()
void FrameBase::getFactorList(FactorBaseConstPtrList& _fac_list) const
{
for (auto c_ptr : getCaptureList())
- c_ptr->getFactorList(_fac_list);
+ if (not c_ptr->isRemoving())
+ c_ptr->getFactorList(_fac_list);
}
void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
{
for (auto c_ptr : getCaptureList())
- c_ptr->getFactorList(_fac_list);
+ if (not c_ptr->isRemoving())
+ c_ptr->getFactorList(_fac_list);
}
bool FrameBase::hasCapture(const CaptureBaseConstPtr& _capture) const
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index c2a59554ae320987eeb8b71acd30107881b95b5b..616b9eaa4404cbd9be34806e20509d9ace328486 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -65,35 +65,73 @@ void MapBase::removeLandmark(LandmarkBasePtr _landmark_ptr)
landmark_list_.remove(_landmark_ptr);
}
-void MapBase::load(const std::string& _map_file_dot_yaml)
+LandmarkBaseConstPtr MapBase::getLandmark(const unsigned int& _id) const
{
- YAML::Node map = YAML::LoadFile(_map_file_dot_yaml);
+ auto lmk_it = std::find_if(landmark_list_.begin(),
+ landmark_list_.end(),
+ [&](LandmarkBaseConstPtr lmk)
+ {
+ return lmk->id() == _id;
+ }); // lambda function for the find_if
- unsigned int nlandmarks = map["nlandmarks"].as<unsigned int>();
+ if (lmk_it == landmark_list_.end())
+ return nullptr;
- assert(nlandmarks == map["landmarks"].size() && "Number of landmarks in map file does not match!");
+ return (*lmk_it);
+}
+
+LandmarkBasePtr MapBase::getLandmark(const unsigned int& _id)
+{
+ auto lmk_it = std::find_if(landmark_list_.begin(),
+ landmark_list_.end(),
+ [&](LandmarkBasePtr lmk)
+ {
+ return lmk->id() == _id;
+ }); // lambda function for the find_if
+
+ if (lmk_it == landmark_list_.end())
+ return nullptr;
+
+ return (*lmk_it);
+}
- for (unsigned int i = 0; i < nlandmarks; i++)
+void MapBase::load(std::string _map_file_dot_yaml)
+{
+ // change ~ with HOME using environment variable
+ if (_map_file_dot_yaml.at(0) == '~')
+ _map_file_dot_yaml = std::string(std::getenv("HOME")) + _map_file_dot_yaml.substr(1);
+
+ try
{
- YAML::Node lmk_node = map["landmarks"][i];
- LandmarkBasePtr lmk_ptr = FactoryLandmark::create(lmk_node["type"].as<std::string>(), lmk_node);
- lmk_ptr->link(shared_from_this());
+ YAML::Node map = YAML::LoadFile(_map_file_dot_yaml);
+ for (unsigned int i = 0; i < map["landmarks"].size(); i++)
+ {
+ YAML::Node lmk_node = map["landmarks"][i];
+ LandmarkBasePtr lmk_ptr = FactoryLandmark::create(lmk_node["type"].as<std::string>(), lmk_node);
+ lmk_ptr->link(shared_from_this());
+ }
}
+ catch(const std::exception& e)
+ {
+ std::cerr << "MapBase::load: " << e.what() << '\n';
+ }
}
-void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_name)
+void MapBase::save(std::string _map_file_yaml, const std::string& _map_name) const
{
+ // change ~ with HOME using environment variable
+ if (_map_file_yaml.at(0) == '~')
+ _map_file_yaml = std::string(std::getenv("HOME")) + _map_file_yaml.substr(1);
+
YAML::Emitter emitter;
emitter << YAML::BeginMap;
- emitter << "map name" << _map_name;
- emitter << "date-time" << dateTimeNow(); // Get date and time for archiving purposes
-
- emitter << "nlandmarks" << getLandmarkList().size();
+ emitter << "map_name" << _map_name;
+ emitter << "date_time" << dateTimeNow(); // Get date and time for archiving purposes
emitter << "landmarks" << YAML::BeginSeq;
- for (LandmarkBasePtr lmk_ptr : getLandmarkList())
+ for (LandmarkBaseConstPtr lmk_ptr : getLandmarkList())
{
emitter << YAML::Flow << lmk_ptr->toYaml();
}
@@ -104,7 +142,7 @@ void MapBase::save(const std::string& _map_file_yaml, const std::string& _map_na
fout.close();
}
-std::string MapBase::dateTimeNow()
+std::string MapBase::dateTimeNow() const
{
// Get date and time for archiving purposes
std::time_t rawtime;
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 7feb7c5c32483e1740bef2e3a078bc8fd1a09155..1240e74e2b0d1e49d82fe2ac071858dfc75b00e8 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -642,7 +642,7 @@ VectorComposite Problem::getOdometry(const StateKeys& _structure) const
}
}
- // check for empty blocks and fill them with the the prior, or with zeros in the worst case
+ // check for empty blocks and fill them with the prior, or with zeros in the worst case
VectorComposite state_last;
diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp
index 6afd1c5c76c9c838e3072711b69ca199da0a48b9..9c260726d7dcb545757dc9e19b66cef3f43584ec 100644
--- a/src/processor/motion_buffer.cpp
+++ b/src/processor/motion_buffer.cpp
@@ -120,8 +120,8 @@ void MotionBuffer::getMotion(const TimeStamp& _ts, Motion& _motion) const
void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before_ts)
{
- assert((this->front().ts_ <= _ts) && "Error: Query time stamp is smaller than the buffer's first tick");
- assert((this->back().ts_ >= _ts) && "Error: Query time stamp is greater than the buffer's last tick");
+ assert((this->front().ts_ <= _ts) && "Error: Query time stamp is smaller or equal than the buffer's first tick");
+ assert((this->back().ts_ >= _ts) && "Error: Query time stamp is greater or equal than the buffer's last tick");
auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m)
{
diff --git a/src/processor/motion_provider.cpp b/src/processor/motion_provider.cpp
index bd232f8d8b23af080711e953998e9042e54d7f20..82b4616e93c48ea088550841aa84124a50d9c218 100644
--- a/src/processor/motion_provider.cpp
+++ b/src/processor/motion_provider.cpp
@@ -36,8 +36,17 @@ void MotionProvider::addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion
void MotionProvider::setOdometry(const VectorComposite& _odom)
{
+ std::lock_guard<std::mutex> lock(mut_odometry_);
assert(_odom.has(getStateKeys()) && "MotionProvider::setOdometry(): any key missing in _odom.");
for (auto key : getStateKeys())
odometry_[key] = _odom.at(key); //overwrite/insert only keys of the state_types_
}
+
+wolf::VectorComposite MotionProvider::getOdometry ( ) const
+{
+ std::lock_guard<std::mutex> lock(mut_odometry_);
+
+ return odometry_;
+}
+
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index e3dfc26a3c9856d76a96ea26efc7a04e996e7493..521b2da665cd2905b0a86d8e864e9c5c97751b06 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -218,23 +218,23 @@ void ProcessorBase::printProfiling(std::ostream& _stream) const
}
-bool ProcessorBase::checkTimeTolerance (const TimeStamp& _ts1, const TimeStamp& _ts2)
+bool ProcessorBase::checkTimeTolerance (const TimeStamp& _ts1, const TimeStamp& _ts2) const
{
auto dt = std::fabs(_ts1 - _ts2);
return dt <= params_->time_tolerance;
}
-bool ProcessorBase::checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp& _ts)
+bool ProcessorBase::checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp& _ts) const
{
return checkTimeTolerance(_cap->getTimeStamp(), _ts);
}
-bool ProcessorBase::checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp& _ts)
+bool ProcessorBase::checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp& _ts) const
{
return checkTimeTolerance(_frm->getTimeStamp(), _ts);
}
-bool ProcessorBase::checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap)
+bool ProcessorBase::checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap) const
{
return checkTimeTolerance(_frm->getTimeStamp(), _cap->getTimeStamp());
}
diff --git a/src/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..30b4ff8400645f18e2f6a7d970c8582a73e30634
--- /dev/null
+++ b/src/processor/processor_landmark_external.cpp
@@ -0,0 +1,497 @@
+//--------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/processor/processor_landmark_external.h"
+#include "core/capture/capture_landmarks_external.h"
+#include "core/feature/feature_base.h"
+#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/landmark/landmark_base.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
+#include "core/math/SE2.h"
+#include "core/math/SE3.h"
+
+using namespace Eigen;
+
+namespace wolf
+{
+
+void ProcessorLandmarkExternal::preProcess()
+{
+ assert(new_features_incoming_.empty());
+
+ auto dim = getProblem()->getDim();
+ auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(incoming_ptr_);
+ if (not cap_landmarks)
+ throw std::runtime_error("ProcessorLandmarkExternal::preProcess incoming_ptr_ should be of type 'CaptureLandmarksExternal'");
+
+ // Extract features from capture detections
+ auto landmark_detections = cap_landmarks->getDetections();
+ std::set<int> ids;
+ for (auto detection : landmark_detections)
+ {
+ WOLF_WARN_COND(ids.count(detection.id), "ProcessorLandmarkExternal::preProcess: detection with repeated id, discarding...");
+
+ // Filter by quality
+ if (detection.quality < params_tfle_->filter_quality_th or ids.count(detection.id))
+ continue;
+
+ // measure and covariance
+ VectorXd meas;
+ MatrixXd cov;
+ if (not params_tfle_->use_orientation)
+ {
+ assert(detection.measure.size() >= dim);
+ assert(detection.covariance.rows() >= dim and detection.covariance.rows() == detection.covariance.cols());
+
+ meas = detection.measure.head(dim);
+ cov = detection.covariance.topLeftCorner(dim,dim);
+ }
+ else
+ {
+ assert(detection.measure.size() == (dim == 2 ? 3 : 7));
+ assert(detection.covariance.rows() == (dim == 2 ? 3 : 6) and detection.covariance.rows() == detection.covariance.cols());
+
+ meas = detection.measure;
+ cov = detection.covariance;
+ }
+
+ // emplace feature
+ FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(cap_landmarks,
+ "FeatureLandmarkExternal",
+ meas,
+ cov);
+ ftr->setLandmarkId(detection.id);
+
+ if (detection.id != -1 and detection.id != 0)
+ ids.insert(detection.id);
+
+ //unmatched_detections_incoming_.push_back(ftr);
+
+ // add new feature
+ new_features_incoming_.push_back(ftr);
+ }
+ WOLF_DEBUG("ProcessorLandmarkExternal::preprocess: found ", new_features_incoming_.size(), " new features");
+}
+
+unsigned int ProcessorLandmarkExternal::processKnown()
+{
+ // clear list of known features
+ known_features_incoming_.clear();
+
+ // Track features from last_ptr_ to incoming_ptr_
+ if (last_ptr_)
+ {
+ WOLF_DEBUG("Searching ", known_features_last_.size(), " tracked features...");
+ auto pose_sen = getSensor()->getState("PO");
+ auto pose_in = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
+ auto pose_out = getProblem()->getState(incoming_ptr_->getTimeStamp(), "PO");
+
+ for (auto feat_last : known_features_last_)
+ {
+ auto feat_candidate_it = new_features_incoming_.begin();
+ while (feat_candidate_it != new_features_incoming_.end())
+ {
+ if ((*feat_candidate_it)->landmarkId() == feat_last->landmarkId() and
+ detectionDistance(feat_last, (*feat_candidate_it), pose_in, pose_out, pose_sen) < params_tfle_->match_dist_th)
+ {
+ // grow track
+ track_matrix_.add(feat_last, *feat_candidate_it);
+
+ // feature is known
+ known_features_incoming_.push_back((*feat_candidate_it));
+
+ // remove from unmatched
+ feat_candidate_it = new_features_incoming_.erase(feat_candidate_it);
+ break;
+ }
+ else
+ feat_candidate_it++;
+ }
+ }
+ WOLF_DEBUG("Tracked ", known_features_incoming_.size(), " features.");
+ }
+
+ // Add new features (not tracked) as known features
+ WOLF_DEBUG_COND(not new_features_incoming_.empty(), "Adding new features ", new_features_incoming_.size());
+ while (not new_features_incoming_.empty())
+ {
+ auto ftr = new_features_incoming_.front();
+
+ // new track
+ track_matrix_.newTrack(ftr);
+
+ // feature is now known
+ known_features_incoming_.push_back(ftr);
+
+ // remove from unmatched
+ new_features_incoming_.pop_front();
+ }
+
+ // check all features have been emplaced
+ assert(std::all_of(known_features_incoming_.begin(), known_features_incoming_.end(), [](FeatureBasePtr f){return f->getCapture() != nullptr;}) &&
+ "any not linked feature returned by trackFeatures()");
+
+ return known_features_incoming_.size();
+}
+
+double ProcessorLandmarkExternal::detectionDistance(FeatureBasePtr _ftr1,
+ FeatureBasePtr _ftr2,
+ const VectorComposite& _pose1,
+ const VectorComposite& _pose2,
+ const VectorComposite& _pose_sen) const
+{
+ // Any not available info of poses, assume identity
+ if (not _pose1.has("PO") or not _pose2.has("PO") or not _pose_sen.has("PO"))
+ {
+ if (getProblem()->getDim() == 2)
+ return (_ftr1->getMeasurement().head<2>() - _ftr2->getMeasurement().head<2>()).norm();
+ else
+ return (_ftr1->getMeasurement().head<3>() - _ftr2->getMeasurement().head<3>()).norm();
+ }
+ else
+ {
+ if (getProblem()->getDim() == 2)
+ {
+ auto pose_s1 = SE2::compose(_pose1, _pose_sen);
+ auto pose_s2 = SE2::compose(_pose2, _pose_sen);
+ auto p1 = pose_s1.at('P') + Rotation2Dd(pose_s1.at('O')(0)) * _ftr1->getMeasurement().head<2>();
+ auto p2 = pose_s2.at('P') + Rotation2Dd(pose_s2.at('O')(0)) * _ftr2->getMeasurement().head<2>();
+ return (p1-p2).norm();
+ }
+ else
+ {
+ auto pose_s1 = SE3::compose(_pose1, _pose_sen);
+ auto pose_s2 = SE3::compose(_pose2, _pose_sen);
+ auto p1 = pose_s1.at('P') + Quaterniond(Vector4d(pose_s1.at('O'))) * _ftr1->getMeasurement().head<3>();
+ auto p2 = pose_s2.at('P') + Quaterniond(Vector4d(pose_s2.at('O'))) * _ftr2->getMeasurement().head<3>();
+ return (p1-p2).norm();
+ }
+ }
+ return 0; // non reachable
+}
+
+bool ProcessorLandmarkExternal::voteForKeyFrame() const
+{
+ auto track_ids_last = track_matrix_.trackIds(last_ptr_);
+
+ WOLF_DEBUG("Active feature tracks: " , track_ids_last.size() );
+
+ // no tracks longer than filter_track_length_th
+ auto n_tracks = 0;
+ auto n_new_tracks = 0;
+ for (auto track_id : track_ids_last)
+ {
+ if (track_matrix_.trackSize(track_id) >= params_tfle_->filter_track_length_th)
+ {
+ n_tracks++;
+ if (not lmks_ids_origin_.count(track_matrix_.feature(track_id, last_ptr_)->landmarkId()))
+ n_new_tracks++;
+ }
+ }
+
+ // Necessary condition: active valid tracks
+ bool vote_min_features = n_tracks >= params_tfle_->min_features_for_keyframe;
+ WOLF_DEBUG("vote_min_features: ", vote_min_features,
+ " - Active feature tracks longer than ", params_tfle_->filter_track_length_th, ": ", n_tracks,
+ " (should be equal or bigger than ", params_tfle_->min_features_for_keyframe, ")");
+
+ bool vote_new_features(true), vote_time_span(true);
+ if (vote_min_features and origin_ptr_)
+ {
+ // Sufficient condition: new valid tracks
+ vote_new_features = n_new_tracks >= params_tfle_->new_features_for_keyframe;
+ WOLF_DEBUG("vote_new_features: " , vote_new_features,
+ " - n_new_tracks = ", n_new_tracks,
+ " (should be equal or bigger than ", params_tfle_->new_features_for_keyframe, ")");
+
+ // Sufficient condition: time span
+ vote_time_span = last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_tfle_->time_span;
+ WOLF_DEBUG("vote_time_span: " , vote_time_span,
+ " - time_span = ", last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp(),
+ " (should be bigger than ", params_tfle_->time_span, ")");
+ }
+
+ bool vote = vote_min_features and (vote_new_features or vote_time_span);
+
+ WOLF_DEBUG( (vote ? "Vote ": "Do NOT vote ") , "for KF" );
+
+ return vote;
+}
+
+void ProcessorLandmarkExternal::establishFactors()
+{
+ if (origin_ptr_ == last_ptr_)
+ return;
+
+ // reset n_tracks_origin_
+ lmks_ids_origin_.clear();
+
+ // will emplace a factor (and landmark if needed) for each known feature in last with long tracks (params_tfle_->filter_track_length_th)
+ FactorBasePtrList fac_list;
+ auto track_ids_last = track_matrix_.trackIds(last_ptr_);
+
+ for (auto track_id : track_ids_last)
+ {
+ // check track length
+ if (track_matrix_.trackSize(track_id) < params_tfle_->filter_track_length_th)
+ continue;
+
+ auto feature = track_matrix_.feature(track_id, last_ptr_);
+
+ // get landmark
+ LandmarkBasePtr lmk = getProblem()->getMap()->getLandmark(feature->landmarkId());
+
+ // emplace landmark
+ if (not lmk)
+ lmk = emplaceLandmark(feature);
+
+ // modify landmark
+ modifyLandmark(lmk, feature);
+
+ // emplace factor
+ auto fac = emplaceFactor(feature, lmk);
+
+ lmks_ids_origin_.insert(lmk->id());
+
+ if (fac)
+ fac_list.push_back(fac);
+ }
+
+ WOLF_DEBUG("ProcessorLandmarkExternal::establishFactors: emplaced ", fac_list.size(), " factors!");
+}
+
+FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature, LandmarkBasePtr _landmark)
+{
+ assert(_feature);
+ assert(_landmark);
+
+ // 2D - Relative Position
+ if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
+ {
+ return FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(_feature,
+ _feature,
+ _landmark,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ // 2D - Relative Pose
+ else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and params_tfle_->use_orientation)
+ {
+ return FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(_feature,
+ _feature,
+ _landmark,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ // 3D - Relative Position
+ else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
+ {
+ return FactorBase::emplace<FactorRelativePosition3dWithExtrinsics>(_feature,
+ _feature,
+ _landmark,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ // 3D - Relative Pose
+ else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
+ {
+ return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(_feature,
+ _feature,
+ _landmark,
+ shared_from_this(),
+ params_tfle_->apply_loss_function);
+ }
+ else
+ throw std::runtime_error("ProcessorLandmarkExternal::emplaceFactor unknown case");
+
+ // not reachable
+ return nullptr;
+
+}
+
+LandmarkBasePtr ProcessorLandmarkExternal::emplaceLandmark(FeatureBasePtr _feature)
+{
+ assert(_feature);
+ assert(_feature->getCapture());
+ assert(_feature->getCapture()->getFrame());
+ assert(_feature->getCapture()->getSensorP());
+ assert(_feature->getCapture()->getSensorO());
+ assert(getProblem());
+ assert(getProblem()->getMap());
+
+ if (getProblem()->getMap()->getLandmark(_feature->landmarkId()) != nullptr)
+ throw std::runtime_error("ProcessorLandmarkExternal::emplaceLandmark: attempting to create an existing landmark");
+
+ LandmarkBasePtr lmk;
+
+ // 2D - Relative Position
+ if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
+ {
+ Vector2d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+ Vector2d sen_p = _feature->getCapture()->getSensorP()->getState();
+ double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
+
+ Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement());
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePosition2d",
+ std::make_shared<StatePoint2d>(lmk_p),
+ nullptr);
+ lmk->setId(_feature->landmarkId());
+ }
+ // 2D - Relative Pose
+ else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and params_tfle_->use_orientation)
+ {
+ Vector2d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+ Vector2d sen_p = _feature->getCapture()->getSensorP()->getState();
+ double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
+
+ Vector2d lmk_p = frm_p + Rotation2Dd(frm_o) * (sen_p + Rotation2Dd(sen_o) * _feature->getMeasurement().head<2>());
+ double lmk_o = frm_o + sen_o + _feature->getMeasurement()(2);
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePose2d",
+ std::make_shared<StatePoint2d>(lmk_p),
+ std::make_shared<StateAngle>(lmk_o));
+ lmk->setId(_feature->landmarkId());
+ }
+ // 3D - Relative Position
+ else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
+ {
+ Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+ Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
+ Quaterniond frm_o = Quaterniond(Vector4d(_feature->getCapture()->getFrame()->getO()->getState()));
+ Quaterniond sen_o = Quaterniond(Vector4d(_feature->getCapture()->getSensorO()->getState()));
+
+ Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement());
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePosition3d",
+ std::make_shared<StatePoint3d>(lmk_p),
+ nullptr);
+ lmk->setId(_feature->landmarkId());
+ }
+ // 3D - Relative Pose
+ else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
+ {
+ Vector3d frm_p = _feature->getCapture()->getFrame()->getP()->getState();
+ Vector3d sen_p = _feature->getCapture()->getSensorP()->getState();
+ Quaterniond frm_o = Quaterniond(Vector4d(_feature->getCapture()->getFrame()->getO()->getState()));
+ Quaterniond sen_o = Quaterniond(Vector4d(_feature->getCapture()->getSensorO()->getState()));
+
+ Vector3d lmk_p = frm_p + frm_o * (sen_p + sen_o * _feature->getMeasurement().head<3>());
+ Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature->getMeasurement().tail<4>());
+
+ lmk = LandmarkBase::emplace<LandmarkBase>(getProblem()->getMap(),
+ "LandmarkRelativePose3d",
+ std::make_shared<StatePoint3d>(lmk_p),
+ std::make_shared<StateQuaternion>(lmk_o));
+ lmk->setId(_feature->landmarkId());
+ }
+ else
+ throw std::runtime_error("ProcessorLandmarkExternal::emplaceLandmark unknown case");
+
+ return lmk;
+}
+
+void ProcessorLandmarkExternal::modifyLandmark(LandmarkBasePtr _landmark,
+ FeatureBasePtr _feature)
+{
+ assert(_feature);
+ assert(_feature->getCapture());
+ assert(_feature->getCapture()->getFrame());
+ assert(_feature->getCapture()->getSensorP());
+ assert(_feature->getCapture()->getSensorO());
+ assert(getProblem());
+
+ if (not _landmark)
+ throw std::runtime_error("ProcessorLandmarkExternal::modifyLandmark: null landmark");
+
+ // 2D - Relative Position
+ if (getProblem()->getDim() == 2 and (_feature->getMeasurement().size() == 2 or not params_tfle_->use_orientation))
+ {
+ // nothing to do (we assume P already in landmark)
+ return;
+ }
+ // 2D - Relative Pose
+ else if (getProblem()->getDim() == 2 and _feature->getMeasurement().size() == 3 and params_tfle_->use_orientation)
+ {
+ // no orientation, add it
+ if (not _landmark->getO())
+ {
+ double frm_o = _feature->getCapture()->getFrame()->getO()->getState()(0);
+ double sen_o = _feature->getCapture()->getSensorO()->getState()(0);
+
+ double lmk_o = frm_o + sen_o + _feature->getMeasurement()(2);
+
+ _landmark->addStateBlock('O', std::make_shared<StateAngle>(lmk_o), getProblem());
+ }
+ }
+ // 3D - Relative Position
+ else if (getProblem()->getDim() == 3 and (_feature->getMeasurement().size() == 3 or not params_tfle_->use_orientation))
+ {
+ // nothing to do (we assume P already in landmark)
+ return;
+ }
+ // 3D - Relative Pose
+ else if (getProblem()->getDim() == 3 and _feature->getMeasurement().size() == 7 and params_tfle_->use_orientation)
+ {
+ // no orientation, add it
+ if (not _landmark->getO())
+ {
+ Quaterniond frm_o = Quaterniond(Vector4d(_feature->getCapture()->getFrame()->getO()->getState()));
+ Quaterniond sen_o = Quaterniond(Vector4d(_feature->getCapture()->getSensorO()->getState()));
+
+ Quaterniond lmk_o = frm_o * sen_o * Quaterniond(_feature->getMeasurement().tail<4>());
+
+ _landmark->addStateBlock('O', std::make_shared<StateQuaternion>(lmk_o), getProblem());
+ }
+ }
+ else
+ throw std::runtime_error("ProcessorLandmarkExternal::modifyLandmark unknown case");
+}
+
+void ProcessorLandmarkExternal::advanceDerived()
+{
+ // Reset here the list of correspondences.
+ known_features_last_ = std::move(known_features_incoming_);
+}
+void ProcessorLandmarkExternal::resetDerived()
+{
+ // Reset here the list of correspondences.
+ known_features_last_ = std::move(known_features_incoming_);
+}
+
+} // namespace wolf
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorLandmarkExternal)
+} // namespace wolf
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 8549943aa71439543fdb74e0a66922342f8b80d0..e7efd16b3f23ce44c83d71d900a981f5489e1223 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -76,13 +76,13 @@ ProcessorMotion::~ProcessorMotion()
// std::cout << "destructed -p-Mot" << id() << std::endl;
}
-
void ProcessorMotion::mergeCaptures(CaptureMotionPtr cap_prev,
CaptureMotionPtr cap_target)
{
assert(cap_prev != nullptr);
assert(cap_target != nullptr);
- assert(cap_prev == cap_target->getOriginCapture() && "merging not consecutive capture motions");
+ assert(cap_prev == cap_target->getOriginCapture() && "ProcessorMotion::mergeCaptures: merging not consecutive capture motions");
+ assert((cap_prev->getBuffer().front().delta_integr_ - deltaZero()).isZero(Constants::EPS_SMALL) && "ProcessorMotion::mergeCaptures: cap_prev buffer's first motion is not zero!");
// add prev buffer (discarding the first zero motion)
cap_target->getBuffer().pop_front();
@@ -136,6 +136,27 @@ void ProcessorMotion::splitBuffer(const CaptureMotionPtr& _capture_source,
* origin target source
*
*/
+ // CHECKS
+ if (checkTimeTolerance(_capture_source, _ts_split))
+ {
+ WOLF_ERROR("ProcessorMotion::splitBuffer: _capture_source.ts == ts_split within tolerance, shouldn't split!");
+ }
+ if (_capture_source->getOriginCapture() and checkTimeTolerance(_capture_source->getOriginCapture(), _ts_split))
+ {
+ WOLF_ERROR("ProcessorMotion::splitBuffer: _capture_source->getOriginCapture().ts == ts_split within tolerance, shouldn't split!");
+ }
+ if (_capture_source->getBuffer().front().ts_ >= _ts_split)
+ {
+ WOLF_ERROR("ProcessorMotion::splitBuffer: Query time stamp is smaller or equal than the buffer's first tick");
+ getProblem()->print();
+ throw std::runtime_error("");
+ }
+ if (_capture_source->getBuffer().back().ts_ <= _ts_split)
+ {
+ WOLF_ERROR("ProcessorMotion::splitBuffer: Query time stamp is greater or equal than the buffer's last tick");
+ getProblem()->print();
+ throw std::runtime_error("");
+ }
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
@@ -186,7 +207,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
if (keyframe_from_callback)
buffer_frame_.removeUpTo( keyframe_from_callback->getTimeStamp() );
- switch(processing_step_)
+ switch(processing_step_) // Things to do before integrating motion data
{
case FIRST_TIME_WITHOUT_KF :
{
@@ -221,14 +242,14 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
}
- // integrate data
+ // integrate motion data
// Done at this place because setPrior() needs
integrateOneStep();
// perform bootstrap steps (usually only IMU requires this)
if (bootstrapping_) bootstrap();
- switch (processing_step_)
+ switch (processing_step_) // Things to do after integrating motion data
{
case RUNNING_WITH_KF_BEFORE_ORIGIN :
{
@@ -264,11 +285,11 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
TimeStamp timestamp_from_callback = keyframe_from_callback->getTimeStamp();
// find the capture whose buffer is affected by the new keyframe
- auto capture_existing = findCaptureContainingTimeStamp(timestamp_from_callback); // k
+ auto capture_existing = findCaptureContainingTimeStamp(timestamp_from_callback);
- if (!capture_existing)
+ if (not capture_existing)
{
- WOLF_WARN("A KF before first motion capture (TS = ", timestamp_from_callback, "). ProcessorMotion cannot do anything.");
+ WOLF_WARN(getName(), ": Cannot join KF. The received KF (TS = ", timestamp_from_callback, ") is older than the first motion capture.");
break;
}
@@ -285,6 +306,11 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// Find the capture acting as the buffer's origin
auto capture_origin = capture_existing->getOriginCapture();
+ if (not capture_origin)
+ {
+ WOLF_WARN(getName(), ": (should be unreachable) Cannot join KF. Capture containing (TS = ", timestamp_from_callback, ") does not have origin capture.");
+ break;
+ }
// Get calibration params for preintegration from origin, since it has chances to have optimized values
VectorXd calib_origin = getCalibration(capture_origin);
@@ -430,7 +456,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
* x : any capture
* o : origin capture
* l : last capture -> we'll make a KF here
- * i : incoming capture
+ * i : incoming capture -> data has already been integrated into last capture
* n : new capture ->
* --- : buffer history
*
@@ -552,8 +578,6 @@ VectorComposite ProcessorMotion::getState(const StateKeys& _keys) const
auto calib_preint = last_ptr_->getCalibrationPreint();
VectorComposite state;
- //WOLF_INFO("processorMotion last timestamp: ", last_ptr_->getTimeStamp());
- //WOLF_INFO("processorMotion origin timestamp: ", origin_ptr_->getTimeStamp());
if ( hasCalibration())
{
// Get current calibration -- from origin capture
@@ -751,8 +775,14 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
void ProcessorMotion::integrateOneStep()
{
+ /* Integrate motiondata in incoming_ptr_ and store all results in the MotionBuffer in last_ptr_
+ */
+
// Set dt
dt_ = updateDt();
+ WOLF_WARN_COND(dt_ < 0, "ProcessorMotion::integrateOneStep: dt < 0, skipping integration");
+ if (dt_ < 0)
+ return;
assert(dt_ >= 0 && "Time interval _dt is negative!");
// get vector of parameters to calibrate
@@ -801,7 +831,9 @@ void ProcessorMotion::integrateOneStep()
jacobian_calib_);
// integrate odometry
- statePlusDelta(odometry_, delta_, dt_, odometry_);
+ VectorComposite odom;
+ statePlusDelta(getOdometry(), delta_, dt_, odom);
+ setOdometry(odom);
}
void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) const
@@ -814,9 +846,9 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr) const
jacobian_calib_ .setZero();
// check that first motion in buffer is zero!
- assert(_capture_ptr->getBuffer().front().delta_integr_ == delta_integrated_ && "Buffer's first motion is not zero!");
- assert(_capture_ptr->getBuffer().front().delta_integr_cov_ == delta_integrated_cov_ && "Buffer's first motion is not zero!");
- assert(_capture_ptr->getBuffer().front().jacobian_calib_ == jacobian_calib_ && "Buffer's first motion is not zero!");
+ assert((_capture_ptr->getBuffer().front().delta_integr_ - delta_integrated_).isZero(Constants::EPS_SMALL) && "Buffer's first motion is not zero!");
+ assert(_capture_ptr->getBuffer().front().delta_integr_cov_.isZero(Constants::EPS_SMALL) && "Buffer's first motion covariance is not zero!");
+ assert(_capture_ptr->getBuffer().front().jacobian_calib_.isZero(Constants::EPS_SMALL) && "Buffer's first motion jacobian is not zero!");
// Iterate all the buffer
auto motion_it = _capture_ptr->getBuffer().begin();
@@ -918,9 +950,7 @@ CaptureMotionConstPtr ProcessorMotion::findCaptureContainingTimeStamp(const Time
CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp& _ts)
{
assert(_ts.ok());
- // assert(last_ptr_ != nullptr);
- //Need to uncomment this line so that wolf_ros_apriltag doesn't crash
if(last_ptr_ == nullptr) return nullptr;
// First check if last_ptr is the one we are looking for
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 61917d40f9f126a9f86626e7c987f7d031cbf407..3d20b4bddd920895e676c76b95189af55be04280 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -155,8 +155,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
incoming_ptr_->link(keyframe);
// Process info
- // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
- if (not getProblem()->getMap()->getLandmarkList().empty())
+ // If we have known landmarks or features. Process them.
+ if (not getProblem()->getMap()->getLandmarkList().empty() or not known_features_last_.empty())
processKnown();
// Both Trackers: We have a last_ Capture with not enough features, so populate it.
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 3638807c5cfc769988aa54eac16e7f7048c4d668..c016966cf21aec738b8e8e29fe6bce81801d1333 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -200,11 +200,11 @@ void ProcessorTrackerFeature::establishFactors()
auto fac_ptr = emplaceFactor(feature_in_last, feature_in_origin);
- assert(fac_ptr->getFeature() != nullptr && "not emplaced factor returned by emplaceFactor()");
+ assert((fac_ptr == nullptr or fac_ptr->getFeature() != nullptr) && "not emplaced factor returned by emplaceFactor()");
- WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
- " origin: " , feature_in_origin->id() ,
- " from last: " , feature_in_last->id() );
+ WOLF_DEBUG_COND(fac_ptr, "New factor: track: " , feature_in_last->trackId(),
+ " origin: " , feature_in_origin->id() ,
+ " from last: " , feature_in_last->id() );
}
}
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 24de3de294a6e65ea409d345a31a23393904ae50..244d6a505d66143823e6a67456c33972ff86a588 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -54,13 +54,15 @@ void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
void TrajectoryBase::getFactorList(FactorBaseConstPtrList & _fac_list) const
{
for(auto fr_pair: frame_map_)
- fr_pair.second->getFactorList(_fac_list);
+ if (not fr_pair.second->isRemoving())
+ fr_pair.second->getFactorList(_fac_list);
}
void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list)
{
for(auto fr_pair: frame_map_)
- fr_pair.second->getFactorList(_fac_list);
+ if (not fr_pair.second->isRemoving())
+ fr_pair.second->getFactorList(_fac_list);
}
TimeStamp TrajectoryBase::closestTimeStampToTimeStamp(const TimeStamp& _ts) const
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index ddbdd9a5d31f42c4b3b33dc5ef59cbd709da88c5..aadf6b087155ce3ac1eeb25a7a45ce66ec969ea3 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -168,6 +168,18 @@ wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
# FactorRelativePose3dWithExtrinsics class test
wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
+# FactorRelativePosition2d class test
+wolf_add_gtest(gtest_factor_relative_position_2d gtest_factor_relative_position_2d.cpp)
+
+# 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)
@@ -186,6 +198,9 @@ wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model
# ProcessorDiffDrive class test
wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
+# ProcessorLandmarkExternal class test
+wolf_add_gtest(gtest_processor_landmark_external gtest_processor_landmark_external.cpp)
+
# ProcessorLoopClosure class test
wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
diff --git a/test/dummy/factor_dummy_zero_12.h b/test/dummy/factor_dummy_zero_15.h
similarity index 79%
rename from test/dummy/factor_dummy_zero_12.h
rename to test/dummy/factor_dummy_zero_15.h
index 908c34b4f4af6f8cc7c943f24f1808b7e27df63f..205eeac84ffc5c7d0710c0b1d1a3624effa72374 100644
--- a/test/dummy/factor_dummy_zero_12.h
+++ b/test/dummy/factor_dummy_zero_15.h
@@ -24,18 +24,16 @@
//Wolf includes
#include "core/factor/factor_autodiff.h"
-//#include "ceres/jet.h"
-
namespace wolf {
//class
template <unsigned int ID>
-class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>
+class FactorDummyZero15 : public FactorAutodiff<FactorDummyZero15<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15>
{
- using Base = FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12>;
+ using Base = FactorAutodiff<FactorDummyZero15<ID>, ID, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15>;
static const unsigned int id = ID;
public:
- FactorDummyZero12(const FeatureBasePtr& _ftr_ptr,
+ FactorDummyZero15(const FeatureBasePtr& _ftr_ptr,
const StateBlockPtr& _sb1_ptr,
const StateBlockPtr& _sb2_ptr,
const StateBlockPtr& _sb3_ptr,
@@ -47,7 +45,10 @@ class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2,
const StateBlockPtr& _sb9_ptr,
const StateBlockPtr& _sb10_ptr,
const StateBlockPtr& _sb11_ptr,
- const StateBlockPtr& _sb12_ptr) :
+ const StateBlockPtr& _sb12_ptr,
+ const StateBlockPtr& _sb13_ptr,
+ const StateBlockPtr& _sb14_ptr,
+ const StateBlockPtr& _sb15_ptr) :
Base("FactorDummy12",
TOP_OTHER,
_ftr_ptr,
@@ -66,12 +67,15 @@ class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2,
_sb9_ptr,
_sb10_ptr,
_sb11_ptr,
- _sb12_ptr)
+ _sb12_ptr,
+ _sb13_ptr,
+ _sb14_ptr,
+ _sb15_ptr)
{
- assert(id > 0 && id <= 12);
+ assert(id > 0 && id <= 15);
}
- ~FactorDummyZero12() override = default;
+ ~FactorDummyZero15() override = default;
template<typename T>
bool operator ()(const T* const _st1,
@@ -86,6 +90,9 @@ class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2,
const T* const _st10,
const T* const _st11,
const T* const _st12,
+ const T* const _st13,
+ const T* const _st14,
+ const T* const _st15,
T* _residuals) const
{
Eigen::Map<Eigen::Matrix<T,ID,1>> residuals(_residuals);
@@ -163,6 +170,24 @@ class FactorDummyZero12 : public FactorAutodiff<FactorDummyZero12<ID>, ID, 1, 2,
residuals = st12;
break;
}
+ case 13:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st13(_st13);
+ residuals = st13;
+ break;
+ }
+ case 14:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st14(_st14);
+ residuals = st14;
+ break;
+ }
+ case 15:
+ {
+ Eigen::Map<const Eigen::Matrix<T,ID,1>> st15(_st15);
+ residuals = st15;
+ break;
+ }
default:
throw std::runtime_error("ID not found");
}
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index dd1d7b0c1958c254085119a763bc2fc1bf400640..67e19eb03e50427092db42e33cd2f6bd77e386d8 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -141,7 +141,6 @@ TEST(SE3, inverseComposite)
VectorComposite pose_vc_out_bis = inverse(pose_vc);
ASSERT_MATRIX_APPROX(pose_vc_out_bis.at('P'), pi_true, 1e-8);
ASSERT_MATRIX_APPROX(pose_vc_out_bis.at('O'), qi_true.coeffs(), 1e-8);
-
}
TEST(SE3, composeBlocks)
@@ -185,7 +184,6 @@ TEST(SE3, composeEigenVectors)
compose(p1, q1, p2, q2, pc, qc); // tested in composeVectorBlocks
-
Vector7d x1; x1 << p1, q1.coeffs();
Vector7d x2; x2 << p2, q2.coeffs();
Vector7d xc, xc_true; xc_true << pc, qc.coeffs();
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 84cddc0e0d66078976fd71a119375018d98ee77c..4a0bf69e58e1d9093faa937202a5b0696f738ae6 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -28,7 +28,8 @@
#include "dummy/factor_odom_2d_autodiff.h"
#include "dummy/factor_dummy_zero_1.h"
-#include "dummy/factor_dummy_zero_12.h"
+// #include "dummy/factor_dummy_zero_12.h"
+#include "dummy/factor_dummy_zero_15.h"
#include "core/state_block/state_block_derived.h"
#include "core/factor/factor_relative_pose_2d.h"
@@ -43,10 +44,35 @@ std::string wolf_root = _WOLF_ROOT_DIR;
using namespace wolf;
using namespace Eigen;
+void testFactorAutodiff15(FactorBasePtr _fac, StateBlockPtr _sb, const std::vector<const double*>& _states_ptr)
+{
+ unsigned int i = _fac->getSize();
+ std::vector<MatrixXd> J;
+ VectorXd residuals(i);
+
+ // Jacobian
+ _fac->evaluate(_states_ptr, residuals, J);
+ ASSERT_MATRIX_APPROX(residuals, _sb->getState(), wolf::Constants::EPS);
+
+ // std::cout << "i = " << i << std::endl;
+ for (unsigned int j = 0; j < 15; j++)
+ {
+ // std::cout << "j = " << j << std::endl;
+ // std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
+ if (j == i-1)
+ {
+ ASSERT_MATRIX_APPROX(J[j], MatrixXd::Identity(i,i), wolf::Constants::EPS);
+ }
+ else
+ {
+ ASSERT_MATRIX_APPROX(J[j], MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
+ }
+ }
+}
TEST(FactorAutodiff, AutodiffDummy1)
{
- StateBlockPtr sb = std::make_shared<StateParams1>(Eigen::Vector1d::Random());
+ StateBlockPtr sb = std::make_shared<StateParams1>(Vector1d::Random());
FeatureBasePtr ftr = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity());
auto fac = std::make_shared<FactorDummyZero1>(ftr, sb);
@@ -54,322 +80,108 @@ TEST(FactorAutodiff, AutodiffDummy1)
// COMPUTE JACOBIANS
std::vector<const double*> states_ptr({sb->getStateData()});
- std::vector<Eigen::MatrixXd> J;
- Eigen::VectorXd residuals(fac->getSize());
+ std::vector<MatrixXd> J;
+ VectorXd residuals(fac->getSize());
fac->evaluate(states_ptr, residuals, J);
- ASSERT_MATRIX_APPROX(J[0], Eigen::Matrix1d::Ones(), wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(J[0], Matrix1d::Ones(), wolf::Constants::EPS);
}
-TEST(FactorAutodiff, AutodiffDummy12)
+TEST(FactorAutodiff, AutodiffDummy15)
{
- StateBlockPtr sb1 = std::make_shared<StateParams1>(Eigen::Vector1d::Random());
- StateBlockPtr sb2 = std::make_shared<StateParams2>(Eigen::Vector2d::Random());
- StateBlockPtr sb3 = std::make_shared<StateParams3>(Eigen::Vector3d::Random());
- StateBlockPtr sb4 = std::make_shared<StateParams4>(Eigen::Vector4d::Random());
- StateBlockPtr sb5 = std::make_shared<StateParams5>(Eigen::Vector5d::Random());
- StateBlockPtr sb6 = std::make_shared<StateParams6>(Eigen::Vector6d::Random());
- StateBlockPtr sb7 = std::make_shared<StateParams7>(Eigen::Vector7d::Random());
- StateBlockPtr sb8 = std::make_shared<StateParams8>(Eigen::Vector8d::Random());
- StateBlockPtr sb9 = std::make_shared<StateParams9>(Eigen::Vector9d::Random());
- StateBlockPtr sb10 = std::make_shared<StateParams10>(Eigen::Vector10d::Random());
- StateBlockPtr sb11 = std::make_shared<StateParams<11>>(Eigen::VectorXd::Random(11));
- StateBlockPtr sb12 = std::make_shared<StateParams<12>>(Eigen::VectorXd::Random(12));
-
- std::vector<const double*> states_ptr({sb1->getStateData(),sb2->getStateData(),sb3->getStateData(),sb4->getStateData(),sb5->getStateData(),sb6->getStateData(),sb7->getStateData(),sb8->getStateData(),sb9->getStateData(),sb10->getStateData(),sb11->getStateData(),sb12->getStateData()});
- std::vector<Eigen::MatrixXd> J;
- Eigen::VectorXd residuals;
- unsigned int i;
+ StateBlockPtr sb1 = std::make_shared<StateParams1> (Vector1d::Random());
+ StateBlockPtr sb2 = std::make_shared<StateParams2> (Vector2d::Random());
+ StateBlockPtr sb3 = std::make_shared<StateParams3> (Vector3d::Random());
+ StateBlockPtr sb4 = std::make_shared<StateParams4> (Vector4d::Random());
+ StateBlockPtr sb5 = std::make_shared<StateParams5> (Vector5d::Random());
+ StateBlockPtr sb6 = std::make_shared<StateParams6> (Vector6d::Random());
+ StateBlockPtr sb7 = std::make_shared<StateParams7> (Vector7d::Random());
+ StateBlockPtr sb8 = std::make_shared<StateParams8> (Vector8d::Random());
+ StateBlockPtr sb9 = std::make_shared<StateParams9> (Vector9d::Random());
+ StateBlockPtr sb10 = std::make_shared<StateParams10> (Vector10d::Random());
+ StateBlockPtr sb11 = std::make_shared<StateParams<11>>(VectorXd::Random(11));
+ StateBlockPtr sb12 = std::make_shared<StateParams<12>>(VectorXd::Random(12));
+ StateBlockPtr sb13 = std::make_shared<StateParams<13>>(VectorXd::Random(13));
+ StateBlockPtr sb14 = std::make_shared<StateParams<14>>(VectorXd::Random(14));
+ StateBlockPtr sb15 = std::make_shared<StateParams<15>>(VectorXd::Random(15));
+
+ std::vector<const double*> states_ptr({sb1->getStateData(),
+ sb2->getStateData(),
+ sb3->getStateData(),
+ sb4->getStateData(),
+ sb5->getStateData(),
+ sb6->getStateData(),
+ sb7->getStateData(),
+ sb8->getStateData(),
+ sb9->getStateData(),
+ sb10->getStateData(),
+ sb11->getStateData(),
+ sb12->getStateData(),
+ sb13->getStateData(),
+ sb14->getStateData(),
+ sb15->getStateData()});
FactorBasePtr fac = nullptr;
FeatureBasePtr ftr = std::make_shared<FeatureBase>("Feature", Vector2d(1,2), Matrix2d::Identity());
// 1 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<1>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<1>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb1, states_ptr);
// 2 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<2>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<2>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb2, states_ptr);
// 3 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<3>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<3>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb3, states_ptr);
// 4 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<4>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<4>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb4, states_ptr);
// 5 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<5>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<5>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb5, states_ptr);
// 6 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<6>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<6>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb6, states_ptr);
// 7 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<7>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<7>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb7, states_ptr);
// 8 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<8>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<8>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb8, states_ptr);
// 9 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<9>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<9>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb9, states_ptr);
// 10 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<10>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<10>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb10, states_ptr);
// 11 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<11>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
-
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
-
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ fac = std::make_shared<FactorDummyZero15<11>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb11, states_ptr);
// 12 ------------------------------------------------------------------------
- fac = std::make_shared<FactorDummyZero12<12>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12);
- i = fac->getSize();
+ fac = std::make_shared<FactorDummyZero15<12>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb12, states_ptr);
- // Jacobian
- J.clear();
- residuals.resize(i);
- fac->evaluate(states_ptr, residuals, J);
+ // 13 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero15<13>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb13, states_ptr);
- std::cout << "i = " << i << std::endl;
- for (unsigned int j = 0; j < 12; j++)
- {
- std::cout << "j = " << j << std::endl;
- std::cout << "Jacobian size: " << J[j].rows() << " " << J[j].cols() << std::endl;
- if (j == i-1)
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Identity(i,i), wolf::Constants::EPS);
- }
- else
- {
- ASSERT_MATRIX_APPROX(J[j], Eigen::MatrixXd::Zero(i,j+1), wolf::Constants::EPS);
- }
- }
+ // 14 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero15<14>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb14, states_ptr);
+
+ // 15 ------------------------------------------------------------------------
+ fac = std::make_shared<FactorDummyZero15<15>>(ftr,sb1,sb2,sb3,sb4,sb5,sb6,sb7,sb8,sb9,sb10,sb11,sb12,sb13,sb14,sb15);
+ testFactorAutodiff15(fac, sb15, states_ptr);
}
TEST(FactorAutodiff, EmplaceOdom2d)
@@ -387,7 +199,7 @@ TEST(FactorAutodiff, EmplaceOdom2d)
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, Vector3d::Zero(), Matrix3d::Identity());
// FACTOR
auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
@@ -403,7 +215,7 @@ TEST(FactorAutodiff, ResidualOdom2d)
{
ProblemPtr problem = Problem::create("PO", 2);
- Eigen::Vector3d f1_pose, f2_pose;
+ Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
@@ -417,24 +229,24 @@ TEST(FactorAutodiff, ResidualOdom2d)
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
- Eigen::Vector3d d;
+ Vector3d d;
d << -1, -4, M_PI/2;
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Matrix3d::Identity());
// FACTOR
auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// EVALUATE
- Eigen::VectorXd fr1_pstate = fr1_ptr->getP()->getState();
- Eigen::VectorXd fr1_ostate = fr1_ptr->getO()->getState();
- Eigen::VectorXd fr2_pstate = fr2_ptr->getP()->getState();
- Eigen::VectorXd fr2_ostate = fr2_ptr->getO()->getState();
+ VectorXd fr1_pstate = fr1_ptr->getP()->getState();
+ VectorXd fr1_ostate = fr1_ptr->getO()->getState();
+ VectorXd fr2_pstate = fr2_ptr->getP()->getState();
+ VectorXd fr2_ostate = fr2_ptr->getO()->getState();
std::vector<double*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
double const* const* parameters = states_ptr.data();
- Eigen::VectorXd residuals(factor_ptr->getSize());
+ VectorXd residuals(factor_ptr->getSize());
factor_ptr->evaluate(parameters, residuals.data(), nullptr);
ASSERT_TRUE(residuals.maxCoeff() < 1e-9);
@@ -445,7 +257,7 @@ TEST(FactorAutodiff, JacobianOdom2d)
{
ProblemPtr problem = Problem::create("PO", 2);
- Eigen::Vector3d f1_pose, f2_pose;
+ Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
@@ -459,51 +271,51 @@ TEST(FactorAutodiff, JacobianOdom2d)
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
- Eigen::Vector3d d;
+ Vector3d d;
d << -1, -4, M_PI/2;
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Matrix3d::Identity());
// FACTOR
auto factor_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
// COMPUTE JACOBIANS
- const Eigen::VectorXd fr1_pstate = fr1_ptr->getP()->getState();
- const Eigen::VectorXd fr1_ostate = fr1_ptr->getO()->getState();
- const Eigen::VectorXd fr2_pstate = fr2_ptr->getP()->getState();
- const Eigen::VectorXd fr2_ostate = fr2_ptr->getO()->getState();
+ const VectorXd fr1_pstate = fr1_ptr->getP()->getState();
+ const VectorXd fr1_ostate = fr1_ptr->getO()->getState();
+ const VectorXd fr2_pstate = fr2_ptr->getP()->getState();
+ const VectorXd fr2_ostate = fr2_ptr->getO()->getState();
std::vector<const double*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
- std::vector<Eigen::MatrixXd> Jauto;
- Eigen::VectorXd residuals(factor_ptr->getSize());
+ std::vector<MatrixXd> Jauto;
+ VectorXd residuals(factor_ptr->getSize());
factor_ptr->evaluate(states_ptr, residuals, Jauto);
ASSERT_EQ(Jauto.size(),4);
// ANALYTIC JACOBIANS
- Eigen::MatrixXd J0(3,2);
+ MatrixXd J0(3,2);
J0 << -cos(f1_pose(2)), -sin(f1_pose(2)),
sin(f1_pose(2)), -cos(f1_pose(2)),
0, 0;
ASSERT_MATRIX_APPROX(J0, Jauto[0], wolf::Constants::EPS);
//ASSERT_TRUE((J0-Jauto[0]).maxCoeff() < 1e-9 && (J0-Jauto[0]).minCoeff() > -1e-9);
- Eigen::MatrixXd J1(3,1);
+ MatrixXd J1(3,1);
J1 << -(f2_pose(0) - f1_pose(0)) * sin(f1_pose(2)) + (f2_pose(1) - f1_pose(1)) * cos(f1_pose(2)),
-(f2_pose(0) - f1_pose(0)) * cos(f1_pose(2)) - (f2_pose(1) - f1_pose(1)) * sin(f1_pose(2)),
-1;
ASSERT_MATRIX_APPROX(J1, Jauto[1], wolf::Constants::EPS);
//ASSERT_TRUE((J1-Jauto[1]).maxCoeff() < 1e-9 && (J1-Jauto[1]).minCoeff() > -1e-9);
- Eigen::MatrixXd J2(3,2);
+ MatrixXd J2(3,2);
J2 << cos(f1_pose(2)), sin(f1_pose(2)),
-sin(f1_pose(2)), cos(f1_pose(2)),
0, 0;
ASSERT_MATRIX_APPROX(J2, Jauto[2], wolf::Constants::EPS);
//ASSERT_TRUE((J2-Jauto[2]).maxCoeff() < 1e-9 && (J2-Jauto[2]).minCoeff() > -1e-9);
- Eigen::MatrixXd J3(3,1);
+ MatrixXd J3(3,1);
J3 << 0, 0, 1;
ASSERT_MATRIX_APPROX(J3, Jauto[3], wolf::Constants::EPS);
//ASSERT_TRUE((J3-Jauto[3]).maxCoeff() < 1e-9 && (J3-Jauto[3]).minCoeff() > -1e-9);
@@ -522,7 +334,7 @@ TEST(FactorAutodiff, AutodiffVsAnalytic)
{
ProblemPtr problem = Problem::create("PO", 2);
- Eigen::Vector3d f1_pose, f2_pose;
+ Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
@@ -536,24 +348,24 @@ TEST(FactorAutodiff, AutodiffVsAnalytic)
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
- Eigen::Vector3d d;
+ Vector3d d;
d << -1, -4, M_PI/2;
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Eigen::Matrix3d::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2d>(capture_ptr, d, Matrix3d::Identity());
// FACTOR
auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2dAutodiff>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false);
auto fac_analytic_ptr = FactorBase::emplace<FactorRelativePose2d>(feature_ptr, feature_ptr, fr1_ptr, nullptr, false, TOP_MOTION);
// COMPUTE JACOBIANS
- const Eigen::VectorXd fr1_pstate = fr1_ptr->getP()->getState();
- const Eigen::VectorXd fr1_ostate = fr1_ptr->getO()->getState();
- const Eigen::VectorXd fr2_pstate = fr2_ptr->getP()->getState();
- const Eigen::VectorXd fr2_ostate = fr2_ptr->getO()->getState();
+ const VectorXd fr1_pstate = fr1_ptr->getP()->getState();
+ const VectorXd fr1_ostate = fr1_ptr->getO()->getState();
+ const VectorXd fr2_pstate = fr2_ptr->getP()->getState();
+ const VectorXd fr2_ostate = fr2_ptr->getO()->getState();
std::vector<const double*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
- std::vector<Eigen::MatrixXd> Jautodiff, Janalytic;
- Eigen::VectorXd residuals(fac_autodiff_ptr->getSize());
+ std::vector<MatrixXd> Jautodiff, Janalytic;
+ VectorXd residuals(fac_autodiff_ptr->getSize());
clock_t t = clock();
fac_autodiff_ptr->evaluate(states_ptr, residuals, Jautodiff);
std::cout << "autodiff evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
diff --git a/test/gtest_factor_relative_pose_2d.cpp b/test/gtest_factor_relative_pose_2d.cpp
index 15a042316c76bba2fe4c5aefcfb87cad01c31cba..72dfe1687e6122e78663114911eabcd9140eb796 100644
--- a/test/gtest_factor_relative_pose_2d.cpp
+++ b/test/gtest_factor_relative_pose_2d.cpp
@@ -25,6 +25,8 @@
#include "core/factor/factor_relative_pose_2d.h"
#include "core/capture/capture_odom_2d.h"
#include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
using namespace Eigen;
@@ -32,6 +34,12 @@ using namespace wolf;
using std::cout;
using std::endl;
+
+int N = 1e2;
+
+// Vectors
+Vector3d delta, x_0, x_1, x_f, x_l;
+
// Input odometry data and covariance
Matrix3d data_cov = 0.1*Matrix3d::Identity();
@@ -43,93 +51,180 @@ SolverCeres solver(problem_ptr);
FrameBasePtr frm0 = problem_ptr->emplaceFrame(0.0, "PO", Vector3d::Zero());
FrameBasePtr frm1 = problem_ptr->emplaceFrame(1.0, "PO", Vector3d::Zero());
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPose2d",
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ std::make_shared<StateAngle>(Vector1d::Zero()));
+
// Capture from frm1 to frm0
auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, nullptr, Vector3d::Zero(), data_cov);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePose2dPtr fac = nullptr;
-TEST(FactorRelativePose2d, check_tree)
+void generateRandomProblemFrame()
{
- ASSERT_TRUE(problem_ptr->check(0));
+ // solver options
+ solver.getSolverOptions().max_num_iterations = 100;
+ solver.getSolverOptions().gradient_tolerance = 1e-9;
+ solver.getSolverOptions().function_tolerance = 1e-9;
+
+ if (fea)
+ fea->remove();
+
+ // Random delta
+ delta = Vector3d::Random() * 1e2;
+ delta(2) = pi2pi(delta(2));
+
+ // Random frame 0 pose
+ x_0 = Vector3d::Random() * 1e2;
+ x_0(2) = pi2pi(x_0(2));
+
+ // Frame 1 pose
+ x_1.head<2>() = x_0.head<2>() + Rotation2Dd(x_0(2)) * delta.head<2>();
+ x_1(2) = pi2pi(x_0(2) + delta(2));
+
+ // Set states
+ frm0->setState(x_0, "PO");
+ frm1->setState(x_1, "PO");
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose2d>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add
}
-TEST(FactorRelativePose2d, fix_0_solve)
+void generateRandomProblemLandmark()
{
- for (int i = 0; i < 1e3; i++)
- {
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
+ // solver options
+ solver.getSolverOptions().max_num_iterations = 100;
+ solver.getSolverOptions().gradient_tolerance = 1e-9;
+ solver.getSolverOptions().function_tolerance = 1e-9;
+
+ if (fea)
+ fea->remove();
+
+ // Random delta
+ delta = Vector3d::Random() * 1e2;
+ delta(2) = pi2pi(delta(2));
+
+ // Random frame 0 pose
+ x_f = Vector3d::Random() * 1e2;
+ x_f(2) = pi2pi(x_f(2));
+
+ // landmark pose
+ x_l(2) = pi2pi(x_f(2) + delta(2));
+ x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * delta.head<2>();
+
+ // Set states
+ frm1->setState(x_f, "PO");
+ lmk->setState(x_l, "PO");
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose2d>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add
+}
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
+void checkStatesFrame()
+{
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x_1, 1e-4);
+ ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x_0, 1e-4);
+}
- // x1 groundtruth
- Vector3d x1;
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
- x1(2) = pi2pi(x0(2) + delta(2));
+void checkStatesLandmark()
+{
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x_f, 1e-4);
+ ASSERT_POSE2d_APPROX(lmk->getStateVector("PO"), x_l, 1e-4);
+}
- // Set states and measurement
- frm0->setState(x0,"PO");
- frm1->setState(x1,"PO");
- cap1->setData(delta);
+////////////// TESTS /////////////////////////////////////////////////////////////////////
+TEST(FactorRelativePose2d, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePose2d, frame_solve_frame1)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
- // Fix frm0, perturb frm1
+ // Perturb frm1, fix the rest
frm0->fix();
frm1->unfix();
- frm1->perturb(5);
+ frm1->perturb(1);
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1, 1e-6);
-
- // remove feature (and factor) for the next loop
- fea1->remove();
+ checkStatesFrame();
}
}
-TEST(FactorRelativePose2d, fix_1_solve)
+TEST(FactorRelativePose2d, frame_solve_frame0)
{
- for (int i = 0; i < 1e3; i++)
+ for (int i = 0; i < N; i++)
{
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->unfix();
+ frm0->perturb(1);
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- // x1 groundtruth
- Vector3d x1;
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
- x1(2) = pi2pi(x0(2) + delta(2));
+ checkStatesFrame();
+ }
+}
+
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePose2d, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb landmark, fix the rest
+ frm1->fix();
+ lmk->unfix();
+ lmk->perturb(1);
- // Set states and measurement
- frm0->setState(x0,"PO");
- frm1->setState(x1,"PO");
- cap1->setData(delta);
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
+ checkStatesLandmark();
+ }
+}
- // Fix frm1, perturb frm0
- frm1->fix();
- frm0->unfix();
- frm0->perturb(5);
+TEST(FactorRelativePose2d, landmark_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm1->unfix();
+ lmk->fix();
+ frm1->perturb(1);
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0, 1e-6);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- // remove feature (and factor) for the next loop
- fea1->remove();
+ checkStatesLandmark();
}
}
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index c15db358f0ec0ef6cade5137841c7faf23f1fdbb..1064bd553313ecfa1653894f29b2522f595a9512 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
@@ -27,6 +27,8 @@
#include "core/sensor/sensor_odom.h"
#include "core/processor/processor_odom_2d.h"
#include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
using namespace Eigen;
using namespace wolf;
@@ -35,220 +37,327 @@ using std::endl;
std::string wolf_root = _WOLF_ROOT_DIR;
+int N = 1e2;
+
+// Vectors
+Vector3d delta, x_0, x_1, x_f, x_l, x_s;
+
// Input odometry data and covariance
-Matrix3d data_cov = 0.1*Matrix3d::Identity();
+Matrix3d data_cov = 1e-6*Matrix3d::Identity();
// Problem and solver
ProblemPtr problem_ptr = Problem::create("PO", 2);
SolverCeres solver(problem_ptr);
// Sensor
-auto sensor_odom2d = problem_ptr->installSensor("SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
-auto processor_odom2d = ProcessorBase::emplace<ProcessorOdom2d>(sensor_odom2d, std::make_shared<ParamsProcessorOdom2d>());
+auto sensor = problem_ptr->installSensor("SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
+auto processor_odom2d = ProcessorBase::emplace<ProcessorOdom2d>(sensor, std::make_shared<ParamsProcessorOdom2d>());
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero() );
FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, "PO", Vector3d::Zero() );
-// Capture from frm1 to frm0
-auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor_odom2d, Vector3d::Zero(), data_cov);
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPose2d",
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ std::make_shared<StateAngle>(Vector1d::Zero()));
-TEST(FactorRelativePose2dWithExtrinsics, check_tree)
+// Capture
+auto cap1 = CaptureBase::emplace<CaptureOdom2d>(frm1, 1, sensor, Vector3d::Zero(), data_cov);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePose2dWithExtrinsicsPtr fac = nullptr;
+
+void generateRandomProblemFrame()
{
- ASSERT_TRUE(problem_ptr->check(0));
+ // solver options
+ solver.getSolverOptions().max_num_iterations = 100;
+ solver.getSolverOptions().gradient_tolerance = 1e-8;
+ solver.getSolverOptions().function_tolerance = 1e-9;
+
+ if (fea)
+ fea->remove();
+
+ // Random delta
+ delta = Vector3d::Random() * 1e2;
+ delta(2) = pi2pi(delta(2));
+
+ // Random frame 0 pose
+ x_0 = Vector3d::Random() * 1e2;
+ x_0(2) = pi2pi(x_0(2));
+
+ // Random extrinsics
+ x_s = Vector3d::Random() * 1e2;
+ x_s(2) = pi2pi(x_s(2));
+
+ // Frame 1 pose
+ x_1(2) = pi2pi(x_0(2) + delta(2));
+ x_1.head<2>() = x_0.head<2>() + Rotation2Dd(x_0(2)) * (x_s.head<2>() + Rotation2Dd(x_s(2)) * delta.head<2>()) - Rotation2Dd(x_1(2)) * x_s.head<2>();
+
+ // Set states
+ frm0->setState(x_0, "PO");
+ frm1->setState(x_1, "PO");
+ cap1->setData(delta);
+ sensor->setState(x_s, "PO");
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add
}
-TEST(FactorRelativePose2dWithExtrinsics, fix_0_solve)
+void generateRandomProblemLandmark()
{
- for (int i = 0; i < 1e2; i++)
- {
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
+ // solver options
+ solver.getSolverOptions().max_num_iterations = 100;
+ solver.getSolverOptions().gradient_tolerance = 1e-9;
+ solver.getSolverOptions().function_tolerance = 1e-9;
+
+ if (fea)
+ fea->remove();
+
+ // Random delta
+ delta = Vector3d::Random() * 1e2;
+ delta(2) = pi2pi(delta(2));
+
+ // Random frame pose
+ x_f = Vector3d::Random() * 1e2;
+ x_f(2) = pi2pi(x_f(2));
+
+ // Random extrinsics
+ x_s = Vector3d::Random() * 1e2;
+ x_s(2) = pi2pi(x_s(2));
+
+ // landmark pose
+ x_l(2) = pi2pi(x_f(2) + x_s(2) + delta(2));
+ x_l.head<2>() = x_f.head<2>() + Rotation2Dd(x_f(2)) * (x_s.head<2>() + Rotation2Dd(x_s(2)) * delta.head<2>());
+
+ // Set states
+ frm1->setState(x_f,"PO");
+ lmk->setState(x_l, "PO");
+ cap1->setData(delta);
+ sensor->setState(x_s, "PO");
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add
+}
+
+void checkStatesFrame()
+{
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x_1, 1e-4);
+ ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x_0, 1e-4);
+ ASSERT_POSE2d_APPROX(sensor->getStateVector("PO"), x_s, 1e-4);
+}
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
+void checkStatesLandmark()
+{
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x_f, 1e-4);
+ ASSERT_POSE2d_APPROX(lmk->getStateVector("PO"), x_l, 1e-4);
+ ASSERT_POSE2d_APPROX(sensor->getStateVector("PO"), x_s, 1e-4);
+}
- // Random extrinsics
- Vector3d xs = Vector3d::Random();
- pi2pi(xs(2));
+////////////// TESTS /////////////////////////////////////////////////////////////////////
- // x1 groundtruth
- Vector3d x1;
- x1(2) = pi2pi(x0(2) + delta(2));
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+TEST(FactorRelativePose2dWithExtrinsics, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
- // Set states
- frm0->setState(x0,"PO");
- frm1->setState(x1,"PO");
- cap1->setData(delta);
- sensor_odom2d->setState(xs, "PO");
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePose2dWithExtrinsics, frame_solve_frame1)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
// Perturb frm1, fix the rest
frm0->fix();
frm1->unfix();
- sensor_odom2d->getP()->fix();
- sensor_odom2d->getO()->fix();
- frm1->perturb(5);
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm1->perturb();
// solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1, 1e-6);
-
- // remove feature (and factor) for the next loop
- fea1->remove();
+ checkStatesFrame();
}
}
-TEST(FactorRelativePose2dWithExtrinsics, fix_1_solve)
+TEST(FactorRelativePose2dWithExtrinsics, frame_solve_frame0)
{
- for (int i = 0; i < 1e2; i++)
+ for (int i = 0; i < N; i++)
{
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
-
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
-
- // Random extrinsics
- Vector3d xs = Vector3d::Random();
- pi2pi(xs(2));
-
- // x1 groundtruth
- Vector3d x1;
- x1(2) = pi2pi(x0(2) + delta(2));
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
-
- // Set states
- frm0->setState(x0,"PO");
- frm1->setState(x1,"PO");
- cap1->setData(delta);
- sensor_odom2d->setState(xs,"PO");
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
// Perturb frm0, fix the rest
frm1->fix();
frm0->unfix();
- sensor_odom2d->getP()->fix();
- sensor_odom2d->getO()->fix();
- frm0->perturb(5);
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm0->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x0, 1e-6);
-
- // remove feature (and factor) for the next loop
- fea1->remove();
+ checkStatesFrame();
}
}
-TEST(FactorRelativePose2dWithExtrinsics, extrinsics_p_solve)
+// JV: The position extrinsics in case of pair of frames is not observable
+TEST(FactorRelativePose2dWithExtrinsics, frame_solve_extrinsics_p)
{
- for (int i = 0; i < 1e2; i++)
+ for (int i = 0; i < N; i++)
{
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
-
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
- // Random extrinsics
- Vector3d xs = Vector3d::Random();
- pi2pi(xs(2));
+ // Perturb sensor P, fix the rest
+ frm1->fix();
+ frm0->fix();
+ sensor->getP()->unfix();
+ sensor->getO()->fix();
+ sensor->getP()->perturb();
- // x1 groundtruth
- Vector3d x1;
- x1(2) = pi2pi(x0(2) + delta(2));
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
- // Set states
- frm0->setState(x0,"PO");
- frm1->setState(x1,"PO");
- cap1->setData(delta);
- sensor_odom2d->setState(xs,"PO");
+ //checkStatesFrame();
+ }
+}
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
+TEST(FactorRelativePose2dWithExtrinsics, frame_solve_extrinsics_o)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
- // Perturb sensor P, fix the rest
+ // Perturb sensor O, fix the rest
frm1->fix();
frm0->fix();
- sensor_odom2d->getP()->unfix();
- sensor_odom2d->getO()->fix();
- sensor_odom2d->getP()->perturb(1);
+ sensor->getP()->fix();
+ sensor->getO()->unfix();
+ sensor->getO()->perturb();
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesFrame();
+ }
+}
+
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePose2dWithExtrinsics, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
+
+ // Perturb landmark, fix the rest
+ frm1->fix();
+ lmk->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ lmk->perturb();
- ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector("PO"), xs, 1e-6);
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- // remove feature (and factor) for the next loop
- fea1->remove();
+ checkStatesLandmark();
}
}
-TEST(FactorRelativePose2dWithExtrinsics, extrinsics_o_solve)
+TEST(FactorRelativePose2dWithExtrinsics, landmark_solve_frame)
{
- for (int i = 0; i < 1e2; i++)
+ for (int i = 0; i < N; i++)
{
- // Random delta
- Vector3d delta = Vector3d::Random() * 10;
- pi2pi(delta(2));
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
- // Random initial pose
- Vector3d x0 = Vector3d::Random() * 10;
- pi2pi(x0(2));
+ // Perturb frm0, fix the rest
+ frm1->unfix();
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm1->perturb();
- // Random extrinsics
- Vector3d xs = Vector3d::Random();
- pi2pi(xs(2));
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- // x1 groundtruth
- Vector3d x1;
- x1(2) = pi2pi(x0(2) + delta(2));
- x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * (xs.head<2>() + Rotation2Dd(xs(2)) * delta.head<2>()) - Rotation2Dd(x1(2)) * xs.head<2>();
+ checkStatesLandmark();
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, landmark_extrinsics_p_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
- // Set states
- frm0->setState(x0,"PO");
- frm1->setState(x1,"PO");
- cap1->setData(delta);
- sensor_odom2d->setState(xs,"PO");
+ // Perturb sensor P, fix the rest
+ frm1->fix();
+ lmk->fix();
+ sensor->getP()->unfix();
+ sensor->getO()->fix();
+ sensor->getP()->perturb();
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
- FactorBase::emplace<FactorRelativePose2dWithExtrinsics>(fea1, fea1, frm0, processor_odom2d, false, TOP_MOTION); // create and add
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesLandmark();
+ }
+}
+
+TEST(FactorRelativePose2dWithExtrinsics, landmark_extrinsics_o_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(3), Constants::EPS);
// Perturb sensor O, fix the rest
frm1->fix();
- frm0->fix();
- sensor_odom2d->getP()->fix();
- sensor_odom2d->getO()->unfix();
- sensor_odom2d->getO()->perturb(.2);
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->unfix();
+ sensor->getO()->perturb();
- //std::cout << "Sensor O (perturbed): " << sensor_odom2d->getO()->getState().transpose() << std::endl;
+ //std::cout << "Sensor O (perturbed): " << sensor->getO()->getState().transpose() << std::endl;
// solve for frm0
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(sensor_odom2d->getStateVector("PO"), xs, 1e-6);
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
- // remove feature (and factor) for the next loop
- fea1->remove();
+ checkStatesLandmark();
}
}
diff --git a/test/gtest_factor_relative_pose_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp
index a0ebf1390e044af47b9167334b937e9bea2e74ad..6d13ac2f7bb6f622f06490258b3977c60c76485b 100644
--- a/test/gtest_factor_relative_pose_3d.cpp
+++ b/test/gtest_factor_relative_pose_3d.cpp
@@ -19,18 +19,14 @@
// 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"
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/factor/factor_relative_pose_3d.h"
-#include "core/capture/capture_motion.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;
@@ -38,72 +34,205 @@ using namespace wolf;
using std::cout;
using std::endl;
-Vector7d data2delta(Vector6d _data)
-{
- return (Vector7d() << _data.head<3>() , v2q(_data.tail<3>()).coeffs()).finished();
-}
+std::string wolf_root = _WOLF_ROOT_DIR;
-// Input odometry data and covariance
-Vector6d data(Vector6d::Random());
-Vector7d delta = data2delta(data);
-Vector6d data_var((Vector6d() << 0.2,0.2,0.2,0.2,0.2,0.2).finished());
-Matrix6d data_cov = data_var.asDiagonal();
+int N = 1e2;
-// perturbated priors
-Vector7d x0 = data2delta(Vector6d::Random()*0.25);
-Vector7d x1 = data2delta(data + Vector6d::Random()*0.25);
+// Vectors
+Vector7d delta, x_0, x_1, x_f, x_l;
+
+// Input odometry data and covariance
+Matrix6d data_cov = (Vector6d() << 1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6).finished().asDiagonal();
// Problem and solver
ProblemPtr problem_ptr = Problem::create("PO", 3);
+
SolverCeres solver(problem_ptr);
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), problem_ptr->stateZero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), "PO", delta);
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), problem_ptr->stateZero());
-// Capture, feature and factor from frm1 to frm0
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPose3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ std::make_shared<StateQuaternion>(Quaterniond::Identity().coeffs()));
+
+// Capture
auto cap1 = CaptureBase::emplace<CaptureMotion>(frm1, "CaptureOdom3d", 1, nullptr, delta, nullptr);
-auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
-FactorRelativePose3dPtr ctr1 = FactorBase::emplace<FactorRelativePose3d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION); // create and add
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePose3dPtr fac = nullptr;
////////////////////////////////////////////////////////
-TEST(FactorRelativePose3d, check_tree)
+void generateRandomProblemFrame()
{
- ASSERT_TRUE(problem_ptr->check(0));
+ // solver options
+ solver.getSolverOptions().max_num_iterations = 100;
+ solver.getSolverOptions().gradient_tolerance = 1e-9;
+ solver.getSolverOptions().function_tolerance = 1e-9;
+
+ if (fea)
+ fea->remove();
+
+ // Random delta
+ delta = Vector7d::Random() * 1e2;
+ delta.tail<4>().normalize();
+ auto q_delta = Quaterniond(delta.tail<4>());
+
+ // Random frame 0 pose
+ x_0 = Vector7d::Random() * 1e2;
+ x_0.tail<4>().normalize();
+ auto q_0 = Quaterniond(x_0.tail<4>());
+
+ // Frame 1 pose
+ x_1.head<3>() = x_0.head<3>() + q_0 * delta.head<3>();
+ x_1.tail<4>() = (q_0 * q_delta).coeffs();
+
+ // Set states
+ frm0->setState(x_0, "PO");
+ frm1->setState(x_1, "PO");
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose3d>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add
}
-TEST(FactorRelativePose3d, expectation)
+void generateRandomProblemLandmark()
{
- ASSERT_POSE3d_APPROX(ctr1->expectation() , delta, 1e-6);
+ // solver options
+ solver.getSolverOptions().max_num_iterations = 100;
+ solver.getSolverOptions().gradient_tolerance = 1e-9;
+ solver.getSolverOptions().function_tolerance = 1e-9;
+
+ if (fea)
+ fea->remove();
+
+ // Random delta
+ delta = Vector7d::Random() * 10;
+ delta.tail<4>().normalize();
+ auto q_delta = Quaterniond(delta.tail<4>());
+
+ // Random frame pose
+ x_f = Vector7d::Random() * 10;
+ x_f.tail<4>().normalize();
+ auto q_f = Quaterniond(x_f.tail<4>());
+
+ // Landmark pose
+ x_l.head<3>() = x_f.head<3>() + q_f * delta.head<3>();
+ x_l.tail<4>() = (q_f * q_delta).coeffs();
+
+ // Set states
+ frm1->setState(x_f, "PO");
+ lmk->setState(x_l, "PO");
+ cap1->setData(delta);
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose3d>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add
}
-TEST(FactorRelativePose3d, fix_0_solve)
+void checkStatesFrame()
{
+ ASSERT_POSE3d_APPROX(frm1->getStateVector("PO"), x_1, 1e-4);
+ ASSERT_POSE3d_APPROX(frm0->getStateVector("PO"), x_0, 1e-4);
+}
- // Fix frame 0, perturb frm1
- frm0->fix();
- frm1->unfix();
- frm1->setState(x1,"PO");
-
- // solve for frm1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+void checkStatesLandmark()
+{
+ ASSERT_POSE3d_APPROX(frm1->getStateVector("PO"), x_f, 1e-4);
+ ASSERT_POSE3d_APPROX(lmk->getStateVector("PO"), x_l, 1e-4);
+}
- ASSERT_POSE3d_APPROX(frm1->getStateVector("PO"), delta, 1e-6);
+TEST(FactorRelativePose3d, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePose3d, frame_solve_frame1)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm1, fix the rest
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(1);
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesFrame();
+ }
}
-TEST(FactorRelativePose3d, fix_1_solve)
+TEST(FactorRelativePose3d, frame_solve_frame0)
{
- // Fix frame 1, perturb frm0
- frm0->unfix();
- frm1->fix();
- frm0->setState(x0,"PO");
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->unfix();
+ frm0->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesFrame();
+ }
+}
- // solve for frm0
- std::string brief_report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePose3d, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb landmark, fix the rest
+ frm1->fix();
+ lmk->unfix();
+ lmk->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesLandmark();
+ }
+}
- ASSERT_POSE3d_APPROX(frm0->getStateVector("PO"), problem_ptr->stateZero().vector("PO"), 1e-6);
+TEST(FactorRelativePose3d, landmark_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm1->unfix();
+ lmk->fix();
+ frm1->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesLandmark();
+ }
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
index b35f00900c03eafbd1bde420ff02439a53367537..6bc7dd9532d60e273398150fdc2bb0423eb6098c 100644
--- a/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_3d_with_extrinsics.cpp
@@ -19,303 +19,351 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#include <core/factor/factor_relative_pose_3d_with_extrinsics.h>
-#include <core/utils/utils_gtest.h>
-
-#include "core/common/wolf.h"
-#include "core/utils/logging.h"
+#include "core/utils/utils_gtest.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
+#include "core/capture/capture_odom_3d.h"
+#include "core/sensor/sensor_odom.h"
+#include "core/math/rotations.h"
#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
-#include "core/ceres_wrapper/solver_ceres.h"
-#include "core/capture/capture_pose.h"
-#include "core/feature/feature_pose.h"
-#include "core/sensor/sensor_pose.h"
+
using namespace Eigen;
using namespace wolf;
-using std::static_pointer_cast;
std::string wolf_root = _WOLF_ROOT_DIR;
-// Use the following in case you want to initialize tests with predefines variables or methods.
-class FactorRelativePose3dWithExtrinsics_class : public testing::Test{
- public:
- Vector3d pos_camera, pos_robot, pos_landmark;
- Vector3d euler_camera, euler_robot, euler_landmark;
- Quaterniond quat_camera, quat_robot, quat_landmark;
- Vector4d vquat_camera, vquat_robot, vquat_landmark; // quaternions as vectors
- Vector7d pose_camera, pose_robot, pose_landmark;
-
- ProblemPtr problem;
- SolverCeresPtr solver;
-
- SensorBasePtr S;
- FrameBasePtr F1;
- CapturePosePtr c1;
- FeaturePosePtr f1;
- LandmarkBasePtr lmk1;
- FactorRelativePose3dWithExtrinsicsPtr fac;
-
- Eigen::Matrix6d meas_cov;
-
- void SetUp() override
- {
- // configuration
-
- /* We have three poses to take into account:
- * - pose of the sensor (extrinsincs)
- * - pose of the landmark
- * - pose of the robot (Keyframe)
- *
- * The measurement provides the pose of the landmark wrt sensor's current pose in the world.
- * Camera's current pose in World is the composition of the robot pose with the sensor extrinsics.
- *
- * The robot has a sensor looking forward
- * S: pos = (0,0,0), ori = (0, 0, 0)
- *
- * There is a point at the origin
- * P: pos = (0,0,0)
- *
- * Therefore, P projects exactly at the origin of the sensor,
- * f: p = (0,0)
- *
- * We form a Wolf tree with 1 frames F1, 1 capture C1,
- * 1 feature f1 (measurement), 1 landmark l and 1 relative kf landmark constraint c1:
- *
- * Frame F1, Capture C1, feature f1, landmark l1, constraint c1
- *
- * The frame pose F1, and the sensor pose S
- * in the robot frame are variables subject to optimization
- *
- * We perform a number of tests based on this configuration.*/
-
-
- // sensor is at origin of robot and looking forward
- // robot is at (0,0,0)
- // landmark is right in front of sensor. Its orientation wrt sensor is identity.
- pos_camera << 0,0,0;
- pos_robot << 0,0,0; //robot is at origin
- pos_landmark << 0,1,0;
- euler_camera << 0,0,0;
- //euler_camera << -M_PI_2, 0, -M_PI_2;
- euler_robot << 0,0,0;
- euler_landmark << 0,0,0;
- quat_camera = e2q(euler_camera);
- quat_robot = e2q(euler_robot);
- quat_landmark = e2q(euler_landmark);
- vquat_camera = quat_camera.coeffs();
- vquat_robot = quat_robot.coeffs();
- vquat_landmark = quat_landmark.coeffs();
- pose_camera << pos_camera, vquat_camera;
- pose_robot << pos_robot, vquat_robot;
- pose_landmark << pos_landmark, vquat_landmark;
-
- // Build problem
- problem = Problem::create("PO", 3);
- solver = std::make_shared<SolverCeres>(problem);
-
- // Create sensor to be able to initialize
- auto S_params = std::make_shared<ParamsSensorPose>();
- S_params->std_noise = Eigen::Vector6d::Ones();
- S = SensorBase::emplace<SensorPose3d>(problem->getHardware(),
- S_params,
- SpecSensorComposite{{'P',SpecStateSensor("StatePoint3d",pos_camera)},
- {'O',SpecStateSensor("StateQuaternion",vquat_camera)}});
-
- // F1 is be origin KF
- SpecComposite prior;
- prior.emplace('P',SpecState("StatePoint3d", pos_robot, "factor", Vector3d::Ones()));
- prior.emplace('O',SpecState("StateQuaternion", vquat_robot, "factor", Vector3d::Ones()));
- F1 = problem->setPrior(prior, 0.0);
-
- // the sensor is at origin as well as the robot. The measurement matches with the pose of the tag wrt sensor (and also wrt robot and world)
- meas_cov = Eigen::Matrix6d::Identity();
- meas_cov.topLeftCorner(3,3) *= 1e-2;
- meas_cov.bottomRightCorner(3,3) *= 1e-3;
-
- //emplace feature and landmark
- c1 = std::static_pointer_cast<CapturePose>(CaptureBase::emplace<CapturePose>(F1, 0, S, pose_landmark, meas_cov));
- f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, pose_landmark, meas_cov));
- lmk1 = LandmarkBase::emplace<LandmarkBase>(problem->getMap(), "LandmarkPose",
- std::make_shared<StatePoint3d>(pos_landmark),
- std::make_shared<StateQuaternion>(Quaterniond(vquat_landmark)));
- }
-};
-
-
-TEST_F(FactorRelativePose3dWithExtrinsics_class, Constructor)
-{
- auto factor = std::make_shared<FactorRelativePose3dWithExtrinsics>(f1,
- lmk1,
- nullptr,
- false);
+int N = 1e2;
- ASSERT_TRUE(factor->getType() == "FactorRelativePose3dWithExtrinsics");
-}
+// Vectors
+Vector7d delta, x_0, x_1, x_f, x_l, x_s;
+
+// Input odometry data and covariance
+Matrix6d data_cov = (Vector6d() << 1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6).finished().asDiagonal();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 3);
+
+SolverCeres solver(problem_ptr);
+
+// Sensor
+auto sensor = problem_ptr->installSensor("SensorOdom3d", wolf_root + "/test/yaml/sensor_odom_3d.yaml", {wolf_root});
+
+// Two frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", (Vector7d() << 0,0,0,0,0,0,1).finished() );
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, "PO", (Vector7d() << 0,0,0,0,0,0,1).finished() );
+
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPose3d",
+ std::make_shared<StatePoint3d>(Vector3d::Zero()),
+ std::make_shared<StateQuaternion>(Quaterniond::Identity().coeffs()));
+
+// Capture
+auto cap1 = CaptureBase::emplace<CaptureOdom3d>(frm1, 1, sensor, Vector7d::Zero(), data_cov);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePose3dWithExtrinsicsPtr fac = nullptr;
-/////////////////////////////////////////////
-// Tree not ok with this gtest implementation
-/////////////////////////////////////////////
-TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_tree)
+void generateRandomProblemFrame()
{
- ASSERT_TRUE(problem->check(1));
-
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
- ASSERT_TRUE(problem->check(1));
+ // solver options
+ solver.getSolverOptions().max_num_iterations = 100;
+ solver.getSolverOptions().gradient_tolerance = 1e-9;
+ solver.getSolverOptions().function_tolerance = 1e-9;
+
+ if (fea)
+ fea->remove();
+
+ // Random delta
+ delta = Vector7d::Random() * 1e2;
+ delta.tail<4>().normalize();
+ auto q_delta = Quaterniond(delta.tail<4>());
+
+ // Random frame 0 pose
+ x_0 = Vector7d::Random() * 1e2;
+ x_0.tail<4>().normalize();
+ auto q_0 = Quaterniond(x_0.tail<4>());
+
+ // Random extrinsics
+ x_s = Vector7d::Random() * 1e2;
+ x_s.tail<4>().normalize();
+ auto q_s = Quaterniond(x_s.tail<4>());
+
+ // Frame 1 pose
+ auto q_1 = q_0 * q_s * q_delta * q_s.conjugate();
+ x_1.head<3>() = x_0.head<3>() + q_0 * (x_s.head<3>() + q_s * delta.head<3>()) - q_1 * x_s.head<3>();
+ x_1.tail<4>() = q_1.coeffs();
+
+ // Set states
+ frm0->setState(x_0, "PO");
+ frm1->setState(x_1, "PO");
+ cap1->setData(delta);
+ sensor->setState(x_s, "PO");
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom3d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(fea, fea, frm0, nullptr, false, TOP_MOTION); // create and add
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_P_perturbated)
+void generateRandomProblemLandmark()
{
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
-
- // unfix F1, perturbate state
- F1->unfix();
- F1->getP()->perturb();
-
- std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_POSE3d_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6);
+ // solver options
+ solver.getSolverOptions().max_num_iterations = 100;
+ solver.getSolverOptions().gradient_tolerance = 1e-9;
+ solver.getSolverOptions().function_tolerance = 1e-9;
+
+ if (fea)
+ fea->remove();
+
+ // Random delta
+ delta = Vector7d::Random() * 10;
+ delta.tail<4>().normalize();
+ auto q_delta = Quaterniond(delta.tail<4>());
+
+ // 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>());
+
+ // Landmark pose
+ x_l.head<3>() = x_f.head<3>() + q_f * (x_s.head<3>() + q_s * delta.head<3>());
+ x_l.tail<4>() = (q_f * q_s * q_delta).coeffs();
+
+ // Set states
+ frm1->setState(x_f, "PO");
+ lmk->setState(x_l, "PO");
+ cap1->setData(delta);
+ sensor->setState(x_s, "PO");
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap1, "FeatureLandmarkPose", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(fea, fea, lmk, nullptr, false, TOP_LMK); // create and add
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_F1_O_perturbated)
+void checkStatesFrame()
{
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
-
- // unfix F1, perturbate state
- F1->unfix();
- F1->getO()->perturb();
-
- std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_POSE3d_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6);
+ ASSERT_POSE3d_APPROX(frm1->getStateVector("PO"), x_1, 1e-4);
+ ASSERT_POSE3d_APPROX(frm0->getStateVector("PO"), x_0, 1e-4);
+ ASSERT_POSE3d_APPROX(sensor->getStateVector("PO"), x_s, 1e-4);
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, Check_initialization)
+void checkStatesLandmark()
{
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
+ ASSERT_POSE3d_APPROX(frm1->getStateVector("PO"), x_f, 1e-4);
+ ASSERT_POSE3d_APPROX(lmk->getStateVector("PO"), x_l, 1e-4);
+ ASSERT_POSE3d_APPROX(sensor->getStateVector("PO"), x_s, 1e-4);
+}
- ASSERT_POSE3d_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6);
- ASSERT_POSE3d_APPROX(f1->getMeasurement(), pose_landmark, 1e-6);
- ASSERT_POSE3d_APPROX(lmk1->getStateVector("PO"), pose_landmark, 1e-6);
+TEST(FactorRelativePose3dWithExtrinsics, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame1)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb frm1, fix the rest
+ frm0->fix();
+ frm1->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm1->perturb(1);
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesFrame();
+ }
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_P_perturbated)
+TEST(FactorRelativePose3dWithExtrinsics, frame_solve_frame0)
{
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
-
- // unfix lmk1, perturbate state
- lmk1->unfix();
- lmk1->getP()->perturb();
-
- std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_POSE3d_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6);
- ASSERT_POSE3d_APPROX(lmk1->getStateVector("PO"), pose_landmark, 1e-6);
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm0->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesFrame();
+ }
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_O_perturbated)
+TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_p)
{
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb sensor P, fix the rest
+ frm1->fix();
+ frm0->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);
+
+ //checkStatesFrame(); // JV: The position extrinsics in case of pair of frames is not observable
+ }
+}
- // unfix F1, perturbate state
- lmk1->unfix();
- lmk1->getO()->perturb();
+TEST(FactorRelativePose3dWithExtrinsics, frame_solve_extrinsics_o)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb sensor O, fix the rest
+ frm1->fix();
+ frm0->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);
+
+ checkStatesFrame();
+ }
+}
- std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_POSE3d_APPROX(F1->getStateVector("PO"), pose_robot, 1e-6);
- ASSERT_POSE3d_APPROX(lmk1->getStateVector("PO"), pose_landmark, 1e-6);
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb landmark, fix the rest
+ frm1->fix();
+ lmk->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ lmk->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesLandmark();
+ }
+}
+TEST(FactorRelativePose3dWithExtrinsics, landmark_solve_frame)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb frm0, fix the rest
+ frm1->unfix();
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm1->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesLandmark();
+ }
}
-TEST_F(FactorRelativePose3dWithExtrinsics_class, solve_L1_PO_perturbated)
+TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_p_solve)
{
- // Change setup
- Vector3d p_w_r, p_r_c, p_c_l, p_w_l;
- Quaterniond q_w_r, q_r_c, q_c_l, q_w_l;
- p_w_r << 1, 2, 3;
- p_r_c << 4, 5, 6;
- p_c_l << 7, 8, 9;
- q_w_r.coeffs() << 1, 2, 3, 4; q_w_r.normalize();
- q_r_c.coeffs() << 4, 5, 6, 7; q_r_c.normalize();
- q_c_l.coeffs() << 7, 8, 9, 0; q_c_l.normalize();
-
- q_w_l = q_w_r * q_r_c * q_c_l;
- p_w_l = p_w_r + q_w_r * (p_r_c + q_r_c * p_c_l);
-
- // Change feature (remove and emplace)
- Vector7d meas;
- meas << p_c_l, q_c_l.coeffs();
- f1->remove();
- f1 = std::static_pointer_cast<FeaturePose>(FeatureBase::emplace<FeaturePose>(c1, meas, meas_cov));
-
- // emplace factor
- auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(f1,
- f1,
- lmk1,
- nullptr,
- false);
-
- // Change Landmark
- lmk1->getP()->setState(p_w_l);
- lmk1->getO()->setState(q_w_l.coeffs());
- ASSERT_TRUE(lmk1->getP()->stateUpdated());
- ASSERT_TRUE(lmk1->getO()->stateUpdated());
-
- // Change Frame
- F1->getP()->setState(p_w_r);
- F1->getO()->setState(q_w_r.coeffs());
- F1->fix();
- ASSERT_TRUE(F1->getP()->stateUpdated());
- ASSERT_TRUE(F1->getO()->stateUpdated());
-
- // Change sensor extrinsics
- S->getP()->setState(p_r_c);
- S->getO()->setState(q_r_c.coeffs());
- ASSERT_TRUE(S->getP()->stateUpdated());
- ASSERT_TRUE(S->getO()->stateUpdated());
-
- Vector7d t_w_r, t_w_l;
- t_w_r << p_w_r, q_w_r.coeffs();
- t_w_l << p_w_l, q_w_l.coeffs();
- ASSERT_POSE3d_APPROX(F1->getStateVector("PO"), t_w_r, 1e-6);
- ASSERT_POSE3d_APPROX(lmk1->getStateVector("PO"), t_w_l, 1e-6);
-
- // unfix LMK, perturbate state
- lmk1->unfix();
- lmk1->perturb();
-
- std::string report = solver->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_POSE3d_APPROX(F1->getStateVector("PO").transpose(), t_w_r.transpose(), 1e-6);
- ASSERT_POSE3d_APPROX(lmk1->getStateVector("PO").transpose(), t_w_l.transpose(), 1e-6);
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb sensor P, fix the rest
+ frm1->fix();
+ lmk->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);
+
+ checkStatesLandmark();
+ }
+}
+TEST(FactorRelativePose3dWithExtrinsics, landmark_extrinsics_o_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ ASSERT_MATRIX_APPROX(fac->residual(), VectorXd::Zero(6), Constants::EPS);
+
+ // Perturb sensor O, fix the rest
+ frm1->fix();
+ lmk->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);
+
+ checkStatesLandmark();
+ }
}
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+testing::InitGoogleTest(&argc, argv);
+return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_factor_relative_position_2d.cpp b/test/gtest_factor_relative_position_2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..21446e9ac5b49ff0cb62053d751f3fe8bcbfd62b
--- /dev/null
+++ b/test/gtest_factor_relative_position_2d.cpp
@@ -0,0 +1,278 @@
+//--------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_2d.h"
+#include "core/capture/capture_void.h"
+#include "core/sensor/sensor_odom.h"
+#include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+int N = 1;//1e2
+
+// Vectors
+Vector3d x_0, x_f, x_1;
+Vector2d delta, x_l;
+
+// Input odometry data and covariance
+Matrix2d data_cov = 0.1*Matrix2d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+SolverCeres solver(problem_ptr);
+
+// Frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero() );
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, "PO", Vector3d::Zero() );
+
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition2d",
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ nullptr);
+
+// Capture
+auto cap = CaptureBase::emplace<CaptureVoid>(frm1, 1, nullptr);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePosition2dPtr fac = nullptr;
+
+void generateRandomProblemFrame()
+{
+ // solver options
+ solver.getSolverOptions().max_num_iterations = 100;
+ solver.getSolverOptions().gradient_tolerance = 1e-9;
+ solver.getSolverOptions().function_tolerance = 1e-9;
+
+ if (fea)
+ fea->remove();
+
+ // Random delta
+ delta = Vector2d::Random() * 1e2;
+
+ // Random frame 0 pose
+ x_0 = Vector3d::Random() * 1e2;
+ x_0(2) = pi2pi(x_0(2));
+
+ // Frame 1 position
+ x_1.head<2>() = x_0.head<2>() + Rotation2Dd(x_0(2)) * delta;
+
+ // Set states
+ frm0->setState(x_0, "PO");
+ frm1->setState(x_1, "PO");
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureFramePosition2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePosition2d>(fea, fea, frm0, nullptr, false, TOP_MOTION);
+}
+
+void generateRandomProblemLandmark()
+{
+ // solver options
+ solver.getSolverOptions().max_num_iterations = 100;
+ solver.getSolverOptions().gradient_tolerance = 1e-9;
+ solver.getSolverOptions().function_tolerance = 1e-9;
+
+ if (fea)
+ fea->remove();
+
+ // Random delta
+ delta = Vector2d::Random() * 1e2;
+
+ // Random frame pose
+ x_f = Vector3d::Random() * 1e2;
+ x_f(2) = pi2pi(x_f(2));
+
+ // landmark position
+ x_l = x_f.head<2>() + Rotation2Dd(x_f(2)) * delta;
+
+ // Set states
+ frm1->setState(x_f, "PO");
+ lmk->setState(x_l, "P");
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPosition2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePosition2d>(fea, fea, lmk, nullptr, false, TOP_LMK);
+}
+
+void checkStatesFrame()
+{
+ ASSERT_POSE2d_APPROX(frm0->getStateVector("PO"), x_0, 1e-4);
+ ASSERT_MATRIX_APPROX(frm1->getStateVector("P"), x_1.head<2>(), 1e-4);
+}
+
+void checkStatesLandmark()
+{
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x_f, 1e-4);
+ ASSERT_MATRIX_APPROX(lmk->getStateVector("P"), x_l, 1e-4);
+}
+
+////////////// TESTS /////////////////////////////////////////////////////////////////////
+
+TEST(FactorRelativePosition2d, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+// FRAME TO FRAME =========================================================================
+TEST(FactorRelativePosition2d, frame_solve_frame1_p)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm1, fix the rest
+ frm0->fix();
+ frm1->getP()->unfix();
+ frm1->getO()->fix();
+ frm1->getP()->perturb();
+
+ // solve for frm1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesFrame();
+ }
+}
+
+TEST(FactorRelativePosition2d, frame_solve_frame0_p)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->getP()->unfix();
+ frm0->getO()->fix();
+ frm0->getP()->perturb();
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesFrame();
+ }
+}
+
+TEST(FactorRelativePosition2d, frame_solve_frame0_o)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemFrame();
+
+ // Perturb frm0, fix the rest
+ frm1->fix();
+ frm0->getP()->fix();
+ frm0->getO()->unfix();
+ frm0->getO()->perturb();
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesFrame();
+ }
+}
+
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePosition2d, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb landmark, fix the rest
+ frm1->fix();
+ lmk->unfix();
+ lmk->perturb();
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesLandmark();
+ }
+}
+
+TEST(FactorRelativePosition2d, landmark_solve_frame_p)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm1->getP()->unfix();
+ frm1->getO()->fix();
+ lmk->fix();
+ frm1->getP()->perturb();
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesLandmark();
+ }
+}
+
+TEST(FactorRelativePosition2d, landmark_solve_frame_o)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm1->getP()->fix();
+ frm1->getO()->unfix();
+ lmk->fix();
+ frm1->getO()->perturb();
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesLandmark();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_position_2d_with_extrinsics.cpp b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..09eff15282f201e243ec00de72c31751b22f88e6
--- /dev/null
+++ b/test/gtest_factor_relative_position_2d_with_extrinsics.cpp
@@ -0,0 +1,239 @@
+//--------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_2d_with_extrinsics.h"
+#include "core/capture/capture_void.h"
+#include "core/sensor/sensor_odom.h"
+#include "core/math/rotations.h"
+#include "core/state_block/state_block_derived.h"
+#include "core/state_block/state_angle.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using std::cout;
+using std::endl;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+int N = 1;//1e2
+
+// Vectors
+Vector3d x_f, x_s;
+Vector2d delta, x_l;
+
+// Input odometry data and covariance
+Matrix2d data_cov = 0.1*Matrix2d::Identity();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("PO", 2);
+SolverCeres solver(problem_ptr);
+
+// Sensor
+auto sensor = problem_ptr->installSensor("SensorOdom2d", wolf_root + "/test/yaml/sensor_odom_2d.yaml", {wolf_root});
+
+// Frame
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", Vector3d::Zero() );
+
+// Landmark
+LandmarkBasePtr lmk = LandmarkBase::emplace<LandmarkBase>(problem_ptr->getMap(),
+ "LandmarkPosition2d",
+ std::make_shared<StatePoint2d>(Vector2d::Zero()),
+ nullptr);
+
+// Capture
+auto cap = CaptureBase::emplace<CaptureVoid>(frm, 1, sensor);
+// Feature
+FeatureBasePtr fea = nullptr;
+// Factor
+FactorRelativePosition2dWithExtrinsicsPtr fac = nullptr;
+
+void generateRandomProblemLandmark()
+{
+ // solver options
+ solver.getSolverOptions().max_num_iterations = 100;
+ solver.getSolverOptions().gradient_tolerance = 1e-9;
+ solver.getSolverOptions().function_tolerance = 1e-9;
+
+ if (fea)
+ fea->remove();
+
+ // Random delta
+ delta = Vector2d::Random() * 1e2;
+
+ // Random frame pose
+ x_f = Vector3d::Random() * 1e2;
+ x_f(2) = pi2pi(x_f(2));
+
+ // Random extrinsics
+ x_s = Vector3d::Random() * 1e2;
+ x_s(2) = pi2pi(x_s(2));
+
+ // landmark position
+ x_l = x_f.head<2>() + Rotation2Dd(x_f(2)) * (x_s.head<2>() + Rotation2Dd(x_s(2)) * delta);
+
+ // Set states
+ frm->setState(x_f, "PO");
+ lmk->setState(x_l, "P");
+ sensor->setState(x_s, "PO");
+
+ // feature & factor with delta measurement
+ fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureLandmarkPosition2d", delta, data_cov);
+ fac = FactorBase::emplace<FactorRelativePosition2dWithExtrinsics>(fea, fea, lmk, nullptr, false, TOP_LMK);
+}
+
+void checkStatesLandmark()
+{
+ ASSERT_POSE2d_APPROX(frm->getStateVector("PO"), x_f, 1e-4);
+ ASSERT_MATRIX_APPROX(lmk->getStateVector("P"), x_l, 1e-4);
+ ASSERT_POSE2d_APPROX(sensor->getStateVector("PO"), x_s, 1e-4);
+}
+
+////////////// TESTS /////////////////////////////////////////////////////////////////////
+
+TEST(FactorRelativePosition2dWithExtrinsics, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+// FRAME TO LANDMARK =========================================================================
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_lmk)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb landmark, fix the rest
+ frm->fix();
+ lmk->unfix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ lmk->perturb(1);
+
+ // solve for landmark
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesLandmark();
+ }
+}
+
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_frame_p)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm->getP()->unfix();
+ frm->getO()->fix();
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm->getP()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesLandmark();
+ }
+}
+
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_solve_frame_o)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb frm0, fix the rest
+ frm->getP()->fix();
+ frm->getO()->unfix();
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->fix();
+ frm->getO()->perturb(1);
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesLandmark();
+ }
+}
+
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_extrinsics_p_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb sensor P, fix the rest
+ frm->fix();
+ lmk->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);
+
+ checkStatesLandmark();
+ }
+}
+
+TEST(FactorRelativePosition2dWithExtrinsics, landmark_extrinsics_o_solve)
+{
+ for (int i = 0; i < N; i++)
+ {
+ // random setup
+ generateRandomProblemLandmark();
+
+ // Perturb sensor O, fix the rest
+ frm->fix();
+ lmk->fix();
+ sensor->getP()->fix();
+ sensor->getO()->unfix();
+ sensor->getO()->perturb(.2);
+
+ //std::cout << "Sensor O (perturbed): " << sensor->getO()->getState().transpose() << std::endl;
+
+ // solve for frm0
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //WOLF_INFO(report);
+
+ checkStatesLandmark();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_relative_position_3d.cpp b/test/gtest_factor_relative_position_3d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a076430e146ad53c314aec7d5bb68755651b8386
--- /dev/null
+++ b/test/gtest_factor_relative_position_3d.cpp
@@ -0,0 +1,296 @@
+//--------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, "PO", (Vector7d() << 0,0,0,0,0,0,1).finished() );
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(1, "PO", (Vector7d() << 0,0,0,0,0,0,1).finished() );
+FrameBasePtr frm2 = problem_ptr->emplaceFrame(2, "PO", (Vector7d() << 0,0,0,0,0,0,1).finished() );
+FrameBasePtr frm3 = problem_ptr->emplaceFrame(3, "PO", (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()
+{
+ // solver options
+ solver.getSolverOptions().max_num_iterations = 100;
+ solver.getSolverOptions().gradient_tolerance = 1e-9;
+ solver.getSolverOptions().function_tolerance = 1e-9;
+
+ if (cap1)
+ {
+ cap1->remove();
+ cap2->remove();
+ cap3->remove();
+ }
+
+ // 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, "PO");
+ frm1->getP()->setState(x_1, "P");
+ frm2->getP()->setState(x_2, "P");
+ frm3->getP()->setState(x_3, "P");
+
+ // 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()
+{
+ // solver options
+ solver.getSolverOptions().max_num_iterations = 100;
+ solver.getSolverOptions().gradient_tolerance = 1e-9;
+ solver.getSolverOptions().function_tolerance = 1e-9;
+
+ if (cap1)
+ {
+ cap1->remove();
+ cap2->remove();
+ cap3->remove();
+ }
+
+ // 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, "PO");
+ lmk1->setState(x_1, "P");
+ lmk2->setState(x_2, "P");
+ lmk3->setState(x_3, "P");
+
+ // 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);
+}
+
+void checkStatesFrame()
+{
+ ASSERT_POSE3d_APPROX(frm->getStateVector("PO"), x_f, 1e-3);
+ 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);
+}
+
+void checkStatesLandmark()
+{
+ ASSERT_POSE3d_APPROX(frm->getStateVector("PO"), x_f, 1e-3);
+ 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);
+}
+
+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);
+
+ checkStatesFrame();
+ }
+}
+
+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);
+
+ checkStatesFrame();
+ }
+}
+
+// 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);
+
+ checkStatesLandmark();
+ }
+}
+
+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);
+
+ checkStatesLandmark();
+ }
+}
+
+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..2425633f4d64310b86f79287b054cc82769e6f95
--- /dev/null
+++ b/test/gtest_factor_relative_position_3d_with_extrinsics.cpp
@@ -0,0 +1,266 @@
+//--------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.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", wolf_root + "/test/yaml/sensor_odom_3d.yaml", {wolf_root});
+
+// Frame
+FrameBasePtr frm = problem_ptr->emplaceFrame(0, "PO", (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()
+{
+ // solver options
+ solver.getSolverOptions().max_num_iterations = 100;
+ solver.getSolverOptions().gradient_tolerance = 1e-9;
+ solver.getSolverOptions().function_tolerance = 1e-9;
+
+ if (fea1)
+ {
+ fea1->remove();
+ fea2->remove();
+ fea3->remove();
+ }
+
+ // 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, "PO");
+ lmk1->setState(x_l1, "P");
+ lmk2->setState(x_l2, "P");
+ lmk3->setState(x_l3, "P");
+ sensor->setState(x_s, "PO");
+
+ // 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);
+}
+
+void checkStatesLandmark()
+{
+ ASSERT_POSE3d_APPROX(frm->getStateVector("PO"), x_f, 1e-3);
+ ASSERT_MATRIX_APPROX(lmk1->getStateVector("P"), x_l1, 1e-3);
+ ASSERT_MATRIX_APPROX(lmk2->getStateVector("P"), x_l2, 1e-3);
+ ASSERT_MATRIX_APPROX(lmk3->getStateVector("P"), x_l3, 1e-3);
+ ASSERT_POSE3d_APPROX(sensor->getStateVector("PO"), x_s, 1e-3);
+}
+
+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);
+
+ checkStatesLandmark();
+ }
+}
+
+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);
+
+ checkStatesLandmark();
+ }
+}
+
+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);
+
+ checkStatesLandmark();
+ }
+}
+
+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);
+
+ checkStatesLandmark();
+ }
+}
+
+int main(int argc, char **argv)
+{
+testing::InitGoogleTest(&argc, argv);
+return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp
index 8898eda8a5426e8170e56de5254b49d87cbf404e..ebc8ddcb56672022b858f5b93620e32b182bea7b 100644
--- a/test/gtest_map_yaml.cpp
+++ b/test/gtest_map_yaml.cpp
@@ -41,8 +41,6 @@
using namespace wolf;
using namespace Eigen;
-std::string map_path = std::string(_WOLF_ROOT_DIR) + "/test/yaml";
-
Vector2d p1_2d = Vector2d::Random();
Vector2d p2_2d = Vector2d::Random();
Vector2d p3_2d = Vector2d::Random();
@@ -58,6 +56,9 @@ Vector4d o3_3d = Vector4d::Random().normalized();
int int_param = (int) (Vector1d::Random()(0) * 1e4);
double double_param = Vector1d::Random()(0) * 1e4;
+std::string wolf_root = _WOLF_ROOT_DIR;
+std::string map_path = wolf_root + "/test/yaml/maps";
+
TEST(MapYaml, save_2d)
{
ProblemPtr problem = Problem::create("PO", 2);
diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..04a42b83ed0f4fe461378a3f651b34120b7c03b0
--- /dev/null
+++ b/test/gtest_processor_landmark_external.cpp
@@ -0,0 +1,521 @@
+//--------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/problem/problem.h"
+#include "core/capture/capture_landmarks_external.h"
+#include "core/landmark/landmark_base.h"
+#include "core/sensor/sensor_odom.h"
+#include "core/processor/processor_odom_2d.h"
+#include "core/processor/processor_odom_3d.h"
+#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_landmark_external.h"
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+std::string wolf_root = _WOLF_ROOT_DIR;
+
+class ProcessorLandmarkExternalTest : public testing::Test
+{
+ protected:
+ int dim;
+ bool orientation;
+ TimeStamp t;
+ double dt;
+
+ ProblemPtr problem;
+ SolverManagerPtr solver;
+ SensorBasePtr sensor, sensor_odom;
+ ProcessorLandmarkExternalPtr processor;
+ ProcessorMotionPtr processor_motion;
+ std::vector<LandmarkBasePtr> landmarks;
+
+ // Groundtruth states
+ VectorComposite state_robot, state_sensor;
+ std::vector<VectorComposite> state_landmarks;
+
+ virtual void SetUp() {};
+ void initProblem(int _dim,
+ bool _orientation,
+ double _quality_th,
+ double _dist_th,
+ unsigned int _track_length_th,
+ double _time_span,
+ bool _add_landmarks);
+ void randomStep();
+ CaptureLandmarksExternalPtr computeCaptureLandmarks() const;
+ void testConfiguration(int dim,
+ bool orientation,
+ double quality_th,
+ double dist_th,
+ int track_length,
+ double time_span,
+ bool add_landmarks);
+ void assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const;
+};
+
+void ProcessorLandmarkExternalTest::initProblem(int _dim,
+ bool _orientation,
+ double _quality_th,
+ double _dist_th,
+ unsigned int _track_length_th,
+ double _time_span,
+ bool _add_landmarks)
+{
+ dim = _dim;
+ orientation = _orientation;
+ t = TimeStamp(0);
+ dt = 0.1;
+
+ problem = Problem::create("PO", dim);
+ solver = std::make_shared<SolverCeres>(problem);
+
+ // Sensors
+ sensor = problem->installSensor("SensorOdom" + toString(dim) + "d",
+ wolf_root + "/test/yaml/sensor_odom_" + toString(dim) + "d.yaml",
+ {wolf_root});
+ sensor_odom = problem->installSensor("SensorOdom" + toString(dim) + "d",
+ wolf_root + "/test/yaml/sensor_odom_" + toString(dim) + "d.yaml",
+ {wolf_root});
+ // sensor random extrinsics
+ VectorXd sensor_state;
+ if (dim == 2)
+ {
+ sensor_state = Vector3d::Random();
+ sensor_state(2) *= M_PI;
+ }
+ else
+ {
+ sensor_state = Vector7d::Random();
+ sensor_state.tail<4>().normalize();
+ }
+ sensor->setState(sensor_state, "PO");
+
+ // Processors
+ ParamsProcessorLandmarkExternalPtr params = std::make_shared<ParamsProcessorLandmarkExternal>();
+ params->time_tolerance = dt/2;
+ params->max_new_features = -1;
+ params->voting_active = true;
+ params->apply_loss_function = false;
+ params->use_orientation = orientation;
+ params->filter_quality_th = _quality_th;
+ params->match_dist_th = _dist_th;
+ params->min_features_for_keyframe = 1;
+ params->new_features_for_keyframe = 1000;
+ params->filter_track_length_th = _track_length_th;
+ params->time_span = _time_span;
+ processor = ProcessorBase::emplace<ProcessorLandmarkExternal>(sensor, params);
+
+ if (dim == 2)
+ {
+ auto params_odom = std::make_shared<ParamsProcessorOdom2d>();
+ params_odom->state_provider = true;
+ params_odom->voting_active = false;
+ processor_motion = std::static_pointer_cast<ProcessorMotion>(ProcessorBase::emplace<ProcessorOdom2d>(sensor_odom,
+ params_odom));
+ }
+ else
+ {
+ auto params_odom = std::make_shared<ParamsProcessorOdom3d>();
+ params_odom->state_provider = true;
+ params_odom->voting_active = false;
+ processor_motion = std::static_pointer_cast<ProcessorMotion>(ProcessorBase::emplace<ProcessorOdom3d>(sensor_odom,
+ params_odom));
+ }
+
+ // Emplace frame
+ auto F0 = problem->emplaceFrame(t, "PO", (dim == 2 ? VectorXd::Zero(3) : (VectorXd(7) << 0,0,0,0,0,0,1).finished()));
+ F0->fix();
+ processor->keyFrameCallback(F0);
+ processor_motion->setOrigin(F0);
+
+ // Emplace 3 random landmarks
+ for (auto i = 0; i < 3; i++)
+ {
+ LandmarkBasePtr lmk;
+ if (dim == 2)
+ lmk = LandmarkBase::emplace<LandmarkBase>(_add_landmarks ? problem->getMap() : nullptr,
+ "LandmarkExternal",
+ std::make_shared<StatePoint2d>(Vector2d::Random() * 10),
+ (orientation ?
+ std::make_shared<StateAngle>(Vector1d::Random() * M_PI) :
+ nullptr));
+
+ else
+ lmk = LandmarkBase::emplace<LandmarkBase>(_add_landmarks ? problem->getMap() : nullptr,
+ "LandmarkExternal",
+ std::make_shared<StatePoint3d>(Vector3d::Random() * 10),
+ (orientation ?
+ std::make_shared<StateQuaternion>(Vector4d::Random().normalized()) :
+ nullptr));
+ lmk->setId(i);
+ landmarks.push_back(lmk);
+ state_landmarks.push_back(lmk->getState());
+ }
+
+ // Store groundtruth poses
+ state_robot = F0->getState("PO");
+ state_sensor = sensor->getState("PO");
+}
+
+void ProcessorLandmarkExternalTest::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, delta_cov, nullptr);
+ cap_odom->process();
+
+ // update groundtruth pose with random movement
+ state_robot = processor_motion->getState("PO");
+}
+
+CaptureLandmarksExternalPtr ProcessorLandmarkExternalTest::computeCaptureLandmarks() const
+{
+ // Detections
+ auto cap = std::make_shared<CaptureLandmarksExternal>(t, sensor);
+ for (auto i = 0; i < landmarks.size(); i++)
+ {
+ // compute detection
+ VectorXd meas(orientation?(dim==2?3:7):(dim==2?2:3));
+ if (dim == 2)
+ {
+ meas.head(2) = Rotation2Dd(-state_sensor.at('O')(0))*(Rotation2Dd(-state_robot.at('O')(0))*(state_landmarks.at(i).at('P') - state_robot.at('P')) - state_sensor.at('P'));
+ if (orientation)
+ meas(2) = state_landmarks.at(i).at('O')(0) - state_robot.at('O')(0) - state_sensor.at('O')(0);
+ }
+ else
+ {
+ auto q_sensor = Quaterniond(Vector4d(state_sensor.at('O')));
+ auto q_robot = Quaterniond(Vector4d(state_robot.at('O')));
+
+ meas.head(3) = q_sensor.conjugate() * (q_robot.conjugate() * (state_landmarks.at(i).at('P') - state_robot.at('P')) - state_sensor.at('P'));
+ if (orientation)
+ meas.tail(4) = (q_sensor.conjugate() * q_robot.conjugate() * Quaterniond(Vector4d(state_landmarks.at(i).at('O')))).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, quality );
+ }
+
+ return cap;
+}
+
+void ProcessorLandmarkExternalTest::testConfiguration(int dim,
+ bool orientation,
+ double quality_th,
+ double dist_th,
+ int track_length,
+ double time_span,
+ bool add_landmarks)
+{
+ initProblem(dim, orientation, quality_th, dist_th, track_length, time_span, add_landmarks);
+
+ ASSERT_TRUE(problem->check());
+
+ for (auto i = 0; i<10; i++)
+ {
+ WOLF_INFO("\n================= STEP ", i, " t = ", t, " =================");
+
+ // detection of landmarks
+ auto cap = computeCaptureLandmarks();
+ ASSERT_TRUE(problem->check());
+
+ // process detections
+ cap->process();
+ ASSERT_TRUE(problem->check());
+ problem->print(4,1,1,1);
+
+ // CHECKS
+ FactorBasePtrList fac_list;
+ problem->getTrajectory()->getFactorList(fac_list);
+ // 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
+ ASSERT_TRUE(problem->getTrajectory()->size() > 1);
+
+ // emplaced factors
+ ASSERT_FALSE(fac_list.empty());
+
+ // factors' type
+ if (should_emplace_KF)
+ for (auto fac : fac_list)
+ {
+ if (fac->getProcessor() == processor)
+ {
+ ASSERT_EQ(fac->getType(), std::string("FactorRelative") +
+ (orientation ? "Pose" : "Position") +
+ (dim == 2 ? "2d" : "3d") +
+ "WithExtrinsics");
+ }
+ }
+ // landmarks created by processor
+ if (not add_landmarks)
+ {
+ 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_TRUE(lmk_map->id() < landmarks.size());
+ ASSERT_EQ(lmk_map->id(), landmarks.at(lmk_map->id())->id());
+ assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
+ }
+ }
+ }
+ else
+ {
+ // didn't vote for keyframe
+ ASSERT_FALSE(problem->getTrajectory()->size() > 1);
+ // no factors emplaced
+ ASSERT_TRUE(fac_list.empty());
+ // no landmarks created yet (if not added by the test)
+ ASSERT_EQ(problem->getMap()->getLandmarkList().empty(), not add_landmarks);
+ }
+
+ // step with random movement
+ t+=dt;
+ randomStep();
+
+ // solve
+ if (should_emplace_KF)
+ {
+ // perturb landmarks
+ for (auto lmk : problem->getMap()->getLandmarkList())
+ lmk->perturb();
+
+ // fix frame and extrinsics
+ sensor->fix();
+ problem->getLastFrame()->fix();
+
+ // solve
+ auto report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ WOLF_INFO(report);
+
+ // check values
+ //assertVectorComposite(sensor->getState("PO"), state_sensor);
+ //assertVectorComposite(problem->getState("PO"), state_robot);
+ for (auto lmk_map : problem->getMap()->getLandmarkList())
+ {
+ assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id()));
+ }
+ }
+ }
+}
+
+void ProcessorLandmarkExternalTest::assertVectorComposite(const VectorComposite& v1, const VectorComposite& v2) const
+{
+ if (v1.has("PO") and v2.has("PO"))
+ {
+ if (dim == 2)
+ {
+ ASSERT_POSE2d_APPROX(v1.vector("PO"), v2.vector("PO"), Constants::EPS);
+ }
+ else
+ {
+ ASSERT_POSE3d_APPROX(v1.vector("PO"), v2.vector("PO"), Constants::EPS);
+ }
+ }
+ else if (v1.has("P") and v2.has("P"))
+ {
+ ASSERT_MATRIX_APPROX(v1.vector("P"), v2.vector("P"), Constants::EPS);
+ }
+ else
+ throw std::runtime_error("wrong vector composite");
+}
+
+// / TESTS //////////////////////////////////////////
+TEST_F(ProcessorLandmarkExternalTest, 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(ProcessorLandmarkExternalTest, 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
+}
+
+TEST_F(ProcessorLandmarkExternalTest, 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(ProcessorLandmarkExternalTest, 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
+}
+
+TEST_F(ProcessorLandmarkExternalTest, 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(ProcessorLandmarkExternalTest, 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
+}
+
+TEST_F(ProcessorLandmarkExternalTest, 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
+}
+
+TEST_F(ProcessorLandmarkExternalTest, 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(ProcessorLandmarkExternalTest, 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
+}
+
+TEST_F(ProcessorLandmarkExternalTest, 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
+}
+
+TEST_F(ProcessorLandmarkExternalTest, 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(ProcessorLandmarkExternalTest, 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)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/yaml/maps/map_2d_manual.yaml b/test/yaml/maps/map_2d_manual.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4a2276ad153b4adfbae3679d42b1aba801f15bb3
--- /dev/null
+++ b/test/yaml/maps/map_2d_manual.yaml
@@ -0,0 +1,13 @@
+map_name: 2d map saved from Map::save()
+date_time: 21/09/2022 at 12:00:59
+useless_parameter: [1, 6, 3]
+landmarks:
+ -
+ id: 1
+ type: LandmarkBase
+ position: [1, 2]
+ position fixed: false
+ transformable: true
+ another_useless_param: "wtf"
+ - {id: 2, type: LandmarkBase, position: [3, 4], position fixed: false, orientation: [2], orientation fixed: false, transformable: true}
+ - {id: 3, type: LandmarkBase, position: [5, 6], position fixed: true, orientation: [3], orientation fixed: true, transformable: true}
\ No newline at end of file
diff --git a/test/yaml/maps/map_3d_manual.yaml b/test/yaml/maps/map_3d_manual.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4b66ed90f51224d03b70cf484ac333da74e777f4
--- /dev/null
+++ b/test/yaml/maps/map_3d_manual.yaml
@@ -0,0 +1,25 @@
+#map_name: 3d map saved from Problem::saveMap()
+#date_time: 21/09/2022 at 12:00:59
+landmarks:
+ -
+ id: 10
+ type: LandmarkBase
+ position: [1, 2, 3]
+ position fixed: false
+ transformable: true
+ -
+ id: 11
+ type: LandmarkBase
+ position: [4, 5, 6]
+ position fixed: false
+ orientation: [0, 1, 0, 0]
+ orientation fixed: false
+ transformable: true
+ -
+ id: 12
+ type: LandmarkBase
+ position: [7, 8, 9]
+ position fixed: true
+ orientation: [0, 0, 1, 0]
+ orientation fixed: true
+ transformable: true
\ No newline at end of file