Skip to content

Commit ce3dda4

Browse files
Al-khwarizmi-780Al-khwarizmi-780
authored
adding copy of Eigen3 lib as third party and use CRT patter for variant handling (#17)
1 parent 711596d commit ce3dda4

2,050 files changed

Lines changed: 396495 additions & 1003 deletions

File tree

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎CMakeLists.txt‎

Lines changed: 0 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,5 @@
11
cmake_minimum_required(VERSION 3.4)
22

3-
# ============================================================================================
4-
# VCPKG Toolchain
5-
# ============================================================================================
6-
if(WIN32)
7-
# use vcpkg as packages manager in windows platform
8-
# environment variable needs to be added for the path to vcpkg installation "VCPKG_ROOT"
9-
set(CMAKE_TOOLCHAIN_FILE "$ENV{VCPKG_ROOT}/scripts/buildsystems/vcpkg.cmake")
10-
endif(WIN32)
11-
123
# ============================================================================================
134
# ============================================================================================
145
set(CMAKE_FIND_PACKAGE_PREFER_CONFIG ON)
@@ -46,7 +37,6 @@ find_package(Eigen3 3.3 REQUIRED NO_MODULE)
4637

4738
include(CTest)
4839

49-
add_subdirectory(third_party/googletest)
5040
add_subdirectory(src)
5141
add_subdirectory(examples)
5242
add_subdirectory(tests)

‎examples/ego_motion_model_adapter/main.cpp‎

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -26,12 +26,12 @@ using namespace kf;
2626
/// 3-dimension state egomotion model to a higher or lower dimension state
2727
/// vector (e.g. 5-dimension state vector and 3-dimension input vector).
2828
class EgoMotionModelAdapter
29-
: public motionmodel::MotionModelExtInput<DIM_X, DIM_U>
29+
: public motionmodel::MotionModelExtInput<EgoMotionModelAdapter, DIM_X,
30+
DIM_U>
3031
{
3132
public:
32-
virtual Vector<DIM_X> f(
33-
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU,
34-
Vector<DIM_X> const& /*vecQ = Vector<DIM_X>::Zero()*/) const override
33+
Vector<DIM_X> f(Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU,
34+
Vector<DIM_X> const& /*vecQ = Vector<DIM_X>::Zero()*/) const
3535
{
3636
Vector<3> tmpVecX; // \vec{x} = [x, y, yaw]^T
3737
tmpVecX << vecX[0], vecX[1], vecX[4];
@@ -50,8 +50,8 @@ class EgoMotionModelAdapter
5050
return vecXout;
5151
}
5252

53-
virtual Matrix<DIM_X, DIM_X> getProcessNoiseCov(
54-
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU) const override
53+
Matrix<DIM_X, DIM_X> getProcessNoiseCov(Vector<DIM_X> const& vecX,
54+
Vector<DIM_U> const& vecU) const
5555
{
5656
// input idx -> output index mapping
5757
// 0 -> 0
@@ -90,8 +90,8 @@ class EgoMotionModelAdapter
9090
return matQout;
9191
}
9292

93-
virtual Matrix<DIM_X, DIM_X> getInputNoiseCov(
94-
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU) const override
93+
Matrix<DIM_X, DIM_X> getInputNoiseCov(Vector<DIM_X> const& vecX,
94+
Vector<DIM_U> const& vecU) const
9595
{
9696
Vector<3> tmpVecX;
9797
tmpVecX << vecX[0], vecX[1], vecX[4];
@@ -117,8 +117,8 @@ class EgoMotionModelAdapter
117117
return matUout;
118118
}
119119

120-
virtual Matrix<DIM_X, DIM_X> getJacobianFk(
121-
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU) const override
120+
Matrix<DIM_X, DIM_X> getJacobianFk(Vector<DIM_X> const& vecX,
121+
Vector<DIM_U> const& vecU) const
122122
{
123123
Vector<3> tmpVecX;
124124
tmpVecX << vecX[0], vecX[1], vecX[4];
@@ -144,8 +144,8 @@ class EgoMotionModelAdapter
144144
return matFkout;
145145
}
146146

147-
virtual Matrix<DIM_X, DIM_U> getJacobianBk(
148-
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU) const override
147+
Matrix<DIM_X, DIM_U> getJacobianBk(Vector<DIM_X> const& vecX,
148+
Vector<DIM_U> const& vecU) const
149149
{
150150
Vector<3> tmpVecX;
151151
tmpVecX << vecX[0], vecX[1], vecX[4];

‎src/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,7 @@ endif(MSVC)
4747

4848
target_include_directories(${LIBRARY_NAME} PUBLIC
4949
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
50+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/third_party/Eigen3/Eigen>
5051
$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${INCLUDE_INSTALL_DIR}>
5152
)
5253

@@ -90,3 +91,5 @@ install(
9091
EXPORT ${TARGETS_EXPORT_NAME}
9192
DESTINATION ${CONFIG_INSTALL_DIR}
9293
)
94+
95+
add_subdirectory(third_party/googletest)

‎src/kalman_filter/kalman_filter.h‎

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -82,7 +82,8 @@ class KalmanFilter
8282
/// @brief predict state with a linear process model.
8383
/// @param motionModel prediction motion model function
8484
///
85-
void predictEkf(motionmodel::MotionModel<DIM_X> const& motionModel)
85+
template <class Derived>
86+
void predictEkf(motionmodel::MotionModel<Derived, DIM_X> const& motionModel)
8687
{
8788
Matrix<DIM_X, DIM_X> const matFk{motionModel.getJacobianFk(m_vecX)};
8889
Matrix<DIM_X, DIM_X> const matQk{motionModel.getProcessNoiseCov(m_vecX)};
@@ -95,10 +96,10 @@ class KalmanFilter
9596
/// @param motionModel prediction motion model function
9697
/// @param vecU input vector
9798
///
98-
template <int32_t DIM_U>
99-
void predictEkf(
100-
motionmodel::MotionModelExtInput<DIM_X, DIM_U> const& motionModel,
101-
Vector<DIM_U> const& vecU)
99+
template <class Derived, int32_t DIM_U>
100+
void predictEkf(motionmodel::MotionModelExtInput<Derived, DIM_X, DIM_U> const&
101+
motionModel,
102+
Vector<DIM_U> const& vecU)
102103
{
103104
Matrix<DIM_X, DIM_X> const matFk{motionModel.getJacobianFk(m_vecX, vecU)};
104105
Matrix<DIM_X, DIM_X> const matQk{

‎src/motion_model/ego_motion_model.h‎

Lines changed: 11 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ static constexpr int32_t DIM_U{2};
2020
/// Motion model for ego vehicle which is used for dead reckoning purposes
2121
/// by utilizing odometry external inputs vehicle displacement and change
2222
/// in heading angle and incrementing them to obtain the new state.
23-
class EgoMotionModel : public MotionModelExtInput<DIM_X, DIM_U>
23+
class EgoMotionModel : public MotionModelExtInput<EgoMotionModel, DIM_X, DIM_U>
2424
{
2525
public:
2626
EgoMotionModel()
@@ -35,37 +35,36 @@ class EgoMotionModel : public MotionModelExtInput<DIM_X, DIM_U>
3535
/// @param vecU Input space vector \vec{u}
3636
/// @param vecQ State white gaussian noise vector \vec{q}
3737
/// @return Predicted/ propagated state space vector
38-
virtual Vector<DIM_X> f(
39-
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU,
40-
Vector<DIM_X> const& vecQ = Vector<DIM_X>::Zero()) const override;
38+
Vector<DIM_X> f(Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU,
39+
Vector<DIM_X> const& vecQ = Vector<DIM_X>::Zero()) const;
4140

4241
/// @brief Get the process noise covariance Q
4342
/// @param vecX State space vector \vec{x}
4443
/// @param vecU Input space vector \vec{u}
4544
/// @return The process noise covariance Q
46-
virtual Matrix<DIM_X, DIM_X> getProcessNoiseCov(
47-
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU) const override;
45+
Matrix<DIM_X, DIM_X> getProcessNoiseCov(Vector<DIM_X> const& vecX,
46+
Vector<DIM_U> const& vecU) const;
4847

4948
/// @brief Get the input noise covariance U
5049
/// @param vecX State space vector \vec{x}
5150
/// @param vecU Input space vector \vec{u}
5251
/// @return The input noise covariance U
53-
virtual Matrix<DIM_X, DIM_X> getInputNoiseCov(
54-
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU) const override;
52+
Matrix<DIM_X, DIM_X> getInputNoiseCov(Vector<DIM_X> const& vecX,
53+
Vector<DIM_U> const& vecU) const;
5554

5655
/// @brief Method that calculates the jacobians of the state transition model.
5756
/// @param vecX State Space vector \vec{x}
5857
/// @param vecU Input Space vector \vec{u}
5958
/// @return The jacobians of the state transition model.
60-
virtual Matrix<DIM_X, DIM_X> getJacobianFk(
61-
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU) const override;
59+
Matrix<DIM_X, DIM_X> getJacobianFk(Vector<DIM_X> const& vecX,
60+
Vector<DIM_U> const& vecU) const;
6261

6362
/// @brief Method that calculates the jacobians of the input transition model.
6463
/// @param vecX State Space vector \vec{x}
6564
/// @param vecU Input Space vector \vec{u}
6665
/// @return The jacobians of the input transition model.
67-
virtual Matrix<DIM_X, DIM_U> getJacobianBk(
68-
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU) const override;
66+
Matrix<DIM_X, DIM_U> getJacobianBk(Vector<DIM_X> const& vecX,
67+
Vector<DIM_U> const& vecU) const;
6968

7069
/// @brief Setter for noise variance of pose-x state.
7170
/// @param val variance value

‎src/motion_model/motion_model.h‎

Lines changed: 41 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,9 @@ namespace kf
88
namespace motionmodel
99
{
1010
/// @brief Base class for motion models used by kalman filters
11+
/// @tparam Derived Derived class which implement the interfaces
1112
/// @tparam DIM_X State space vector dimension
12-
template <int32_t DIM_X>
13+
template <class Derived, int32_t DIM_X>
1314
class MotionModel
1415
{
1516
public:
@@ -18,28 +19,34 @@ class MotionModel
1819
/// @param vecX State space vector \vec{x}
1920
/// @param vecQ State white gaussian noise vector \vec{q}
2021
/// @return Predicted/ propagated state space vector
21-
virtual Vector<DIM_X> f(
22-
Vector<DIM_X> const& vecX,
23-
Vector<DIM_X> const& vecQ = Vector<DIM_X>::Zero()) const = 0;
22+
Vector<DIM_X> f(Vector<DIM_X> const& vecX,
23+
Vector<DIM_X> const& vecQ = Vector<DIM_X>::Zero()) const
24+
{
25+
return static_cast<Derived const*>(this)->f(vecX, vecQ);
26+
}
2427

2528
/// @brief Get the process noise covariance Q
2629
/// @param vecX State space vector \vec{x}
2730
/// @return The process noise covariance Q
28-
virtual Matrix<DIM_X, DIM_X> getProcessNoiseCov(
29-
Vector<DIM_X> const& vecX) const = 0;
31+
Matrix<DIM_X, DIM_X> getProcessNoiseCov(Vector<DIM_X> const& vecX) const
32+
{
33+
return static_cast<Derived const*>(this)->getProcessNoiseCov(vecX);
34+
}
3035

3136
/// @brief Method that calculates the jacobians of the state transition model.
3237
/// @param vecX State Space vector \vec{x}
3338
/// @return The jacobians of the state transition model.
34-
virtual Matrix<DIM_X, DIM_X> getJacobianFk(
35-
Vector<DIM_X> const& vecX) const = 0;
39+
Matrix<DIM_X, DIM_X> getJacobianFk(Vector<DIM_X> const& vecX) const
40+
{
41+
return static_cast<Derived const*>(this)->getJacobianFk(vecX);
42+
}
3643
};
3744

3845
/// @brief Base class for motion models with external inputs used by kalman
3946
/// filters
4047
/// @tparam DIM_X State space vector dimension
4148
/// @tparam DIM_U Input space vector dimension
42-
template <int32_t DIM_X, int32_t DIM_U>
49+
template <class Derived, int32_t DIM_X, int32_t DIM_U>
4350
class MotionModelExtInput
4451
{
4552
public:
@@ -49,37 +56,51 @@ class MotionModelExtInput
4956
/// @param vecU Input space vector \vec{u}
5057
/// @param vecQ State white gaussian noise vector \vec{q}
5158
/// @return Predicted/ propagated state space vector
52-
virtual Vector<DIM_X> f(
53-
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU,
54-
Vector<DIM_X> const& vecQ = Vector<DIM_X>::Zero()) const = 0;
59+
Vector<DIM_X> f(Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU,
60+
Vector<DIM_X> const& vecQ = Vector<DIM_X>::Zero()) const
61+
{
62+
return static_cast<Derived const*>(this)->f(vecX, vecU, vecQ);
63+
}
5564

5665
/// @brief Get the process noise covariance Q
5766
/// @param vecX State space vector \vec{x}
5867
/// @param vecU Input space vector \vec{u}
5968
/// @return The process noise covariance Q
60-
virtual Matrix<DIM_X, DIM_X> getProcessNoiseCov(
61-
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU) const = 0;
69+
Matrix<DIM_X, DIM_X> getProcessNoiseCov(Vector<DIM_X> const& vecX,
70+
Vector<DIM_U> const& vecU) const
71+
{
72+
return static_cast<Derived const*>(this)->getProcessNoiseCov(vecX, vecU);
73+
}
6274

6375
/// @brief Get the input noise covariance U
6476
/// @param vecX State space vector \vec{x}
6577
/// @param vecU Input space vector \vec{u}
6678
/// @return The input noise covariance U
67-
virtual Matrix<DIM_X, DIM_X> getInputNoiseCov(
68-
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU) const = 0;
79+
Matrix<DIM_X, DIM_X> getInputNoiseCov(Vector<DIM_X> const& vecX,
80+
Vector<DIM_U> const& vecU) const
81+
{
82+
return static_cast<Derived const*>(this)->getInputNoiseCov(vecX, vecU);
83+
}
6984

7085
/// @brief Method that calculates the jacobians of the state transition model.
7186
/// @param vecX State Space vector \vec{x}
7287
/// @param vecU Input Space vector \vec{u}
7388
/// @return The jacobians of the state transition model.
74-
virtual Matrix<DIM_X, DIM_X> getJacobianFk(
75-
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU) const = 0;
89+
Matrix<DIM_X, DIM_X> getJacobianFk(Vector<DIM_X> const& vecX,
90+
Vector<DIM_U> const& vecU) const
91+
{
92+
return static_cast<Derived const*>(this)->getJacobianFk(vecX, vecU);
93+
}
7694

7795
/// @brief Method that calculates the jacobians of the input transition model.
7896
/// @param vecX State Space vector \vec{x}
7997
/// @param vecU Input Space vector \vec{u}
8098
/// @return The jacobians of the input transition model.
81-
virtual Matrix<DIM_X, DIM_U> getJacobianBk(
82-
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU) const = 0;
99+
Matrix<DIM_X, DIM_U> getJacobianBk(Vector<DIM_X> const& vecX,
100+
Vector<DIM_U> const& vecU) const
101+
{
102+
return static_cast<Derived const*>(this)->getJacobianBk(vecX, vecU);
103+
}
83104
};
84105
} // namespace motionmodel
85106
} // namespace kf

‎src/third_party/Eigen3/.clang-format‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
---
2+
BasedOnStyle: Google
3+
ColumnLimit: 120
4+
---
5+
Language: Cpp
6+
BasedOnStyle: Google
7+
ColumnLimit: 120
8+
StatementMacros:
9+
- EIGEN_STATIC_ASSERT
10+
- EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
11+
- EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN
12+
SortIncludes: false
13+
AttributeMacros:
14+
- EIGEN_STRONG_INLINE
15+
- EIGEN_ALWAYS_INLINE
16+
- EIGEN_DEVICE_FUNC
17+
- EIGEN_DONT_INLINE
18+
- EIGEN_DEPRECATED
19+
- EIGEN_UNUSED

‎src/third_party/Eigen3/.gitignore‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
qrc_*cxx
2+
*.orig
3+
*.pyc
4+
*.diff
5+
diff
6+
*.save
7+
save
8+
*.old
9+
*.gmo
10+
*.qm
11+
core
12+
core.*
13+
*.bak
14+
*~
15+
*build*
16+
*.moc.*
17+
*.moc
18+
ui_*
19+
CMakeCache.txt
20+
tags
21+
.*.swp
22+
activity.png
23+
*.out
24+
*.php*
25+
*.log
26+
*.orig
27+
*.rej
28+
log
29+
patch
30+
*.patch
31+
a
32+
a.*
33+
lapack/testing
34+
lapack/reference
35+
.*project
36+
.settings
37+
Makefile
38+
!ci/build.gitlab-ci.yml
39+
!scripts/buildtests.in
40+
!Eigen/Core
41+
!Eigen/src/Core

‎src/third_party/Eigen3/.gitlab-ci.yml‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
# This file is part of Eigen, a lightweight C++ template library
2+
# for linear algebra.
3+
#
4+
# Copyright (C) 2023, The Eigen Authors
5+
#
6+
# This Source Code Form is subject to the terms of the Mozilla
7+
# Public License v. 2.0. If a copy of the MPL was not distributed
8+
# with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9+
10+
stages:
11+
- checkformat
12+
- build
13+
- test
14+
- deploy
15+
16+
variables:
17+
# CMake build directory.
18+
EIGEN_CI_BUILDDIR: .build
19+
# Specify the CMake build target.
20+
EIGEN_CI_BUILD_TARGET: ""
21+
# If a test regex is specified, that will be selected.
22+
# Otherwise, we will try a label if specified.
23+
EIGEN_CI_CTEST_REGEX: ""
24+
EIGEN_CI_CTEST_LABEL: ""
25+
EIGEN_CI_CTEST_ARGS: ""
26+
27+
include:
28+
- "/ci/checkformat.gitlab-ci.yml"
29+
- "/ci/common.gitlab-ci.yml"
30+
- "/ci/build.linux.gitlab-ci.yml"
31+
- "/ci/build.windows.gitlab-ci.yml"
32+
- "/ci/test.linux.gitlab-ci.yml"
33+
- "/ci/test.windows.gitlab-ci.yml"
34+
- "/ci/deploy.gitlab-ci.yml"

0 commit comments

Comments
 (0)