Skip to content

Commit

Permalink
Feature/clang format 2 (#12)
Browse files Browse the repository at this point in the history 
* add clang format files

* format header anc cpp files

* make intended clang format  mistake

* fix clang mistake
  • Loading branch information
@Al-khwarizmi-780
Al-khwarizmi-780 authored Apr 28, 2024
1 parent 2a5568f commit 3aa4627
Show file tree
Hide file tree
Showing 25 changed files with 2,693 additions and 2,478 deletions.
28 changes: 28 additions & 0 deletions .clang-format
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
---
BasedOnStyle: Microsoft
AccessModifierOffset: '-1'
AlignAfterOpenBracket: Align
AllowShortFunctionsOnASingleLine: Inline
AllowShortIfStatementsOnASingleLine: Never
AllowShortLambdasOnASingleLine: Inline
AlwaysBreakAfterReturnType: None
AlwaysBreakTemplateDeclarations: 'Yes'
BreakBeforeBinaryOperators: None
BreakBeforeBraces: Custom
BreakConstructorInitializers: BeforeColon
BreakInheritanceList: BeforeColon
ColumnLimit: '80'
ContinuationIndentWidth: '4'
IncludeBlocks: Preserve
IndentPPDirectives: None
IndentWidth: '2'
Language: Cpp
MaxEmptyLinesToKeep: '1'
NamespaceIndentation: None
PointerAlignment: Left
SpaceBeforeParens: ControlStatements
SpacesBeforeTrailingComments: '2'
TabWidth: '4'
UseTab: Never

...
22 changes: 22 additions & 0 deletions .github/workflows/clang-format-check.yml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
name: clang-format Check
on: [push, pull_request]
jobs:
formatting-check:
name: Formatting Check
runs-on: ubuntu-latest
strategy:
matrix:
path:
- check: 'src'
exclude: 'third_party' # Exclude file paths containing "hello" or "world"
- check: 'tests'
exclude: '' # Nothing to exclude
steps:
- uses: actions/checkout@v3
- name: Run clang-format style check for C/C++/Protobuf programs.
uses: jidicula/[email protected]
with:
clang-format-version: '13'
check-path: ${{ matrix.path['check'] }}
exclude-regex: ${{ matrix.path['exclude'] }}
fallback-style: 'Microsoft' # optional
8 changes: 8 additions & 0 deletions .vscode/settings.json
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
{
"editor.formatOnSave": true,
"editor.formatOnType": true,
"clang-format.executable": "${workspaceFolder}/.clang-format",
"clang-format.style": "file",
"clang-format.fallbackStyle": "Google",
"clang-format.language.cpp.enable": true,
}
303 changes: 155 additions & 148 deletions src/examples/ego_motion_model_adapter/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,165 +12,172 @@
#include <iostream>
#include <stdint.h>

#include "types.h"
#include "motion_model/ego_motion_model.h"
#include "kalman_filter/kalman_filter.h"
#include "motion_model/ego_motion_model.h"
#include "types.h"

static constexpr size_t DIM_X{ 5 }; /// \vec{x} = [x, y, vx, vy, yaw]^T
static constexpr size_t DIM_U{ 3 }; /// \vec{u} = [steeringAngle, ds, dyaw]^T
static constexpr size_t DIM_Z{ 2 };
static constexpr size_t DIM_X{5}; /// \vec{x} = [x, y, vx, vy, yaw]^T
static constexpr size_t DIM_U{3}; /// \vec{u} = [steeringAngle, ds, dyaw]^T
static constexpr size_t DIM_Z{2};

using namespace kf;


/// @brief This is an adapter example to show case how to convert from the 3-dimension state egomotion model
/// to a higher or lower dimension state vector (e.g. 5-dimension state vector and 3-dimension input vector).
class EgoMotionModelAdapter : public motionmodel::MotionModelExtInput<DIM_X, DIM_U>
/// @brief This is an adapter example to show case how to convert from the
/// 3-dimension state egomotion model to a higher or lower dimension state
/// vector (e.g. 5-dimension state vector and 3-dimension input vector).
class EgoMotionModelAdapter
: public motionmodel::MotionModelExtInput<DIM_X, DIM_U>
{
public:
virtual Vector<DIM_X> f(Vector<DIM_X> const & vecX, Vector<DIM_U> const & vecU, Vector<DIM_X> const & /*vecQ = Vector<DIM_X>::Zero()*/) const override
{
Vector<3> tmpVecX; // \vec{x} = [x, y, yaw]^T
tmpVecX << vecX[0], vecX[1], vecX[4];

Vector<2> tmpVecU; // \vec{u} = [ds, dyaw]^T
tmpVecU << vecU[1], vecU[2];

motionmodel::EgoMotionModel const egoMotionModel;
tmpVecX = egoMotionModel.f(tmpVecX, tmpVecU);

Vector<DIM_X> vecXout;
vecXout[0] = tmpVecX[0];
vecXout[1] = tmpVecX[1];
vecXout[4] = tmpVecX[2];

return vecXout;
}

virtual Matrix<DIM_X, DIM_X> getProcessNoiseCov(Vector<DIM_X> const & vecX, Vector<DIM_U> const & vecU) const override
{
// input idx -> output index mapping
// 0 -> 0
// 1 -> 1
// 2 -> 4
Vector<3> tmpVecX;
tmpVecX << vecX[0], vecX[1], vecX[4];

// input idx -> output index mapping
// 0 -> 1
// 1 -> 2
Vector<2> tmpVecU;
tmpVecU << vecU[1], vecU[2];

motionmodel::EgoMotionModel const egoMotionModel;

Matrix<3, 3> matQ = egoMotionModel.getProcessNoiseCov(tmpVecX, tmpVecU);

// |q00 q01 x x q02|
// |q10 q11 x x q12| |q00 q01 q02|
// Qout = | x x x x x| <- Q = |q10 q11 q12|
// | x x x x x| |q20 q21 q22|
// |q20 q21 x x q22|

Matrix<DIM_X, DIM_X> matQout;
matQout(0, 0) = matQ(0, 0);
matQout(0, 1) = matQ(0, 1);
matQout(0, 4) = matQ(0, 2);
matQout(1, 0) = matQ(1, 0);
matQout(1, 1) = matQ(1, 1);
matQout(1, 4) = matQ(1, 2);
matQout(4, 0) = matQ(2, 0);
matQout(4, 1) = matQ(2, 1);
matQout(4, 4) = matQ(2, 1);

return matQout;
}

virtual Matrix<DIM_X, DIM_X> getInputNoiseCov(Vector<DIM_X> const & vecX, Vector<DIM_U> const & vecU) const override
{
Vector<3> tmpVecX;
tmpVecX << vecX[0], vecX[1], vecX[4];

Vector<2> tmpVecU;
tmpVecU << vecU[1], vecU[2];

motionmodel::EgoMotionModel const egoMotionModel;

Matrix<3, 3> matU = egoMotionModel.getInputNoiseCov(tmpVecX, tmpVecU);

Matrix<DIM_X, DIM_X> matUout;
matUout(0, 0) = matU(0, 0);
matUout(0, 1) = matU(0, 1);
matUout(0, 4) = matU(0, 2);
matUout(1, 0) = matU(1, 0);
matUout(1, 1) = matU(1, 1);
matUout(1, 4) = matU(1, 2);
matUout(4, 0) = matU(2, 0);
matUout(4, 1) = matU(2, 1);
matUout(4, 4) = matU(2, 1);

return matUout;
}

virtual Matrix<DIM_X, DIM_X> getJacobianFk(Vector<DIM_X> const & vecX, Vector<DIM_U> const & vecU) const override
{
Vector<3> tmpVecX;
tmpVecX << vecX[0], vecX[1], vecX[4];

Vector<2> tmpVecU;
tmpVecU << vecU[1], vecU[2];

motionmodel::EgoMotionModel const egoMotionModel;

Matrix<3, 3> matFk = egoMotionModel.getJacobianFk(tmpVecX, tmpVecU);

Matrix<DIM_X, DIM_X> matFkout;
matFkout(0, 0) = matFk(0, 0);
matFkout(0, 1) = matFk(0, 1);
matFkout(0, 4) = matFk(0, 2);
matFkout(1, 0) = matFk(1, 0);
matFkout(1, 1) = matFk(1, 1);
matFkout(1, 4) = matFk(1, 2);
matFkout(4, 0) = matFk(2, 0);
matFkout(4, 1) = matFk(2, 1);
matFkout(4, 4) = matFk(2, 1);

return matFkout;
}

virtual Matrix<DIM_X, DIM_U> getJacobianBk(Vector<DIM_X> const & vecX, Vector<DIM_U> const & vecU) const override
{
Vector<3> tmpVecX;
tmpVecX << vecX[0], vecX[1], vecX[4];

Vector<2> tmpVecU;
tmpVecU << vecU[1], vecU[2];

motionmodel::EgoMotionModel const egoMotionModel;

Matrix<3, 2> matBk = egoMotionModel.getJacobianBk(tmpVecX, tmpVecU);

Matrix<DIM_X, DIM_U> matBkout;
matBkout(0, 1) = matBk(0, 0);
matBkout(0, 2) = matBk(0, 1);
matBkout(1, 1) = matBk(1, 0);
matBkout(1, 2) = matBk(1, 1);
matBkout(4, 1) = matBk(2, 0);
matBkout(4, 2) = matBk(2, 1);

return matBkout;
}
public:
virtual Vector<DIM_X> f(
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU,
Vector<DIM_X> const& /*vecQ = Vector<DIM_X>::Zero()*/) const override
{
Vector<3> tmpVecX; // \vec{x} = [x, y, yaw]^T
tmpVecX << vecX[0], vecX[1], vecX[4];

Vector<2> tmpVecU; // \vec{u} = [ds, dyaw]^T
tmpVecU << vecU[1], vecU[2];

motionmodel::EgoMotionModel const egoMotionModel;
tmpVecX = egoMotionModel.f(tmpVecX, tmpVecU);

Vector<DIM_X> vecXout;
vecXout[0] = tmpVecX[0];
vecXout[1] = tmpVecX[1];
vecXout[4] = tmpVecX[2];

return vecXout;
}

virtual Matrix<DIM_X, DIM_X> getProcessNoiseCov(
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU) const override
{
// input idx -> output index mapping
// 0 -> 0
// 1 -> 1
// 2 -> 4
Vector<3> tmpVecX;
tmpVecX << vecX[0], vecX[1], vecX[4];

// input idx -> output index mapping
// 0 -> 1
// 1 -> 2
Vector<2> tmpVecU;
tmpVecU << vecU[1], vecU[2];

motionmodel::EgoMotionModel const egoMotionModel;

Matrix<3, 3> matQ = egoMotionModel.getProcessNoiseCov(tmpVecX, tmpVecU);

// |q00 q01 x x q02|
// |q10 q11 x x q12| |q00 q01 q02|
// Qout = | x x x x x| <- Q = |q10 q11 q12|
// | x x x x x| |q20 q21 q22|
// |q20 q21 x x q22|

Matrix<DIM_X, DIM_X> matQout;
matQout(0, 0) = matQ(0, 0);
matQout(0, 1) = matQ(0, 1);
matQout(0, 4) = matQ(0, 2);
matQout(1, 0) = matQ(1, 0);
matQout(1, 1) = matQ(1, 1);
matQout(1, 4) = matQ(1, 2);
matQout(4, 0) = matQ(2, 0);
matQout(4, 1) = matQ(2, 1);
matQout(4, 4) = matQ(2, 1);

return matQout;
}

virtual Matrix<DIM_X, DIM_X> getInputNoiseCov(
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU) const override
{
Vector<3> tmpVecX;
tmpVecX << vecX[0], vecX[1], vecX[4];

Vector<2> tmpVecU;
tmpVecU << vecU[1], vecU[2];

motionmodel::EgoMotionModel const egoMotionModel;

Matrix<3, 3> matU = egoMotionModel.getInputNoiseCov(tmpVecX, tmpVecU);

Matrix<DIM_X, DIM_X> matUout;
matUout(0, 0) = matU(0, 0);
matUout(0, 1) = matU(0, 1);
matUout(0, 4) = matU(0, 2);
matUout(1, 0) = matU(1, 0);
matUout(1, 1) = matU(1, 1);
matUout(1, 4) = matU(1, 2);
matUout(4, 0) = matU(2, 0);
matUout(4, 1) = matU(2, 1);
matUout(4, 4) = matU(2, 1);

return matUout;
}

virtual Matrix<DIM_X, DIM_X> getJacobianFk(
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU) const override
{
Vector<3> tmpVecX;
tmpVecX << vecX[0], vecX[1], vecX[4];

Vector<2> tmpVecU;
tmpVecU << vecU[1], vecU[2];

motionmodel::EgoMotionModel const egoMotionModel;

Matrix<3, 3> matFk = egoMotionModel.getJacobianFk(tmpVecX, tmpVecU);

Matrix<DIM_X, DIM_X> matFkout;
matFkout(0, 0) = matFk(0, 0);
matFkout(0, 1) = matFk(0, 1);
matFkout(0, 4) = matFk(0, 2);
matFkout(1, 0) = matFk(1, 0);
matFkout(1, 1) = matFk(1, 1);
matFkout(1, 4) = matFk(1, 2);
matFkout(4, 0) = matFk(2, 0);
matFkout(4, 1) = matFk(2, 1);
matFkout(4, 4) = matFk(2, 1);

return matFkout;
}

virtual Matrix<DIM_X, DIM_U> getJacobianBk(
Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU) const override
{
Vector<3> tmpVecX;
tmpVecX << vecX[0], vecX[1], vecX[4];

Vector<2> tmpVecU;
tmpVecU << vecU[1], vecU[2];

motionmodel::EgoMotionModel const egoMotionModel;

Matrix<3, 2> matBk = egoMotionModel.getJacobianBk(tmpVecX, tmpVecU);

Matrix<DIM_X, DIM_U> matBkout;
matBkout(0, 1) = matBk(0, 0);
matBkout(0, 2) = matBk(0, 1);
matBkout(1, 1) = matBk(1, 0);
matBkout(1, 2) = matBk(1, 1);
matBkout(4, 1) = matBk(2, 0);
matBkout(4, 2) = matBk(2, 1);

return matBkout;
}
};

int main()
{
EgoMotionModelAdapter egoMotionModelAdapter;
EgoMotionModelAdapter egoMotionModelAdapter;

Vector<DIM_U> vecU;
vecU << 1.0F, 2.0F, 0.01F;
Vector<DIM_U> vecU;
vecU << 1.0F, 2.0F, 0.01F;

KalmanFilter<DIM_X, DIM_Z> kf;
kf.predictEkf(egoMotionModelAdapter, vecU);
KalmanFilter<DIM_X, DIM_Z> kf;
kf.predictEkf(egoMotionModelAdapter, vecU);

return 0;
return 0;
}
Loading

0 comments on commit 3aa4627

Please sign in to comment.