From d7829995ee0384cbfdfe25f294335ff0168e5a32 Mon Sep 17 00:00:00 2001
From: Joaquim Casals <jcasals@iri.upc.edu>
Date: Thu, 25 Apr 2019 15:28:29 +0200
Subject: [PATCH] Adapted branch to new code organization
---
CMakeLists.txt | 18 +-
hello_plugin/hello_plugin.cpp | 20 +-
hello_plugin/params_autoconf.cpp | 6 +-
hello_wolf/sensor_range_bearing.h | 2 +-
include/base/common/wolf.h | 2 +-
include/base/problem/problem.h | 2 +-
include/base/processor/processor_base.h | 2 +-
include/base/processor/processor_diff_drive.h | 2 +-
.../processor_frame_nearest_neighbor_filter.h | 2 +-
include/base/processor/processor_motion.h | 2 +-
include/base/processor/processor_odom_2D.h | 2 +-
.../base/processor/processor_params_image.h | 2 +-
include/base/processor/processor_tracker.h | 5 +-
.../processor_tracker_feature_corner.h | 2 +-
.../processor_tracker_feature_trifocal.h | 2 +-
include/base/sensor/sensor_GPS_fix.h | 2 +-
include/base/sensor/sensor_IMU.h | 2 +-
include/base/sensor/sensor_camera.h | 2 +-
include/base/sensor/sensor_diff_drive.h | 2 +-
include/base/sensor/sensor_odom_2D.h | 2 +-
include/base/sensor/sensor_odom_3D.h | 2 +-
include/base/solver/solver_factory.h | 2 +-
include/base/three_D_tools.h | 375 ------------------
include/base/{ => utils}/converter.h | 0
include/base/{ => utils}/loader.hpp | 0
include/base/{ => utils}/params_server.hpp | 2 +-
test/gtest_converter.cpp | 2 +-
test/gtest_param_server.cpp | 6 +-
test/gtest_parser_yaml.cpp | 4 +-
29 files changed, 54 insertions(+), 420 deletions(-)
delete mode 100644 include/base/three_D_tools.h
rename include/base/{ => utils}/converter.h (100%)
rename include/base/{ => utils}/loader.hpp (100%)
rename include/base/{ => utils}/params_server.hpp (99%)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f688e5267..b65f0c839 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -273,6 +273,9 @@ SET(HDRS_UTILS
include/base/utils/logging.h
include/base/utils/singleton.h
include/base/utils/make_unique.h
+ include/base/utils/params_server.hpp
+ include/base/utils/converter.h
+ include/base/utils/loader.hpp
)
SET(HDRS_PROBLEM
include/base/problem/problem.h
@@ -429,15 +432,15 @@ SET(HDRS_DTASSC
include/base/association/association_nnls.h
include/base/association/matrix.h
)
+SET(HDRS_YAML
+ include/base/yaml/parser_yaml.hpp
+ )
#SOURCES
SET(SRCS_PROBLEM
src/problem/problem.cpp
)
-SET(HDRS_YAML
- include/base/yaml/parser_yaml.hpp
- )
SET(SRCS_HARDWARE
src/hardware/hardware_base.cpp
)
@@ -480,6 +483,7 @@ SET(SRCS_CAPTURE
src/capture/capture_odom_3D.cpp
src/capture/capture_velocity.cpp
src/capture/capture_wheel_joint_position.cpp
+ src/capture/capture_image.cpp
)
SET(SRCS_FACTOR
src/factor/factor_analytic.cpp
@@ -495,6 +499,7 @@ SET(SRCS_FEATURE
src/feature/feature_odom_2D.cpp
src/feature/feature_polyline_2D.cpp
src/feature/feature_pose.cpp
+ src/feature/feature_point_image.cpp
)
SET(SRCS_LANDMARK
src/landmark/landmark_corner_2D.cpp
@@ -614,10 +619,6 @@ IF (vision_utils_FOUND)
include/base/landmark/landmark_point_3D.h
include/base/landmark/landmark_AHP.h
)
- SET(SRCS ${SRCS}
- src/capture/capture_image.cpp
- src/feature/feature_point_image.cpp
- )
SET(SRCS_LANDMARK ${SRCS_LANDMARK}
src/landmark/landmark_point_3D.cpp
src/landmark/landmark_AHP.cpp
@@ -655,7 +656,7 @@ IF(YAMLCPP_FOUND)
src/yaml/sensor_IMU_yaml.cpp
)
IF(laser_scan_utils_FOUND)
- SET(SRCS ${SRCS_YAML}
+ SET(SRCS_YAML ${SRCS_YAML}
src/yaml/sensor_laser_2D_yaml.cpp
)
ENDIF(laser_scan_utils_FOUND)
@@ -670,6 +671,7 @@ ENDIF(YAMLCPP_FOUND)
# create the shared library
ADD_LIBRARY(${PROJECT_NAME}
SHARED
+ ${SRCS}
${SRCS_BASE}
${SRCS_CAPTURE}
${SRCS_COMMON}
diff --git a/hello_plugin/hello_plugin.cpp b/hello_plugin/hello_plugin.cpp
index fbe3955dc..e02c232a8 100644
--- a/hello_plugin/hello_plugin.cpp
+++ b/hello_plugin/hello_plugin.cpp
@@ -5,17 +5,17 @@
* Author: jcasals
*/
#include "base/sensor/sensor_base.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
// #include "sensor_odom_2D.cpp"
#include <yaml-cpp/yaml.h>
#include "base/yaml/parser_yaml.hpp"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
#include "../hello_wolf/capture_range_bearing.h"
#include "../hello_wolf/feature_range_bearing.h"
#include "../hello_wolf/factor_range_bearing.h"
#include "../hello_wolf/landmark_point_2D.h"
-#include "base/loader.hpp"
+#include "base/utils/loader.hpp"
#include "base/processor/processor_odom_2D.h"
#include "base/solver/solver_factory.h"
@@ -162,11 +162,17 @@ int main(int argc, char** argv) {
// GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- for (auto kf : problem->getTrajectory()->getFrameList())
+ for (auto kf : problem->getTrajectory()->getFrameList()){
if (kf->isKey())
- WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance());
- for (auto lmk : problem->getMap()->getLandmarkList())
- WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance());
+ {
+ Eigen::MatrixXs cov;
+ WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance(cov));
+ }
+ for (auto lmk : problem->getMap()->getLandmarkList()) {
+ Eigen::MatrixXs cov;
+ WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance(cov));
+ }
+ }
std::cout << std::endl;
WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
diff --git a/hello_plugin/params_autoconf.cpp b/hello_plugin/params_autoconf.cpp
index 70f562ffc..189bc777f 100644
--- a/hello_plugin/params_autoconf.cpp
+++ b/hello_plugin/params_autoconf.cpp
@@ -5,17 +5,17 @@
* Author: jcasals
*/
#include "base/sensor/sensor_base.h"
-#include "base/wolf.h"
+#include "base/common/wolf.h"
// #include "sensor_odom_2D.cpp"
#include <yaml-cpp/yaml.h>
#include "base/yaml/parser_yaml.hpp"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
#include "../hello_wolf/capture_range_bearing.h"
#include "../hello_wolf/feature_range_bearing.h"
#include "../hello_wolf/factor_range_bearing.h"
#include "../hello_wolf/landmark_point_2D.h"
-#include "base/loader.hpp"
+#include "base/utils/loader.hpp"
#include "base/processor/processor_odom_2D.h"
#include "base/solver/solver_factory.h"
diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h
index 29adf8ae1..5eaebc6b3 100644
--- a/hello_wolf/sensor_range_bearing.h
+++ b/hello_wolf/sensor_range_bearing.h
@@ -9,7 +9,7 @@
#define HELLO_WOLF_SENSOR_RANGE_BEARING_H_
#include "base/sensor/sensor_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
namespace wolf
{
diff --git a/include/base/common/wolf.h b/include/base/common/wolf.h
index c0a9a9a2d..3d5ea9efc 100644
--- a/include/base/common/wolf.h
+++ b/include/base/common/wolf.h
@@ -10,7 +10,7 @@
// Enable project-specific definitions and macros
#include "internal/config.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
#include "base/utils/logging.h"
//includes from Eigen lib
diff --git a/include/base/problem/problem.h b/include/base/problem/problem.h
index d79f730cd..156077ec8 100644
--- a/include/base/problem/problem.h
+++ b/include/base/problem/problem.h
@@ -14,7 +14,7 @@ struct ProcessorParamsBase;
}
//wolf includes
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
#include "base/sensor/new_sensor_factory.h"
#include "base/processor/new_processor_factory.h"
#include "base/common/wolf.h"
diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h
index aa6aaf7ac..a188d8178 100644
--- a/include/base/processor/processor_base.h
+++ b/include/base/processor/processor_base.h
@@ -7,7 +7,7 @@ class SensorBase;
}
// Wolf includes
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
#include "base/common/wolf.h"
#include "base/common/node_base.h"
#include "base/common/time_stamp.h"
diff --git a/include/base/processor/processor_diff_drive.h b/include/base/processor/processor_diff_drive.h
index a486c7218..87fc7f6bc 100644
--- a/include/base/processor/processor_diff_drive.h
+++ b/include/base/processor/processor_diff_drive.h
@@ -9,7 +9,7 @@
#define _WOLF_PROCESSOR_DIFF_DRIVE_H_
#include "base/processor/processor_motion.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
#include "base/processor/diff_drive_tools.h"
namespace wolf {
diff --git a/include/base/processor/processor_frame_nearest_neighbor_filter.h b/include/base/processor/processor_frame_nearest_neighbor_filter.h
index 619b88b79..219d03864 100644
--- a/include/base/processor/processor_frame_nearest_neighbor_filter.h
+++ b/include/base/processor/processor_frame_nearest_neighbor_filter.h
@@ -3,7 +3,7 @@
// Wolf related headers
#include "base/processor/processor_loopclosure_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
#include "base/state_block/state_block.h"
namespace wolf{
diff --git a/include/base/processor/processor_motion.h b/include/base/processor/processor_motion.h
index 805ef9261..7faf2ecc0 100644
--- a/include/base/processor/processor_motion.h
+++ b/include/base/processor/processor_motion.h
@@ -11,7 +11,7 @@
// Wolf
#include "base/capture/capture_motion.h"
#include "base/processor/processor_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
#include "base/common/time_stamp.h"
// std
diff --git a/include/base/processor/processor_odom_2D.h b/include/base/processor/processor_odom_2D.h
index 5f4718c23..5e0eff3a5 100644
--- a/include/base/processor/processor_odom_2D.h
+++ b/include/base/processor/processor_odom_2D.h
@@ -11,7 +11,7 @@
#include "base/processor/processor_motion.h"
#include "base/capture/capture_odom_2D.h"
#include "base/factor/factor_odom_2D.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
#include "base/math/rotations.h"
namespace wolf {
diff --git a/include/base/processor/processor_params_image.h b/include/base/processor/processor_params_image.h
index 33e92c38e..f493db6ec 100644
--- a/include/base/processor/processor_params_image.h
+++ b/include/base/processor/processor_params_image.h
@@ -4,7 +4,7 @@
// wolf
#include "base/processor/processor_tracker_feature.h"
#include "base/processor/processor_tracker_landmark.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
namespace wolf
{
diff --git a/include/base/processor/processor_tracker.h b/include/base/processor/processor_tracker.h
index ea414cfff..b583db352 100644
--- a/include/base/processor/processor_tracker.h
+++ b/include/base/processor/processor_tracker.h
@@ -18,13 +18,14 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTracker);
struct ProcessorParamsTracker : public ProcessorParamsBase
{
unsigned int min_features_for_keyframe; ///< minimum nbr. of features to vote for keyframe
+ int max_new_features; ///< maximum nbr. of new features to be processed when adding a keyframe (-1: unlimited. 0: none.)
+
ProcessorParamsTracker() = default;
ProcessorParamsTracker(std::string _unique_name, const wolf::paramsServer & _server):
ProcessorParamsBase(_unique_name, _server)
{
min_features_for_keyframe = _server.getParam<unsigned int>(_unique_name + "/min_features_for_keyframe", "1");
- max_new_features = _server.getParam<unsigned int>(_unique_name + "/max_new_features", "10");
- }
+ max_new_features = _server.getParam<int>(_unique_name + "/max_new_features", "-1");
}
};
diff --git a/include/base/processor/processor_tracker_feature_corner.h b/include/base/processor/processor_tracker_feature_corner.h
index 5ab8b2027..a5b7e107f 100644
--- a/include/base/processor/processor_tracker_feature_corner.h
+++ b/include/base/processor/processor_tracker_feature_corner.h
@@ -17,7 +17,7 @@
#include "base/state_block/state_block.h"
#include "base/association/association_tree.h"
#include "base/processor/processor_tracker_feature.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
//laser_scan_utils
//#include "laser_scan_utils/scan_basics.h"
diff --git a/include/base/processor/processor_tracker_feature_trifocal.h b/include/base/processor/processor_tracker_feature_trifocal.h
index a017004cd..7a71a038f 100644
--- a/include/base/processor/processor_tracker_feature_trifocal.h
+++ b/include/base/processor/processor_tracker_feature_trifocal.h
@@ -4,7 +4,7 @@
//Wolf includes
#include "base/processor/processor_tracker_feature.h"
#include "base/capture/capture_image.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
// Vision utils
#include <vision_utils.h>
diff --git a/include/base/sensor/sensor_GPS_fix.h b/include/base/sensor/sensor_GPS_fix.h
index dd2e85700..88b2e54d8 100644
--- a/include/base/sensor/sensor_GPS_fix.h
+++ b/include/base/sensor/sensor_GPS_fix.h
@@ -4,7 +4,7 @@
//wolf includes
#include "base/sensor/sensor_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
// std includes
diff --git a/include/base/sensor/sensor_IMU.h b/include/base/sensor/sensor_IMU.h
index 4ae415566..c9dc9ad4b 100644
--- a/include/base/sensor/sensor_IMU.h
+++ b/include/base/sensor/sensor_IMU.h
@@ -3,7 +3,7 @@
//wolf includes
#include "base/sensor/sensor_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
#include <iostream>
namespace wolf {
diff --git a/include/base/sensor/sensor_camera.h b/include/base/sensor/sensor_camera.h
index 44bd1dd0b..04d2bd8aa 100644
--- a/include/base/sensor/sensor_camera.h
+++ b/include/base/sensor/sensor_camera.h
@@ -3,7 +3,7 @@
//wolf includes
#include "base/sensor/sensor_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
namespace wolf
{
diff --git a/include/base/sensor/sensor_diff_drive.h b/include/base/sensor/sensor_diff_drive.h
index fd0a80110..e84be0627 100644
--- a/include/base/sensor/sensor_diff_drive.h
+++ b/include/base/sensor/sensor_diff_drive.h
@@ -10,7 +10,7 @@
//wolf includes
#include "base/sensor/sensor_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
#include "base/processor/diff_drive_tools.h"
namespace wolf {
diff --git a/include/base/sensor/sensor_odom_2D.h b/include/base/sensor/sensor_odom_2D.h
index 934ef2615..92b6bb111 100644
--- a/include/base/sensor/sensor_odom_2D.h
+++ b/include/base/sensor/sensor_odom_2D.h
@@ -3,7 +3,7 @@
//wolf includes
#include "base/sensor/sensor_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
namespace wolf {
diff --git a/include/base/sensor/sensor_odom_3D.h b/include/base/sensor/sensor_odom_3D.h
index c1128098e..dfa736fe9 100644
--- a/include/base/sensor/sensor_odom_3D.h
+++ b/include/base/sensor/sensor_odom_3D.h
@@ -10,7 +10,7 @@
//wolf includes
#include "base/sensor/sensor_base.h"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
namespace wolf {
diff --git a/include/base/solver/solver_factory.h b/include/base/solver/solver_factory.h
index 5a0ce6417..8d37673fe 100644
--- a/include/base/solver/solver_factory.h
+++ b/include/base/solver/solver_factory.h
@@ -15,7 +15,7 @@ struct IntrinsicsBase;
}
// wolf
-#include "base/factory.h"
+#include "base/common/factory.h"
#include "base/solver/solver_manager.h"
namespace wolf
diff --git a/include/base/three_D_tools.h b/include/base/three_D_tools.h
deleted file mode 100644
index 86e2086f6..000000000
--- a/include/base/three_D_tools.h
+++ /dev/null
@@ -1,375 +0,0 @@
-/*
- * three_D_tools.h
- *
- * Created on: Mar 15, 2018
- * Author: jsola
- */
-
-#ifndef THREE_D_TOOLS_H_
-#define THREE_D_TOOLS_H_
-
-#include "base/wolf.h"
-#include "base/rotations.h"
-
-/*
- * The functions in this file are related to manipulations of Delta motion magnitudes used in 3D motion.
- *
- * The Delta is defined as a simple 3D pose with the rotation expressed in quaternion form,
- * Delta = [Dp, Dq]
- * with
- * Dp : position delta
- * Dq : quaternion delta
- *
- * The functions are listed below:
- *
- * - compose: Dc = D1 (+) D2
- * - identity: I = Delta at the origin, with Dp = [0,0,0]; Dq = [0,0,0,1], so that D (+) I = I (+) D = D
- * - inverse: so that D (+) D.inv = D.inv (+) D = I
- * - between: Db = D2 (-) D1 := D1.inv (+) D2, so that D2 = D1 (+) Db
- * - lift: go from Delta manifold to tangent space (equivalent to log() in rotations)
- * - retract: go from tangent space to delta manifold (equivalent to exp() in rotations)
- * - plus: D2 = D1 (+) retract(d)
- * - diff: d = lift( D2 (-) D1 )
- */
-
-namespace wolf
-{
-namespace three_D {
-using namespace Eigen;
-
-template<typename D1, typename D2>
-inline void identity(MatrixBase<D1>& p, QuaternionBase<D2>& q)
-{
- MatrixSizeCheck<3, 1>::check(p);
- p = MatrixBase<D1>::Zero(3,1);
- q = QuaternionBase<D2>::Identity();
-}
-
-template<typename D1, typename D2>
-inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& q)
-{
- MatrixSizeCheck<3, 1>::check(p);
- MatrixSizeCheck<4, 1>::check(q);
- typedef typename D1::Scalar T1;
- typedef typename D2::Scalar T2;
- p << T1(0), T1(0), T1(0);
- q << T2(0), T2(0), T2(0), T2(1);
-}
-
-template<typename T = Scalar>
-inline Matrix<T, 7, 1> identity()
-{
- Matrix<T, 7, 1> ret;
- ret<< T(0), T(0), T(0),
- T(0), T(0), T(0), T(1);
- return ret;
-}
-
-template<typename D1, typename D2, typename D4, typename D5>
-inline void inverse(const MatrixBase<D1>& dp, const QuaternionBase<D2>& dq,
- MatrixBase<D4>& idp, QuaternionBase<D5>& idq)
-{
- MatrixSizeCheck<3, 1>::check(dp);
- MatrixSizeCheck<3, 1>::check(idp);
-
- idp = - dq.conjugate() * dp ;
- idq = dq.conjugate() ;
-}
-
-template<typename D1, typename D2>
-inline void inverse(const MatrixBase<D1>& d,
- MatrixBase<D2>& id)
-{
- MatrixSizeCheck<7, 1>::check(d);
- MatrixSizeCheck<7, 1>::check(id);
-
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp ( & d( 0 ) );
- Map<const Quaternion<typename D1::Scalar> > dq ( & d( 3 ) );
- Map<Matrix<typename D2::Scalar, 3, 1> > idp ( & id( 0 ) );
- Map<Quaternion<typename D2::Scalar> > idq ( & id( 3 ) );
-
- inverse(dp, dq, idp, idq);
-}
-
-template<typename D>
-inline Matrix<typename D::Scalar, 7, 1> inverse(const MatrixBase<D>& d)
-{
- Matrix<typename D::Scalar, 7, 1> id;
- inverse(d, id);
- return id;
-}
-
-template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void compose(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D7>& sum_p, QuaternionBase<D8>& sum_q )
-{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dp2);
- MatrixSizeCheck<3, 1>::check(sum_p);
-
- sum_p = dp1 + dq1*dp2;
- sum_q = dq1*dq2; // dq here to avoid possible aliasing between d1 and sum
-}
-
-template<typename D1, typename D2, typename D3>
-inline void compose(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& sum)
-{
- MatrixSizeCheck<7, 1>::check(d1);
- MatrixSizeCheck<7, 1>::check(d2);
- MatrixSizeCheck<7, 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 D2::Scalar, 3, 1> > dp2 ( & d2( 0 ) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2( 3 ) );
- Map<Matrix<typename D3::Scalar, 3, 1> > sum_p ( & sum( 0 ) );
- Map<Quaternion<typename D3::Scalar> > sum_q ( & sum( 3 ) );
-
- compose(dp1, dq1, dp2, dq2, sum_p, sum_q);
-}
-
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 7, 1> compose(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2 )
-{
- Matrix<typename D1::Scalar, 7, 1> ret;
- compose(d1, d2, ret);
- return ret;
-}
-
-template<typename D1, typename D2, typename D3, typename D4, typename D5>
-inline void compose(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& sum,
- MatrixBase<D4>& J_sum_d1,
- MatrixBase<D5>& J_sum_d2)
-{
- MatrixSizeCheck<7, 1>::check(d1);
- MatrixSizeCheck<7, 1>::check(d2);
- MatrixSizeCheck<7, 1>::check(sum);
- MatrixSizeCheck< 6, 6>::check(J_sum_d1);
- MatrixSizeCheck< 6, 6>::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(3,3,3,3) = dR2.transpose(); // dDo'/dDo
-
- // Jac wrt second delta
- J_sum_d2.setIdentity(); //
- J_sum_d2.block(0,0,3,3) = dR1; // dDp'/ddp
- // 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, sum);
-}
-
-template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void between(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D7>& dp12, QuaternionBase<D8>& dq12)
-{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dp2);
- MatrixSizeCheck<3, 1>::check(dp12);
-
- dp12 = dq1.conjugate() * ( dp2 - dp1 );
- dq12 = dq1.conjugate() * dq2;
-}
-
-template<typename D1, typename D2, typename D3>
-inline void between(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2,
- MatrixBase<D3>& d2_minus_d1)
-{
- MatrixSizeCheck<7, 1>::check(d1);
- MatrixSizeCheck<7, 1>::check(d2);
- MatrixSizeCheck<7, 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 D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dp12 ( & d2_minus_d1(0) );
- Map<Quaternion<typename D3::Scalar> > dq12 ( & d2_minus_d1(3) );
-
- between(dp1, dq1, dp2, dq2, dp12, dq12);
-}
-
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 7, 1> between(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2 )
-{
- MatrixSizeCheck<7, 1>::check(d1);
- MatrixSizeCheck<7, 1>::check(d2);
- Matrix<typename D1::Scalar, 7, 1> d12;
- between(d1, d2, d12);
- return d12;
-}
-
-template<typename Derived>
-Matrix<typename Derived::Scalar, 6, 1> lift(const MatrixBase<Derived>& delta_in)
-{
- MatrixSizeCheck<7, 1>::check(delta_in);
-
- Matrix<typename Derived::Scalar, 6, 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<Matrix<typename Derived::Scalar, 3, 1> > dp_ret ( & ret(0) );
- Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) );
-
- dp_ret = dp_in;
- do_ret = log_q(dq_in);
-
- return ret;
-}
-
-template<typename Derived>
-Matrix<typename Derived::Scalar, 7, 1> retract(const MatrixBase<Derived>& d_in)
-{
- MatrixSizeCheck<6, 1>::check(d_in);
-
- Matrix<typename Derived::Scalar, 7, 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<Matrix<typename Derived::Scalar, 3, 1> > dp ( & ret(0) );
- Map<Quaternion<typename Derived::Scalar> > dq ( & ret(3) );
-
- dp = dp_in;
- dq = exp_q(do_in);
-
- return ret;
-}
-
-template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2,
- MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q)
-{
- MatrixSizeCheck<3, 1>::check(dp1);
- MatrixSizeCheck<3, 1>::check(dp2);
- MatrixSizeCheck<3, 1>::check(plus_p);
- plus_p = dp1 + dp2;
- 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_plus)
-{
- Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
- Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Matrix<typename D2::Scalar, 3, 1> > do2 ( & d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > dp_p ( & d_plus(0) );
- Map<Quaternion<typename D3::Scalar> > dq_p ( & d_plus(3) );
-
- plus(dp1, dq1, dp2, do2, dp_p, dq_p);
-}
-
-template<typename D1, typename D2>
-inline Matrix<typename D1::Scalar, 7, 1> plus(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2)
-{
- Matrix<typename D1::Scalar, 7, 1> d_plus;
- plus(d1, d2, d_plus);
- return d_plus;
-}
-
-template<typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
-inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1,
- const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o )
-{
- diff_p = dp2 - dp1;
- diff_o = log_q(dq1.conjugate() * dq2);
-}
-
-template<typename D1, typename D2, 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<D4>& dp2, const QuaternionBase<D5>& dq2,
- MatrixBase<D6>& diff_p, MatrixBase<D7>& diff_o,
- MatrixBase<D8>& J_do_dq1, MatrixBase<D9>& J_do_dq2)
-{
- diff(dp1, dq1, dp2, dq2, diff_p, diff_o);
-
- 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 D2::Scalar, 3, 1> > dp2 ( & d2(0) );
- Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & err(0) );
- Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & err(3) );
-
- diff(dp1, dq1, dp2, dq2, diff_p, diff_o);
-}
-
-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, 6, 6>::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(6,6); // 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, 6, 1> diff(const MatrixBase<D1>& d1,
- const MatrixBase<D2>& d2)
-{
- Matrix<typename D1::Scalar, 6, 1> ret;
- diff(d1, d2, ret);
- return ret;
-}
-
-} // namespace three_d
-} // namespace wolf
-
-#endif /* THREE_D_TOOLS_H_ */
diff --git a/include/base/converter.h b/include/base/utils/converter.h
similarity index 100%
rename from include/base/converter.h
rename to include/base/utils/converter.h
diff --git a/include/base/loader.hpp b/include/base/utils/loader.hpp
similarity index 100%
rename from include/base/loader.hpp
rename to include/base/utils/loader.hpp
diff --git a/include/base/params_server.hpp b/include/base/utils/params_server.hpp
similarity index 99%
rename from include/base/params_server.hpp
rename to include/base/utils/params_server.hpp
index 55f268d78..186bd8286 100644
--- a/include/base/params_server.hpp
+++ b/include/base/utils/params_server.hpp
@@ -3,7 +3,7 @@
#include <vector>
#include <regex>
#include <map>
-#include "base/converter.h"
+#include "base/utils/converter.h"
namespace wolf{
class paramsServer{
struct ParamsInitSensor{
diff --git a/test/gtest_converter.cpp b/test/gtest_converter.cpp
index 6f821cba2..e594458e9 100644
--- a/test/gtest_converter.cpp
+++ b/test/gtest_converter.cpp
@@ -1,5 +1,5 @@
#include "utils_gtest.h"
-#include "base/converter.h"
+#include "base/utils/converter.h"
using namespace std;
using namespace wolf;
diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp
index a17791973..9b80933a1 100644
--- a/test/gtest_param_server.cpp
+++ b/test/gtest_param_server.cpp
@@ -1,8 +1,8 @@
#include "utils_gtest.h"
-#include "base/converter.h"
-#include "base/wolf.h"
+#include "base/utils/converter.h"
+#include "base/common/wolf.h"
#include "base/yaml/parser_yaml.hpp"
-#include "base/params_server.hpp"
+#include "base/utils/params_server.hpp"
using namespace std;
using namespace wolf;
diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
index 437879ad2..006bd5729 100644
--- a/test/gtest_parser_yaml.cpp
+++ b/test/gtest_parser_yaml.cpp
@@ -1,6 +1,6 @@
#include "utils_gtest.h"
-#include "base/converter.h"
-#include "base/wolf.h"
+#include "base/utils/converter.h"
+#include "base/common/wolf.h"
#include "base/yaml/parser_yaml.hpp"
using namespace std;
--
GitLab