From 079a72040b60b57b9fee5cdcd5dfa2de12b5740a Mon Sep 17 00:00:00 2001
From: Joaquim Casals <jcasals@iri.upc.edu>
Date: Fri, 3 May 2019 09:29:17 +0200
Subject: [PATCH] Cleaned some files that shouldn't be in core
---
CMakeLists.txt | 17 +-
include/base/capture/capture_GPS_fix.h | 30 -
include/base/capture/capture_IMU.h | 42 -
include/base/factor/factor_AHP.h | 200 --
include/base/factor/factor_GPS_2D.h | 41 -
.../base/factor/factor_GPS_pseudorange_2D.h | 241 --
.../base/factor/factor_GPS_pseudorange_3D.h | 152 -
include/base/factor/factor_IMU.h | 498 ---
.../base/factor/factor_autodiff_trifocal.h | 397 ---
include/base/factor/factor_corner_2D.h | 124 -
include/base/factor/factor_point_2D.h | 140 -
include/base/factor/factor_point_to_line_2D.h | 149 -
include/base/feature/feature_GPS_fix.h | 31 -
.../base/feature/feature_GPS_pseudorange.h | 36 -
include/base/feature/feature_IMU.h | 76 -
include/base/feature/feature_corner_2D.h | 32 -
include/base/feature/feature_line_2D.h | 53 -
include/base/feature/feature_polyline_2D.h | 78 -
include/base/landmark/landmark_container.h | 107 -
include/base/landmark/landmark_corner_2D.h | 41 -
include/base/landmark/landmark_line_2D.h | 56 -
include/base/landmark/landmark_polyline_2D.h | 191 --
include/base/math/IMU_tools.h | 610 ----
include/base/processor/processor_IMU.h | 85 -
.../processor_tracker_landmark_dummy.h | 93 -
include/base/sensor/sensor_GPS.h | 47 -
include/base/sensor/sensor_GPS_fix.h | 37 -
include/base/sensor/sensor_IMU.h | 98 -
src/CMakeLists.txt | 762 -----
src/capture/CMakeLists.txt | 26 -
src/capture/capture_IMU.cpp | 33 -
src/capture/capture_laser_2D.cpp | 11 -
src/feature/feature_GPS_fix.cpp | 16 -
src/feature/feature_GPS_pseudorange.cpp | 32 -
src/feature/feature_IMU.cpp | 33 -
src/feature/feature_corner_2D.cpp | 22 -
src/feature/feature_line_2D.cpp | 20 -
src/feature/feature_polyline_2D.cpp | 13 -
src/landmark/CMakeLists.txt | 24 -
src/landmark/landmark_container.cpp | 192 --
src/landmark/landmark_corner_2D.cpp | 22 -
src/landmark/landmark_line_2D.cpp | 58 -
src/landmark/landmark_polyline_2D.cpp | 397 ---
src/processor/CMakeLists.txt | 43 -
src/processor/processor_IMU.cpp | 338 --
.../processor_tracker_landmark_dummy.cpp | 104 -
src/sensor/CMakeLists.txt | 27 -
src/sensor/sensor_GPS.cpp | 56 -
src/sensor/sensor_GPS_fix.cpp | 43 -
src/sensor/sensor_IMU.cpp | 49 -
src/yaml/processor_IMU_yaml.cpp | 51 -
...rocessor_tracker_feature_trifocal_yaml.cpp | 75 -
...ocessor_tracker_landmark_apriltag_yaml.cpp | 94 -
src/yaml/sensor_IMU_yaml.cpp | 54 -
src/yaml/sensor_camera_yaml.cpp | 132 -
src/yaml/sensor_laser_2D_yaml.cpp | 35 -
test/data/IMU/imu_static_biasNull.txt~ | 1003 ------
test/gtest_IMU.cpp | 1531 ---------
test/gtest_IMU_tools.cpp | 629 ----
test/gtest_factor_IMU.cpp | 2840 -----------------
test/gtest_factor_autodiff_apriltag.cpp | 413 ---
test/gtest_factor_autodiff_trifocal.cpp | 948 ------
test/gtest_feature_IMU.cpp | 167 -
test/gtest_feature_apriltag.cpp | 119 -
test/gtest_landmark_apriltag.cpp | 87 -
test/gtest_processor_IMU.cpp | 1064 ------
test/gtest_processor_IMU_jacobians.cpp | 550 ----
...est_processor_tracker_feature_trifocal.cpp | 153 -
...st_processor_tracker_landmark_apriltag.cpp | 413 ---
test/gtest_sensor_camera.cpp | 123 -
test/processor_IMU_UnitTester.cpp | 13 -
test/processor_IMU_UnitTester.h | 379 ---
72 files changed, 2 insertions(+), 16894 deletions(-)
delete mode 100644 include/base/capture/capture_GPS_fix.h
delete mode 100644 include/base/capture/capture_IMU.h
delete mode 100644 include/base/factor/factor_AHP.h
delete mode 100644 include/base/factor/factor_GPS_2D.h
delete mode 100644 include/base/factor/factor_GPS_pseudorange_2D.h
delete mode 100644 include/base/factor/factor_GPS_pseudorange_3D.h
delete mode 100644 include/base/factor/factor_IMU.h
delete mode 100644 include/base/factor/factor_autodiff_trifocal.h
delete mode 100644 include/base/factor/factor_corner_2D.h
delete mode 100644 include/base/factor/factor_point_2D.h
delete mode 100644 include/base/factor/factor_point_to_line_2D.h
delete mode 100644 include/base/feature/feature_GPS_fix.h
delete mode 100644 include/base/feature/feature_GPS_pseudorange.h
delete mode 100644 include/base/feature/feature_IMU.h
delete mode 100644 include/base/feature/feature_corner_2D.h
delete mode 100644 include/base/feature/feature_line_2D.h
delete mode 100644 include/base/feature/feature_polyline_2D.h
delete mode 100644 include/base/landmark/landmark_container.h
delete mode 100644 include/base/landmark/landmark_corner_2D.h
delete mode 100644 include/base/landmark/landmark_line_2D.h
delete mode 100644 include/base/landmark/landmark_polyline_2D.h
delete mode 100644 include/base/math/IMU_tools.h
delete mode 100644 include/base/processor/processor_IMU.h
delete mode 100644 include/base/processor/processor_tracker_landmark_dummy.h
delete mode 100644 include/base/sensor/sensor_GPS.h
delete mode 100644 include/base/sensor/sensor_GPS_fix.h
delete mode 100644 include/base/sensor/sensor_IMU.h
delete mode 100644 src/CMakeLists.txt
delete mode 100644 src/capture/CMakeLists.txt
delete mode 100644 src/capture/capture_IMU.cpp
delete mode 100644 src/capture/capture_laser_2D.cpp
delete mode 100644 src/feature/feature_GPS_fix.cpp
delete mode 100644 src/feature/feature_GPS_pseudorange.cpp
delete mode 100644 src/feature/feature_IMU.cpp
delete mode 100644 src/feature/feature_corner_2D.cpp
delete mode 100644 src/feature/feature_line_2D.cpp
delete mode 100644 src/feature/feature_polyline_2D.cpp
delete mode 100644 src/landmark/CMakeLists.txt
delete mode 100644 src/landmark/landmark_container.cpp
delete mode 100644 src/landmark/landmark_corner_2D.cpp
delete mode 100644 src/landmark/landmark_line_2D.cpp
delete mode 100644 src/landmark/landmark_polyline_2D.cpp
delete mode 100644 src/processor/CMakeLists.txt
delete mode 100644 src/processor/processor_IMU.cpp
delete mode 100644 src/processor/processor_tracker_landmark_dummy.cpp
delete mode 100644 src/sensor/CMakeLists.txt
delete mode 100644 src/sensor/sensor_GPS.cpp
delete mode 100644 src/sensor/sensor_GPS_fix.cpp
delete mode 100644 src/sensor/sensor_IMU.cpp
delete mode 100644 src/yaml/processor_IMU_yaml.cpp
delete mode 100644 src/yaml/processor_tracker_feature_trifocal_yaml.cpp
delete mode 100644 src/yaml/processor_tracker_landmark_apriltag_yaml.cpp
delete mode 100644 src/yaml/sensor_IMU_yaml.cpp
delete mode 100644 src/yaml/sensor_camera_yaml.cpp
delete mode 100644 src/yaml/sensor_laser_2D_yaml.cpp
delete mode 100644 test/data/IMU/imu_static_biasNull.txt~
delete mode 100644 test/gtest_IMU.cpp
delete mode 100644 test/gtest_IMU_tools.cpp
delete mode 100644 test/gtest_factor_IMU.cpp
delete mode 100644 test/gtest_factor_autodiff_apriltag.cpp
delete mode 100644 test/gtest_factor_autodiff_trifocal.cpp
delete mode 100644 test/gtest_feature_IMU.cpp
delete mode 100644 test/gtest_feature_apriltag.cpp
delete mode 100644 test/gtest_landmark_apriltag.cpp
delete mode 100644 test/gtest_processor_IMU.cpp
delete mode 100644 test/gtest_processor_IMU_jacobians.cpp
delete mode 100644 test/gtest_processor_tracker_feature_trifocal.cpp
delete mode 100644 test/gtest_processor_tracker_landmark_apriltag.cpp
delete mode 100644 test/gtest_sensor_camera.cpp
delete mode 100644 test/processor_IMU_UnitTester.cpp
delete mode 100644 test/processor_IMU_UnitTester.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0809d9a08..fc9c17868 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -233,7 +233,6 @@ SET(HDRS_COMMON
include/base/common/wolf.h
)
SET(HDRS_MATH
- include/base/math/IMU_tools.h
include/base/math/SE3.h
include/base/math/pinhole_tools.h
include/base/math/rotations.h
@@ -289,8 +288,6 @@ SET(HDRS_FACTOR
include/base/factor/factor_autodiff_distance_3D.h
include/base/factor/factor_base.h
include/base/factor/factor_block_absolute.h
- include/base/factor/factor_container.h
- include/base/factor/factor_corner_2D.h
include/base/factor/factor_diff_drive.h
include/base/factor/factor_epipolar.h
include/base/factor/factor_fix_bias.h
@@ -304,7 +301,6 @@ SET(HDRS_FACTOR
)
SET(HDRS_FEATURE
include/base/feature/feature_base.h
- include/base/feature/feature_corner_2D.h
include/base/feature/feature_diff_drive.h
include/base/feature/feature_match.h
include/base/feature/feature_odom_2D.h
@@ -312,16 +308,12 @@ SET(HDRS_FEATURE
)
SET(HDRS_LANDMARK
include/base/landmark/landmark_base.h
- include/base/landmark/landmark_container.h
- include/base/landmark/landmark_corner_2D.h
- include/base/landmark/landmark_line_2D.h
include/base/landmark/landmark_match.h
)
SET(HDRS_PROCESSOR
include/base/processor/diff_drive_tools.h
include/base/processor/diff_drive_tools.hpp
include/base/processor/motion_buffer.h
- include/base/processor/processor_IMU.h
include/base/processor/processor_base.h
include/base/processor/processor_capture_holder.h
include/base/processor/processor_diff_drive.h
@@ -336,7 +328,7 @@ SET(HDRS_PROCESSOR
include/base/processor/processor_tracker_feature.h
include/base/processor/processor_tracker_feature_dummy.h
include/base/processor/processor_tracker_landmark.h
- include/base/processor/processor_tracker_landmark_dummy.h
+ # include/base/processor/processor_tracker_landmark_dummy.h
include/base/processor/track_matrix.h
)
SET(HDRS_SENSOR
@@ -390,7 +382,6 @@ SET(SRCS_UTILS
)
SET(SRCS_CAPTURE
- src/capture/capture_IMU.cpp
src/capture/capture_base.cpp
src/capture/capture_motion.cpp
src/capture/capture_odom_2D.cpp
@@ -406,16 +397,12 @@ SET(SRCS_FACTOR
)
SET(SRCS_FEATURE
src/feature/feature_base.cpp
- src/feature/feature_corner_2D.cpp
src/feature/feature_diff_drive.cpp
src/feature/feature_odom_2D.cpp
src/feature/feature_pose.cpp
)
SET(SRCS_LANDMARK
src/landmark/landmark_base.cpp
- src/landmark/landmark_container.cpp
- src/landmark/landmark_corner_2D.cpp
- src/landmark/landmark_line_2D.cpp
)
SET(SRCS_PROCESSOR
src/processor/motion_buffer.cpp
@@ -431,7 +418,7 @@ SET(SRCS_PROCESSOR
src/processor/processor_tracker_feature.cpp
src/processor/processor_tracker_feature_dummy.cpp
src/processor/processor_tracker_landmark.cpp
- src/processor/processor_tracker_landmark_dummy.cpp
+ # src/processor/processor_tracker_landmark_dummy.cpp
src/processor/track_matrix.cpp
)
SET(SRCS_SENSOR
diff --git a/include/base/capture/capture_GPS_fix.h b/include/base/capture/capture_GPS_fix.h
deleted file mode 100644
index 8cb4afbf4..000000000
--- a/include/base/capture/capture_GPS_fix.h
+++ /dev/null
@@ -1,30 +0,0 @@
-#ifndef CAPTURE_GPS_FIX_H_
-#define CAPTURE_GPS_FIX_H_
-
-//Wolf includes
-#include "base/feature/feature_GPS_fix.h"
-#include "base/capture/capture_base.h"
-
-//std includes
-//
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureGPSFix);
-
-//class CaptureGPSFix
-class CaptureGPSFix : public CaptureBase
-{
- protected:
- Eigen::VectorXs data_; ///< Raw data.
- Eigen::MatrixXs data_covariance_; ///< Noise of the capture.
-
- public:
- CaptureGPSFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data);
- CaptureGPSFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
- virtual ~CaptureGPSFix();
- virtual void process();
-};
-
-} //namespace wolf
-#endif
diff --git a/include/base/capture/capture_IMU.h b/include/base/capture/capture_IMU.h
deleted file mode 100644
index 9308c1bc0..000000000
--- a/include/base/capture/capture_IMU.h
+++ /dev/null
@@ -1,42 +0,0 @@
-#ifndef CAPTURE_IMU_H
-#define CAPTURE_IMU_H
-
-//Wolf includes
-#include "base/math/IMU_tools.h"
-#include "base/capture/capture_motion.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureIMU);
-
-class CaptureIMU : public CaptureMotion
-{
- public:
-
- CaptureIMU(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector6s& _data,
- const Eigen::MatrixXs& _data_cov,
- FrameBasePtr _origin_frame_ptr = nullptr);
-
- CaptureIMU(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector6s& _data,
- const Eigen::MatrixXs& _data_cov,
- const Vector6s& _bias,
- FrameBasePtr _origin_frame_ptr = nullptr);
-
- virtual ~CaptureIMU();
-
- virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) override;
-
-};
-
-inline Eigen::VectorXs CaptureIMU::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error)
-{
- return imu::plus(_delta, _delta_error);
-}
-
-} // namespace wolf
-
-#endif // CAPTURE_IMU_H
diff --git a/include/base/factor/factor_AHP.h b/include/base/factor/factor_AHP.h
deleted file mode 100644
index efb0837c8..000000000
--- a/include/base/factor/factor_AHP.h
+++ /dev/null
@@ -1,200 +0,0 @@
-#ifndef FACTOR_AHP_H
-#define FACTOR_AHP_H
-
-//Wolf includes
-#include "base/factor/factor_autodiff.h"
-#include "base/landmark/landmark_AHP.h"
-#include "base/sensor/sensor_camera.h"
-//#include "base/feature/feature_point_image.h"
-#include "base/math/pinhole_tools.h"
-
-#include <iomanip> //setprecision
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorAHP);
-
-//class
-class FactorAHP : public FactorAutodiff<FactorAHP, 2, 3, 4, 3, 4, 4>
-{
- protected:
- Eigen::Vector3s anchor_sensor_extrinsics_p_;
- Eigen::Vector4s anchor_sensor_extrinsics_o_;
- Eigen::Vector4s intrinsic_;
- Eigen::VectorXs distortion_;
-
- public:
-
- FactorAHP(const FeatureBasePtr& _ftr_ptr,
- const LandmarkAHPPtr& _landmark_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE);
-
- virtual ~FactorAHP() = default;
-
- template<typename T>
- void expectation(const T* const _current_frame_p,
- const T* const _current_frame_o,
- const T* const _anchor_frame_p,
- const T* const _anchor_frame_o,
- const T* const _lmk_hmg,
- T* _expectation) const;
-
- Eigen::VectorXs expectation() const;
-
- template<typename T>
- bool operator ()(const T* const _current_frame_p,
- const T* const _current_frame_o,
- const T* const _anchor_frame_p,
- const T* const _anchor_frame_o,
- const T* const _lmk_hmg,
- T* _residuals) const;
-
- // Static creator method
- static FactorAHPPtr create(const FeatureBasePtr& _ftr_ptr,
- const LandmarkAHPPtr& _lmk_ahp_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE);
-
-};
-
-inline FactorAHP::FactorAHP(const FeatureBasePtr& _ftr_ptr,
- const LandmarkAHPPtr& _landmark_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff<FactorAHP, 2, 3, 4, 3, 4, 4>("AHP",
- _landmark_ptr->getAnchorFrame(),
- nullptr,
- nullptr,
- _landmark_ptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _ftr_ptr->getCapture()->getFrame()->getP(),
- _ftr_ptr->getCapture()->getFrame()->getO(),
- _landmark_ptr->getAnchorFrame()->getP(),
- _landmark_ptr->getAnchorFrame()->getO(),
- _landmark_ptr->getP()),
- anchor_sensor_extrinsics_p_(_ftr_ptr->getCapture()->getSensorP()->getState()),
- anchor_sensor_extrinsics_o_(_ftr_ptr->getCapture()->getSensorO()->getState()),
- intrinsic_(_ftr_ptr->getCapture()->getSensor()->getIntrinsic()->getState())
-{
- // obtain some intrinsics from provided sensor
- distortion_ = (std::static_pointer_cast<SensorCamera>(_ftr_ptr->getCapture()->getSensor()))->getDistortionVector();
-}
-
-inline Eigen::VectorXs FactorAHP::expectation() const
-{
- FrameBasePtr frm_current = getFeature()->getCapture()->getFrame();
- FrameBasePtr frm_anchor = getFrameOther();
- LandmarkBasePtr lmk = getLandmarkOther();
-
- const Eigen::MatrixXs frame_current_pos = frm_current->getP()->getState();
- const Eigen::MatrixXs frame_current_ori = frm_current->getO()->getState();
- const Eigen::MatrixXs frame_anchor_pos = frm_anchor ->getP()->getState();
- const Eigen::MatrixXs frame_anchor_ori = frm_anchor ->getO()->getState();
- const Eigen::MatrixXs lmk_pos_hmg = lmk ->getP()->getState();
-
- Eigen::Vector2s exp;
- expectation(frame_current_pos.data(), frame_current_ori.data(), frame_anchor_pos.data(),
- frame_anchor_ori.data(), lmk_pos_hmg.data(), exp.data());
-
- return exp;
-}
-
-template<typename T>
-inline void FactorAHP::expectation(const T* const _current_frame_p,
- const T* const _current_frame_o,
- const T* const _anchor_frame_p,
- const T* const _anchor_frame_o,
- const T* const _lmk_hmg,
- T* _expectation) const
-{
- using namespace Eigen;
-
- // All involved transforms typedef
- typedef Eigen::Transform<T, 3, Eigen::Affine> TransformType;
-
- // world to anchor robot transform
- Map<const Matrix<T, 3, 1> > p_w_r0(_anchor_frame_p);
- Translation<T, 3> t_w_r0(p_w_r0);
- Map<const Quaternion<T> > q_w_r0(_anchor_frame_o);
- TransformType T_W_R0 = t_w_r0 * q_w_r0;
-
- // world to current robot transform
- Map<const Matrix<T, 3, 1> > p_w_r1(_current_frame_p);
- Translation<T, 3> t_w_r1(p_w_r1);
- Map<const Quaternion<T> > q_w_r1(_current_frame_o);
- TransformType T_W_R1 = t_w_r1 * q_w_r1;
-
- // anchor robot to anchor camera transform
- Translation<T, 3> t_r0_c0(anchor_sensor_extrinsics_p_.cast<T>());
- Quaternion<T> q_r0_c0(anchor_sensor_extrinsics_o_.cast<T>());
- TransformType T_R0_C0 = t_r0_c0 * q_r0_c0;
-
- // current robot to current camera transform
- CaptureBasePtr current_capture = this->getFeature()->getCapture();
- Translation<T, 3> t_r1_c1 (current_capture->getSensorP()->getState().cast<T>());
- Quaternions q_r1_c1_s(Eigen::Vector4s(current_capture->getSensorO()->getState()));
- Quaternion<T> q_r1_c1 = q_r1_c1_s.cast<T>();
- TransformType T_R1_C1 = t_r1_c1 * q_r1_c1;
-
- // hmg point in current camera frame C1
- Eigen::Map<const Eigen::Matrix<T, 4, 1> > landmark_hmg_c0(_lmk_hmg);
- Eigen::Matrix<T, 4, 1> landmark_hmg_c1 = T_R1_C1.inverse(Eigen::Affine)
- * T_W_R1. inverse(Eigen::Affine)
- * T_W_R0
- * T_R0_C0
- * landmark_hmg_c0;
-
- // lmk direction vector
- Eigen::Matrix<T, 3, 1> v_dir = landmark_hmg_c1.head(3);
-
- // camera parameters
- Matrix<T, 4, 1> intrinsic = intrinsic_.cast<T>();
- Eigen::Matrix<T, Eigen::Dynamic, 1> distortion = distortion_.cast<T>();
-
- // project point and exit
- Eigen::Map<Eigen::Matrix<T, 2, 1> > expectation(_expectation);
- expectation = pinhole::projectPoint(intrinsic, distortion, v_dir);
-}
-
-template<typename T>
-inline bool FactorAHP::operator ()(const T* const _current_frame_p,
- const T* const _current_frame_o,
- const T* const _anchor_frame_p,
- const T* const _anchor_frame_o,
- const T* const _lmk_hmg,
- T* _residuals) const
-{
- // expected
- Eigen::Matrix<T, 2, 1> expected;
- expectation(_current_frame_p, _current_frame_o, _anchor_frame_p, _anchor_frame_o, _lmk_hmg, expected.data());
-
- // measured
- Eigen::Matrix<T, 2, 1> measured = getMeasurement().cast<T>();
-
- // residual
- Eigen::Map<Eigen::Matrix<T, 2, 1> > residuals(_residuals);
- residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (expected - measured);
- return true;
-}
-
-inline FactorAHPPtr FactorAHP::create(const FeatureBasePtr& _ftr_ptr,
- const LandmarkAHPPtr& _lmk_ahp_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status)
-{
- // construct factor
- FactorAHPPtr fac_ahp = std::make_shared<FactorAHP>(_ftr_ptr, _lmk_ahp_ptr, _processor_ptr, _apply_loss_function, _status);
-
- return fac_ahp;
-}
-
-} // namespace wolf
-
-#endif // FACTOR_AHP_H
diff --git a/include/base/factor/factor_GPS_2D.h b/include/base/factor/factor_GPS_2D.h
deleted file mode 100644
index bac2e381c..000000000
--- a/include/base/factor/factor_GPS_2D.h
+++ /dev/null
@@ -1,41 +0,0 @@
-
-#ifndef FACTOR_GPS_2D_H_
-#define FACTOR_GPS_2D_H_
-
-//Wolf includes
-#include "base/common/wolf.h"
-#include "base/factor/factor_autodiff.h"
-#include "base/frame/frame_base.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorGPS2D);
-
-class FactorGPS2D : public FactorAutodiff<FactorGPS2D, 2, 2>
-{
- public:
-
- FactorGPS2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorGPS2D, 2, 2>("GPS 2D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP())
- {
- //
- }
-
- virtual ~FactorGPS2D() = default;
-
- template<typename T>
- bool operator ()(const T* const _x, T* _residuals) const;
-
-};
-
-template<typename T>
-inline bool FactorGPS2D::operator ()(const T* const _x, T* _residuals) const
-{
- _residuals[0] = (T(getMeasurement()(0)) - _x[0]) / T(sqrt(getMeasurementCovariance()(0, 0)));
- _residuals[1] = (T(getMeasurement()(1)) - _x[1]) / T(sqrt(getMeasurementCovariance()(1, 1)));
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factor/factor_GPS_pseudorange_2D.h b/include/base/factor/factor_GPS_pseudorange_2D.h
deleted file mode 100644
index 82b186fc6..000000000
--- a/include/base/factor/factor_GPS_pseudorange_2D.h
+++ /dev/null
@@ -1,241 +0,0 @@
-#ifndef FACTOR_GPS_PSEUDORANGE_2D_H_
-#define FACTOR_GPS_PSEUDORANGE_2D_H_
-
-#define LIGHT_SPEED_ 299792458
-
-//Wolf includes
-#include "base/sensor/sensor_GPS.h"
-#include "base/feature/feature_GPS_pseudorange.h"
-#include "base/factor/factor_autodiff.h"
-
-//std
-#include <string>
-#include <sstream>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorGPSPseudorange2D);
-
-/*
- * NB:
- * FROM THIS CLASS AND ALL THE CLASS INCLUDED, THE LIBRARY RAW_GPS_UTILS
- * MUST NOT BE REACHABLE!
- * OTHERWISE WOLF WON'T COMPILE WITHOUT INSTALLING THIS LIBRARY.
- *
- * TODO maybe is no more true
- */
-class FactorGPSPseudorange2D : public FactorAutodiff<FactorGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>
-{
- public:
- FactorGPSPseudorange2D(const FeatureBasePtr& _ftr_ptr, const ProcessorBasePtr& _pr_ptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>("GPS PR 2D",
- nullptr,
- nullptr,
- nullptr,
- nullptr,
- _pr_ptr,
- _apply_loss_function,
- _status,
- _ftr_ptr->getFrame()->getP(), // position of the vehicle's frame with respect to the initial pos frame
- _ftr_ptr->getFrame()->getO(), // orientation of the vehicle's frame
- _ftr_ptr->getCapture()->getSensorP(), // position of the sensor (gps antenna) with respect to the vehicle frame
- // orientation of antenna is not needed, because omnidirectional
- _ftr_ptr->getCapture()->getSensor()->getIntrinsic(), //intrinsic parameter = receiver time bias
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapture()->getSensor())->getMapP()), // map position with respect to ecef
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapture()->getSensor())->getMapO())) // map orientation with respect to ecef
- {
- sat_position_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getSatPosition();
- pseudorange_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getPseudorange();
- //std::cout << "FactorGPSPseudorange2D() pr=" << pseudorange_ << "\tsat_pos=(" << sat_position_[0] << ", " << sat_position_[1] << ", " << sat_position_[2] << ")" << std::endl;
- }
-
- virtual ~FactorGPSPseudorange2D() = default;
-
- template<typename T>
- bool operator ()(const T* const _vehicle_p, const T* const _vehicle_o, const T* const _sensor_p,
- const T* const _bias, const T* const _map_p, const T* const _map_o,
- T* _residual) const;
-
- protected:
- Eigen::Vector3s sat_position_;
- Scalar pseudorange_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
-};
-
-/*
- * naming convention for transformation matrix:
- * T_a_b is "transformation from b to a"
- * To transform a point from b to a: p_a = T_a_b * P_b
- * T_a_b also means "the pose of b expressed in frame a"
- */
-
-template<typename T>
-inline bool FactorGPSPseudorange2D::operator ()(const T* const _vehicle_p, const T* const _vehicle_o,
- const T* const _sensor_p, const T* const _bias,
- const T* const _map_p, const T* const _map_o,
- T* _residual) const
-{
- int verbose_level_ = 0; // 0=nothing printed. only for debug purpose
-
- std::stringstream aux;
- aux << std::setprecision(12);
- std::cout << std::setprecision(12);
-
- if (verbose_level_ >= 2)
- {
- std::cout << "+OPERATOR()+" << id() << std::endl;
- std::cout << "_sensor_p(_base): " << _sensor_p[0] << ", " << _sensor_p[1] << ", " << _sensor_p[2] << std::endl;
- std::cout << "_vehicle_p(_map): " << _vehicle_p[0] << ", " << _vehicle_p[1] << std::endl;
- std::cout << "_vehicle_o(_map): " << _vehicle_o[0] << std::endl;
- std::cout << "_map_p: " << _map_p[0] << ", " << _map_p[1] << ", " << _map_p[2] << std::endl;
- std::cout << "_map_o: " << _map_o[0] << std::endl;
- }
- //Filling Eigen vectors
- Eigen::Matrix<T, 4, 1> sensor_p_base(_sensor_p[0], _sensor_p[1], _sensor_p[2], T(1)); //sensor position with respect base frame
-
- /*
- * Base-to-map transform matrix
- */
- Eigen::Matrix<T, 4, 4> T_map_base = Eigen::Matrix<T, 4, 4>::Identity();
- T_map_base(0, 0) = T(cos(_vehicle_o[0]));
- T_map_base(0, 1) = T(-sin(_vehicle_o[0]));
- T_map_base(1, 0) = T(sin(_vehicle_o[0]));
- T_map_base(1, 1) = T(cos(_vehicle_o[0]));
- T_map_base(0, 3) = T(_vehicle_p[0]);
- T_map_base(1, 3) = T(_vehicle_p[1]);
-
- // sensor position with respect to map frame
- Eigen::Matrix<T, 4, 1> sensor_p_map = T_map_base * sensor_p_base;
-
- if (verbose_level_ >= 2)
- {
- aux.str(std::string());
- aux << sensor_p_map(0);
- if (aux.str().substr(0, 1) != "[")
- std::cout << "!!! sensor_p_map: " << sensor_p_map[0] << ", " << sensor_p_map[1] << ", " << sensor_p_map[2] << std::endl;
- else {
- std::cout << "!!! sensor_p_map: " << aux.str().substr(1, aux.str().find(" ") - 1) << ", ";
- aux.str(std::string());
- aux << sensor_p_map(1);
- std::cout << aux.str().substr(1, aux.str().find(" ") - 1) << ", ";
- aux.str(std::string());
- aux << sensor_p_map(2);
- std::cout << aux.str().substr(1, aux.str().find(" ") - 1) << std::endl;
- aux.str(std::string());
- }
- }
-
- /*
- * _map_p from ECEF to LLA (math from https://microem.ru/files/2012/08/GPS.G1-X-00006.pdf )
- */
- // WGS84 ellipsoid constants
- T a = T(6378137); // earth's radius
- T e = T(8.1819190842622e-2); // eccentricity
- T asq = a * a;
- T esq = e * e;
- T b = T(sqrt(asq * (T(1) - esq)));
- T bsq = T(b * b);
- T ep = T(sqrt((asq - bsq) / bsq));
- T p = T(sqrt(_map_p[0] * _map_p[0] + _map_p[1] * _map_p[1]));
- T th = T(atan2(a * _map_p[2], b * p));
- T lon = T(atan2(_map_p[1], _map_p[0]));
- T lat = T(atan2((_map_p[2] + ep * ep * b * pow(sin(th), 3)), (p - esq * a * pow(cos(th), 3))));
-
- if (verbose_level_ >= 3)
- {
- std::cout << "_map_p: " << _map_p[0] << ", " << _map_p[1] << ", " << _map_p[2] << std::endl;
- std::cout << "_map_p LLA: " << lat << ", " << lon << std::endl;
- std::cout << "_map_p LLA degrees: " << lat * T(180 / M_PI) << ", " << lon * T(180 / M_PI) << std::endl;
- }
-
- /*
- * map-to-ECEF transform matrix
- * made by the product of the next 4 matrixes
- */
- Eigen::Matrix<T, 4, 4> T_ecef_aux = Eigen::Matrix<T, 4, 4>::Identity();
- T_ecef_aux(0, 3) = T(_map_p[0]);
- T_ecef_aux(1, 3) = T(_map_p[1]);
- T_ecef_aux(2, 3) = T(_map_p[2]);
-
- Eigen::Matrix<T, 4, 4> T_aux_lon = Eigen::Matrix<T, 4, 4>::Identity();
- T_aux_lon(0, 0) = T(cos(lon));
- T_aux_lon(0, 1) = T(-sin(lon));
- T_aux_lon(1, 0) = T(sin(lon));
- T_aux_lon(1, 1) = T(cos(lon));
-
- Eigen::Matrix<T, 4, 4> T_lon_lat = Eigen::Matrix<T, 4, 4>::Identity();
- T_lon_lat(0, 0) = T(cos(lat));
- T_lon_lat(0, 2) = T(-sin(lat));
- T_lon_lat(2, 0) = T(sin(lat));
- T_lon_lat(2, 2) = T(cos(lat));
-
- Eigen::Matrix<T, 4, 4> T_lat_enu = Eigen::Matrix<T, 4, 4>::Zero();
- T_lat_enu(0, 2) = T_lat_enu(1, 0) = T_lat_enu(2, 1) = T_lat_enu(3, 3) = T(1);
-
- Eigen::Matrix<T, 4, 4> T_enu_map = Eigen::Matrix<T, 4, 4>::Identity();
- T_enu_map(0, 0) = T(cos(_map_o[0]));
- T_enu_map(0, 1) = T(-sin(_map_o[0]));
- T_enu_map(1, 0) = T(sin(_map_o[0]));
- T_enu_map(1, 1) = T(cos(_map_o[0]));
-
- Eigen::Matrix<T, 4, 4> T_ecef_map = T_ecef_aux * T_aux_lon * T_lon_lat * T_lat_enu * T_enu_map;
-
- //sensor position with respect to ecef coordinate system
- Eigen::Matrix<T, 4, 1> sensor_p_ecef = T_ecef_map * sensor_p_map;
-
- /*
- * calculate the residual
- */
- T square_sum = T(0);
- for (int i = 0; i < 3; ++i)
- square_sum += (sensor_p_ecef[i] - T(sat_position_[i]))*(sensor_p_ecef[i] - T(sat_position_[i]));
-
- T distance = (square_sum != T(0)) ? sqrt(square_sum) : T(0);
-
- // error = (expected measurement) - (actual measurement)
- _residual[0] = (distance + _bias[0] * T(LIGHT_SPEED_)) - T(pseudorange_);
-
- if (verbose_level_ >= 2)
- std::cout << "!!! Residual: " << _residual[0] << "\n";
-
- // normalizing by the covariance
- _residual[0] = _residual[0] / T(getMeasurementCovariance()(0, 0));//T(sqrt(getMeasurementCovariance()(0, 0)));
-
- if (verbose_level_ >= 1)
- {
- aux.str(std::string());
- aux << sensor_p_ecef(0);
- if (aux.str().substr(0, 1) != "[")
- std::cout << "!!! sensor_p_ecef: " << sensor_p_ecef[0] << ", " << sensor_p_ecef[1] << ", " << sensor_p_ecef[2];
- else {
- std::cout << "!!! sensor_p_ecef: " << aux.str().substr(1, aux.str().find(" ") - 1) << ", ";
- aux.str(std::string());
- aux << sensor_p_ecef(1);
- std::cout << aux.str().substr(1, aux.str().find(" ") - 1) << ", ";
- aux.str(std::string());
- aux << sensor_p_ecef(2);
- std::cout << aux.str().substr(1, aux.str().find(" ") - 1);
- }
-
- //std::cout << "Expected: " << (distance + _bias[0]*T(LIGHT_SPEED)) << "\nreceived = " << pseudorange_ << "\n";
- aux.str(std::string());
- aux << _residual[0];
- if (aux.str().substr(0,1) != "[" )
- std::cout << "\t Residual norm: " << _residual[0] << "\n";
- else
- {
- std::cout << "\t Residual norm: " << aux.str().substr(1, aux.str().find(" ") - 1) << "\n";
- }
- }
-
- return true;
-}
-
-} // namespace wolf
-
-#endif //FACTOR_GPS_PSEUDORANGE_2D_H_
diff --git a/include/base/factor/factor_GPS_pseudorange_3D.h b/include/base/factor/factor_GPS_pseudorange_3D.h
deleted file mode 100644
index a28c069ec..000000000
--- a/include/base/factor/factor_GPS_pseudorange_3D.h
+++ /dev/null
@@ -1,152 +0,0 @@
-#ifndef FACTOR_GPS_PSEUDORANGE_3D_H_
-#define FACTOR_GPS_PSEUDORANGE_3D_H_
-
-#define LIGHT_SPEED 299792458
-
-//Wolf includes
-#include "base/sensor/sensor_GPS.h"
-#include "base/feature/feature_GPS_pseudorange.h"
-#include "base/factor/factor_autodiff.h"
-
-namespace wolf {
-
-// Set ClassPtr, ClassConstPtr and ClassWPtr typedefs;
-WOLF_PTR_TYPEDEFS(FactorGPSPseudorange3D);
-
-/*
- * NB:
- * FROM THIS CLASS AND ALL THE CLASS INCLUDED, THE LIBRARY RAW_GPS_UTILS
- * MUST NOT BE REACHABLE!
- * OTHERWISE WOLF WON'T COMPILE WITHOUT INSTALLING THIS LIBRARY.
- *
- * TODO maybe is no more true
- */
-class FactorGPSPseudorange3D: public FactorAutodiff<FactorGPSPseudorange3D, 1, 3, 4, 3, 1, 3, 4>
-{
-
- public:
-
-<<<<<<< HEAD:include/base/constraint/constraint_GPS_pseudorange_3D.h
- ConstraintGPSPseudorange3D(FeatureBasePtr _ftr_ptr, const ProcessorBasePtr& _pr_ptr,
- bool _apply_loss_function = false,
- ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintGPSPseudorange3D, 1, 3, 4, 3, 1, 3, 4>("GPS PR 3D",
- nullptr, nullptr, nullptr, nullptr, _pr_ptr, _apply_loss_function, _status,
- _ftr_ptr->getFramePtr()->getPPtr(), // position of the vehicle's frame with respect to map frame
- _ftr_ptr->getFramePtr()->getOPtr(), // orientation of the vehicle's frame wrt map frame
- _ftr_ptr->getCapturePtr()->getSensorPPtr(), // position of the sensor (gps antenna) with respect to base frame
-=======
- FactorGPSPseudorange3D(FeatureBasePtr _ftr_ptr, const ProcessorBasePtr& _pr_ptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorGPSPseudorange3D, 1, 3, 4, 3, 1, 3, 4>("GPS PR 3D",
- nullptr, nullptr, nullptr, nullptr, _pr_ptr, _apply_loss_function, _status,
- _ftr_ptr->getFrame()->getP(), // position of the vehicle's frame with respect to map frame
- _ftr_ptr->getFrame()->getO(), // orientation of the vehicle's frame wrt map frame
- _ftr_ptr->getCapture()->getSensorP(), // position of the sensor (gps antenna) with respect to base frame
->>>>>>> devel:include/base/factor/factor_GPS_pseudorange_3D.h
- // orientation of antenna is not needed, because omnidirectional
- _ftr_ptr->getCapture()->getSensor()->getIntrinsic(), //intrinsic parameter = receiver time bias
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapture()->getSensor()))->getMapP(), // initial vehicle position wrt ecef frame
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapture()->getSensor()))->getMapO()) // initial vehicle orientation wrt ecef frame
- {
- sat_position_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getSatPosition();
- pseudorange_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getPseudorange();
-
- //std::cout << "FactorGPSPseudorange3D() pr=" << pseudorange_ << "\tsat_pos=(" << sat_position_[0] << ", " << sat_position_[1] << ", " << sat_position_[2] << ")" << std::endl;
- }
-
- virtual ~FactorGPSPseudorange3D() = default;
-
- template<typename T>
- bool operator ()(const T* const _vehicle_p, const T* const _vehicle_o, const T* const _sensor_p,
- const T* const _bias, const T* const _init_vehicle_p, const T* const _init_vehicle_o,
- T* _residual) const;
-
- protected:
- Eigen::Vector3s sat_position_;
- Scalar pseudorange_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- static FactorBasePtr create(FeatureBasePtr _feature_ptr, //
- NodeBasePtr _correspondant_ptr = nullptr)
- {
- return std::make_shared<FactorGPSPseudorange3D>(_feature_ptr);
- }
-
-};
-
-/*
- * naming convention for transformation matrix:
- * T_a_b is "transformation from b to a"
- * To transform a point from b to a: p_a = T_a_b * P_b
- * T_a_b also means "the pose of b expressed in frame a"
- */
-
-template<typename T>
-inline bool FactorGPSPseudorange3D::operator ()(const T* const _vehicle_p, const T* const _vehicle_o,
- const T* const _sensor_p, const T* const _bias,
- const T* const _map_p, const T* const _map_o,
- T* _residual) const
-{
-// std::cout << "OPERATOR()\n";
-// std::cout << "_map_p: " << _map_p[0] << ", " << _map_p[1] << ", " << _map_p[2] << std::endl;
-// std::cout << "_map_o: " << _map_o[0] << ", " << _map_o[1] << ", " << _map_o[2] << ", " << _map_o[3] << std::endl;
-// std::cout << "_vehicle_p: " << _vehicle_p[0] << ", " << _vehicle_p[1] << ", " << _vehicle_p[2] << std::endl;
-// std::cout << "_vehicle_o: " << _vehicle_o[0] << ", " << _vehicle_o[1] << ", " << _vehicle_o[2] << ", " << _vehicle_o[3] << std::endl;
-// std::cout << "_sensor_p: " << _sensor_p[0] << ", " << _sensor_p[1] << ", " << _sensor_p[2] << std::endl;
- Eigen::Matrix<T, 4, 1> sensor_p_ecef; //sensor position with respect to ecef coordinate system
- Eigen::Matrix<T, 4, 1> sensor_p_base(_sensor_p[0], _sensor_p[1], _sensor_p[2], T(1)); //sensor position with respect to the base (the vehicle)
-
- /*
- * map-to-ECEF transformation matrix: To transform a point from map to ecef
- */
- Eigen::Matrix<T, 4, 4> T_ecef_map = Eigen::Matrix<T, 4, 4>::Identity();
- Eigen::Quaternion<T> map_o_quat(_map_o[0], _map_o[1], _map_o[2], _map_o[3]);
- Eigen::Matrix<T, 3, 3> rot_matr_init = map_o_quat.toRotationMatrix();
- for (int i = 0; i < 3; ++i)
- for (int j = 0; j < 3; ++j)
- T_ecef_map(i, j) = rot_matr_init(i, j);
- for (int i = 0; i < 3; ++i)
- T_ecef_map(i, 3) = _map_p[i];
-
- /*
- * Base-to-map transformation matrix: To transform a point from base to map
- */
- Eigen::Matrix<T, 4, 4> T_map_base = Eigen::Matrix<T, 4, 4>::Identity();
- Eigen::Quaternion<T> vehicle_o_quat(_vehicle_o[0], _vehicle_o[1], _vehicle_o[2], _vehicle_o[3]);
- Eigen::Matrix<T, 3, 3> rot_matr_vehicle = vehicle_o_quat.toRotationMatrix();
- for (int i = 0; i < 3; ++i)
- for (int j = 0; j < 3; ++j)
- T_map_base(i, j) = rot_matr_vehicle(i, j);
- for (int i = 0; i < 3; ++i)
- T_map_base(i, 3) = _vehicle_p[i];
-
- /*
- * Compute sensor_p wrt ECEF
- */
- sensor_p_ecef = T_ecef_map * T_map_base * sensor_p_base;
- //std::cout << "sensor_p_ecef: " << sensor_p_ecef[0] << ", " << sensor_p_ecef[1] << ", " << sensor_p_ecef[2] << std::endl;
-
- /*
- * Compute residual
- */
- T square_sum = T(0);
- for (int i = 0; i < 3; ++i)
- square_sum += pow(sensor_p_ecef[i] - T(sat_position_[i]), 2);
-
- T distance = (square_sum != T(0)) ? sqrt(square_sum) : T(0);
- // error = (expected measurement) - (actual measurement)
- _residual[0] = (distance + _bias[0] * T(LIGHT_SPEED)) - (pseudorange_);
-
- // Normalize by covariance
- _residual[0] = _residual[0] / T(sqrt(getMeasurementCovariance()(0, 0)));
-
- return true;
-}
-
-} // namespace wolf
-
-#endif //FACTOR_GPS_PSEUDORANGE_3D_H_
diff --git a/include/base/factor/factor_IMU.h b/include/base/factor/factor_IMU.h
deleted file mode 100644
index 942cbf51c..000000000
--- a/include/base/factor/factor_IMU.h
+++ /dev/null
@@ -1,498 +0,0 @@
-#ifndef FACTOR_IMU_THETA_H_
-#define FACTOR_IMU_THETA_H_
-
-//Wolf includes
-#include "base/feature/feature_IMU.h"
-#include "base/sensor/sensor_IMU.h"
-#include "base/factor/factor_autodiff.h"
-#include "base/math/rotations.h"
-
-//Eigen include
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorIMU);
-
-//class
-class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>
-{
- public:
- FactorIMU(const FeatureIMUPtr& _ftr_ptr,
- const CaptureIMUPtr& _capture_origin_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE);
-
- virtual ~FactorIMU() = default;
-
- /* \brief : compute the residual from the state blocks being iterated by the solver.
- -> computes the expected measurement
- -> corrects actual measurement with new bias
- -> compares the corrected measurement with the expected one
- -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
- */
- template<typename T>
- bool operator ()(const T* const _p1,
- const T* const _o1,
- const T* const _v1,
- const T* const _b1,
- const T* const _p2,
- const T* const _o2,
- const T* const _v2,
- const T* const _b2,
- T* _res) const;
-
- /* \brief : compute the residual from the state blocks being iterated by the solver. (same as operator())
- -> computes the expected measurement
- -> corrects actual measurement with new bias
- -> compares the corrected measurement with the expected one
- -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
- * params :
- * Vector3s _p1 : position in imu frame
- * Vector4s _q1 : orientation quaternion in imu frame
- * Vector3s _v1 : velocity in imu frame
- * Vector3s _ab : accelerometer bias in imu frame
- * Vector3s _wb : gyroscope bias in imu frame
- * Vector3s _p2 : position in current frame
- * Vector4s _q2 : orientation quaternion in current frame
- * Vector3s _v2 : velocity in current frame
- * Matrix<9,1, Scalar> _res : to retrieve residuals (POV) O is rotation vector... NOT A QUATERNION
- */
- template<typename D1, typename D2, typename D3>
- bool residual(const Eigen::MatrixBase<D1> & _p1,
- const Eigen::QuaternionBase<D2> & _q1,
- const Eigen::MatrixBase<D1> & _v1,
- const Eigen::MatrixBase<D1> & _ab1,
- const Eigen::MatrixBase<D1> & _wb1,
- const Eigen::MatrixBase<D1> & _p2,
- const Eigen::QuaternionBase<D2> & _q2,
- const Eigen::MatrixBase<D1> & _v2,
- const Eigen::MatrixBase<D1> & _ab2,
- const Eigen::MatrixBase<D1> & _wb2,
- Eigen::MatrixBase<D3> & _res) const;
-
- /* Function expectation(...)
- * params :
- * Vector3s _p1 : position in imu frame
- * Vector4s _q1 : orientation quaternion in imu frame
- * Vector3s _v1 : velocity in imu frame
- * Vector3s _p2 : position in current frame
- * Vector4s _q2 : orientation quaternion in current frame
- * Vector3s _v2 : velocity in current frame
- * Scalar _dt : time interval between the two states
- * _pe : expected position delta
- * _qe : expected quaternion delta
- * _ve : expected velocity delta
- */
- template<typename D1, typename D2, typename D3, typename D4>
- void expectation(const Eigen::MatrixBase<D1> & _p1,
- const Eigen::QuaternionBase<D2> & _q1,
- const Eigen::MatrixBase<D1> & _v1,
- const Eigen::MatrixBase<D1> & _p2,
- const Eigen::QuaternionBase<D2> & _q2,
- const Eigen::MatrixBase<D1> & _v2,
- typename D1::Scalar _dt,
- Eigen::MatrixBase<D3> & _pe,
- Eigen::QuaternionBase<D4> & _qe,
- Eigen::MatrixBase<D3> & _ve) const;
-
- /* \brief : return the expected delta given the state blocks in the wolf tree
- */
- Eigen::VectorXs expectation() const;
-
- private:
- /// Preintegrated delta
- Eigen::Vector3s dp_preint_;
- Eigen::Quaternions dq_preint_;
- Eigen::Vector3s dv_preint_;
-
- // Biases used during preintegration
- Eigen::Vector3s acc_bias_preint_;
- Eigen::Vector3s gyro_bias_preint_;
-
- // Jacobians of preintegrated deltas wrt biases
- Eigen::Matrix3s dDp_dab_;
- Eigen::Matrix3s dDv_dab_;
- Eigen::Matrix3s dDp_dwb_;
- Eigen::Matrix3s dDv_dwb_;
- Eigen::Matrix3s dDq_dwb_;
-
- /// Metrics
- const Scalar dt_; ///< delta-time and delta-time-squared between keyframes
- const Scalar ab_rate_stdev_, wb_rate_stdev_; //stdev for standard_deviation (= sqrt(variance))
-
- /* bias covariance and bias residuals
- *
- * continuous bias covariance matrix for accelerometer would then be A_r = diag(ab_stdev_^2, ab_stdev_^2, ab_stdev_^2)
- * To compute bias residuals, we will need to do (sqrt(A_r)).inverse() * ab_error
- *
- * In our case, we introduce time 'distance' in the computation of this residual (SEE FORSTER17), thus we have to use the discret covariance matrix
- * discrete bias covariance matrix for accelerometer : A_r_dt = dt_ * A_r
- * taking the square root for bias residuals : sqrt_A_r_dt = sqrt(dt_ * A_r) = sqrt(dt_) * sqrt(A_r)
- * then with the inverse : sqrt_A_r_dt_inv = (sqrt(dt_ * A_r)).inverse() = (1/sqrt(dt_)) * sqrt(A_r).inverse()
- *
- * same logic for gyroscope bias
- */
- const Eigen::Matrix3s sqrt_A_r_dt_inv;
- const Eigen::Matrix3s sqrt_W_r_dt_inv;
-
-};
-
-///////////////////// IMPLEMENTAITON ////////////////////////////
-
-inline FactorIMU::FactorIMU(const FeatureIMUPtr& _ftr_ptr,
- const CaptureIMUPtr& _cap_origin_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>( // ...
- "IMU",
- _cap_origin_ptr->getFrame(),
- _cap_origin_ptr,
- nullptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _cap_origin_ptr->getFrame()->getP(),
- _cap_origin_ptr->getFrame()->getO(),
- _cap_origin_ptr->getFrame()->getV(),
- _cap_origin_ptr->getSensorIntrinsic(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _ftr_ptr->getFrame()->getV(),
- _ftr_ptr->getCapture()->getSensorIntrinsic()),
- dp_preint_(_ftr_ptr->getMeasurement().head<3>()), // dp, dv, dq at preintegration time
- dq_preint_(_ftr_ptr->getMeasurement().data()+3),
- dv_preint_(_ftr_ptr->getMeasurement().tail<3>()),
- acc_bias_preint_(_ftr_ptr->getAccBiasPreint()), // state biases at preintegration time
- gyro_bias_preint_(_ftr_ptr->getGyroBiasPreint()),
- dDp_dab_(_ftr_ptr->getJacobianBias().block(0,0,3,3)), // Jacs of dp dv dq wrt biases
- dDv_dab_(_ftr_ptr->getJacobianBias().block(6,0,3,3)),
- dDp_dwb_(_ftr_ptr->getJacobianBias().block(0,3,3,3)),
- dDv_dwb_(_ftr_ptr->getJacobianBias().block(6,3,3,3)),
- dDq_dwb_(_ftr_ptr->getJacobianBias().block(3,3,3,3)),
- dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
- ab_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapture()->getSensor())->getAbRateStdev()),
- wb_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapture()->getSensor())->getWbRateStdev()),
- sqrt_A_r_dt_inv((Eigen::Matrix3s::Identity() * ab_rate_stdev_ * sqrt(dt_)).inverse()),
- sqrt_W_r_dt_inv((Eigen::Matrix3s::Identity() * wb_rate_stdev_ * sqrt(dt_)).inverse())
-{
- //
-}
-
-template<typename T>
-inline bool FactorIMU::operator ()(const T* const _p1,
- const T* const _q1,
- const T* const _v1,
- const T* const _b1,
- const T* const _p2,
- const T* const _q2,
- const T* const _v2,
- const T* const _b2,
- T* _res) const
-{
- using namespace Eigen;
-
- // MAPS
- Map<const Matrix<T,3,1> > p1(_p1);
- Map<const Quaternion<T> > q1(_q1);
- Map<const Matrix<T,3,1> > v1(_v1);
- Map<const Matrix<T,3,1> > ab1(_b1);
- Map<const Matrix<T,3,1> > wb1(_b1 + 3);
-
- Map<const Matrix<T,3,1> > p2(_p2);
- Map<const Quaternion<T> > q2(_q2);
- Map<const Matrix<T,3,1> > v2(_v2);
- Map<const Matrix<T,3,1> > ab2(_b2);
- Map<const Matrix<T,3,1> > wb2(_b2 + 3);
-
- Map<Matrix<T,15,1> > res(_res);
-
- residual(p1, q1, v1, ab1, wb1, p2, q2, v2, ab2, wb2, res);
-
- return true;
-}
-
-template<typename D1, typename D2, typename D3>
-inline bool FactorIMU::residual(const Eigen::MatrixBase<D1> & _p1,
- const Eigen::QuaternionBase<D2> & _q1,
- const Eigen::MatrixBase<D1> & _v1,
- const Eigen::MatrixBase<D1> & _ab1,
- const Eigen::MatrixBase<D1> & _wb1,
- const Eigen::MatrixBase<D1> & _p2,
- const Eigen::QuaternionBase<D2> & _q2,
- const Eigen::MatrixBase<D1> & _v2,
- const Eigen::MatrixBase<D1> & _ab2,
- const Eigen::MatrixBase<D1> & _wb2,
- Eigen::MatrixBase<D3> & _res) const
-{
-
- /* Help for the IMU residual function
- *
- * Notations:
- * D_* : a motion in the Delta manifold -- implemented as 10-vectors with [Dp, Dq, Dv]
- * T_* : a motion difference in the Tangent space to the manifold -- implemented as 9-vectors with [Dp, Do, Dv]
- * b* : a bias
- * J* : a Jacobian matrix
- *
- * We use the following functions from imu_tools.h:
- * D = betweenStates(x1,x2,dt) : obtain a Delta from two states separated dt=t2-t1
- * D2 = plus(D1,T) : plus operator, D2 = D1 (+) T
- * T = diff(D1,D2) : minus operator, T = D2 (-) D1
- *
- * Two methods are possible (select with #define below this note) :
- *
- * Computations common to methods 1 and 2:
- * D_exp = betweenStates(x1,x2,dt) // Predict delta from states
- * T_step = J_preint * (b - b_preint) // compute the delta correction step due to bias change
- *
- * Method 1:
- * D_corr = plus(D_preint, T_step) // correct the pre-integrated delta with correction step due to bias change
- * T_err = diff(D_exp, D_corr) // compare against expected delta
- * res = W.sqrt * T_err
- *
- * results in:
- * res = W.sqrt * ( diff ( D_exp, plus(D_preint, J_preint * (b - b_preint) ) ) )
- *
- * Method 2:
- * T_diff = diff(D_preint, D_exp) // compare pre-integrated against expected delta
- * T_err = T_diff - T_step // the difference should match the correction step due to bias change
- * res = W.sqrt * err
- *
- * results in :
- * res = W.sqrt * ( ( diff ( D_preint , D_exp ) ) - J_preint * (b - b_preint) )
- *
- * NOTE: See optimization report at the end of this file for comparisons of both methods.
- */
-#define METHOD_1 // if commented, then METHOD_2 will be applied
-
- //needed typedefs
- typedef typename D1::Scalar T;
-
- EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D3, 15);
-
- // 1. Expected delta from states
- Eigen::Matrix<T, 3, 1 > dp_exp;
- Eigen::Quaternion<T> dq_exp;
- Eigen::Matrix<T, 3, 1> dv_exp;
-
- imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, (T)dt_, dp_exp, dq_exp, dv_exp);
-
- // 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint)
-
- // 2.a. Compute the delta step in tangent space: step = J_bias * (bias - bias_preint)
- Eigen::Matrix<T, 9, 1> d_step;
-
- d_step.block(0,0,3,1) = dDp_dab_.cast<T>() * (_ab1 - acc_bias_preint_ .cast<T>()) + dDp_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
- d_step.block(3,0,3,1) = dDq_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
- d_step.block(6,0,3,1) = dDv_dab_.cast<T>() * (_ab1 - acc_bias_preint_ .cast<T>()) + dDv_dwb_.cast<T>() * (_wb1 - gyro_bias_preint_.cast<T>());
-
-#ifdef METHOD_1 // method 1
- Eigen::Matrix<T, 3, 1> dp_step = d_step.block(0,0,3,1);
- Eigen::Matrix<T, 3, 1> do_step = d_step.block(3,0,3,1);
- Eigen::Matrix<T, 3, 1> dv_step = d_step.block(6,0,3,1);
-
- // 2.b. Add the correction step to the preintegrated delta: delta_cor = delta_preint (+) step
- Eigen::Matrix<T,3,1> dp_correct;
- Eigen::Quaternion<T> dq_correct;
- Eigen::Matrix<T,3,1> dv_correct;
-
- imu::plus(dp_preint_.cast<T>(), dq_preint_.cast<T>(), dv_preint_.cast<T>(),
- dp_step, do_step, dv_step,
- dp_correct, dq_correct, dv_correct);
-
- // 3. Delta error in minimal form: D_err = diff(D_exp , D_corr)
- // Note the Dt here is zero because it's the delta-time between the same time stamps!
- Eigen::Matrix<T, 9, 1> d_error;
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dp_error(d_error.data() );
- Eigen::Map<Eigen::Matrix<T, 3, 1> > do_error(d_error.data() + 3);
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dv_error(d_error.data() + 6);
-
- Eigen::Matrix<T, 3, 3> J_do_dq1, J_do_dq2;
- Eigen::Matrix<T, 9, 9> J_err_corr;
-
-//#define WITH_JAC
-#ifdef WITH_JAC
- imu::diff(dp_exp, dq_exp, dv_exp, dp_correct, dq_correct, dv_correct, dp_error, do_error, dv_error, J_do_dq1, J_do_dq2);
-
- J_err_corr.setIdentity();
- J_err_corr.block(3,3,3,3) = J_do_dq2;
-
- // 4. Residuals are the weighted errors
- _res.head(9) = J_err_corr.inverse().transpose() * getMeasurementSquareRootInformationUpper().cast<T>() * d_error;
-#else
- imu::diff(dp_exp, dq_exp, dv_exp, dp_correct, dq_correct, dv_correct, dp_error, do_error, dv_error);
- _res.head(9) = getMeasurementSquareRootInformationUpper().cast<T>() * d_error;
-#endif
-
-#else // method 2
-
- Eigen::Matrix<T, 9, 1> d_diff;
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dp_diff(d_diff.data() );
- Eigen::Map<Eigen::Matrix<T, 3, 1> > do_diff(d_diff.data() + 3);
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dv_diff(d_diff.data() + 6);
-
- imu::diff(dp_preint_.cast<T>(), dq_preint_.cast<T>(), dv_preint_.cast<T>(),
- dp_exp, dq_exp, dv_exp,
- dp_diff, do_diff, dv_diff);
-
- Eigen::Matrix<T, 9, 1> d_error;
- d_error << d_diff - d_step;
-
- // 4. Residuals are the weighted errors
- _res.head(9) = getMeasurementSquareRootInformationUpper().cast<T>() * d_error;
-
-#endif
-
- // Errors between biases
- Eigen::Matrix<T,3,1> ab_error(_ab1 - _ab2); // KF1.bias - KF2.bias
- Eigen::Matrix<T,3,1> wb_error(_wb1 - _wb2);
-
- // 4. Residuals are the weighted errors
- _res.segment(9,3) = sqrt_A_r_dt_inv.cast<T>() * ab_error;
- _res.tail(3) = sqrt_W_r_dt_inv.cast<T>() * wb_error;
-
- //////////////////////////////////////////////////////////////////////////////////////////////
- ///////////////////////////////// PRINT VALUES ///////////////////////////////////
-#if 0
- // print values -----------------------
- Matrix<T, 10, 1> x1; x1 << _p1 , _q1.coeffs(), _v1;
- Matrix<T, 10, 1> x2; x2 << _p2 , _q2.coeffs(), _v2;
- print("x1 ", x1);
- print("x2 ", x2);
- Matrix<T, 6, 1> bp; bp << acc_bias_preint_.cast<T>(), gyro_bias_preint_.cast<T>();
- Matrix<T, 6, 1> b1; b1 << _ab1, _wb1;
- Matrix<T, 6, 1> b2; b2 << _ab2, _wb2;
- print("bp ", bp);
- print("b1 ", b1);
- print("b2 ", b2);
- Matrix<T, 10, 1> exp; exp << dp_exp , dq_exp.coeffs(), dv_exp;
- print("D exp", exp);
- Matrix<T, 10, 1> pre; pre << dp_preint_.cast<T>() , dq_preint_.cast<T>().coeffs(), dv_preint_.cast<T>();
- print("D preint", pre);
- Matrix<T, 9, 6> J_b_r0; J_b_r0.setZero();
- J_b_r0.block(0,0,3,3) = dDp_dab_.cast<T>();
- J_b_r0.block(0,3,3,3) = dDp_dwb_.cast<T>();
- J_b_r0.block(3,3,3,3) = dDq_dwb_.cast<T>();
- J_b_r0.block(6,0,3,3) = dDv_dab_.cast<T>();
- J_b_r0.block(6,3,3,3) = dDv_dwb_.cast<T>();
- print("J bias", J_b_r0);
- print("D step", d_step);
-#ifndef METHOD_1
- Matrix<T, 9, 1> dif; dif << dp_diff , do_diff, dv_diff;
- print("D diff", dif);
-#endif
- print("D err", d_error);
- WOLF_TRACE("-----------------------------------------")
-#endif
- /////////////////////////////////////////////////////////////////////////////////////////////
-
- return true;
-}
-
-inline Eigen::VectorXs FactorIMU::expectation() const
-{
- FrameBasePtr frm_current = getFeature()->getFrame();
- FrameBasePtr frm_previous = getFrameOther();
-
- //get information on current_frame in the FactorIMU
- const Eigen::Vector3s frame_current_pos = (frm_current->getP()->getState());
- const Eigen::Quaternions frame_current_ori (frm_current->getO()->getState().data());
- const Eigen::Vector3s frame_current_vel = (frm_current->getV()->getState());
-
- // get info on previous frame in the FactorIMU
- const Eigen::Vector3s frame_previous_pos = (frm_previous->getP()->getState());
- const Eigen::Quaternions frame_previous_ori (frm_previous->getO()->getState().data());
- const Eigen::Vector3s frame_previous_vel = (frm_previous->getV()->getState());
-
- // Define results vector and Map bits over it
- Eigen::Matrix<Scalar, 10, 1> exp;
- Eigen::Map<Eigen::Matrix<Scalar, 3, 1> > pe(exp.data() );
- Eigen::Map<Eigen::Quaternion<Scalar> > qe(exp.data() + 3);
- Eigen::Map<Eigen::Matrix<Scalar, 3, 1> > ve(exp.data() + 7);
-
- expectation(frame_previous_pos, frame_previous_ori, frame_previous_vel,
- frame_current_pos, frame_current_ori, frame_current_vel,
- dt_,
- pe, qe, ve);
-
- return exp;
-}
-
-template<typename D1, typename D2, typename D3, typename D4>
-inline void FactorIMU::expectation(const Eigen::MatrixBase<D1> & _p1,
- const Eigen::QuaternionBase<D2> & _q1,
- const Eigen::MatrixBase<D1> & _v1,
- const Eigen::MatrixBase<D1> & _p2,
- const Eigen::QuaternionBase<D2> & _q2,
- const Eigen::MatrixBase<D1> & _v2,
- typename D1::Scalar _dt,
- Eigen::MatrixBase<D3> & _pe,
- Eigen::QuaternionBase<D4> & _qe,
- Eigen::MatrixBase<D3> & _ve) const
-{
- imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, _dt, _pe, _qe, _ve);
-}
-
-} // namespace wolf
-
-#endif
-
-/*
- * Optimization results
- * ================================================
- *
- * Using gtest_IMU.cpp
- *
- * Conclusion: Residuals with method 1 and 2 are essentially identical, after exactly the same number of iterations.
- *
- * You can verify this by looking at the 'Iterations' and 'Final costs' in the Ceres reports below.
- *
- * With Method 1:
- *
-[ RUN ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
-[trace][10:58:16] [gtest_IMU.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251480e-06, Termination: CONVERGENCE
-[trace][10:58:16] [gtest_IMU.cpp L490 : TestBody] w * DT (rather be lower than 1.57 approx) = 0.5 1 1.5
-[ OK ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (53 ms)
-[ RUN ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
-[trace][10:58:16] [gtest_IMU.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.238969e+03, Final cost: 1.059256e-19, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (56 ms)
-[ RUN ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
-[trace][10:58:16] [gtest_IMU.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial cost: 4.769588e+03, Final cost: 3.767740e-19, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (50 ms)
-[----------] 3 tests from Process_Factor_IMU (159 ms total)
-
-[----------] 2 tests from Process_Factor_IMU_ODO
-[ RUN ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
-[trace][10:58:16] [gtest_IMU.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.867678e-22, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
-[ RUN ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
-[trace][10:58:16] [gtest_IMU.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.363681e+04, Final cost: 1.879880e-20, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (52 ms)
-[----------] 2 tests from Process_Factor_IMU_ODO (120 ms total)
-*
-* With Method 2:
-*
-[ RUN ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
-[trace][11:15:43] [gtest_IMU.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251479e-06, Termination: CONVERGENCE
-[trace][11:15:43] [gtest_IMU.cpp L490 : TestBody] w * DT (rather be lower than 1.57 approx) = 0.5 1 1.5
-[ OK ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (37 ms)
-[ RUN ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
-[trace][11:15:43] [gtest_IMU.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.238985e+03, Final cost: 1.058935e-19, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (48 ms)
-[ RUN ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
-[trace][11:15:43] [gtest_IMU.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial cost: 4.769603e+03, Final cost: 3.762091e-19, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (47 ms)
-[----------] 3 tests from Process_Factor_IMU (133 ms total)
-
-[----------] 2 tests from Process_Factor_IMU_ODO
-[ RUN ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
-[trace][11:15:43] [gtest_IMU.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.855814e-22, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
-[ RUN ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
-[trace][11:15:43] [gtest_IMU.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.363675e+04, Final cost: 1.880084e-20, Termination: CONVERGENCE
-[ OK ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (59 ms)
-[----------] 2 tests from Process_Factor_IMU_ODO (127 ms total)
-*
-*/
diff --git a/include/base/factor/factor_autodiff_trifocal.h b/include/base/factor/factor_autodiff_trifocal.h
deleted file mode 100644
index 5b88708c4..000000000
--- a/include/base/factor/factor_autodiff_trifocal.h
+++ /dev/null
@@ -1,397 +0,0 @@
-#ifndef _FACTOR_AUTODIFF_TRIFOCAL_H_
-#define _FACTOR_AUTODIFF_TRIFOCAL_H_
-
-//Wolf includes
-//#include "base/common/wolf.h"
-#include "base/factor/factor_autodiff.h"
-#include "base/sensor/sensor_camera.h"
-
-#include <common_class/trifocaltensor.h>
-#include <vision_utils.h>
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(FactorAutodiffTrifocal);
-
-using namespace Eigen;
-
-class FactorAutodiffTrifocal : public FactorAutodiff<FactorAutodiffTrifocal, 3, 3, 4, 3, 4, 3, 4, 3, 4>
-{
- public:
-
- /** \brief Class constructor
- */
- FactorAutodiffTrifocal(const FeatureBasePtr& _feature_prev_ptr,
- const FeatureBasePtr& _feature_origin_ptr,
- const FeatureBasePtr& _feature_last_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status);
-
- /** \brief Class Destructor
- */
- virtual ~FactorAutodiffTrifocal();
-
- FeatureBasePtr getFeaturePrev();
-
- const Vector3s& getPixelCanonicalLast() const
- {
- return pixel_canonical_last_;
- }
-
- void setPixelCanonicalLast(const Vector3s& pixelCanonicalLast)
- {
- pixel_canonical_last_ = pixelCanonicalLast;
- }
-
- const Vector3s& getPixelCanonicalOrigin() const
- {
- return pixel_canonical_origin_;
- }
-
- void setPixelCanonicalOrigin(const Vector3s& pixelCanonicalOrigin)
- {
- pixel_canonical_origin_ = pixelCanonicalOrigin;
- }
-
- const Vector3s& getPixelCanonicalPrev() const
- {
- return pixel_canonical_prev_;
- }
-
- void setPixelCanonicalPrev(const Vector3s& pixelCanonicalPrev)
- {
- pixel_canonical_prev_ = pixelCanonicalPrev;
- }
-
- const Matrix3s& getSqrtInformationUpper() const
- {
- return sqrt_information_upper;
- }
-
- /** brief : compute the residual from the state blocks being iterated by the solver.
- **/
- template<typename T>
- bool operator ()( const T* const _prev_pos,
- const T* const _prev_quat,
- const T* const _origin_pos,
- const T* const _origin_quat,
- const T* const _last_pos,
- const T* const _last_quat,
- const T* const _sen_pos,
- const T* const _sen_quat,
- T* _residuals) const;
-
- public:
- template<typename D1, typename D2, class T, typename D3>
- void expectation(const MatrixBase<D1>& _wtr1,
- const QuaternionBase<D2>& _wqr1,
- const MatrixBase<D1>& _wtr2,
- const QuaternionBase<D2>& _wqr2,
- const MatrixBase<D1>& _wtr3,
- const QuaternionBase<D2>& _wqr3,
- const MatrixBase<D1>& _rtc,
- const QuaternionBase<D2>& _rqc,
- vision_utils::TrifocalTensorBase<T>& _tensor,
- MatrixBase<D3>& _c2Ec1) const;
-
- template<typename T, typename D1>
- Matrix<T, 3, 1> residual(const vision_utils::TrifocalTensorBase<T>& _tensor,
- const MatrixBase<D1>& _c2Ec1) const;
-
- // Helper functions to be used by the above
- template<class T, typename D1, typename D2, typename D3, typename D4>
- Matrix<T, 3, 1> error_jacobians(const vision_utils::TrifocalTensorBase<T>& _tensor,
- const MatrixBase<D1>& _c2Ec1,
- MatrixBase<D2>& _J_e_m1,
- MatrixBase<D3>& _J_e_m2,
- MatrixBase<D4>& _J_e_m3);
-
- private:
- FeatureBaseWPtr feature_prev_ptr_; // To look for measurements
- SensorCameraPtr camera_ptr_; // To look for intrinsics
- Vector3s pixel_canonical_prev_, pixel_canonical_origin_, pixel_canonical_last_;
- Matrix3s sqrt_information_upper;
-};
-
-} // namespace wolf
-
-// Includes for implentation
-#include "base/math/rotations.h"
-
-namespace wolf
-{
-
-using namespace Eigen;
-
-// Constructor
-FactorAutodiffTrifocal::FactorAutodiffTrifocal(const FeatureBasePtr& _feature_prev_ptr,
- const FeatureBasePtr& _feature_origin_ptr,
- const FeatureBasePtr& _feature_last_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff( "TRIFOCAL PLP",
- nullptr,
- nullptr,
- _feature_origin_ptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _feature_prev_ptr->getFrame()->getP(),
- _feature_prev_ptr->getFrame()->getO(),
- _feature_origin_ptr->getFrame()->getP(),
- _feature_origin_ptr->getFrame()->getO(),
- _feature_last_ptr->getFrame()->getP(),
- _feature_last_ptr->getFrame()->getO(),
- _feature_last_ptr->getCapture()->getSensorP(),
- _feature_last_ptr->getCapture()->getSensorO() ),
- feature_prev_ptr_(_feature_prev_ptr),
- camera_ptr_(std::static_pointer_cast<SensorCamera>(_processor_ptr->getSensor())),
- sqrt_information_upper(Matrix3s::Zero())
-{
- setFeature(_feature_last_ptr);
- Matrix3s K_inv = camera_ptr_->getIntrinsicMatrix().inverse();
- pixel_canonical_prev_ = K_inv * Vector3s(_feature_prev_ptr ->getMeasurement(0), _feature_prev_ptr ->getMeasurement(1), 1.0);
- pixel_canonical_origin_ = K_inv * Vector3s(_feature_origin_ptr->getMeasurement(0), _feature_origin_ptr->getMeasurement(1), 1.0);
- pixel_canonical_last_ = K_inv * Vector3s(_feature_last_ptr ->getMeasurement(0), _feature_last_ptr ->getMeasurement(1), 1.0);
- Matrix<Scalar,3,2> J_m_u = K_inv.block(0,0,3,2);
-
- // extract relevant states
- Vector3s wtr1 = _feature_prev_ptr ->getFrame() ->getP() ->getState();
- Quaternions wqr1 = Quaternions(_feature_prev_ptr ->getFrame() ->getO() ->getState().data() );
- Vector3s wtr2 = _feature_origin_ptr->getFrame() ->getP() ->getState();
- Quaternions wqr2 = Quaternions(_feature_origin_ptr->getFrame() ->getO() ->getState().data() );
- Vector3s wtr3 = _feature_last_ptr ->getFrame() ->getP() ->getState();
- Quaternions wqr3 = Quaternions(_feature_last_ptr ->getFrame() ->getO() ->getState().data() );
- Vector3s rtc = _feature_last_ptr ->getCapture()->getSensorP()->getState();
- Quaternions rqc = Quaternions(_feature_last_ptr ->getCapture()->getSensorO()->getState().data() );
-
- // expectation // canonical units
- vision_utils::TrifocalTensorBase<Scalar> tensor;
- Matrix3s c2Ec1;
- expectation(wtr1, wqr1,
- wtr2, wqr2,
- wtr3, wqr3,
- rtc, rqc,
- tensor, c2Ec1);
-
- // error Jacobians // canonical units
- Matrix<Scalar,3,3> J_e_m1, J_e_m2, J_e_m3;
- error_jacobians(tensor, c2Ec1, J_e_m1, J_e_m2, J_e_m3);
-
- // chain rule to go from homogeneous pixel to Euclidean pixel
- Matrix<Scalar,3,2> J_e_u1 = J_e_m1 * J_m_u;
- Matrix<Scalar,3,2> J_e_u2 = J_e_m2 * J_m_u;
- Matrix<Scalar,3,2> J_e_u3 = J_e_m3 * J_m_u;
-
- // Error covariances induced by each of the measurement covariance // canonical units
- Matrix3s Q1 = J_e_u1 * getFeaturePrev() ->getMeasurementCovariance() * J_e_u1.transpose();
- Matrix3s Q2 = J_e_u2 * getFeatureOther()->getMeasurementCovariance() * J_e_u2.transpose();
- Matrix3s Q3 = J_e_u3 * getFeature() ->getMeasurementCovariance() * J_e_u3.transpose();
-
- // Total error covariance // canonical units
- Matrix3s Q = Q1 + Q2 + Q3;
-
- // Sqrt of information matrix // canonical units
- Eigen::LLT<Eigen::MatrixXs> llt_of_info(Q.inverse()); // Cholesky decomposition
- sqrt_information_upper = llt_of_info.matrixU();
-
- // Re-write info matrix (for debug only)
- // Scalar pix_noise = 1.0;
- // sqrt_information_upper = pow(1.0/pix_noise, 2) * Matrix3s::Identity(); // one PLP (2D) and one epipolar (1D) factor
-}
-
-// Destructor
-FactorAutodiffTrifocal::~FactorAutodiffTrifocal()
-{
-}
-
-inline FeatureBasePtr FactorAutodiffTrifocal::getFeaturePrev()
-{
- return feature_prev_ptr_.lock();
-}
-
-template<typename T>
-bool FactorAutodiffTrifocal::operator ()( const T* const _prev_pos,
- const T* const _prev_quat,
- const T* const _origin_pos,
- const T* const _origin_quat,
- const T* const _last_pos,
- const T* const _last_quat,
- const T* const _sen_pos,
- const T* const _sen_quat,
- T* _residuals) const
-{
-
- // MAPS
- Map<const Matrix<T,3,1> > wtr1(_prev_pos);
- Map<const Quaternion<T> > wqr1(_prev_quat);
- Map<const Matrix<T,3,1> > wtr2(_origin_pos);
- Map<const Quaternion<T> > wqr2(_origin_quat);
- Map<const Matrix<T,3,1> > wtr3(_last_pos);
- Map<const Quaternion<T> > wqr3(_last_quat);
- Map<const Matrix<T,3,1> > rtc (_sen_pos);
- Map<const Quaternion<T> > rqc (_sen_quat);
- Map<Matrix<T,3,1> > res (_residuals);
-
- vision_utils::TrifocalTensorBase<T> tensor;
- Matrix<T, 3, 3> c2Ec1;
- expectation(wtr1, wqr1, wtr2, wqr2, wtr3, wqr3, rtc, rqc, tensor, c2Ec1);
- res = residual(tensor, c2Ec1);
- return true;
-}
-
-template<typename D1, typename D2, class T, typename D3>
-inline void FactorAutodiffTrifocal::expectation(const MatrixBase<D1>& _wtr1,
- const QuaternionBase<D2>& _wqr1,
- const MatrixBase<D1>& _wtr2,
- const QuaternionBase<D2>& _wqr2,
- const MatrixBase<D1>& _wtr3,
- const QuaternionBase<D2>& _wqr3,
- const MatrixBase<D1>& _rtc,
- const QuaternionBase<D2>& _rqc,
- vision_utils::TrifocalTensorBase<T>& _tensor,
- MatrixBase<D3>& _c2Ec1) const
-{
-
- typedef Translation<T, 3> TranslationType;
- typedef Eigen::Transform<T, 3, Eigen::Affine> TransformType;
-
- // All input Transforms
- TransformType wHr1 = TranslationType(_wtr1) * _wqr1;
- TransformType wHr2 = TranslationType(_wtr2) * _wqr2;
- TransformType wHr3 = TranslationType(_wtr3) * _wqr3;
- TransformType rHc = TranslationType(_rtc) * _rqc ;
-
- // Relative camera transforms
- TransformType c1Hc2 = rHc.inverse() * wHr1.inverse() * wHr2 * rHc;
- TransformType c1Hc3 = rHc.inverse() * wHr1.inverse() * wHr3 * rHc;
-
- // Projection matrices
- Matrix<T,3,4> c2Pc1 = c1Hc2.inverse().affine();
- Matrix<T,3,4> c3Pc1 = c1Hc3.inverse().affine();
-
- // Trifocal tensor
- _tensor.computeTensorFromProjectionMat(c2Pc1, c3Pc1);
-
- /* Essential matrix convention disambiguation
- *
- * C1 is the origin frame or reference
- * C2 is another cam
- * C2 is specified by R and T wrt C1 so that
- * T is the position of C2 wrt C1
- * R is the orientation of C2 wrt C1
- * There is a 3D point P, noted P1 when expressed in C1 and P2 when expressed in C2:
- * P1 = T + R * P2
- *
- * Coplanarity condition: a' * (b x c) = 0 with {a,b,c} three coplanar vectors.
- *
- * The three vectors are:
- *
- * baseline: b = T
- * ray 1 : r1 = P1
- * ray 2 : r2 = P1 - T = R*P2;
- *
- * so,
- *
- * (r1)' * (b x r2) = 0 , which develops as:
- *
- * P1' * (T x (R*P2)) = 0
- * P1' * [T]x * R * P2 = 0
- * P1' * c1Ec2 * P2 = 0 <--- Epipolar factor
- *
- * therefore:
- * c1Ec2 = [T]x * R <--- Essential matrix
- *
- * or, if we prefer the factor P2' * c2Ec1 * P1 = 0,
- * c2Ec1 = c1Ec2' = R' * [T]x (we obviate the sign change)
- */
- Matrix<T,3,3> Rtr = c1Hc2.matrix().block(0,0,3,3).transpose();
- Matrix<T,3,1> t = c1Hc2.matrix().block(0,3,3,1);
- _c2Ec1 = Rtr * skew(t);
-// _c2Ec1 = c1Hc2.rotation().transpose() * skew(c1Hc2.translation()) ;
-}
-
-template<typename T, typename D1>
-inline Matrix<T, 3, 1> FactorAutodiffTrifocal::residual(const vision_utils::TrifocalTensorBase<T>& _tensor,
- const MatrixBase<D1>& _c2Ec1) const
-{
- // 1. COMMON COMPUTATIONS
-
- // m1, m2, m3: canonical pixels in cams 1,2,3 -- canonical means m = K.inv * u, with _u_ a homogeneous pixel [ux; uy; 1].
- Matrix<T,3,1> m1(pixel_canonical_prev_ .cast<T>());
- Matrix<T,3,1> m2(pixel_canonical_origin_.cast<T>());
- Matrix<T,3,1> m3(pixel_canonical_last_ .cast<T>());
-
- // 2. TRILINEARITY PLP
-
- Matrix<T,2,1> e1;
- vision_utils::errorTrifocalPLP(m1, m2, m3, _tensor, _c2Ec1, e1);
-
- // 3. EPIPOLAR
- T e2;
- vision_utils::errorEpipolar(m1, m2, _c2Ec1, e2);
-
- // 4. RESIDUAL
-
- Matrix<T,3,1> errors, residual;
- errors << e1, e2;
- residual = sqrt_information_upper.cast<T>() * errors;
-
- return residual;
-}
-
-// Helper functions to be used by the above
-template<class T, typename D1, typename D2, typename D3, typename D4>
-inline Matrix<T, 3, 1> FactorAutodiffTrifocal::error_jacobians(const vision_utils::TrifocalTensorBase<T>& _tensor,
- const MatrixBase<D1>& _c2Ec1,
- MatrixBase<D2>& _J_e_m1,
- MatrixBase<D3>& _J_e_m2,
- MatrixBase<D4>& _J_e_m3)
-{
- // 1. COMMON COMPUTATIONS
-
- // m1, m2, m3: canonical pixels in cams 1,2,3 -- canonical means m = K.inv * u, with _u_ a homogeneous pixel [ux; uy; 1].
- Matrix<T,3,1> m1(pixel_canonical_prev_.cast<T>());
- Matrix<T,3,1> m2(pixel_canonical_origin_.cast<T>());
- Matrix<T,3,1> m3(pixel_canonical_last_.cast<T>());
-
- // 2. TRILINEARITY PLP
-
- Matrix<T,2,3> J_e1_m1, J_e1_m2, J_e1_m3;
- Matrix<T,2,1> e1;
-
- vision_utils::errorTrifocalPLP(m1, m2, m3, _tensor, _c2Ec1, e1, J_e1_m1, J_e1_m2, J_e1_m3);
-
- // 3. EPIPOLAR
-
- T e2;
- Matrix<T,1,3> J_e2_m1, J_e2_m2, J_e2_m3;
-
- vision_utils::errorEpipolar(m1, m2, _c2Ec1, e2, J_e2_m1, J_e2_m2);
-
- J_e2_m3.setZero(); // Not involved in epipolar c1->c2
-
- // Compact Jacobians
- _J_e_m1.topRows(2) = J_e1_m1;
- _J_e_m1.row(2) = J_e2_m1;
- _J_e_m2.topRows(2) = J_e1_m2;
- _J_e_m2.row(2) = J_e2_m2;
- _J_e_m3.topRows(2) = J_e1_m3;
- _J_e_m3.row(2) = J_e2_m3;
-
- // 4. ERRORS
-
- Matrix<T,3,1> errors;
- errors << e1, e2;
-
- return errors;
-
-}
-
-} // namespace wolf
-
-#endif /* _FACTOR_AUTODIFF_TRIFOCAL_H_ */
diff --git a/include/base/factor/factor_corner_2D.h b/include/base/factor/factor_corner_2D.h
deleted file mode 100644
index 2bcacaac7..000000000
--- a/include/base/factor/factor_corner_2D.h
+++ /dev/null
@@ -1,124 +0,0 @@
-#ifndef FACTOR_CORNER_2D_THETA_H_
-#define FACTOR_CORNER_2D_THETA_H_
-
-//Wolf includes
-#include "base/factor/factor_autodiff.h"
-#include "base/landmark/landmark_corner_2D.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorCorner2D);
-
-class FactorCorner2D: public FactorAutodiff<FactorCorner2D, 3,2,1,2,1>
-{
- public:
-
- FactorCorner2D(const FeatureBasePtr _ftr_ptr,
- const LandmarkCorner2DPtr _lmk_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function = false,
- FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorCorner2D,3,2,1,2,1>("CORNER 2D",
- nullptr,
- nullptr,
- nullptr,
- _lmk_ptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _lmk_ptr->getP(),
- _lmk_ptr->getO())
- {
- //
- }
-
- virtual ~FactorCorner2D() = default;
-
- LandmarkCorner2DPtr getLandmark()
- {
- return std::static_pointer_cast<LandmarkCorner2D>( landmark_other_ptr_.lock() );
- }
-
- template <typename T>
- bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkP,
- const T* const _landmarkO, T* _residuals) const;
-
- static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr = nullptr)
- {
- return std::make_shared<FactorCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_correspondant_ptr), _processor_ptr);
- }
-
-};
-
-template<typename T>
-inline bool FactorCorner2D::operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkP,
- const T* const _landmarkO, T* _residuals) const
-{
- // Mapping
- Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position_map(_landmarkP);
- Eigen::Map<const Eigen::Matrix<T,2,1>> robot_position_map(_robotP);
- Eigen::Map<Eigen::Matrix<T,3,1>> residuals_map(_residuals);
-
- //std::cout << "getSensorPosition: " << std::endl;
- //std::cout << getCapture()->getSensor()->getSensorPosition()->head(2).transpose() << std::endl;
- //std::cout << "getSensorRotation: " << std::endl;
- //std::cout << getCapture()->getSensor()->getSensorRotation()->topLeftCorner<2,2>() << std::endl;
- //std::cout << "atan2: " << atan2(getCapture()->getSensor()->getSensorRotation()->transpose()(0,1),getCapture()->getSensor()->getSensorRotation()->transpose()(0,0)) << std::endl;
-
- // sensor transformation
- Eigen::Matrix<T, 2, 1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
- // robot transformation
- Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
-
- // Expected measurement
- Eigen::Matrix<T, 2, 1> expected_measurement_position = inverse_R_sensor * (inverse_R_robot * (landmark_position_map - robot_position_map) - sensor_position);
- T expected_measurement_orientation = _landmarkO[0] - _robotO[0] - T(getCapture()->getSensor()->getO()->getState()(0));
-
- // Error
- residuals_map.head(2) = expected_measurement_position - getMeasurement().head<2>().cast<T>();
- residuals_map(2) = expected_measurement_orientation - T(getMeasurement()(2));
-
- // pi 2 pi
- while (_residuals[2] > T(M_PI))
- _residuals[2] = _residuals[2] - T(2*M_PI);
- while (_residuals[2] <= T(-M_PI))
- _residuals[2] = _residuals[2] + T(2*M_PI);
-
- // Residuals
- residuals_map = getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>().cast<T>() * residuals_map;
-
- //std::cout << "\nFACTOR: " << id() << std::endl;
- //std::cout << "Feature: " << getFeature()->id() << std::endl;
- //std::cout << "Landmark: " << lmk_ptr_->id() << std::endl;
- //std::cout << "measurement:\n\t" << getMeasurement().transpose() << std::endl;
- //
- //std::cout << "robot pose:";
- //for (int i=0; i < 2; i++)
- // std::cout << "\n\t" << _robotP[i];
- //std::cout << "\n\t" << _robotO[0];
- //std::cout << std::endl;
- //
- //std::cout << "landmark pose:";
- //for (int i=0; i < 2; i++)
- // std::cout << "\n\t" << _landmarkP[i];
- //std::cout << "\n\t" << _landmarkO[0];
- //std::cout << std::endl;
- //
- //std::cout << "expected_measurement: ";
- //for (int i=0; i < 2; i++)
- // std::cout << "\n\t" << expected_measurement_position.data()[i];
- //std::cout << std::endl;
- //
- //std::cout << "_residuals: "<< std::endl;
- //for (int i=0; i < 3; i++)
- // std::cout << "\n\t" << _residuals[i] << " ";
- //std::cout << std::endl;
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factor/factor_point_2D.h b/include/base/factor/factor_point_2D.h
deleted file mode 100644
index 4d9686e38..000000000
--- a/include/base/factor/factor_point_2D.h
+++ /dev/null
@@ -1,140 +0,0 @@
-#ifndef FACTOR_POINT_2D_THETA_H_
-#define FACTOR_POINT_2D_THETA_H_
-
-//Wolf includes
-#include "base/factor/factor_autodiff.h"
-#include "base/feature/feature_polyline_2D.h"
-#include "base/landmark/landmark_polyline_2D.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorPoint2D);
-
-/**
- * @brief The FactorPoint2D class
- */
-class FactorPoint2D: public FactorAutodiff<FactorPoint2D, 2,2,1,2,1,2>
-{
- protected:
- unsigned int feature_point_id_;
- int landmark_point_id_;
- StateBlockPtr point_state_ptr_;
- Eigen::VectorXs measurement_; ///< the measurement vector
- Eigen::MatrixXs measurement_covariance_; ///< the measurement covariance matrix
- Eigen::MatrixXs measurement_sqrt_information_; ///< the squared root information matrix
-
- public:
-
- FactorPoint2D(const FeaturePolyline2DPtr& _ftr_ptr,
- const LandmarkPolyline2DPtr& _lmk_ptr,
- const ProcessorBasePtr& _processor_ptr,
- unsigned int _ftr_point_id, int _lmk_point_id, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorPoint2D,2,2,1,2,1,2>("POINT 2D",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlock(_lmk_point_id)),
- feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
- {
- //std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
- //std::cout << "landmark state block " << _lmk_ptr->getPointStateBlock(_lmk_point_id)->getVector().transpose() << std::endl;
- Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
- Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
- measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
- }
-
- virtual ~FactorPoint2D() = default;
-
- /**
- * @brief getLandmarkPtr
- * @return
- */
- LandmarkPolyline2DPtr getLandmark()
- {
- return std::static_pointer_cast<LandmarkPolyline2D>(landmark_other_ptr_.lock());
- }
-
- /**
- * @brief getLandmarkPointId
- * @return
- */
- int getLandmarkPointId()
- {
- return landmark_point_id_;
- }
-
- /**
- * @brief getFeaturePointId
- * @return
- */
- unsigned int getFeaturePointId()
- {
- return feature_point_id_;
- }
-
- /**
- * @brief getLandmarkPointPtr
- * @return
- */
- StateBlockPtr getLandmarkPoint()
- {
- return point_state_ptr_;
- }
-
- /**
- *
- */
- template <typename T>
- bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginP, const T* const _landmarkOriginO, const T* const _landmarkPoint, T* _residuals) const;
-
- /** \brief Returns a reference to the feature measurement
- **/
- virtual const Eigen::VectorXs& getMeasurement() const override
- {
- return measurement_;
- }
-
- /** \brief Returns a reference to the feature measurement covariance
- **/
- virtual const Eigen::MatrixXs& getMeasurementCovariance() const override
- {
- return measurement_covariance_;
- }
-
- /** \brief Returns a reference to the feature measurement square root information
- **/
- virtual const Eigen::MatrixXs& getMeasurementSquareRootInformationUpper() const override
- {
- return measurement_sqrt_information_;
- }
-};
-
-template<typename T>
-inline bool FactorPoint2D::operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginP, const T* const _landmarkOriginO, const T* const _landmarkPoint, T* _residuals) const
-{
- //std::cout << "FactorPointToLine2D::operator" << std::endl;
- // Mapping
- Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_origin_position_map(_landmarkOriginP);
- Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position_map(_landmarkPoint);
- Eigen::Map<const Eigen::Matrix<T,2,1>> robot_position_map(_robotP);
- Eigen::Map<Eigen::Matrix<T,2,1>> residuals_map(_residuals);
-
- // Landmark point global position
- Eigen::Matrix<T,2,1> landmark_point = landmark_origin_position_map + Eigen::Rotation2D<T>(*_landmarkOriginO) * landmark_position_map;
-
- // sensor transformation
- Eigen::Matrix<T,2,1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
- // robot transformation
- Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
-
- // Expected measurement
- Eigen::Matrix<T,2,1> expected_measurement_position = inverse_R_sensor * (inverse_R_robot * (landmark_point - robot_position_map) - sensor_position);
-
- // Residuals
- residuals_map = getMeasurementSquareRootInformationUpper().cast<T>() * (expected_measurement_position - getMeasurement().head<2>().cast<T>());
-
- //std::cout << "residuals_map" << residuals_map[0] << std::endl;
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/factor/factor_point_to_line_2D.h b/include/base/factor/factor_point_to_line_2D.h
deleted file mode 100644
index d4b891f3e..000000000
--- a/include/base/factor/factor_point_to_line_2D.h
+++ /dev/null
@@ -1,149 +0,0 @@
-#ifndef FACTOR_POINT_TO_LINE_2D_H_
-#define FACTOR_POINT_TO_LINE_2D_H_
-
-//Wolf includes
-#include "base/factor/factor_autodiff.h"
-#include "base/feature/feature_polyline_2D.h"
-#include "base/landmark/landmark_polyline_2D.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorPointToLine2D);
-
-//class
-class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,2,2>
-{
- protected:
- int landmark_point_id_;
- int landmark_point_aux_id_;
- unsigned int feature_point_id_;
- StateBlockPtr point_state_ptr_;
- StateBlockPtr point_aux_state_ptr_;
- Eigen::VectorXs measurement_; ///< the measurement vector
- Eigen::MatrixXs measurement_covariance_; ///< the measurement covariance matrix
- Eigen::MatrixXs measurement_sqrt_information_; ///< the squared root information matrix
-
- public:
-
- FactorPointToLine2D(const FeaturePolyline2DPtr& _ftr_ptr,
- const LandmarkPolyline2DPtr& _lmk_ptr,
- const ProcessorBasePtr& _processor_ptr,
- unsigned int _ftr_point_id, int _lmk_point_id, int _lmk_point_aux_id,
- bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,2,2>("POINT TO LINE 2D",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlock(_lmk_point_id), _lmk_ptr->getPointStateBlock(_lmk_point_aux_id)),
- landmark_point_id_(_lmk_point_id), landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), point_aux_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_aux_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
- {
- //std::cout << "FactorPointToLine2D" << std::endl;
- Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
- Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
- measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
- }
-
- virtual ~FactorPointToLine2D() = default;
-
- LandmarkPolyline2DPtr getLandmark()
- {
- return std::static_pointer_cast<LandmarkPolyline2D>( landmark_other_ptr_.lock() );
- }
-
- int getLandmarkPointId()
- {
- return landmark_point_id_;
- }
-
- int getLandmarkPointAuxId()
- {
- return landmark_point_aux_id_;
- }
-
- unsigned int getFeaturePointId()
- {
- return feature_point_id_;
- }
-
- StateBlockPtr getLandmarkPoint()
- {
- return point_state_ptr_;
- }
-
- StateBlockPtr getLandmarkPointAux()
- {
- return point_state_ptr_;
- }
-
- template <typename T>
- bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginPosition, const T* const _landmarkOriginOrientation, const T* const _landmarkPoint, const T* const _landmarkPointAux, T* _residuals) const;
-
- /** \brief Returns a reference to the feature measurement
- **/
- virtual const Eigen::VectorXs& getMeasurement() const override
- {
- return measurement_;
- }
-
- /** \brief Returns a reference to the feature measurement covariance
- **/
- virtual const Eigen::MatrixXs& getMeasurementCovariance() const override
- {
- return measurement_covariance_;
- }
-
- /** \brief Returns a reference to the feature measurement square root information
- **/
- virtual const Eigen::MatrixXs& getMeasurementSquareRootInformationUpper() const override
- {
- return measurement_sqrt_information_;
- }
-};
-
-template<typename T>
-inline bool FactorPointToLine2D::operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginPosition, const T* const _landmarkOriginOrientation, const T* const _landmarkPoint, const T* const _landmarkPointAux, T* _residuals) const
-{
- //std::cout << "FactorPointToLine2D::operator" << std::endl;
- // Mapping
- Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_origin_position_map(_landmarkOriginPosition);
- Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position_map(_landmarkPoint);
- Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_aux_position_map(_landmarkPointAux);
- Eigen::Map<const Eigen::Matrix<T,2,1>> robot_position_map(_robotP);
-
- Eigen::Matrix<T,2,1> landmark_point = landmark_origin_position_map + Eigen::Rotation2D<T>(*_landmarkOriginOrientation) * landmark_position_map;
- Eigen::Matrix<T,2,1> landmark_point_aux = landmark_origin_position_map + Eigen::Rotation2D<T>(*_landmarkOriginOrientation) * landmark_aux_position_map;
-
- // sensor transformation
- Eigen::Matrix<T,2,1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
- // robot transformation
- Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
-
- // Expected measurement
- Eigen::Matrix<T,2,1> expected_P = inverse_R_sensor * (inverse_R_robot * (landmark_point - robot_position_map) - sensor_position);
- Eigen::Matrix<T,2,1> expected_Paux = inverse_R_sensor * (inverse_R_robot * (landmark_point_aux - robot_position_map) - sensor_position);
-
- // Case projection inside the segment P-Paux
-
- // Case projection ouside (P side)
-
- // Case projection ouside (Paux side)
-
- // normal vector
- Eigen::Matrix<T,2,1> normal(expected_Paux(1)-expected_P(1), expected_P(0)-expected_Paux(0));
- normal = normal / normal.norm();
-
- // Residual: distance = projection to the normal vector
- _residuals[0] = ((expected_P-measurement_.head<2>().cast<T>()).dot(normal));
-
- T projected_cov = normal.transpose() * measurement_covariance_.cast<T>() * normal;
- _residuals[0] = _residuals[0] / sqrt(projected_cov);
-
- //std::cout << "landmark points:" << std::endl;
- //std::cout << "A: " << expected_P(0) << " " << expected_P(1) << std::endl;
- //std::cout << "Aaux: " << expected_Paux(0) << " " << expected_Paux(1) << std::endl;
- //std::cout << "B: " << measurement_.transpose() << std::endl;
- //std::cout << "d: " << _residuals[0] << std::endl;
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/feature/feature_GPS_fix.h b/include/base/feature/feature_GPS_fix.h
deleted file mode 100644
index d69446548..000000000
--- a/include/base/feature/feature_GPS_fix.h
+++ /dev/null
@@ -1,31 +0,0 @@
-#ifndef FEATURE_GPS_FIX_H_
-#define FEATURE_GPS_FIX_H_
-
-//Wolf includes
-#include "base/factor/factor_GPS_2D.h"
-#include "base/feature/feature_base.h"
-
-//std includes
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FeatureGPSFix);
-
-//class FeatureGPSFix
-class FeatureGPSFix : public FeatureBase
-{
- public:
-
- /** \brief Constructor from capture pointer and measure
- *
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- *
- */
- FeatureGPSFix(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
- virtual ~FeatureGPSFix();
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/feature/feature_GPS_pseudorange.h b/include/base/feature/feature_GPS_pseudorange.h
deleted file mode 100644
index 57f327e25..000000000
--- a/include/base/feature/feature_GPS_pseudorange.h
+++ /dev/null
@@ -1,36 +0,0 @@
-
-#ifndef FEATURE_GPS_PSEUDORANGE_H_
-#define FEATURE_GPS_PSEUDORANGE_H_
-
-//Wolf includes
-#include "base/feature/feature_base.h"
-
-//std includes
-#include <iomanip>
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FeatureGPSPseudorange);
-
-// TODO manage covariance
-class FeatureGPSPseudorange : public FeatureBase
-{
-protected:
- Eigen::Vector3s sat_position_;
- Scalar pseudorange_;
-
-public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- FeatureGPSPseudorange(Eigen::Vector3s& _sat_position, Scalar _pseudorange, Scalar _covariance);
- virtual ~FeatureGPSPseudorange();
-
- const Eigen::Vector3s & getSatPosition() const;
-
- Scalar getPseudorange() const;
-
-};
-
-} // namespace wolf
-
-#endif //FEATURE_GPS_PSEUDORANGE_H_
diff --git a/include/base/feature/feature_IMU.h b/include/base/feature/feature_IMU.h
deleted file mode 100644
index c82083332..000000000
--- a/include/base/feature/feature_IMU.h
+++ /dev/null
@@ -1,76 +0,0 @@
-#ifndef FEATURE_IMU_H_
-#define FEATURE_IMU_H_
-
-//Wolf includes
-#include "base/capture/capture_IMU.h"
-#include "base/feature/feature_base.h"
-#include "base/common/wolf.h"
-
-//std includes
-
-namespace wolf {
-
-//WOLF_PTR_TYPEDEFS(CaptureIMU);
-WOLF_PTR_TYPEDEFS(FeatureIMU);
-
-class FeatureIMU : public FeatureBase
-{
- public:
-
- /** \brief Constructor from and measures
- *
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- * \param _dD_db_jacobians Jacobians of preintegrated delta wrt IMU biases
- * \param acc_bias accelerometer bias of origin frame
- * \param gyro_bias gyroscope bias of origin frame
- * \param _cap_imu_ptr pointer to parent CaptureMotion
- */
- FeatureIMU(const Eigen::VectorXs& _delta_preintegrated,
- const Eigen::MatrixXs& _delta_preintegrated_covariance,
- const Eigen::Vector6s& _bias,
- const Eigen::Matrix<Scalar,9,6>& _dD_db_jacobians,
- CaptureMotionPtr _cap_imu_ptr = nullptr);
-
- /** \brief Constructor from capture pointer
- *
- * \param _cap_imu_ptr pointer to parent CaptureMotion
- */
- FeatureIMU(CaptureMotionPtr _cap_imu_ptr);
-
- virtual ~FeatureIMU();
-
- const Eigen::Vector3s& getAccBiasPreint() const;
- const Eigen::Vector3s& getGyroBiasPreint() const;
- const Eigen::Matrix<Scalar, 9, 6>& getJacobianBias() const;
-
- private:
-
- // Used biases
- Eigen::Vector3s acc_bias_preint_; ///< Acceleration bias used for delta preintegration
- Eigen::Vector3s gyro_bias_preint_; ///< Gyrometer bias used for delta preintegration
-
- Eigen::Matrix<Scalar, 9, 6> jacobian_bias_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
-};
-
-inline const Eigen::Vector3s& FeatureIMU::getAccBiasPreint() const
-{
- return acc_bias_preint_;
-}
-
-inline const Eigen::Vector3s& FeatureIMU::getGyroBiasPreint() const
-{
- return gyro_bias_preint_;
-}
-
-inline const Eigen::Matrix<Scalar, 9, 6>& FeatureIMU::getJacobianBias() const
-{
- return jacobian_bias_;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/feature/feature_corner_2D.h b/include/base/feature/feature_corner_2D.h
deleted file mode 100644
index 8e206749d..000000000
--- a/include/base/feature/feature_corner_2D.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#ifndef FEATURE_CORNER_2D_H_
-#define FEATURE_CORNER_2D_H_
-
-//Wolf includes
-#include "base/feature/feature_base.h"
-
-//std includes
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FeatureCorner2D);
-
-//class FeatureCorner2D
-class FeatureCorner2D : public FeatureBase
-{
- public:
-
- FeatureCorner2D(const Eigen::Vector4s & _measurement, const Eigen::Matrix4s & _meas_covariance);
- virtual ~FeatureCorner2D();
-
- /** \brief Returns aperture
- *
- * Returns aperture
- *
- **/
- Scalar getAperture() const;
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/feature/feature_line_2D.h b/include/base/feature/feature_line_2D.h
deleted file mode 100644
index 5d5e6162d..000000000
--- a/include/base/feature/feature_line_2D.h
+++ /dev/null
@@ -1,53 +0,0 @@
-#ifndef FEATURE_LINE_2D_H_
-#define FEATURE_LINE_2D_H_
-
-//Wolf includes
-#include "base/feature/feature_base.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FeatureLine2D);
-
-/** \brief class FeatureLine2D
- *
- * Line segment feature.
- *
- * Measurement is a 3-vector of its homogeneous coordinates,
- * normalized according http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
- *
- * Descriptor are the homogeneous coordinates of its first and last point, which may or may not be extreme points of the line
- *
- **/
-class FeatureLine2D : public FeatureBase
-{
- protected:
- Eigen::Vector3s first_point_;
- Eigen::Vector3s last_point_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- /** \brief Constructor
- *
- * Constructor, with measurement and descriptor
- *
- */
- FeatureLine2D(const Eigen::Vector3s & _line_homogeneous_params, const Eigen::Matrix3s & _params_covariance,
- Eigen::Vector3s & _point1, Eigen::Vector3s & _point2);
-
- virtual ~FeatureLine2D();
-
- /** \brief Returns a const reference to first_point_
- */
- const Eigen::Vector3s & firstPoint() const;
-
- /** \brief Returns a const reference to last_point_
- */
- const Eigen::Vector3s & lastPoint() const;
-
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/feature/feature_polyline_2D.h b/include/base/feature/feature_polyline_2D.h
deleted file mode 100644
index d289a58f3..000000000
--- a/include/base/feature/feature_polyline_2D.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/**
- * \file feature_polyline_2D.h
- *
- * Created on: May 26, 2016
- * \author: jvallve
- */
-
-#ifndef FEATURE_POLYLINE_2D_H_
-#define FEATURE_POLYLINE_2D_H_
-
-#include "base/feature/feature_base.h"
-
-namespace wolf
-{
-WOLF_PTR_TYPEDEFS(FeaturePolyline2D);
-
-//class
-class FeaturePolyline2D : public FeatureBase
-{
- protected:
- Eigen::MatrixXs points_;
- Eigen::MatrixXs points_cov_;
- bool first_defined_;
- bool last_defined_;
-
- public:
- FeaturePolyline2D(const Eigen::MatrixXs& _points, const Eigen::MatrixXs& _points_cov, const bool& _first_defined, const bool& _last_defined);
- virtual ~FeaturePolyline2D();
- const Eigen::MatrixXs& getPoints() const;
- const Eigen::MatrixXs& getPointsCov() const;
- bool isFirstDefined() const;
- bool isLastDefined() const;
- int getNPoints() const;
-};
-
-inline FeaturePolyline2D::FeaturePolyline2D(const Eigen::MatrixXs& _points, const Eigen::MatrixXs& _points_cov, const bool& _first_defined, const bool& _last_defined) :
- FeatureBase("POLYLINE 2D", Eigen::VectorXs(), Eigen::MatrixXs()),
- points_(_points),
- points_cov_(_points_cov),
- first_defined_(_first_defined),
- last_defined_(_last_defined)
-{
- assert(points_.rows() == 3 && points_cov_.rows() == 2 && points_cov_.cols() == 2*points_.cols() && "FeaturePolyline2D::FeaturePolyline2D: Bad points or covariance matrix size");
-}
-
-inline FeaturePolyline2D::~FeaturePolyline2D()
-{
- //
-}
-
-inline const Eigen::MatrixXs& FeaturePolyline2D::getPoints() const
-{
- return points_;
-}
-
-inline const Eigen::MatrixXs& FeaturePolyline2D::getPointsCov() const
-{
- return points_cov_;
-}
-
-inline bool FeaturePolyline2D::isFirstDefined() const
-{
- return first_defined_;
-}
-
-inline bool FeaturePolyline2D::isLastDefined() const
-{
- return last_defined_;
-}
-
-inline int FeaturePolyline2D::getNPoints() const
-{
- return points_.cols();
-}
-
-} /* namespace wolf */
-
-#endif /* FEATURE_POLYLINE_2D_H_ */
diff --git a/include/base/landmark/landmark_container.h b/include/base/landmark/landmark_container.h
deleted file mode 100644
index 122b77071..000000000
--- a/include/base/landmark/landmark_container.h
+++ /dev/null
@@ -1,107 +0,0 @@
-
-#ifndef LANDMARK_CONTAINER_H_
-#define LANDMARK_CONTAINER_H_
-
-//Wolf includes
-#include "base/landmark/landmark_base.h"
-#include "base/common/wolf.h"
-
-// Std includes
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(LandmarkContainer);
-
-//class LandmarkContainer
-class LandmarkContainer : public LandmarkBase
-{
- protected:
- Eigen::MatrixXs corners_;
-
- public:
- /** \brief Constructor with type, time stamp and the position state pointer
- *
- * Constructor with type, and state pointer
- * \param _p_ptr StateBlock pointer to the position
- * \param _o_ptr StateBlock pointer to the orientation
- * \param _witdh descriptor of the landmark: container width
- * \param _length descriptor of the landmark: container length
- *
- **/
- LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _witdh=2.44, const Scalar& _length=12.2);
-
- /** \brief Constructor with type, time stamp and the position state pointer
- *
- * The vertices are refered as A, B, C and D:
- *
- * B ---------------------------- A
- * | |
- * | <---+ |
- * | | |
- * | v |
- * C ---------------------------- D
- *
- * Constructor with type, and state pointer
- * \param _p_ptr StateBlock pointer to the position
- * \param _o_ptr StateBlock pointer to the orientation
- * \param _corner_1_pose pose of corner 1
- * \param _corner_2_pose pose of corner 2
- * \param _corner_1_idx index of corner 1 (A = 0, B = 1, C = 2, D = 3)
- * \param _corner_2_idx index of corner 2 (A = 0, B = 1, C = 2, D = 3)
- * \param _witdh descriptor of the landmark: container width
- * \param _length descriptor of the landmark: container length
- *
- **/
- LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const Scalar& _witdh=2.44, const Scalar& _length=12.2);
-
- /** \brief Constructor with type, time stamp and the position state pointer
- *
- * Constructor with type, and state pointer
- * \param _p_ptr StateBlock pointer to the position
- * \param _o_ptr StateBlock pointer to the orientation
- * \param _corner_A_ptr LandmarkCorner2D pointer to one of its corners
- * \param _corner_B_ptr LandmarkCorner2D pointer to one of its corners
- * \param _corner_C_ptr LandmarkCorner2D pointer to one of its corners
- * \param _corner_D_ptr LandmarkCorner2D pointer to one of its corners
- * \param _witdh descriptor of the landmark: container width
- * \param _length descriptor of the landmark: container length
- *
- **/
- //LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const Scalar& _witdh=2.44, const Scalar& _length=12.2);
-
- virtual ~LandmarkContainer();
-
- /** \brief Returns the container width
- *
- * Returns the container width
- *
- **/
- Scalar getWidth() const;
-
- /** \brief Returns the container length
- *
- * Returns the container length
- *
- **/
- Scalar getLength() const;
-
- /** \brief Returns the container corners in container coordinates
- *
- * Returns the container corners in container coordinates
- *
- **/
- Eigen::MatrixXs getCorners() const;
-
- /** \brief Returns a corner in container coordinates
- *
- * Returns a corner in container coordinates
- * \param _id the index of the corner to be provided
- *
- **/
- Eigen::VectorXs getCorner(const unsigned int _id) const;
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/landmark/landmark_corner_2D.h b/include/base/landmark/landmark_corner_2D.h
deleted file mode 100644
index c1ce86fbd..000000000
--- a/include/base/landmark/landmark_corner_2D.h
+++ /dev/null
@@ -1,41 +0,0 @@
-
-#ifndef LANDMARK_CORNER_H_
-#define LANDMARK_CORNER_H_
-
-//Wolf includes
-#include "base/landmark/landmark_base.h"
-
-// Std includes
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(LandmarkCorner2D);
-
-//class LandmarkCorner2D
-class LandmarkCorner2D : public LandmarkBase
-{
- public:
- /** \brief Constructor with type, time stamp and the position state pointer
- *
- * Constructor with type, and state pointer
- * \param _p_ptr StateBlock shared pointer to the position
- * \param _o_ptr StateBlock shared pointer to the orientation
- * \param _aperture descriptor of the landmark: aperture of the corner
- *
- **/
- LandmarkCorner2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _aperture=0);
-
- virtual ~LandmarkCorner2D();
-
- /** \brief Returns aperture
- *
- * Returns aperture
- *
- **/
- Scalar getAperture() const;
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/landmark/landmark_line_2D.h b/include/base/landmark/landmark_line_2D.h
deleted file mode 100644
index 7caf3b290..000000000
--- a/include/base/landmark/landmark_line_2D.h
+++ /dev/null
@@ -1,56 +0,0 @@
-
-#ifndef LANDMARK_LINE_2D_H_
-#define LANDMARK_LINE_2D_H_
-
-//Wolf includes
-#include "base/landmark/landmark_base.h"
-#include "base/common/wolf.h"
-
-//std includes
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(LandmarkLine2D);
-
-//class LandmarkLine2D
-class LandmarkLine2D : public LandmarkBase
-{
- protected:
- //extreme points of the line segment
- Eigen::Vector3s point1_;
- Eigen::Vector3s point2_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- /** \brief Constructor with homogeneous parameters of the line
- *
- * \param _p_ptr homogeneous parameters of the line: (a,b,c) from ax+by+c=0, normalized according line/sqrt(a*a+b*b)
- * See http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/BEARDSLEY/node2.html
- * \param _point1 Extreme point 1 in homogeneous coordinates (3-vector)
- * \param _point2 Extreme point 2 in homogeneous coordinates (3-vector)
- *
- **/
- LandmarkLine2D(StateBlockPtr _p_ptr, Eigen::Vector3s & _point1, Eigen::Vector3s & _point2);
-
- virtual ~LandmarkLine2D();
-
- /** \brief Line segment update
- * Update extreme points according new inputs.
- * Assumes normalized points (x/z, y/z, 1)
- **/
- void updateExtremePoints(Eigen::Vector3s & _q1, Eigen::Vector3s & _q2);
-
- /** \brief Gets extreme point1
- **/
- const Eigen::Vector3s & point1() const;
-
- /** \brief Gets extreme point2
- **/
- const Eigen::Vector3s & point2() const;
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/landmark/landmark_polyline_2D.h b/include/base/landmark/landmark_polyline_2D.h
deleted file mode 100644
index d6b5d51f9..000000000
--- a/include/base/landmark/landmark_polyline_2D.h
+++ /dev/null
@@ -1,191 +0,0 @@
-/**
- * \file landmark_poliyline_2D.h
- *
- * Created on: May 26, 2016
- * \author: jvallve
- */
-
-#ifndef LANDMARK_POLYLINE_2D_H_
-#define LANDMARK_POLYLINE_2D_H_
-
-// Wolf
-#include "base/landmark/landmark_base.h"
-
-// STL
-#include <deque>
-
-// yaml-cpp library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-
-//landmark classification
-typedef enum
-{
- UNCLASSIFIED,
- CONTAINER, ///< A container 12.2 x 2.44 (m)
- SMALL_CONTAINER, ///< A small container 6.1 x 2.44 (m)
- PALLET, ///< A pallet box 0.9 x 1.2 (m)
-} LandmarkClassification;
-
-WOLF_PTR_TYPEDEFS(LandmarkPolyline2D);
-
-//class
-class LandmarkPolyline2D : public LandmarkBase
-{
- protected:
- std::deque<StateBlockPtr> point_state_ptr_vector_; ///< polyline points state blocks
- int first_id_;
- bool first_defined_; ///< Wether the first point is an extreme of a line or the line may continue
- bool last_defined_; ///< Wether the last point is an extreme of a line or the line may continue
- bool closed_; ///< Wether the polyline is closed or not
- LandmarkClassification classification_; ///< The classification of the landmark
-
- public:
- LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id = 0, LandmarkClassification _class = UNCLASSIFIED);
- virtual ~LandmarkPolyline2D();
-
- /** \brief Gets a const reference to the point state block pointer vector
- **/
- std::deque<StateBlockPtr>& getPointStatePtrDeque();
-
- /** \brief Gets wether the first/last points are defined or not
- **/
- bool isFirstDefined() const;
- bool isLastDefined() const;
-
- /** \brief Gets whether the polyline is closed or not
- **/
- bool isClosed() const;
-
- /** \brief Gets whether the given state block point is defined or not (assumes the state block is in the landmark)
- **/
- bool isDefined(StateBlockPtr _state_block) const;
-
- /** \brief Sets the first/last extreme point
- **/
- void setFirst(const Eigen::VectorXs& _point, bool _defined);
- void setLast(const Eigen::VectorXs& _point, bool _defined);
-
- int getNPoints() const;
- int getFirstId() const;
- int getLastId() const;
-
- const Eigen::VectorXs getPointVector(int _i) const;
-
- StateBlockPtr getPointStateBlock(int _i);
-
- /** \brief Gets a vector of all state blocks pointers
- **/
- virtual std::vector<StateBlockPtr> getPointsStateBlockVector() const;
-
- /** \brief Adds a new point to the landmark
- * \param _point: the point to be added
- * \param _extreme: if its extreme or not
- * \param _back: if it have to be added in the back (true) or in the front (false)
- **/
- void addPoint(const Eigen::VectorXs& _point, const bool& _defined, const bool& _back);
-
- /** \brief Adds new points to the landmark
- * \param _points: a matrix containing points, some of them to be added
- * \param _idx: from wich position of the first point to be added
- * \param _extreme: if last point to be added is extreme or not
- * \param _back: if the points have to be added in the back (true) or in the front (false)
- **/
- void addPoints(const Eigen::MatrixXs& _points, const unsigned int& _idx, const bool& _defined, const bool& _back);
-
- /** \brief Gets a vector of all state blocks pointers
- **/
- virtual void defineExtreme(const bool _back);
-
- /** \brief Set the polyline as closed
- **/
- virtual void setClosed();
-
- /** \brief merge points
- **/
- void mergePoints(int _remove_id, int _remain_id);
-
- /** \brief Classify as a known object
- **/
- void classify(LandmarkClassification _class);
-
- /** \brief get classification
- **/
- LandmarkClassification getClassification() const;
-
- /** \brief Adds all stateBlocks of the frame to the wolfProblem list of new stateBlocks
- **/
- virtual void registerNewStateBlocks();
- virtual void removeStateBlocks();
-
- /** Factory method to create new landmarks from YAML nodes
- */
- static LandmarkBasePtr create(const YAML::Node& _lmk_node);
-
- YAML::Node saveToYaml() const;
-};
-
-inline std::deque<StateBlockPtr>& LandmarkPolyline2D::getPointStatePtrDeque()
-{
- return point_state_ptr_vector_;
-}
-
-inline bool LandmarkPolyline2D::isFirstDefined() const
-{
- return first_defined_;
-}
-
-inline bool LandmarkPolyline2D::isLastDefined() const
-{
- return last_defined_;
-}
-
-inline bool LandmarkPolyline2D::isClosed() const
-{
- return closed_;
-}
-
-inline bool LandmarkPolyline2D::isDefined(StateBlockPtr _state_block) const
-{
- if (_state_block == point_state_ptr_vector_.front())
- return first_defined_;
-
- if (_state_block == point_state_ptr_vector_.back())
- return last_defined_;
-
- return true;
-}
-
-inline int LandmarkPolyline2D::getNPoints() const
-{
- return (int)point_state_ptr_vector_.size();
-}
-
-inline int LandmarkPolyline2D::getFirstId() const {
- return first_id_;
-}
-
-inline int LandmarkPolyline2D::getLastId() const {
- return first_id_ + (int) (point_state_ptr_vector_.size()) - 1;
-}
-
-inline std::vector<StateBlockPtr> LandmarkPolyline2D::getPointsStateBlockVector() const
-{
- return std::vector<StateBlockPtr>(point_state_ptr_vector_.begin(), point_state_ptr_vector_.end());
-}
-
-inline void LandmarkPolyline2D::classify(LandmarkClassification _class)
-{
- classification_ = _class;
-}
-
-inline LandmarkClassification LandmarkPolyline2D::getClassification() const
-{
- return classification_;
-}
-
-} /* namespace wolf */
-
-#endif /* LANDMARK_POLYLINE_2D_H_ */
diff --git a/include/base/math/IMU_tools.h b/include/base/math/IMU_tools.h
deleted file mode 100644
index 815ed2aa2..000000000
--- a/include/base/math/IMU_tools.h
+++ /dev/null
@@ -1,610 +0,0 @@
-/*
- * imu_tools.h
- *
- * Created on: Jul 29, 2017
- * Author: jsola
- */
-
-#ifndef IMU_TOOLS_H_
-#define IMU_TOOLS_H_
-
-#include "base/common/wolf.h"
-#include "base/math/rotations.h"
-
-/*
- * Most functions in this file are explained in the document:
- *
- * Joan Sola, "IMU pre-integration", 2015-2017 IRI-CSIC
- *
- * They relate manipulations of Delta motion magnitudes used for IMU pre-integration.
- *
- * The Delta is defined as
- * Delta = [Dp, Dq, Dv]
- * with
- * Dp : position delta
- * Dq : quaternion delta
- * Dv : velocity delta
- *
- * They are listed below:
- *
- * - identity: I = Delta at the origin, with Dp = [0,0,0]; Dq = [0,0,0,1], Dv = [0,0,0]
- * - inverse: so that D (+) D.inv = I
- * - compose: Dc = D1 (+) D2
- * - between: Db = D2 (-) D1, so that D2 = D1 (+) Db
- * - composeOverState: x2 = x1 (+) D
- * - betweenStates: D = x2 (-) x1, so that x2 = x1 (+) D
- * - log: got from Delta manifold to tangent space (equivalent to log() in rotations)
- * - exp_IMU: go from tangent space to delta manifold (equivalent to exp() in rotations)
- * - plus: D2 = D1 (+) exp_IMU(d)
- * - diff: d = log_IMU( D2 (-) D1 )
- * - body2delta: construct a delta from body magnitudes of linAcc and angVel
- */
-
-namespace wolf
-{
-namespace imu {
-using namespace Eigen;
-
-template<typename D1, typename D2, typename D3>
-inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q, MatrixBase<D3>& v)
-{
- p = MatrixBase<D1>::Zero(3,1);
- q = QuaternionBase<D2>::Identity();
- v = MatrixBase<D3>::Zero(3,1);
-}
-
-template<typename D1, typename D2, typename D3>
-inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q, MatrixBase<D3>& v)
-{
- typedef typename D1::Scalar T1;
- typedef typename D2::Scalar T2;
- typedef typename D3::Scalar T3;
- p << T1(0), T1(0), T1(0);
- q << T2(0), T2(0), T2(0), T2(1);
- v << T3(0), T3(0), T3(0);
-}
-
-template<typename T = Scalar>
-inline Matrix<T, 10, 1> identity()
-{
- Matrix<T, 10, 1> ret;
- ret<< T(0), T(0), T(0),
- T(0), T(0), T(0), T(1),
- T(0), T(0), T(0);
- return ret;
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, class T>
-inline void inverse(const MatrixBase<D1>& dp, const QuaternionBase<D2>& dq, const MatrixBase<D3>& dv,
- const T dt,
- MatrixBase<D4>& idp, QuaternionBase<D5>& idq, MatrixBase<D6>& idv )
-{
- MatrixSizeCheck<3, 1>::check(dp);
- MatrixSizeCheck<3, 1>::check(dv);
- MatrixSizeCheck<3, 1>::check(idp);
- MatrixSizeCheck<3, 1>::check(idv);
-
- idp = - ( dq.conjugate() * (dp - dv * typename D3::Scalar(dt) ) );
- idq = dq.conjugate();
- idv = - ( dq.conjugate() * dv );
-}
-
-template<typename D1, typename D2, class T>
-inline void inverse(const MatrixBase<D1>& d,
- T dt,
- MatrixBase<D2>& id)
-{
- MatrixSizeCheck<10, 1>::check(d);
- MatrixSizeCheck<10, 1>::check(id);
-
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( & d( 0 ) );
- Map<const Quaternion<typename D1::Scalar> > dq ( & d( 3 ) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dv ( & d( 7 ) );
- Map<Matrix<typename D2::Scalar, 3, 1> > idp ( & id( 0 ) );
- Map<Quaternion<typename D2::Scalar> > idq ( & id( 3 ) );
- Map<Matrix<typename D2::Scalar, 3, 1> > idv ( & id( 7 ) );
-
- inverse(dp, dq, dv, dt, idp, idq, idv);
-}
-
-template<typename D, class T>
-inline Matrix<typename D::Scalar, 10, 1> inverse(const MatrixBase<D>& d,
- T dt)
-{
- Matrix<typename D::Scalar, 10, 1> id;
- inverse(d, dt, id);
- return id;
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, class T>
-inline void compose(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
- const T dt,
- MatrixBase<D7>& sum_p, QuaternionBase<D8>& sum_q, MatrixBase<D9>& sum_v )
-{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dv1);
- MatrixSizeCheck<3, 1>::check(dp2);
- MatrixSizeCheck<3, 1>::check(dv2);
- MatrixSizeCheck<3, 1>::check(sum_p);
- MatrixSizeCheck<3, 1>::check(sum_v);
-
- sum_p = dp1 + dv1*dt + dq1*dp2;
- sum_v = dv1 + dq1*dv2;
- sum_q = dq1*dq2; // dq here to avoid possible aliasing between d1 and sum
-}
-
-template<typename D1, typename D2, typename D3, class T>
-inline void compose(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- T dt,
- MatrixBase<D3>& sum)
-{
- MatrixSizeCheck<10, 1>::check(d1);
- MatrixSizeCheck<10, 1>::check(d2);
- MatrixSizeCheck<10, 1>::check(sum);
-
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1( 0 ) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1( 3 ) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1( 7 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2( 0 ) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2( 3 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2( 7 ) );
- Map<Matrix<typename D3::Scalar, 3, 1> > sum_p ( & sum( 0 ) );
- Map<Quaternion<typename D3::Scalar> > sum_q ( & sum( 3 ) );
- Map<Matrix<typename D3::Scalar, 3, 1> > sum_v ( & sum( 7 ) );
-
- compose(dp1, dq1, dv1, dp2, dq2, dv2, dt, sum_p, sum_q, sum_v);
-}
-
-template<typename D1, typename D2, class T>
-inline Matrix<typename D1::Scalar, 10, 1> compose(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- T dt)
-{
- Matrix<typename D1::Scalar, 10, 1> ret;
- compose(d1, d2, dt, ret);
- return ret;
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, class T>
-inline void compose(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- T dt,
- MatrixBase<D3>& sum,
- MatrixBase<D4>& J_sum_d1,
- MatrixBase<D5>& J_sum_d2)
-{
- MatrixSizeCheck<10, 1>::check(d1);
- MatrixSizeCheck<10, 1>::check(d2);
- MatrixSizeCheck<10, 1>::check(sum);
- MatrixSizeCheck< 9, 9>::check(J_sum_d1);
- MatrixSizeCheck< 9, 9>::check(J_sum_d2);
-
- // Some useful temporaries
- Matrix<typename D1::Scalar, 3, 3> dR1 = q2R(d1.segment(3,4)); //dq1.matrix(); // First Delta, DR
- Matrix<typename D2::Scalar, 3, 3> dR2 = q2R(d2.segment(3,4)); //dq2.matrix(); // Second delta, dR
-
- // Jac wrt first delta
- J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I
- J_sum_d1.block(0,3,3,3).noalias() = - dR1 * skew(d2.head(3)) ; // dDp'/dDo
- J_sum_d1.block(0,6,3,3) = Matrix3s::Identity() * dt; // dDp'/dDv = I*dt
- J_sum_d1.block(3,3,3,3) = dR2.transpose(); // dDo'/dDo
- J_sum_d1.block(6,3,3,3).noalias() = - dR1 * skew(d2.tail(3)) ; // dDv'/dDo
-
- // Jac wrt second delta
- J_sum_d2.setIdentity(); //
- J_sum_d2.block(0,0,3,3) = dR1; // dDp'/ddp
- J_sum_d2.block(6,6,3,3) = dR1; // dDv'/ddv
- // J_sum_d2.block(3,3,3,3) = Matrix3s::Identity(); // dDo'/ddo = I
-
- // compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
- compose(d1, d2, dt, sum);
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, class T>
-inline void between(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
- const T dt,
- MatrixBase<D7>& diff_p, QuaternionBase<D8>& diff_q, MatrixBase<D9>& diff_v )
-{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dv1);
- MatrixSizeCheck<3, 1>::check(dp2);
- MatrixSizeCheck<3, 1>::check(dv2);
- MatrixSizeCheck<3, 1>::check(diff_p);
- MatrixSizeCheck<3, 1>::check(diff_v);
-
- diff_p = dq1.conjugate() * ( dp2 - dp1 - dv1*dt );
- diff_v = dq1.conjugate() * ( dv2 - dv1 );
- diff_q = dq1.conjugate() * dq2;
-}
-
-template<typename D1, typename D2, typename D3, class T>
-inline void between(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- T dt,
- MatrixBase<D3>& d2_minus_d1)
-{
- MatrixSizeCheck<10, 1>::check(d1);
- MatrixSizeCheck<10, 1>::check(d2);
- MatrixSizeCheck<10, 1>::check(d2_minus_d1);
-
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(7) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & d2_minus_d1(0) );
- Map<Quaternion<typename D3::Scalar> > diff_q ( & d2_minus_d1(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & d2_minus_d1(7) );
-
- between(dp1, dq1, dv1, dp2, dq2, dv2, dt, diff_p, diff_q, diff_v);
-}
-
-template<typename D1, typename D2, class T>
-inline Matrix<typename D1::Scalar, 10, 1> between(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- T dt)
-{
- Matrix<typename D1::Scalar, 10, 1> diff;
- between(d1, d2, dt, diff);
- return diff;
-}
-
-template<typename D1, typename D2, typename D3, class T>
-inline void composeOverState(const MatrixBase<D1>& x,
- const MatrixBase<D2>& d,
- T dt,
- MatrixBase<D3>& x_plus_d)
-{
- MatrixSizeCheck<10, 1>::check(x);
- MatrixSizeCheck<10, 1>::check(d);
- MatrixSizeCheck<10, 1>::check(x_plus_d);
-
- Map<const Matrix<typename D1::Scalar, 3, 1> > p ( & x( 0 ) );
- Map<const Quaternion<typename D1::Scalar> > q ( & x( 3 ) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > v ( & x( 7 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp ( & d( 0 ) );
- Map<const Quaternion<typename D2::Scalar> > dq ( & d( 3 ) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dv ( & d( 7 ) );
- Map<Matrix<typename D3::Scalar, 3, 1> > p_plus_d ( & x_plus_d( 0 ) );
- Map<Quaternion<typename D3::Scalar> > q_plus_d ( & x_plus_d( 3 ) );
- Map<Matrix<typename D3::Scalar, 3, 1> > v_plus_d ( & x_plus_d( 7 ) );
-
- p_plus_d = p + v*dt + 0.5*gravity()*dt*dt + q*dp;
- v_plus_d = v + gravity()*dt + q*dv;
- q_plus_d = q*dq; // dq here to avoid possible aliasing between x and x_plus_d
-}
-
-template<typename D1, typename D2, class T>
-inline Matrix<typename D1::Scalar, 10, 1> composeOverState(const MatrixBase<D1>& x,
- const MatrixBase<D2>& d,
- T dt)
-{
- Matrix<typename D1::Scalar, 10, 1> ret;
- composeOverState(x, d, dt, ret);
- return ret;
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, class T>
-inline void betweenStates(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1, const MatrixBase<D3>& v1,
- const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2, const MatrixBase<D6>& v2,
- const T dt,
- MatrixBase<D7>& dp, QuaternionBase<D8>& dq, MatrixBase<D9>& dv )
-{
- MatrixSizeCheck<3, 1>::check(p1);
- MatrixSizeCheck<3, 1>::check(v1);
- MatrixSizeCheck<3, 1>::check(p2);
- MatrixSizeCheck<3, 1>::check(v2);
- MatrixSizeCheck<3, 1>::check(dp);
- MatrixSizeCheck<3, 1>::check(dv);
-
- dp = q1.conjugate() * ( p2 - p1 - v1*dt - (T)0.5*gravity().cast<T>()*(T)dt*(T)dt );
- dv = q1.conjugate() * ( v2 - v1 - gravity().cast<T>()*(T)dt );
- dq = q1.conjugate() * q2;
-}
-
-template<typename D1, typename D2, typename D3, class T>
-inline void betweenStates(const MatrixBase<D1>& x1,
- const MatrixBase<D2>& x2,
- T dt,
- MatrixBase<D3>& x2_minus_x1)
-{
- MatrixSizeCheck<10, 1>::check(x1);
- MatrixSizeCheck<10, 1>::check(x2);
- MatrixSizeCheck<10, 1>::check(x2_minus_x1);
-
- Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & x1(0) );
- Map<const Quaternion<typename D1::Scalar> > q1 ( & x1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > v1 ( & x1(7) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & x2(0) );
- Map<const Quaternion<typename D2::Scalar> > q2 ( & x2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > v2 ( & x2(7) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dp ( & x2_minus_x1(0) );
- Map<Quaternion<typename D3::Scalar> > dq ( & x2_minus_x1(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dv ( & x2_minus_x1(7) );
-
- betweenStates(p1, q1, v1, p2, q2, v2, dt, dp, dq, dv);
-}
-
-template<typename D1, typename D2, class T>
-inline Matrix<typename D1::Scalar, 10, 1> betweenStates(const MatrixBase<D1>& x1,
- const MatrixBase<D2>& x2,
- T dt)
-{
- Matrix<typename D1::Scalar, 10, 1> ret;
- betweenStates(x1, x2, dt, ret);
- return ret;
-}
-
-template<typename Derived>
-Matrix<typename Derived::Scalar, 9, 1> log_IMU(const MatrixBase<Derived>& delta_in)
-{
- MatrixSizeCheck<10, 1>::check(delta_in);
-
- Matrix<typename Derived::Scalar, 9, 1> ret;
-
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & delta_in(0) );
- Map<const Quaternion<typename Derived::Scalar> > dq_in ( & delta_in(3) );
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dv_in ( & delta_in(7) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dp_ret ( & ret(0) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dv_ret ( & ret(6) );
-
- dp_ret = dp_in;
- dv_ret = dv_in;
- do_ret = log_q(dq_in);
-
- return ret;
-}
-
-template<typename Derived>
-Matrix<typename Derived::Scalar, 10, 1> exp_IMU(const MatrixBase<Derived>& d_in)
-{
- MatrixSizeCheck<9, 1>::check(d_in);
-
- Matrix<typename Derived::Scalar, 10, 1> ret;
-
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & d_in(0) );
- Map<const Matrix<typename Derived::Scalar, 3, 1> > do_in ( & d_in(3) );
- Map<const Matrix<typename Derived::Scalar, 3, 1> > dv_in ( & d_in(6) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dp ( & ret(0) );
- Map<Quaternion<typename Derived::Scalar> > dq ( & ret(3) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > dv ( & ret(7) );
-
- dp = dp_in;
- dv = dv_in;
- dq = exp_q(do_in);
-
- return ret;
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
-inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
- const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2, const MatrixBase<D6>& dv2,
- MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q, MatrixBase<D9>& plus_v )
-{
- plus_p = dp1 + dp2;
- plus_v = dv1 + dv2;
- plus_q = dq1 * exp_q(do2);
-}
-
-template<typename D1, typename D2, typename D3>
-inline void plus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& d_pert)
-{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > do2 ( & d2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(6) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dp_p ( & d_pert(0) );
- Map<Quaternion<typename D3::Scalar> > dq_p ( & d_pert(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dv_p ( & d_pert(7) );
-
- plus(dp1, dq1, dv1, dp2, do2, dv2, dp_p, dq_p, dv_p);
-}
-
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 10, 1> plus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2)
-{
- Matrix<typename D1::Scalar, 10, 1> ret;
- plus(d1, d2, ret);
- return ret;
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
-inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
- MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o, MatrixBase<D9>& diff_v )
-{
- diff_p = dp2 - dp1;
- diff_v = dv2 - dv1;
- diff_o = log_q(dq1.conjugate() * dq2);
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11>
-inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
- MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o, MatrixBase<D9>& diff_v ,
- MatrixBase<D10>& J_do_dq1, MatrixBase<D11>& J_do_dq2)
-{
- diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v);
-
- J_do_dq1 = - jac_SO3_left_inv(diff_o);
- J_do_dq2 = jac_SO3_right_inv(diff_o);
-}
-
-template<typename D1, typename D2, typename D3>
-inline void diff(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& err)
-{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(7) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & err(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & err(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & err(6) );
-
- diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v);
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5>
-inline void diff(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& dif,
- MatrixBase<D4>& J_diff_d1,
- MatrixBase<D5>& J_diff_d2)
-{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(7) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & dif(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & dif(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & dif(6) );
-
- Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
-
- diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
-
- /* d = diff(d1, d2) is
- * dp = dp2 - dp1
- * do = Log(dq1.conj * dq2)
- * dv = dv2 - dv1
- *
- * With trivial Jacobians for dp and dv, and:
- * J_do_dq1 = - J_l_inv(theta)
- * J_do_dq2 = J_r_inv(theta)
- */
-
- J_diff_d1 = - Matrix<typename D4::Scalar, 9, 9>::Identity();// d(p2 - p1) / d(p1) = - Identity
- J_diff_d1.block(3,3,3,3) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
-
- J_diff_d2.setIdentity(); // d(R1.tr*R2) / d(R2) = Identity
- J_diff_d2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
-}
-
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 9, 1> diff(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2)
-{
- Matrix<typename D1::Scalar, 9, 1> ret;
- diff(d1, d2, ret);
- return ret;
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5>
-inline void body2delta(const MatrixBase<D1>& a,
- const MatrixBase<D2>& w,
- const typename D1::Scalar& dt,
- MatrixBase<D3>& dp,
- QuaternionBase<D4>& dq,
- MatrixBase<D5>& dv)
-{
- MatrixSizeCheck<3,1>::check(a);
- MatrixSizeCheck<3,1>::check(w);
- MatrixSizeCheck<3,1>::check(dp);
- MatrixSizeCheck<3,1>::check(dv);
-
- dp = 0.5 * a * dt * dt;
- dv = a * dt;
- dq = exp_q(w * dt);
-}
-
-template<typename D1>
-inline Matrix<typename D1::Scalar, 10, 1> body2delta(const MatrixBase<D1>& body,
- const typename D1::Scalar& dt)
-{
- MatrixSizeCheck<6,1>::check(body);
-
- typedef typename D1::Scalar T;
-
- Matrix<T, 10, 1> delta;
-
- Map< Matrix<T, 3, 1>> dp ( & delta(0) );
- Map< Quaternion<T>> dq ( & delta(3) );
- Map< Matrix<T, 3, 1>> dv ( & delta(7) );
-
- body2delta(body.block(0,0,3,1), body.block(3,0,3,1), dt, dp, dq, dv);
-
- return delta;
-}
-
-template<typename D1, typename D2, typename D3>
-inline void body2delta(const MatrixBase<D1>& body,
- const typename D1::Scalar& dt,
- MatrixBase<D2>& delta,
- MatrixBase<D3>& jac_body)
-{
- MatrixSizeCheck<6,1>::check(body);
- MatrixSizeCheck<9,6>::check(jac_body);
-
- typedef typename D1::Scalar T;
-
- delta = body2delta(body, dt);
-
- Matrix<T, 3, 1> w = body.block(3,0,3,1);
-
- jac_body.setZero();
- jac_body.block(0,0,3,3) = 0.5 * dt * dt * Matrix<T, 3, 3>::Identity();
- jac_body.block(3,3,3,3) = dt * jac_SO3_right(w * dt);
- jac_body.block(6,0,3,3) = dt * Matrix<T, 3, 3>::Identity();
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7>
-void motion2data(const MatrixBase<D1>& a, const MatrixBase<D2>& w, const QuaternionBase<D3>& q, const MatrixBase<D4>& a_b, const MatrixBase<D5>& w_b, MatrixBase<D6>& a_m, MatrixBase<D7>& w_m)
-{
- MatrixSizeCheck<3,1>::check(a);
- MatrixSizeCheck<3,1>::check(w);
- MatrixSizeCheck<3,1>::check(a_b);
- MatrixSizeCheck<3,1>::check(w_b);
- MatrixSizeCheck<3,1>::check(a_m);
- MatrixSizeCheck<3,1>::check(w_m);
-
- // Note: data = (a_m , w_m)
- a_m = a + a_b - q.conjugate()*gravity();
- w_m = w + w_b;
-}
-
-/* Create IMU data from body motion
- * Input:
- * motion : [ax, ay, az, wx, wy, wz] the motion in body frame
- * q : the current orientation wrt horizontal
- * bias : the bias of the IMU
- * Output:
- * return : the data vector as created by the IMU (with motion, gravity, and bias)
- */
-template<typename D1, typename D2, typename D3>
-Matrix<typename D1::Scalar, 6, 1> motion2data(const MatrixBase<D1>& motion, const QuaternionBase<D2>& q, const MatrixBase<D3>& bias)
-{
- Matrix<typename D1::Scalar, 6, 1> data;
- Map<Matrix<typename D1::Scalar, 3, 1>> a_m (data.data() );
- Map<Matrix<typename D1::Scalar, 3, 1>> w_m (data.data() + 3);
-
- motion2data(motion.block(0,0,3,1),
- motion.block(3,0,3,1),
- q,
- bias.block(0,0,3,1),
- bias.block(3,0,3,1),
- a_m,
- w_m );
-
- return data;
-}
-
-} // namespace imu
-} // namespace wolf
-
-#endif /* IMU_TOOLS_H_ */
diff --git a/include/base/processor/processor_IMU.h b/include/base/processor/processor_IMU.h
deleted file mode 100644
index 53204c6a0..000000000
--- a/include/base/processor/processor_IMU.h
+++ /dev/null
@@ -1,85 +0,0 @@
-#ifndef PROCESSOR_IMU_H
-#define PROCESSOR_IMU_H
-
-// Wolf
-#include "base/capture/capture_IMU.h"
-#include "base/feature/feature_IMU.h"
-#include "base/processor/processor_motion.h"
-
-namespace wolf {
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsIMU);
-
-struct ProcessorParamsIMU : public ProcessorParamsMotion
-{
- //
-};
-
-WOLF_PTR_TYPEDEFS(ProcessorIMU);
-
-//class
-class ProcessorIMU : public ProcessorMotion{
- public:
- ProcessorIMU(ProcessorParamsIMUPtr _params_motion_IMU);
- virtual ~ProcessorIMU();
- virtual void configure(SensorBasePtr _sensor) override { };
-
- protected:
- virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
- const Scalar _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) override;
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _delta_preint_plus_delta) override;
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _delta_preint_plus_delta,
- Eigen::MatrixXs& _jacobian_delta_preint,
- Eigen::MatrixXs& _jacobian_delta) override;
- virtual void statePlusDelta(const Eigen::VectorXs& _x,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _x_plus_delta ) override;
- virtual Eigen::VectorXs deltaZero() const override;
- virtual Motion interpolate(const Motion& _motion_ref,
- Motion& _motion,
- TimeStamp& _ts) override;
- virtual bool voteForKeyFrame() override;
- virtual CaptureMotionPtr createCapture(const TimeStamp& _ts,
- const SensorBasePtr& _sensor,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin) override;
- virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
- virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override;
-
- protected:
- ProcessorParamsIMUPtr params_motion_IMU_;
-
- public:
- //for factory
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-};
-
-}
-
-/////////////////////////////////////////////////////////
-// IMPLEMENTATION. Put your implementation includes here
-/////////////////////////////////////////////////////////
-
-namespace wolf{
-
-inline Eigen::VectorXs ProcessorIMU::deltaZero() const
-{
- return (Eigen::VectorXs(10) << 0,0,0, 0,0,0,1, 0,0,0 ).finished(); // p, q, v
-}
-
-} // namespace wolf
-
-#endif // PROCESSOR_IMU_H
diff --git a/include/base/processor/processor_tracker_landmark_dummy.h b/include/base/processor/processor_tracker_landmark_dummy.h
deleted file mode 100644
index c0c3424cb..000000000
--- a/include/base/processor/processor_tracker_landmark_dummy.h
+++ /dev/null
@@ -1,93 +0,0 @@
-/**
- * \file processor_tracker_landmark_dummy.h
- *
- * Created on: Apr 12, 2016
- * \author: jvallve
- */
-
-#ifndef PROCESSOR_TRACKER_LANDMARK_DUMMY_H_
-#define PROCESSOR_TRACKER_LANDMARK_DUMMY_H_
-
-#include "base/processor/processor_tracker_landmark.h"
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkDummy);
-
-class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
-{
- public:
- ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark);
- virtual ~ProcessorTrackerLandmarkDummy();
- virtual void configure(SensorBasePtr _sensor) { };
-
- protected:
-
- unsigned int n_feature_;
- unsigned int landmark_idx_non_visible_;
-
-// virtual void preProcess() { }
- virtual void postProcess(); // implemented
-
- /** \brief Find provided landmarks in the incoming capture
- * \param _landmarks_in input list of landmarks to be found in incoming
- * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
- * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
- */
- virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_out,
- LandmarkMatchMap& _feature_landmark_correspondences);
-
- /** \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!
- */
- virtual bool voteForKeyFrame();
-
- /** \brief Detect new Features
- * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
- * the list of newly detected features of the capture last_ptr_.
- */
- virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
-
- /** \brief Create one landmark
- *
- * Implement in derived classes to build the type of landmark you need for this tracker.
- */
- virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
-
- /** \brief Create a new factor
- * \param _feature_ptr pointer to the Feature to constrain
- * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
- *
- * Implement this method in derived classes.
- */
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
-};
-
-inline void ProcessorTrackerLandmarkDummy::postProcess()
-{
- landmark_idx_non_visible_++;
- std::cout << "------- Landmarks until " << landmark_idx_non_visible_ << " are now out of scope" << std::endl
- << std::endl;
-}
-
-} // namespace wolf
-
-// IMPLEMENTATION
-
-namespace wolf
-{
-
-} // namespace wolf
-
-#endif /* PROCESSOR_TRACKER_LANDMARK_DUMMY_H_ */
diff --git a/include/base/sensor/sensor_GPS.h b/include/base/sensor/sensor_GPS.h
deleted file mode 100644
index 6881537b6..000000000
--- a/include/base/sensor/sensor_GPS.h
+++ /dev/null
@@ -1,47 +0,0 @@
-#ifndef SENSOR_GPS_H_
-#define SENSOR_GPS_H_
-
-/* WARNING: from here you cannot include gps_scan_utils
- * because is included in factorGPS, and factorGPS is included in wolf.h (or some other wolf file)
- * Otherwise wolf will not build without my library installed
- *
- * --- MAYBE IS NO MORE TRUE, AFTER THE INCLUDES FIX!! ---
- */
-
-//wolf
-#include "base/sensor/sensor_base.h"
-//#include "sensor_factory.h"
-//#include "base/common/factory.h"
-
-// std
-
-namespace wolf {
-
-struct IntrinsicsGPS : public IntrinsicsBase
-{
- // add GPS parameters here
- virtual ~IntrinsicsGPS() = default;
-};
-
-WOLF_PTR_TYPEDEFS(SensorGPS);
-
-class SensorGPS : public SensorBase
-{
-public:
- //pointer to sensor position, orientation, bias, init vehicle position and orientation
- SensorGPS(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _bias_ptr, StateBlockPtr _map_position_ptr, StateBlockPtr _map_orientation_ptr);
-
- StateBlockPtr getMapP() const;
-
- StateBlockPtr getMapO() const;
-
- virtual ~SensorGPS();
-
-public:
- static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p, const IntrinsicsBasePtr _intrinsics);
-
-};
-
-} // namespace wolf
-
-#endif //SENSOR_GPS_H_
diff --git a/include/base/sensor/sensor_GPS_fix.h b/include/base/sensor/sensor_GPS_fix.h
deleted file mode 100644
index 17db7c12c..000000000
--- a/include/base/sensor/sensor_GPS_fix.h
+++ /dev/null
@@ -1,37 +0,0 @@
-
-#ifndef SENSOR_GPS_FIX_H_
-#define SENSOR_GPS_FIX_H_
-
-//wolf includes
-#include "base/sensor/sensor_base.h"
-
-// std includes
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsGPSFix);
-struct IntrinsicsGPSFix : public IntrinsicsBase
-{
- Eigen::Vector3s noise_std;
- // Empty -- it acts only as a typedef for IntrinsicsBase, but allows future extension with parameters
- virtual ~IntrinsicsGPSFix() = default;
-};
-
-WOLF_PTR_TYPEDEFS(SensorGPSFix);
-
-class SensorGPSFix : public SensorBase
-{
- public:
- SensorGPSFix(const Eigen::VectorXs & _extrinsics, const IntrinsicsGPSFix& _intrinsics);
- SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr);
-
- virtual ~SensorGPSFix();
-
- public:
- static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics);
-
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/sensor/sensor_IMU.h b/include/base/sensor/sensor_IMU.h
deleted file mode 100644
index c62ec6fc1..000000000
--- a/include/base/sensor/sensor_IMU.h
+++ /dev/null
@@ -1,98 +0,0 @@
-#ifndef SENSOR_IMU_H
-#define SENSOR_IMU_H
-
-//wolf includes
-#include "base/sensor/sensor_base.h"
-#include <iostream>
-
-namespace wolf {
-
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsIMU);
-
-//TODO : bias_ptr defined as intrinsics StateBlock in constructor (see SensorBase) but here we also have another intrinsics
-// This is confusing.
-
-struct IntrinsicsIMU : public IntrinsicsBase
-{
- //noise std dev
- Scalar w_noise = 0.001; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
- Scalar a_noise = 0.04; //standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s)
-
- //Initial biases std dev
- Scalar ab_initial_stdev = 0.01; //accelerometer micro_g/sec
- Scalar wb_initial_stdev = 0.01; //gyroscope rad/sec
-
- // bias rate of change std dev
- Scalar ab_rate_stdev = 0.00001;
- Scalar wb_rate_stdev = 0.00001;
-
- virtual ~IntrinsicsIMU() = default;
-};
-
-WOLF_PTR_TYPEDEFS(SensorIMU);
-
-class SensorIMU : public SensorBase
-{
-
- protected:
- Scalar a_noise; //Power Spectral Density (same for all the axis) in micro_g/ sqrt(Hz)
- Scalar w_noise; //Rate Noise Spectral Density (same for all the axis) in deg/sec/ sqrt(Hz)
-
- //This is a trial to factor how much can the bias change in 1 sec at most
- Scalar ab_initial_stdev; //accelerometer micro_g/sec
- Scalar wb_initial_stdev; //gyroscope rad/sec
- Scalar ab_rate_stdev; //accelerometer micro_g/sec
- Scalar wb_rate_stdev; //gyroscope rad/sec
-
- public:
-
- SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _params);
- SensorIMU(const Eigen::VectorXs& _extrinsics, IntrinsicsIMUPtr _params);
-
- Scalar getGyroNoise() const;
- Scalar getAccelNoise() const;
- Scalar getWbInitialStdev() const;
- Scalar getAbInitialStdev() const;
- Scalar getWbRateStdev() const;
- Scalar getAbRateStdev() const;
-
- virtual ~SensorIMU();
-
- public:
- static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics = nullptr);
-
-};
-
-inline Scalar SensorIMU::getGyroNoise() const
-{
- return w_noise;
-}
-
-inline Scalar SensorIMU::getAccelNoise() const
-{
- return a_noise;
-}
-
-inline Scalar SensorIMU::getWbInitialStdev() const
-{
- return wb_initial_stdev;
-}
-
-inline Scalar SensorIMU::getAbInitialStdev() const
-{
- return ab_initial_stdev;
-}
-
-inline Scalar SensorIMU::getWbRateStdev() const
-{
- return wb_rate_stdev;
-}
-
-inline Scalar SensorIMU::getAbRateStdev() const
-{
- return ab_rate_stdev;
-}
-
-} // namespace wolf
-
-#endif // SENSOR_IMU_H
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
deleted file mode 100644
index caab481fc..000000000
--- a/src/CMakeLists.txt
+++ /dev/null
@@ -1,762 +0,0 @@
-#Start WOLF build
-MESSAGE("Starting WOLF CMakeLists ...")
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-
-#CMAKE modules
-
-SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
-MESSAGE(STATUS ${CMAKE_MODULE_PATH})
-
-# Some wolf compilation options
-
-IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
- set(_WOLF_DEBUG true)
-ENDIF()
-
-option(_WOLF_TRACE "Enable wolf tracing macro" ON)
-
-option(BUILD_EXAMPLES "Build examples" OFF)
-set(BUILD_TESTS true)
-
-# Does this has any other interest
-# but for the examples ?
-# yes, for the tests !
-IF(BUILD_EXAMPLES OR BUILD_TESTS)
- set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
-ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
-
-#find dependencies.
-
-FIND_PACKAGE(Eigen3 3.2.92 REQUIRED)
-
-FIND_PACKAGE(Threads REQUIRED)
-
-FIND_PACKAGE(Ceres QUIET) #Ceres is not required
-IF(Ceres_FOUND)
- MESSAGE("Ceres Library FOUND: Ceres related sources will be built.")
-ENDIF(Ceres_FOUND)
-
-FIND_PACKAGE(faramotics QUIET) #faramotics is not required
-IF(faramotics_FOUND)
- FIND_PACKAGE(GLUT REQUIRED)
- FIND_PACKAGE(pose_state_time REQUIRED)
- MESSAGE("Faramotics Library FOUND: Faramotics related sources will be built.")
-ENDIF(faramotics_FOUND)
-
-FIND_PACKAGE(laser_scan_utils QUIET) #laser_scan_utils is not required
-IF(laser_scan_utils_FOUND)
- MESSAGE("laser_scan_utils Library FOUND: laser_scan_utils related sources will be built.")
-ENDIF(laser_scan_utils_FOUND)
-
-FIND_PACKAGE(raw_gps_utils QUIET) #raw_gps_utils is not required
-IF(raw_gps_utils_FOUND)
- MESSAGE("raw_gps_utils Library FOUND: raw_gps_utils related sources will be built.")
-ENDIF(raw_gps_utils_FOUND)
-
-# Vision Utils
-FIND_PACKAGE(vision_utils QUIET)
-IF (vision_utils_FOUND)
- MESSAGE("vision_utils Library FOUND: vision related sources will be built.")
- SET(PRINT_INFO_VU false)
- FIND_PACKAGE(OpenCV QUIET)
-ENDIF(vision_utils_FOUND)
-
-# OpenCV
-FIND_PACKAGE(OpenCV QUIET)
-IF (OPENCV_FOUND)
- MESSAGE("opencv Library FOUND: opencv related sources will be built.")
-ENDIF(OPENCV_FOUND)
-
-# Cereal
-FIND_PACKAGE(cereal QUIET)
-IF(cereal_FOUND)
- MESSAGE("cereal Library FOUND: cereal related sources will be built.")
-ENDIF(cereal_FOUND)
-
-# Apriltag
-# TODO: write proper files to be able to use find_package with apriltag library
-FIND_PATH(APRILTAG_INCLUDE_DIR NAMES apriltag.h PATH_SUFFIXES "apriltag" ${APRILTAG_INCLUDE_PATH})
-FIND_LIBRARY(APRILTAG_LIBRARY NAMES apriltag PATH_SUFFIXES "${CMAKE_LIBRARY_ARCHITECTURE}" "apriltag" ${APRILTAG_LIBRARY_PATH})
-
-IF(APRILTAG_LIBRARY)
- SET(Apriltag_FOUND TRUE)
- MESSAGE("apriltag Library FOUND in ${APRILTAG_LIBRARY}: apriltag related sources will be built.")
-ENDIF(APRILTAG_LIBRARY)
-
-# YAML with yaml-cpp
-INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake)
-IF(YAMLCPP_FOUND)
- MESSAGE("yaml-cpp Library FOUND: yaml-cpp related sources will be built.")
-ELSEIF(YAMLCPP_FOUND)
- MESSAGE("yaml-cpp Library NOT FOUND!")
-ENDIF(YAMLCPP_FOUND)
-
-#GLOG
-INCLUDE (${PROJECT_SOURCE_DIR}/cmake_modules/FindGlog.cmake)
-IF(GLOG_FOUND)
- MESSAGE("glog Library FOUND: glog related sources will be built.")
- MESSAGE(STATUS ${GLOG_INCLUDE_DIR})
- MESSAGE(STATUS ${GLOG_LIBRARY})
-ELSEIF(GLOG_FOUND)
- MESSAGE("glog Library NOT FOUND!")
-ENDIF(GLOG_FOUND)
-
-# SuiteSparse doesn't have find*.cmake:
-FIND_PATH(
- Suitesparse_INCLUDE_DIRS
- NAMES SuiteSparse_config.h
- PATHS /usr/include/suitesparse /usr/local/include/suitesparse)
-MESSAGE("Found suitesparse_INCLUDE_DIRS:" ${Suitesparse_INCLUDE_DIRS})
-
-IF(Suitesparse_INCLUDE_DIRS)
- SET(Suitesparse_FOUND TRUE)
- MESSAGE("Suitesparse FOUND: wolf_solver will be built.")
-ELSE (Suitesparse_INCLUDE_DIRS)
- SET(Suitesparse_FOUND FALSE)
- MESSAGE(FATAL_ERROR "Suitesparse NOT FOUND")
-ENDIF (Suitesparse_INCLUDE_DIRS)
-
-# Define the directory where will be the configured config.h
-SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/internal)
-
-# Create the specified output directory if it does not exist.
-IF(NOT EXISTS "${WOLF_CONFIG_DIR}")
- message(STATUS "Creating config output directory: ${WOLF_CONFIG_DIR}")
- file(MAKE_DIRECTORY "${WOLF_CONFIG_DIR}")
-ENDIF()
-IF(EXISTS "${WOLF_CONFIG_DIR}" AND NOT IS_DIRECTORY "${WOLF_CONFIG_DIR}")
- message(FATAL_ERROR "Bug: Specified CONFIG_DIR: "
- "${WOLF_CONFIG_DIR} exists, but is not a directory.")
-ENDIF()
-# Configure config.h
-configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_DIR}/config.h")
-
-#TEMPORAL INCLUDE UNTIL WE FIGURE OUT WHAT TO DO WITH FILES in src/temp
-
-include_directories(${CMAKE_CURRENT_SOURCE_DIR})
-include_directories(../dummyInclude)
-
-# Include config.h directory at first.
-include_directories("${PROJECT_BINARY_DIR}/conf")
-
-INCLUDE_DIRECTORIES(.)
-
-# include spdlog (logging library)
-FIND_PATH(SPDLOG_INCLUDE_DIR spdlog.h /usr/local/include/spdlog /usr/include/spdlog)
-IF (SPDLOG_INCLUDE_DIR)
- INCLUDE_DIRECTORIES(${SPDLOG_INCLUDE_DIR})
- MESSAGE(STATUS "Found spdlog: ${SPDLOG_INCLUDE_DIR}")
-ELSE (SPDLOG_INCLUDE_DIR)
- MESSAGE(FATAL_ERROR "Could not find spdlog")
-ENDIF (SPDLOG_INCLUDE_DIR)
-
-INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS})
-
-IF(Ceres_FOUND)
- INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-ENDIF(Ceres_FOUND)
-
-IF(faramotics_FOUND)
- INCLUDE_DIRECTORIES(${faramotics_INCLUDE_DIRS})
-ENDIF(faramotics_FOUND)
-
-IF(laser_scan_utils_FOUND)
- INCLUDE_DIRECTORIES(${laser_scan_utils_INCLUDE_DIRS})
-ENDIF(laser_scan_utils_FOUND)
-
-IF(raw_gps_utils_FOUND)
- INCLUDE_DIRECTORIES(${raw_gps_utils_INCLUDE_DIRS})
-ENDIF(raw_gps_utils_FOUND)
-
-IF(vision_utils_FOUND)
- INCLUDE_DIRECTORIES(${vision_utils_INCLUDE_DIR})
- INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
-ENDIF(vision_utils_FOUND)
-
-IF(OPENCV_FOUND)
- INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS})
-ENDIF(OPENCV_FOUND)
-
-# cereal
-IF(cereal_FOUND)
- INCLUDE_DIRECTORIES(${cereal_INCLUDE_DIRS})
-ENDIF(cereal_FOUND)
-
-IF(Suitesparse_FOUND)
- INCLUDE_DIRECTORIES(${Suitesparse_INCLUDE_DIRS})
-ENDIF(Suitesparse_FOUND)
-
-IF(YAMLCPP_FOUND)
- INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
-ENDIF(YAMLCPP_FOUND)
-
-IF(GLOG_FOUND)
- INCLUDE_DIRECTORIES(${GLOG_INCLUDE_DIR})
-ENDIF(GLOG_FOUND)
-
-IF(APRILTAG_INCLUDE_DIR)
- INCLUDE_DIRECTORIES(${APRILTAG_INCLUDE_DIR})
-ENDIF(APRILTAG_INCLUDE_DIR)
-
-#headers
-SET(HDRS_BASE
-capture/capture_motion.h
- eigen_assert.h
- eigen_predicates.h
-landmark/landmark_match.h
- make_unique.h
- pinhole_tools.h
-processor/processor_capture_holder.h
-processor/processor_tracker_landmark.h
- )
-SET(HDRS
-capture/capture_motion.h
-capture/capture_GPS_fix.h
-capture/capture_IMU.h
-capture/capture_odom_2D.h
-capture/capture_odom_3D.h
-factor/factor_block_absolute.h
-factor/factor_container.h
-factor/factor_corner_2D.h
-factor/factor_AHP.h
-factor/factor_epipolar.h
-factor/factor_IMU.h
-factor/factor_fix_bias.h
-factor/factor_GPS_2D.h
-factor/factor_GPS_pseudorange_3D.h
-factor/factor_GPS_pseudorange_2D.h
-factor/factor_odom_2D.h
-factor/factor_odom_2D_analytic.h
-factor/factor_odom_3D.h
-factor/factor_point_2D.h
-factor/factor_point_to_line_2D.h
-factor/factor_pose_2D.h
-factor/factor_pose_3D.h
-factor/factor_quaternion_absolute.h
-factor/factor_relative_2D_analytic.h
- temp/diff_drive_tools.h
- temp/diff_drive_tools.hpp
-feature/feature_corner_2D.h
-feature/feature_GPS_fix.h
-feature/feature_GPS_pseudorange.h
-feature/feature_IMU.h
-feature/feature_odom_2D.h
-feature/feature_polyline_2D.h
- IMU_tools.h
-landmark/landmark_corner_2D.h
-landmark/landmark_container.h
-landmark/landmark_line_2D.h
-landmark/landmark_polyline_2D.h
- local_parametrization_polyline_extreme.h
-processor/processor_frame_nearest_neighbor_filter.h
-processor/processor_IMU.h
- test/processor_IMU_UnitTester.h
-processor/processor_odom_2D.h
-processor/processor_odom_3D.h
-processor/processor_tracker_feature_dummy.h
-processor/processor_tracker_landmark_dummy.h
-sensor/sensor_camera.h
-sensor/sensor_GPS.h
-sensor/sensor_GPS_fix.h
-sensor/sensor_IMU.h
-sensor/sensor_odom_2D.h
-sensor/sensor_odom_3D.h
- )
- SET(HDRS_CAPTURE
-capture/capture_GPS_fix.h
-capture/capture_IMU.h
-capture/capture_odom_2D.h
-capture/capture_odom_3D.h
-capture/capture_velocity.h
-capture/capture_wheel_joint_position.h
- )
- SET(HDRS_CONSTRAINT
-factor/factor_autodiff_trifocal.h
-factor/factor_autodiff_distance_3D.h
-factor/factor_AHP.h
-factor/factor_block_absolute.h
-factor/factor_container.h
-factor/factor_corner_2D.h
-factor/factor_diff_drive.h
-factor/factor_epipolar.h
-factor/factor_IMU.h
-factor/factor_fix_bias.h
-factor/factor_GPS_2D.h
-factor/factor_GPS_pseudorange_3D.h
-factor/factor_GPS_pseudorange_2D.h
-factor/factor_odom_2D.h
-factor/factor_odom_2D_analytic.h
-factor/factor_odom_3D.h
-factor/factor_point_2D.h
-factor/factor_point_to_line_2D.h
-factor/factor_pose_2D.h
-factor/factor_pose_3D.h
-factor/factor_quaternion_absolute.h
-factor/factor_relative_2D_analytic.h
- )
- SET(HDRS_FEATURE
-feature/feature_corner_2D.h
-feature/feature_diff_drive.h
-feature/feature_GPS_fix.h
-feature/feature_GPS_pseudorange.h
-feature/feature_IMU.h
-feature/feature_odom_2D.h
-feature/feature_polyline_2D.h
- )
- SET(HDRS_LANDMARK
-landmark/landmark_match.h
-landmark/landmark_corner_2D.h
-landmark/landmark_container.h
-landmark/landmark_line_2D.h
-landmark/landmark_polyline_2D.h
- )
- SET(HDRS_PROCESSOR
-processor/processor_capture_holder.h
-processor/processor_diff_drive.h
-processor/processor_frame_nearest_neighbor_filter.h
-processor/processor_IMU.h
-processor/processor_odom_2D.h
-processor/processor_odom_3D.h
-processor/processor_tracker_feature_dummy.h
-processor/processor_tracker_landmark.h
-processor/processor_tracker_landmark_dummy.h
- )
- SET(HDRS_SENSOR
-sensor/sensor_camera.h
-sensor/sensor_diff_drive.h
-sensor/sensor_GPS.h
-sensor/sensor_GPS_fix.h
-sensor/sensor_IMU.h
-sensor/sensor_odom_2D.h
-sensor/sensor_odom_3D.h
- )
-# [Add generic derived header before this line]
-
-SET(HDRS_DTASSC
- data_association/matrix.h
- data_association/association_solver.h
- data_association/association_node.h
- data_association/association_tree.h
- data_association/association_nnls.h
- )
-
-SET(HDRS_CORE
-core/capture_base.h
-core/capture_buffer.h
-core/capture_pose.h
-core/capture_void.h
-core/factor_analytic.h
-core/factor_autodiff.h
-core/factor_base.h
-core/factory.h
-core/feature_base.h
-core/feature_match.h
-core/feature_pose.h
-core/frame_base.h
-core/hardware_base.h
-core/landmark_base.h
-core/local_parametrization_angle.h
-core/local_parametrization_base.h
-core/local_parametrization_homogeneous.h
-core/local_parametrization_quaternion.h
-core/logging.h
-core/map_base.h
-core/motion_buffer.h
-core/node_base.h
-core/problem.h
-core/processor_base.h
-core/factory.h
-core/processor_loopclosure_base.h
-core/processor_motion.h
-core/processor_tracker_feature.h
-core/processor_tracker.h
-core/rotations.h
-core/sensor_base.h
-core/factory.h
-core/singleton.h
-core/state_angle.h
-core/state_block.h
-core/state_homogeneous_3D.h
-core/state_quaternion.h
-core/three_D_tools.h
-core/time_stamp.h
-core/track_matrix.h
-core/trajectory_base.h
-core/wolf.h
- )
-SET(SRCS_CORE
-core/capture_base.cpp
-core/capture_pose.cpp
-core/capture_void.cpp
-core/factor_analytic.cpp
-core/factor_base.cpp
-core/feature_base.cpp
-core/feature_pose.cpp
-core/frame_base.cpp
-core/hardware_base.cpp
-core/landmark_base.cpp
-core/local_parametrization_base.cpp
-core/local_parametrization_homogeneous.cpp
-core/local_parametrization_quaternion.cpp
-core/map_base.cpp
-core/motion_buffer.cpp
-core/node_base.cpp
-core/problem.cpp
-core/processor_base.cpp
-core/processor_loopclosure_base.cpp
-core/processor_motion.cpp
-core/processor_tracker.cpp
-core/processor_tracker_feature.cpp
-core/sensor_base.cpp
-core/state_block.cpp
-core/time_stamp.cpp
-core/track_matrix.cpp
-core/trajectory_base.cpp
- )
-#sources
-SET(SRCS_BASE
-capture/capture_motion.cpp
-processor/processor_capture_holder.cpp
-processor/processor_tracker_landmark.cpp
- )
-
-SET(SRCS
- local_parametrization_polyline_extreme.cpp
- test/processor_IMU_UnitTester.cpp
- )
- SET(SRCS_CAPTURE
-capture/capture_GPS_fix.cpp
-capture/capture_IMU.cpp
-capture/capture_odom_2D.cpp
-capture/capture_odom_3D.cpp
-capture/capture_velocity.cpp
-capture/capture_wheel_joint_position.cpp
- )
- SET(SRCS_FEATURE
-feature/feature_corner_2D.cpp
-feature/feature_diff_drive.cpp
-feature/feature_GPS_fix.cpp
-feature/feature_GPS_pseudorange.cpp
-feature/feature_IMU.cpp
-feature/feature_odom_2D.cpp
-feature/feature_polyline_2D.cpp
- )
- SET(SRCS_LANDMARK
-landmark/landmark_corner_2D.cpp
-landmark/landmark_container.cpp
-landmark/landmark_line_2D.cpp
-landmark/landmark_polyline_2D.cpp
- )
- SET(SRCS_PROCESSOR
-processor/processor_frame_nearest_neighbor_filter.cpp
-processor/processor_diff_drive.cpp
-processor/processor_IMU.cpp
-processor/processor_odom_2D.cpp
-processor/processor_odom_3D.cpp
-processor/processor_tracker_feature_dummy.cpp
-processor/processor_tracker_landmark_dummy.cpp
- )
- SET(SRCS_SENSOR
-sensor/sensor_camera.cpp
-sensor/sensor_diff_drive.cpp
-sensor/sensor_GPS.cpp
-sensor/sensor_GPS_fix.cpp
-sensor/sensor_IMU.cpp
-sensor/sensor_odom_2D.cpp
-sensor/sensor_odom_3D.cpp
- )
-SET(SRCS_DTASSC
- data_association/association_solver.cpp
- data_association/association_node.cpp
- data_association/association_tree.cpp
- data_association/association_nnls.cpp
- )
-
-# Add the solver sub-directory
-add_subdirectory(solver)
-
-#optional HDRS and SRCS
-IF (Ceres_FOUND)
- SET(HDRS_WRAPPER
- ceres_wrapper/sparse_utils.h
- #ceres_wrapper/solver_manager.h
- ceres_wrapper/ceres_manager.h
- #ceres_wrapper/qr_manager.h
- ceres_wrapper/cost_function_wrapper.h
- ceres_wrapper/create_numeric_diff_cost_function.h
- ceres_wrapper/local_parametrization_wrapper.h
- )
- SET(SRCS_WRAPPER
- #ceres_wrapper/solver_manager.cpp
- ceres_wrapper/ceres_manager.cpp
- #ceres_wrapper/qr_manager.cpp
- ceres_wrapper/local_parametrization_wrapper.cpp
- )
-ELSE(Ceres_FOUND)
- SET(HDRS_WRAPPER)
- SET(SRCS_WRAPPER)
-ENDIF(Ceres_FOUND)
-
-IF (laser_scan_utils_FOUND)
- SET(HDRS ${HDRS}
-capture/capture_laser_2D.h
-sensor/sensor_laser_2D.h
-processor/processor_tracker_feature_corner.h
-processor/processor_tracker_landmark_corner.h
-processor/processor_tracker_landmark_polyline.h
- )
- SET(SRCS ${SRCS}
-capture/capture_laser_2D.cpp
-sensor/sensor_laser_2D.cpp
-processor/processor_tracker_feature_corner.cpp
-processor/processor_tracker_landmark_corner.cpp
-processor/processor_tracker_landmark_polyline.cpp
- )
-ENDIF(laser_scan_utils_FOUND)
-
-IF (raw_gps_utils_FOUND)
- SET(HDRS ${HDRS}
-capture/capture_GPS.h
-processor/processor_GPS.h
- )
- SET(SRCS ${SRCS}
-capture/capture_GPS.cpp
-processor/processor_GPS.cpp
- )
-ENDIF(raw_gps_utils_FOUND)
-
-# Vision
-IF (vision_utils_FOUND)
- SET(HDRS ${HDRS}
-capture/capture_image.h
-feature/feature_point_image.h
-landmark/landmark_AHP.h
-processor/processor_params_image.h
-processor/processor_tracker_feature_image.h
-processor/processor_tracker_landmark_image.h
- )
- SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
-processor/processor_tracker_feature_trifocal.h
- )
- SET(HDRS_LANDMARK ${HDRS_LANDMARK}
-landmark/landmark_point_3D.h
- )
- SET(SRCS ${SRCS}
-capture/capture_image.cpp
-feature/feature_point_image.cpp
-landmark/landmark_AHP.cpp
-processor/processor_tracker_feature_image.cpp
-processor/processor_tracker_landmark_image.cpp
- )
- SET(SRCS_LANDMARK ${SRCS_LANDMARK}
-landmark/landmark_point_3D.cpp
- )
- SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
-processor/processor_tracker_feature_trifocal.cpp
- )
-ENDIF(vision_utils_FOUND)
-
-IF (OPENCV_FOUND AND Apriltag_FOUND)
- SET(HDRS ${HDRS}
- landmark_apriltag.h
- )
- SET(SRCS ${SRCS}
- landmark_apriltag.cpp
- )
-ENDIF(OPENCV_FOUND AND Apriltag_FOUND)
-
-# Add the capture sub-directory
-# ADD_SUBDIRECTORY(captures)
-
-# Add the factors sub-directory
-# ADD_SUBDIRECTORY(factors)
-
-# Add the features sub-directory
-# ADD_SUBDIRECTORY(features)
-
-# Add the landmarks sub-directory
-# ADD_SUBDIRECTORY(landmarks)
-
-# Add the processors sub-directory
-# ADD_SUBDIRECTORY(processors)
-
-# Add the sensors sub-directory
-# ADD_SUBDIRECTORY(sensors)
-
-# Add the hello_wolf sub-directory
-ADD_SUBDIRECTORY(hello_wolf)
-
-IF (cereal_FOUND)
- ADD_SUBDIRECTORY(serialization/cereal)
-ENDIF(cereal_FOUND)
-
-IF (Suitesparse_FOUND)
- ADD_SUBDIRECTORY(solver_suitesparse)
-ENDIF(Suitesparse_FOUND)
-
-# LEAVE YAML FILES ALWAYS IN THE LAST POSITION !!
-IF(YAMLCPP_FOUND)
- # headers
- SET(HDRS ${HDRS}
- yaml/yaml_conversion.h
- )
-
- # sources
- SET(SRCS ${SRCS}
- yaml/processor_odom_3D_yaml.cpp
- yaml/processor_IMU_yaml.cpp
- yaml/sensor_camera_yaml.cpp
- yaml/sensor_odom_3D_yaml.cpp
- yaml/sensor_IMU_yaml.cpp
- )
- IF(laser_scan_utils_FOUND)
- SET(SRCS ${SRCS}
- yaml/sensor_laser_2D_yaml.cpp
- )
- ENDIF(laser_scan_utils_FOUND)
- IF(vision_utils_FOUND)
- SET(SRCS ${SRCS}
- yaml/processor_image_yaml.cpp
- yaml/processor_tracker_feature_trifocal_yaml.cpp
- )
- IF(Apriltag_FOUND)
- SET(SRCS ${SRCS}
- yaml/processor_tracker_landmark_apriltag_yaml.cpp
- )
- ENDIF(Apriltag_FOUND)
- ENDIF(vision_utils_FOUND)
-ENDIF(YAMLCPP_FOUND)
-
-# create the shared library
-add_library(${PROJECT_NAME}_core SHARED
- ${SRCS_CORE}
- ${SRCS_BASE}
- )
-ADD_LIBRARY(${PROJECT_NAME}
- SHARED
- ${SRCS_BASE}
- ${SRCS}
- ${SRCS_CAPTURE}
- ${SRCS_CONSTRAINT}
- ${SRCS_FEATURE}
- ${SRCS_LANDMARK}
- ${SRCS_PROCESSOR}
- ${SRCS_SENSOR}
- #${SRCS_DTASSC}
- ${SRCS_SOLVER}
- ${SRCS_WRAPPER}
- )
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT})
-
-TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${PROJECT_NAME}_core)
-#Link the created libraries
-#=============================================================
-IF (Ceres_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
-ENDIF(Ceres_FOUND)
-
-IF (laser_scan_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${laser_scan_utils_LIBRARY})
-ENDIF (laser_scan_utils_FOUND)
-
-IF (raw_gps_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${raw_gps_utils_LIBRARY})
-ENDIF (raw_gps_utils_FOUND)
-
-IF (OPENCV_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OpenCV_LIBS})
- IF (vision_utils_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${vision_utils_LIBRARY})
- ENDIF (vision_utils_FOUND)
-ENDIF (OPENCV_FOUND)
-
-IF (OPENCV_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${OpenCV_LIBS})
-ENDIF (OPENCV_FOUND)
-
-IF (YAMLCPP_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${YAMLCPP_LIBRARY})
-ENDIF (YAMLCPP_FOUND)
-
-IF (GLOG_FOUND)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${GLOG_LIBRARY})
-ENDIF (GLOG_FOUND)
-
-#check if this is done correctly
-IF (OPENCV_FOUND AND Apriltag_FOUND)
- LINK_LIBRARIES(apriltag m)
- TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${APRILTAG_LIBRARY} ${CMAKE_THREAD_LIBS_INIT} ${OPENCV_LDFLAGS} m)
-ENDIF(OPENCV_FOUND AND Apriltag_FOUND)
-
-#install library
-install(TARGETS ${PROJECT_NAME}_core DESTINATION lib/iri-algorithms EXPORT ${PROJECT_NAME}_core-targets)
-install(EXPORT ${PROJECT_NAME}_core-targets DESTINATION lib/iri-algorithms)
-#=============================================================
-INSTALL(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Targets
- RUNTIME DESTINATION bin
- LIBRARY DESTINATION lib/iri-algorithms
- ARCHIVE DESTINATION lib/iri-algorithms)
-
-install(EXPORT ${PROJECT_NAME}Targets DESTINATION lib/cmake/${PROJECT_NAME})
-
-#install headers
-INSTALL(FILES ${HDRS_BASE}
- DESTINATION include/iri-algorithms/wolf)
-INSTALL(FILES ${HDRS}
- DESTINATION include/iri-algorithms/wolf)
-INSTALL(FILES ${HDRS_CORE}
- DESTINATION include/iri-algorithms/wolf/core)
-#INSTALL(FILES ${HDRS_DTASSC}
-# DESTINATION include/iri-algorithms/wolf/data_association)
-INSTALL(FILES ${HDRS_CAPTURE}
- DESTINATION include/iri-algorithms/wolf/capture)
-INSTALL(FILES ${HDRS_CONSTRAINT}
- DESTINATION include/iri-algorithms/wolf/factor)
-INSTALL(FILES ${HDRS_FEATURE}
- DESTINATION include/iri-algorithms/wolf/feature)
-INSTALL(FILES ${HDRS_SENSOR}
- DESTINATION include/iri-algorithms/wolf/sensor)
-INSTALL(FILES ${HDRS_PROCESSOR}
- DESTINATION include/iri-algorithms/wolf/processor)
-INSTALL(FILES ${HDRS_LANDMARK}
- DESTINATION include/iri-algorithms/wolf/landmark)
-INSTALL(FILES ${HDRS_WRAPPER}
- DESTINATION include/iri-algorithms/wolf/ceres_wrapper)
-#INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
-# DESTINATION include/iri-algorithms/wolf/solver_suitesparse)
-INSTALL(FILES ${HDRS_SOLVER}
- DESTINATION include/iri-algorithms/wolf/solver)
-INSTALL(FILES ${HDRS_SERIALIZATION}
- DESTINATION include/iri-algorithms/wolf/serialization)
-INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/Findwolf.cmake"
- DESTINATION "lib/cmake/${PROJECT_NAME}")
-
-#install Find*.cmake
-configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/wolfConfig.cmake"
- "${CMAKE_BINARY_DIR}/wolfConfig.cmake" @ONLY)
-
-INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
-DESTINATION include/iri-algorithms/wolf/internal)
-
-INSTALL(FILES "${CMAKE_BINARY_DIR}/wolfConfig.cmake" DESTINATION "lib/cmake/${PROJECT_NAME}")
-
-INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
-
-export(PACKAGE ${PROJECT_NAME})
-
-#############
-## Testing ##
-#############
-IF (GLOG_FOUND)
- IF(BUILD_TESTS)
- MESSAGE("Building tests.")
- add_subdirectory(test)
- ENDIF(BUILD_TESTS)
-ENDIF (GLOG_FOUND)
-
-IF(BUILD_EXAMPLES)
- #Build examples
- MESSAGE("Building examples.")
- ADD_SUBDIRECTORY(examples)
-ENDIF(BUILD_EXAMPLES)
-
diff --git a/src/capture/CMakeLists.txt b/src/capture/CMakeLists.txt
deleted file mode 100644
index 57f34f813..000000000
--- a/src/capture/CMakeLists.txt
+++ /dev/null
@@ -1,26 +0,0 @@
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
-
-#=========================================
-#=========================================
-# Add in this section the CONDITIONAL CLUES [IF/ELSE]
-# for external libraries and move inside them the respective lines from above.
-
-
-
-#=========================================
-#=========================================
-
-SET(HDRS_CAPTURE ${HDRS_CAPTURE}
- ${CMAKE_CURRENT_SOURCE_DIR}/capture_velocity.h
- ${CMAKE_CURRENT_SOURCE_DIR}/capture_wheel_joint_position.h
- )
-
-SET(SRCS_CAPTURE ${SRCS_CAPTURE}
- ${CMAKE_CURRENT_SOURCE_DIR}/capture_velocity.cpp
- ${CMAKE_CURRENT_SOURCE_DIR}/capture_wheel_joint_position.cpp
- )
-
-# Forward var to parent scope
-# These lines always at the end
-SET(HDRS_CAPTURE ${HDRS_CAPTURE} PARENT_SCOPE)
-SET(SRCS_CAPTURE ${SRCS_CAPTURE} PARENT_SCOPE)
diff --git a/src/capture/capture_IMU.cpp b/src/capture/capture_IMU.cpp
deleted file mode 100644
index 8bfc7af04..000000000
--- a/src/capture/capture_IMU.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-#include "base/capture/capture_IMU.h"
-#include "base/sensor/sensor_IMU.h"
-#include "base/state_block/state_quaternion.h"
-
-namespace wolf {
-
-CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector6s& _acc_gyro_data,
- const Eigen::MatrixXs& _data_cov,
- FrameBasePtr _origin_frame_ptr) :
- CaptureMotion("IMU", _init_ts, _sensor_ptr, _acc_gyro_data, _data_cov, 10, 9, _origin_frame_ptr, nullptr, nullptr, std::make_shared<StateBlock>(6, false))
-{
- //
-}
-
-CaptureIMU::CaptureIMU(const TimeStamp& _init_ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector6s& _acc_gyro_data,
- const Eigen::MatrixXs& _data_cov,
- const Vector6s& _bias,
- FrameBasePtr _origin_frame_ptr) :
- CaptureMotion("IMU", _init_ts, _sensor_ptr, _acc_gyro_data, _data_cov, 10, 9, _origin_frame_ptr, nullptr, nullptr, std::make_shared<StateBlock>(_bias, false))
-{
- //
-}
-
-CaptureIMU::~CaptureIMU()
-{
- //
-}
-
-} //namespace wolf
diff --git a/src/capture/capture_laser_2D.cpp b/src/capture/capture_laser_2D.cpp
deleted file mode 100644
index a2cdd817e..000000000
--- a/src/capture/capture_laser_2D.cpp
+++ /dev/null
@@ -1,11 +0,0 @@
-#include "base/capture/capture_laser_2D.h"
-
-namespace wolf {
-
-CaptureLaser2D::CaptureLaser2D(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const std::vector<float>& _ranges) :
- CaptureBase("LASER 2D", _ts, _sensor_ptr), laser_ptr_(std::static_pointer_cast<SensorLaser2D>(getSensor())), scan_(_ranges)
-{
- //
-}
-
-} // namespace wolf
diff --git a/src/feature/feature_GPS_fix.cpp b/src/feature/feature_GPS_fix.cpp
deleted file mode 100644
index d4b5a12f2..000000000
--- a/src/feature/feature_GPS_fix.cpp
+++ /dev/null
@@ -1,16 +0,0 @@
-#include "base/feature/feature_GPS_fix.h"
-
-namespace wolf {
-
-FeatureGPSFix::FeatureGPSFix(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
- FeatureBase("GPS FIX", _measurement, _meas_covariance)
-{
- //
-}
-
-FeatureGPSFix::~FeatureGPSFix()
-{
- //
-}
-
-} // namespace wolf
diff --git a/src/feature/feature_GPS_pseudorange.cpp b/src/feature/feature_GPS_pseudorange.cpp
deleted file mode 100644
index 7979f30e4..000000000
--- a/src/feature/feature_GPS_pseudorange.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-#include "base/feature/feature_GPS_pseudorange.h"
-
-namespace wolf {
-
-FeatureGPSPseudorange::FeatureGPSPseudorange(Eigen::Vector3s &_sat_position, Scalar _pseudorange, Scalar _covariance) :
- FeatureBase("GPS PR", Eigen::VectorXs::Constant(1,_pseudorange), Eigen::MatrixXs::Identity(1,1)*_covariance),
- sat_position_(_sat_position),
- pseudorange_(_pseudorange)
-{
-// std::cout << "FeatureGPSPseudorange() " << std::setprecision(12)
-// << " --pr=" << pseudorange_
-// << "\t--pos(" << sat_position_[0] << ", " << sat_position_[1] << ", " << sat_position_[2] << ")"
-// << " --covariance =" << getMeasurementCovariance()
-// << std::endl;
-}
-
-FeatureGPSPseudorange::~FeatureGPSPseudorange()
-{
-
-}
-
-Scalar FeatureGPSPseudorange::getPseudorange() const
-{
- return pseudorange_;
-}
-
-const Eigen::Vector3s &FeatureGPSPseudorange::getSatPosition() const
-{
- return sat_position_;
-}
-
-} // namespace wolf
diff --git a/src/feature/feature_IMU.cpp b/src/feature/feature_IMU.cpp
deleted file mode 100644
index b35baf2d0..000000000
--- a/src/feature/feature_IMU.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-#include "base/feature/feature_IMU.h"
-
-namespace wolf {
-
-FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated,
- const Eigen::MatrixXs& _delta_preintegrated_covariance,
- const Eigen::Vector6s& _bias,
- const Eigen::Matrix<Scalar,9,6>& _dD_db_jacobians,
- CaptureMotionPtr _cap_imu_ptr) :
- FeatureBase("IMU", _delta_preintegrated, _delta_preintegrated_covariance),
- acc_bias_preint_(_bias.head<3>()),
- gyro_bias_preint_(_bias.tail<3>()),
- jacobian_bias_(_dD_db_jacobians)
-{
- if (_cap_imu_ptr)
- this->setCapture(_cap_imu_ptr);
-}
-
-FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr):
- FeatureBase("IMU", _cap_imu_ptr->getDeltaPreint(), _cap_imu_ptr->getDeltaPreintCov()),
- acc_bias_preint_ (_cap_imu_ptr->getCalibrationPreint().head<3>()),
- gyro_bias_preint_(_cap_imu_ptr->getCalibrationPreint().tail<3>()),
- jacobian_bias_(_cap_imu_ptr->getJacobianCalib())
-{
- this->setCapture(_cap_imu_ptr);
-}
-
-FeatureIMU::~FeatureIMU()
-{
- //
-}
-
-} // namespace wolf
diff --git a/src/feature/feature_corner_2D.cpp b/src/feature/feature_corner_2D.cpp
deleted file mode 100644
index 27803ef4a..000000000
--- a/src/feature/feature_corner_2D.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-
-#include "base/feature/feature_corner_2D.h"
-
-namespace wolf {
-
-FeatureCorner2D::FeatureCorner2D(const Eigen::Vector4s & _measurement, const Eigen::Matrix4s & _meas_covariance) :
- FeatureBase("CORNER 2D", _measurement, _meas_covariance)
-{
- //std::cout << "feature: "<< _measurement.transpose() << std::endl;
-}
-
-FeatureCorner2D::~FeatureCorner2D()
-{
- //
-}
-
-Scalar FeatureCorner2D::getAperture() const
-{
- return measurement_(3);
-}
-
-} // namespace wolf
diff --git a/src/feature/feature_line_2D.cpp b/src/feature/feature_line_2D.cpp
deleted file mode 100644
index c69ba2c1d..000000000
--- a/src/feature/feature_line_2D.cpp
+++ /dev/null
@@ -1,20 +0,0 @@
-
-#include "base/feature/feature_corner_2D.h"
-
-namespace wolf {
-
-FeatureLine2D::FeatureLine2D(const Eigen::Vector3s & _line_homogeneous_params,
- const Eigen::Matrix3s & _params_covariance, Eigen::Vector3s & _point1, Eigen::Vector3s & _point2) :
- FeatureBase("LINE_2D", _line_homogeneous_params, _params_covariance),
- first_point_(_point1),
- last_point_(_point2)
-{
- //
-}
-
-FeatureLine2D::~FeatureLine2D()
-{
- //
-}
-
-} // namespace wolf
diff --git a/src/feature/feature_polyline_2D.cpp b/src/feature/feature_polyline_2D.cpp
deleted file mode 100644
index 0f14a701c..000000000
--- a/src/feature/feature_polyline_2D.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-/**
- * \file feature_polyline.cpp
- *
- * Created on: May 26, 2016
- * \author: jvallve
- */
-
-#include "base/feature/feature_polyline_2D.h"
-
-namespace wolf
-{
-
-} /* namespace wolf */
diff --git a/src/landmark/CMakeLists.txt b/src/landmark/CMakeLists.txt
deleted file mode 100644
index bdfe8f65b..000000000
--- a/src/landmark/CMakeLists.txt
+++ /dev/null
@@ -1,24 +0,0 @@
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
-
-#=========================================
-#=========================================
-# Add in this section the CONDITIONAL CLUES [IF/ELSE]
-# for external libraries and move inside them the respective lines from above.
-
-
-
-#=========================================
-#=========================================
-
-SET(HDRS_LANDMARK ${HDRS_LANDMARK}
- ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_3D.h
- )
-
-SET(SRCS_LANDMARK ${SRCS_LANDMARK}
- ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_3D.cpp
- )
-
-# Forward var to parent scope
-# These lines always at the end
-SET(HDRS_LANDMARK ${HDRS_LANDMARK} PARENT_SCOPE)
-SET(SRCS_LANDMARK ${SRCS_LANDMARK} PARENT_SCOPE)
\ No newline at end of file
diff --git a/src/landmark/landmark_container.cpp b/src/landmark/landmark_container.cpp
deleted file mode 100644
index 3a043fd7d..000000000
--- a/src/landmark/landmark_container.cpp
+++ /dev/null
@@ -1,192 +0,0 @@
-
-#include "base/landmark/landmark_container.h"
-#include "base/state_block/state_block.h"
-
-namespace wolf {
-
-LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _witdh, const Scalar& _length) :
- LandmarkBase("CONTAINER", _p_ptr, _o_ptr),
- corners_(3,4)
-{
- Eigen::VectorXs descriptor(2);
- descriptor << _witdh, _length;
- setDescriptor(descriptor);
-
- corners_ << -_length / 2, _length / 2, _length / 2, -_length / 2,
- -_witdh / 2, -_witdh / 2, _witdh / 2, _witdh / 2,
- M_PI / 4, 3 * M_PI / 4,-3 * M_PI / 4,-M_PI / 4;
-}
-
-LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::Vector3s& _corner_1_pose, const Eigen::Vector3s& _corner_2_pose, const int& _corner_1_idx, const int& _corner_2_idx, const Scalar& _witdh, const Scalar& _length) :
- LandmarkBase("CONTAINER", _p_ptr, _o_ptr),
- corners_(3,4)
-{
- Eigen::VectorXs descriptor(2);
- descriptor << _witdh, _length;
- setDescriptor(descriptor);
-
- corners_ << -_length / 2, _length / 2, _length / 2, -_length / 2,
- -_witdh / 2, -_witdh / 2, _witdh / 2, _witdh / 2,
- M_PI / 4, 3 * M_PI / 4,-3 * M_PI / 4,-M_PI / 4;
-
- // Computing center
- WOLF_DEBUG( "Container constructor: Computing center pose... " );
- Eigen::Vector2s container_position(getP()->getState());
- Eigen::Vector1s container_orientation(getO()->getState());
-
- WOLF_DEBUG( "Container constructor: _corner_1_idx ", _corner_1_idx,
- "_corner_2_idx ", _corner_2_idx );
-
- // Large side detected (A & B)
- if ( (_corner_1_idx == 0 && _corner_2_idx == 1) || (_corner_1_idx == 1 && _corner_2_idx == 0) )
- {
- WOLF_DEBUG( "Large side detected" );
- Eigen::Vector2s AB = (_corner_1_idx == 0 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
- Eigen::Vector2s perpendicularAB;
- perpendicularAB << -AB(1)/AB.norm(), AB(0)/AB.norm();
- container_position = (_corner_1_idx == 0 ? _corner_1_pose.head(2) : _corner_2_pose.head(2)) + AB / 2 + perpendicularAB * _witdh / 2;
- container_orientation(0) = atan2(AB(1),AB(0));
- }
-
- // Short side detected (B & C)
- else if ( (_corner_1_idx == 1 && _corner_2_idx == 2) || (_corner_1_idx == 2 && _corner_2_idx == 1) )
- {
- WOLF_DEBUG( "Short side detected" );
- Eigen::Vector2s BC = (_corner_1_idx == 1 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
- Eigen::Vector2s perpendicularBC;
- perpendicularBC << -BC(1)/BC.norm(), BC(0)/BC.norm();
- container_position = (_corner_1_idx == 1 ? _corner_1_pose.head(2) : _corner_2_pose.head(2)) + BC / 2 + perpendicularBC * _length / 2;
- container_orientation(0) = atan2(BC(1),BC(0)) - M_PI / 2;
- }
-
- // Diagonal detected
- // A & C
- else if ( (_corner_1_idx == 0 && _corner_2_idx == 2) || (_corner_1_idx == 2 && _corner_2_idx == 0) )
- {
- WOLF_DEBUG( "diagonal AC detected" );
- Eigen::Vector2s AC = (_corner_1_idx == 0 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
- container_position = (_corner_1_idx == 0 ? _corner_1_pose.head(2) : _corner_2_pose.head(2)) + AC / 2;
- container_orientation(0) = atan2(AC(1),AC(0)) - atan2(_witdh,_length);
- }
- // B & D
- else if ( (_corner_1_idx == 1 && _corner_2_idx == 3) || (_corner_1_idx == 3 && _corner_2_idx == 1) )
- {
- WOLF_DEBUG( "diagonal BD detected" );
- Eigen::Vector2s BD = (_corner_1_idx == 1 ? _corner_2_pose.head(2) - _corner_1_pose.head(2) : _corner_1_pose.head(2) - _corner_2_pose.head(2));
- container_position = (_corner_1_idx == 1 ? _corner_1_pose.head(2) : _corner_2_pose.head(2)) + BD / 2;
- container_orientation(0) = atan2(BD(1),BD(0)) + atan2(_witdh,_length);
- }
- else
- assert(0 && "index corners combination not implemented!");
-
- WOLF_DEBUG( "_corner_1_pose... ", _corner_1_pose.transpose() );
- WOLF_DEBUG( "_corner_2_pose... ", _corner_2_pose.transpose() );
- WOLF_DEBUG( "Container center pose... ", container_position.transpose(), " ", container_orientation.transpose() );
-
- getP()->setState(container_position);
- getO()->setState(container_orientation);
-}
-
-//LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const Scalar& _witdh, const Scalar& _length) :
-// LandmarkBase(_p_ptr, _o_ptr),
-// corners_(3,4)
-//{
-// assert((_corner_A_ptr != nullptr || _corner_B_ptr != nullptr || _corner_C_ptr != nullptr || _corner_D_ptr != nullptr) && "all corner pointer are null in landmark container constructor from corners");
-//
-// Eigen::VectorXs descriptor(2);
-// descriptor << _witdh, _length;
-// setDescriptor(descriptor);
-//
-// corners_ << -_length / 2, _length / 2, _length / 2, -_length / 2,
-// -_witdh / 2, -_witdh / 2, _witdh / 2, _witdh / 2,
-// 0, M_PI/2, M_PI, -M_PI/2;
-//
-// // Computing center
-// //std::cout << "Container constructor: Computing center position... " << std::endl;
-// Eigen::Map<Eigen::Vector2s> container_position(_p_ptr->get());
-// Eigen::Map<Eigen::VectorXs> container_orientation(_o_ptr->get(), _o_ptr->getSize());
-//
-// container_position = Eigen::Vector2s::Zero();
-//
-// // Large side detected
-// // A & B
-// if (_corner_A_ptr != nullptr && _corner_B_ptr != nullptr)
-// {
-// Eigen::Vector2s AB = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get());
-// Eigen::Vector2s perpendicularAB;
-// perpendicularAB << -AB(1)/AB.norm(), AB(0)/AB.norm();
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get()) + AB / 2 + perpendicularAB * _witdh / 2;
-// container_orientation(0) = atan2(AB(1),AB(0));
-// }
-// // C & D
-// else if (_corner_C_ptr != nullptr && _corner_D_ptr != nullptr)
-// {
-// Eigen::Vector2s CD = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getP()->get());
-// Eigen::Vector2s perpendicularCD;
-// perpendicularCD << -CD(1)/CD.norm(), CD(0)/CD.norm();
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getP()->get()) + CD / 2 + perpendicularCD * _witdh / 2;
-// container_orientation(0) = atan2(-CD(1),-CD(0));
-// }
-// // Short side detected
-// // B & C
-// else if (_corner_B_ptr != nullptr && _corner_C_ptr != nullptr)
-// {
-// Eigen::Vector2s BC = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get());
-// Eigen::Vector2s perpendicularBC;
-// perpendicularBC << -BC(1)/BC.norm(), BC(0)/BC.norm();
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get()) + BC / 2 + perpendicularBC * _length / 2;
-// container_orientation(0) = atan2(BC(1),BC(0));
-// }
-// // D & A
-// else if (_corner_D_ptr != nullptr && _corner_A_ptr != nullptr)
-// {
-// Eigen::Vector2s DA = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getP()->get());
-// Eigen::Vector2s perpendicularDA;
-// perpendicularDA << -DA(1)/DA.norm(), DA(0)/DA.norm();
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getP()->get()) + DA / 2 + perpendicularDA * _length / 2;
-// container_orientation(0) = atan2(-DA(1),-DA(0));
-// }
-// // Diagonal detected
-// // A & C
-// else if (_corner_A_ptr != nullptr && _corner_C_ptr != nullptr)
-// {
-// Eigen::Vector2s AC = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get());
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get()) + AC / 2;
-// container_orientation(0) = atan2(AC(1),AC(0)) - atan2(_witdh,_length);
-// }
-// // B & D
-// else if (_corner_B_ptr != nullptr && _corner_D_ptr != nullptr)
-// {
-// Eigen::Vector2s BD = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get());
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get()) + BD / 2;
-// container_orientation(0) = atan2(BD(1),BD(0)) + atan2(_witdh,_length);
-// }
-//}
-
-LandmarkContainer::~LandmarkContainer()
-{
- //
-}
-
-Scalar LandmarkContainer::getWidth() const
-{
- return descriptor_(0);
-}
-
-Scalar LandmarkContainer::getLength() const
-{
- return descriptor_(1);
-}
-
-Eigen::MatrixXs LandmarkContainer::getCorners() const
-{
- return corners_;
-}
-
-Eigen::VectorXs LandmarkContainer::getCorner(const unsigned int _id) const
-{
- assert(/*_id >= 0 &&*/ _id <= 4 && "wrong corner id parameter in getCorner(id)");
- return corners_.col(_id);
-}
-
-} // namespace wolf
diff --git a/src/landmark/landmark_corner_2D.cpp b/src/landmark/landmark_corner_2D.cpp
deleted file mode 100644
index 558292f90..000000000
--- a/src/landmark/landmark_corner_2D.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-
-#include "base/landmark/landmark_corner_2D.h"
-
-namespace wolf {
-
-LandmarkCorner2D::LandmarkCorner2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Scalar& _aperture) :
- LandmarkBase("CORNER 2D", _p_ptr, _o_ptr)
-{
- setDescriptor(Eigen::VectorXs::Constant(1,_aperture));
-}
-
-LandmarkCorner2D::~LandmarkCorner2D()
-{
- //
-}
-
-Scalar LandmarkCorner2D::getAperture() const
-{
- return descriptor_(0);
-}
-
-} // namespace wolf
diff --git a/src/landmark/landmark_line_2D.cpp b/src/landmark/landmark_line_2D.cpp
deleted file mode 100644
index aa2a14d9f..000000000
--- a/src/landmark/landmark_line_2D.cpp
+++ /dev/null
@@ -1,58 +0,0 @@
-
-#include "base/landmark/landmark_line_2D.h"
-
-namespace wolf {
-
-LandmarkLine2D::LandmarkLine2D(StateBlockPtr _p_ptr, Eigen::Vector3s & _point1, Eigen::Vector3s & _point2) :
- LandmarkBase("LINE 2D", _p_ptr)
-{
- //set extreme points
- point1_ = _point1;
- point2_ = _point2;
-}
-
-LandmarkLine2D::~LandmarkLine2D()
-{
- //
-}
-
-void LandmarkLine2D::updateExtremePoints(Eigen::Vector3s & _q1, Eigen::Vector3s & _q2)
-{
- //Mainly this method performs two actions:
- // 1. First check if new points are "inside" or "outside" the segment
- // 2. In case extremes have been updated, fit them on the line
- //
- // p1 & p2 are this->point1_ and point2_
- // _q1 & _q2 are the new points to evaluate
- // vector p1p2 is the vector from p1 to p2
- // dot product s_p1q1 is the dot product between vector p1p2 and vector p1q1
- // dot product s_p2q1 is the dot product between vector p2p1 and vector p2q1
-
- //local variables
- Eigen::Vector2s p1p2, p1q1, p1q2, p2p1, p2q1, p2q2;
- //Scalar s_p1q1, s_p1q2, s_p2q1, s_p2q2;
-
- //compute all necessary vectors
- p1p2 = point2_.head(2) - point1_.head(1);
- p1q1 = _q1.head(2) - point1_.head(1);
- p1q2 = _q2.head(2) - point1_.head(1);
- p2p1 = point1_.head(2) - point2_.head(1);
- p2q1 = _q1.head(2) - point2_.head(1);
- p2q2 = _q2.head(2) - point2_.head(1);
-
- //compute all necessary scalar products.
- //s_p1q1 = p1p2.dot()
-
-}
-
-const Eigen::Vector3s & LandmarkLine2D::point1() const
-{
- return point1_;
-}
-
-const Eigen::Vector3s & LandmarkLine2D::point2() const
-{
- return point2_;
-}
-
-} // namespace wolf
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
deleted file mode 100644
index bbe0747e0..000000000
--- a/src/landmark/landmark_polyline_2D.cpp
+++ /dev/null
@@ -1,397 +0,0 @@
-/**
- * \file landmark_polyline_2D.cpp
- *
- * Created on: May 26, 2016
- * \author: jvallve
- */
-
-#include "base/feature/feature_polyline_2D.h"
-#include "base/landmark/landmark_polyline_2D.h"
-#include "base/state_block/local_parametrization_polyline_extreme.h"
-#include "base/factor/factor_point_2D.h"
-#include "base/factor/factor_point_to_line_2D.h"
-#include "base/state_block/state_block.h"
-#include "base/common/factory.h"
-#include "base/yaml/yaml_conversion.h"
-
-namespace wolf
-{
-
-LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _points, const bool _first_extreme, const bool _last_extreme, unsigned int _first_id, LandmarkClassification _class) :
- LandmarkBase("POLYLINE 2D", _p_ptr, _o_ptr), first_id_(_first_id), first_defined_(_first_extreme), last_defined_(_last_extreme), closed_(false), classification_(_class)
-{
- //std::cout << "LandmarkPolyline2D::LandmarkPolyline2D" << std::endl;
- assert(_points.cols() >= 2 && "LandmarkPolyline2D::LandmarkPolyline2D: 2 points at least needed.");
- for (auto i = 0; i < _points.cols(); i++)
- point_state_ptr_vector_.push_back(std::make_shared<StateBlock>(_points.col(i).head<2>()));
-
- if (!first_defined_)
- point_state_ptr_vector_.front()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[1]));
- if (!last_defined_)
- point_state_ptr_vector_.back()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[point_state_ptr_vector_.size() - 2]));
-
- assert(point_state_ptr_vector_.front()->hasLocalParametrization() ? !first_defined_ : first_defined_);
- assert(point_state_ptr_vector_.back()->hasLocalParametrization() ? !last_defined_ : last_defined_);
-}
-
-LandmarkPolyline2D::~LandmarkPolyline2D()
-{
- removeStateBlocks();
-}
-
-void LandmarkPolyline2D::setFirst(const Eigen::VectorXs& _point, bool _defined)
-{
- //std::cout << "LandmarkPolyline2D::setFirst. Defined " << _defined << std::endl;
- assert(_point.size() >= 2 && "LandmarkPolyline2D::setFirstExtreme: bad point size");
- assert(!(!_defined && first_defined_) && "setting a defined extreme with a not defined point");
- point_state_ptr_vector_.front()->setState(_point.head(2));
- if (!first_defined_ && _defined)
- defineExtreme(false);
-}
-
-void LandmarkPolyline2D::setLast(const Eigen::VectorXs& _point, bool _defined)
-{
- //std::cout << "LandmarkPolyline2D::setLast. Defined " << _defined << std::endl;
- assert(_point.size() >= 2 && "LandmarkPolyline2D::setLastExtreme: bad point size");
- assert(!(!_defined && last_defined_) && "setting a defined extreme with a not defined point");
- point_state_ptr_vector_.back()->setState(_point.head(2));
- if (!last_defined_ && _defined)
- defineExtreme(true);
-}
-
-const Eigen::VectorXs LandmarkPolyline2D::getPointVector(int _i) const
-{
- //std::cout << "LandmarkPolyline2D::getPointVector: " << _i << std::endl;
- //std::cout << "First: " << first_id_ << " - size: " << point_state_ptr_vector_.size() << std::endl;
- assert(_i >= first_id_ && _i < first_id_+(int)(point_state_ptr_vector_.size()));
- return point_state_ptr_vector_[_i-first_id_]->getState();
-}
-
-StateBlockPtr LandmarkPolyline2D::getPointStateBlock(int _i)
-{
- assert(_i-first_id_ >= 0 && _i-first_id_ <= (int)(point_state_ptr_vector_.size()) && "out of range!");
- return point_state_ptr_vector_[_i-first_id_];
-}
-
-void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _defined, const bool& _back)
-{
- assert(!closed_ && "adding point to a closed polyline!");
-
- //std::cout << "LandmarkPolyline2D::addPoint. Defined " << _defined << std::endl;
- assert(_point.size() >= 2 && "bad point size");
-
- // define previous extreme if not defined
- if (_back ? !last_defined_ : !first_defined_)
- defineExtreme(_back);
-
- // add new extreme
- if (_back)
- {
- point_state_ptr_vector_.push_back(std::make_shared<StateBlock>(_point.head<2>(), false,
- (!_defined ?
- std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_.back()) :
- nullptr)));
- if (getProblem() != nullptr)
- getProblem()->addStateBlock(point_state_ptr_vector_.back());
- last_defined_ = _defined;
- assert(point_state_ptr_vector_.back()->hasLocalParametrization() ? !last_defined_ : last_defined_);
- }
- else
- {
- point_state_ptr_vector_.push_front(std::make_shared<StateBlock>(_point.head<2>(), false,
- (!_defined ?
- std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_.front()) :
- nullptr)));
- if (getProblem() != nullptr)
- getProblem()->addStateBlock(point_state_ptr_vector_.front());
- first_defined_ = _defined;
- first_id_--;
- assert(point_state_ptr_vector_.front()->hasLocalParametrization() ? !first_defined_ : first_defined_);
- }
-}
-
-void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigned int& _idx, const bool& _defined,
- const bool& _back)
-{
- assert(!closed_ && "adding points to a closed polyline!");
-
- //std::cout << "LandmarkPolyline2D::addPoints from/to: " << _idx << " Defined " << _defined << std::endl;
- assert(_points.rows() >= 2 && "bad points size");
- assert(_idx < _points.cols() && "bad index!");
-
- // define previous extreme if not defined
- if (_back ? !last_defined_ : !first_defined_)
- defineExtreme(_back);
-
- // add new extreme points
- if (_back)
- {
- for (int i = _idx; i < _points.cols(); i++)
- {
- point_state_ptr_vector_.push_back(std::make_shared<StateBlock>(_points.block(0,i,2,1),
- false,
- (i == _points.cols()-1 && !_defined ?
- std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_.back()) :
- nullptr)));
- if (getProblem() != nullptr)
- getProblem()->addStateBlock(point_state_ptr_vector_.back());
- }
- last_defined_ = _defined;
- assert(point_state_ptr_vector_.back()->hasLocalParametrization() ? !last_defined_ : last_defined_);
- }
- else
- {
- for (int i = _idx; i >= 0; i--)
- {
- point_state_ptr_vector_.push_front(std::make_shared<StateBlock>(_points.block(0,i,2,1),
- false,
- (i == 0 && !_defined ?
- std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_.front()) :
- nullptr)));
- if (getProblem() != nullptr)
- getProblem()->addStateBlock(point_state_ptr_vector_.front());
- first_id_--;
- }
- first_defined_ = _defined;
- assert(point_state_ptr_vector_.front()->hasLocalParametrization() ? !first_defined_ : first_defined_);
- }
-
- //std::cout << "final number of points: " << point_state_ptr_vector_.size() << std::endl;
-}
-
-void LandmarkPolyline2D::defineExtreme(const bool _back)
-{
- StateBlockPtr state = (_back ? point_state_ptr_vector_.back() : point_state_ptr_vector_.front());
- assert((_back ? !last_defined_: !first_defined_) && "defining an already defined extreme");
- assert(state->hasLocalParametrization() && "not defined extreme without local parameterization");
-
- //std::cout << "Defining extreme --> Removing and adding state blocks and factors" << std::endl;
-
- // remove and add state block without local parameterization
- if (getProblem() != nullptr)
- getProblem()->removeStateBlock(state);
-
- state->removeLocalParametrization();
-
- if (getProblem() != nullptr)
- getProblem()->addStateBlock(state);
-
- // remove and add all factors to the point
- for (auto fac_ptr : getConstrainedByList())
- for (auto st_ptr : fac_ptr->getStateBlockPtrVector())
- if (st_ptr == state && getProblem() != nullptr)
- {
- getProblem()->removeFactor(fac_ptr);
- getProblem()->addFactor(fac_ptr);
- }
-
- // update boolean
- if (_back)
- last_defined_ = true;
- else
- first_defined_ = true;
-}
-
-void LandmarkPolyline2D::setClosed()
-{
- std::cout << "setting polyline landmark closed" << std::endl;
-
- assert(getNPoints() - (first_defined_ ? 0 : 1) - (last_defined_ ? 0 : 1) >= 2 && "closing a polyline with less than 2 defined points");
-
- // merge first not defined with last defined
- if (!first_defined_)
- {
- std::cout << "not defined first point: merging with last definite point" << std::endl;
-
- mergePoints(first_id_, getLastId() - (last_defined_ ? 0 : 1));
- first_id_++;
- first_defined_ = true;
- }
- // merge last not defined with first (defined for sure)
- if (!last_defined_)
- {
- std::cout << "not defined last point: merging with first point" << std::endl;
-
- mergePoints(getLastId(), first_id_);
- last_defined_ = true;
- }
-
- // set closed
- closed_ = true;
-}
-
-void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
-{
- std::cout << "merge points: remove " << _remove_id << " and keep " << _remain_id << " (ids: " << first_id_ << " to " << getLastId() << ")" << std::endl;
-
- assert(_remove_id >= first_id_ && _remove_id <= getLastId());
- assert(_remain_id >= first_id_ && _remain_id <= getLastId());
- assert(_remain_id > first_id_ || first_defined_);
- assert(_remain_id < getLastId() || last_defined_);
-
- // take a defined extreme as remaining
- StateBlockPtr remove_state = getPointStateBlock(_remove_id);
- std::cout << "state block to remove " << remove_state->getState().transpose() << std::endl;
-
- // Change factors from remove_state to remain_state
- FactorBasePtrList old_factors_list = getConstrainedByList();
- std::cout << "changing factors: " << old_factors_list.size() << std::endl;
- FactorBasePtr new_fac_ptr = nullptr;
- for (auto fac_ptr : old_factors_list)
- {
- FactorPoint2DPtr fac_point_ptr;
- FactorPointToLine2DPtr fac_point_line_ptr;
- if ( (fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr)))
-// if (fac_ptr->getTypeId() == FAC_POINT_2D)
- {
-// FactorPoint2DPtr fac_point_ptr = std::static_pointer_cast<FactorPoint2D>(fac_ptr);
-
- // If landmark point constrained -> new factor
- if (fac_point_ptr->getLandmarkPointId() == _remove_id)
- new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
- std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- fac_point_ptr->getProcessor(),
- fac_point_ptr->getFeaturePointId(),
- _remain_id,
- fac_point_ptr->getApplyLossFunction(),
- fac_point_ptr->getStatus());
- }
- else if ((fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr)))
-// else if (fac_ptr->getTypeId() == FAC_POINT_TO_LINE_2D)
- {
-// FactorPointToLine2DPtr fac_point_line_ptr = std::static_pointer_cast<FactorPointToLine2D>(fac_ptr);
-
- // If landmark point constrained -> new factor
- if (fac_point_line_ptr->getLandmarkPointId() == _remove_id)
- new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
- std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- fac_point_line_ptr->getProcessor(),
- fac_point_line_ptr->getFeaturePointId(),
- _remain_id,
- fac_point_line_ptr->getLandmarkPointAuxId(),
- fac_point_ptr->getApplyLossFunction(),
- fac_point_line_ptr->getStatus());
- // If landmark point is aux point -> new factor
- else if (fac_point_line_ptr->getLandmarkPointAuxId() == _remove_id)
- new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
- std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- fac_point_line_ptr->getProcessor(),
- fac_point_line_ptr->getFeaturePointId(),
- fac_point_line_ptr->getLandmarkPointId(),
- _remain_id,
- fac_point_line_ptr->getApplyLossFunction(),
- fac_point_line_ptr->getStatus());
- }
- else
- throw std::runtime_error ("polyline factor of unknown type");
-
- // If new factor
- if (new_fac_ptr != nullptr)
- {
- std::cout << "created new factor: " << new_fac_ptr->id() << std::endl;
- std::cout << "deleting factor: " << fac_ptr->id() << std::endl;
-
- // add new factor
- fac_ptr->getFeature()->addFactor(new_fac_ptr);
-
- // remove factor
- fac_ptr->remove();
-
- new_fac_ptr = nullptr;
- }
- }
-
- // Remove remove_state
- if (getProblem() != nullptr)
- getProblem()->removeStateBlock(remove_state);
- std::cout << "state removed " << std::endl;
-
- // remove element from deque
- point_state_ptr_vector_.erase(point_state_ptr_vector_.begin() + _remove_id - first_id_);
- std::cout << "state removed from point vector " << std::endl;
-}
-
-void LandmarkPolyline2D::registerNewStateBlocks()
-{
- LandmarkBase::registerNewStateBlocks();
- if (getProblem() != nullptr)
- for (auto state : point_state_ptr_vector_)
- getProblem()->addStateBlock(state);
-}
-
-void LandmarkPolyline2D::removeStateBlocks()
-{
- for (unsigned int i = 0; i < point_state_ptr_vector_.size(); i++)
- {
- auto sbp = point_state_ptr_vector_[i];
- if (sbp != nullptr)
- {
- if (getProblem() != nullptr)
- {
- getProblem()->removeStateBlock(sbp);
- }
- point_state_ptr_vector_[i] = nullptr;
- }
- }
- LandmarkBase::removeStateBlocks();
-}
-
-// static
-LandmarkBasePtr LandmarkPolyline2D::create(const YAML::Node& _lmk_node)
-{
- // Parse YAML node with lmk info and data
- unsigned int id = _lmk_node["id"].as<unsigned int>();
- Eigen::VectorXs pos = _lmk_node["position"].as<Eigen::VectorXs>();
- bool pos_fixed = true;//_lmk_node["position fixed"].as<bool>();
- Eigen::VectorXs ori = _lmk_node["orientation"].as<Eigen::VectorXs>();
- bool ori_fixed = true;//_lmk_node["orientation fixed"].as<bool>();
- int first_id = _lmk_node["first_id"].as<int>();
- bool first_defined = _lmk_node["first_defined"].as<bool>();
- bool last_defined = _lmk_node["last_defined"].as<bool>();
- unsigned int npoints = _lmk_node["points"].size();
- LandmarkClassification classification = (LandmarkClassification)(_lmk_node["classification"].as<int>());
- Eigen::MatrixXs points(2,npoints);
- for (unsigned int i = 0; i < npoints; i++)
- {
- points.col(i) = _lmk_node["points"][i].as<Eigen::Vector2s>();
- }
-
- // Create a new landmark
- LandmarkPolyline2DPtr lmk_ptr = std::make_shared<LandmarkPolyline2D>(std::make_shared<StateBlock>(pos, pos_fixed), std::make_shared<StateBlock>(ori, ori_fixed), points, first_defined, last_defined, first_id, classification);
- lmk_ptr->setId(id);
-
- // fix all points
- for (auto st_ptr : lmk_ptr->getPointsStateBlockVector())
- st_ptr->fix();
-
- return lmk_ptr;
-}
-
-YAML::Node LandmarkPolyline2D::saveToYaml() const
-{
- // First base things
- YAML::Node node = LandmarkBase::saveToYaml();
-
- // Then add specific things
- node["first_id"] = first_id_;
- node["first_defined"] = first_defined_;
- node["last_defined"] = last_defined_;
- node["classification"] = (int)classification_;
-
- int npoints = point_state_ptr_vector_.size();
-
- for (int i = 0; i < npoints; i++)
- {
- node["points"].push_back(point_state_ptr_vector_[i]->getState());
- }
-
- return node;
-}
-
-// Register landmark creator
-namespace
-{
-const bool WOLF_UNUSED registered_lmk_polyline_2D = LandmarkFactory::get().registerCreator("POLYLINE 2D", LandmarkPolyline2D::create);
-}
-
-} /* namespace wolf */
diff --git a/src/processor/CMakeLists.txt b/src/processor/CMakeLists.txt
deleted file mode 100644
index 202687990..000000000
--- a/src/processor/CMakeLists.txt
+++ /dev/null
@@ -1,43 +0,0 @@
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
-
-#=========================================
-#=========================================
-# Add in this section the CONDITIONAL CLUES [IF/ELSE]
-# for external libraries and move inside them the respective lines from above.
-
-IF (vision_utils_FOUND)
-SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- ${CMAKE_CURRENT_SOURCE_DIR}/processor_tracker_feature_trifocal.h
- )
-
-SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
- ${CMAKE_CURRENT_SOURCE_DIR}/processor_tracker_feature_trifocal.cpp
- )
-ENDIF (vision_utils_FOUND)
-
-IF (OPENCV_FOUND AND Apriltag_FOUND)
- SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- ${CMAKE_CURRENT_SOURCE_DIR}/processor_tracker_landmark_apriltag.h
- ${CMAKE_CURRENT_SOURCE_DIR}/ippe.h
- )
- SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
- ${CMAKE_CURRENT_SOURCE_DIR}/processor_tracker_landmark_apriltag.cpp
- ${CMAKE_CURRENT_SOURCE_DIR}/ippe.cpp
- )
-ENDIF(OPENCV_FOUND AND Apriltag_FOUND)
-#=========================================
-#=========================================
-
-
-SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
- ${CMAKE_CURRENT_SOURCE_DIR}/processor_diff_drive.h
- )
-
-SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
- ${CMAKE_CURRENT_SOURCE_DIR}/processor_diff_drive.cpp
- )
-
-# Forward var to parent scope
-# These lines always at the end
-SET(HDRS_PROCESSOR ${HDRS_PROCESSOR} PARENT_SCOPE)
-SET(SRCS_PROCESSOR ${SRCS_PROCESSOR} PARENT_SCOPE)
diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp
deleted file mode 100644
index 2dfa2cd7d..000000000
--- a/src/processor/processor_IMU.cpp
+++ /dev/null
@@ -1,338 +0,0 @@
-// wolf
-#include "base/processor/processor_IMU.h"
-#include "base/factor/factor_IMU.h"
-#include "base/math/IMU_tools.h"
-
-namespace wolf {
-
-ProcessorIMU::ProcessorIMU(ProcessorParamsIMUPtr _params_motion_IMU) :
- ProcessorMotion("IMU", 10, 10, 9, 6, 6, _params_motion_IMU),
- params_motion_IMU_(std::make_shared<ProcessorParamsIMU>(*_params_motion_IMU))
-{
- // Set constant parts of Jacobians
- jacobian_delta_preint_.setIdentity(9,9); // dDp'/dDp, dDv'/dDv, all zeros
- jacobian_delta_.setIdentity(9,9); //
- jacobian_calib_.setZero(9,6);
-}
-
-ProcessorIMU::~ProcessorIMU()
-{
- //
-}
-
-ProcessorBasePtr ProcessorIMU::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr)
-{
- std::shared_ptr<ProcessorParamsIMU> prc_imu_params;
- if (_params)
- prc_imu_params = std::static_pointer_cast<ProcessorParamsIMU>(_params);
- else
- prc_imu_params = std::make_shared<ProcessorParamsIMU>();
-
- ProcessorIMUPtr prc_ptr = std::make_shared<ProcessorIMU>(prc_imu_params);
- prc_ptr->setName(_unique_name);
- return prc_ptr;
-}
-
-bool ProcessorIMU::voteForKeyFrame()
-{
- if(!isVotingActive())
- return false;
- // time span
- if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_motion_IMU_->max_time_span)
- {
- WOLF_DEBUG( "PM: vote: time span" );
- return true;
- }
- // buffer length
- if (getBuffer().get().size() > params_motion_IMU_->max_buff_length)
- {
- WOLF_DEBUG( "PM: vote: buffer length" );
- return true;
- }
- // angle turned
- Scalar angle = 2.0 * asin(delta_integrated_.segment(3,3).norm());
- if (angle > params_motion_IMU_->angle_turned)
- {
- WOLF_DEBUG( "PM: vote: angle turned" );
- return true;
- }
- //WOLF_DEBUG( "PM: do not vote" );
- return false;
-}
-
-Motion ProcessorIMU::interpolate(const Motion& _motion_ref, Motion& _motion_second, TimeStamp& _ts)
-{
- /* Note: See extensive documentation in ProcessorMotion::interpolate().
- *
- * Interpolate between motion_ref and motion, as in:
- *
- * motion_ref ------ ts_ ------ motion_sec
- * return
- *
- * and return the motion at the given time_stamp ts_.
- *
- * DATA:
- * Data receives no change
- *
- * DELTA:
- * The delta's position and velocity receive linear interpolation:
- * p_ret = (ts - t_ref) / dt * (p - p_ref)
- * v_ret = (ts - t_ref) / dt * (v - v_ref)
- *
- * The delta's quaternion receives a slerp interpolation
- * q_ret = q_ref.slerp( (ts - t_ref) / dt , q);
- *
- * DELTA_INTEGR:
- * The interpolated delta integral is just the reference integral plus the interpolated delta
- *
- * The second integral does not change
- *
- * Covariances receive linear interpolation
- * Q_ret = (ts - t_ref) / dt * Q_sec
- */
- /*
- // resolve out-of-bounds time stamp as if the time stamp was exactly on the bounds
- if (_ts <= _motion_ref.ts_) // behave as if _ts == _motion_ref.ts_
- {
- // return null motion. Second stays the same.
- Motion motion_int ( _motion_ref );
- motion_int.data_ = _motion_second.data_;
- motion_int.data_cov_ = _motion_second.data_cov_;
- motion_int.delta_ = deltaZero();
- motion_int.delta_cov_ . setZero();
- return motion_int;
- }
- if (_ts >= _motion_second.ts_) // behave as if _ts == _motion_second.ts_
- {
- // return original second motion. Second motion becomes null motion
- Motion motion_int ( _motion_second );
- _motion_second.delta_ = deltaZero();
- _motion_second.delta_cov_ . setZero();
- return motion_int;
- }
- assert(_motion_ref.ts_ <= _ts && "Interpolation time stamp out of bounds");
- assert(_ts <= _motion_second.ts_ && "Interpolation time stamp out of bounds");
-
- assert(_motion_ref.delta_.size() == delta_size_ && "Wrong delta size");
- assert(_motion_ref.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_ref.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_ref.delta_integr_.size() == delta_size_ && "Wrong delta size");
- assert(_motion_ref.delta_integr_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_ref.delta_integr_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
-
- assert(_motion_second.delta_.size() == delta_size_ && "Wrong delta size");
- assert(_motion_second.delta_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_second.delta_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_second.delta_integr_.size() == delta_size_ && "Wrong delta size");
- assert(_motion_second.delta_integr_cov_.cols() == delta_cov_size_ && "Wrong delta cov size");
- assert(_motion_second.delta_integr_cov_.rows() == delta_cov_size_ && "Wrong delta cov size");
-
- // reference
- TimeStamp t_ref = _motion_ref.ts_;
-
- // second
- TimeStamp t_sec = _motion_second.ts_;
- Map<VectorXs> motion_sec_dp (_motion_second.delta_.data() + 0, 3);
- Map<Quaternions> motion_sec_dq (_motion_second.delta_.data() + 3 );
- Map<VectorXs> motion_sec_dv (_motion_second.delta_.data() + 7, 3);
-
- // interpolated
- Motion motion_int = motionZero(_ts);
-
- // Jacobians for covariance propagation
- MatrixXs J_ref(delta_cov_size_, delta_cov_size_);
- MatrixXs J_int(delta_cov_size_, delta_cov_size_);
-
- // interpolation factor
- Scalar dt1 = _ts - t_ref;
- Scalar dt2 = t_sec - _ts;
- Scalar tau = dt1 / (t_sec - t_ref); // interpolation factor (0 to 1)
- Scalar tau_sq = tau * tau;
-
- // copy data
- motion_int.data_ = _motion_second.data_;
- motion_int.data_cov_ = _motion_second.data_cov_;
-
- // interpolate delta
- motion_int.ts_ = _ts;
- Map<VectorXs> motion_int_dp (motion_int.delta_.data() + 0, 3);
- Map<Quaternions> motion_int_dq (motion_int.delta_.data() + 3 );
- Map<VectorXs> motion_int_dv (motion_int.delta_.data() + 7, 3);
- motion_int_dp = tau_sq * motion_sec_dp; // FIXME: delta_p not correctly interpolated
- motion_int_dv = tau * motion_sec_dv;
- motion_int_dq = Quaternions::Identity().slerp(tau, motion_sec_dq);
- motion_int.delta_cov_ = tau * _motion_second.delta_cov_;
-
- // integrate
- deltaPlusDelta(_motion_ref.delta_integr_, motion_int.delta_, dt1, motion_int.delta_integr_, J_ref, J_int);
- motion_int.delta_integr_cov_ = J_ref * _motion_ref.delta_integr_cov_ * J_ref.transpose()
- + J_int * _motion_second.delta_cov_ * J_int.transpose();
-
- // update second delta ( in place update )
- motion_sec_dp = motion_int_dq.conjugate() * (motion_sec_dp - motion_int_dp - motion_int_dv * dt2);
- motion_sec_dv = motion_int_dq.conjugate() * (motion_sec_dv - motion_int_dv);
- motion_sec_dq = motion_int_dq.conjugate() * motion_sec_dq;
- _motion_second.delta_cov_ *= (1 - tau); // easy interpolation // TODO check for correctness
- //Dp = Dp; // trivial, just leave the code commented
- //Dq = Dq; // trivial, just leave the code commented
- //_motion.delta_integr_cov_ = _motion.delta_integr_cov_; // trivial, just leave the code commented
-
- return motion_int;
- */
-
- // return _motion_ref;
-
- return ProcessorMotion::interpolate(_motion_ref, _motion_second, _ts);
-
-}
-
-CaptureMotionPtr ProcessorIMU::createCapture(const TimeStamp& _ts,
- const SensorBasePtr& _sensor,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin)
-{
- CaptureIMUPtr capture_imu = std::make_shared<CaptureIMU>(_ts,
- _sensor,
- _data,
- _data_cov,
- _frame_origin);
- return capture_imu;
-}
-
-FeatureBasePtr ProcessorIMU::createFeature(CaptureMotionPtr _capture_motion)
-{
- FeatureIMUPtr key_feature_ptr = std::make_shared<FeatureIMU>(
- _capture_motion->getBuffer().get().back().delta_integr_,
- _capture_motion->getBuffer().get().back().delta_integr_cov_,
- _capture_motion->getBuffer().getCalibrationPreint(),
- _capture_motion->getBuffer().get().back().jacobian_calib_);
- return key_feature_ptr;
-}
-
-FactorBasePtr ProcessorIMU::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
-{
- CaptureIMUPtr cap_imu = std::static_pointer_cast<CaptureIMU>(_capture_origin);
- FeatureIMUPtr ftr_imu = std::static_pointer_cast<FeatureIMU>(_feature_motion);
- FactorIMUPtr fac_imu = std::make_shared<FactorIMU>(ftr_imu, cap_imu, shared_from_this());
-
- // link ot wolf tree
- _feature_motion->addFactor(fac_imu);
- _capture_origin->addConstrainedBy(fac_imu);
- _capture_origin->getFrame()->addConstrainedBy(fac_imu);
-
- return fac_imu;
-}
-
-void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
- const Scalar _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jac_delta_calib)
-{
- assert(_data.size() == data_size_ && "Wrong data size!");
-
- using namespace Eigen;
- Matrix<Scalar, 9, 6> jac_delta_data;
-
- /*
- * We have the following computing pipeline:
- * Input: data, calib, dt
- * Output: delta, delta_cov, jac_calib
- *
- * We do:
- * body = data - calib
- * delta = body2delta(body, dt) --> jac_body
- * jac_data = jac_body
- * jac_calib = - jac_body
- * delta_cov <-- propagate data_cov using jac_data
- *
- * where body2delta(.) produces a delta from body=(a,w) as follows:
- * dp = 1/2 * a * dt^2
- * dq = exp(w * dt)
- * dv = a * dt
- */
-
- // create delta
- imu::body2delta(_data - _calib, _dt, _delta, jac_delta_data); // Jacobians tested in imu_tools
-
- // compute delta_cov
- _delta_cov = jac_delta_data * _data_cov * jac_delta_data.transpose();
-
- // compute jacobian_calib
- _jac_delta_calib = - jac_delta_data;
-
-}
-
-void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _delta_preint_plus_delta)
-{
- /* MATHS according to Sola-16
- * Dp' = Dp + Dv*dt + 1/2*Dq*(a-a_b)*dt^2 = Dp + Dv*dt + Dq*dp if dp = 1/2*(a-a_b)*dt^2
- * Dv' = Dv + Dq*(a-a_b)*dt = Dv + Dq*dv if dv = (a-a_b)*dt
- * Dq' = Dq * exp((w-w_b)*dt) = Dq * dq if dq = exp((w-w_b)*dt)
- *
- * where (dp, dq, dv) need to be computed in data2delta(), and Dq*dx =is_equivalent_to= Dq*dx*Dq'.
- */
- _delta_preint_plus_delta = imu::compose(_delta_preint, _delta, _dt);
-}
-
-void ProcessorIMU::statePlusDelta(const Eigen::VectorXs& _x,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _x_plus_delta)
-{
- assert(_x.size() == 10 && "Wrong _x vector size");
- assert(_delta.size() == 10 && "Wrong _delta vector size");
- assert(_x_plus_delta.size() == 10 && "Wrong _x_plus_delta vector size");
- assert(_dt >= 0 && "Time interval _dt is negative!");
-
- _x_plus_delta = imu::composeOverState(_x, _delta, _dt);
-}
-
-void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
- const Eigen::VectorXs& _delta,
- const Scalar _dt,
- Eigen::VectorXs& _delta_preint_plus_delta,
- Eigen::MatrixXs& _jacobian_delta_preint,
- Eigen::MatrixXs& _jacobian_delta)
-{
- /*
- * Expression of the delta integration step, D' = D (+) d:
- *
- * Dp' = Dp + Dv*dt + Dq*dp
- * Dv' = Dv + Dq*dv
- * Dq' = Dq * dq
- *
- * Jacobians for covariance propagation.
- *
- * a. With respect to Delta, gives _jacobian_delta_preint = D'_D as:
- *
- * D'_D = [ I -DR*skew(dp) I*dt
- * 0 dR.tr 0
- * 0 -DR*skew(dv) I ] // See Sola-16
- *
- * b. With respect to delta, gives _jacobian_delta = D'_d as:
- *
- * D'_d = [ DR 0 0
- * 0 I 0
- * 0 0 DR ] // See Sola-16
- *
- * Note: covariance propagation, i.e., P+ = D_D * P * D_D' + D_d * M * D_d', is done in ProcessorMotion.
- */
- imu::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in imu_tools
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/processor/processor_factory.h"
-
-namespace wolf
-{
-WOLF_REGISTER_PROCESSOR("IMU", ProcessorIMU)
-}
diff --git a/src/processor/processor_tracker_landmark_dummy.cpp b/src/processor/processor_tracker_landmark_dummy.cpp
deleted file mode 100644
index f514cb7ef..000000000
--- a/src/processor/processor_tracker_landmark_dummy.cpp
+++ /dev/null
@@ -1,104 +0,0 @@
-/**
- * \file processor_tracker_landmark_dummy.cpp
- *
- * Created on: Apr 12, 2016
- * \author: jvallve
- */
-
-#include "base/processor/processor_tracker_landmark_dummy.h"
-#include "base/landmark/landmark_corner_2D.h"
-#include "base/factor/factor_corner_2D.h"
-
-namespace wolf
-{
-
-ProcessorTrackerLandmarkDummy::ProcessorTrackerLandmarkDummy(ProcessorParamsTrackerLandmarkPtr _params_tracker_landmark) :
- ProcessorTrackerLandmark("TRACKER LANDMARK DUMMY", _params_tracker_landmark),
- n_feature_(0),
- landmark_idx_non_visible_(0)
-{
- //
-
-}
-
-ProcessorTrackerLandmarkDummy::~ProcessorTrackerLandmarkDummy()
-{
- //
-}
-
-unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
- FeatureBasePtrList& _features_incoming_out,
- LandmarkMatchMap& _feature_landmark_correspondences)
-{
- std::cout << "\tProcessorTrackerLandmarkDummy::findLandmarks" << std::endl;
- std::cout << "\t\t" << _landmarks_in.size() << " landmarks..." << std::endl;
-
- // loosing the track of the first 2 features
- auto landmarks_lost = 0;
- for (auto landmark_in_ptr : _landmarks_in)
- {
- if (landmark_in_ptr->getDescriptor(0) <= landmark_idx_non_visible_)
- {
- landmarks_lost++;
- std::cout << "\t\tlandmark " << landmark_in_ptr->getDescriptor() << " lost!" << std::endl;
- }
- else
- {
- _features_incoming_out.push_back(std::make_shared<FeatureBase>(
- "POINT IMAGE",
- landmark_in_ptr->getDescriptor(),
- Eigen::MatrixXs::Identity(1,1)));
- _feature_landmark_correspondences[_features_incoming_out.back()] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1);
- std::cout << "\t\tlandmark " << landmark_in_ptr->getDescriptor() << " found!" << std::endl;
- }
- }
- return _features_incoming_out.size();
-}
-
-bool ProcessorTrackerLandmarkDummy::voteForKeyFrame()
-{
- std::cout << "N features: " << incoming_ptr_->getFeatureList().size() << std::endl;
- bool vote = incoming_ptr_->getFeatureList().size() < 4;
- std::cout << (vote ? "Vote ": "Not vote ") << "for KF" << std::endl;
- return incoming_ptr_->getFeatureList().size() < 4;
-}
-
-unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
-{
- std::cout << "\tProcessorTrackerLandmarkDummy::detectNewFeatures" << std::endl;
-
- unsigned int max_features = _max_features;
-
- if (max_features == -1)
- {
- max_features = 10;
- WOLF_INFO("max_features unlimited, setting it to " , max_features);
- }
-
- // detecting new features
- for (unsigned int i = 1; i <= max_features; i++)
- {
- n_feature_++;
- _features_incoming_out.push_back(std::make_shared<FeatureBase>("POINT IMAGE",
- n_feature_ * Eigen::Vector1s::Ones(),
- Eigen::MatrixXs::Ones(1, 1)));
- std::cout << "\t\tfeature " << _features_incoming_out.back()->getMeasurement() << " detected!" << std::endl;
- }
- return _features_incoming_out.size();
-}
-
-LandmarkBasePtr ProcessorTrackerLandmarkDummy::createLandmark(FeatureBasePtr _feature_ptr)
-{
- //std::cout << "ProcessorTrackerLandmarkDummy::createLandmark" << std::endl;
- return std::make_shared<LandmarkCorner2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), _feature_ptr->getMeasurement(0));
-}
-
-FactorBasePtr ProcessorTrackerLandmarkDummy::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
-{
- std::cout << "\tProcessorTrackerLandmarkDummy::createFactor" << std::endl;
- std::cout << "\t\tfeature " << _feature_ptr->getMeasurement() << std::endl;
- std::cout << "\t\tlandmark "<< _landmark_ptr->getDescriptor() << std::endl;
- return std::make_shared<FactorCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_landmark_ptr), shared_from_this());
-}
-
-} //namespace wolf
diff --git a/src/sensor/CMakeLists.txt b/src/sensor/CMakeLists.txt
deleted file mode 100644
index 661731d27..000000000
--- a/src/sensor/CMakeLists.txt
+++ /dev/null
@@ -1,27 +0,0 @@
-INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
-
-#=========================================
-#=========================================
-# Add in this section the CONDITIONAL CLUES [IF/ELSE]
-# for external libraries and move inside them the respective lines from above.
-
-
-
-#=========================================
-#=========================================
-
-SET(HDRS_SENSOR ${HDRS_SENSOR}
- ${CMAKE_CURRENT_SOURCE_DIR}/diff_drive_tools.h
- ${CMAKE_CURRENT_SOURCE_DIR}/diff_drive_tools.hpp
- ${CMAKE_CURRENT_SOURCE_DIR}/sensor_diff_drive.h
- )
-
-SET(SRCS_SENSOR ${SRCS_SENSOR}
- ${CMAKE_CURRENT_SOURCE_DIR}/sensor_diff_drive.cpp
- )
-
-# Forward var to parent scope
-# These lines always at the end
-SET(HDRS_SENSOR ${HDRS_SENSOR} PARENT_SCOPE)
-SET(SRCS_SENSOR ${SRCS_SENSOR} PARENT_SCOPE)
-
\ No newline at end of file
diff --git a/src/sensor/sensor_GPS.cpp b/src/sensor/sensor_GPS.cpp
deleted file mode 100644
index f6c1eace8..000000000
--- a/src/sensor/sensor_GPS.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-
-#include "base/sensor/sensor_GPS.h"
-#include "base/state_block/state_block.h"
-#include "base/state_block/state_quaternion.h"
-
-namespace wolf {
-
-SensorGPS::SensorGPS(StateBlockPtr _p_ptr, //GPS sensor position with respect to the car frame (base frame)
- StateBlockPtr _o_ptr, //GPS sensor orientation with respect to the car frame (base frame)
- StateBlockPtr _bias_ptr, //GPS sensor time bias
- StateBlockPtr _map_p_ptr, //initial position of vehicle's frame with respect to starting point frame
- StateBlockPtr _map_o_ptr) //initial orientation of vehicle's frame with respect to the starting point frame
- :
- SensorBase("GPS", _p_ptr, _o_ptr, _bias_ptr, 0)
-{
- getStateBlockVec().resize(5);
- setStateBlockPtrStatic(3, _map_p_ptr); // Map position
- setStateBlockPtrStatic(4, _map_o_ptr); // Map orientation
- //
-}
-
-SensorGPS::~SensorGPS()
-{
- //
-}
-
-StateBlockPtr SensorGPS::getMapP() const
-{
- return getStateBlockPtrStatic(3);
-}
-
-StateBlockPtr SensorGPS::getMapO() const
-{
- return getStateBlockPtrStatic(4);
-}
-
-// Define the factory method
-SensorBasePtr SensorGPS::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p,
- const IntrinsicsBasePtr _intrinsics)
-{
- // decode extrinsics vector
- assert(_extrinsics_p.size() == 3 && "Bad extrinsics vector length. Should be 3 for 3D.");
- StateBlockPtr pos_ptr = std::make_shared<StateBlock>(_extrinsics_p, true);
- StateBlockPtr ori_ptr = nullptr;
- SensorBasePtr sen = std::make_shared<SensorGPS>(pos_ptr, ori_ptr, nullptr, nullptr, nullptr);
- sen->setName(_unique_name);
- return sen;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("GPS",SensorGPS)
-} // namespace wolf
diff --git a/src/sensor/sensor_GPS_fix.cpp b/src/sensor/sensor_GPS_fix.cpp
deleted file mode 100644
index 3c781095f..000000000
--- a/src/sensor/sensor_GPS_fix.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-#include "base/sensor/sensor_GPS_fix.h"
-#include "base/state_block/state_block.h"
-#include "base/state_block/state_quaternion.h"
-
-namespace wolf {
-
-SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, const IntrinsicsGPSFix& _intrinsics) :
- SensorBase("GPS FIX", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), nullptr, _intrinsics.noise_std)
-{
- assert((_extrinsics.size() == 2 || _extrinsics.size() == 3)
- && "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D.");
-}
-
-SensorGPSFix::SensorGPSFix(const Eigen::VectorXs & _extrinsics, IntrinsicsGPSFixPtr _intrinsics_ptr) :
- SensorGPSFix(_extrinsics, *_intrinsics_ptr)
-{
- //
-}
-
-SensorGPSFix::~SensorGPSFix()
-{
- //
-}
-
-// Define the factory method
-SensorBasePtr SensorGPSFix::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics,
- const IntrinsicsBasePtr _intrinsics)
-{
- assert((_extrinsics.size() == 2 || _extrinsics.size() == 3)
- && "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D.");
- SensorGPSFixPtr sen = std::make_shared<SensorGPSFix>(_extrinsics, std::static_pointer_cast<IntrinsicsGPSFix>(_intrinsics));
- sen->getP()->fix();
- sen->setName(_unique_name);
- return sen;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("GPS FIX", SensorGPSFix)
-} // namespace wolf
diff --git a/src/sensor/sensor_IMU.cpp b/src/sensor/sensor_IMU.cpp
deleted file mode 100644
index d5b684163..000000000
--- a/src/sensor/sensor_IMU.cpp
+++ /dev/null
@@ -1,49 +0,0 @@
-#include "base/sensor/sensor_IMU.h"
-#include "base/state_block/state_block.h"
-#include "base/state_block/state_quaternion.h"
-
-namespace wolf {
-
-SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, IntrinsicsIMUPtr _params) :
- SensorIMU(_extrinsics, *_params)
-{
- //
-}
-
-SensorIMU::SensorIMU(const Eigen::VectorXs& _extrinsics, const IntrinsicsIMU& _params) :
- SensorBase("IMU", std::make_shared<StateBlock>(_extrinsics.head(3), true), std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), std::make_shared<StateBlock>(6, false, nullptr), (Eigen::Vector6s()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(), false, true),
- a_noise(_params.a_noise),
- w_noise(_params.w_noise),
- ab_initial_stdev(_params.ab_initial_stdev),
- wb_initial_stdev(_params.wb_initial_stdev),
- ab_rate_stdev(_params.ab_rate_stdev),
- wb_rate_stdev(_params.wb_rate_stdev)
-{
- assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2D.");
-}
-
-SensorIMU::~SensorIMU()
-{
- //
-}
-
-// Define the factory method
-SensorBasePtr SensorIMU::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq,
- const IntrinsicsBasePtr _intrinsics)
-{
- // decode extrinsics vector
- assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
-
- IntrinsicsIMUPtr params = std::static_pointer_cast<IntrinsicsIMU>(_intrinsics);
- SensorIMUPtr sen = std::make_shared<SensorIMU>(_extrinsics_pq, params);
- sen->setName(_unique_name);
- return sen;
-}
-
-} // namespace wolf
-
-// Register in the SensorFactory
-#include "base/sensor/sensor_factory.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR("IMU", SensorIMU)
-} // namespace wolf
diff --git a/src/yaml/processor_IMU_yaml.cpp b/src/yaml/processor_IMU_yaml.cpp
deleted file mode 100644
index 5b3f12ac6..000000000
--- a/src/yaml/processor_IMU_yaml.cpp
+++ /dev/null
@@ -1,51 +0,0 @@
-/**
- * \file processor_imu_yaml.cpp
- *
- * Created on: jan 19, 2017
- * \author: Dinesh Atchuthan
- */
-
-// wolf yaml
-#include "base/processor/processor_IMU.h"
-#include "base/yaml/yaml_conversion.h"
-
-// wolf
-#include "base/common/factory.h"
-
-// yaml-cpp library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-
-namespace
-{
-static ProcessorParamsBasePtr createProcessorIMUParams(const std::string & _filename_dot_yaml)
-{
- YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
-
- if (config["processor type"].as<std::string>() == "IMU")
- {
- YAML::Node kf_vote = config["keyframe vote"];
-
- ProcessorParamsIMUPtr params = std::make_shared<ProcessorParamsIMU>();
-
- params->max_time_span = kf_vote["max time span"] .as<Scalar>();
- params->max_buff_length = kf_vote["max buffer length"] .as<SizeEigen >();
- params->dist_traveled = kf_vote["dist traveled"] .as<Scalar>();
- params->angle_turned = kf_vote["angle turned"] .as<Scalar>();
- params->voting_active = kf_vote["voting_active"] .as<bool>();
-
- return params;
- }
-
- std::cout << "Bad configuration file. No processor type found." << std::endl;
- return nullptr;
-}
-
-// Register in the SensorFactory
-const bool WOLF_UNUSED registered_prc_odom_3D = ProcessorParamsFactory::get().registerCreator("IMU", createProcessorIMUParams);
-
-} // namespace [unnamed]
-
-} // namespace wolf
diff --git a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
deleted file mode 100644
index 0af63b582..000000000
--- a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
+++ /dev/null
@@ -1,75 +0,0 @@
-/*
- * processor_tracker_feature_trifocal_yaml.cpp
- *
- * Created on: Apr 12, 2018
- * Author: asantamaria
- */
-
-// wolf yaml
-#include "base/processor/processor_tracker_feature_trifocal.h"
-#include "base/yaml/yaml_conversion.h"
-
-// wolf
-#include "base/common/factory.h"
-
-// yaml-cpp library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-
-namespace
-{
-static ProcessorParamsBasePtr createProcessorParamsTrackerFeatureTrifocal(const std::string & _filename_dot_yaml)
-{
- YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
-
- if (config.IsNull())
- {
- WOLF_ERROR("Invalid YAML file!");
- return nullptr;
- }
- else if (config["processor type"].as<std::string>() == "TRACKER FEATURE TRIFOCAL")
- {
- ProcessorParamsTrackerFeatureTrifocalPtr params = std::make_shared<ProcessorParamsTrackerFeatureTrifocal>();
-
- YAML::Node vision_utils = config ["vision_utils"];
- params->yaml_file_params_vision_utils = vision_utils["YAML file params"].as<std::string>();
-
- // relative to global path for Vision Utils YAML
- assert(params->yaml_file_params_vision_utils.at(0) != ('/') && "The parameter YAML FILE PARAMS (in processor params YAML file) must be specified with a path relative to the processor YAML file.");
- unsigned first = _filename_dot_yaml.find("/");
- unsigned last = _filename_dot_yaml.find_last_of("/");
- std::string strNew = _filename_dot_yaml.substr (first,last-first);
- params->yaml_file_params_vision_utils = _filename_dot_yaml.substr (first,last-first) + "/" + params->yaml_file_params_vision_utils;
-
- YAML::Node algorithm = config ["algorithm"];
- params->time_tolerance = algorithm["time tolerance"] .as<Scalar>();
- params->voting_active = algorithm["voting active"] .as<bool>();
- params->min_features_for_keyframe = algorithm["minimum features for keyframe"] .as<unsigned int>();
- params->max_new_features = algorithm["maximum new features"] .as<unsigned int>();
- params->n_cells_h = algorithm["grid horiz cells"] .as<int>();
- params->n_cells_v = algorithm["grid vert cells"] .as<int>();
- params->min_response_new_feature = algorithm["min response new features"] .as<int>();
- params->max_euclidean_distance = algorithm["max euclidean distance"] .as<Scalar>();
- params->min_track_length_for_factor = algorithm["min track length for factor"].as<int>();
-
- YAML::Node noise = config["noise"];
- params->pixel_noise_std = noise ["pixel noise std"].as<Scalar>();
-
- return params;
- }
- else
- {
- WOLF_ERROR("Wrong processor type! Should be \"TRACKER FEATURE TRIFOCAL\"");
- return nullptr;
- }
- return nullptr;
-}
-
-// Register in the SensorFactory
-const bool WOLF_UNUSED registered_prc_trifocal = ProcessorParamsFactory::get().registerCreator("TRACKER FEATURE TRIFOCAL", createProcessorParamsTrackerFeatureTrifocal);
-
-} // namespace [unnamed]
-
-} // namespace wolf
diff --git a/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp b/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp
deleted file mode 100644
index 4cd2d67e8..000000000
--- a/src/yaml/processor_tracker_landmark_apriltag_yaml.cpp
+++ /dev/null
@@ -1,94 +0,0 @@
-/**
- * \file processor_tracker_landmark_apriltag_yaml.cpp
- *
- * Created on: Dec 6, 2018
- * \author: jsola
- */
-
-
-// wolf
-#include "base/processor/processor_tracker_landmark_apriltag.h"
-#include "base/yaml/yaml_conversion.h"
-#include "base/factory.h"
-
-// yaml-cpp library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-
-namespace
-{
-static ProcessorParamsBasePtr createProcessorParamsLandmarkApriltag(const std::string & _filename_dot_yaml)
-{
- YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
-
- if (config.IsNull())
- {
- WOLF_ERROR("Invalid YAML file!");
- return nullptr;
- }
- else if (config["processor type"].as<std::string>() == "TRACKER LANDMARK APRILTAG")
- {
- ProcessorParamsTrackerLandmarkApriltagPtr params = std::make_shared<ProcessorParamsTrackerLandmarkApriltag>();
-
- YAML::Node detector_parameters = config["detector parameters"];
- params->quad_decimate_ = detector_parameters["quad_decimate"] .as<Scalar>();
- params->quad_sigma_ = detector_parameters["quad_sigma"] .as<Scalar>();
- params->nthreads_ = detector_parameters["nthreads"] .as<int>();
- params->debug_ = detector_parameters["debug"] .as<bool>();
- params->refine_edges_ = detector_parameters["refine_edges"] .as<bool>();
- params->refine_decode_ = detector_parameters["refine_decode"] .as<bool>();
- params->refine_pose_ = detector_parameters["refine_pose"] .as<bool>();
- params->ippe_min_ratio_ = detector_parameters["ippe_min_ratio"] .as<Scalar>();
- params->ippe_max_rep_error_ = detector_parameters["ippe_max_rep_error"] .as<Scalar>();
-
- YAML::Node tag_parameters = config["tag parameters"];
- params->tag_family_ = tag_parameters["tag_family"] .as<std::string>();
- params->tag_black_border_ = tag_parameters["tag_black_border"] .as<int>();
- params->tag_width_default_ = tag_parameters["tag_width_default"] .as<Scalar>();
-
- // read list of tag widths
- YAML::Node tag_widths = config["tag widths"];
- for (auto pair_id_width : tag_widths)
- {
- int tag_id = pair_id_width.first .as<int>();
- Scalar tag_width = pair_id_width.second .as<Scalar>();
- params->tag_widths_.emplace(tag_id, tag_width);
- }
-
- YAML::Node noise = config["noise"];
- params->std_xy_ = noise["std_xy"] .as<Scalar>();
- params->std_z_ = noise["std_z"] .as<Scalar>();
- params->std_rpy_ = M_TORAD * noise["std_rpy_degrees"] .as<Scalar>();
- params->std_pix_ = noise["std_pix"] .as<Scalar>();
-
- YAML::Node vote = config["vote"];
- params->voting_active = vote["voting active"] .as<bool>();
- params->min_time_vote_ = vote["min_time_vote"] .as<Scalar>();
- params->max_time_vote_ = vote["max_time_vote"] .as<Scalar>();
- params->min_features_for_keyframe = vote["min_features_for_keyframe"] .as<unsigned int>();
- params->max_features_diff_ = vote["max_features_diff"] .as<int>();
- params->nb_vote_for_every_first_ = vote["nb_vote_for_every_first"] .as<int>();
- params->enough_info_necessary_ = vote["enough_info_necessary"] .as<bool>();
-
- params->reestimate_last_frame_ = config["reestimate_last_frame"] .as<bool>();
- params->add_3D_cstr_ = config["add_3D_cstr"] .as<bool>();
-
- return params;
- }
- else
- {
- WOLF_ERROR("Wrong processor type! Should be \"TRACKER LANDMARK APRILTAG\"");
- return nullptr;
- }
- return nullptr;
-}
-
-// Register in the SensorFactory
-const bool WOLF_UNUSED registered_prc_apriltag = ProcessorParamsFactory::get().registerCreator("TRACKER LANDMARK APRILTAG", createProcessorParamsLandmarkApriltag);
-const bool WOLF_UNUSED registered_prc_apriltag_wrapper = ProcessorParamsFactory::get().registerCreator("TRACKER LANDMARK APRILTAG WRAPPER", createProcessorParamsLandmarkApriltag);
-
-} // namespace [unnamed]
-
-} // namespace wolf
diff --git a/src/yaml/sensor_IMU_yaml.cpp b/src/yaml/sensor_IMU_yaml.cpp
deleted file mode 100644
index 79cf183cd..000000000
--- a/src/yaml/sensor_IMU_yaml.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-/**
- * \file sensor_imu_yaml.cpp
- *
- * Created on: Jan 18, 2017
- * \author: Dinesh Atchuthan
- */
-
-// wolf yaml
-#include "base/sensor/sensor_IMU.h"
-#include "base/yaml/yaml_conversion.h"
-
-// wolf
-#include "base/common/factory.h"
-
-// yaml-cpp library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-
-namespace
-{
-
-static IntrinsicsBasePtr createIntrinsicsIMU(const std::string & _filename_dot_yaml)
-{
- YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
-
- if (config["sensor type"].as<std::string>() == "IMU")
- {
- YAML::Node variances = config["motion variances"];
- YAML::Node kf_vote = config["keyframe vote"];
-
- IntrinsicsIMUPtr params = std::make_shared<IntrinsicsIMU>();
-
- params->a_noise = variances["a_noise"] .as<Scalar>();
- params->w_noise = variances["w_noise"] .as<Scalar>();
- params->ab_initial_stdev = variances["ab_initial_stdev"] .as<Scalar>();
- params->wb_initial_stdev = variances["wb_initial_stdev"] .as<Scalar>();
- params->ab_rate_stdev = variances["ab_rate_stdev"] .as<Scalar>();
- params->wb_rate_stdev = variances["wb_rate_stdev"] .as<Scalar>();
-
- return params;
- }
-
- std::cout << "Bad configuration file. No sensor type found." << std::endl;
- return nullptr;
-}
-
-// Register in the SensorFactory
-const bool WOLF_UNUSED registered_imu_intr = IntrinsicsFactory::get().registerCreator("IMU", createIntrinsicsIMU);
-
-} // namespace [unnamed]
-
-} // namespace wolf
diff --git a/src/yaml/sensor_camera_yaml.cpp b/src/yaml/sensor_camera_yaml.cpp
deleted file mode 100644
index ccfb3d203..000000000
--- a/src/yaml/sensor_camera_yaml.cpp
+++ /dev/null
@@ -1,132 +0,0 @@
-/**
- * \file sensor_camera_yaml.cpp
- *
- * Created on: May 13, 2016
- * \author: jsola
- */
-
-// wolf yaml
-#include "base/yaml/yaml_conversion.h"
-
-// wolf
-#include "base/sensor/sensor_camera.h"
-#include "base/common/factory.h"
-
-// yaml-cpp library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-
-namespace
-{
-static IntrinsicsBasePtr createIntrinsicsCamera(const std::string & _filename_dot_yaml)
-{
- YAML::Node camera_config = YAML::LoadFile(_filename_dot_yaml);
-
- // if (camera_config["sensor type"].as<std::string>() == "CAMERA") // this does not work: camera YAML files are ROS-styled
- if (camera_config["camera_matrix"]) // check that at least this field exists to validate YAML file of the correct type
- {
-
- // YAML:: to Eigen::
- using namespace Eigen;
- unsigned int width = camera_config["image_width"] .as<unsigned int>();
- unsigned int height = camera_config["image_height"] .as<unsigned int>();
-//<<<<<<< HEAD
-// VectorXd intrinsic = camera_config["camera_matrix"]["data"] .as<VectorXd>();
-// VectorXd projection = camera_config["projection_matrix"]["data"] .as<VectorXd>();
-//=======
-//>>>>>>> devel
- VectorXd distortion = camera_config["distortion_coefficients"]["data"] .as<VectorXd>();
- VectorXd intrinsic = camera_config["camera_matrix"]["data"] .as<VectorXd>();
- VectorXd projection = camera_config["projection_matrix"]["data"] .as<VectorXd>();
-
- // Eigen:: to wolf::
- std::shared_ptr<IntrinsicsCamera> intrinsics_cam = std::make_shared<IntrinsicsCamera>();
-//<<<<<<< HEAD
-// intrinsics_cam->type = sensor_type;
-// intrinsics_cam->name = sensor_name;
-//// intrinsics_cam->pinhole_model[0] = intrinsic[2];
-//// intrinsics_cam->pinhole_model[1] = intrinsic[5];
-//// intrinsics_cam->pinhole_model[2] = intrinsic[0];
-//// intrinsics_cam->pinhole_model[3] = intrinsic[4];
-// intrinsics_cam->pinhole_model[0] = projection[2]; // u0
-// intrinsics_cam->pinhole_model[1] = projection[6]; // v0
-// intrinsics_cam->pinhole_model[2] = projection[0]; // au
-// intrinsics_cam->pinhole_model[3] = projection[5]; // av
-//=======
-
- intrinsics_cam->width = width;
- intrinsics_cam->height = height;
-
- intrinsics_cam->pinhole_model_raw[0] = intrinsic[2];
- intrinsics_cam->pinhole_model_raw[1] = intrinsic[5];
- intrinsics_cam->pinhole_model_raw[2] = intrinsic[0];
- intrinsics_cam->pinhole_model_raw[3] = intrinsic[4];
-
- intrinsics_cam->pinhole_model_rectified[0] = projection[2];
- intrinsics_cam->pinhole_model_rectified[1] = projection[6];
- intrinsics_cam->pinhole_model_rectified[2] = projection[0];
- intrinsics_cam->pinhole_model_rectified[3] = projection[5];
-
-//>>>>>>> devel
- assert (distortion.size() == 5 && "Distortion size must be size 5!");
-
- WOLF_WARN_COND( distortion(2) != 0 || distortion(3) != 0 , "Wolf does not handle tangential distortion. Please consider re-calibrating without tangential distortion!");
-
- if (distortion(4) == 0)
- if (distortion(1) == 0)
- if (distortion(0) == 0)
- intrinsics_cam->distortion.resize(0);
- else
- {
- intrinsics_cam->distortion.resize(1);
- intrinsics_cam->distortion = distortion.head<1>();
- }
- else
- {
- intrinsics_cam->distortion.resize(2);
- intrinsics_cam->distortion = distortion.head<2>();
- }
- else
- {
- intrinsics_cam->distortion.resize(3);
- intrinsics_cam->distortion.head<2>() = distortion.head<2>();
- intrinsics_cam->distortion.tail<1>() = distortion.tail<1>();
- }
-
- //=========================================
- // ===== this part for debugging only =====
- //=========================================
-// std::cout << "\n--- Parameters Parsed from YAML file ---" << std::endl;
-// std::cout << "sensor type: " << sensor_type << std::endl;
-// std::cout << "sensor name: " << sensor_name << std::endl;
-
-// // extrinsics discarded in this creator
-// Vector3d pos = camera_config["extrinsic"]["position"].as<Vector3d>();
-// Vector3d ori = camera_config["extrinsic"]["orientation"].as<Vector3d>() * M_PI / 180; // roll, pitch, yaw [rad]
-// Quaternions quat; v2q(ori, quat);
-// std::cout << "sensor extrinsics: " << std::endl;
-// std::cout << "\tposition : " << pos.transpose() << std::endl;
-// std::cout << "\torientation : " << ori.transpose() << std::endl;
-
-// std::cout << "sensor intrinsics: " << std::endl;
-// std::cout << "\timage size : " << width << "x" << height << std::endl;
-// std::cout << "\tintrinsic : " << intrinsic.transpose() << std::endl;
-// std::cout << "\tdistoriton : " << distortion.transpose() << std::endl;
- //=========================================
-
- return intrinsics_cam;
- }
-
- std::cout << "Bad configuration file. No or bad sensor type found." << std::endl;
- return nullptr;
-}
-
-// Register in the SensorFactory
-const bool WOLF_UNUSED registered_camera_intr = IntrinsicsFactory::get().registerCreator("CAMERA", createIntrinsicsCamera);
-
-} // namespace [unnamed]
-
-} // namespace wolf
-
diff --git a/src/yaml/sensor_laser_2D_yaml.cpp b/src/yaml/sensor_laser_2D_yaml.cpp
deleted file mode 100644
index bd553d20d..000000000
--- a/src/yaml/sensor_laser_2D_yaml.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-/**
- * \file sensor_laser_2D_yaml.cpp
- *
- * Created on: May 13, 2016
- * \author: jsola
- */
-
-// wolf yaml
-#include "base/yaml/yaml_conversion.h"
-
-// wolf
-//#include "base/intrinsics_factory.h"
-#include "base/common/factory.h"
-#include "base/sensor/sensor_laser_2D.h"
-
-// yaml library
-#include <yaml-cpp/yaml.h>
-
-namespace wolf
-{
-namespace {
-// intrinsics creator
-IntrinsicsBasePtr createIntrinsicsLaser2D(const std::string& _filename_dot_yaml)
-{
- // If required: Parse YAML
-
- IntrinsicsLaser2DPtr params; // dummy
- return params;
-}
-
-// register into factory
-const bool WOLF_UNUSED registered_laser_params = IntrinsicsFactory::get().registerCreator("LASER 2D", createIntrinsicsLaser2D);
-
-} // namespace [void]
-} // namespace wolf
diff --git a/test/data/IMU/imu_static_biasNull.txt~ b/test/data/IMU/imu_static_biasNull.txt~
deleted file mode 100644
index ddc5fdeab..000000000
--- a/test/data/IMU/imu_static_biasNull.txt~
+++ /dev/null
@@ -1,1003 +0,0 @@
-0.0000000000000000 0.0000000000000000 0.0000000000000000 1.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0000000000000000 0.0000000000000000 0.0000000000000435 1.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000000000 0.0000000000001137
-0.0000000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0010000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0020000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0030000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0040000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0050000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0060000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0070000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0080000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0090000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0100000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0110000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0120000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0130000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0140000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0150000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0160000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0170000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0180000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0190000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0200000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0210000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0220000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0230000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0240000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0250000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0260000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0270000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0280000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0290000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0300000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0310000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0320000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0330000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0340000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0350000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0360000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0370000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0380000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0390000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0400000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0410000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0420000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0430000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0440000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0450000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0460000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0470000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0480000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0490000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0500000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0510000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0520000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0530000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0540000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0550000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0560000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0570000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0580000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0590000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0600000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0610000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0620000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0630000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0640000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0650000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0660000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0670000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0680000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0690000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0700000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0710000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0720000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0730000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0740000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0750000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0760000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0770000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0780000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0790000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0800000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0810000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0820000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0830000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0840000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0850000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0860000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0870000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0880000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0890000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0900000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0910000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0920000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0930000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0940000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0950000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0960000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0970000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0980000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.0990000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1000000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1010000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1020000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1030000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1040000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1050000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1060000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1070000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1080000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1090000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1100000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1110000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1120000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1130000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1140000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1150000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1160000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1170000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1180000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1190000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1200000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1210000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1220000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1230000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1240000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1250000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1260000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1270000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1280000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1290000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1300000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1310000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1320000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1330000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1340000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1350000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1360000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1370000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1380000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1390000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1400000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1410000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1420000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1430000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1440000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1450000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1460000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1470000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1480000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1490000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1500000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1510000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1520000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1530000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1540000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1550000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1560000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1570000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1580000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1590000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1600000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1610000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1620000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1630000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1640000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1650000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1660000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1670000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1680000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1690000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1700000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1710000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1720000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1730000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1740000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1750000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1760000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1770000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1780000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1790000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1800000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1810000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1820000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1830000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1840000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1850000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1860000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1870000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1880000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1890000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1900000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1910000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1920000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1930000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1940000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1950000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1960000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1970000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1980000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.1990000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2000000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2010000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2020000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2030000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2040000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2050000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2060000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2070000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2080000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2090000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2100000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2110000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2120000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2130000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2140000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2150000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2160000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2170000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2180000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2190000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2200000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2210000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2220000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2230000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2240000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2250000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2260000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2270000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2280000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2290000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2300000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2310000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2320000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2330000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2340000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2350000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2360000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2370000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2380000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2390000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2400000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2410000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2420000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2430000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2440000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2450000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2460000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2470000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2480000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2490000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2500000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2510000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2520000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2530000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2540000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2550000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2560000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2570000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2580000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2590000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2600000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2610000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2620000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2630000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2640000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2650000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2660000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2670000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2680000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2690000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2700000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2710000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2720000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2730000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2740000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2750000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2760000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2770000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2780000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2790000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2800000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2810000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2820000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2830000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2840000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2850000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2860000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2870000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2880000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2890000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2900000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2910000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2920000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2930000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2940000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2950000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2960000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2970000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2980000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.2990000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3000000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3010000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3020000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3030000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3040000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3050000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3060000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3070000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3080000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3090000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3100000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3110000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3120000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3130000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3140000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3150000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3160000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3170000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3180000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3190000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3200000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3210000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3220000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3230000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3240000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3250000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3260000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3270000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3280000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3290000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3300000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3310000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3320000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3330000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3340000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3350000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3360000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3370000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3380000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3390000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3400000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3410000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3420000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3430000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3440000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3450000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3460000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3470000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3480000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3490000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3500000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3510000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3520000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3530000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3540000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3550000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3560000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3570000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3580000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3590000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3600000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3610000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3620000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3630000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3640000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3650000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3660000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3670000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3680000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3690000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3700000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3710000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3720000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3730000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3740000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3750000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3760000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3770000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3780000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3790000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3800000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3810000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3820000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3830000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3840000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3850000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3860000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3870000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3880000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3890000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3900000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3910000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3920000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3930000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3940000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3950000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3960000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3970000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3980000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.3990000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4000000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4010000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4020000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4030000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4040000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4050000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4060000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4070000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4080000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4090000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4100000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4110000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4120000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4130000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4140000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4150000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4160000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4170000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4180000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4190000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4200000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4210000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4220000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4230000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4240000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4250000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4260000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4270000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4280000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4290000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4300000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4310000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4320000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4330000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4340000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4350000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4360000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4370000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4380000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4390000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4400000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4410000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4420000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4430000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4440000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4450000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4460000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4470000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4480000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4490000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4500000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4510000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4520000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4530000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4540000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4550000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4560000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4570000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4580000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4590000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4600000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4610000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4620000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4630000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4640000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4650000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4660000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4670000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4680000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4690000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4700000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4710000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4720000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4730000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4740000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4750000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4760000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4770000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4780000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4790000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4800000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4810000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4820000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4830000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4840000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4850000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4860000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4870000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4880000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4890000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4900000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4910000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4920000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4930000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4940000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4950000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4960000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4970000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4980000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.4990000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5000000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5010000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5020000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5030000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5040000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5050000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5060000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5070000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5080000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5090000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5100000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5110000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5120000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5130000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5140000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5150000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5160000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5170000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5180000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5190000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5200000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5209999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5220000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5229999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5240000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5249999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5260000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5269999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5280000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5289999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5300000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5309999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5320000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5329999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5340000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5349999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5360000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5369999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5380000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5389999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5400000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5409999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5420000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5429999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5440000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5449999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5460000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5469999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5480000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5489999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5500000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5509999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5520000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5529999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5540000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5549999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5560000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5569999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5580000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5589999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5600000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5609999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5620000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5629999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5640000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5649999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5660000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5669999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5680000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5690000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5700000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5710000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5720000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5730000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5740000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5750000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5760000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5770000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5780000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5790000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5800000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5810000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5820000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5830000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5840000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5850000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5860000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5870000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5880000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5890000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5900000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5910000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5920000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5930000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5940000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5950000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5960000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5970000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5980000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.5990000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6000000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6010000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6020000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6030000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6040000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6050000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6060000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6070000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6080000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6090000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6100000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6110000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6120000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6130000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6140000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6150000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6160000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6170000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6180000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6190000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6200000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6210000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6220000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6230000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6240000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6250000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6260000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6270000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6280000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6290000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6300000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6310000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6320000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6330000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6340000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6350000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6360000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6370000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6380000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6390000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6400000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6410000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6420000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6430000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6440000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6450000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6460000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6470000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6480000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6490000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6499999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6510000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6519999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6530000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6539999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6550000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6559999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6570000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6579999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6590000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6599999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6610000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6619999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6630000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6639999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6650000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6659999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6670000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6679999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6690000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6699999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6710000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6719999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6730000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6739999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6750000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6759999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6770000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6779999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6790000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6799999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6810000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6819999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6830000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6839999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6850000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6859999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6870000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6879999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6890000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6899999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6910000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6919999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6930000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6940000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6950000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6960000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6970000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6980000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.6990000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7000000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7010000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7020000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7030000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7040000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7050000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7060000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7070000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7080000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7090000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7100000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7110000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7120000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7130000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7140000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7150000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7160000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7170000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7180000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7190000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7200000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7210000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7220000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7230000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7240000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7250000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7260000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7270000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7280000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7290000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7300000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7310000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7320000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7330000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7340000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7350000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7360000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7370000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7380000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7390000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7400000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7410000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7420000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7430000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7440000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7450000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7460000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7470000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7480000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7490000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7500000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7510000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7520000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7530000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7540000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7550000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7560000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7570000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7580000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7590000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7600000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7610000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7620000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7630000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7640000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7650000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7660000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7670000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7680000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7690000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7700000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7710000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7720000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7730000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7740000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7750000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7760000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7770000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7780000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7790000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7800000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7810000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7820000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7830000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7840000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7850000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7860000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7870000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7880000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7890000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7900000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7910000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7920000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7929999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7940000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7949999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7960000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7969999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7980000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.7989999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8000000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8009999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8020000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8029999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8040000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8049999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8060000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8069999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8080000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8089999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8100000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8109999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8120000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8129999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8140000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8149999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8160000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8169999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8180000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8190000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8200000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8210000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8220000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8230000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8240000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8250000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8260000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8270000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8280000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8290000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8300000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8310000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8320000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8330000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8340000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8350000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8360000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8370000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8380000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8390000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8400000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8410000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8420000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8430000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8440000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8450000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8460000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8470000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8480000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8490000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8500000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8510000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8520000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8530000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8540000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8550000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8560000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8570000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8580000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8590000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8600000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8610000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8620000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8630000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8640000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8650000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8660000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8670000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8680000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8690000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8700000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8710000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8720000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8730000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8740000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8750000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8760000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8770000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8780000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8790000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8800000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8810000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8820000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8830000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8840000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8850000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8860000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8870000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8880000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8890000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8900000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8910000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8920000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8930000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8940000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8950000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8960000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8970000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8980000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.8990000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9000000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9010000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9020000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9030000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9040000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9050000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9060000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9070000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9080000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9090000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9100000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9110000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9120000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9130000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9140000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9150000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9160000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9170000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9180000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9190000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9200000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9210000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9220000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9230000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9240000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9250000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9260000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9270000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9279999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9290000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9299999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9310000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9319999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9330000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9339999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9350000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9359999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9370000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9379999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9390000000000001 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9399999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9409999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9419999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9429999999999999 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9440000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9450000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9460000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9470000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9480000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9490000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9500000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9510000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9520000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9530000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9540000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9550000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9560000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9570000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9580000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9590000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9600000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9610000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9620000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9630000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9640000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9650000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9660000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9670000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9680000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9690000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9700000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9710000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9720000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9730000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9740000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9750000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9760000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9770000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9780000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9790000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9800000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9810000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9820000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9830000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9840000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9850000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9860000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9870000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9880000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9890000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9900000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9910000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9920000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9930000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9940000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9950000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9960000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9970000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9980000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-0.9990000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
-1.0000000000000000 0.0000000000000000 0.0000000000000000 9.8000000000000007 0.0000000000000000 0.0000000000000000 0.0000000000000000
diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp
deleted file mode 100644
index d2657e415..000000000
--- a/test/gtest_IMU.cpp
+++ /dev/null
@@ -1,1531 +0,0 @@
-/*
- * gtest_IMU.cpp
- *
- * Created on: Nov 14, 2017
- * Author: jsola
- */
-
-//Wolf
-#include "base/processor/processor_IMU.h"
-#include "base/sensor/sensor_IMU.h"
-#include "base/common/wolf.h"
-#include "base/sensor/sensor_odom_3D.h"
-#include "base/processor/processor_odom_3D.h"
-#include "base/ceres_wrapper/ceres_manager.h"
-
-#include "utils_gtest.h"
-#include "base/utils/logging.h"
-
-// make my life easier
-using namespace Eigen;
-using namespace wolf;
-using std::shared_ptr;
-using std::make_shared;
-using std::static_pointer_cast;
-using std::string;
-
-class Process_Factor_IMU : public testing::Test
-{
- public:
- // Wolf objects
- ProblemPtr problem;
- CeresManagerPtr ceres_manager;
- SensorIMUPtr sensor_imu;
- ProcessorIMUPtr processor_imu;
- CaptureIMUPtr capture_imu;
- FrameBasePtr KF_0, KF_1; // keyframes
- CaptureBasePtr C_0 , C_1; // base captures
- CaptureMotionPtr CM_0, CM_1; // motion captures
-
- // time
- TimeStamp t0, t;
- Scalar dt, DT;
- int num_integrations;
-
- // initial state
- VectorXs x0; // initial state
- Vector3s p0, v0; // initial pos and vel
- Quaternions q0, q; // initial and current orientations
- Matrix<Scalar,9,9> P0; // initial state covariance
-
- // bias
- Vector6s bias_real, bias_preint, bias_null; // real, pre-int and zero biases.
- Vector6s bias_0, bias_1; // optimized bias at KF's 0 and 1
-
- // input
- Matrix<Scalar, 6, Dynamic> motion; // Motion in IMU frame. Each column is a motion step. If just one column, then the number of steps is defined in num_integrations
- Matrix<Scalar, 3, Dynamic> a, w; // True acc and angvel in IMU frame. Each column is a motion step. Used to create motion with `motion << a,w;`
- Vector6s data; // IMU data. It's the motion with gravity and bias. See imu::motion2data().
-
- // Deltas and states (exact, integrated, corrected, solved, etc)
- VectorXs D_exact, x1_exact; // exact or ground truth
- VectorXs D_preint_imu, x1_preint_imu; // preintegrated with imu_tools
- VectorXs D_corrected_imu, x1_corrected_imu; // corrected with imu_tools
- VectorXs D_preint, x1_preint; // preintegrated with processor_imu
- VectorXs D_corrected, x1_corrected; // corrected with processor_imu
- VectorXs D_optim, x1_optim; // optimized using factor_imu
- VectorXs D_optim_imu, x1_optim_imu; // corrected with imu_tools using optimized bias
- VectorXs x0_optim; // optimized using factor_imu
-
- // Trajectory buffer of correction Jacobians
- std::vector<MatrixXs> Buf_Jac_preint_prc;
-
- // Trajectory matrices
- MatrixXs Trj_D_exact, Trj_D_preint_imu, Trj_D_preint_prc, Trj_D_corrected_imu, Trj_D_corrected_prc;
- MatrixXs Trj_x_exact, Trj_x_preint_imu, Trj_x_preint_prc, Trj_x_corrected_imu, Trj_x_corrected_prc;
-
- // Delta correction Jacobian and step
- Matrix<Scalar,9,6> J_D_bias; // Jacobian of pre-integrated delta w
- Vector9s step;
-
- // Flags for fixing/unfixing state blocks
- bool p0_fixed, q0_fixed, v0_fixed;
- bool p1_fixed, q1_fixed, v1_fixed;
-
- virtual void SetUp( )
- {
- string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================== SETTING PROBLEM
- problem = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_manager = make_shared<CeresManager>(problem, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sensor = problem->installSensor ("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- ProcessorBasePtr processor = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_no_vote.yaml");
- sensor_imu = static_pointer_cast<SensorIMU> (sensor);
- processor_imu = static_pointer_cast<ProcessorIMU>(processor);
-
- dt = 0.01;
- processor_imu->setTimeTolerance(dt/2);
-
- // Some initializations
- bias_null .setZero();
- x0 .resize(10);
- D_preint .resize(10);
- D_corrected .resize(10);
- x1_optim .resize(10);
- x1_optim_imu.resize(10);
-
- }
-
-
- /* Integrate one step of acc and angVel motion, obtain Delta_preintegrated
- * Input:
- * q: current orientation
- * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
- * bias_real: the real bias of the IMU
- * bias_preint: the bias used for Delta pre-integration
- * Input/output
- * Delta: the preintegrated delta
- * J_D_b_ptr: a pointer to the Jacobian of Delta wrt bias. Defaults to nullptr.
- */
- static void integrateOneStep(const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Quaternions& q_real, VectorXs& Delta, Matrix<Scalar, 9, 6>* J_D_b_ptr = nullptr)
- {
- VectorXs delta(10), data(6);
- VectorXs Delta_plus(10);
- Matrix<Scalar, 9, 6> J_d_d, J_D_b, J_d_b;
- Matrix<Scalar, 9, 9> J_D_D, J_D_d;
-
- data = imu::motion2data(motion, q_real, bias_real);
- q_real = q_real*exp_q(motion.tail(3)*dt);
- Vector6s body = data - bias_preint;
- if (J_D_b_ptr == nullptr)
- {
- delta = imu::body2delta(body, dt);
- Delta_plus = imu::compose(Delta, delta, dt);
- }
- else
- {
- imu::body2delta(body, dt, delta, J_d_d);
- imu::compose(Delta, delta, dt, Delta_plus, J_D_D, J_D_d);
- J_D_b = *J_D_b_ptr;
- J_d_b = - J_d_d;
- J_D_b = J_D_D*J_D_b + J_D_d*J_d_b;
- *J_D_b_ptr = J_D_b;
- }
- Delta = Delta_plus;
- }
-
- /* Integrate acc and angVel motion, obtain Delta_preintegrated
- * Input:
- * N: number of steps
- * q0: initial orientation
- * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
- * bias_real: the real bias of the IMU
- * bias_preint: the bias used for Delta pre-integration
- * Output:
- * return: the preintegrated delta
- */
- static VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt)
- {
- VectorXs Delta(10);
- Delta = imu::identity();
- Quaternions q(q0);
- for (int n = 0; n < N; n++)
- {
- integrateOneStep(motion, bias_real, bias_preint, dt, q, Delta);
- }
- return Delta;
- }
-
- /* Integrate acc and angVel motion, obtain Delta_preintegrated
- * Input:
- * N: number of steps
- * q0: initial orientation
- * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body frame
- * bias_real: the real bias of the IMU
- * bias_preint: the bias used for Delta pre-integration
- * Output:
- * J_D_b: the Jacobian of the preintegrated delta wrt the bias
- * return: the preintegrated delta
- */
- static VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b)
- {
- VectorXs Delta(10);
- Quaternions q;
-
- Delta = imu::identity();
- J_D_b .setZero();
- q = q0;
- for (int n = 0; n < N; n++)
- {
- integrateOneStep(motion, bias_real, bias_preint, dt, q, Delta, &J_D_b);
- }
- return Delta;
- }
-
- /* Integrate acc and angVel motion, obtain Delta_preintegrated
- * Input:
- * q0: initial orientation
- * motion: Matrix with N columns [ax, ay, az, wx, wy, wz] with the true magnitudes in body frame
- * bias_real: the real bias of the IMU
- * bias_preint: the bias used for Delta pre-integration
- * Output:
- * J_D_b: the Jacobian of the preintegrated delta wrt the bias
- * return: the preintegrated delta
- */
- static VectorXs integrateDelta(const Quaternions& q0, const MatrixXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b)
- {
- VectorXs Delta(10);
- Quaternions q;
-
- Delta = imu::identity();
- J_D_b .setZero();
- q = q0;
- for (int n = 0; n < motion.cols(); n++)
- {
- integrateOneStep(motion.col(n), bias_real, bias_preint, dt, q, Delta, &J_D_b);
- }
- return Delta;
- }
-
- /* Integrate acc and angVel motion, obtain Delta_preintegrated
- * Input:
- * q0: initial orientation
- * motion: Matrix with N columns [ax, ay, az, wx, wy, wz] with the true magnitudes in body frame
- * bias_real: the real bias of the IMU
- * bias_preint: the bias used for Delta pre-integration
- * Output:
- * J_D_b: the Jacobian of the preintegrated delta wrt the bias
- * return: the preintegrated delta
- */
- static MotionBuffer integrateDeltaTrajectory(const Quaternions& q0, const MatrixXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b)
- {
- MotionBuffer trajectory(6, 10, 9, 6);
- VectorXs Delta(10);
- MatrixXs M9(9,9), M6(6,6), J9(9,9), J96(9,6), V10(10,1), V6(6,1);
- Quaternions q;
-
- Delta = imu::identity();
- J_D_b .setZero();
- q = q0;
- TimeStamp t(0);
- trajectory.get().emplace_back(t, Vector6s::Zero(), M6, VectorXs::Zero(10), M9, imu::identity(), M9, J9, J9, MatrixXs::Zero(9,6));
- for (int n = 0; n < motion.cols(); n++)
- {
- t += dt;
- integrateOneStep(motion.col(n), bias_real, bias_preint, dt, q, Delta, &J_D_b);
- trajectory.get().emplace_back(t, motion.col(n), M6, V10, M9, Delta, M9, J9, J9, J_D_b);
- }
- return trajectory;
- }
-
- MotionBuffer integrateWithProcessor(int N, const TimeStamp& t0, const Quaternions q0, const MatrixXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, VectorXs& D_preint, VectorXs& D_corrected)
- {
- Vector6s motion_now;
- data = imu::motion2data(motion.col(0), q0, bias_real);
- capture_imu = make_shared<CaptureIMU>(t0, sensor_imu, data, sensor_imu->getNoiseCov());
- q = q0;
- t = t0;
- for (int i= 0; i < N; i++)
- {
- t += dt;
- motion_now = motion.cols() == 1
- ? motion
- : motion.col(i);
- data = imu::motion2data(motion_now, q, bias_real);
- w = motion_now.tail<3>();
- q = q * exp_q(w*dt);
-
- capture_imu->setTimeStamp(t);
- capture_imu->setData(data);
-
- sensor_imu->process(capture_imu);
-
- D_preint = processor_imu->getLast()->getDeltaPreint();
- D_corrected = processor_imu->getLast()->getDeltaCorrected(bias_real);
- }
- return processor_imu->getBuffer();
- }
-
- // Initial configuration of variables
- bool configureAll()
- {
- // initial state
- q0 .normalize();
- x0 << p0, q0.coeffs(), v0;
- P0 .setIdentity() * 0.01;
-
- // motion
- if (motion.cols() == 0)
- {
- motion.resize(6,a.cols());
- motion << a, w;
- }
- else
- {
- // if motion has any column at all, then it is already initialized in TEST_F(...) and we do nothing.
- }
- if (motion.cols() != 1)
- {
- // if motion has more than 1 col, make num_integrations consistent with nbr of cols, just for consistency
- num_integrations = motion.cols();
- }
-
- // total run time
- DT = num_integrations * dt;
-
- // wolf objects
- KF_0 = problem->setPrior(x0, P0, t0, dt/2);
- C_0 = processor_imu->getOrigin();
-
- processor_imu->getLast()->setCalibrationPreint(bias_preint);
-
- return true;
- }
-
- // Integrate using all methods
- virtual void integrateAll()
- {
- // ===================================== INTEGRATE EXACTLY WITH IMU_TOOLS with no bias at all
- if (motion.cols() == 1)
- D_exact = integrateDelta(num_integrations, q0, motion, bias_null, bias_null, dt);
- else
- D_exact = integrateDelta(q0, motion, bias_null, bias_null, dt, J_D_bias);
- x1_exact = imu::composeOverState(x0, D_exact, DT );
-
- // ===================================== INTEGRATE USING IMU_TOOLS
- // pre-integrate
- if (motion.cols() == 1)
- D_preint_imu = integrateDelta(num_integrations, q0, motion, bias_real, bias_preint, dt, J_D_bias);
- else
- D_preint_imu = integrateDelta(q0, motion, bias_real, bias_preint, dt, J_D_bias);
-
- // correct perturbated
- step = J_D_bias * (bias_real - bias_preint);
- D_corrected_imu = imu::plus(D_preint_imu, step);
-
- // compose states
- x1_preint_imu = imu::composeOverState(x0, D_preint_imu , DT );
- x1_corrected_imu = imu::composeOverState(x0, D_corrected_imu , DT );
-
- // ===================================== INTEGRATE USING PROCESSOR_IMU
-
- integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected);
-
- // compose states
- x1_preint = imu::composeOverState(x0, D_preint , DT );
- x1_corrected = imu::composeOverState(x0, D_corrected , DT );
- }
-
- // Integrate Trajectories all methods
- virtual void integrateAllTrajectories()
- {
- SizeEigen cols = motion.cols() + 1;
- Trj_D_exact.resize(10,cols); Trj_D_preint_imu.resize(10,cols); Trj_D_preint_prc.resize(10,cols); Trj_D_corrected_imu.resize(10,cols); Trj_D_corrected_prc.resize(10,cols);
- Trj_x_exact.resize(10,cols); Trj_x_preint_imu.resize(10,cols); Trj_x_preint_prc.resize(10,cols); Trj_x_corrected_imu.resize(10,cols); Trj_x_corrected_prc.resize(10,cols);
-
- // ===================================== INTEGRATE EXACTLY WITH IMU_TOOLS with no bias at all
- MotionBuffer Buf_exact = integrateDeltaTrajectory(q0, motion, bias_null, bias_null, dt, J_D_bias);
-
- // Build exact trajectories
- int col = 0;
- Scalar Dt = 0;
- for (auto m : Buf_exact.get())
- {
- Trj_D_exact.col(col) = m.delta_integr_;
- Trj_x_exact.col(col) = imu::composeOverState(x0, m.delta_integr_, Dt );
- Dt += dt;
- col ++;
- }
-
- // set
- D_exact = Trj_D_exact.col(cols-1);
- x1_exact = Trj_x_exact.col(cols-1);
-
- // ===================================== INTEGRATE USING IMU_TOOLS
- // pre-integrate
- MotionBuffer Buf_preint_imu = integrateDeltaTrajectory(q0, motion, bias_real, bias_preint, dt, J_D_bias);
-
- // Build trajectories preintegrated and corrected with imu_tools
- col = 0;
- Dt = 0;
- for (auto m : Buf_preint_imu.get())
- {
- // preint
- Trj_D_preint_imu.col(col) = m.delta_integr_;
- Trj_x_preint_imu.col(col) = imu::composeOverState(x0, Trj_D_preint_imu.col(col).eval(), Dt );
-
- // corrected
- VectorXs step = m.jacobian_calib_ * (bias_real - bias_preint);
- Trj_D_corrected_imu.col(col) = imu::plus(m.delta_integr_, step) ;
- Trj_x_corrected_imu.col(col) = imu::composeOverState(x0, Trj_D_corrected_imu.col(col).eval(), Dt );
- Dt += dt;
- col ++;
- }
-
- D_preint_imu = Trj_D_preint_imu.col(cols-1);
-
- // correct perturbated
- step = J_D_bias * (bias_real - bias_preint);
- D_corrected_imu = imu::plus(D_preint_imu, step);
-
- // compose states
- x1_preint_imu = imu::composeOverState(x0, D_preint_imu , DT );
- x1_corrected_imu = imu::composeOverState(x0, D_corrected_imu , DT );
-
- // ===================================== INTEGRATE USING PROCESSOR_IMU
-
- MotionBuffer Buf_D_preint_prc = integrateWithProcessor(num_integrations, t0, q0, motion, bias_real, bias_preint, dt, D_preint, D_corrected);
-
- // Build trajectories preintegrated and corrected with processorIMU
- Dt = 0;
- col = 0;
- Buf_Jac_preint_prc.clear();
- for (auto m : Buf_D_preint_prc.get())
- {
- // preint
- Trj_D_preint_prc.col(col) = m.delta_integr_;
- Trj_x_preint_prc.col(col) = imu::composeOverState(x0, Trj_D_preint_prc.col(col).eval(), Dt );
-
- // corrected
- VectorXs step = m.jacobian_calib_ * (bias_real - bias_preint);
- Trj_D_corrected_prc.col(col) = imu::plus(m.delta_integr_, step) ;
- Trj_x_corrected_prc.col(col) = imu::composeOverState(x0, Trj_D_corrected_prc.col(col).eval(), Dt );
-
- Buf_Jac_preint_prc.push_back(m.jacobian_calib_);
- Dt += dt;
- col ++;
- }
-
- // compose states
- x1_preint = imu::composeOverState(x0, D_preint , DT );
- x1_corrected = imu::composeOverState(x0, D_corrected , DT );
- }
-
- // Set state_blocks status
- void setFixedBlocks()
- {
- // this sets each state block status fixed / unfixed
- KF_0->getP()->setFixed(p0_fixed);
- KF_0->getO()->setFixed(q0_fixed);
- KF_0->getV()->setFixed(v0_fixed);
- KF_1->getP()->setFixed(p1_fixed);
- KF_1->getO()->setFixed(q1_fixed);
- KF_1->getV()->setFixed(v1_fixed);
- }
-
- void setKfStates()
- {
- // This perturbs states to estimate around the exact value, then assigns to the keyframe
- // Perturbations are applied only if the state block is unfixed
-
- VectorXs x_pert(10);
-
- // KF 0
- x_pert = x0;
- if (!p0_fixed)
- x_pert.head(3) += Vector3s::Random() * 0.01;
- if (!q0_fixed)
- x_pert.segment(3,4) = (Quaternions(x_pert.data() + 3) * exp_q(Vector3s::Random() * 0.01)).coeffs().normalized();
- if (!v0_fixed)
- x_pert.tail(3) += Vector3s::Random() * 0.01;
- KF_0->setState(x_pert);
-
- // KF 1
- x_pert = x1_exact;
- if (!p1_fixed)
- x_pert.head(3) += Vector3s::Random() * 0.01;
- if (!q1_fixed)
- x_pert.segment(3,4) = (Quaternions(x_pert.data() + 3) * exp_q(Vector3s::Random() * 0.01)).coeffs().normalized();
- if (!v1_fixed)
- x_pert.tail(3) += Vector3s::Random() * 0.01;
- KF_1->setState(x_pert);
- }
-
- virtual void buildProblem()
- {
- // ===================================== SET KF in Wolf tree
- FrameBasePtr KF = problem->emplaceFrame(KEY_FRAME, x1_exact, t);
-
- // ===================================== IMU CALLBACK
- problem->keyFrameCallback(KF, nullptr, dt/2);
-
- // Process IMU for the callback to take effect
- data = Vector6s::Zero();
- capture_imu = make_shared<CaptureIMU>(t+dt, sensor_imu, data, sensor_imu->getNoiseCov());
- processor_imu->process(capture_imu);
-
- KF_1 = problem->getLastKeyFrame();
- C_1 = KF_1->getCaptureList().front(); // front is IMU
- CM_1 = static_pointer_cast<CaptureMotion>(C_1);
-
- // ===================================== SET BOUNDARY CONDITIONS
- setFixedBlocks();
- setKfStates();
- }
-
- string solveProblem(SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::BRIEF)
- {
- string report = ceres_manager->solve(verbose);
-
- bias_0 = C_0->getCalibration();
- bias_1 = C_1->getCalibration();
-
- // ===================================== GET INTEGRATED STATE WITH SOLVED BIAS
- // with processor
- x0_optim = KF_0->getState();
- D_optim = CM_1->getDeltaCorrected(bias_0);
- x1_optim = KF_1->getState();
-
- // with imu_tools
- step = J_D_bias * (bias_0 - bias_preint);
- D_optim_imu = imu::plus(D_preint, step);
- x1_optim_imu = imu::composeOverState(x0_optim, D_optim_imu, DT);
-
- return report;
- }
-
- string runAll(SolverManager::ReportVerbosity verbose)
- {
- configureAll();
- integrateAll();
- buildProblem();
- string report = solveProblem(verbose);
- return report;
- }
-
- void printAll(std::string report = "")
- {
- WOLF_TRACE(report);
-
- WOLF_TRACE("D_exact : ", D_exact .transpose() ); // exact delta integrated, with absolutely no bias
- WOLF_TRACE("D_preint : ", D_preint .transpose() ); // pre-integrated delta using processor
- WOLF_TRACE("D_preint_imu : ", D_preint_imu .transpose() ); // pre-integrated delta using imu_tools
- WOLF_TRACE("D_correct : ", D_corrected .transpose() ); // corrected delta using processor
- WOLF_TRACE("D_correct_imu : ", D_corrected_imu .transpose() ); // corrected delta using imu_tools
- WOLF_TRACE("D_optim : ", D_optim .transpose() ); // corrected delta using optimum bias after solving using processor
- WOLF_TRACE("D_optim_imu : ", D_optim_imu .transpose() ); // corrected delta using optimum bias after solving using imu_tools
-
- WOLF_TRACE("bias real : ", bias_real .transpose() ); // real bias
- WOLF_TRACE("bias preint : ", bias_preint .transpose() ); // bias used during pre-integration
- WOLF_TRACE("bias optim 0 : ", bias_0 .transpose() ); // solved bias at KF_0
- WOLF_TRACE("bias optim 1 : ", bias_1 .transpose() ); // solved bias at KF_1
-
- // states at the end of integration, i.e., at KF_1
- WOLF_TRACE("X1_exact : ", x1_exact .transpose() ); // exact state
- WOLF_TRACE("X1_preint : ", x1_preint .transpose() ); // state using delta preintegrated by processor
- WOLF_TRACE("X1_preint_imu : ", x1_preint_imu .transpose() ); // state using delta preintegrated by imu_tools
- WOLF_TRACE("X1_correct : ", x1_corrected .transpose() ); // corrected state vy processor
- WOLF_TRACE("X1_correct_imu: ", x1_corrected_imu .transpose() ); // corrected state by imu_tools
- WOLF_TRACE("X1_optim : ", x1_optim .transpose() ); // optimal state after solving using processor
- WOLF_TRACE("X1_optim_imu : ", x1_optim_imu .transpose() ); // optimal state after solving using imu_tools
- WOLF_TRACE("err1_optim : ", (x1_optim-x1_exact).transpose() ); // error of optimal state corrected by imu_tools w.r.t. exact state
- WOLF_TRACE("err1_optim_imu: ", (x1_optim_imu-x1_exact).transpose() ); // error of optimal state corrected by imu_tools w.r.t. exact state
- WOLF_TRACE("X0_optim : ", x0_optim .transpose() ); // optimal state after solving using processor
- }
-
- virtual void assertAll()
- {
- // check delta and state integrals
- ASSERT_MATRIX_APPROX(D_preint , D_preint_imu , 1e-8 );
- ASSERT_MATRIX_APPROX(D_corrected , D_corrected_imu , 1e-8 );
- ASSERT_MATRIX_APPROX(D_corrected_imu , D_exact , 1e-5 );
- ASSERT_MATRIX_APPROX(D_corrected , D_exact , 1e-5 );
- ASSERT_MATRIX_APPROX(x1_corrected_imu , x1_exact , 1e-5 );
- ASSERT_MATRIX_APPROX(x1_corrected , x1_exact , 1e-5 );
-
- // check optimal solutions
- ASSERT_MATRIX_APPROX(x0_optim , x0 , 1e-5 );
- ASSERT_NEAR(x0_optim.segment(3,4).norm(), 1.0 , 1e-8 );
- ASSERT_MATRIX_APPROX(bias_0 , bias_real , 1e-4 );
- ASSERT_MATRIX_APPROX(bias_1 , bias_real , 1e-4 );
- ASSERT_MATRIX_APPROX(D_optim , D_exact , 1e-5 );
- ASSERT_MATRIX_APPROX(x1_optim , x1_exact , 1e-5 );
- ASSERT_MATRIX_APPROX(D_optim_imu , D_exact , 1e-5 );
- ASSERT_MATRIX_APPROX(x1_optim_imu , x1_exact , 1e-5 );
- ASSERT_NEAR(x1_optim.segment(3,4).norm(), 1.0 , 1e-8 );
- }
-
-};
-
-class Process_Factor_IMU_ODO : public Process_Factor_IMU
-{
- public:
- // Wolf objects
- SensorOdom3DPtr sensor_odo;
- ProcessorOdom3DPtr processor_odo;
- CaptureOdom3DPtr capture_odo;
-
- virtual void SetUp( ) override
- {
-
- // ===================================== IMU
- Process_Factor_IMU::SetUp();
-
- // ===================================== ODO
- string wolf_root = _WOLF_ROOT_DIR;
-
- SensorBasePtr sensor = problem->installSensor ("ODOM 3D", "Odometer", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D.yaml" );
- ProcessorBasePtr processor = problem->installProcessor("ODOM 3D", "Odometer", "Odometer" , wolf_root + "/src/examples/processor_odom_3D.yaml");
- sensor_odo = static_pointer_cast<SensorOdom3D>(sensor);
-
- processor_odo = static_pointer_cast<ProcessorOdom3D>(processor);
-
- processor_odo->setTimeTolerance(dt/2);
-
- // prevent this processor from voting by setting high thresholds :
- processor_odo->setAngleTurned(999);
- processor_odo->setDistTraveled(999);
- processor_odo->setMaxBuffLength(999);
- processor_odo->setMaxTimeSpan(999);
- }
-
- virtual void integrateAll() override
- {
- // ===================================== IMU
- Process_Factor_IMU::integrateAll();
-
- // ===================================== ODO
- Vector6s data;
- Vector3s p1 = x1_exact.head(3);
- Quaternions q1 (x1_exact.data() + 3);
- Vector3s dp = q0.conjugate() * (p1 - p0);
- Vector3s dth = log_q( q0.conjugate() * q1 );
- data << dp, dth;
-
- CaptureOdom3DPtr capture_odo = make_shared<CaptureOdom3D>(t, sensor_odo, data, sensor_odo->getNoiseCov());
-
- sensor_odo->process(capture_odo);
- }
-
- virtual void integrateAllTrajectories() override
- {
- // ===================================== IMU
- Process_Factor_IMU::integrateAllTrajectories();
-
- // ===================================== ODO
- Vector6s data;
- Vector3s p1 = x1_exact.head(3);
- Quaternions q1 (x1_exact.data() + 3);
- Vector3s dp = q0.conjugate() * (p1 - p0);
- Vector3s dth = log_q( q0.conjugate() * q1 );
- data << dp, dth;
-
- CaptureOdom3DPtr capture_odo = make_shared<CaptureOdom3D>(t, sensor_odo, data, sensor_odo->getNoiseCov());
-
- sensor_odo->process(capture_odo);
- }
-
- virtual void buildProblem() override
- {
- // ===================================== IMU
- Process_Factor_IMU::buildProblem();
-
- // ===================================== ODO
- // Process ODO for the callback to take effect
- data = Vector6s::Zero();
- capture_odo = make_shared<CaptureOdom3D>(t+dt, sensor_odo, data, sensor_odo->getNoiseCov());
- processor_odo->process(capture_odo);
-
- }
-
-};
-
-TEST_F(Process_Factor_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
-// printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU, test_capture) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- Eigen::Vector6s initial_bias((Eigen::Vector6s()<< .002, .0007, -.001, .003, -.002, .001).finished());
- sensor_imu->getIntrinsic()->setState(initial_bias);
- configureAll();
- integrateAll();
- buildProblem();
- //Since we did not solve, hence bias estimates did not change yet, both capture should have the same calibration
- ASSERT_MATRIX_APPROX(KF_0->getCaptureOf(sensor_imu)->getCalibration(), initial_bias, 1e-8 );
- ASSERT_MATRIX_APPROX(KF_0->getCaptureOf(sensor_imu)->getCalibration(), KF_1->getCaptureOf(sensor_imu)->getCalibration(), 1e-8 );
-}
-
-TEST_F(Process_Factor_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = false;
- q0_fixed = false;
- v0_fixed = false;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = false;
- q0_fixed = false;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = false;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Matrix<Scalar, 3, 50>::Random();
- w = Matrix<Scalar, 3, 50>::Random();
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
-// printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Matrix<Scalar, 3, 50>::Random();
- w = Matrix<Scalar, 3, 50>::Random();
-
- // ---------- fix boundaries
- p0_fixed = false;
- q0_fixed = false;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = false;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
-// printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Matrix<Scalar, 3, 50>::Random();
- w = Matrix<Scalar, 3, 50>::Random();
-
- // ---------- fix boundaries
- p0_fixed = false;
- q0_fixed = false;
- v0_fixed = true;
- p1_fixed = false;
- q1_fixed = true;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 3,2,1;
- q0.coeffs() << 0.5,0.5,0.5,.5;
- v0 << 1,2,3;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
-// printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 0,0,0 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 0,0,0 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = true;
- v1_fixed = false;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 0,0,0 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = true;
- q1_fixed = false;
- v1_fixed = false;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 0,0,0 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = false;
- q1_fixed = true;
- v1_fixed = false;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 0,0,0 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = false;
- q1_fixed = false;
- v1_fixed = false;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = false;
- q0_fixed = false;
- v0_fixed = false;
- p1_fixed = false;
- q1_fixed = false;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
-// printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Vector3s( 1,2,3 );
- w = Vector3s( 1,2,3 );
-
- // ---------- fix boundaries
- p0_fixed = false;
- q0_fixed = false;
- v0_fixed = true;
- p1_fixed = false;
- q1_fixed = false;
- v1_fixed = false;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
-// printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Matrix<Scalar, 3, 50>::Random();
- w = Matrix<Scalar, 3, 50>::Random();
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = true;
- v0_fixed = true;
- p1_fixed = false;
- q1_fixed = false;
- v1_fixed = false;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
- // printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 50;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Matrix<Scalar, 3, 50>::Random();
- w = Matrix<Scalar, 3, 50>::Random();
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = false;
- v0_fixed = true;
- p1_fixed = false;
- q1_fixed = false;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- string report = runAll(SolverManager::ReportVerbosity::BRIEF);
-
-// printAll(report);
-
- assertAll();
-
-}
-
-TEST_F(Process_Factor_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
-{
-
- // ================================================================================================================ //
- // ==================================== INITIAL CONDITIONS -- USER OPTIONS ======================================== //
- // ================================================================================================================ //
- //
- // ---------- time
- t0 = 0;
- num_integrations = 25;
-
- // ---------- initial pose
- p0 << 0,0,0;
- q0.coeffs() << 0,0,0,1;
- v0 << 0,0,0;
-
- // ---------- bias
- bias_real << .001, .002, .003, -.001, -.002, -.003;
- bias_preint = -bias_real;
-
- // ---------- motion params
- a = Matrix<Scalar, 3, 25>::Ones() + 0.1 * Matrix<Scalar, 3, 25>::Random();
- w = Matrix<Scalar, 3, 25>::Ones() + 0.1 * Matrix<Scalar, 3, 25>::Random();
-
- // ---------- fix boundaries
- p0_fixed = true;
- q0_fixed = false;
- v0_fixed = true;
- p1_fixed = false;
- q1_fixed = false;
- v1_fixed = true;
- //
- // ===================================== INITIAL CONDITIONS -- USER INPUT ENDS HERE =============================== //
- // ================================================================================================================ //
-
- // ===================================== RUN ALL
- configureAll();
- integrateAllTrajectories();
- buildProblem();
- string report = solveProblem(SolverManager::ReportVerbosity::BRIEF);
-
- assertAll();
-
- // Build optimal trajectory
- MatrixXs Trj_x_optim(10,Trj_D_preint_prc.cols());
- Scalar Dt = 0;
- SizeEigen i = 0;
- for (auto J_D_b : Buf_Jac_preint_prc)
- {
- VectorXs step = J_D_b * (bias_0 - bias_preint);
- VectorXs D_opt = imu::plus(Trj_D_preint_prc.col(i).eval(), step);
- Trj_x_optim.col(i) = imu::composeOverState(x0_optim, D_opt, Dt);
- Dt += dt;
- i ++;
- }
-
- // Get optimal trajectory via getState()
- i = 0;
- t = 0;
- MatrixXs Trj_x_optim_prc(10,Trj_D_preint_prc.cols());
- for (int i = 0; i < Trj_x_optim_prc.cols(); i++)
- {
- Trj_x_optim_prc.col(i) = problem->getState(t);
- t += dt;
- }
-
- // printAll(report);
-
-// WOLF_INFO("------------------------------------");
-// WOLF_INFO("Exact x0 \n", x0 .transpose());
-// WOLF_INFO("Optim x0 \n", x0_optim .transpose());
-// WOLF_INFO("Optim x \n", Trj_x_optim_prc.transpose());
-// WOLF_INFO("Optim x1 \n", x1_optim .transpose());
-// WOLF_INFO("Exact x1 \n", x1_exact .transpose());
-// WOLF_INFO("------------------------------------");
-
- // The Mother of Asserts !!!
- ASSERT_MATRIX_APPROX(Trj_x_optim, Trj_x_optim_prc, 1e-6);
-
- // First and last trj states match known extrema
- ASSERT_MATRIX_APPROX(Trj_x_optim_prc.leftCols (1), x0, 1e-6);
- ASSERT_MATRIX_APPROX(Trj_x_optim_prc.rightCols(1), x1_exact, 1e-6);
-
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU.*";
- // ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU_ODO.*";
- // ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";
-
- return RUN_ALL_TESTS();
-}
-
-/* Some notes :
- *
- * - Process_Factor_IMU_ODO.MotionConstant_PQv_b__PQv_b :
- * this test will not work + jacobian is rank deficient because of estimating both initial
- * and final velocities.
- * IMU data integration is done with correct biases (so this is the case of a calibrated IMU). Before solving the problem, we perturbate the initial bias.
- * We solve the problem by fixing all states excepted V1 and V2. while creating the factors, both velocities are corrected using the difference between the actual
- * bias and the bias used during preintegration. One way to solve this in the solver side would be to make the actual bias match the preintegraion bias so that the
- * difference is 0 and does not affect the states of the KF. Another possibility is to have both velocities modified without changing the biases. it is likely that this
- * solution is chosen in this case (bias changes is penalized between 2 KeyFrames, but velocities have no other factors here.)
- *
- * - Bias evaluation with a precision of 1e-4 :
- * The bias introduced in the data for the preintegration steps is different of the real bias. This is simulating the case of a non calibrated IMU
- * Because of cross relations between acc and gyro biases (respectively a_b and w_b) it is difficult to expect a better estimation.
- * A small change in a_b can be cancelled by a small variation in w_b. in other words : there are local minima.
- * In addition, for Process_Factor_IMU tests, P and V are tested against 1e-5 precision while 1e-8 is used for Q.
- * Errors tend to be distributed in different estimated variable when we get into a local minima (to minimize residuals in a least square sense).
- */
diff --git a/test/gtest_IMU_tools.cpp b/test/gtest_IMU_tools.cpp
deleted file mode 100644
index 3cc7d33e2..000000000
--- a/test/gtest_IMU_tools.cpp
+++ /dev/null
@@ -1,629 +0,0 @@
-/*
- * gtest_imu_tools.cpp
- *
- * Created on: Jul 29, 2017
- * Author: jsola
- */
-
-#include "base/math/IMU_tools.h"
-#include "utils_gtest.h"
-
-using namespace Eigen;
-using namespace wolf;
-using namespace imu;
-
-TEST(IMU_tools, identity)
-{
- VectorXs id_true;
- id_true.setZero(10);
- id_true(6) = 1.0;
-
- VectorXs id = identity<Scalar>();
- ASSERT_MATRIX_APPROX(id, id_true, 1e-10);
-}
-
-TEST(IMU_tools, identity_split)
-{
- VectorXs p(3), qv(4), v(3);
- Quaternions q;
-
- identity(p,q,v);
- ASSERT_MATRIX_APPROX(p, Vector3s::Zero(), 1e-10);
- ASSERT_QUATERNION_APPROX(q, Quaternions::Identity(), 1e-10);
- ASSERT_MATRIX_APPROX(v, Vector3s::Zero(), 1e-10);
-
- identity(p,qv,v);
- ASSERT_MATRIX_APPROX(p , Vector3s::Zero(), 1e-10);
- ASSERT_MATRIX_APPROX(qv, (Vector4s()<<0,0,0,1).finished(), 1e-10);
- ASSERT_MATRIX_APPROX(v , Vector3s::Zero(), 1e-10);
-}
-
-TEST(IMU_tools, inverse)
-{
- VectorXs d(10), id(10), iid(10), iiid(10), I(10);
- Vector4s qv;
- Scalar dt = 0.1;
-
- qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
- d << 0, 1, 2, qv, 7, 8, 9;
-
- id = inverse(d, dt);
-
- compose(id, d, dt, I);
- ASSERT_MATRIX_APPROX(I, identity(), 1e-10);
- compose(d, id, -dt, I); // Observe -dt is reversed !!
- ASSERT_MATRIX_APPROX(I, identity(), 1e-10);
-
- inverse(id, -dt, iid); // Observe -dt is reversed !!
- ASSERT_MATRIX_APPROX( d, iid, 1e-10);
- iiid = inverse(iid, dt);
- ASSERT_MATRIX_APPROX(id, iiid, 1e-10);
-}
-
-TEST(IMU_tools, compose_between)
-{
- VectorXs dx1(10), dx2(10), dx3(10);
- Matrix<Scalar, 10, 1> d1, d2, d3;
- Vector4s qv;
- Scalar dt = 0.1;
-
- qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
- dx1 << 0, 1, 2, qv, 7, 8, 9;
- d1 = dx1;
- qv = (Vector4s() << 6, 5, 4, 3).finished().normalized();
- dx2 << 9, 8, 7, qv, 2, 1, 0;
- d2 = dx2;
-
- // combinations of templates for sum()
- compose(dx1, dx2, dt, dx3);
- compose(d1, d2, dt, d3);
- ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
-
- compose(d1, dx2, dt, dx3);
- d3 = compose(dx1, d2, dt);
- ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
-
- // minus(d1, d3) should recover delta_2
- VectorXs diffx(10);
- Matrix<Scalar,10,1> diff;
- between(d1, d3, dt, diff);
- ASSERT_MATRIX_APPROX(diff, d2, 1e-10);
-
- // minus(d3, d1, -dt) should recover inverse(d2, dt)
- diff = between(d3, d1, -dt);
- ASSERT_MATRIX_APPROX(diff, inverse(d2, dt), 1e-10);
-}
-
-TEST(IMU_tools, compose_between_with_state)
-{
- VectorXs x(10), d(10), x2(10), x3(10), d2(10), d3(10);
- Vector4s qv;
- Scalar dt = 0.1;
-
- qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
- x << 0, 1, 2, qv, 7, 8, 9;
- qv = (Vector4s() << 6, 5, 4, 3).finished().normalized();
- d << 9, 8, 7, qv, 2, 1, 0;
-
- composeOverState(x, d, dt, x2);
- x3 = composeOverState(x, d, dt);
- ASSERT_MATRIX_APPROX(x3, x2, 1e-10);
-
- // betweenStates(x, x2) should recover d
- betweenStates(x, x2, dt, d2);
- d3 = betweenStates(x, x2, dt);
- ASSERT_MATRIX_APPROX(d2, d, 1e-10);
- ASSERT_MATRIX_APPROX(d3, d, 1e-10);
- ASSERT_MATRIX_APPROX(betweenStates(x, x2, dt), d, 1e-10);
-
- // x + (x2 - x) = x2
- ASSERT_MATRIX_APPROX(composeOverState(x, betweenStates(x, x2, dt), dt), x2, 1e-10);
-
- // (x + d) - x = d
- ASSERT_MATRIX_APPROX(betweenStates(x, composeOverState(x, d, dt), dt), d, 1e-10);
-}
-
-TEST(IMU_tools, lift_retract)
-{
- VectorXs d_min(9); d_min << 0, 1, 2, .3, .4, .5, 6, 7, 8; // use angles in the ball theta < pi
-
- // transform to delta
- VectorXs delta = exp_IMU(d_min);
-
- // expected delta
- Vector3s dp = d_min.head(3);
- Quaternions dq = v2q(d_min.segment(3,3));
- Vector3s dv = d_min.tail(3);
- VectorXs delta_true(10); delta_true << dp, dq.coeffs(), dv;
- ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10);
-
- // transform to a new tangent -- should be the original tangent
- VectorXs d_from_delta = log_IMU(delta);
- ASSERT_MATRIX_APPROX(d_from_delta, d_min, 1e-10);
-
- // transform to a new delta -- should be the previous delta
- VectorXs delta_from_d = exp_IMU(d_from_delta);
- ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
-}
-
-TEST(IMU_tools, plus)
-{
- VectorXs d1(10), d2(10), d3(10);
- VectorXs err(9);
- Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
- d1 << 0, 1, 2, qv, 7, 8, 9;
- err << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09;
-
- d3.head(3) = d1.head(3) + err.head(3);
- d3.segment(3,4) = (Quaternions(qv.data()) * exp_q(err.segment(3,3))).coeffs();
- d3.tail(3) = d1.tail(3) + err.tail(3);
-
- plus(d1, err, d2);
- ASSERT_MATRIX_APPROX(diff(d3, d2), VectorXs::Zero(9), 1e-10);
-}
-
-TEST(IMU_tools, diff)
-{
- VectorXs d1(10), d2(10);
- Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
- d1 << 0, 1, 2, qv, 7, 8, 9;
- d2 = d1;
-
- VectorXs err(9);
- diff(d1, d2, err);
- ASSERT_MATRIX_APPROX(err, VectorXs::Zero(9), 1e-10);
- ASSERT_MATRIX_APPROX(diff(d1, d2), VectorXs::Zero(9), 1e-10);
-
- VectorXs d3(10);
- d3.setRandom(); d3.segment(3,4).normalize();
- err.head(3) = d3.head(3) - d1.head(3);
- err.segment(3,3) = log_q(Quaternions(d1.data()+3).conjugate()*Quaternions(d3.data()+3));
- err.tail(3) = d3.tail(3) - d1.tail(3);
-
- ASSERT_MATRIX_APPROX(err, diff(d1, d3), 1e-10);
-
-}
-
-TEST(IMU_tools, compose_jacobians)
-{
- VectorXs d1(10), d2(10), d3(10), d1_pert(10), d2_pert(10), d3_pert(10); // deltas
- VectorXs t1(9), t2(9); // tangents
- Matrix<Scalar, 9, 9> J1_a, J2_a, J1_n, J2_n;
- Vector4s qv1, qv2;
- Scalar dt = 0.1;
- Scalar dx = 1e-6;
- qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
- d1 << 0, 1, 2, qv1, 7, 8, 9;
- qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
- d2 << 9, 8, 7, qv2, 2, 1, 0;
-
- // analytical jacobians
- compose(d1, d2, dt, d3, J1_a, J2_a);
-
- // numerical jacobians
- for (unsigned int i = 0; i<9; i++)
- {
- t1 . setZero();
- t1(i) = dx;
-
- // Jac wrt first delta
- d1_pert = plus(d1, t1); // (d1 + t1)
- d3_pert = compose(d1_pert, d2, dt); // (d1 + t1) + d2
- t2 = diff(d3, d3_pert); // { (d2 + t1) + d2 } - { d1 + d2 }
- J1_n.col(i) = t2/dx; // [ { (d2 + t1) + d2 } - { d1 + d2 } ] / dx
-
- // Jac wrt second delta
- d2_pert = plus(d2, t1); // (d2 + t1)
- d3_pert = compose(d1, d2_pert, dt); // d1 + (d2 + t1)
- t2 = diff(d3, d3_pert); // { d1 + (d2 + t1) } - { d1 + d2 }
- J2_n.col(i) = t2/dx; // [ { d1 + (d2 + t1) } - { d1 + d2 } ] / dx
- }
-
- // check that numerical and analytical match
- ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4);
- ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
-}
-
-TEST(IMU_tools, diff_jacobians)
-{
- VectorXs d1(10), d2(10), d3(9), d1_pert(10), d2_pert(10), d3_pert(9); // deltas
- VectorXs t1(9), t2(9); // tangents
- Matrix<Scalar, 9, 9> J1_a, J2_a, J1_n, J2_n;
- Vector4s qv1, qv2;
- Scalar dx = 1e-6;
- qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
- d1 << 0, 1, 2, qv1, 7, 8, 9;
- qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
- d2 << 9, 8, 7, qv2, 2, 1, 0;
-
- // analytical jacobians
- diff(d1, d2, d3, J1_a, J2_a);
-
- // numerical jacobians
- for (unsigned int i = 0; i<9; i++)
- {
- t1 . setZero();
- t1(i) = dx;
-
- // Jac wrt first delta
- d1_pert = plus(d1, t1); // (d1 + t1)
- d3_pert = diff(d1_pert, d2); // d2 - (d1 + t1)
- t2 = d3_pert - d3; // { d2 - (d1 + t1) } - { d2 - d1 }
- J1_n.col(i) = t2/dx; // [ { d2 - (d1 + t1) } - { d2 - d1 } ] / dx
-
- // Jac wrt second delta
- d2_pert = plus(d2, t1); // (d2 + t1)
- d3_pert = diff(d1, d2_pert); // (d2 + t1) - d1
- t2 = d3_pert - d3; // { (d2 + t1) - d1 } - { d2 - d1 }
- J2_n.col(i) = t2/dx; // [ { (d2 + t1) - d1 } - { d2 - d1 } ] / dx
- }
-
- // check that numerical and analytical match
- ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4);
- ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
-}
-
-TEST(IMU_tools, body2delta_jacobians)
-{
- VectorXs delta(10), delta_pert(10); // deltas
- VectorXs body(6), pert(6);
- VectorXs tang(9); // tangents
- Matrix<Scalar, 9, 6> J_a, J_n; // analytic and numerical jacs
- Vector4s qv;;
- Scalar dt = 0.1;
- Scalar dx = 1e-6;
- qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
- delta << 0, 1, 2, qv, 7, 8, 9;
- body << 1, 2, 3, 3, 2, 1;
-
- // analytical jacobians
- body2delta(body, dt, delta, J_a);
-
- // numerical jacobians
- for (unsigned int i = 0; i<6; i++)
- {
- pert . setZero();
- pert(i) = dx;
-
- // Jac wrt first delta
- delta_pert = body2delta(body + pert, dt); // delta(body + pert)
- tang = diff(delta, delta_pert); // delta(body + pert) -- delta(body)
- J_n.col(i) = tang/dx; // ( delta(body + pert) -- delta(body) ) / dx
- }
-
- // check that numerical and analytical match
- ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
-}
-
-TEST(motion2data, zero)
-{
- Vector6s motion;
- Vector6s bias;
- Quaternions q;
-
- motion .setZero();
- bias .setZero();
- q .setIdentity();
-
- Vector6s data = imu::motion2data(motion, q, bias);
-
- Vector6s data_true; data_true << -gravity(), Vector3s::Zero();
-
- ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
-}
-
-TEST(motion2data, motion)
-{
- Vector6s motion, g_extended;
- Vector6s bias;
- Quaternions q;
-
- g_extended << gravity() , Vector3s::Zero();
-
- motion << 1,2,3, 4,5,6;
- bias .setZero();
- q .setIdentity();
-
- Vector6s data = imu::motion2data(motion, q, bias);
-
- Vector6s data_true; data_true = motion - g_extended;
-
- ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
-}
-
-TEST(motion2data, bias)
-{
- Vector6s motion, g_extended;
- Vector6s bias;
- Quaternions q;
-
- g_extended << gravity() , Vector3s::Zero();
-
- motion .setZero();
- bias << 1,2,3, 4,5,6;
- q .setIdentity();
-
- Vector6s data = imu::motion2data(motion, q, bias);
-
- Vector6s data_true; data_true = bias - g_extended;
-
- ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
-}
-
-TEST(motion2data, orientation)
-{
- Vector6s motion, g_extended;
- Vector6s bias;
- Quaternions q;
-
- g_extended << gravity() , Vector3s::Zero();
-
- motion .setZero();
- bias .setZero();
- q = v2q(Vector3s(M_PI/2, 0, 0)); // turn 90 deg in X axis
-
- Vector6s data = imu::motion2data(motion, q, bias);
-
- Vector6s data_true; data_true << 0,-gravity()(2),0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
-}
-
-TEST(motion2data, AllRandom)
-{
- Vector6s motion, g_extended;
- Vector6s bias;
- Quaternions q;
-
- motion .setRandom();
- bias .setRandom();
- q.coeffs() .setRandom().normalize();
-
- g_extended << q.conjugate()*gravity() , Vector3s::Zero();
-
- Vector6s data = imu::motion2data(motion, q, bias);
-
- Vector6s data_true; data_true = motion + bias - g_extended;
-
- ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
-}
-
-/* Integrate acc and angVel motion, obtain Delta_preintegrated
- * Input:
- * N: number of steps
- * q0: initial orientaiton
- * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
- * bias_real: the real bias of the IMU
- * bias_preint: the bias used for Delta pre-integration
- * Output:
- * return: the preintegrated delta
- */
-VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt)
-{
- VectorXs data(6);
- VectorXs body(6);
- VectorXs delta(10);
- VectorXs Delta(10), Delta_plus(10);
- Delta << 0,0,0, 0,0,0,1, 0,0,0;
- Quaternions q(q0);
- for (int n = 0; n<N; n++)
- {
- data = motion2data(motion, q, bias_real);
- q = q*exp_q(motion.tail(3)*dt);
- body = data - bias_preint;
- delta = body2delta(body, dt);
- Delta_plus = compose(Delta, delta, dt);
- Delta = Delta_plus;
- }
- return Delta;
-}
-
-/* Integrate acc and angVel motion, obtain Delta_preintegrated
- * Input:
- * N: number of steps
- * q0: initial orientaiton
- * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
- * bias_real: the real bias of the IMU
- * bias_preint: the bias used for Delta pre-integration
- * Output:
- * J_D_b: the Jacobian of the preintegrated delta wrt the bias
- * return: the preintegrated delta
- */
-VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b)
-{
- VectorXs data(6);
- VectorXs body(6);
- VectorXs delta(10);
- Matrix<Scalar, 9, 6> J_d_d, J_d_b;
- Matrix<Scalar, 9, 9> J_D_D, J_D_d;
- VectorXs Delta(10), Delta_plus(10);
- Quaternions q;
-
- Delta << 0,0,0, 0,0,0,1, 0,0,0;
- J_D_b.setZero();
- q = q0;
- for (int n = 0; n<N; n++)
- {
- // Simulate data
- data = motion2data(motion, q, bias_real);
- q = q*exp_q(motion.tail(3)*dt);
- // Motion::integrateOneStep()
- { // IMU::computeCurrentDelta
- body = data - bias_preint;
- body2delta(body, dt, delta, J_d_d);
- J_d_b = - J_d_d;
- }
- { // IMU::deltaPlusDelta
- compose(Delta, delta, dt, Delta_plus, J_D_D, J_D_d);
- }
- // Motion:: jac calib
- J_D_b = J_D_D*J_D_b + J_D_d*J_d_b;
- // Motion:: buffer
- Delta = Delta_plus;
- }
- return Delta;
-}
-
-TEST(IMU_tools, integral_jacobian_bias)
-{
- VectorXs Delta(10), Delta_pert(10); // deltas
- VectorXs bias_real(6), pert(6), bias_pert(6), bias_preint(6);
- VectorXs body(6), data(6), motion(6);
- VectorXs tang(9); // tangents
- Matrix<Scalar, 9, 6> J_a, J_n; // analytic and numerical jacs
- Scalar dt = 0.001;
- Scalar dx = 1e-4;
- Quaternions q0(3, 4, 5, 6); q0.normalize();
- motion << .1, .2, .3, .3, .2, .1;
- bias_real << .002, .004, .001, .003, .002, .001;
- bias_preint << .004, .005, .006, .001, .002, .003;
-
- int N = 500; // # steps of integration
-
- // Analytical Jacobians
- Delta = integrateDelta(N, q0, motion, bias_real, bias_preint, dt, J_a);
-
- // numerical jacobians
- for (unsigned int i = 0; i<6; i++)
- {
- pert . setZero();
- pert(i) = dx;
-
- bias_pert = bias_preint + pert;
- Delta_pert = integrateDelta(N, q0, motion, bias_real, bias_pert, dt);
- tang = diff(Delta, Delta_pert); // Delta(bias + pert) -- Delta(bias)
- J_n.col(i) = tang/dx; // ( Delta(bias + pert) -- Delta(bias) ) / dx
- }
-
- // check that numerical and analytical match
- ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
-}
-
-TEST(IMU_tools, delta_correction)
-{
- VectorXs Delta(10), Delta_preint(10), Delta_corr; // deltas
- VectorXs bias(6), pert(6), bias_preint(6);
- VectorXs body(6), motion(6);
- VectorXs tang(9); // tangents
- Matrix<Scalar, 9, 6> J_b; // analytic and numerical jacs
- Vector4s qv;;
- Scalar dt = 0.001;
- Quaternions q0(3, 4, 5, 6); q0.normalize();
- motion << 1, 2, 3, 3, 2, 1; motion *= .1;
-
- int N = 1000; // # steps of integration
-
- // preintegration with correct bias
- bias << .004, .005, .006, .001, .002, .003;
- Delta = integrateDelta(N, q0, motion, bias, bias, dt);
-
- // preintegration with wrong bias
- pert << .002, -.001, .003, -.0003, .0002, -.0001;
- bias_preint = bias + pert;
- Delta_preint = integrateDelta(N, q0, motion, bias, bias_preint, dt, J_b);
-
- // correct perturbated
- Vector9s step = J_b*(bias-bias_preint);
- Delta_corr = plus(Delta_preint, step);
-
- // Corrected delta should match real delta
- ASSERT_MATRIX_APPROX(Delta, Delta_corr, 1e-5);
-
- // diff between real and corrected should be zero
- ASSERT_MATRIX_APPROX(diff(Delta, Delta_corr), Vector9s::Zero(), 1e-5);
-
- // diff between preint and corrected deltas should be the jacobian-computed step
- ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-5);
-}
-
-TEST(IMU_tools, full_delta_residual)
-{
- VectorXs x1(10), x2(10);
- VectorXs Delta(10), Delta_preint(10), Delta_corr(10);
- VectorXs Delta_real(9), Delta_err(9), Delta_diff(10), Delta_exp(10);
- VectorXs bias(6), pert(6), bias_preint(6), bias_null(6);
- VectorXs body(6), motion(6);
- VectorXs tang(9); // tangents
- Matrix<Scalar, 9, 6> J_b; // analytic and numerical jacs
- Scalar dt = 0.001;
- Quaternions q0; q0.setIdentity();
-
- x1 << 0,0,0, 0,0,0,1, 0,0,0;
- motion << .3, .2, .1, .1, .2, .3; // acc and gyro
-// motion << .3, .2, .1, .0, .0, .0; // only acc
-// motion << .0, .0, .0, .1, .2, .3; // only gyro
- bias << 0.01, 0.02, 0.003, 0.002, 0.005, 0.01;
- bias_null << 0, 0, 0, 0, 0, 0;
-// bias_preint << 0.003, 0.002, 0.001, 0.001, 0.002, 0.003;
- bias_preint = bias_null;
-
- int N = 1000; // # steps of integration
-
- // preintegration with no bias
- Delta_real = integrateDelta(N, q0, motion, bias_null, bias_null, dt);
-
- // final state
- x2 = composeOverState(x1, Delta_real, 1000*dt);
-
- // preintegration with the correct bias
- Delta = integrateDelta(N, q0, motion, bias, bias, dt);
-
- ASSERT_MATRIX_APPROX(Delta, Delta_real, 1e-4);
-
- // preintegration with wrong bias
- Delta_preint = integrateDelta(N, q0, motion, bias, bias_preint, dt, J_b);
-
- // compute correction step
- Vector9s step = J_b*(bias-bias_preint);
-
- // correct preintegrated delta
- Delta_corr = plus(Delta_preint, step);
-
- // Corrected delta should match real delta
- ASSERT_MATRIX_APPROX(Delta_real, Delta_corr, 1e-3);
-
- // diff between real and corrected should be zero
- ASSERT_MATRIX_APPROX(diff(Delta_real, Delta_corr), Vector9s::Zero(), 1e-3);
-
- // expected delta
- Delta_exp = betweenStates(x1, x2, N*dt);
-
- ASSERT_MATRIX_APPROX(Delta_exp, Delta_corr, 1e-3);
-
- // compute diff between expected and corrected
-// Matrix<Scalar, 9, 9> J_err_exp, J_err_corr;
- diff(Delta_exp, Delta_corr, Delta_err); //, J_err_exp, J_err_corr);
-
- ASSERT_MATRIX_APPROX(Delta_err, Vector9s::Zero(), 1e-3);
-
- // compute error between expected and preintegrated
- Delta_err = diff(Delta_preint, Delta_exp);
-
- // diff between preint and corrected deltas should be the jacobian-computed step
- ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-3);
- /* This implies:
- * - Since D_corr is expected to be similar to D_exp
- * step ~ diff(Delta_preint, Delta_exp)
- * - the residual can be computed with a regular '-' :
- * residual = diff(D_preint, D_exp) - J_bias * (bias - bias_preint)
- */
-
-// WOLF_TRACE("Delta real: ", Delta_real.transpose());
-// WOLF_TRACE("x2: ", x2.transpose());
-// WOLF_TRACE("Delta: ", Delta.transpose());
-// WOLF_TRACE("Delta pre: ", Delta_preint.transpose());
-// WOLF_TRACE("Jac bias: \n", J_b);
-// WOLF_TRACE("Delta step: ", step.transpose());
-// WOLF_TRACE("Delta cor: ", Delta_corr.transpose());
-// WOLF_TRACE("Delta exp: ", Delta_exp.transpose());
-// WOLF_TRACE("Delta err: ", Delta_err.transpose());
-// WOLF_TRACE("Delta err exp-pre: ", Delta_err.transpose());
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
-// ::testing::GTEST_FLAG(filter) = "IMU_tools.delta_correction";
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_factor_IMU.cpp b/test/gtest_factor_IMU.cpp
deleted file mode 100644
index e9692a62c..000000000
--- a/test/gtest_factor_IMU.cpp
+++ /dev/null
@@ -1,2840 +0,0 @@
-/**
- * \file gtest_factor_imu.cpp
- *
- * Created on: Jan 01, 2017
- * \author: Dinesh Atchuthan
- */
-
-//Wolf
-#include "base/capture/capture_IMU.h"
-#include "base/factor/factor_pose_3D.h"
-#include "base/processor/processor_IMU.h"
-#include "base/sensor/sensor_IMU.h"
-#include "base/processor/processor_odom_3D.h"
-#include "ceres_wrapper/ceres_manager.h"
-
-#include "utils_gtest.h"
-#include "base/utils/logging.h"
-
-#include <iostream>
-#include <fstream>
-
-//#define GET_RESIDUALS
-
-using namespace Eigen;
-using namespace std;
-using namespace wolf;
-
-/*
- * This test is designed to test IMU biases in a particular case : perfect IMU, not moving.
- * var(b1,b2,p2,v2,q2), inv(p1,q1,v1); fac1: imu+(b1=b2)
- * So there is no odometry data.
- * IMU data file should first contain initial conditions, then the time_step between each new imu and the last one (in seconds) data,
- * and finally the last stateafter integration and the last timestamp, Then it should contain all IMU data and related timestamps
- */
-
-class FactorIMU_biasTest_Static_NullBias : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr KF0;
- FrameBasePtr KF1;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
-
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- t.set(0);
-
- origin_bias << 0,0,0, 0,0,0;
- expected_final_state = x_origin; //null bias + static
-
- //set origin of the problem
- KF0 = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
-
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu;
- data_imu << -wolf::gravity(), 0,0,0;
-
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on IMU (measure only gravity here)
-
- for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
- {
- t.set(t.get() + 0.001); //increment of 1 ms
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sen_imu->process(imu_ptr);
-
- }
-
- KF1 = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
- KF1->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
-
- //===================================================== END{PROCESS DATA}
- KF0->unfix();
- KF1->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr KF0;
- FrameBasePtr KF1;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- t.set(0);
- origin_bias << 0.02, 0.05, 0.1, 0,0,0;
-
- expected_final_state = x_origin; //null bias + static
-
- //set origin of the problem
- KF0 = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu;
- data_imu << -wolf::gravity(), 0,0,0;
- data_imu = data_imu + origin_bias;
-
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), origin_bias); //set data on IMU (measure only gravity here)
-
- for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
- {
- t.set(t.get() + 0.001); //increment of 1 ms
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sen_imu->process(imu_ptr);
- }
-
- KF1 = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
- KF1->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
-
- //===================================================== END{PROCESS DATA}
- KF0->unfix();
- KF1->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
-// ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
-// ceres_options.max_line_search_step_contraction = 1e-3;
-// ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- t.set(0);
- origin_bias << 0, 0, 0, 0.0002, 0.0005, 0.001;
-
- expected_final_state = x_origin; //null bias + static,
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu;
- data_imu << -wolf::gravity(), 0,0,0;
- data_imu = data_imu + origin_bias;
-
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov() );//, origin_bias); //set data on IMU (measure only gravity here)
-
- for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
- {
- t.set(t.get() + 0.001); //increment of 1 ms
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sen_imu->process(imu_ptr);
- }
-
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
- last_KF->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- t.set(0);
- origin_bias << 0.002, 0.005, 0.1, 0.07,-0.035,-0.1;
- origin_bias *= .01;
-
- expected_final_state = x_origin; //null bias + static
-
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu;
- data_imu << -wolf::gravity(), 0,0,0;
- data_imu = data_imu + origin_bias;
-
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero()); //set data on IMU (measure only gravity here)
-
- for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
- {
- t.set(t.get() + 0.001); //increment of 1 ms
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sen_imu->process(imu_ptr);
- }
-
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
- last_KF->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_biasTest_Move_NullBias : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
-
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- t.set(0);
-
- Eigen::Vector6s data_imu;
- data_imu << 0,10,-wolf::gravity()(2), 0,0,0; //10m/s on y direction
- expected_final_state << 0,5,0, 0,0,0,1, 0,10,0; // advanced at a=10m/s2 during 1s ==> dx = 0.5*10*1^2 = 5; dvx = 10*1 = 10
-
- origin_bias<< 0,0,0,0,0,0;
-
- expected_final_state = x_origin;
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
-
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
-
- for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
- {
- t.set(t.get() + 0.001); //increment of 1 ms
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sen_imu->process(imu_ptr);
- }
-
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
- last_KF->setState(expected_final_state);
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- t.set(0);
-
- Eigen::Vector6s data_imu;
- origin_bias = Eigen::Vector6s::Random() * 0.001;
- data_imu << 0,10,-wolf::gravity()(2), 0,0,0; //10m/s on y direction
- data_imu = data_imu + origin_bias;
- expected_final_state << 0,5,0, 0,0,0,1, 0,10,0; // advanced at a=10m/s2 during 1s ==> dx = 0.5*10*1^2 = 5; dvx = 10*1 = 10
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
-
- for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
- {
- t.set(t.get() + 0.001); //increment of 1 ms
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sen_imu->process(imu_ptr);
- }
-
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
- last_KF->setState(expected_final_state);
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- t.set(0);
-
- Eigen::Vector6s data_imu, data_imu_initial;
- origin_bias = Eigen::Vector6s::Random() * 0.001;
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0; // rad/s
- data_imu << -wolf::gravity(), rate_of_turn,0,0; //rotation only
- data_imu_initial = data_imu;
-
- // Expected state after one second integration
- Eigen::Quaternions quat_comp(Eigen::Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data_imu.tail(3)*1);
- expected_final_state << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.1s = 5 deg => 5 * M_PI/180
-
- data_imu = data_imu + origin_bias; // bias measurements
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
-
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
-
- for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
- {
- //gravity measure depends on current IMU orientation + bias
- //use data_imu_initial to measure gravity from real orientation of IMU then add biases
- data_imu.head(3) = (v2q(data_imu_initial.tail(3) * t.get()).conjugate() * data_imu_initial.head(3)) + origin_bias.head(3);
- t.set(t.get() + 0.001); //increment of 1 ms
- imu_ptr->setData(data_imu);
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sen_imu->process(imu_ptr);
- }
-
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
- last_KF->setState(expected_final_state);
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;ceres::LINE_SEARCH
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 10,-3,4;
- t.set(0);
-
- Eigen::Vector6s data_imu, data_imu_initial;
- origin_bias = Eigen::Vector6s::Random();
- origin_bias << 0,0,0, 0,0,0;
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0; // rad/s
- data_imu << -wolf::gravity(), rate_of_turn,0,0; //rotation only
- data_imu_initial = data_imu;
-
- // Expected state after one second integration
- Eigen::Quaternions quat_comp(Eigen::Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data_imu.tail(3)*1);
-
- // rotated at 5 deg/s for 1s = 5 deg => 5 * M_PI/180
- // no other acceleration : we should still be moving at initial velocity
- // position = V*dt, dt = 1s
- expected_final_state << 10,-3,4, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 10,-3,4;
-
- data_imu = data_imu + origin_bias; // bias measurements
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(t, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
-
- for(unsigned int i = 0; i < 1000; i++) //integrate during 1 second
- {
- //gravity measure depends on current IMU orientation + bias
- //use data_imu_initial to measure gravity from real orientation of IMU then add biases
- data_imu.head(3) = (v2q(data_imu_initial.tail(3) * t.get()).conjugate() * data_imu_initial.head(3)) + origin_bias.head(3);
- t.set(t.get() + 0.001); //increment of 1 ms
- imu_ptr->setData(data_imu);
- imu_ptr->setTimeStamp(t);
-
- // process data in capture
- sen_imu->process(imu_ptr);
- }
-
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
- last_KF->setState(expected_final_state);
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-// var(b1,b2), inv(p1,q1,v1,p2,q2,v2); fac1: imu(p,q,v)+(b1=b2)
-
-class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION;
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t1.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- origin_bias << 0.01, 0.02, 0.003, 0.002, 0.005, 0.01;
- t.set(0);
- Eigen::Quaternions quat_current(Eigen::Quaternions::Identity());
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu(Eigen::Vector6s::Zero());
- Eigen::Vector3s rateOfTurn(Eigen::Vector3s::Zero()); //deg/s
-
- Scalar dt(0.001);
- TimeStamp ts(0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
-
- while( ts.get() < 1 )
- {
- // PROCESS IMU DATA
- // Time and data variables
- ts += dt;
-
- rateOfTurn << .1, .2, .3; //to have rate of turn > 0 deg/s
- data_imu.head(3) = quat_current.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
- data_imu.tail(3) = rateOfTurn;
-
- //compute current orientaton taking this measure into account
- quat_current = quat_current * wolf::v2q(rateOfTurn*dt);
-
- //set timestamp, add bias, set data and process
- imu_ptr->setTimeStamp(ts);
- data_imu = data_imu + origin_bias;
- imu_ptr->setData(data_imu);
- sen_imu->process(imu_ptr);
-
- }
-
- expected_final_state << 0,0,0, quat_current.x(), quat_current.y(), quat_current.z(), quat_current.w(), 0,0,0;
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts);
- last_KF->setState(expected_final_state);
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- ProblemPtr problem;
- CeresManagerPtr ceres_manager;
- SensorBasePtr sensor;
- SensorIMUPtr sensor_imu;
- SensorOdom3DPtr sensor_odo;
- ProcessorBasePtr processor;
- ProcessorIMUPtr processor_imu;
- ProcessorOdom3DPtr processor_odo;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- CaptureIMUPtr capture_imu;
- CaptureOdom3DPtr capture_odo;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- problem = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION;
- ceres_manager = std::make_shared<CeresManager>(problem, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- sensor = problem->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- sensor_imu = std::static_pointer_cast<SensorIMU>(sensor);
- processor = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml");
- processor_imu = std::static_pointer_cast<ProcessorIMU>(processor);
-
- // SENSOR + PROCESSOR ODOM 3D
- sensor = problem->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml");
- sensor_odo = std::static_pointer_cast<SensorOdom3D>(sensor);
-
- sensor_imu->setNoiseStd((sensor_imu->getNoiseStd()/10.0).eval());
- sensor_odo->setNoiseStd((sensor_odo->getNoiseStd()/10.0).eval());
- WOLF_TRACE("IMU cov: ", sensor_imu->getNoiseCov().diagonal().transpose());
- WOLF_TRACE("ODO cov: ", sensor_odo->getNoiseCov().diagonal().transpose());
-
- ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
- prc_odom3D_params->max_time_span = 0.0099;
- prc_odom3D_params->max_buff_length = 1000; //make it very high so that this condition will not pass
- prc_odom3D_params->dist_traveled = 1000;
- prc_odom3D_params->angle_turned = 1000;
-
- processor = problem->installProcessor("ODOM 3D", "odom", sensor_odo, prc_odom3D_params);
- processor_odo = std::static_pointer_cast<ProcessorOdom3D>(processor);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- origin_bias << 0.0015, 0.004, -0.002, 0.005, -0.0074, -0.003;
-// origin_bias /= 100;
- t.set(0);
-
- //set origin of the problem
-
- origin_KF = processor_imu->setOrigin(x_origin, t);
- processor_odo->setOrigin(origin_KF);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu(Eigen::Vector6s::Zero()),
- data_odo(Eigen::Vector6s::Zero());
- Eigen::Vector3s rateOfTurn(Eigen::Vector3s::Zero()); //deg/s
-
- TimeStamp t_imu(0.0), t_odo(0.0);
- Scalar dt_imu(0.001), dt_odo(.01);
-
- capture_imu = std::make_shared<CaptureIMU> (t_imu, sensor_imu, data_imu, sensor_imu->getNoiseCov(), sensor_imu->getCalibration(), nullptr);
-
- capture_odo = std::make_shared<CaptureOdom3D>(t_odo, sensor_odo, data_odo, sensor_odo->getNoiseCov(), nullptr);
- sensor_odo->process(capture_odo);
- t_odo += dt_odo; //first odometry data will be processed at this timestamp
-
- // ground truth for quaternion:
- Eigen::Quaternions quat_odo(Eigen::Quaternions::Identity());
- Eigen::Quaternions quat_imu(Eigen::Quaternions::Identity());
-
- WOLF_TRACE("last delta preint: ", processor_imu->getLast()->getDeltaPreint().transpose());
- WOLF_TRACE("last jacoob bias : ", processor_imu->getLast()->getJacobianCalib().row(0));
-
- for(unsigned int i = 1; i<=1000; i++)
- {
-
- // PROCESS IMU DATA
- // Time and data variables
- t_imu += dt_imu;
-
-// rateOfTurn = Eigen::Vector3s::Random()*10; //to have rate of turn > 0.99 deg/s
- rateOfTurn << 5, 10, 15; // deg/s
- data_imu.tail<3>() = rateOfTurn * M_PI/180.0;
- data_imu.head<3>() = quat_imu.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
-
- //compute odometry + current orientaton taking this measure into account
- quat_odo = quat_odo * wolf::v2q(data_imu.tail(3)*dt_imu);
- quat_imu = quat_imu * wolf::v2q(data_imu.tail(3)*dt_imu);
-
- //set timestamp, add bias, set data and process
- capture_imu->setTimeStamp(t_imu);
- data_imu = data_imu + origin_bias;
- capture_imu->setData(data_imu);
- sensor_imu->process(capture_imu);
-
- WOLF_TRACE("last delta preint: ", processor_imu->getLast()->getDeltaPreint().transpose());
- WOLF_TRACE("last jacoob bias : ", processor_imu->getLast()->getJacobianCalib().row(0));
-
- //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
- if(t_imu.get() >= t_odo.get())
- {
- WOLF_TRACE("====== create ODOM KF ========");
-// WOLF_TRACE("Jac calib: ", processor_imu->getLast()->getJacobianCalib().row(0));
-// WOLF_TRACE("last calib: ", processor_imu->getLast()->getCalibration().transpose());
-// WOLF_TRACE("last calib preint: ", processor_imu->getLast()->getCalibrationPreint().transpose());
-
- // PROCESS ODOM 3D DATA
- data_odo.head(3) << 0,0,0;
- data_odo.tail(3) = q2v(quat_odo);
- capture_odo->setTimeStamp(t_odo);
- capture_odo->setData(data_odo);
-
-// WOLF_TRACE("Jac calib: ", processor_imu->getLast()->getJacobianCalib().row(0));
-// WOLF_TRACE("last calib: ", processor_imu->getLast()->getCalibration().transpose());
-// WOLF_TRACE("last calib preint: ", processor_imu->getLast()->getCalibrationPreint().transpose());
-
- sensor_odo->process(capture_odo);
-
-// WOLF_TRACE("Jac calib: ", processor_imu->getOrigin()->getJacobianCalib().row(0));
-// WOLF_TRACE("orig calib: ", processor_imu->getOrigin()->getCalibration().transpose());
-// WOLF_TRACE("orig calib preint: ", processor_imu->getOrigin()->getCalibrationPreint().transpose());
-
- //prepare next odometry measurement
- quat_odo = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
- t_odo += dt_odo;
- break;
- }
- }
-
- expected_final_state.resize(10);
- expected_final_state << 0,0,0, quat_imu.x(), quat_imu.y(), quat_imu.z(), quat_imu.w(), 0,0,0;
-
- last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t_imu);
- last_KF->setState(expected_final_state);
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
-
- CaptureBasePtr origin_CB = origin_KF->getCaptureOf(sensor_imu);
- CaptureMotionPtr last_CM = std::static_pointer_cast<CaptureMotion>(last_KF ->getCaptureOf(sensor_imu));
-
-// WOLF_TRACE("KF1 calib : ", origin_CB->getCalibration().transpose());
-// WOLF_TRACE("KF2 calib pre: ", last_CM ->getCalibrationPreint().transpose());
-// WOLF_TRACE("KF2 calib : ", last_CM ->getCalibration().transpose());
-// WOLF_TRACE("KF2 delta pre: ", last_CM ->getDeltaPreint().transpose());
-// WOLF_TRACE("KF2 delta cor: ", last_CM ->getDeltaCorrected(origin_CB->getCalibration()).transpose());
-// WOLF_TRACE("KF2 jacob : ", last_CM ->getJacobianCalib().row(0));
-
- // ==================================================== show problem status
-
- problem->print(4,1,1,1);
-
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- SensorOdom3DPtr sen_odom3D;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- ProcessorOdom3DPtr processor_ptr_odom3D;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION;
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- // SENSOR + PROCESSOR ODOM 3D
- SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml");
- ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
- prc_odom3D_params->max_time_span = 0.9999;
- prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
- prc_odom3D_params->dist_traveled = 1000000000;
- prc_odom3D_params->angle_turned = 1000000000;
-
- ProcessorBasePtr processor_ptr_odom = wolf_problem_ptr_->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params);
- sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr);
- processor_ptr_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_ptr_odom);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- origin_bias << 0.0015, 0.004, -0.002, 0.005, -0.0074, -0.003;
- origin_bias *= .1;
- t.set(0);
- Eigen::Quaternions odom_quat(Eigen::Quaternions::Identity());
- Eigen::Quaternions current_quatState(Eigen::Quaternions::Identity());
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
- processor_ptr_odom3D->setOrigin(origin_KF);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu(Eigen::Vector6s::Zero()), data_odom3D(Eigen::Vector6s::Zero());
- Eigen::Vector3s rateOfTurn; //deg/s
- rateOfTurn << 0,90,0;
- VectorXs D_cor(10);
-
- Scalar dt(0.0010), dt_odom(1.0);
- TimeStamp ts(0.0), t_odom(0.0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
- wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, sen_odom3D->getNoiseCov(), 7, 6, nullptr);
- sen_odom3D->process(mot_ptr);
- //first odometry data will be processed at this timestamp
- t_odom.set(t_odom.get() + dt_odom);
-
- data_imu.tail<3>() = rateOfTurn* M_PI/180.0; //constant rotation =
-
- //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
- for(unsigned int i = 1; i<=1000; i++)
- {
- // PROCESS IMU DATA
- // Time and data variables
- ts.set(i*dt);
- data_imu.head<3>() = current_quatState.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
-
- //compute odometry + current orientaton taking this measure into account
- odom_quat = odom_quat * wolf::v2q(data_imu.tail(3)*dt);
- current_quatState = current_quatState * wolf::v2q(data_imu.tail(3)*dt);
-
- //set timestamp, add bias, set data and process
- imu_ptr->setTimeStamp(ts);
- data_imu = data_imu + origin_bias;
- imu_ptr->setData(data_imu);
- sen_imu->process(imu_ptr);
-
- D_cor = processor_ptr_imu->getLast()->getDeltaCorrected(origin_bias);
-
- if(ts.get() >= t_odom.get())
- {
- WOLF_TRACE("X_preint(t) : ", wolf_problem_ptr_->getState(ts).transpose());
-
- // PROCESS ODOM 3D DATA
- data_odom3D.head(3) << 0,0,0;
- data_odom3D.tail(3) = q2v(odom_quat);
- mot_ptr->setTimeStamp(t_odom);
- mot_ptr->setData(data_odom3D);
- sen_odom3D->process(mot_ptr);
-
- //prepare next odometry measurement
- odom_quat = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
- t_odom.set(t_odom.get() + dt_odom);
- }
- }
-
- expected_final_state << 0,0,0, current_quatState.x(), current_quatState.y(), current_quatState.z(), current_quatState.w(), 0,0,0;
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts);
- last_KF->setState(expected_final_state);
-
- WOLF_TRACE("X_correct(t) : ", imu::composeOverState(x_origin, D_cor, ts - t).transpose());
- WOLF_TRACE("X_true(t) : ", expected_final_state.transpose());
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
-{
- public:
- wolf::TimeStamp t;
- SensorIMUPtr sen_imu;
- SensorOdom3DPtr sen_odom3D;
- ProblemPtr wolf_problem_ptr_;
- CeresManager* ceres_manager_wolf_diff;
- ProcessorBasePtr processor_ptr_;
- ProcessorIMUPtr processor_ptr_imu;
- ProcessorOdom3DPtr processor_ptr_odom3D;
- FrameBasePtr origin_KF;
- FrameBasePtr last_KF;
- Eigen::Vector6s origin_bias;
- Eigen::VectorXs expected_final_state;
- Eigen::VectorXs x_origin;
-
- virtual void SetUp()
- {
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- //===================================================== SETTING PROBLEM
- // WOLF PROBLEM
- wolf_problem_ptr_ = Problem::create("POV 3D");
-
- // CERES WRAPPER
- ceres::Solver::Options ceres_options;
- ceres_options.minimizer_type = ceres::TRUST_REGION;
- ceres_options.max_line_search_step_contraction = 1e-3;
- ceres_options.max_num_iterations = 1e4;
- ceres_manager_wolf_diff = new CeresManager(wolf_problem_ptr_, ceres_options);
-
- // SENSOR + PROCESSOR IMU
- SensorBasePtr sen0_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_imu.yaml");
- processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu_t6.yaml");
- sen_imu = std::static_pointer_cast<SensorIMU>(sen0_ptr);
- processor_ptr_imu = std::static_pointer_cast<ProcessorIMU>(processor_ptr_);
-
- // SENSOR + PROCESSOR ODOM 3D
- SensorBasePtr sen1_ptr = wolf_problem_ptr_->installSensor("ODOM 3D", "odom", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/sensor_odom_3D_HQ.yaml");
- ProcessorParamsOdom3DPtr prc_odom3D_params = std::make_shared<ProcessorParamsOdom3D>();
- prc_odom3D_params->max_time_span = 0.9999;
- prc_odom3D_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
- prc_odom3D_params->dist_traveled = 1000000000;
- prc_odom3D_params->angle_turned = 1000000000;
-
- ProcessorBasePtr processor_ptr_odom = wolf_problem_ptr_->installProcessor("ODOM 3D", "odom", sen1_ptr, prc_odom3D_params);
- sen_odom3D = std::static_pointer_cast<SensorOdom3D>(sen1_ptr);
- processor_ptr_odom3D = std::static_pointer_cast<ProcessorOdom3D>(processor_ptr_odom);
-
- //===================================================== END{SETTING PROBLEM}
- //===================================================== INITIALIZATION
-
- expected_final_state.resize(10);
- x_origin.resize(10);
- x_origin << 0,0,0, 0,0,0,1, 0,0,0;
- origin_bias << 0.0015, 0.004, -0.002, 0.005, -0.0074, -0.003;
- t.set(0);
- Eigen::Quaternions odom_quat(Eigen::Quaternions::Identity());
- Eigen::Quaternions current_quatState(Eigen::Quaternions::Identity());
-
- //set origin of the problem
- origin_KF = processor_ptr_imu->setOrigin(x_origin, t);
- processor_ptr_odom3D->setOrigin(origin_KF);
-
- //===================================================== END{INITIALIZATION}
- //===================================================== PROCESS DATA
- // PROCESS DATA
-
- Eigen::Vector6s data_imu(Eigen::Vector6s::Zero()), data_odom3D(Eigen::Vector6s::Zero());
- Eigen::Vector3s rateOfTurn; //deg/s
- rateOfTurn << 45,90,0;
-
- Scalar dt(0.0010), dt_odom(1.0);
- TimeStamp ts(0.0), t_odom(1.0);
- wolf::CaptureIMUPtr imu_ptr = std::make_shared<CaptureIMU>(ts, sen_imu, data_imu, sen_imu->getNoiseCov(), Eigen::Vector6s::Zero());
- wolf::CaptureMotionPtr mot_ptr = std::make_shared<CaptureMotion>(t, sen_odom3D, data_odom3D, 7, 6, nullptr);
- sen_odom3D->process(mot_ptr);
- //first odometry data will be processed at this timestamp
- //t_odom.set(t_odom.get() + dt_odom);
-
- Eigen::Vector2s randomPart;
- data_imu.tail<3>() = rateOfTurn* M_PI/180.0; //constant rotation =
-
- //when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
- for(unsigned int i = 1; i<=500; i++)
- {
- // PROCESS IMU DATA
- // Time and data variables
- ts.set(i*dt);
-
- data_imu.head<3>() = current_quatState.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
-
- //compute odometry + current orientaton taking this measure into account
- odom_quat = odom_quat * wolf::v2q(data_imu.tail(3)*dt);
- current_quatState = current_quatState * wolf::v2q(data_imu.tail(3)*dt);
-
- //set timestamp, add bias, set data and process
- imu_ptr->setTimeStamp(ts);
- data_imu = data_imu + origin_bias;
- imu_ptr->setData(data_imu);
- sen_imu->process(imu_ptr);
-
- if(ts.get() >= t_odom.get())
- {
- // PROCESS ODOM 3D DATA
- data_odom3D.head(3) << 0,0,0;
- data_odom3D.tail(3) = q2v(odom_quat);
- mot_ptr->setTimeStamp(t_odom);
- mot_ptr->setData(data_odom3D);
- sen_odom3D->process(mot_ptr);
-
- //prepare next odometry measurement
- odom_quat = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
- t_odom.set(t_odom.get() + dt_odom);
- }
- }
-
- rateOfTurn << 30,10,0;
- data_imu.tail<3>() = rateOfTurn* M_PI/180.0;
-
- for(unsigned int j = 1; j<=500; j++)
- {
- // PROCESS IMU DATA
- // Time and data variables
- ts.set((500 + j)*dt);
-
- /*data_imu.tail<3>() = rateOfTurn* M_PI/180.0;
- randomPart = Eigen::Vector2s::Random(); //to have rate of turn > 0.99 deg/s
- data_imu.segment<2>(3) += randomPart* M_PI/180.0;*/
- data_imu.head<3>() = current_quatState.conjugate() * (- wolf::gravity()); //gravity measured, we have no other translation movement
-
- //compute odometry + current orientaton taking this measure into account
- odom_quat = odom_quat * wolf::v2q(data_imu.tail(3)*dt);
- current_quatState = current_quatState * wolf::v2q(data_imu.tail(3)*dt);
-
- //set timestamp, add bias, set data and process
- imu_ptr->setTimeStamp(ts);
- data_imu = data_imu + origin_bias;
- imu_ptr->setData(data_imu);
- sen_imu->process(imu_ptr);
-
- if(ts.get() >= t_odom.get())
- {
- // PROCESS ODOM 3D DATA
- data_odom3D.head(3) << 0,0,0;
- data_odom3D.tail(3) = q2v(odom_quat);
- mot_ptr->setTimeStamp(t_odom);
- mot_ptr->setData(data_odom3D);
- sen_odom3D->process(mot_ptr);
-
- //prepare next odometry measurement
- odom_quat = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
- t_odom.set(t_odom.get() + dt_odom);
- }
- }
-
- expected_final_state << 0,0,0, current_quatState.x(), current_quatState.y(), current_quatState.z(), current_quatState.w(), 0,0,0;
- last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts);
- last_KF->setState(expected_final_state);
-
- //===================================================== END{PROCESS DATA}
- origin_KF->unfix();
- last_KF->unfix();
- }
-
- virtual void TearDown(){}
-};
-
-// tests with following conditions :
-// var(b1,b2), invar(p1,q1,v1,p2,q2,v2), factor : imu(p,q,v)
-
-TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- KF0->getP()->fix();
- KF0->getO()->fix();
- KF0->getV()->fix();
- KF0->getCaptureOf(sen_imu)->setCalibration((Vector6s()<<1,2,3,1,2,3).finished());
-
- KF1->getP()->setState(expected_final_state.head(3));
- KF1->getO()->setState(expected_final_state.segment(3,4));
- KF1->getV()->setState(expected_final_state.segment(7,3));
-
- KF1->getP()->fix();
- KF1->getO()->fix();
- KF1->getV()->fix();
- KF1->getCaptureOf(sen_imu)->setCalibration((Vector6s()<<-1,-2,-3,-1,-2,-3).finished());
-
- std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
-}
-
-TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
-{
- //prepare problem for solving
- KF0->getP()->fix();
- KF0->getO()->fix();
- KF0->getV()->fix();
- KF1->getP()->fix();
- KF1->getO()->fix();
- KF1->getV()->fix();
-
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
- std::string report;
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-6")
- epsilon_bias = 0.000001;
- Eigen::Vector6s err;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-4")
- epsilon_bias = 0.0001;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-2")
- epsilon_bias = 0.01;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-1")
- epsilon_bias = 0.1;
-
- for(int i = 1; i<10; i++)
- {
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
- }
-}
-
-TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- KF0->getP()->fix();
- KF0->getO()->fix();
- KF0->getV()->fix();
-
- KF1->getP()->setState(expected_final_state.head(3));
- KF1->getO()->setState(expected_final_state.segment(3,4));
- KF1->getV()->setState(expected_final_state.segment(7,3));
-
- KF1->getP()->fix();
- KF1->getO()->fix();
- KF1->getV()->fix();
-
- std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
-}
-
-TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
-{
- //prepare problem for solving
- KF0->getP()->fix();
- KF0->getO()->fix();
- KF0->getV()->fix();
- KF1->getP()->fix();
- KF1->getO()->fix();
- KF1->getV()->fix();
-
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
- std::string report;
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-6")
- epsilon_bias = 0.000001;
- Eigen::Vector6s err;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-4")
- epsilon_bias = 0.0001;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-2")
- epsilon_bias = 0.01;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-1")
- epsilon_bias = 0.1;
-
- for(int i = 1; i<10; i++)
- {
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- KF0->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- KF1->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS) //Acc bias
- ASSERT_MATRIX_APPROX(KF0->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS) //Gyro bias
-
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().head(3), origin_bias.head(3), wolf::Constants::EPS)
- ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
- }
-}
-
-TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->getP()->setState(expected_final_state.head(3));
- last_KF->getO()->setState(expected_final_state.segment(3,4));
- last_KF->getV()->setState(expected_final_state.segment(7,3));
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-}
-
-TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
- std::string report;
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-6")
- epsilon_bias = 0.000001;
- Eigen::Vector6s err;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
-
- origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- last_KF ->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-4")
- epsilon_bias = 0.0001;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-2")
- epsilon_bias = 0.01;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-1")
- epsilon_bias = 0.1;
-
- for(int i = 1; i<10; i++)
- {
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->getCaptureOf(sen_imu)->setCalibration(perturbed_bias);
- last_KF->getCaptureOf(sen_imu)->setCalibration(origin_bias);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- }
-}
-
-TEST_F(FactorIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
- std::string report;
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-6")
- epsilon_bias = 0.000001;
- Eigen::Vector6s err;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
-
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-4")
- epsilon_bias = 0.0001;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-2")
- epsilon_bias = 0.01;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-1")
- epsilon_bias = 0.1;
-
- for(int i = 1; i<10; i++)
- {
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- }
-}
-
-TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
-}
-
-TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-6")
- epsilon_bias = 0.000001;
- Eigen::Vector6s err;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-4")
- epsilon_bias = 0.0001;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-2")
- epsilon_bias = 0.01;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-1")
- epsilon_bias = 0.1;
-
- for(int i = 1; i<10; i++)
- {
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- }
-}
-
-TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
-}
-
-TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-6")
- epsilon_bias = 0.000001;
- Eigen::Vector6s err;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-4")
- epsilon_bias = 0.0001;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-2")
- epsilon_bias = 0.01;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-1")
- epsilon_bias = 0.1;
-
- for(int i = 1; i<10; i++)
- {
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- }
-}
-
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
-}
-
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-6")
- epsilon_bias = 0.000001;
- Eigen::Vector6s err;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-4")
- epsilon_bias = 0.0001;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-2")
- epsilon_bias = 0.01;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-1")
- epsilon_bias = 0.1;
-
- for(int i = 1; i<10; i++)
- {
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- }
-}
-
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
-}
-
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- wolf::Scalar epsilon_bias = 0.0000001;
- Eigen::VectorXs perturbed_bias(origin_bias);
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-6")
- epsilon_bias = 0.000001;
- Eigen::Vector6s err;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-4")
- epsilon_bias = 0.0001;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-2")
- epsilon_bias = 0.01;
-
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- //==============================================================
- //WOLF_INFO("Starting error bias 1e-1")
- epsilon_bias = 0.1;
-
- for(int i = 1; i<10; i++)
- {
- err = Eigen::Vector6s::Random() * epsilon_bias*10;
- perturbed_bias = origin_bias + err;
- origin_KF->setState(x_origin);
- last_KF->setState(expected_final_state);
-
- report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- }
-}
-
-TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->getP()->unfix();
- last_KF->getO()->fix();
- last_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
-}
-
-//TEST_F(FactorIMU_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
-//{
-// //prepare problem for solving
-// origin_KF->getP()->fix();
-// origin_KF->getO()->fix();
-// origin_KF->getV()->unfix();
-//
-// last_KF->getP()->unfix();
-// last_KF->getO()->fix();
-// last_KF->getV()->unfix();
-//
-// last_KF->setState(expected_final_state);
-//
-// std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-//
-// //Only biases are unfixed
-// ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-3)
-// ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-3)
-//
-//}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(ALL);
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
-
- //get data from covariance blocks
- wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
-
- for(int i = 0; i<10; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- /*TEST_COUT << "2*std : " << cov_stdev.transpose();
- TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
- TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
-
- for(unsigned int i = 0; i<10; i++)
- assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
-
- if(cov_stdev.tail(6).maxCoeff()>=1)
- WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(ALL);
-
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
-
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
-
- //get data from covariance blocks
- wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
-
- for(int i = 0; i<10; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- /*TEST_COUT << "2*std : " << cov_stdev.transpose();
- TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
- TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
-
- for(unsigned int i = 0; i<10; i++)
- assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
-
- if(cov_stdev.tail(6).maxCoeff()>=1)
- WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
-}
-
-//jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- //ceres_manager_wolf_diff->computeCovariances(ALL);
-
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
-}
-
-//jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->unfix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
-}
-
-//jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->unfix();
- last_KF->getO()->fix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
-}
-
-//jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->unfix();
- last_KF->getO()->unfix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*10000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->unfix();
- last_KF->getO()->unfix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(ALL);
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
-
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
-
- //get data from covariance blocks
- wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
-
- for(int i = 0; i<10; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- /*TEST_COUT << "2*std : " << cov_stdev.transpose();
- TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
- TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
-
- for(unsigned int i = 0; i<10; i++)
- assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
-
- if(cov_stdev.tail(6).maxCoeff()>=1)
- WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
-{
- //Add fix factor on yaw to make the problem observable
- Eigen::MatrixXs featureFix_cov(6,6);
- featureFix_cov = Eigen::MatrixXs::Identity(6,6);
- featureFix_cov(5,5) = 0.1;
- CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr));
- FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
- FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
-
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->unfix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->unfix();
- last_KF->getO()->unfix();
- last_KF->getV()->fix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(ALL);
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
-
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
-
- //get data from covariance blocks
- wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
-
- for(int i = 0; i<10; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- /*TEST_COUT << "2*std : " << cov_stdev.transpose();
- TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
- TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
-
- for(unsigned int i = 0; i<10; i++)
- assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
-
- if(cov_stdev.tail(6).maxCoeff()>=1)
- WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
-{
- //Add fix factor on yaw to make the problem observable
- Eigen::MatrixXs featureFix_cov(6,6);
- featureFix_cov = Eigen::MatrixXs::Identity(6,6);
- featureFix_cov(5,5) = 0.1;
- CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr));
- FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
- FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
-
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->unfix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->unfix();
- last_KF->getO()->unfix();
- last_KF->getV()->fix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sen_imu)->getCalibration();
- origin_KF->getCaptureOf(sen_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager_wolf_diff->computeCovariances(ALL);
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
-
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
-
- //get data from covariance blocks
- wolf_problem_ptr_->getFrameCovariance(last_KF, covX);
-
- for(int i = 0; i<10; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- /*TEST_COUT << "2*std : " << cov_stdev.transpose();
- TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
- TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
-
- for(unsigned int i = 0; i<10; i++)
- assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
-
- if(cov_stdev.tail(6).maxCoeff()>=1)
- WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->fix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
- origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
-
-// WOLF_TRACE("real bias : ", origin_bias.transpose());
-// WOLF_TRACE("origin bias : ", origin_KF->getCaptureOf(sensor_imu)->getCalibration().transpose());
-// WOLF_TRACE("last bias : ", last_KF->getCaptureOf(sensor_imu)->getCalibration().transpose());
-// WOLF_TRACE("jacob bias : ", std::static_pointer_cast<CaptureIMU>(last_KF->getCaptureOf(sensor_imu))->getJacobianCalib().row(0));
-
- std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- std::cout << report << std::endl;
- ceres_manager->computeCovariances(ALL);
-
- //Only biases are unfixed
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
-
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
-
- //get data from covariance blocks
- problem->getFrameCovariance(last_KF, covX);
-
- for(int i = 0; i<10; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- /*TEST_COUT << "2*std : " << cov_stdev.transpose();
- TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
- TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
-
- for(unsigned int i = 0; i<10; i++)
- assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
-
- if(cov_stdev.tail(6).maxCoeff()>=1)
- WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
- origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager->computeCovariances(ALL);
-
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
-
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
-
- //get data from covariance blocks
- problem->getFrameCovariance(last_KF, covX);
-
- for(int i = 0; i<10; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- /*TEST_COUT << "2*std : " << cov_stdev.transpose();
- TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
- TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
-
- for(unsigned int i = 0; i<10; i++)
- assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
-
- if(cov_stdev.tail(6).maxCoeff()>=1)
- WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->fix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
- origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->fix();
- last_KF->getO()->unfix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
- origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->unfix();
- last_KF->getO()->fix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
- origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->unfix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->unfix();
- last_KF->getO()->unfix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
- origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*10000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
-}
-
-TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
-{
- //prepare problem for solving
- origin_KF->getP()->fix();
- origin_KF->getO()->fix();
- origin_KF->getV()->fix();
-
- last_KF->setState(expected_final_state);
-
- last_KF->getP()->unfix();
- last_KF->getO()->unfix();
- last_KF->getV()->unfix();
-
- //perturbation of origin bias
- Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
- Eigen::Vector6s bias = origin_KF->getCaptureOf(sensor_imu)->getCalibration();
- origin_KF->getCaptureOf(sensor_imu)->setCalibration(bias + random_err);
-
- std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ceres_manager->computeCovariances(ALL);
-
- ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
-
- ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
- ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
-
- Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
- Eigen::MatrixXs covX(10,10);
-
- //get data from covariance blocks
- problem->getFrameCovariance(last_KF, covX);
-
- for(int i = 0; i<10; i++)
- cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
-
- /*TEST_COUT << "2*std : " << cov_stdev.transpose();
- TEST_COUT << "expect : " << expected_final_state.transpose(); //expected final state
- TEST_COUT << "estim : " << last_KF->getState().transpose(); //estimated final state*/
-
- for(unsigned int i = 0; i<10; i++)
- assert((expected_final_state(i) <= actual_state(i) + cov_stdev(i)) && (expected_final_state(i) >= actual_state(i) - cov_stdev(i)));
-
-// if(cov_stdev.tail(6).maxCoeff()>=1)
-// WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
-}
-
-//Tests related to noise
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- ::testing::GTEST_FLAG(filter) = "FactorIMU_biasTest_Move_NonNullBiasRot.*:FactorIMU_biasTest_Static_NullBias.*:FactorIMU_biasTest_Static_NonNullAccBias.*:FactorIMU_biasTest_Static_NonNullGyroBias.*";
-// ::testing::GTEST_FLAG(filter) = "FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY.VarB1B2V1V2_InvarP1Q1P2Q2_initOK";
-// ::testing::GTEST_FLAG(filter) = "FactorIMU_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK";
-// ::testing::GTEST_FLAG(filter) = "FactorIMU_biasTest_Move_NonNullBiasRot.VarB1B2V1P2V2_InvarP1Q1Q2_initOK";
-
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_factor_autodiff_apriltag.cpp b/test/gtest_factor_autodiff_apriltag.cpp
deleted file mode 100644
index f88c461d5..000000000
--- a/test/gtest_factor_autodiff_apriltag.cpp
+++ /dev/null
@@ -1,413 +0,0 @@
-#include "utils_gtest.h"
-
-#include "base/wolf.h"
-#include "base/logging.h"
-
-#include "base/ceres_wrapper/ceres_manager.h"
-#include "base/processor/processor_tracker_landmark_apriltag.h"
-#include "base/capture/capture_image.h"
-#include "base/factor/factor_autodiff_apriltag.h"
-#include "base/processor/processor_factory.h"
-
-#include <apriltag.h>
-
-using namespace Eigen;
-using namespace wolf;
-using std::static_pointer_cast;
-
-////////////////////////////////////////////////////////////////
-/*
- * Wrapper class to be able to have setOrigin() and setLast() in ProcessorTrackerLandmarkApriltag
- */
-WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkApriltag_Wrapper);
-class ProcessorTrackerLandmarkApriltag_Wrapper : public ProcessorTrackerLandmarkApriltag
-{
- public:
- ProcessorTrackerLandmarkApriltag_Wrapper(ProcessorParamsTrackerLandmarkApriltagPtr _params_tracker_landmark_apriltag) :
- ProcessorTrackerLandmarkApriltag(_params_tracker_landmark_apriltag)
- {
- setType("TRACKER LANDMARK APRILTAG WRAPPER");
- };
- ~ProcessorTrackerLandmarkApriltag_Wrapper(){}
- void setOriginPtr(const CaptureBasePtr _origin_ptr) { origin_ptr_ = _origin_ptr; }
- void setLastPtr (const CaptureBasePtr _last_ptr) { last_ptr_ = _last_ptr; }
- void setIncomingPtr (const CaptureBasePtr _incoming_ptr) { incoming_ptr_ = _incoming_ptr; }
-
- // for factory
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr)
- {
- std::shared_ptr<ProcessorParamsTrackerLandmarkApriltag> prc_apriltag_params_;
- if (_params)
- prc_apriltag_params_ = std::static_pointer_cast<ProcessorParamsTrackerLandmarkApriltag>(_params);
- else
- prc_apriltag_params_ = std::make_shared<ProcessorParamsTrackerLandmarkApriltag>();
-
- ProcessorTrackerLandmarkApriltag_WrapperPtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkApriltag_Wrapper>(prc_apriltag_params_);
- prc_ptr->setName(_unique_name);
- return prc_ptr;
- }
-
-};
-namespace wolf{
-// Register in the Factories
-WOLF_REGISTER_PROCESSOR("TRACKER LANDMARK APRILTAG WRAPPER", ProcessorTrackerLandmarkApriltag_Wrapper);
-}
-////////////////////////////////////////////////////////////////
-
-// Use the following in case you want to initialize tests with predefines variables or methods.
-class FactorAutodiffApriltag_class : public testing::Test{
- public:
- Vector3s pos_camera, pos_robot, pos_landmark;
- Vector3s euler_camera, euler_robot, euler_landmark;
- Quaternions quat_camera, quat_robot, quat_landmark;
- Vector4s vquat_camera, vquat_robot, vquat_landmark; // quaternions as vectors
- Vector7s pose_camera, pose_robot, pose_landmark;
-
- ProblemPtr problem;
- CeresManagerPtr ceres_manager;
-
- SensorCameraPtr camera;
- ProcessorTrackerLandmarkApriltag_WrapperPtr proc_apriltag;
-
- SensorBasePtr S;
- FrameBasePtr F1;
- CaptureImagePtr C1;
- FeatureApriltagPtr f1;
- LandmarkApriltagPtr lmk1;
- FactorAutodiffApriltagPtr c_tag;
- apriltag_detection_t det;
-
- virtual void SetUp()
- {
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- // configuration
-
- /* We have three poses to take into account:
- * - pose of the camera (extrinsincs)
- * - pose of the landmark
- * - pose of the robot (Keyframe)
- *
- * The measurement provides the pose of the landmark wrt camera's current pose in the world.
- * Camera's current pose in World is the composition of the robot pose with the camera extrinsics.
- *
- * The robot has a camera looking forward
- * S: pos = (0,0,0), ori = (0, 0, 0)
- *
- * There is a point at the origin
- * P: pos = (0,0,0)
- *
- * The camera is canonical
- * K = Id.
- *
- * Therefore, P projects exactly at the origin of the camera,
- * 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 apriltag constraint c1:
- *
- * Frame F1, Capture C1, feature f1, landmark l1, constraint c1
- *
- * The frame pose F1, and the camera pose S
- * in the robot frame are variables subject to optimization
- *
- * We perform a number of tests based on this configuration.*/
-
-
- // camera is at origin of robot and looking forward
- // robot is at (0,0,0)
- // landmark is right in front of camera. Its orientation wrt camera 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 3D");
- ceres::Solver::Options options;
- ceres_manager = std::make_shared<CeresManager>(problem, options);
-
- // Install sensor and processor
- S = problem->installSensor("CAMERA", "canonical", pose_camera, wolf_root + "/src/examples/camera_params_canonical.yaml");
- camera = std::static_pointer_cast<SensorCamera>(S);
-
- ProcessorParamsTrackerLandmarkApriltagPtr params = std::make_shared<ProcessorParamsTrackerLandmarkApriltag>();
- // Need to set some parameters ? do it now !
- params->tag_family_ = "tag36h11";
- //params->name = params->tag_family_;
-
- ProcessorBasePtr proc = problem->installProcessor("TRACKER LANDMARK APRILTAG WRAPPER", "apriltags_wrapper", camera, params);
- proc_apriltag = std::static_pointer_cast<ProcessorTrackerLandmarkApriltag_Wrapper>(proc);
-
- // F1 is be origin KF
- F1 = problem->setPrior(pose_robot, Matrix6s::Identity(), 0.0, 0.1);
-
- //create feature and landmark
- C1 = std::make_shared<CaptureImage>(1.0, camera, cv::Mat(2,2,CV_8UC1));
- F1-> addCapture(C1);
- proc_apriltag->setOriginPtr(C1);
- proc_apriltag->setLastPtr(C1);
-
- // the sensor is at origin as well as the robot. The measurement matches with the pose of the tag wrt camera (and also wrt robot and world)
- Eigen::Matrix6s meas_cov(Eigen::Matrix6s::Identity());
- meas_cov.topLeftCorner(3,3) *= 1e-2;
- meas_cov.bottomRightCorner(3,3) *= 1e-3;
- int tag_id = 1;
-
- det.id = tag_id;
- det.p[0][0] = 1.0;
- det.p[0][1] = -1.0;
- det.p[1][0] = 1.0;
- det.p[1][1] = 1.0;
- det.p[2][0] = -1.0;
- det.p[2][1] = 1.0;
- det.p[3][0] = -1.0;
- det.p[3][1] = -1.0;
-
- Scalar rep_error1 = 0.01;
- Scalar rep_error2 = 0.1;
- bool use_rotation = true;
-
- f1 = std::make_shared<FeatureApriltag>(pose_landmark, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
- lmk1 = std::static_pointer_cast<LandmarkApriltag>(proc_apriltag->createLandmark(f1));
-
- // Add the feature and the landmark in the graph as needed
- C1->addFeature(f1); // add feature to capture
- problem->addLandmark(lmk1); // add landmark to map
- }
-};
-
-
-TEST_F(FactorAutodiffApriltag_class, Constructor)
-{
- FactorAutodiffApriltagPtr constraint = std::make_shared<FactorAutodiffApriltag>(
- S,
- F1,
- lmk1,
- f1,
- false,
- CTR_ACTIVE
- );
-
- ASSERT_TRUE(constraint->getType() == "AUTODIFF APRILTAG");
-}
-
-TEST_F(FactorAutodiffApriltag_class, Check_tree)
-{
- FactorAutodiffApriltagPtr constraint = std::make_shared<FactorAutodiffApriltag>(
- S,
- F1,
- lmk1,
- f1,
- false,
- CTR_ACTIVE
- );
-
- FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint));
- lmk1->addConstrainedBy(constraint);
- //check is returning true even without the lines below....
- WOLF_WARN("I think the lines below are needed... to be checked !")
- F1->addConstrainedBy(constraint);
- f1->addConstrainedBy(constraint);
-
- ASSERT_TRUE(problem->check(0));
-}
-
-TEST_F(FactorAutodiffApriltag_class, solve_F1_P_perturbated)
-{
- FactorAutodiffApriltagPtr constraint = std::make_shared<FactorAutodiffApriltag>(
- S,
- F1,
- lmk1,
- f1,
- false,
- CTR_ACTIVE
- );
-
- FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint));
- lmk1->addConstrainedBy(constraint);
- F1->addConstrainedBy(constraint);
- f1->addConstrainedBy(constraint);
-
- // unfix F1, perturbate state
- F1->unfix();
- Vector3s p0 = Vector3s::Random() * 0.25;
-// WOLF_DEBUG("Perturbation: ")
-// WOLF_DEBUG(p0.transpose());
- Vector7s x0(pose_robot);
-
- x0.head<3>() += p0;
- WOLF_DEBUG("State before perturbation: ");
- WOLF_DEBUG(F1->getState().transpose());
- F1->setState(x0);
-// WOLF_DEBUG("State after perturbation: ");
-// WOLF_DEBUG(F1->getState().transpose());
-
-// solve
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
-// WOLF_DEBUG("State after solve: ");
-// WOLF_DEBUG(F1->getState().transpose());
- ASSERT_MATRIX_APPROX(F1->getState(), pose_robot, 1e-6);
-
-}
-
-TEST_F(FactorAutodiffApriltag_class, solve_F1_O_perturbated)
-{
- FactorAutodiffApriltagPtr constraint = std::make_shared<FactorAutodiffApriltag>(
- S,
- F1,
- lmk1,
- f1,
- false,
- CTR_ACTIVE
- );
-
- FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint));
- lmk1->addConstrainedBy(constraint);
- F1->addConstrainedBy(constraint);
- f1->addConstrainedBy(constraint);
-
- // unfix F1, perturbate state
- F1->unfix();
- Vector3s e0 = euler_robot + Vector3s::Random() * 0.25;
- Quaternions e0_quat = e2q(e0);
- Vector4s e0_vquat = e0_quat.coeffs();
-// WOLF_DEBUG("Perturbation: ")
-// WOLF_DEBUG(e0.transpose());
- Vector7s x0(pose_robot);
-
- x0.tail<4>() = e0_vquat;
- WOLF_DEBUG("State before perturbation: ");
- WOLF_DEBUG(F1->getState().transpose());
- F1->setState(x0);
-// WOLF_DEBUG("State after perturbation: ");
-// WOLF_DEBUG(F1->getState().transpose());
-
-// solve
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
-// WOLF_DEBUG("State after solve: ");
-// WOLF_DEBUG(F1->getState().transpose());
- ASSERT_MATRIX_APPROX(F1->getState(), pose_robot, 1e-6);
-
-}
-
-TEST_F(FactorAutodiffApriltag_class, Check_initialization)
-{
- FactorAutodiffApriltagPtr constraint = std::make_shared<FactorAutodiffApriltag>(
- S,
- F1,
- lmk1,
- f1,
- false,
- CTR_ACTIVE
- );
-
- FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint));
- lmk1->addConstrainedBy(constraint);
- F1->addConstrainedBy(constraint);
- f1->addConstrainedBy(constraint);
-
- ASSERT_MATRIX_APPROX(F1->getState(), pose_robot, 1e-6);
- ASSERT_MATRIX_APPROX(f1->getMeasurement(), pose_landmark, 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getState(), pose_landmark, 1e-6);
-
-}
-
-TEST_F(FactorAutodiffApriltag_class, solve_L1_P_perturbated)
-{
- FactorAutodiffApriltagPtr constraint = std::make_shared<FactorAutodiffApriltag>(
- S,
- F1,
- lmk1,
- f1,
- false,
- CTR_ACTIVE
- );
-
- FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint));
- lmk1->addConstrainedBy(constraint);
- F1->addConstrainedBy(constraint);
- f1->addConstrainedBy(constraint);
-
-
- // unfix lmk1, perturbate state
- lmk1->unfix();
- Vector3s p0 = Vector3s::Random() * 0.25;
-// WOLF_DEBUG("Perturbation: ")
-// WOLF_DEBUG(p0.transpose());
- Vector7s x0(pose_landmark);
-
- x0.head<3>() += p0;
- //WOLF_DEBUG("Landmark state before perturbation: ");
- //WOLF_DEBUG(lmk1->getState().transpose());
- lmk1->getP()->setState(x0.head<3>());
- //WOLF_DEBUG("Landmark state after perturbation: ");
- //WOLF_DEBUG(lmk1->getState().transpose());
-
-// solve
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- //WOLF_DEBUG("Landmark state after solve: ");
- //WOLF_DEBUG(lmk1->getState().transpose());
- ASSERT_MATRIX_APPROX(F1->getState(), pose_robot, 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getState(), pose_landmark, 1e-6);
-}
-
-TEST_F(FactorAutodiffApriltag_class, solve_L1_O_perturbated)
-{
- FactorAutodiffApriltagPtr constraint = std::make_shared<FactorAutodiffApriltag>(
- S,
- F1,
- lmk1,
- f1,
- false,
- CTR_ACTIVE
- );
-
- FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint));
- lmk1->addConstrainedBy(constraint);
- F1->addConstrainedBy(constraint);
- f1->addConstrainedBy(constraint);
-
- // unfix F1, perturbate state
- lmk1->unfix();
- Vector3s e0 = euler_landmark + Vector3s::Random() * 0.25;
- Quaternions e0_quat = e2q(e0);
- Vector4s e0_vquat = e0_quat.coeffs();
-// WOLF_DEBUG("Perturbation: ")
-// WOLF_DEBUG(e0.transpose());
-
- //WOLF_DEBUG("Landmark state before perturbation: ");
- //WOLF_DEBUG(lmk1->getState().transpose());
- lmk1->getO()->setState(e0_vquat);
- //WOLF_DEBUG("Landmark state after perturbation: ");
- //WOLF_DEBUG(lmk1->getState().transpose());
-
-// solve
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
- //WOLF_DEBUG("Landmark state after solve: ");
- //WOLF_DEBUG(lmk1->getState().transpose());
- ASSERT_MATRIX_APPROX(F1->getState(), pose_robot, 1e-6);
- ASSERT_MATRIX_APPROX(lmk1->getState(), pose_landmark, 1e-6);
-
-}
-
-//[Class methods]
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_factor_autodiff_trifocal.cpp b/test/gtest_factor_autodiff_trifocal.cpp
deleted file mode 100644
index 98af13f6e..000000000
--- a/test/gtest_factor_autodiff_trifocal.cpp
+++ /dev/null
@@ -1,948 +0,0 @@
-#include "utils_gtest.h"
-
-#include "base/utils/logging.h"
-
-#include "base/ceres_wrapper/ceres_manager.h"
-#include "base/processor/processor_tracker_feature_trifocal.h"
-#include "base/capture/capture_image.h"
-#include "base/factor/factor_autodiff_trifocal.h"
-
-using namespace Eigen;
-using namespace wolf;
-
-class FactorAutodiffTrifocalTest : public testing::Test{
- public:
- Vector3s pos1, pos2, pos3, pos_cam, point;
- Vector3s euler1, euler2, euler3, euler_cam;
- Quaternions quat1, quat2, quat3, quat_cam;
- Vector4s vquat1, vquat2, vquat3, vquat_cam; // quaternions as vectors
- Vector7s pose1, pose2, pose3, pose_cam;
-
- ProblemPtr problem;
- CeresManagerPtr ceres_manager;
-
- SensorCameraPtr camera;
- ProcessorTrackerFeatureTrifocalPtr proc_trifocal;
-
- SensorBasePtr S;
- FrameBasePtr F1, F2, F3;
- CaptureImagePtr I1, I2, I3;
- FeatureBasePtr f1, f2, f3;
- FactorAutodiffTrifocalPtr c123;
-
- virtual void SetUp() override
- {
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- // configuration
- /*
- * We have three robot poses, in three frames, with cameras C1, C2, C3
- * looking towards the origin of coordinates:
- *
- * Z
- * |
- * ________ C3
- * / | /
- * --------- /| C2
- * | / |
- * |____________/_ | ___ Y
- * / | /
- * / | /
- * --------- C1 |/
- * | / |
- * ---------
- * /
- * Y
- *
- * Each robot pose is at one axis, facing the origin:
- * F1: pos = (1,0,0), ori = (0,0,pi)
- * F2: pos = (0,1,0), ori = (0,0,-pi/2)
- * F3: pos = (0,0,1), ori = (0,pi/2,pi)
- *
- * The robot has a camera looking forward
- * S: pos = (0,0,0), ori = (-pi/1, 0, -pi/1)
- *
- * There is a point at the origin
- * P: pos = (0,0,0)
- *
- * The camera is canonical
- * K = Id.
- *
- * Therefore, P projects exactly at the origin on each camera,
- * creating three features:
- * f1: p1 = (0,0)
- * f2: p2 = (0,0)
- * f3: p3 = (0,0)
- *
- * We form a Wolf tree with three frames, three captures,
- * three features, and one trifocal factor:
- *
- * Frame F1, Capture C1, feature f1
- * Frame F2, Capture C2, feature f2
- * Frame F3, Capture C3, feature f3, factor c123
- *
- * The three frame poses F1, F2, F3 and the camera pose S
- * in the robot frame are variables subject to optimization
- *
- * We perform a number of tests based on this configuration.
- */
-
- // all frames look to the origin
- pos1 << 1, 0, 0;
- pos2 << 0, 1, 0;
- pos3 << 0, 0, 1;
- euler1 << 0, 0 , M_PI ;
- euler2 << 0, 0 , -M_PI_2 ;
- euler3 << 0, M_PI_2, M_PI ;
- quat1 = e2q(euler1);
- quat2 = e2q(euler2);
- quat3 = e2q(euler3);
- vquat1 = quat1.coeffs();
- vquat2 = quat2.coeffs();
- vquat3 = quat3.coeffs();
- pose1 << pos1, vquat1;
- pose2 << pos2, vquat2;
- pose3 << pos3, vquat3;
-
- // camera at the robot origin looking forward
- pos_cam << 0, 0, 0;
- euler_cam << -M_PI_2, 0, -M_PI_2;// euler_cam *= M_TORAD;
- quat_cam = e2q(euler_cam);
- vquat_cam = quat_cam.coeffs();
- pose_cam << pos_cam, vquat_cam;
-
- // Build problem
- problem = Problem::create("PO 3D");
- ceres::Solver::Options options;
- ceres_manager = std::make_shared<CeresManager>(problem, options);
-
- // Install sensor and processor
- S = problem->installSensor("CAMERA", "canonical", pose_cam, wolf_root + "/src/examples/camera_params_canonical.yaml");
- camera = std::static_pointer_cast<SensorCamera>(S);
-
- ProcessorParamsTrackerFeatureTrifocalPtr params_tracker_feature_trifocal_trifocal = std::make_shared<ProcessorParamsTrackerFeatureTrifocal>();
- params_tracker_feature_trifocal_trifocal->time_tolerance = 1.0/2;
- params_tracker_feature_trifocal_trifocal->max_new_features = 5;
- params_tracker_feature_trifocal_trifocal->min_features_for_keyframe = 5;
- params_tracker_feature_trifocal_trifocal->yaml_file_params_vision_utils = wolf_root + "/src/examples/vision_utils_active_search.yaml";
-
- ProcessorBasePtr proc = problem->installProcessor("TRACKER FEATURE TRIFOCAL", "trifocal", camera, params_tracker_feature_trifocal_trifocal);
- proc_trifocal = std::static_pointer_cast<ProcessorTrackerFeatureTrifocal>(proc);
-
- // Add three viewpoints with frame, capture and feature
- Vector2s pix(0,0);
- Matrix2s pix_cov; pix_cov.setIdentity();
-
- F1 = problem->emplaceFrame(KEY_FRAME, pose1, 1.0);
- I1 = std::make_shared<CaptureImage>(1.0, camera, cv::Mat(2,2,CV_8UC1));
- F1-> addCapture(I1);
- f1 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin
- I1-> addFeature(f1);
-
- F2 = problem->emplaceFrame(KEY_FRAME, pose2, 2.0);
- I2 = std::make_shared<CaptureImage>(2.0, camera, cv::Mat(2,2,CV_8UC1));
- F2-> addCapture(I2);
- f2 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin
- I2-> addFeature(f2);
-
- F3 = problem->emplaceFrame(KEY_FRAME, pose3, 3.0);
- I3 = std::make_shared<CaptureImage>(3.0, camera, cv::Mat(2,2,CV_8UC1));
- F3-> addCapture(I3);
- f3 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin
- I3-> addFeature(f3);
-
- // trifocal factor
- c123 = std::make_shared<FactorAutodiffTrifocal>(f1, f2, f3, proc_trifocal, false, FAC_ACTIVE);
- f3 ->addFactor (c123);
- f1 ->addConstrainedBy(c123);
- f2 ->addConstrainedBy(c123);
- }
-};
-
-TEST_F(FactorAutodiffTrifocalTest, expectation)
-{
- // Homogeneous transform C2 wrt C1
- Matrix4s _c1Hc2; _c1Hc2 <<
- 0, 0, -1, 1,
- 0, 1, 0, 0,
- 1, 0, 0, 1,
- 0, 0, 0, 1;
-
- // rotation and translation
- Matrix3s _c1Rc2 = _c1Hc2.block(0,0,3,3);
- Vector3s _c1Tc2 = _c1Hc2.block(0,3,3,1);
-
- // Essential matrix, ground truth (fwd and bkwd)
- Matrix3s _c1Ec2 = wolf::skew(_c1Tc2) * _c1Rc2;
- Matrix3s _c2Ec1 = _c1Ec2.transpose();
-
- // Expected values
- vision_utils::TrifocalTensor tensor;
- Matrix3s c2Ec1;
- c123->expectation(pos1, quat1, pos2, quat2, pos3, quat3, pos_cam, quat_cam, tensor, c2Ec1);
-
- // check trilinearities
-
- // Elements computed using the tensor
- Matrix3s T0, T1, T2;
- tensor.getLayers(T0,T1,T2);
- Vector3s _m1 (0,0,1),
- _m2 (0,0,1),
- _m3 (0,0,1); // ground truth
- Matrix3s m1Tt = _m1(0)*T0.transpose()
- + _m1(1)*T1.transpose()
- + _m1(2)*T2.transpose();
-
- // Projective line: l = (nx ny dn), with (nx ny): normal vector; dn: distance to origin times norm(nx,ny)
- Vector3s _l2(0,1,0),
- _p2(1,0,0),
- _p3(0,1,0); // ground truth
- Vector3s l2;
- l2 = c2Ec1 * _m1;
-
- // check epipolar lines (check only director vectors for equal direction)
- ASSERT_MATRIX_APPROX(l2/l2(1), _l2/_l2(1), 1e-8);
-
- // check perpendicular lines (check only director vectors for orthogonal direction)
- ASSERT_NEAR(l2(0)*_p2(0) + l2(1)*_p2(1), 0, 1e-8);
-
- // Verify trilinearities
-
- // Point-line-line
- Matrix1s pll = _p3.transpose() * m1Tt * _p2;
- ASSERT_TRUE(pll(0)<1e-5);
-
- // Point-line-point
- Vector3s plp = wolf::skew(_m3) * m1Tt * _p2;
- ASSERT_MATRIX_APPROX(plp, Vector3s::Zero(), 1e-8);
-
- // Point-point-line
- Vector3s ppl = _p3.transpose() * m1Tt * wolf::skew(_m2);
- ASSERT_MATRIX_APPROX(ppl, Vector3s::Zero(), 1e-8);
-
- // Point-point-point
- Matrix3s ppp = wolf::skew(_m3) * m1Tt * wolf::skew(_m2);
- ASSERT_MATRIX_APPROX(ppp, Matrix3s::Zero(), 1e-8);
-
- // check epipolars
- ASSERT_MATRIX_APPROX(c2Ec1/c2Ec1(0,1), _c2Ec1/_c2Ec1(0,1), 1e-8);
-}
-
-TEST_F(FactorAutodiffTrifocalTest, residual)
-{
- vision_utils::TrifocalTensor tensor;
- Matrix3s c2Ec1;
- Vector3s residual;
-
- // Nominal values
- c123->expectation(pos1, quat1, pos2, quat2, pos3, quat3, pos_cam, quat_cam, tensor, c2Ec1);
- residual = c123->residual(tensor, c2Ec1);
-
- ASSERT_MATRIX_APPROX(residual, Vector3s::Zero(), 1e-8);
-}
-
-TEST_F(FactorAutodiffTrifocalTest, error_jacobians)
-{
- vision_utils::TrifocalTensor tensor;
- Matrix3s c2Ec1;
- Vector3s residual, residual_pert;
- Vector3s pix0, pert, pix_pert;
- Scalar epsilon = 1e-8;
-
- // Nominal values
- c123->expectation(pos1, quat1, pos2, quat2, pos3, quat3, pos_cam, quat_cam, tensor, c2Ec1);
- residual = c123->residual(tensor, c2Ec1);
-
- Matrix<Scalar, 3, 3> J_e_m1, J_e_m2, J_e_m3, J_r_m1, J_r_m2, J_r_m3;
- c123->error_jacobians(tensor, c2Ec1, J_e_m1, J_e_m2, J_e_m3);
- J_r_m1 = c123->getSqrtInformationUpper() * J_e_m1;
- J_r_m2 = c123->getSqrtInformationUpper() * J_e_m2;
- J_r_m3 = c123->getSqrtInformationUpper() * J_e_m3;
-
- // numerical jacs
- Matrix<Scalar,3,3> Jn_r_m1, Jn_r_m2, Jn_r_m3;
-
- // jacs wrt m1
- pix0 = c123->getPixelCanonicalPrev();
- for (int i=0; i<3; i++)
- {
- pert.setZero();
- pert(i) = epsilon;
- pix_pert = pix0 + pert;
- c123->setPixelCanonicalPrev(pix_pert); // m1
- residual_pert = c123->residual(tensor, c2Ec1);
-
- Jn_r_m1.col(i) = (residual_pert - residual) / epsilon;
- }
- c123->setPixelCanonicalPrev(pix0);
-
- ASSERT_MATRIX_APPROX(J_r_m1, Jn_r_m1, 1e-6);
-
- // jacs wrt m2
- pix0 = c123->getPixelCanonicalOrigin();
- for (int i=0; i<3; i++)
- {
- pert.setZero();
- pert(i) = epsilon;
- pix_pert = pix0 + pert;
- c123->setPixelCanonicalOrigin(pix_pert); // m2
- residual_pert = c123->residual(tensor, c2Ec1);
-
- Jn_r_m2.col(i) = (residual_pert - residual) / epsilon;
- }
- c123->setPixelCanonicalOrigin(pix0);
-
- ASSERT_MATRIX_APPROX(J_r_m2, Jn_r_m2, 1e-6);
-
- // jacs wrt m3
- pix0 = c123->getPixelCanonicalLast();
- for (int i=0; i<3; i++)
- {
- pert.setZero();
- pert(i) = epsilon;
- pix_pert = pix0 + pert;
- c123->setPixelCanonicalLast(pix_pert); // m3
- residual_pert = c123->residual(tensor, c2Ec1);
-
- Jn_r_m3.col(i) = (residual_pert - residual) / epsilon;
- }
- c123->setPixelCanonicalLast(pix0);
-
- ASSERT_MATRIX_APPROX(J_r_m3, Jn_r_m3, 1e-6);
-
-}
-
-TEST_F(FactorAutodiffTrifocalTest, operator_parenthesis)
-{
- Vector3s res;
-
- // Factor with exact states should give zero residual
- c123->operator ()(pos1.data(), vquat1.data(),
- pos2.data(), vquat2.data(),
- pos3.data(), vquat3.data(),
- pos_cam.data(), vquat_cam.data(),
- res.data());
-
- ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8);
-}
-
-TEST_F(FactorAutodiffTrifocalTest, solve_F1)
-{
- F1->setState(pose1);
- F2->setState(pose2);
- F3->setState(pose3);
- S ->getP()->setState(pos_cam);
- S ->getO()->setState(vquat_cam);
- // Residual with prior
-
- Vector3s res;
-
- Eigen::VectorXs F1_p = F1->getP()->getState();
- Eigen::VectorXs F1_o = F1->getO()->getState();
- Eigen::VectorXs F2_p = F2->getP()->getState();
- Eigen::VectorXs F2_o = F2->getO()->getState();
- Eigen::VectorXs F3_p = F3->getP()->getState();
- Eigen::VectorXs F3_o = F3->getO()->getState();
- Eigen::VectorXs S_p = S ->getP()->getState();
- Eigen::VectorXs S_o = S ->getO()->getState();
-
- c123->operator ()(F1_p.data(), F1_o.data(),
- F2_p.data(), F2_o.data(),
- F3_p.data(), F3_o.data(),
- S_p. data(), S_o. data(),
- res.data());
-
- WOLF_DEBUG("Initial state: ", F1->getState().transpose());
- WOLF_DEBUG("residual before perturbing: ", res.transpose());
- ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8);
-
- // Residual with perturbated state
-
- Vector7s pose_perturbated = F1->getState() + 0.1 * Vector7s::Random();
- pose_perturbated.segment(3,4).normalize();
- F1->setState(pose_perturbated);
-
- F1_p = F1->getP()->getState();
- F1_o = F1->getO()->getState();
-
- c123->operator ()(F1_p.data(), F1_o.data(),
- F2_p.data(), F2_o.data(),
- F3_p.data(), F3_o.data(),
- S_p. data(), S_o. data(),
- res.data());
-
- WOLF_DEBUG("perturbed state: ", pose_perturbated.transpose());
- WOLF_DEBUG("residual before solve: ", res.transpose());
-
- // Residual with solved state
-
- S ->fixExtrinsics();
- F1->unfix();
- F2->fix();
- F3->fix();
-
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
-
- F1_p = F1->getP()->getState();
- F1_o = F1->getO()->getState();
- F2_p = F2->getP()->getState();
- F2_o = F2->getO()->getState();
- F3_p = F3->getP()->getState();
- F3_o = F3->getO()->getState();
- S_p = S ->getP()->getState();
- S_o = S ->getO()->getState();
-
- c123->operator ()(F1_p.data(), F1_o.data(),
- F2_p.data(), F2_o.data(),
- F3_p.data(), F3_o.data(),
- S_p. data(), S_o. data(),
- res.data());
-
- WOLF_DEBUG("solved state: ", F1->getState().transpose());
- WOLF_DEBUG("residual after solve: ", res.transpose());
-
- WOLF_DEBUG(report, " AND UNION");
-
- ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8);
-
-}
-
-TEST_F(FactorAutodiffTrifocalTest, solve_F2)
-{
- F1->setState(pose1);
- F2->setState(pose2);
- F3->setState(pose3);
- S ->getP()->setState(pos_cam);
- S ->getO()->setState(vquat_cam);
-
- // Residual with prior
-
- Vector3s res;
-
- Eigen::VectorXs F1_p = F1->getP()->getState();
- Eigen::VectorXs F1_o = F1->getO()->getState();
- Eigen::VectorXs F2_p = F2->getP()->getState();
- Eigen::VectorXs F2_o = F2->getO()->getState();
- Eigen::VectorXs F3_p = F3->getP()->getState();
- Eigen::VectorXs F3_o = F3->getO()->getState();
- Eigen::VectorXs S_p = S ->getP()->getState();
- Eigen::VectorXs S_o = S ->getO()->getState();
-
- c123->operator ()(F1_p.data(), F1_o.data(),
- F2_p.data(), F2_o.data(),
- F3_p.data(), F3_o.data(),
- S_p. data(), S_o. data(),
- res.data());
-
- WOLF_DEBUG("Initial state: ", F2->getState().transpose());
- WOLF_DEBUG("residual before perturbing: ", res.transpose());
- ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8);
-
- // Residual with perturbated state
-
- Vector7s pose_perturbated = F2->getState() + 0.1 * Vector7s::Random();
- pose_perturbated.segment(3,4).normalize();
- F2->setState(pose_perturbated);
-
- F2_p = F2->getP()->getState();
- F2_o = F2->getO()->getState();
-
- c123->operator ()(F1_p.data(), F1_o.data(),
- F2_p.data(), F2_o.data(),
- F3_p.data(), F3_o.data(),
- S_p. data(), S_o. data(),
- res.data());
-
- WOLF_DEBUG("perturbed state: ", pose_perturbated.transpose());
- WOLF_DEBUG("residual before solve: ", res.transpose());
-
- // Residual with solved state
-
- S ->fixExtrinsics();
- F1->fix();
- F2->unfix();
- F3->fix();
-
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
-
- F1_p = F1->getP()->getState();
- F1_o = F1->getO()->getState();
- F2_p = F2->getP()->getState();
- F2_o = F2->getO()->getState();
- F3_p = F3->getP()->getState();
- F3_o = F3->getO()->getState();
- S_p = S ->getP()->getState();
- S_o = S ->getO()->getState();
-
- c123->operator ()(F1_p.data(), F1_o.data(),
- F2_p.data(), F2_o.data(),
- F3_p.data(), F3_o.data(),
- S_p. data(), S_o. data(),
- res.data());
-
- WOLF_DEBUG("solved state: ", F2->getState().transpose());
- WOLF_DEBUG("residual after solve: ", res.transpose());
-
- WOLF_DEBUG(report, " AND UNION");
-
- ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8);
-
-}
-
-TEST_F(FactorAutodiffTrifocalTest, solve_F3)
-{
- F1->setState(pose1);
- F2->setState(pose2);
- F3->setState(pose3);
- S ->getP()->setState(pos_cam);
- S ->getO()->setState(vquat_cam);
-
- // Residual with prior
-
- Vector3s res;
-
- Eigen::VectorXs F1_p = F1->getP()->getState();
- Eigen::VectorXs F1_o = F1->getO()->getState();
- Eigen::VectorXs F2_p = F2->getP()->getState();
- Eigen::VectorXs F2_o = F2->getO()->getState();
- Eigen::VectorXs F3_p = F3->getP()->getState();
- Eigen::VectorXs F3_o = F3->getO()->getState();
- Eigen::VectorXs S_p = S ->getP()->getState();
- Eigen::VectorXs S_o = S ->getO()->getState();
-
- c123->operator ()(F1_p.data(), F1_o.data(),
- F2_p.data(), F2_o.data(),
- F3_p.data(), F3_o.data(),
- S_p. data(), S_o. data(),
- res.data());
-
- WOLF_DEBUG("Initial state: ", F3->getState().transpose());
- WOLF_DEBUG("residual before perturbing: ", res.transpose());
- ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8);
-
- // Residual with perturbated state
-
- Vector7s pose_perturbated = F3->getState() + 0.1 * Vector7s::Random();
- pose_perturbated.segment(3,4).normalize();
- F3->setState(pose_perturbated);
-
- F3_p = F3->getP()->getState();
- F3_o = F3->getO()->getState();
-
- c123->operator ()(F1_p.data(), F1_o.data(),
- F2_p.data(), F2_o.data(),
- F3_p.data(), F3_o.data(),
- S_p. data(), S_o. data(),
- res.data());
-
- WOLF_DEBUG("perturbed state: ", pose_perturbated.transpose());
- WOLF_DEBUG("residual before solve: ", res.transpose());
- ASSERT_NEAR(res(2), 0, 1e-8); // Epipolar c2-c1 should be respected when perturbing F3
-
- // Residual with solved state
-
- S ->fixExtrinsics();
- F1->fix();
- F2->fix();
- F3->unfix();
-
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
-
- F1_p = F1->getP()->getState();
- F1_o = F1->getO()->getState();
- F2_p = F2->getP()->getState();
- F2_o = F2->getO()->getState();
- F3_p = F3->getP()->getState();
- F3_o = F3->getO()->getState();
- S_p = S ->getP()->getState();
- S_o = S ->getO()->getState();
-
- c123->operator ()(F1_p.data(), F1_o.data(),
- F2_p.data(), F2_o.data(),
- F3_p.data(), F3_o.data(),
- S_p. data(), S_o. data(),
- res.data());
-
- WOLF_DEBUG("solved state: ", F3->getState().transpose());
- WOLF_DEBUG("residual after solve: ", res.transpose());
-
- WOLF_DEBUG(report, " AND UNION");
-
- ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8);
-
-}
-
-TEST_F(FactorAutodiffTrifocalTest, solve_S)
-{
- F1->setState(pose1);
- F2->setState(pose2);
- F3->setState(pose3);
- S ->getP()->setState(pos_cam);
- S ->getO()->setState(vquat_cam);
-
- // Residual with prior
-
- Vector3s res;
-
- Eigen::VectorXs F1_p = F1->getP()->getState();
- Eigen::VectorXs F1_o = F1->getO()->getState();
- Eigen::VectorXs F2_p = F2->getP()->getState();
- Eigen::VectorXs F2_o = F2->getO()->getState();
- Eigen::VectorXs F3_p = F3->getP()->getState();
- Eigen::VectorXs F3_o = F3->getO()->getState();
- Eigen::VectorXs S_p = S ->getP()->getState();
- Eigen::VectorXs S_o = S ->getO()->getState();
-
- c123->operator ()(F1_p.data(), F1_o.data(),
- F2_p.data(), F2_o.data(),
- F3_p.data(), F3_o.data(),
- S_p. data(), S_o. data(),
- res.data());
-
- WOLF_DEBUG("Initial state: ", S->getP()->getState().transpose(), " ", S->getO()->getState().transpose());
- WOLF_DEBUG("residual before perturbing: ", res.transpose());
- ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8);
-
- // Residual with perturbated state
-
- Vector3s pos_perturbated = pos_cam + 0.1 * Vector3s::Random();
- Vector4s ori_perturbated = vquat_cam + 0.1 * Vector4s::Random();
- ori_perturbated.normalize();
- Vector7s pose_perturbated; pose_perturbated << pos_perturbated, ori_perturbated;
- S->getP()->setState(pos_perturbated);
- S->getO()->setState(ori_perturbated);
-
- S_p = S->getP()->getState();
- S_o = S->getO()->getState();
-
- c123->operator ()(F1_p.data(), F1_o.data(),
- F2_p.data(), F2_o.data(),
- F3_p.data(), F3_o.data(),
- S_p. data(), S_o. data(),
- res.data());
-
- WOLF_DEBUG("perturbed state: ", pose_perturbated.transpose());
- WOLF_DEBUG("residual before solve: ", res.transpose());
-
- // Residual with solved state
-
- S ->unfixExtrinsics();
- F1->fix();
- F2->fix();
- F3->fix();
-
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
-
- F1_p = F1->getP()->getState();
- F1_o = F1->getO()->getState();
- F2_p = F2->getP()->getState();
- F2_o = F2->getO()->getState();
- F3_p = F3->getP()->getState();
- F3_o = F3->getO()->getState();
- S_p = S ->getP()->getState();
- S_o = S ->getO()->getState();
-
- c123->operator ()(F1_p.data(), F1_o.data(),
- F2_p.data(), F2_o.data(),
- F3_p.data(), F3_o.data(),
- S_p. data(), S_o. data(),
- res.data());
-
- WOLF_DEBUG("solved state: ", S->getP()->getState().transpose(), " ", S->getO()->getState().transpose());
- WOLF_DEBUG("residual after solve: ", res.transpose());
-
- WOLF_DEBUG(report, " AND UNION");
-
- ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8);
-
-}
-
-class FactorAutodiffTrifocalMultiPointTest : public FactorAutodiffTrifocalTest
-{
- /*
- * In this test class we add 8 more points and perform optimization on the camera frames.
- *
- * C1 is not optimized as it acts as the reference
- * S is not optimized either as observability is compromised
- * C2.pos is fixed to set the unobservable scale
- * C2.ori and all C3 are optimized
- *
- */
-
- public:
- std::vector<FeatureBasePtr> fv1, fv2, fv3;
- std::vector<FactorAutodiffTrifocalPtr> cv123;
-
- virtual void SetUp() override
- {
- FactorAutodiffTrifocalTest::SetUp();
-
- Matrix<Scalar, 2, 9> c1p_can;
- c1p_can <<
- 0, -1/3.00, -1/3.00, 1/3.00, 1/3.00, -1.0000, -1.0000, 1.0000, 1.0000,
- 0, 1/3.00, -1/3.00, 1/3.00, -1/3.00, 1.0000, -1.0000, 1.0000, -1.0000;
-
- Matrix<Scalar, 2, 9> c2p_can;
- c2p_can <<
- 0, 1/3.00, 1/3.00, 1.0000, 1.0000, -1/3.00, -1/3.00, -1.0000, -1.0000,
- 0, 1/3.00, -1/3.00, 1.0000, -1.0000, 1/3.00, -1/3.00, 1.0000, -1.0000;
-
- Matrix<Scalar, 2, 9> c3p_can;
- c3p_can <<
- 0, -1/3.00, -1.0000, 1/3.00, 1.0000, -1/3.00, -1.0000, 1/3.00, 1.0000,
- 0, -1/3.00, -1.0000, -1/3.00, -1.0000, 1/3.00, 1.0000, 1/3.00, 1.0000;
-
- Matrix2s pix_cov; pix_cov.setIdentity(); //pix_cov *= 1e-4;
-
- // for i==0 we already have them
- fv1.push_back(f1);
- fv2.push_back(f2);
- fv3.push_back(f3);
- cv123.push_back(c123);
- for (size_t i=1; i<c1p_can.cols() ; i++)
- {
- fv1.push_back(std::make_shared<FeatureBase>("PIXEL", c1p_can.col(i), pix_cov));
- I1->addFeature(fv1.at(i));
-
- fv2.push_back(std::make_shared<FeatureBase>("PIXEL", c2p_can.col(i), pix_cov));
- I2->addFeature(fv2.at(i));
-
- fv3.push_back(std::make_shared<FeatureBase>("PIXEL", c3p_can.col(i), pix_cov));
- I3->addFeature(fv3.at(i));
-
- cv123.push_back(std::make_shared<FactorAutodiffTrifocal>(fv1.at(i), fv2.at(i), fv3.at(i), proc_trifocal, false, FAC_ACTIVE));
- fv3.at(i)->addFactor(cv123.at(i));
- fv1.at(i)->addConstrainedBy(cv123.at(i));
- fv2.at(i)->addConstrainedBy(cv123.at(i));
- }
-
- }
-
-};
-
-TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point)
-{
- /*
- * In this test we add 8 more points and perform optimization on the camera frames.
- *
- * C1 is not optimized as it acts as the reference
- * S is not optimized either as observability is compromised
- * C2.pos is fixed to set the unobservable scale
- * C2.ori and all C3 are optimized
- *
- */
-
- S ->getP()->fix(); // do not calibrate sensor pos
- S ->getO()->fix(); // do not calibrate sensor ori
- F1->getP()->fix(); // Cam 1 acts as reference
- F1->getO()->fix(); // Cam 1 acts as reference
- F2->getP()->fix(); // This fixes the scale
- F2->getO()->unfix(); // Estimate Cam 2 ori
- F3->getP()->unfix(); // Estimate Cam 3 pos
- F3->getO()->unfix(); // Estimate Cam 3 ori
-
- // Perturbate states, keep scale
- F1->getP()->setState( pos1 );
- F1->getO()->setState( vquat1 );
- F2->getP()->setState( pos2 ); // this fixes the scale
- F2->getO()->setState((vquat2 + 0.2*Vector4s::Random()).normalized());
- F3->getP()->setState( pos3 + 0.2*Vector3s::Random());
- F3->getO()->setState((vquat3 + 0.2*Vector4s::Random()).normalized());
-
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
-
- // Print results
- WOLF_DEBUG("report: ", report);
- problem->print(1,0,1,0);
-
- // Evaluate final states
- ASSERT_MATRIX_APPROX(F2->getP()->getState(), pos2 , 1e-10);
- ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-10);
- ASSERT_MATRIX_APPROX(F3->getP()->getState(), pos3 , 1e-10);
- ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-10);
-
- Eigen::VectorXs F1_p = F1->getP()->getState();
- Eigen::VectorXs F1_o = F1->getO()->getState();
- Eigen::VectorXs F2_p = F2->getP()->getState();
- Eigen::VectorXs F2_o = F2->getO()->getState();
- Eigen::VectorXs F3_p = F3->getP()->getState();
- Eigen::VectorXs F3_o = F3->getO()->getState();
- Eigen::VectorXs S_p = S ->getP()->getState();
- Eigen::VectorXs S_o = S ->getO()->getState();
-
- // evaluate residuals
- Vector3s res;
- for (size_t i=0; i<cv123.size(); i++)
- {
- cv123.at(i)->operator ()(F1_p.data(), F1_o.data(),
- F2_p.data(), F2_o.data(),
- F3_p.data(), F3_o.data(),
- S_p. data(), S_o. data(),
- res.data());
-
- ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-10);
- }
-
-}
-
-TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_scale)
-{
- /*
- * In this test we add 8 more points and perform optimization on the camera frames.
- *
- * C1 is not optimized as it acts as the reference
- * S is not optimized either as observability is compromised
- * C2.pos is fixed to set the unobservable scale
- * C2.ori and all C3 are optimized
- *
- */
-
- S ->getP()->fix(); // do not calibrate sensor pos
- S ->getO()->fix(); // do not calibrate sensor ori
- F1->getP()->fix(); // Cam 1 acts as reference
- F1->getO()->fix(); // Cam 1 acts as reference
- F2->getP()->fix(); // This fixes the scale
- F2->getO()->unfix(); // Estimate Cam 2 ori
- F3->getP()->unfix(); // Estimate Cam 3 pos
- F3->getO()->unfix(); // Estimate Cam 3 ori
-
- // Perturbate states, change scale
- F1->getP()->setState( 2 * pos1 );
- F1->getO()->setState( vquat1 );
- F2->getP()->setState( 2 * pos2 );
- F2->getO()->setState(( vquat2 + 0.2*Vector4s::Random()).normalized());
- F3->getP()->setState( 2 * pos3 + 0.2*Vector3s::Random());
- F3->getO()->setState(( vquat3 + 0.2*Vector4s::Random()).normalized());
-
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
-
- // Print results
- WOLF_DEBUG("report: ", report);
- problem->print(1,0,1,0);
-
- // Evaluate final states
- ASSERT_MATRIX_APPROX(F2->getP()->getState(), 2 * pos2, 1e-8);
- ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-8);
- ASSERT_MATRIX_APPROX(F3->getP()->getState(), 2 * pos3, 1e-8);
- ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-8);
-
- Eigen::VectorXs F1_p = F1->getP()->getState();
- Eigen::VectorXs F1_o = F1->getO()->getState();
- Eigen::VectorXs F2_p = F2->getP()->getState();
- Eigen::VectorXs F2_o = F2->getO()->getState();
- Eigen::VectorXs F3_p = F3->getP()->getState();
- Eigen::VectorXs F3_o = F3->getO()->getState();
- Eigen::VectorXs S_p = S ->getP()->getState();
- Eigen::VectorXs S_o = S ->getO()->getState();
-
- // evaluate residuals
- Vector3s res;
- for (size_t i=0; i<cv123.size(); i++)
- {
- cv123.at(i)->operator ()(F1_p.data(), F1_o.data(),
- F2_p.data(), F2_o.data(),
- F3_p.data(), F3_o.data(),
- S_p. data(), S_o. data(),
- res.data());
-
- ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8);
- }
-}
-
-#include "base/factor/factor_autodiff_distance_3D.h"
-
-TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
-{
- /*
- * In this test we add 8 more points and perform optimization on the camera frames.
- *
- * C1 is not optimized as it acts as the reference
- * S is not optimized either as observability is compromised
- * C2 and C3 are optimized
- * The scale is observed through a distance factor
- *
- */
-
- S ->getP()->fix(); // do not calibrate sensor pos
- S ->getO()->fix(); // do not calibrate sensor ori
- F1->getP()->fix(); // Cam 1 acts as reference
- F1->getO()->fix(); // Cam 1 acts as reference
- F2->getP()->unfix(); // Estimate Cam 2 pos
- F2->getO()->unfix(); // Estimate Cam 2 ori
- F3->getP()->unfix(); // Estimate Cam 3 pos
- F3->getO()->unfix(); // Estimate Cam 3 ori
-
- // Perturbate states, change scale
- F1->getP()->setState( pos1 );
- F1->getO()->setState( vquat1 );
- F2->getP()->setState( pos2 + 0.2*Vector3s::Random() );
- F2->getO()->setState((vquat2 + 0.2*Vector4s::Random()).normalized());
- F3->getP()->setState( pos3 + 0.2*Vector3s::Random());
- F3->getO()->setState((vquat3 + 0.2*Vector4s::Random()).normalized());
-
- // Add a distance factor to fix the scale
- Scalar distance = sqrt(2.0);
- Scalar distance_std = 0.1;
-
- CaptureBasePtr Cd = std::make_shared<CaptureBase>("DISTANCE", F3->getTimeStamp());
- F3->addCapture(Cd);
- FeatureBasePtr fd = std::make_shared<FeatureBase>("DISTANCE", Vector1s(distance), Matrix1s(distance_std * distance_std));
- Cd->addFeature(fd);
- FactorAutodiffDistance3DPtr cd = std::make_shared<FactorAutodiffDistance3D>(fd, F1, nullptr, false, FAC_ACTIVE);
- fd->addFactor(cd);
- F1->addConstrainedBy(cd);
-
- cd->setStatus(FAC_INACTIVE);
- std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
- WOLF_DEBUG("DISTANCE CONSTRAINT INACTIVE: \n", report);
-
- problem->print(1,0,1,0);
-
- cd->setStatus(FAC_ACTIVE);
- report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
-
- // Print results
- WOLF_DEBUG("DISTANCE CONSTRAINT ACTIVE: \n", report);
- problem->print(1,0,1,0);
-
- // Evaluate final states
- ASSERT_MATRIX_APPROX(F2->getP()->getState(), pos2 , 1e-8);
- ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-8);
- ASSERT_MATRIX_APPROX(F3->getP()->getState(), pos3 , 1e-8);
- ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-8);
-
- Eigen::VectorXs F1_p = F1->getP()->getState();
- Eigen::VectorXs F1_o = F1->getO()->getState();
- Eigen::VectorXs F2_p = F2->getP()->getState();
- Eigen::VectorXs F2_o = F2->getO()->getState();
- Eigen::VectorXs F3_p = F3->getP()->getState();
- Eigen::VectorXs F3_o = F3->getO()->getState();
- Eigen::VectorXs S_p = S ->getP()->getState();
- Eigen::VectorXs S_o = S ->getO()->getState();
-
- // evaluate residuals
- Vector3s res;
- for (size_t i=0; i<cv123.size(); i++)
- {
- cv123.at(i)->operator ()(F1_p.data(), F1_o.data(),
- F2_p.data(), F2_o.data(),
- F3_p.data(), F3_o.data(),
- S_p. data(), S_o. data(),
- res.data());
-
- ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8);
- }
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_F1";
- // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_F2";
- // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_F3";
- // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_S";
- // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_multi_point";
- // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalMultiPointTest.solve_multi_point_distance";
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
deleted file mode 100644
index 082e438d0..000000000
--- a/test/gtest_feature_IMU.cpp
+++ /dev/null
@@ -1,167 +0,0 @@
-//Wolf
-#include "base/capture/capture_IMU.h"
-#include "base/processor/processor_IMU.h"
-#include "base/sensor/sensor_IMU.h"
-#include "base/common/wolf.h"
-#include "base/problem/problem.h"
-#include "base/state_block/state_block.h"
-#include "base/state_block/state_quaternion.h"
-#include "utils_gtest.h"
-#include "base/utils/logging.h"
-
-class FeatureIMU_test : public testing::Test
-{
-
- public: //These can be accessed in fixtures
- wolf::ProblemPtr problem;
- wolf::TimeStamp ts;
- wolf::CaptureIMUPtr imu_ptr;
- Eigen::VectorXs state_vec;
- Eigen::VectorXs delta_preint;
- Eigen::Matrix<wolf::Scalar,9,9> delta_preint_cov;
- std::shared_ptr<wolf::FeatureIMU> feat_imu;
- wolf::FrameBasePtr last_frame;
- wolf::FrameBasePtr origin_frame;
- Eigen::MatrixXs dD_db_jacobians;
- wolf::ProcessorBasePtr processor_ptr_;
-
- //a new of this will be created for each new test
- virtual void SetUp()
- {
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
-
- // Wolf problem
- problem = Problem::create("POV 3D");
- Eigen::VectorXs IMU_extrinsics(7);
- IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
- IntrinsicsIMUPtr sen_imu_params = std::make_shared<IntrinsicsIMU>();
- SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", IMU_extrinsics, sen_imu_params);
- ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
- prc_imu_params->max_time_span = 0.5;
- prc_imu_params->max_buff_length = 10;
- prc_imu_params->dist_traveled = 5;
- prc_imu_params->angle_turned = 0.5;
- processor_ptr_ = problem->installProcessor("IMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
-
- // Time and data variables
- TimeStamp t;
- Eigen::Vector6s data_;
-
- t.set(0);
-
- // Set the origin
- Eigen::VectorXs x0(10);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0; P0.setIdentity(9,9);
- origin_frame = problem->setPrior(x0, P0, 0.0, 0.01);
-
- // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
- // give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
- imu_ptr = std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero());
- imu_ptr->setFrame(origin_frame); //to get ptr to Frm in processorIMU and then get biases
-
- //process data
- data_ << 2, 0, 9.8, 0, 0, 0;
- t.set(0.1);
- // Expected state after one integration
- //x << 0.01,0,0, 0,0,0,1, 0.2,0,0; // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2
- // assign data to capture
- imu_ptr->setData(data_);
- imu_ptr->setTimeStamp(t);
- // process data in capture
- sensor_ptr->process(imu_ptr);
-
- //create Frame
- ts = problem->getProcessorMotion()->getBuffer().get().back().ts_;
- state_vec = problem->getProcessorMotion()->getCurrentState();
- last_frame = std::make_shared<FrameBase>(KEY_FRAME, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3)));
- problem->getTrajectory()->addFrame(last_frame);
-
- //create a feature
- delta_preint = problem->getProcessorMotion()->getMotion().delta_integr_;
- delta_preint_cov = problem->getProcessorMotion()->getMotion().delta_integr_cov_ + MatrixXs::Identity(9,9)*1e-08;
- VectorXs calib_preint = problem->getProcessorMotion()->getBuffer().getCalibrationPreint();
- dD_db_jacobians = problem->getProcessorMotion()->getMotion().jacobian_calib_;
- feat_imu = std::make_shared<FeatureIMU>(delta_preint,
- delta_preint_cov,
- calib_preint,
- dD_db_jacobians,
- imu_ptr);
- feat_imu->setCapture(imu_ptr); //associate the feature to a capture
-
- }
-
- virtual void TearDown()
- {
- // code here will be called just after the test completes
- // ok to through exceptions from here if need be
- /*
- You can do deallocation of resources in TearDown or the destructor routine.
- However, if you want exception handling you must do it only in the TearDown code because throwing an exception
- from the destructor results in undefined behavior.
- The Google assertion macros may throw exceptions in platforms where they are enabled in future releases.
- Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
- */
- }
-};
-
-TEST_F(FeatureIMU_test, check_frame)
-{
- // set variables
- using namespace wolf;
-
- FrameBasePtr left_frame = feat_imu->getFrame();
- wolf::TimeStamp t;
- left_frame->getTimeStamp(t);
- origin_frame->getTimeStamp(ts);
-
- ASSERT_NEAR(t.get(), ts.get(), wolf::Constants::EPS_SMALL) << "t != ts \t t=" << t << "\t ts=" << ts << std::endl;
- ASSERT_TRUE(origin_frame->isKey());
- ASSERT_TRUE(last_frame->isKey());
- ASSERT_TRUE(left_frame->isKey());
-
- wolf::StateBlockPtr origin_pptr, origin_optr, origin_vptr, left_pptr, left_optr, left_vptr;
- origin_pptr = origin_frame->getP();
- origin_optr = origin_frame->getO();
- origin_vptr = origin_frame->getV();
- left_pptr = left_frame->getP();
- left_optr = left_frame->getO();
- left_vptr = left_frame->getV();
-
- ASSERT_MATRIX_APPROX(origin_pptr->getState(), left_pptr->getState(), wolf::Constants::EPS_SMALL);
- Eigen::Map<const Eigen::Quaternions> origin_Quat(origin_optr->getState().data()), left_Quat(left_optr->getState().data());
- ASSERT_QUATERNION_APPROX(origin_Quat, left_Quat, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(origin_vptr->getState(), left_vptr->getState(), wolf::Constants::EPS_SMALL);
-
- ASSERT_EQ(origin_frame->id(), left_frame->id());
-}
-
-TEST_F(FeatureIMU_test, access_members)
-{
- using namespace wolf;
-
- Eigen::VectorXs delta(10);
- //dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2; dz = 0.5*9.8*0.1^2 = 0.049; dvz = 9.8*0.1 = 0.98
- delta << 0.01,0,0.049, 0,0,0,1, 0.2,0,0.98;
- ASSERT_MATRIX_APPROX(feat_imu->getMeasurement().transpose(), delta.transpose(), wolf::Constants::EPS);
-}
-
-//TEST_F(FeatureIMU_test, addFactor)
-//{
-// using namespace wolf;
-//
-// FrameBasePtr frm_imu = std::static_pointer_cast<FrameBase>(last_frame);
-// auto cap_imu = last_frame->getCaptureList().back();
-// FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, std::static_pointer_cast<CaptureIMU>(cap_imu), processor_ptr_);
-// feat_imu->addFactor(factor_imu);
-// origin_frame->addConstrainedBy(factor_imu);
-//}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_feature_apriltag.cpp b/test/gtest_feature_apriltag.cpp
deleted file mode 100644
index e2610d5be..000000000
--- a/test/gtest_feature_apriltag.cpp
+++ /dev/null
@@ -1,119 +0,0 @@
-/**
- * \file gtest_feature_apriltag.cpp
- *
- * Created on: Dec 22, 2018
- * \author: jsola
- */
-
-
-#include "utils_gtest.h"
-
-#include "base/feature/feature_apriltag.h"
-
-using namespace wolf;
-
-class FeatureApriltag_test : public testing::Test
-{
- public:
- Eigen::Vector7s pose;
- Eigen::Matrix6s cov;
- int tag_id;
- apriltag_detection_t det;
- Scalar rep_error1;
- Scalar rep_error2;
- bool use_rotation;
-
- virtual void SetUp()
- {
- pose << 1,2,3,4,5,6,7;
- cov.setIdentity() * 2.0;
-
- det.id = 1;
- tag_id = det.id;
- det.p[0][0] = 1.0;
- det.p[0][1] = -1.0;
- det.p[1][0] = 1.0;
- det.p[1][1] = 1.0;
- det.p[2][0] = -1.0;
- det.p[2][1] = 1.0;
- det.p[3][0] = -1.0;
- det.p[3][1] = -1.0;
-
- rep_error1 = 0.01;
- rep_error2 = 0.1;
- use_rotation = true;
- }
-};
-
-TEST_F(FeatureApriltag_test, type)
-{
- FeatureApriltag f(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
-
- ASSERT_EQ(f.getType(), "APRILTAG");
-}
-
-TEST_F(FeatureApriltag_test, getTagId)
-{
- FeatureApriltag f(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
-
- ASSERT_EQ(f.getTagId(), 1);
-}
-
-TEST_F(FeatureApriltag_test, getCorners)
-{
- FeatureApriltag f(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
-
- ASSERT_EQ(f.getTagCorners().size(), 4);
-
- ASSERT_EQ(f.getTagCorners()[0].x, 1.0);
- ASSERT_EQ(f.getTagCorners()[0].y, -1.0);
- ASSERT_EQ(f.getTagCorners()[1].x, 1.0);
- ASSERT_EQ(f.getTagCorners()[1].y, 1.0);
- ASSERT_EQ(f.getTagCorners()[2].x, -1.0);
- ASSERT_EQ(f.getTagCorners()[2].y, 1.0);
- ASSERT_EQ(f.getTagCorners()[3].x, -1.0);
- ASSERT_EQ(f.getTagCorners()[3].y, -1.0);
-}
-
-TEST_F(FeatureApriltag_test, getDetection)
-{
- FeatureApriltag f(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
-
- ASSERT_EQ(f.getDetection().id, 1);
-}
-
-TEST_F(FeatureApriltag_test, tagid_detid_equality)
-{
- FeatureApriltag f(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
-
- ASSERT_EQ(f.getDetection().id, f.getTagId());
-}
-
-TEST_F(FeatureApriltag_test, tagCorners_detection_equality)
-{
- FeatureApriltag f(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
-
- for (int i = 0; i<f.getTagCorners().size(); i++)
- {
- ASSERT_EQ(f.getTagCorners()[i].x, f.getDetection().p[i][0]);
- ASSERT_EQ(f.getTagCorners()[i].y, f.getDetection().p[i][1]);
- }
-}
-
-TEST_F(FeatureApriltag_test, getRepErrors)
-{
- FeatureApriltag f(pose, cov, tag_id, det, rep_error1, rep_error2, use_rotation);
-
- Scalar err1 = f.getRepError1();
- Scalar err2 = f.getRepError2();
-
- ASSERT_EQ(err1, rep_error1);
- ASSERT_EQ(err2, rep_error2);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_landmark_apriltag.cpp b/test/gtest_landmark_apriltag.cpp
deleted file mode 100644
index 9c9e88990..000000000
--- a/test/gtest_landmark_apriltag.cpp
+++ /dev/null
@@ -1,87 +0,0 @@
-/**
- * \file gtest_landmark_apriltag.cpp
- *
- * Created on: Dec 6, 2018
- * \author: jsola
- */
-
-
-#include "utils_gtest.h"
-
-
-#include "base/wolf.h"
-#include "base/logging.h"
-
-#include "base/landmark/landmark_apriltag.h"
-
-using namespace Eigen;
-using namespace wolf;
-using std::static_pointer_cast;
-
-class LandmarkApriltag_class : public testing::Test{
- public:
- virtual void SetUp()
- {
- wolf_root = _WOLF_ROOT_DIR;
- problem = Problem::create("PO 3D");
- }
- public:
- std::string wolf_root;
- ProblemPtr problem;
-};
-
-TEST(LandmarkApriltag, getTagId)
-{
- Vector7s p;
- LandmarkApriltagPtr l = std::make_shared<LandmarkApriltag>(p, 5, 0.2); // pose, tag_id, tag_width
- ASSERT_EQ(l->getTagId(), 5);
-}
-
-TEST(LandmarkApriltag, getTagWidth)
-{
- Vector7s p;
- LandmarkApriltagPtr l = std::make_shared<LandmarkApriltag>(p, 5, 0.2); // pose, tag_id, tag_width
- ASSERT_EQ(l->getTagWidth(), 0.2);
-}
-
-TEST(LandmarkApriltag, getPose)
-{
- Vector7s p;
- p << 0,0,0, 0,0,0,1;
- LandmarkApriltagPtr l = std::make_shared<LandmarkApriltag>(p, 5, 0.2); // pose, tag_id, tag_width
- ASSERT_MATRIX_APPROX(l->getState(), p, 1e-6);
-}
-
-TEST_F(LandmarkApriltag_class, create)
-{
- // load original hand-written map
- problem->loadMap(wolf_root + "/src/examples/map_apriltag_1.yaml"); // this will call create()
- ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 4);
- ASSERT_EQ(problem->getMap()->getLandmarkList().front()->getType(), "APRILTAG");
-}
-
-TEST_F(LandmarkApriltag_class, saveToYaml)
-{
- // load original hand-written map
- problem->loadMap(wolf_root + "/src/examples/map_apriltag_1.yaml");
- ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 4);
-
- // write map on new file
- problem->saveMap(wolf_root + "/src/examples/map_apriltag_save.yaml"); // this will call saveToYaml()
-
- // delete existing map
- problem->getMap()->getLandmarkList().clear();
- ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 0);
-
- // reload the saved map
- problem->loadMap(wolf_root + "/src/examples/map_apriltag_save.yaml");
- ASSERT_EQ(problem->getMap()->getLandmarkList().size(), 4);
- ASSERT_EQ(problem->getMap()->getLandmarkList().front()->getType(), "APRILTAG");
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_processor_IMU.cpp b/test/gtest_processor_IMU.cpp
deleted file mode 100644
index 822fc5b81..000000000
--- a/test/gtest_processor_IMU.cpp
+++ /dev/null
@@ -1,1064 +0,0 @@
-/**
- * \file gtest_processor_imu.cpp
- *
- * Created on: Nov 23, 2016
- * \author: jsola
- */
-
-#include "base/capture/capture_IMU.h"
-#include "base/processor/processor_IMU.h"
-#include "base/sensor/sensor_IMU.h"
-#include "base/common/wolf.h"
-
-#include "utils_gtest.h"
-#include "base/utils/logging.h"
-
-#include "base/math/rotations.h"
-#include "base/ceres_wrapper/ceres_manager.h"
-
-#include <cmath>
-#include <iostream>
-
-using namespace Eigen;
-
-class ProcessorIMUt : public testing::Test
-{
-
- public: //These can be accessed in fixtures
- wolf::ProblemPtr problem;
- wolf::SensorBasePtr sensor_ptr;
- wolf::TimeStamp t;
- wolf::Scalar mti_clock, tmp;
- wolf::Scalar dt;
- Vector6s data;
- Matrix6s data_cov;
- VectorXs x0;
- std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr;
-
- //a new of this will be created for each new test
- virtual void SetUp()
- {
- using namespace wolf;
- using namespace Eigen;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
- using namespace wolf::Constants;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- // Wolf problem
- problem = Problem::create("POV 3D");
- Vector7s extrinsics = (Vector7s() << 0,0,0, 0,0,0,1).finished();
- sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
- ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml");
-
- // Time and data variables
- data = Vector6s::Zero();
- data_cov = Matrix6s::Identity();
-
- // Set the origin
- x0.resize(10);
-
- // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
- cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
- }
-
- virtual void TearDown()
- {
- // code here will be called just after the test completes
- // ok to through exceptions from here if need be
- /*
- You can do deallocation of resources in TearDown or the destructor routine.
- However, if you want exception handling you must do it only in the TearDown code because throwing an exception
- from the destructor results in undefined behavior.
- The Google assertion macros may throw exceptions in platforms where they are enabled in future releases.
- Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
- */
- }
-};
-
-TEST(ProcessorIMU_constructors, ALL)
-{
- using namespace wolf;
-
- //constructor with ProcessorIMUParamsPtr argument only
- ProcessorParamsIMUPtr param_ptr = std::make_shared<ProcessorParamsIMU>();
- param_ptr->max_time_span = 2.0;
- param_ptr->max_buff_length = 20000;
- param_ptr->dist_traveled = 2.0;
- param_ptr->angle_turned = 2.0;
-
- ProcessorIMUPtr prc1 = std::make_shared<ProcessorIMU>(param_ptr);
- ASSERT_EQ(prc1->getMaxTimeSpan(), param_ptr->max_time_span);
- ASSERT_EQ(prc1->getMaxBuffLength(), param_ptr->max_buff_length);
- ASSERT_EQ(prc1->getDistTraveled(), param_ptr->dist_traveled);
- ASSERT_EQ(prc1->getAngleTurned(), param_ptr->angle_turned);
-
- //Factory constructor without yaml
- std::string wolf_root = _WOLF_ROOT_DIR;
- ProblemPtr problem = Problem::create("POV 3D");
- Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
- SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
- ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
- ProcessorParamsIMU params_default;
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), params_default.max_time_span);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), params_default.max_buff_length);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), params_default.dist_traveled);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), params_default.angle_turned);
-
- //Factory constructor with yaml
- processor_ptr = problem->installProcessor("IMU", "Sec IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml");
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), 2.0);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), 20000);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), 2.0);
- ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), 0.2);
-}
-
-TEST(ProcessorIMU, voteForKeyFrame)
-{
- using namespace wolf;
- using namespace Eigen;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
- using namespace wolf::Constants;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- // Wolf problem
- ProblemPtr problem = Problem::create("POV 3D");
- Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
- SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
- ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
- prc_imu_params->max_time_span = 1;
- prc_imu_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
- prc_imu_params->dist_traveled = 1000000000;
- prc_imu_params->angle_turned = 1000000000;
- prc_imu_params->voting_active = true;
- ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
-
- //setting origin
- VectorXs x0(10);
- TimeStamp t(0);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- //data variable and covariance matrix
- // since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
- Vector6s data;
- data << 1,0,0, 0,0,0;
- Matrix6s data_cov(Matrix6s::Identity());
- data_cov(0,0) = 0.5;
-
- // Create the captureIMU to store the IMU data arriving from (sensor / callback / file / etc.)
- std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
-
- // Time
- // we want more than one data to integrate otherwise covariance will be 0
- Scalar dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() - 0.1;
- t.set(dt);
- cap_imu_ptr->setTimeStamp(t);
- sensor_ptr->process(cap_imu_ptr);
-
- dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() + 0.1;
- t.set(dt);
- cap_imu_ptr->setTimeStamp(t);
- sensor_ptr->process(cap_imu_ptr);
-
- /*There should be 3 frames :
- - 1 KeyFrame at origin
- - 1 keyframe created by process() in voteForKeyFrame() since conditions to create a keyframe are met
- - 1 frame that would be used by future data
- */
- ASSERT_EQ(problem->getTrajectory()->getFrameList().size(),(unsigned int) 3);
-
- /* if max_time_span == 2, Wolf tree should be
-
- Hardware
- S1
- pm5
- o: C2 - F2
- l: C4 - F3
- Trajectory
- KF1
- Estim, ts=0, x = ( 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0)
- C1 -> S1
- KF2
- Estim, ts=2.1, x = ( 2.2 0 -22 0 0 0 1 2.1 0 -21 0 0 0 0 0 0 )
- C2 -> S1
- f1
- m = ( 2.21 0 0 0 0 0 1 2.1 0 0 )
- c1 --> F1
- F3
- Estim, ts=2.1, x = ( . . .)
- C4 -> S1
- */
- //TODO : ASSERT TESTS to make sure the factors are correctly set + check the tree above
-
-}
-
-//replace TEST by TEST_F if SetUp() needed
-TEST_F(ProcessorIMUt, interpolate)
-{
- using namespace wolf;
-
- t.set(0);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 2, 0, 0, 0, 0, 0; // only acc_x
- cap_imu_ptr->setData(data);
-
- // make one step to depart from origin
- cap_imu_ptr->setTimeStamp(0.05);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_ref = problem->getProcessorMotion()->getMotion();
-
- // make two steps, then pretend it's just one
- cap_imu_ptr->setTimeStamp(0.10);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_int_gt = problem->getProcessorMotion()->getMotion();
-
- cap_imu_ptr->setTimeStamp(0.15);
- sensor_ptr->process(cap_imu_ptr);
- Motion mot_final = problem->getProcessorMotion()->getMotion();
- mot_final.delta_ = mot_final.delta_integr_;
- Motion mot_sec = mot_final;
-
-// problem->getProcessorMotion()->getBuffer().print(0,1,1,0);
-
-class P : public wolf::ProcessorIMU
-{
- public:
- P() : ProcessorIMU(std::make_shared<ProcessorParamsIMU>()) {}
- Motion interp(const Motion& ref, Motion& sec, TimeStamp ts)
- {
- return ProcessorIMU::interpolate(ref, sec, ts);
- }
-} imu;
-
-Motion mot_int = imu.interp(mot_ref, mot_sec, TimeStamp(0.10));
-
-ASSERT_MATRIX_APPROX(mot_int.data_, mot_int_gt.data_, 1e-6);
-//ASSERT_MATRIX_APPROX(mot_int.delta_, mot_int_gt.delta_, 1e-6); // FIXME: delta_p not correctly interpolated
-ASSERT_MATRIX_APPROX(mot_final.delta_integr_, mot_sec.delta_integr_, 1e-6);
-
-}
-
-TEST_F(ProcessorIMUt, acc_x)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.1);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- VectorXs x(10);
- x << 0.01,0,0, 0,0,0,1, 0.2,0,0; // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2
- Vector6s b; b << 0,0,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
-}
-
-TEST_F(ProcessorIMUt, acc_y)
-{
- // last part of this test fails with precision wolf::Constants::EPS_SMALL beacause error is in 1e-12
- // difference hier is that we integrate over 1ms periods
-
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 0, 20, 9.806, 0, 0, 0; // only acc_y, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- VectorXs x(10);
- x << 0,0.00001,0, 0,0,0,1, 0,0.02,0; // advanced at a=20m/s2 during 0.001s ==> dx = 0.5*20*0.001^2 = 0.00001; dvx = 20*0.001 = 0.02
- Vector6s b; b<< 0,0,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- cap_imu_ptr->setTimeStamp(i*0.001 + 0.001); //first one will be 0.002 and last will be 5.000
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // advanced at a=20m/s2 during 1s ==> dx = 0.5*20*1^2 = 10; dvx = 20*1 = 20
- x << 0,10,0, 0,0,0,1, 0,20,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, acc_z)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 0, 0, 9.806 + 2.0, 0, 0, 0; // only acc_z, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.1);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- VectorXs x(10);
- x << 0,0,0.01, 0,0,0,1, 0,0,0.2; // advanced at a=2m/s2 during 0.1s ==> dz = 0.5*2*0.1^2 = 0.01; dvz = 2*0.1 = 0.2
- Vector6s b; b<< 0,0,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 50; //how 0.1s
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- cap_imu_ptr->setTimeStamp(i*0.1 + 0.1); //first one will be 0.2 and last will be 5.0
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // advanced at a=2m/s2 during 5s ==> dz = 0.5*2*5^2 = 25; dvz = 2*5 = 10
- x << 0,0,25, 0,0,0,1, 0,0,10;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, check_Covariance)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.1);
- sensor_ptr->process(cap_imu_ptr);
-
- VectorXs delta_preint(problem->getProcessorMotion()->getMotion().delta_integr_);
-// Matrix<wolf::Scalar,9,9> delta_preint_cov = problem->getProcessorMotion()->getCurrentDeltaPreintCov();
-
- ASSERT_FALSE(delta_preint.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
-// ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXs::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
-}
-
-TEST_F(ProcessorIMUt, gyro_x)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
-{
- // time
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
-
- // bias
- wolf::Scalar abx = 0.002;
- Vector6s bias; bias << abx,0,0, 0,0,0;
- Vector3s acc_bias = bias.head(3);
- // state
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
-
- // init things
- problem->setPrior(x0, P0, t, 0.01);
-
- problem->getProcessorMotion()->getOrigin()->setCalibration(bias);
- problem->getProcessorMotion()->getLast()->setCalibrationPreint(bias);
-// WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
-
- // data
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << acc_bias - wolf::gravity(), rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x_true(10);
- x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
-
- VectorXs x_est(10);
- x_est = problem->getCurrentState().head(10);
-
- ASSERT_MATRIX_APPROX(x_est.transpose() , x_true.transpose(), wolf::Constants::EPS);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias;
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x_true, wolf::Constants::EPS);
-
-}
-
-TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- wolf::Scalar abx(0.002), aby(0.01);
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- Vector6s bias; bias << abx,aby,0, 0,0,0;
- Vector3s acc_bias = bias.head(3);
-
- problem->getProcessorMotion()->getOrigin()->setCalibration(bias);
- problem->getProcessorMotion()->getLast()->setCalibrationPreint(bias);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
-// data << 0+abx, 0+aby, 9.806, rate_of_turn, rate_of_turn*1.5, 0; // measure gravity
- data << acc_bias - wolf::gravity(), rate_of_turn*1.5, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;//, abx,aby,0, 0,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);// << "expected state : " << problem->getCurrentState().transpose()
-// << "\n estimated state : " << x.transpose();
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias;
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);// << "estimated state is : \n" << problem->getCurrentState().transpose() <<
-// "\n expected state is : \n" << x.transpose() << std::endl;
-}
-
-TEST_F(ProcessorIMUt, gyro_z)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 5s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_xyz)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- Vector3s tmp_vec; //will be used to store rate of turn data
- Quaternions quat_comp(Quaternions::Identity());
- Matrix3s R0(Matrix3s::Identity());
- wolf::Scalar time = 0;
- const unsigned int x_rot_vel = 5;
- const unsigned int y_rot_vel = 6;
- const unsigned int z_rot_vel = 13;
-
- wolf::Scalar tmpx, tmpy, tmpz;
-
- /*
- ox oy oz evolution in degrees (for understanding) --> converted in rad
- with * pi/180
- ox = pi*sin(alpha*t*pi/180); %express angle in rad before using sinus
- oy = pi*sin(beta*t*pi/180);
- oz = pi*sin(gamma*t*pi/180);
-
- corresponding rate of turn
- %rate of turn expressed in radians/s
- wx = pi*alpha*cos(alpha*t*pi/180)*pi/180;
- wy = pi*beta*cos(beta*t*pi/180)*pi/180;
- wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
- */
-
- const wolf::Scalar dt = 0.001;
-
- for(unsigned int data_iter = 0; data_iter <= 1000; data_iter ++)
- {
- time += dt;
-
- tmpx = M_PI*x_rot_vel*cos((M_PI/180) * x_rot_vel * time)*M_PI/180;
- tmpy = M_PI*y_rot_vel*cos((M_PI/180) * y_rot_vel * time)*M_PI/180;
- tmpz = M_PI*z_rot_vel*cos((M_PI/180) * z_rot_vel * time)*M_PI/180;
- tmp_vec << tmpx, tmpy, tmpz;
-
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
- R0 = R0 * wolf::v2R(tmp_vec*dt);
- // use processorIMU
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
- data.tail(3) = tmp_vec;
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(time);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- /* We focus on orientation here. position is supposed not to have moved
- * we integrated using 2 ways :
- - a direct quaternion composition q = q (x) q(w*dt)
- - using methods implemented in processorIMU
-
- We check one against the other
- */
-
- // validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
- Quaternions R2quat(wolf::v2q(wolf::R2v(R0)));
- Vector4s quat_comp_vec((Vector4s() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
- Vector4s R2quat_vec((Vector4s() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
-
- ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS);// << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
-
- VectorXs x(10);
- x << 0,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0,0,0;
-
- Quaternions result_quat(problem->getCurrentState().data() + 3);
- //std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
-
- //check position part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), wolf::Constants::EPS);
-
- //check orientation part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , wolf::Constants::EPS);
-
- //check velocity and bias parts
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002m
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 5.000
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
- x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 1; i < iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity());
-
- cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
-{
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- Vector3s tmp_vec; //will be used to store rate of turn data
- Quaternions quat_comp(Quaternions::Identity());
- Matrix3s R0(Matrix3s::Identity());
- wolf::Scalar time = 0;
- const unsigned int x_rot_vel = 5;
- const unsigned int y_rot_vel = 6;
- const unsigned int z_rot_vel = 13;
-
- wolf::Scalar tmpx, tmpy, tmpz;
-
- /*
- ox oy oz evolution in degrees (for understanding) --> converted in rad
- with * pi/180
- ox = pi*sin(alpha*t*pi/180); %express angle in rad before using sinus
- oy = pi*sin(beta*t*pi/180);
- oz = pi*sin(gamma*t*pi/180);
-
- corresponding rate of turn
- %rate of turn expressed in radians/s
- wx = pi*alpha*cos(alpha*t*pi/180)*pi/180;
- wy = pi*beta*cos(beta*t*pi/180)*pi/180;
- wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
- */
-
- const wolf::Scalar dt = 0.001;
-
- for(unsigned int data_iter = 0; data_iter < 1000; data_iter ++)
- {
- time += dt;
-
- tmpx = M_PI*x_rot_vel*cos((M_PI/180) * x_rot_vel * time)*M_PI/180;
- tmpy = M_PI*y_rot_vel*cos((M_PI/180) * y_rot_vel * time)*M_PI/180;
- tmpz = M_PI*z_rot_vel*cos((M_PI/180) * z_rot_vel * time)*M_PI/180;
- tmp_vec << tmpx, tmpy, tmpz;
-
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
- R0 = R0 * wolf::v2R(tmp_vec*dt);
- // use processorIMU
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
- data.tail(3) = tmp_vec;
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(time);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- /* We focus on orientation here. position is supposed not to have moved
- * we integrated using 2 ways :
- - a direct quaternion composition q = q (x) q(w*dt)
- - using methods implemented in processorIMU
-
- We check one against the other
- */
-
- // validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
- Quaternions R2quat(wolf::v2q(wolf::R2v(R0)));
- Vector4s quat_comp_vec((Vector4s() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
- Vector4s R2quat_vec((Vector4s() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
-
- ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS); // << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
-
- VectorXs x(10);
- //rotation + translation due to initial velocity
- x << 2,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 2,0,0;
-
- Quaternions result_quat(problem->getCurrentState().data() + 3);
- //std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
-
- //check position part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), wolf::Constants::EPS);
-
- //check orientation part
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , wolf::Constants::EPS);
-
- //check velocity
- ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS);
-
-}
-
-TEST_F(ProcessorIMUt, gyro_x_acc_x)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 1, 0, -wolf::gravity()(2), rate_of_turn, 0, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on x axis
- // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on x axis
- // v = a*dt = 0.001
- x << 0.0000005,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0.001,0,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() <<
-// "\n current x is : \n" << x.transpose() << std::endl;
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 2; i <= iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<1,0,0).finished();
-
- cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on x axis
- // v = a*dt = 1
- x << 0.5,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 1,0,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_y_acc_y)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 1, -wolf::gravity()(2), 0, rate_of_turn, 0; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on y axis
- // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on y axis
- // v = a*dt = 0.001
- x << 0,0.0000005,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0.001,0;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() <<
-// "\n current x is : \n" << x.transpose() << std::endl;
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 2; i <= iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,1,0).finished();
-
- cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on y axis
- // v = a*dt = 1
- x << 0,0.5,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,1,0;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-TEST_F(ProcessorIMUt, gyro_z_acc_z)
-{
- wolf::Scalar dt(0.001);
- t.set(0); // clock in 0,1 ms ticks
- x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
- problem->setPrior(x0, P0, t, 0.01);
-
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
- data << 0, 0, -wolf::gravity()(2) + 1, 0, 0, rate_of_turn; // measure gravity
-
- cap_imu_ptr->setData(data);
- cap_imu_ptr->setTimeStamp(0.001);
- sensor_ptr->process(cap_imu_ptr);
-
- // Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- VectorXs x(10);
- // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on z axis
- // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on z axis
- // v = a*dt = 0.001
- x << 0,0,0.0000005, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0.001;
-
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-
- //do so for 1s
- const unsigned int iter = 1000; //how many ms
- for(unsigned int i = 2; i <= iter; i++) //already did one integration above
- {
- // quaternion composition
- quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
-
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,0,1).finished();
-
- cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
- cap_imu_ptr->setData(data);
- sensor_ptr->process(cap_imu_ptr);
- }
-
- // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on z axis
- // v = a*dt = 1
- x << 0,0,0.5, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,1;
- ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- //::testing::GTEST_FLAG(filter) = "ProcessorIMUt.check_Covariance";
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_processor_IMU_jacobians.cpp b/test/gtest_processor_IMU_jacobians.cpp
deleted file mode 100644
index 627549043..000000000
--- a/test/gtest_processor_IMU_jacobians.cpp
+++ /dev/null
@@ -1,550 +0,0 @@
-/**
- * \file gtest_imu_preintegration_jacobians.cpp
- *
- * Created on: Nov 29, 2016
- * \author: AtDinesh
- */
-
- //Wolf
-#include "base/capture/capture_IMU.h"
-#include "base/sensor/sensor_IMU.h"
-#include "test/processor_IMU_UnitTester.h"
-#include "base/common/wolf.h"
-#include "base/problem/problem.h"
-#include "base/state_block/state_block.h"
-#include "base/state_block/state_quaternion.h"
-#include <iostream>
-#include <fstream>
-#include <iomanip>
-#include <ctime>
-#include <cmath>
-
-//google test
-#include "utils_gtest.h"
-
-//#define DEBUG_RESULTS
-//#define WRITE_RESULTS
-
-using namespace wolf;
-
-// A new one of these is created for each test
-class ProcessorIMU_jacobians : public testing::Test
-{
- public:
- TimeStamp t;
- Eigen::Vector6s data_;
- Eigen::Matrix<wolf::Scalar,10,1> Delta0;
- wolf::Scalar ddelta_bias;
- wolf::Scalar dt;
- Eigen::Matrix<wolf::Scalar,9,1> Delta_noise;
- Eigen::Matrix<wolf::Scalar,9,1> delta_noise;
- struct IMU_jac_bias bias_jac;
- struct IMU_jac_deltas deltas_jac;
-
- void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq0, Eigen::Map<Eigen::Quaternions>& _dq0){
-
- new (&_Dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta0_.data() + 3);
- new (&_dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta0_.data() + 3);
- }
-
- void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq, Eigen::Map<Eigen::Quaternions>& _dq, const int& place ){
-
- assert(place < _jac_delta.Delta_noisy_vect_.size());
- new (&_Dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
- new (&_dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta_noisy_vect_(place).data() + 3);
- }
-
- virtual void SetUp()
- {
- //SetUp for jacobians
- wolf::Scalar deg_to_rad = M_PI/180.0;
- data_ << 10,0.5,3, 100*deg_to_rad,110*deg_to_rad,30*deg_to_rad;
-
- // Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create("POV 3D");
- Eigen::VectorXs IMU_extrinsics(7);
- IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
-
- ProcessorIMU_UnitTester processor_imu;
- ddelta_bias = 0.00000001;
- dt = 0.001;
-
- //defining a random Delta to begin with (not to use Origin point)
- Eigen::Matrix<wolf::Scalar,10,1> Delta0;
- Delta0 = Eigen::Matrix<wolf::Scalar,10,1>::Random();
- Delta0.head<3>() = Delta0.head<3>()*100;
- Delta0.tail<3>() = Delta0.tail<3>()*10;
- Eigen::Vector3s ang0, ang;
- ang0 << 120.08*deg_to_rad, 12.36*deg_to_rad, 54.32*deg_to_rad;
-
- Eigen::Map<Eigen::Quaternions> Delta0_quat(Delta0.data()+3);
- Delta0_quat = v2q(ang0);
- Delta0_quat.normalize();
- ang = q2v(Delta0_quat);
-
- //std::cout << "\ninput Delta0 : " << Delta0 << std::endl;
- //std::cout << "\n rotation vector we start with :\n" << ang << std::endl;
-
- //get data to compute jacobians
- struct IMU_jac_bias bias_jac_c = processor_imu.finite_diff_ab(dt, data_, ddelta_bias, Delta0);
- bias_jac.copyfrom(bias_jac_c);
-
- Delta_noise << 0.00000001, 0.00000001, 0.00000001, 0.0001, 0.0001, 0.0001, 0.00000001, 0.00000001, 0.00000001;
- delta_noise << 0.00000001, 0.00000001, 0.00000001, 0.0001, 0.0001, 0.0001, 0.00000001, 0.00000001, 0.00000001;
-
- struct IMU_jac_deltas deltas_jac_c = processor_imu.finite_diff_noise(dt, data_, Delta_noise, delta_noise, Delta0);
- deltas_jac = deltas_jac_c;
- }
-
- virtual void TearDown()
- {
- // code here will be called just after the test completes
- // ok to through exceptions from here if need be
- /*
- You can do deallocation of resources in TearDown or the destructor routine.
- However, if you want exception handling you must do it only in the TearDown code because throwing an exception
- from the destructor results in undefined behavior.
- The Google assertion macros may throw exceptions in platforms where they are enabled in future releases.
- Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
- */
- }
-};
-
-class ProcessorIMU_jacobians_Dq : public testing::Test
-{
- public:
- TimeStamp t;
- Eigen::Vector6s data_;
- Eigen::Matrix<wolf::Scalar,10,1> Delta0;
- wolf::Scalar ddelta_bias2;
- wolf::Scalar dt;
- struct IMU_jac_bias bias_jac2;
-
- void remapJacDeltas_quat0(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq0, Eigen::Map<Eigen::Quaternions>& _dq0){
-
- new (&_Dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta0_.data() + 3);
- new (&_dq0) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta0_.data() + 3);
- }
-
- void remapJacDeltas_quat(IMU_jac_deltas& _jac_delta, Eigen::Map<Eigen::Quaternions>& _Dq, Eigen::Map<Eigen::Quaternions>& _dq, const int& place ){
-
- assert(place < _jac_delta.Delta_noisy_vect_.size());
- new (&_Dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.Delta_noisy_vect_(place).data() + 3);
- new (&_dq) Eigen::Map<const Eigen::Quaternions>(_jac_delta.delta_noisy_vect_(place).data() + 3);
- }
-
- virtual void SetUp()
- {
- //SetUp for jacobians
- wolf::Scalar deg_to_rad = M_PI/180.0;
- data_ << 10,0.5,3, 100*deg_to_rad,110*deg_to_rad,30*deg_to_rad;
-
- // Wolf problem
- ProblemPtr wolf_problem_ptr_ = Problem::create("POV 3D");
- Eigen::VectorXs IMU_extrinsics(7);
- IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
-
- ProcessorIMU_UnitTester processor_imu;
- ddelta_bias2 = 0.0001;
- dt = 0.001;
-
- //defining a random Delta to begin with (not to use Origin point)
- Eigen::Matrix<wolf::Scalar,10,1> Delta0;
- Delta0 = Eigen::Matrix<wolf::Scalar,10,1>::Random();
- Delta0.head<3>() = Delta0.head<3>()*100;
- Delta0.tail<3>() = Delta0.tail<3>()*10;
- Eigen::Vector3s ang0, ang;
- ang0 << 120.08*deg_to_rad, 12.36*deg_to_rad, 54.32*deg_to_rad;
-
- Eigen::Map<Eigen::Quaternions> Delta0_quat(Delta0.data()+3);
- Delta0_quat = v2q(ang0);
- Delta0_quat.normalize();
- ang = q2v(Delta0_quat);
-
- //std::cout << "\ninput Delta0 : " << Delta0 << std::endl;
- //std::cout << "\n rotation vector we start with :\n" << ang << std::endl;
-
- //get data to compute jacobians
- struct IMU_jac_bias bias_jac_c = processor_imu.finite_diff_ab(dt, data_, ddelta_bias2, Delta0);
- bias_jac2.copyfrom(bias_jac_c);
- }
-
- virtual void TearDown()
- {
- // code here will be called just after the test completes
- // ok to through exceptions from here if need be
- /*
- You can do deallocation of resources in TearDown or the destructor routine.
- However, if you want exception handling you must do it only in the TearDown code because throwing an exception
- from the destructor results in undefined behavior.
- The Google assertion macros may throw exceptions in platforms where they are enabled in future releases.
- Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
- */
- }
-};
-
- ///BIAS TESTS
-
-/* IMU_jac_deltas struct form :
- contains vectors of size 7 :
- Elements at place 0 are those not affected by the bias noise that we add (da_bx,..., dw_bx,... ).
- place 1 : added da_bx in data place 2 : added da_by in data place 3 : added da_bz in data
- place 4 : added dw_bx in data place 5 : added dw_by in data place 6 : added dw_bz in data
-*/
-
-/* Mathematics
-
- dDp_dab = [dDp_dab_x, dDp_dab_y, dDp_dab_z]
- dDp_dab_x = (dDp(ab_x + dab_x, ab_y, ab_z) - dDp(ab_x,ab_y,ab_z)) / dab_x
- dDp_dab_x = (dDp(ab_x, ab_y + dab_y, ab_z) - dDp(ab_x,ab_y,ab_z)) / dab_y
- dDp_dab_x = (dDp(ab_x, ab_y, ab_z + dab_z) - dDp(ab_x,ab_y,ab_z)) / dab_z
-
- similar for dDv_dab
- note dDp_dab_x, dDp_dab_y, dDp_dab_z, dDv_dab_x, dDv_dab_y, dDv_dab_z are 3x1 vectors !
-
- dDq_dab = 0_{3x3}
- dDq_dwb = [dDq_dwb_x, dDq_dwb_y, dDq_dwb_z]
- dDq_dwb_x = log( dR(wb).transpose() * dR(wb - dwb_x))/dwb_x
- = log( dR(wb).transpose() * exp((wx - wbx - dwb_x)dt, (wy - wby)dt, (wy - wby)dt))/dwb_x
- dDq_dwb_y = log( dR(wb).transpose() * dR(wb - dwb_y))/dwb_y
- dDq_dwb_z = log( dR(wb).transpose() * dR(wb + dwb_z))/dwb_z
-
- Note : dDq_dwb must be computed recursively ! So comparing the one returned by processor_imu and the numerical
- one will have no sense if we aredoing this from a random Delta. The Delta here should be the origin.
- dDq_dwb_ = dR.tr()*dDq_dwb - Jr(wdt)*dt
- Then at first step, dR.tr() = Id, dDq_dwb = 0_{3x3}, which boils down to dDq_dwb_ = Jr(wdt)*dt
-*/
-
-TEST_F(ProcessorIMU_jacobians, dDp_dab)
-{
- using namespace wolf;
- Eigen::Matrix3s dDp_dab;
-
- for(int i=0;i<3;i++)
- dDp_dab.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i).head(3) - bias_jac.Delta0_.head(3))/ddelta_bias;
-
- ASSERT_TRUE((dDp_dab - bias_jac.dDp_dab_).isMuchSmallerThan(1,0.000001)) << "dDp_dab : \n" << dDp_dab << "\n bias_jac.dDp_dab_ :\n" << bias_jac.dDp_dab_ <<
- "\ndDp_dab_a - dDp_dab_ : \n" << bias_jac.dDp_dab_ - dDp_dab << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDv_dab)
-{
- using namespace wolf;
- Eigen::Matrix3s dDv_dab;
-
- for(int i=0;i<3;i++)
- dDv_dab.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i).tail(3) - bias_jac.Delta0_.tail(3))/ddelta_bias;
-
- ASSERT_TRUE((dDv_dab - bias_jac.dDv_dab_).isMuchSmallerThan(1,0.000001)) << "dDv_dab : \n" << dDv_dab << "\n bias_jac.dDv_dab_ :\n" << bias_jac.dDv_dab_ <<
- "\ndDv_dab_a - dDv_dab_ : \n" << bias_jac.dDv_dab_ - dDv_dab << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDp_dwb)
-{
- using namespace wolf;
- Eigen::Matrix3s dDp_dwb;
-
- for(int i=0;i<3;i++)
- dDp_dwb.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i+3).head(3) - bias_jac.Delta0_.head(3))/ddelta_bias;
-
- ASSERT_TRUE((dDp_dwb - bias_jac.dDp_dwb_).isMuchSmallerThan(1,0.000001)) << "dDp_dwb : \n" << dDp_dwb << "\n bias_jac.dDp_dwb_ :\n" << bias_jac.dDp_dwb_ <<
- "\ndDp_dwb_a - dDv_dab_ : \n" << bias_jac.dDp_dwb_ - dDp_dwb << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDv_dwb_)
-{
- using namespace wolf;
- Eigen::Matrix3s dDv_dwb;
-
- for(int i=0;i<3;i++)
- dDv_dwb.block<3,1>(0,i) = (bias_jac.Deltas_noisy_vect_(i+3).tail(3) - bias_jac.Delta0_.tail(3))/ddelta_bias;
-
- ASSERT_TRUE((dDv_dwb - bias_jac.dDv_dwb_).isMuchSmallerThan(1,0.000001)) << "dDv_dwb : \n" << dDv_dwb << "\n bias_jac.dDv_dwb_ :\n" << bias_jac.dDv_dwb_ <<
- "\ndDv_dwb_a - dDv_dwb_ : \n" << bias_jac.dDv_dwb_ - dDv_dwb << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDq_dab)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> q_in_1(NULL), q_in_2(NULL);
- Eigen::Matrix3s dDq_dab;
-
- new (&q_in_1) Eigen::Map<Eigen::Quaternions>(bias_jac.Delta0_.data() + 3);
- for(int i=0;i<3;i++){
- new (&q_in_2) Eigen::Map<Eigen::Quaternions>(bias_jac.Deltas_noisy_vect_(i).data() + 3);
- dDq_dab.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias;
- }
-
- ASSERT_TRUE(dDq_dab.isZero(wolf::Constants::EPS)) << "\t\tdDq_dab_ jacobian is not Zero :" << dDq_dab << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDq_dwb_noise_1Em8_)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> q_in_1(NULL), q_in_2(NULL);
- Eigen::Matrix3s dDq_dwb;
-
- new (&q_in_1) Eigen::Map<Eigen::Quaternions>(bias_jac.Delta0_.data() + 3);
-
- for(int i=0;i<3;i++){
- new (&q_in_2) Eigen::Map<Eigen::Quaternions>(bias_jac.Deltas_noisy_vect_(i+3).data() + 3);
-
- dDq_dwb.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias;
- }
-
- ASSERT_TRUE((dDq_dwb - bias_jac.dDq_dwb_).isMuchSmallerThan(1,0.000001))
- << "dDq_dwb : \n" << dDq_dwb
- << "\n bias_jac.dDq_dwb_ :\n" << bias_jac.dDq_dwb_
- << "\ndDq_dwb_a - dDq_dwb_ : \n" << bias_jac.dDq_dwb_ - dDq_dwb
- << "\n R1^T * R2 : \n" << q_in_1.matrix().transpose() * q_in_2.matrix()
- << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians_Dq, dDq_dwb_noise_1Em4_precision_1Em3_)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> q_in_1(NULL), q_in_2(NULL);
- Eigen::Matrix3s dDq_dwb;
-
- new (&q_in_1) Eigen::Map<Eigen::Quaternions>(bias_jac2.Delta0_.data() + 3);
- for(int i=0;i<3;i++){
- new (&q_in_2) Eigen::Map<Eigen::Quaternions>(bias_jac2.Deltas_noisy_vect_(i+3).data() + 3);
- dDq_dwb.block<3,1>(0,i) = R2v( q_in_1.matrix().transpose() * q_in_2.matrix())/ddelta_bias2;
- }
-
- ASSERT_TRUE((dDq_dwb - bias_jac2.dDq_dwb_).isMuchSmallerThan(1,0.001)) << "dDq_dwb : \n" << dDq_dwb << "\n bias_jac2.dDq_dwb_ :\n" << bias_jac2.dDq_dwb_ <<
- "\ndDq_dwb_a - dDq_dwb_ : \n" << bias_jac2.dDq_dwb_ - dDq_dwb << "\n R1^T * R2 : \n" << q_in_1.matrix().transpose() * q_in_2.matrix() << std::endl;
-}
-
- ///Perturbation TESTS
-
-/* reminder :
- jacobian_delta_preint_vect_ jacobian_delta_vect_
- 0: + 0, 0: + 0
- 1: +dPx, 2: +dPy, 3: +dPz 1: + dpx, 2: +dpy, 3: +dpz
- 4: +dOx, 5: +dOy, 6: +dOz 4: + dox, 5: +doy, 6: +doz
- 7: +dVx, 8: +dVy, 9: +dVz 7: + dvx, 8: +dvy, 9: +dvz
-*/
-
-/* Numerical method to check jacobians wrt noise
-
- dDp_dP = [dDp_dPx, dDp_dPy, dDp_dPz]
- dDp_dPx = ((P + dPx) - P)/dPx = Id
- dDp_dPy = ((P + dPy) - P)/dPy = Id
- dDp_dPz = ((P + dPz) - P)/dPz = Id
-
- dDp_dV = [dDp_dVx, dDp_dVy, dDp_dVz]
- dDp_dVx = ((V + dVx)*dt - V*dt)/dVx = Id*dt
- dDp_dVy = ((V + dVy)*dt - V*dt)/dVy = Id*dt
- dDp_dVz = ((V + dVz)*dt - V*dt)/dVz = Id*dt
-
- dDp_dO = [dDp_dOx, dDp_dOy, dDp_dOz]
- dDp_dOx = (( dR(Theta + dThetax)*dp ) - ( dR(Theta)*dp ))/dThetax
- = (( dR(Theta) * exp(dThetax,0,0)*dp ) - ( dR(Theta)*dp ))/dThetax
- dDp_dOy = (( dR(Theta) * exp(0,dThetay,0)*dp ) - ( dR(Theta)*dp ))/dThetay
- dDp_dOz = (( dR(Theta) * exp(0,0,dThetaz)*dp ) - ( dR(Theta)*dp ))/dThetaz
-
- dDv_dP = [dDv_dPx, dDv_dPy, dDv_dPz] = [0, 0, 0]
-
- dDv_dV = [dDv_dVx, dDv_dVy, dDv_dVz]
- dDv_dVx = ((V + dVx) - V)/dVx = Id
- dDv_dVy = ((V + dVy) - V)/dVy = Id
- dDv_dVz = ((V + dVz) - V)/dVz = Id
-
- dDv_dO = [dDv_dOx, dDv_dOy, dDv_dOz]
- dDv_dOx = (( dR(Theta + dThetax)*dv ) - ( dR(Theta)*dv ))/dThetax
- = (( dR(Theta) * exp(dThetax,0,0)*dv ) - ( dR(Theta)*dv ))/dThetax
- dDv_dOx = (( dR(Theta) * exp(0,dThetay,0)*dv ) - ( dR(Theta)*dv ))/dThetay
- dDv_dOz = (( dR(Theta) * exp(0,0,dThetaz)*dv ) - ( dR(Theta)*dv ))/dThetaz
-
- dDp_dp = [dDp_dpx, dDp_dpy, dDp_dpz]
- dDp_dpx = ( dR*(p + dpx) - dR*(p))/dpx
- dDp_dpy = ( dR*(p + dpy) - dR*(p))/dpy
- dDp_dpz = ( dR*(p + dpz) - dR*(p))/dpy
-
- dDp_dv = [dDp_dvx, dDp_dvy, dDp_dvz] = [0, 0, 0]
-
- dDp_do = [dDp_dox, dDp_doy, dDp_doz] = [0, 0, 0]
-
- dDv_dp = [dDv_dpx, dDv_dpy, dDv_dpz] = [0, 0, 0]
-
- dDv_dv = [dDv_dvx, dDv_dvy, dDv_dvz]
- dDv_dvx = ( dR*(v + dvx) - dR*(v))/dvx
- dDv_dvy = ( dR*(v + dvy) - dR*(v))/dvy
- dDv_dvz = ( dR*(v + dvz) - dR*(v))/dvz
-
- dDv_do = [dDv_dox, dDv_doy, dDv_doz] = [0, 0, 0]
-
- dDo_dp = dDo_dv = dDo_dP = dDo_dV = [0, 0, 0]
-
- dDo_dO = [dDo_dOx, dDo_dOy, dDo_dOz]
-
- dDo_dOx = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta+dThetax) * dr(theta) )/dThetax
- = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(dThetax,0,0)) * dr(theta) )/dThetax
- = log( (_Delta * _delta).transpose() * (_Delta_noisy * _delta))
- dDo_dOy = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(0,dThetay,0)) * dr(theta) )/dThetay
- dDo_dOz = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(0,0,dThetaz)) * dr(theta) )/dThetaz
-
- dDo_do = [dDo_dox, dDo_doy, dDo_doz]
-
- dDo_dox = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * dr(theta+dthetax) )/dthetax
- = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(dthetax,0,0)) )/dthetax
- = log( (_Delta * _delta).transpose() * (_Delta * _delta_noisy))
- dDo_doy = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(0,dthetay,0)) )/dthetay
- dDo_doz = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(0,0,dthetaz)) )/dthetaz
- */
-
-TEST_F(ProcessorIMU_jacobians, dDp_dP)
-{
- using namespace wolf;
- Eigen::Matrix3s dDp_dP;
-
- //dDp_dPx = ((P + dPx) - P)/dPx
- for(int i=0;i<3;i++)
- dDp_dP.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i).head(3) - deltas_jac.Delta0_.head(3))/Delta_noise(i);
-
- ASSERT_TRUE((dDp_dP - deltas_jac.jacobian_delta_preint_.block(0,0,3,3)).isMuchSmallerThan(1,0.000001)) << "dDp_dP : \n" << dDp_dP << "\n deltas_jac.jacobian_delta_preint_.block(0,0,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) <<
- "\ndDp_dP_a - dDp_dP_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,0,3,3) - dDp_dP << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDp_dV)
-{
- using namespace wolf;
- Eigen::Matrix3s dDp_dV;
-
- //Dp_dVx = ((V + dVx)*dt - V*dt)/dVx
- for(int i=0;i<3;i++)
- dDp_dV.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i+6).tail(3)*dt - deltas_jac.Delta0_.tail(3)*dt)/Delta_noise(i+6);
-
- ASSERT_TRUE((dDp_dV - deltas_jac.jacobian_delta_preint_.block(0,6,3,3)).isMuchSmallerThan(1,0.000001)) << "dDp_dV : \n" << dDp_dV << "\n deltas_jac.jacobian_delta_preint_.block(0,6,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) <<
- "\ndDp_dV_a - dDp_dV_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,6,3,3) - dDp_dV << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDp_dO)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
- Eigen::Matrix3s dDp_dO;
-
- //dDp_dOx = (( dR(Theta) * exp(dThetax,0,0)*dp ) - ( dR(Theta)*dp ))/dThetax
- remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
- for(int i=0;i<3;i++){
- remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i+3);
- dDp_dO.block<3,1>(0,i) = ((Dq_noisy.matrix() * deltas_jac.delta0_.head(3)) - (Dq0.matrix()* deltas_jac.delta0_.head(3)))/Delta_noise(i+3);
- }
-
- ASSERT_TRUE((dDp_dO - deltas_jac.jacobian_delta_preint_.block(0,3,3,3)).isMuchSmallerThan(1,0.000001)) << "dDp_dO : \n" << dDp_dO << "\n deltas_jac.jacobian_delta_preint_.block(0,3,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(0,3,3,3) <<
- "\ndDp_dO_a - dDp_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(0,3,3,3) - dDp_dO << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDv_dV)
-{
- using namespace wolf;
- Eigen::Matrix3s dDv_dV;
-
- //dDv_dVx = ((V + dVx) - V)/dVx
- for(int i=0;i<3;i++)
- dDv_dV.block<3,1>(0,i) = (deltas_jac.Delta_noisy_vect_(i+6).tail(3) - deltas_jac.Delta0_.tail(3))/Delta_noise(i+6);
-
- ASSERT_TRUE((dDv_dV - deltas_jac.jacobian_delta_preint_.block(6,6,3,3)).isMuchSmallerThan(1,0.000001)) << "dDv_dV : \n" << dDv_dV << "\n deltas_jac.jacobian_delta_preint_.block(6,6,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(6,6,3,3) <<
- "\ndDv_dV_a - dDv_dV_ : \n" << deltas_jac.jacobian_delta_preint_.block(6,6,3,3) - dDv_dV << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDv_dO)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
- Eigen::Matrix3s dDv_dO;
-
- //dDv_dOx = (( dR(Theta) * exp(dThetax,0,0)*dv ) - ( dR(Theta)*dv ))/dThetax
- remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
- for(int i=0;i<3;i++){
- remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i+3);
- dDv_dO.block<3,1>(0,i) = ((Dq_noisy.matrix() * deltas_jac.delta0_.tail(3)) - (Dq0.matrix()* deltas_jac.delta0_.tail(3)))/Delta_noise(i+3);
- }
-
- ASSERT_TRUE((dDv_dO - deltas_jac.jacobian_delta_preint_.block(6,3,3,3)).isMuchSmallerThan(1,0.000001)) << "dDv_dO : \n" << dDv_dO << "\n deltas_jac.jacobian_delta_preint_.block(6,3,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(6,3,3,3) <<
- "\ndDv_dO_a - dDv_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(6,3,3,3) - dDv_dO << std::endl;
-}
-
-//dDo_dP = dDo_dV = [0, 0, 0]
-
-TEST_F(ProcessorIMU_jacobians, dDo_dO)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
- Eigen::Matrix3s dDo_dO;
-
- //dDo_dOx = log( (dR(Theta) * dr(theta)).transpose() * (dR(Theta)*exp(dThetax,0,0)) * dr(theta) )/dThetax
- remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
- for(int i=0;i<3;i++){
- remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i+3);
- dDo_dO.block<3,1>(0,i) = R2v( (Dq0.matrix() * dq0.matrix()).transpose() * (Dq_noisy.matrix() * dq0.matrix()) )/Delta_noise(i+3);
- }
-
- ASSERT_TRUE((dDo_dO - deltas_jac.jacobian_delta_preint_.block(3,3,3,3)).isMuchSmallerThan(1,0.000001)) << "dDo_dO : \n" << dDo_dO << "\n deltas_jac.jacobian_delta_preint_.block(3,3,3,3) :\n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) <<
- "\ndDo_dO_a - dDo_dO_ : \n" << deltas_jac.jacobian_delta_preint_.block(3,3,3,3) - dDo_dO << std::endl;
-}
-
-TEST_F(ProcessorIMU_jacobians, dDp_dp)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL);
- Eigen::Matrix3s dDp_dp, dDp_dp_a;
-
- dDp_dp_a = deltas_jac.jacobian_delta_.block(0,0,3,3);
- remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
- //dDp_dpx = ( dR*(p + dpx) - dR*(p))/dpx
- for(int i=0;i<3;i++)
- dDp_dp.block<3,1>(0,i) = ( (Dq0.matrix() * deltas_jac.delta_noisy_vect_(i).head(3)) - (Dq0.matrix() * deltas_jac.delta0_.head(3)) )/delta_noise(i);
-
- ASSERT_TRUE((dDp_dp - dDp_dp_a).isMuchSmallerThan(1,0.000001)) << "dDp_dp : \n" << dDp_dp << "\n dDp_dp_a :\n" << dDp_dp_a <<
- "\ndDp_dp_a - dDp_dp_ : \n" << dDp_dp_a - dDp_dp << std::endl;
-}
-
-//dDv_dp = [0, 0, 0]
-
-TEST_F(ProcessorIMU_jacobians, dDv_dv)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL);
- Eigen::Matrix3s dDv_dv, dDv_dv_a;
-
- dDv_dv_a = deltas_jac.jacobian_delta_.block(6,6,3,3);
- remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
- //dDv_dvx = ( dR*(v + dvx) - dR*(v))/dvx
- for(int i=0;i<3;i++)
- dDv_dv.block<3,1>(0,i) = ( (Dq0 * deltas_jac.delta_noisy_vect_(i+6).tail(3)) - (Dq0 * deltas_jac.delta0_.tail(3)) )/delta_noise(i+6);
-
- ASSERT_TRUE((dDv_dv - dDv_dv_a).isMuchSmallerThan(1,0.000001)) << "dDv_dv : \n" << dDv_dv << "\n dDv_dv_a :\n" << dDv_dv_a <<
- "\ndDv_dv_a - dDv_dv_ : \n" << dDv_dv_a - dDv_dv << std::endl;
-}
-
-//dDo_dp = dDo_dv = [0, 0, 0]
-
-TEST_F(ProcessorIMU_jacobians, dDo_do)
-{
- using namespace wolf;
- Eigen::Map<Eigen::Quaternions> Dq0(NULL), dq0(NULL), Dq_noisy(NULL), dq_noisy(NULL);
- Eigen::Matrix3s dDo_do, dDo_do_a;
-
- //dDo_dox = log( (dR(Theta) * dr(theta)).transpose() * dR(Theta) * (dr(theta)*exp(dthetax,0,0)) )/dthetax
- remapJacDeltas_quat0(deltas_jac, Dq0, dq0);
- dDo_do_a = deltas_jac.jacobian_delta_.block(3,3,3,3);
-
- for(int i=0;i<3;i++){
- remapJacDeltas_quat(deltas_jac, Dq_noisy, dq_noisy, i+3);
- dDo_do.block<3,1>(0,i) = R2v( (Dq0.matrix() * dq0.matrix()).transpose() * (Dq0.matrix() * dq_noisy.matrix()) )/Delta_noise(i+3);
- }
-
- ASSERT_TRUE((dDo_do - dDo_do_a).isMuchSmallerThan(1,0.000001)) << "dDo_do : \n" << dDo_do << "\n dDo_do_a :\n" << dDo_do_a <<
- "\ndDo_do_a - dDo_do_ : \n" << dDo_do_a - dDo_do << std::endl;
-}
-
-int main(int argc, char **argv)
-{
- using namespace wolf;
-
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_processor_tracker_feature_trifocal.cpp b/test/gtest_processor_tracker_feature_trifocal.cpp
deleted file mode 100644
index d7b13f0c4..000000000
--- a/test/gtest_processor_tracker_feature_trifocal.cpp
+++ /dev/null
@@ -1,153 +0,0 @@
-#include "utils_gtest.h"
-
-#include "base/common/wolf.h"
-#include "base/utils/logging.h"
-
-#include "vision_utils.h"
-
-#include "base/processor/processor_tracker_feature_trifocal.h"
-#include "base/processor/processor_odom_3D.h"
-#include "base/capture/capture_image.h"
-#include "base/sensor/sensor_camera.h"
-
-using namespace Eigen;
-using namespace wolf;
-
-// Use the following in case you want to initialize tests with predefines variables or methods.
-//class ProcessorTrackerFeatureTrifocal_class : public testing::Test{
-// public:
-// virtual void SetUp()
-// {
-// }
-//};
-
-//TEST(ProcessorTrackerFeatureTrifocal, Constructor)
-//{
-// std::cout << "\033[1;33m [WARN]:\033[0m gtest for ProcessorTrackerFeatureTrifocal Constructor is empty." << std::endl;
-//}
-
-//TEST(ProcessorTrackerFeatureTrifocal, Destructor)
-//{
-// std::cout << "\033[1;33m [WARN]:\033[0m gtest for ProcessorTrackerFeatureTrifocal Destructor is empty." << std::endl;
-//}
-
-////[Class methods]
-//TEST(ProcessorTrackerFeatureTrifocal, trackFeatures)
-//{
-// std::cout << "033[1;33m [WARN]:033[0m gtest for ProcessorTrackerFeatureTrifocal trackFeatures is empty." << std::endl;
-//}
-//
-//TEST(ProcessorTrackerFeatureTrifocal, correctFeatureDrift)
-//{
-// std::cout << "033[1;33m [WARN]:033[0m gtest for ProcessorTrackerFeatureTrifocal correctFeatureDrift is empty." << std::endl;
-//}
-//
-//TEST(ProcessorTrackerFeatureTrifocal, voteForKeyFrame)
-//{
-// std::cout << "033[1;33m [WARN]:033[0m gtest for ProcessorTrackerFeatureTrifocal voteForKeyFrame is empty." << std::endl;
-//}
-//
-//TEST(ProcessorTrackerFeatureTrifocal, detectNewFeatures)
-//{
-// std::cout << "033[1;33m [WARN]:033[0m gtest for ProcessorTrackerFeatureTrifocal detectNewFeatures is empty." << std::endl;
-//}
-//
-//TEST(ProcessorTrackerFeatureTrifocal, createFactor)
-//{
-// std::cout << "033[1;33m [WARN]:033[0m gtest for ProcessorTrackerFeatureTrifocal createFactor is empty." << std::endl;
-//}
-
-TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
-{
-
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
- using Eigen::Vector2s;
-
- std::string wolf_root = _WOLF_ROOT_DIR;
-
- Scalar dt = 0.01;
-
- // Wolf problem
- ProblemPtr problem = Problem::create("PO 3D");
-
- // Install tracker (sensor and processor)
- IntrinsicsCameraPtr intr = make_shared<IntrinsicsCamera>(); // TODO init params or read from YAML
- intr->width = 640;
- intr->height = 480;
- SensorCameraPtr sens_trk = make_shared<SensorCamera>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(),
- intr);
-
- ProcessorParamsTrackerFeatureTrifocalPtr params_tracker_feature_trifocal = std::make_shared<ProcessorParamsTrackerFeatureTrifocal>();
-// params_tracker_feature_trifocal->name = "trifocal";
- params_tracker_feature_trifocal->pixel_noise_std = 1.0;
- params_tracker_feature_trifocal->voting_active = true;
- params_tracker_feature_trifocal->min_features_for_keyframe = 5;
- params_tracker_feature_trifocal->time_tolerance = dt/2;
- params_tracker_feature_trifocal->max_new_features = 5;
- params_tracker_feature_trifocal->min_response_new_feature = 25;
- params_tracker_feature_trifocal->n_cells_h = 10;
- params_tracker_feature_trifocal->n_cells_v = 10;
- params_tracker_feature_trifocal->max_euclidean_distance = 20;
- params_tracker_feature_trifocal->yaml_file_params_vision_utils = wolf_root + "/src/examples/ACTIVESEARCH.yaml";
-
- ProcessorTrackerFeatureTrifocalPtr proc_trk = make_shared<ProcessorTrackerFeatureTrifocal>(params_tracker_feature_trifocal);
- proc_trk->configure(sens_trk);
-
- problem->addSensor(sens_trk);
- sens_trk->addProcessor(proc_trk);
-
- // Install odometer (sensor and processor)
- IntrinsicsOdom3DPtr params = std::make_shared<IntrinsicsOdom3D>();
- SensorBasePtr sens_odo = problem->installSensor("ODOM 3D", "odometer", (Vector7s() << 0,0,0, 0,0,0,1).finished(), params);
- ProcessorParamsOdom3DPtr proc_odo_params = make_shared<ProcessorParamsOdom3D>();
- proc_odo_params->time_tolerance = dt/2;
- ProcessorBasePtr proc_odo = problem->installProcessor("ODOM 3D", "odometer", sens_odo, proc_odo_params);
-
- std::cout << "sensor & processor created and added to wolf problem" << std::endl;
-
- // Sequence to test KeyFrame creations (callback calls)
-
- // initialize
- TimeStamp t(0.0);
- Vector7s x; x << 0,0,0, 0,0,0,1;
- Matrix6s P = Matrix6s::Identity() * 0.1;
- problem->setPrior(x, P, t, dt/2); // KF1
-
- CaptureOdom3DPtr capt_odo = make_shared<CaptureOdom3D>(t, sens_odo, Vector6s::Zero(), P);
-
- // Track
- cv::Mat image(intr->height, intr->width, CV_8UC3); // OpenCV cv::Mat(rows, cols)
- image *= 0; // TODO see if we want to use a real image
- CaptureImagePtr capt_trk = make_shared<CaptureImage>(t, sens_trk, image);
- proc_trk->process(capt_trk);
-
- for (size_t ii=0; ii<32; ii++ )
- {
- // Move
- t = t+dt;
- WOLF_INFO("----------------------- ts: ", t , " --------------------------");
-
- capt_odo->setTimeStamp(t);
- proc_odo->process(capt_odo);
-
- // Track
- capt_trk = make_shared<CaptureImage>(t, sens_trk, image);
- proc_trk->process(capt_trk);
-
- CaptureBasePtr prev = proc_trk->getPrevOrigin();
- problem->print(2,0,0,0);
-
- // Only odom creating KFs
- ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 3D")==0 );
- }
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_processor_tracker_landmark_apriltag.cpp b/test/gtest_processor_tracker_landmark_apriltag.cpp
deleted file mode 100644
index 6cb146f61..000000000
--- a/test/gtest_processor_tracker_landmark_apriltag.cpp
+++ /dev/null
@@ -1,413 +0,0 @@
-#include "utils_gtest.h"
-
-#include "base/wolf.h"
-#include "base/logging.h"
-
-#include "base/processor/processor_tracker_landmark_apriltag.h"
-#include "base/feature/feature_apriltag.h"
-#include "base/landmark/landmark_apriltag.h"
-#include "base/capture/capture_pose.h"
-#include "base/processor/processor_factory.h"
-
-using namespace Eigen;
-using namespace wolf;
-using std::static_pointer_cast;
-
-
-////////////////////////////////////////////////////////////////
-/*
- * Wrapper class to be able to have setOriginPtr() and setLastPtr() in ProcessorTrackerLandmarkApriltag
- */
-WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkApriltag_Wrapper);
-class ProcessorTrackerLandmarkApriltag_Wrapper : public ProcessorTrackerLandmarkApriltag
-{
- public:
- ProcessorTrackerLandmarkApriltag_Wrapper(ProcessorParamsTrackerLandmarkApriltagPtr _params_tracker_landmark_apriltag) :
- ProcessorTrackerLandmarkApriltag(_params_tracker_landmark_apriltag)
- {
- setType("TRACKER LANDMARK APRILTAG WRAPPER");
- };
- ~ProcessorTrackerLandmarkApriltag_Wrapper(){}
- void setOriginPtr(const CaptureBasePtr _origin_ptr) { origin_ptr_ = _origin_ptr; }
- void setLastPtr (const CaptureBasePtr _last_ptr) { last_ptr_ = _last_ptr; }
- void setIncomingPtr (const CaptureBasePtr _incoming_ptr) { incoming_ptr_ = _incoming_ptr; }
- unsigned int getMinFeaturesForKeyframe (){return min_features_for_keyframe_;}
- Scalar getMinTimeVote (){return min_time_vote_;}
- void setIncomingDetections(const FeatureBasePtrList _incoming_detections) { detections_incoming_ = _incoming_detections; }
- void setLastDetections(const FeatureBasePtrList _last_detections) { detections_last_ = _last_detections; }
-
- // for factory
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr)
- {
- std::shared_ptr<ProcessorParamsTrackerLandmarkApriltag> prc_apriltag_params_;
- if (_params)
- prc_apriltag_params_ = std::static_pointer_cast<ProcessorParamsTrackerLandmarkApriltag>(_params);
- else
- prc_apriltag_params_ = std::make_shared<ProcessorParamsTrackerLandmarkApriltag>();
-
- ProcessorTrackerLandmarkApriltag_WrapperPtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkApriltag_Wrapper>(prc_apriltag_params_);
- prc_ptr->setName(_unique_name);
- return prc_ptr;
- }
-
-};
-namespace wolf{
-// Register in the Factories
-WOLF_REGISTER_PROCESSOR("TRACKER LANDMARK APRILTAG WRAPPER", ProcessorTrackerLandmarkApriltag_Wrapper);
-}
-////////////////////////////////////////////////////////////////
-
-
-
-////////////////////////////////////////////////////////////////
-/*
- * Test class to prepare a little wolf problem to test the class ProcessorTrackerLandmarkApriltag
- *
- * The class ProcessorTrackerLandmarkApriltag is sometimes tested via the wrapper ProcessorTrackerLandmarkApriltag_Wrapper
- */
-// Use the following in case you want to initialize tests with predefined variables or methods.
-class ProcessorTrackerLandmarkApriltag_class : public testing::Test{
- public:
- virtual void SetUp()
- {
- wolf_root = _WOLF_ROOT_DIR;
-
- // configure wolf problem
- problem = Problem::create("PO 3D");
- sen = problem->installSensor("CAMERA", "camera", (Vector7s()<<0,0,0,0,0,0,1).finished(), wolf_root + "/src/examples/camera_params_canonical.yaml");
- prc = problem->installProcessor("TRACKER LANDMARK APRILTAG WRAPPER", "apriltags_wrapper", "camera", wolf_root + "/src/examples/processor_tracker_landmark_apriltag.yaml");
- prc_apr = std::static_pointer_cast<ProcessorTrackerLandmarkApriltag_Wrapper>(prc);
-
- // set prior
- F1 = problem->setPrior((Vector7s()<<0,0,0,0,0,0,1).finished(), Matrix6s::Identity(), 0.0, 0.1);
-
- // minimal config for the processor to be operative
- C1 = std::make_shared<CapturePose>(1.0, sen, Vector7s(), Matrix6s());
- F1->addCapture(C1);
- prc_apr->setOriginPtr(C1);
- prc_apr->setLastPtr(C1);
-
- det.p[0][0] = 1.0;
- det.p[0][1] = -1.0;
- det.p[1][0] = 1.0;
- det.p[1][1] = 1.0;
- det.p[2][0] = -1.0;
- det.p[2][1] = 1.0;
- det.p[3][0] = -1.0;
- det.p[3][1] = -1.0;
-
- rep_error1 = 0.01;
- rep_error2 = 0.1;
- use_rotation = true;
- }
-
- public:
- ProcessorTrackerLandmarkApriltag_WrapperPtr prc_apr;
- std::string wolf_root;
- ProblemPtr problem;
- SensorBasePtr sen;
- ProcessorBasePtr prc;
- FrameBasePtr F1;
- CaptureBasePtr C1;
- apriltag_detection_t det;
- Scalar rep_error1;
- Scalar rep_error2;
- bool use_rotation;
-};
-////////////////////////////////////////////////////////////////
-
-
-
-/////////////////// TESTS START HERE ///////////////////////////
-// //
-TEST(ProcessorTrackerLandmarkApriltag, Constructor)
-{
- std::string s1;
- std::string s2;
-
- ProcessorParamsTrackerLandmarkApriltagPtr params = std::make_shared<ProcessorParamsTrackerLandmarkApriltag>();
- params->tag_family_ = "tag36h11";
- ProcessorTrackerLandmarkApriltagPtr p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
- ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
-
- params->tag_family_ = "tag36h10";
- p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
- ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
-
- params->tag_family_ = "tag36artoolkit";
- p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
- ASSERT_TRUE(p->getTagFamily() == "artoolkit"); // This tagfamily is stored differently by apriltag library
-
- params->tag_family_ = "tag25h9";
- p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
- ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
-
- params->tag_family_ = "tag25h7";
- p = std::make_shared<ProcessorTrackerLandmarkApriltag>(params);
- ASSERT_TRUE(p->getTagFamily() == params->tag_family_);
-
- params->tag_family_ = "wrong_family";
- WOLF_INFO("The following runtime error \"Unrecognized tag family name. Use e.g. \"tag36h11\".\" is expected and does not imply a failed test:");
- ASSERT_DEATH( { std::make_shared<ProcessorTrackerLandmarkApriltag>(params); }, "" );
-}
-
-TEST_F(ProcessorTrackerLandmarkApriltag_class, voteForKeyFrame)
-{
- Scalar min_time_vote = prc_apr->getMinTimeVote();
- unsigned int min_features_for_keyframe = prc_apr->getMinFeaturesForKeyframe();
- Scalar start_ts = 2.0;
-
- CaptureBasePtr Ca = std::make_shared<CapturePose>(start_ts, sen, Vector7s(), Matrix6s());
- CaptureBasePtr Cb = std::make_shared<CapturePose>(start_ts + min_time_vote/2, sen, Vector7s(), Matrix6s());
- CaptureBasePtr Cc = std::make_shared<CapturePose>(start_ts + 2*min_time_vote, sen, Vector7s(), Matrix6s());
- CaptureBasePtr Cd = std::make_shared<CapturePose>(start_ts + 2.5*min_time_vote, sen, Vector7s(), Matrix6s());
- CaptureBasePtr Ce = std::make_shared<CapturePose>(start_ts + 3*min_time_vote, sen, Vector7s(), Matrix6s());
-
- for (int i=0; i < min_features_for_keyframe; i++){
- det.id = i;
- FeatureApriltagPtr f = std::make_shared<FeatureApriltag>((Vector7s()<<0,0,0,0,0,0,1).finished(), Matrix6s::Identity(), i, det, rep_error1, rep_error2, use_rotation);
- Ca->addFeature(f);
- Ca->addFeature(f);
- Cc->addFeature(f);
- if (i != min_features_for_keyframe-1){
- Cd->addFeature(f);
- Ce->addFeature(f);
- }
- }
- F1->addCapture(Ca);
- F1->addCapture(Cb);
- F1->addCapture(Cc);
- F1->addCapture(Cd);
- F1->addCapture(Ce);
-
- // CASE 1: Not enough time between origin and incoming
- prc_apr->setOriginPtr(Ca);
- prc_apr->setIncomingPtr(Cb);
- ASSERT_FALSE(prc_apr->voteForKeyFrame());
-
- // CASE 2: Enough time but still too many features in image to trigger a KF
- prc_apr->setOriginPtr(Ca);
- prc_apr->setLastPtr(Cb);
- prc_apr->setIncomingPtr(Cc);
- ASSERT_FALSE(prc_apr->voteForKeyFrame());
-
- // CASE 3: Enough time, enough features in last, not enough features in incoming
- prc_apr->setOriginPtr(Ca);
- prc_apr->setLastPtr(Cc);
- prc_apr->setIncomingPtr(Cd);
- ASSERT_TRUE(prc_apr->voteForKeyFrame());
-
- // CASE 4: Enough time, not enough features in last, not enough features in incoming
- prc_apr->setOriginPtr(Ca);
- prc_apr->setLastPtr(Cd);
- prc_apr->setIncomingPtr(Ce);
- ASSERT_FALSE(prc_apr->voteForKeyFrame());
-
-}
-
-TEST_F(ProcessorTrackerLandmarkApriltag_class, detectNewFeatures)
-{
- // No detected features
- FeatureBasePtrList features_out;
- prc_apr->detectNewFeatures(1, features_out);
- ASSERT_EQ(features_out.size(), 0);
-
- // Some detected features TODO
- FeatureBasePtrList features_in;
- Eigen::Vector3s pos;
- Eigen::Vector3s ori; //Euler angles in rad
- Eigen::Quaternions quat;
- Eigen::Vector7s pose;
- Eigen::Matrix6s meas_cov( (prc_apr->getVarVec()).asDiagonal() );
- int tag_id;
-
- // feature 0
- pos << 0,2,0;
- ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
- quat = e2q(ori);
- pose << pos, quat.coeffs();
- tag_id = 0;
- det.id = tag_id;
- FeatureBasePtr f0 = std::make_shared<FeatureApriltag>(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
-
- // feature 1
- pos << 1,2,0;
- ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
- quat = e2q(ori);
- pose << pos, quat.coeffs();
- tag_id = 1;
- det.id = tag_id;
- FeatureBasePtr f1 = std::make_shared<FeatureApriltag>(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
-
- // feature 2
- pos << 0,2,1;
- ori << M_TORAD * 0, M_TORAD * 0, M_TORAD * 0;
- quat = e2q(ori);
- pose << pos, quat.coeffs();
- tag_id = 2;
- det.id = tag_id;
- FeatureBasePtr f2 = std::make_shared<FeatureApriltag>(pose, meas_cov, tag_id, det, rep_error1, rep_error2, use_rotation);
-
- features_in.push_back(f0);
- features_in.push_back(f0);
-
- // We just added twice the same feature in the list.
- prc_apr->setLastDetections(features_in);
- // at this point we have 0 detections in last, 2 detections in incoming with same id. We should keep only one in the final list of new detected features
- prc_apr->detectNewFeatures(2, features_out);
- ASSERT_EQ(features_out.size(), 1);
-
- //we add new different features in the list
- features_in.clear();
- features_in.push_back(f0);
- features_in.push_back(f1);
- //these features are set as the incoming detections due to processing an image
- prc_apr->setLastDetections(features_in);
- // at this point we have 0 detections in last, 2 detections in incoming with different ids, thus we should have 2 new detected features (if max_features set to >= 2)
- prc_apr->detectNewFeatures(2, features_out);
- ASSERT_EQ(features_out.size(), 2);
-
- // Put some of the features in the graph with createLandmark() and detect some of them as well as others with detectNewFeatures() running again.
- WOLF_WARN("call to function createLandmark() in unit test for detectNewFeatures().")
- C1->addFeature(f0);
- LandmarkBasePtr lmk0 = prc_apr->createLandmark(f0);
- C1->addFeature(f1);
- LandmarkBasePtr lmk1 = prc_apr->createLandmark(f1);
-
- // Add landmarks to the map
- LandmarkBasePtrList landmark_list;
- landmark_list.push_back(lmk0);
- landmark_list.push_back(lmk1);
- problem->addLandmarkList(landmark_list);
- //problem->print(4,1,1,1);
-
- // Add 1 one more new feature to the detection list
- features_in.push_back(f2);
- prc_apr->setLastDetections(features_in);
- // At this point we have 2 landmarks (for f0 and f1), and 3 detections (f0, f1 and f2).
- // Hence we should 1 new detected feature : f2
- features_out.clear();
- prc_apr->detectNewFeatures(2, features_out);
- ASSERT_EQ(features_out.size(), 1);
- ASSERT_EQ(std::static_pointer_cast<FeatureApriltag>(features_out.front())->getTagId(), 2);
-}
-
-TEST_F(ProcessorTrackerLandmarkApriltag_class, createLandmark)
-{
- Vector7s pose_landmark((Vector7s()<<0,0,0,0,0,0,1).finished());
- det.id = 1;
- FeatureApriltagPtr f1 = std::make_shared<FeatureApriltag>(pose_landmark, Matrix6s::Identity(), 1, det, rep_error1, rep_error2, use_rotation);
-
- C1->addFeature(f1);
- LandmarkBasePtr lmk = prc_apr->createLandmark(f1);
- LandmarkApriltagPtr lmk_april = std::static_pointer_cast<LandmarkApriltag>(lmk);
- ASSERT_TRUE(lmk_april->getType() == "APRILTAG");
- ASSERT_MATRIX_APPROX(lmk_april->getState(), pose_landmark, 1e-6);
-}
-
-TEST_F(ProcessorTrackerLandmarkApriltag_class, createFactor)
-{
- det.id = 1;
- FeatureApriltagPtr f1 = std::make_shared<FeatureApriltag>((Vector7s()<<0,0,0,0,0,0,1).finished(), Matrix6s::Identity(), 1, det, rep_error1, rep_error2, use_rotation);
-
- C1->addFeature(f1);
- LandmarkBasePtr lmk = prc_apr->createLandmark(f1);
- LandmarkApriltagPtr lmk_april = std::static_pointer_cast<LandmarkApriltag>(lmk);
-
- FactorBasePtr ctr = prc_apr->createFactor(f1, lmk);
-
- ASSERT_TRUE(ctr->getType() == "AUTODIFF APRILTAG");
-}
-
-TEST_F(ProcessorTrackerLandmarkApriltag_class, computeInformation)
-{
- Scalar cx = 320;
- Scalar cy = 240;
- Scalar fx = 320;
- Scalar fy = 320;
- Eigen::Matrix3s K;
- K << fx, 0, cx,
- 0, fy, cy,
- 0, 0, 1;
- Eigen::Vector3s t; t << 0.0, 0.0, 0.4;
- Eigen::Vector3s v; v << 0.2, 0.0, 0.0;
- Scalar tag_width = 0.05;
- Scalar s = tag_width/2;
- Eigen::Vector3s p1; p1 << s, s, 0; // bottom right
- Eigen::Vector3s p2; p2 << -s, s, 0; // bottom left
-
- // Got from Matlab code:
- // Top left corner
- Eigen::Vector3s h1_matlab; h1_matlab << 137.5894, 105.0325, 0.4050;
- Eigen::Matrix3s J_h_T1_matlab;
- J_h_T1_matlab << 320, 0, 320,
- 0, 320, 240,
- 0, 0, 1;
- Eigen::Matrix3s J_h_R1_matlab;
- J_h_R1_matlab << 7.8405, -7.8405, -6.4106,
- 4.2910, -4.2910, 9.0325,
- 0.0245, -0.0245, 0.0050;
- // Top right corner
- Eigen::Vector3s h2_matlab; h2_matlab << 121.5894, 105.0325, 0.4050;
- Eigen::Matrix3s J_h_T2_matlab;
- J_h_T2_matlab << 320, 0, 320,
- 0, 320, 240,
- 0, 0, 1;
- Eigen::Matrix3s J_h_R2_matlab;
- J_h_R2_matlab << 7.8405, 7.8405, -9.5894,
- 4.2910, 4.2910, -9.0325,
- 0.0245, 0.0245, -0.0050;
-
- Eigen::Vector3s h1;
- Eigen::Matrix3s J_h_T1;
- Eigen::Matrix3s J_h_R1;
- Eigen::Vector3s h2;
- Eigen::Matrix3s J_h_T2;
- Eigen::Matrix3s J_h_R2;
-
- prc_apr->pinholeHomogeneous(K, t, v2R(v), p1, h1, J_h_T1, J_h_R1);
- prc_apr->pinholeHomogeneous(K, t, v2R(v), p2, h2, J_h_T2, J_h_R2);
-
- ASSERT_MATRIX_APPROX(h1, h1_matlab, 1e-3);
- ASSERT_MATRIX_APPROX(J_h_T1, J_h_T1_matlab, 1e-3);
- ASSERT_MATRIX_APPROX(J_h_R1, J_h_R1_matlab, 1e-3);
- ASSERT_MATRIX_APPROX(h2, h2_matlab, 1e-3);
- ASSERT_MATRIX_APPROX(J_h_T2, J_h_T2_matlab, 1e-3);
- ASSERT_MATRIX_APPROX(J_h_R2, J_h_R2_matlab, 1e-3);
-
- Scalar sig_q = 2;
- Eigen::Matrix6s transformation_info = prc_apr->computeInformation(t, v2R(v), K, tag_width, sig_q);
-
- // From Matlab
-// Eigen::Matrix6s transformation_cov_matlab;
-// transformation_cov_matlab <<
-// 0.0000, 0.0000, -0.0000, 0.0000, -0.0002, 0.0000,
-// 0.0000, 0.0000, -0.0000, 0.0002, 0.0000, 0.0000,
-// -0.0000, -0.0000, 0.0004, -0.0040, -0.0000, 0.0000,
-// 0.0000, 0.0002, -0.0040, 0.1027, 0.0000, 0.0000,
-// -0.0002, 0.0000, -0.0000, 0.0000, 0.1074, -0.0106,
-// 0.0000, 0.0000, 0.0000, 0.0000, -0.0106, 0.0023;
-
- Eigen::Matrix6s transformation_info_matlab;
- transformation_info_matlab <<
- 6.402960973553990, 0, 0.000000000000000, -0.000000000000000, 0.009809735541319, 0.001986080274985,
- 0, 6.402960973553990, 0.014610695222409, -0.008824560412472, 0.000000000000000, 0.000000000000000,
- 0.000000000000000, 0.014610695222409, 0.049088870761338, 0.001889201771982, 0.000000000000000, 0.000000000000000,
- -0.000000000000000, -0.008824560412472, 0.001889201771982, 0.000183864607538, -0.000000000000000, 0.000000000000000,
- 0.009809735541319, 0.000000000000000, 0.000000000000000, -0.000000000000000, 0.000183864607538, 0.000773944077821,
- 0.001986080274985, 0.000000000000000, 0.000000000000000, -0.000000000000000, 0.000773944077821, 0.007846814985446;
-
- transformation_info_matlab = transformation_info_matlab*100000.0;
-
-
- ASSERT_MATRIX_APPROX(transformation_info, transformation_info_matlab, 1e-3);
-
-
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_sensor_camera.cpp b/test/gtest_sensor_camera.cpp
deleted file mode 100644
index 9ca4d6571..000000000
--- a/test/gtest_sensor_camera.cpp
+++ /dev/null
@@ -1,123 +0,0 @@
-/**
- * \file gtest_sensor_camera.cpp
- *
- * Created on: Feb 7, 2019
- * \author: jsola
- */
-
-
-#include "utils_gtest.h"
-
-#include "src/sensor/sensor_camera.cpp"
-#include "base/sensor/sensor_factory.h"
-
-using namespace wolf;
-
-TEST(SensorCamera, Img_size)
-{
- Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
- IntrinsicsCamera params;
- params.width = 640;
- params.height = 480;
- SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params);
-
- ASSERT_EQ(cam->getImgWidth() , 640);
- ASSERT_EQ(cam->getImgHeight(), 480);
-
- cam->setImgWidth(100);
- ASSERT_EQ(cam->getImgWidth() , 100);
-
- cam->setImgHeight(100);
- ASSERT_EQ(cam->getImgHeight(), 100);
-}
-
-TEST(SensorCamera, Intrinsics_Raw_Rectified)
-{
- Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
- IntrinsicsCamera params;
- params.pinhole_model_raw << 321, 241, 321, 321;
- params.pinhole_model_rectified << 320, 240, 320, 320;
- SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params);
-
- Eigen::Matrix3s K_raw, K_rectified;
- K_raw << 321, 0, 321, 0, 321, 241, 0, 0, 1;
- K_rectified << 320, 0, 320, 0, 320, 240, 0, 0, 1;
- Eigen::Vector4s k_raw(321,241,321,321);
- Eigen::Vector4s k_rectified(320,240,320,320);
-
- // default is raw
- ASSERT_TRUE(cam->isUsingRawImages());
- ASSERT_MATRIX_APPROX(K_raw, cam->getIntrinsicMatrix(), 1e-8);
- ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsic()->getState(), 1e-8);
-
- cam->useRectifiedImages();
- ASSERT_FALSE(cam->isUsingRawImages());
- ASSERT_MATRIX_APPROX(K_rectified, cam->getIntrinsicMatrix(), 1e-8);
- ASSERT_MATRIX_APPROX(k_rectified, cam->getIntrinsic()->getState(), 1e-8);
-
- cam->useRawImages();
- ASSERT_TRUE(cam->isUsingRawImages());
- ASSERT_MATRIX_APPROX(K_raw, cam->getIntrinsicMatrix(), 1e-8);
- ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsic()->getState(), 1e-8);
-}
-
-TEST(SensorCamera, Distortion)
-{
- Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
- IntrinsicsCamera params;
- params.width = 640;
- params.height = 480;
- params.pinhole_model_raw << 321, 241, 321, 321;
- params.pinhole_model_rectified << 320, 240, 320, 320;
- params.distortion = Eigen::Vector3s( -0.3, 0.096, 0 );
- SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params);
-
- Eigen::Vector3s d(-0.3, 0.096, 0);
-
- ASSERT_MATRIX_APPROX(d, cam->getDistortionVector(), 1e-8);
-}
-
-TEST(SensorCamera, Correction_zero)
-{
- Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
- IntrinsicsCamera params;
- params.width = 640;
- params.height = 480;
- params.pinhole_model_raw << 321, 241, 321, 321;
- params.pinhole_model_rectified << 320, 240, 320, 320;
- params.distortion = Eigen::Vector3s( 0, 0, 0 );
- SensorCameraPtr cam = std::make_shared<SensorCamera>(extrinsics, params);
-
- Eigen::MatrixXs c(cam->getCorrectionVector().size(), 1);
- c.setZero();
-
- ASSERT_GE(cam->getCorrectionVector().size(), cam->getDistortionVector().size());
- ASSERT_MATRIX_APPROX(c, cam->getCorrectionVector(), 1e-8);
-}
-
-TEST(SensorCamera, create)
-{
- Eigen::VectorXs extrinsics(7); extrinsics << 0,0,0, 0,0,0,1;
- IntrinsicsCameraPtr params = std::make_shared<IntrinsicsCamera>();
- params->width = 640;
- params->height = 480;
- params->pinhole_model_raw << 321, 241, 321, 321;
- params->pinhole_model_rectified << 320, 240, 320, 320;
- params->distortion = Eigen::Vector3s( 0, 0, 0 );
-
- SensorBasePtr sen = SensorCamera::create("camera", extrinsics, params);
-
- ASSERT_NE(sen, nullptr);
-
- SensorCameraPtr cam = std::static_pointer_cast<SensorCamera>(sen);
-
- ASSERT_NE(cam, nullptr);
- ASSERT_EQ(cam->getImgWidth(), 640);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/processor_IMU_UnitTester.cpp b/test/processor_IMU_UnitTester.cpp
deleted file mode 100644
index aa4f46db6..000000000
--- a/test/processor_IMU_UnitTester.cpp
+++ /dev/null
@@ -1,13 +0,0 @@
-#include "processor_IMU_UnitTester.h"
-
-namespace wolf {
-
-ProcessorIMU_UnitTester::ProcessorIMU_UnitTester() :
- ProcessorIMU(std::make_shared<ProcessorParamsIMU>()),
- Dq_out_(nullptr)
-{}
-
-ProcessorIMU_UnitTester::~ProcessorIMU_UnitTester(){}
-
-} // namespace wolf
-
diff --git a/test/processor_IMU_UnitTester.h b/test/processor_IMU_UnitTester.h
deleted file mode 100644
index 2538981e0..000000000
--- a/test/processor_IMU_UnitTester.h
+++ /dev/null
@@ -1,379 +0,0 @@
-
-#ifndef PROCESSOR_IMU_UNITTESTER_H
-#define PROCESSOR_IMU_UNITTESTER_H
-
-// Wolf
-#include "base/processor/processor_IMU.h"
-#include "base/processor/processor_motion.h"
-
-namespace wolf {
- struct IMU_jac_bias{ //struct used for checking jacobians by finite difference
-
- IMU_jac_bias(Eigen::Matrix<Eigen::VectorXs,6,1> _Deltas_noisy_vect,
- Eigen::VectorXs _Delta0 ,
- Eigen::Matrix3s _dDp_dab,
- Eigen::Matrix3s _dDv_dab,
- Eigen::Matrix3s _dDp_dwb,
- Eigen::Matrix3s _dDv_dwb,
- Eigen::Matrix3s _dDq_dwb) :
- Deltas_noisy_vect_(_Deltas_noisy_vect),
- Delta0_(_Delta0) ,
- dDp_dab_(_dDp_dab),
- dDv_dab_(_dDv_dab),
- dDp_dwb_(_dDp_dwb),
- dDv_dwb_(_dDv_dwb),
- dDq_dwb_(_dDq_dwb)
- {
- //
- }
-
- IMU_jac_bias(){
-
- for (int i=0; i<6; i++){
- Deltas_noisy_vect_(i) = Eigen::VectorXs::Zero(1,1);
- }
-
- Delta0_ = Eigen::VectorXs::Zero(1,1);
- dDp_dab_ = Eigen::Matrix3s::Zero();
- dDv_dab_ = Eigen::Matrix3s::Zero();
- dDp_dwb_ = Eigen::Matrix3s::Zero();
- dDv_dwb_ = Eigen::Matrix3s::Zero();
- dDq_dwb_ = Eigen::Matrix3s::Zero();
- }
-
- IMU_jac_bias(IMU_jac_bias const & toCopy){
-
- Deltas_noisy_vect_ = toCopy.Deltas_noisy_vect_;
- Delta0_ = toCopy.Delta0_;
- dDp_dab_ = toCopy.dDp_dab_;
- dDv_dab_ = toCopy.dDv_dab_;
- dDp_dwb_ = toCopy.dDp_dwb_;
- dDv_dwb_ = toCopy.dDv_dwb_;
- dDq_dwb_ = toCopy.dDq_dwb_;
- }
-
- public:
- /*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
- place 1 : added da_bx in data place 2 : added da_by in data place 3 : added da_bz in data
- place 4 : added dw_bx in data place 5 : added dw_by in data place 6 : added dw_bz in data
- */
- Eigen::Matrix<Eigen::VectorXs,6,1> Deltas_noisy_vect_;
- Eigen::VectorXs Delta0_;
- Eigen::Matrix3s dDp_dab_;
- Eigen::Matrix3s dDv_dab_;
- Eigen::Matrix3s dDp_dwb_;
- Eigen::Matrix3s dDv_dwb_;
- Eigen::Matrix3s dDq_dwb_;
-
- public:
- void copyfrom(IMU_jac_bias const& right){
-
- Deltas_noisy_vect_ = right.Deltas_noisy_vect_;
- Delta0_ = right.Delta0_;
- dDp_dab_ = right.dDp_dab_;
- dDv_dab_ = right.dDv_dab_;
- dDp_dwb_ = right.dDp_dwb_;
- dDv_dwb_ = right.dDv_dwb_;
- dDq_dwb_ = right.dDq_dwb_;
- }
- };
-
- struct IMU_jac_deltas{
-
- IMU_jac_deltas(Eigen::VectorXs _Delta0,
- Eigen::VectorXs _delta0,
- Eigen::Matrix<Eigen::VectorXs,9,1> _Delta_noisy_vect,
- Eigen::Matrix<Eigen::VectorXs,9,1> _delta_noisy_vect,
- Eigen::MatrixXs _jacobian_delta_preint,
- Eigen::MatrixXs _jacobian_delta ) :
- Delta0_(_Delta0),
- delta0_(_delta0),
- Delta_noisy_vect_(_Delta_noisy_vect),
- delta_noisy_vect_(_delta_noisy_vect),
- jacobian_delta_preint_(_jacobian_delta_preint),
- jacobian_delta_(_jacobian_delta)
- {
- //
- }
-
- IMU_jac_deltas(){
- for (int i=0; i<9; i++){
- Delta_noisy_vect_(i) = Eigen::VectorXs::Zero(1,1);
- delta_noisy_vect_(i) = Eigen::VectorXs::Zero(1,1);
- }
-
- Delta0_ = Eigen::VectorXs::Zero(1,1);
- delta0_ = Eigen::VectorXs::Zero(1,1);
- jacobian_delta_preint_ = Eigen::MatrixXs::Zero(9,9);
- jacobian_delta_ = Eigen::MatrixXs::Zero(9,9);
- }
-
- IMU_jac_deltas(IMU_jac_deltas const & toCopy){
-
- Delta_noisy_vect_ = toCopy.Delta_noisy_vect_;
- delta_noisy_vect_ = toCopy.delta_noisy_vect_;
-
- Delta0_ = toCopy.Delta0_;
- delta0_ = toCopy.delta0_;
- jacobian_delta_preint_ = toCopy.jacobian_delta_preint_;
- jacobian_delta_ = toCopy.jacobian_delta_;
- }
-
- public:
- /*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
- Elements at place 0 are those not affected by the bias noise that we add (Delta_noise, delta_noise -> dPx, dpx, dVx, dvx,..., dOz,doz).
- Delta_noisy_vect_ delta_noisy_vect_
- 0: + 0, 0: + 0
- 1: +dPx, 2: +dPy, 3: +dPz 1: + dpx, 2: +dpy, 3: +dpz
- 4: +dOx, 5: +dOy, 6: +dOz 4: + dox, 5: +doy, 6: +doz
- 7: +dVx, 8: +dVy, 9: +dVz 7: + dvx, 9: +dvy, +: +dvz
- */
- Eigen::VectorXs Delta0_; //this will contain the Delta not affected by noise
- Eigen::VectorXs delta0_; //this will contain the delta not affected by noise
- Eigen::Matrix<Eigen::VectorXs,9,1> Delta_noisy_vect_; //this will contain the Deltas affected by noises
- Eigen::Matrix<Eigen::VectorXs,9,1> delta_noisy_vect_; //this will contain the deltas affected by noises
- Eigen::MatrixXs jacobian_delta_preint_;
- Eigen::MatrixXs jacobian_delta_;
-
- public:
- void copyfrom(IMU_jac_deltas const& right){
-
- Delta_noisy_vect_ = right.Delta_noisy_vect_;
- delta_noisy_vect_ = right.delta_noisy_vect_;
- Delta0_ = right.Delta0_;
- delta0_ = right.delta0_;
- jacobian_delta_preint_ = right.jacobian_delta_preint_;
- jacobian_delta_ = right.jacobian_delta_;
- }
- };
-
- class ProcessorIMU_UnitTester : public ProcessorIMU{
-
- public:
-
- ProcessorIMU_UnitTester();
- virtual ~ProcessorIMU_UnitTester();
-
- //Functions to test jacobians with finite difference method
-
- /* params :
- _data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
- _dt : time interval between 2 IMU measurements
- da_b : bias noise to add - scalar because adding the same noise to each component of bias (abx, aby, abz, wbx, wby, wbz) one by one.
- */
- IMU_jac_bias finite_diff_ab(const Scalar _dt,
- Eigen::Vector6s& _data,
- const wolf::Scalar& da_b,
- const Eigen::Matrix<wolf::Scalar,10,1>& _delta_preint0);
-
- /* params :
- _data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
- _dt : time interval between 2 IMU measurements
- _Delta_noise : noise to add to Delta_preint (D1 in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
- _delta_noise : noise to add to instantaneous delta (d in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
- */
- IMU_jac_deltas finite_diff_noise(const Scalar& _dt,
- Eigen::Vector6s& _data,
- const Eigen::Matrix<wolf::Scalar,9,1>& _Delta_noise,
- const Eigen::Matrix<wolf::Scalar,9,1>& _delta_noise,
- const Eigen::Matrix<wolf::Scalar,10,1>& _Delta0);
-
- public:
- static ProcessorBasePtr create(const std::string& _unique_name,
- const ProcessorParamsBasePtr _params,
- const SensorBasePtr = nullptr);
-
- public:
- // Maps quat, to be used as temporary
- Eigen::Map<Eigen::Quaternions> Dq_out_;
-
- };
-
-}
-
-/////////////////////////////////////////////////////////
-// IMPLEMENTATION. Put your implementation includes here
-/////////////////////////////////////////////////////////
-
-// Wolf
-#include "base/state_block/state_block.h"
-#include "base/math/rotations.h"
-
-namespace wolf{
-
- //Functions to test jacobians with finite difference method
-inline IMU_jac_bias ProcessorIMU_UnitTester::finite_diff_ab(const Scalar _dt,
- Eigen::Vector6s& _data,
- const wolf::Scalar& da_b,
- const Eigen::Matrix<wolf::Scalar,10,1>& _delta_preint0)
-{
- //TODO : need to use a reset function here to make sure jacobians have not been used before --> reset everything
- ///Define all the needed variables
- Eigen::VectorXs Delta0;
- Eigen::Matrix<Eigen::VectorXs,6,1> Deltas_noisy_vect;
- Eigen::Vector6s data0;
- data0 = _data;
-
- Eigen::MatrixXs data_cov;
- Eigen::MatrixXs jacobian_delta_preint;
- Eigen::MatrixXs jacobian_delta;
- Eigen::VectorXs delta_preint_plus_delta0;
- data_cov.resize(6,6);
- jacobian_delta_preint.resize(9,9);
- jacobian_delta.resize(9,9);
- delta_preint_plus_delta0.resize(10);
-
- //set all variables to 0
- data_cov = Eigen::MatrixXs::Zero(6,6);
- jacobian_delta_preint = Eigen::MatrixXs::Zero(9,9);
- jacobian_delta = Eigen::MatrixXs::Zero(9,9);
- delta_preint_plus_delta0 << 0,0,0, 0,0,0,1 ,0,0,0; //PQV
-
- Vector6s bias = Vector6s::Zero();
-
- /*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
- place 1 : added da_bx in data place 2 : added da_by in data place 3 : added da_bz in data
- place 4 : added dw_bx in data place 5 : added dw_by in data place 6 : added dw_bz in data
- */
-
- Eigen::Matrix3s dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb;
-
- //Deltas without use of da_b
- computeCurrentDelta(_data, data_cov, bias, _dt,delta_,delta_cov_,jacobian_delta_calib_);
- deltaPlusDelta(_delta_preint0, delta_, _dt, delta_preint_plus_delta0, jacobian_delta_preint, jacobian_delta);
- MatrixXs jacobian_bias = jacobian_delta * jacobian_delta_calib_;
- Delta0 = delta_preint_plus_delta0; //this is the first preintegrated delta, not affected by any added bias noise
- dDp_dab = jacobian_bias.block(0,0,3,3);
- dDp_dwb = jacobian_bias.block(0,3,3,3);
- dDq_dwb = jacobian_bias.block(3,3,3,3);
- dDv_dab = jacobian_bias.block(6,0,3,3);
- dDv_dwb = jacobian_bias.block(6,3,3,3);
-
-
- // propagate bias noise
- for(int i=0; i<6; i++){
- //need to reset stuff here
- delta_preint_plus_delta0 << 0,0,0, 0,0,0,1 ,0,0,0; //PQV
- data_cov = Eigen::MatrixXs::Zero(6,6);
-
- // add da_b
- _data = data0;
- _data(i) = _data(i) - da_b; //- because a = a_m − a_b + a_n, in out case, a = a_m − a_b - da_b + a_n
- //data2delta
- computeCurrentDelta(_data, data_cov, bias, _dt, delta_, delta_cov_, jacobian_delta_calib_);
- deltaPlusDelta(_delta_preint0, delta_, _dt, delta_preint_plus_delta0, jacobian_delta_preint, jacobian_delta);
- Deltas_noisy_vect(i) = delta_preint_plus_delta0; //preintegrated deltas affected by added bias noise
- }
-
- IMU_jac_bias bias_jacobians(Deltas_noisy_vect, Delta0, dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb);
- return bias_jacobians;
-}
-
-inline IMU_jac_deltas ProcessorIMU_UnitTester::finite_diff_noise(const Scalar& _dt, Eigen::Vector6s& _data, const Eigen::Matrix<wolf::Scalar,9,1>& _Delta_noise, const Eigen::Matrix<wolf::Scalar,9,1>& _delta_noise, const Eigen::Matrix<wolf::Scalar,10,1>& _Delta0)
-{
- //we do not propagate any noise from data vector
- Eigen::VectorXs Delta_; //will contain the preintegrated Delta affected by added noise
- Eigen::VectorXs delta0; //will contain the delta not affected by added noise
- // delta_ that /will contain the delta affected by added noise is declared in processor_motion.h
- Eigen::VectorXs delta_preint_plus_delta;
- delta0.resize(10);
- delta_preint_plus_delta.resize(10);
- delta_preint_plus_delta << 0,0,0 ,0,0,0,1 ,0,0,0;
-
- Eigen::MatrixXs jacobian_delta_preint; //will be used as input for deltaPlusDelta
- Eigen::MatrixXs jacobian_delta; //will be used as input for deltaPlusDelta
- jacobian_delta_preint.resize(9,9);
- jacobian_delta.resize(9,9);
- jacobian_delta_preint.setIdentity(9,9);
- jacobian_delta.setIdentity(9,9);
- Eigen::MatrixXs jacobian_delta_preint0; //will contain the jacobian we want to check
- Eigen::MatrixXs jacobian_delta0; //will contain the jacobian we want to check
- jacobian_delta_preint0.resize(9,9);
- jacobian_delta0.resize(9,9);
- jacobian_delta_preint0.setIdentity(9,9);
- jacobian_delta0.setIdentity(9,9);
-
- Eigen::MatrixXs data_cov; //will be used filled with Zeros as input for data2delta
- data_cov.resize(6,6);
- data_cov = Eigen::MatrixXs::Zero(6,6);
-
- Eigen::Matrix<Eigen::VectorXs,9,1> Delta_noisy_vect; //this will contain the Deltas affected by noises
- Eigen::Matrix<Eigen::VectorXs,9,1> delta_noisy_vect; //this will contain the deltas affected by noises
-
- Vector6s bias = Vector6s::Zero();
-
- computeCurrentDelta(_data, data_cov, bias,_dt,delta_,delta_cov_,jacobian_delta_calib_); //Affects dp_out, dv_out and dq_out
- delta0 = delta_; //save the delta that is not affected by noise
- deltaPlusDelta(_Delta0, delta0, _dt, delta_preint_plus_delta, jacobian_delta_preint, jacobian_delta);
- jacobian_delta_preint0 = jacobian_delta_preint;
- jacobian_delta0 = jacobian_delta;
-
- //We compute all the jacobians wrt deltas and noisy deltas
- for(int i=0; i<3; i++) //for 3 first and 3 last components we just add to add noise as vector component since it is in the R^3 space
- {
- //PQV formulation
- //Add perturbation in position
- delta_ = delta0;
- delta_(i) = delta_(i) + _delta_noise(i); //noise has been added
- delta_noisy_vect(i) = delta_;
-
- //Add perturbation in velocity
- /*
- delta_ is size 10 (P Q V), _delta_noise is size 9 (P O V)
- */
- delta_ = delta0;
- delta_(i+7) = delta_(i+7) + _delta_noise(i+6); //noise has been added
- delta_noisy_vect(i+6) = delta_;
- }
-
- for(int i=0; i<3; i++) //for noise dtheta, it is in SO3, need to work on quaternions
- {
- //PQV formulation
- //fist we need to reset some stuff
- Eigen::Vector3s dtheta = Eigen::Vector3s::Zero();
-
- delta_ = delta0;
- new (&Dq_out_) Map<Quaternions>(delta_.data() + 3); //not sure that we need this
- dtheta(i) += _delta_noise(i+3); //introduce perturbation
- Dq_out_ = Dq_out_ * v2q(dtheta);
- delta_noisy_vect(i+3) = delta_;
- }
-
- //We compute all the jacobians wrt Deltas and noisy Deltas
- for(int i=0; i<3; i++) //for 3 first and 3 last components we just add to add noise as vector component since it is in the R^3 space
- {
- //PQV formulation
- //Add perturbation in position
- Delta_ = _Delta0;
- Delta_(i) = Delta_(i) + _Delta_noise(i); //noise has been added
- Delta_noisy_vect(i) = Delta_;
-
- //Add perturbation in velocity
- /*
- Delta_ is size 10 (P Q V), _Delta_noise is size 9 (P O V)
- */
- Delta_ = _Delta0;
- Delta_(i+7) = Delta_(i+7) + _Delta_noise(i+6); //noise has been added
- Delta_noisy_vect(i+6) = Delta_;
- }
-
- for(int i=0; i<3; i++) //for noise dtheta, it is in SO3, need to work on quaternions
- {
- //fist we need to reset some stuff
- Eigen::Vector3s dtheta = Eigen::Vector3s::Zero();
-
- Delta_ = _Delta0;
- new (&Dq_out_) Map<Quaternions>(Delta_.data() + 3);
- dtheta(i) += _Delta_noise(i+3); //introduce perturbation
- Dq_out_ = Dq_out_ * v2q(dtheta);
- Delta_noisy_vect(i+3) = Delta_;
- }
-
- IMU_jac_deltas jac_deltas(_Delta0, delta0, Delta_noisy_vect, delta_noisy_vect, jacobian_delta_preint0, jacobian_delta0);
- return jac_deltas;
-
-}
-
-} // namespace wolf
-
-#endif // PROCESSOR_IMU_UNITTESTER_H
--
GitLab