Skip to content
Snippets Groups Projects
Commit dede3645 authored by cont-integration's avatar cont-integration
Browse files

[skip ci] applied clang format

parent 4b3086c7
No related branches found
No related tags found
1 merge request!49Draft: Resolve "Adapt to new sensor constructors in core"
Hide whitespace changes
Inline Side-by-side
include/imu/factor/factor_imu_2d.h
+ 9
− 9
View file @ dede3645
......@@ -37,8 +37,8 @@ WOLF_PTR_TYPEDEFS(FactorImu2d);
class FactorImu2d : public FactorAutodiff<FactorImu2d, 5, 2, 1, 2, 3, 2, 1, 2>
{
public:
FactorImu2d(const FeatureImu2dPtr &_ftr_ptr,
const CaptureImuPtr &_capture_origin_ptr,
FactorImu2d(const FeatureImu2dPtr & _ftr_ptr,
const CaptureImuPtr & _capture_origin_ptr,
const ProcessorBasePtr &_processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
......@@ -61,7 +61,7 @@ class FactorImu2d : public FactorAutodiff<FactorImu2d, 5, 2, 1, 2, 3, 2, 1, 2>
const T *const _p2,
const T *const _o2,
const T *const _v2,
T *_res) const;
T * _res) const;
private:
/// Preintegrated delta
......@@ -84,8 +84,8 @@ class FactorImu2d : public FactorAutodiff<FactorImu2d, 5, 2, 1, 2, 3, 2, 1, 2>
///////////////////// IMPLEMENTAITON ////////////////////////////
inline FactorImu2d::FactorImu2d(const FeatureImu2dPtr &_ftr_ptr,
const CaptureImuPtr &_cap_origin_ptr,
inline FactorImu2d::FactorImu2d(const FeatureImu2dPtr & _ftr_ptr,
const CaptureImuPtr & _cap_origin_ptr,
const ProcessorBasePtr &_processor_ptr,
bool _apply_loss_function,
FactorStatus _status)
......@@ -125,18 +125,18 @@ inline bool FactorImu2d::operator()(const T *const _p1,
const T *const _p2,
const T *const _th2,
const T *const _v2,
T *_res) const
T * _res) const
{
using namespace Eigen;
// MAPS
Map<const Matrix<T, 2, 1>> p1(_p1);
const T &th1 = *_th1;
const T & th1 = *_th1;
Map<const Matrix<T, 2, 1>> v1(_v1);
Map<const Matrix<T, 3, 1>> b1(_b1);
Map<const Matrix<T, 2, 1>> p2(_p2);
const T &th2 = *_th2;
const T & th2 = *_th2;
Map<const Matrix<T, 2, 1>> v2(_v2);
Map<Matrix<T, 5, 1>> res(_res);
......@@ -153,7 +153,7 @@ inline bool FactorImu2d::operator()(const T *const _p1,
Matrix<T, 5, 1> delta_predicted;
Map<Matrix<T, 2, 1>> dp_predicted(&delta_predicted(0) + 0);
T &dth_predicted = delta_predicted(2);
T & dth_predicted = delta_predicted(2);
Map<Matrix<T, 2, 1>> dv_predicted(&delta_predicted(0) + 3);
imu2d::betweenStates(p1, th1, v1, p2, th2, v2, dt_, dp_predicted, dth_predicted, dv_predicted);
......
include/imu/factor/factor_imu_2d_with_gravity.h
+ 9
− 9
View file @ dede3645
......@@ -35,8 +35,8 @@ WOLF_PTR_TYPEDEFS(FactorImu2dWithGravity);
class FactorImu2dWithGravity : public FactorAutodiff<FactorImu2dWithGravity, 5, 2, 1, 2, 3, 2, 1, 2, 2>
{
public:
FactorImu2dWithGravity(const FeatureImu2dPtr &_ftr_ptr,
const CaptureImuPtr &_capture_origin_ptr,
FactorImu2dWithGravity(const FeatureImu2dPtr & _ftr_ptr,
const CaptureImuPtr & _capture_origin_ptr,
const ProcessorBasePtr &_processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
......@@ -60,7 +60,7 @@ class FactorImu2dWithGravity : public FactorAutodiff<FactorImu2dWithGravity, 5,
const T *const _o2,
const T *const _v2,
const T *const _g,
T *_res) const;
T * _res) const;
private:
/// Preintegrated delta
......@@ -83,8 +83,8 @@ class FactorImu2dWithGravity : public FactorAutodiff<FactorImu2dWithGravity, 5,
///////////////////// IMPLEMENTAITON ////////////////////////////
inline FactorImu2dWithGravity::FactorImu2dWithGravity(const FeatureImu2dPtr &_ftr_ptr,
const CaptureImuPtr &_cap_origin_ptr,
inline FactorImu2dWithGravity::FactorImu2dWithGravity(const FeatureImu2dPtr & _ftr_ptr,
const CaptureImuPtr & _cap_origin_ptr,
const ProcessorBasePtr &_processor_ptr,
bool _apply_loss_function,
FactorStatus _status)
......@@ -137,18 +137,18 @@ inline bool FactorImu2dWithGravity::operator()(const T *const _p1,
const T *const _th2,
const T *const _v2,
const T *const _g,
T *_res) const
T * _res) const
{
using namespace Eigen;
// MAPS
Map<const Matrix<T, 2, 1>> p1(_p1);
const T &th1 = *_th1;
const T & th1 = *_th1;
Map<const Matrix<T, 2, 1>> v1(_v1);
Map<const Matrix<T, 3, 1>> b1(_b1);
Map<const Matrix<T, 2, 1>> p2(_p2);
const T &th2 = *_th2;
const T & th2 = *_th2;
Map<const Matrix<T, 2, 1>> v2(_v2);
Map<const Matrix<T, 2, 1>> g(_g);
......@@ -166,7 +166,7 @@ inline bool FactorImu2dWithGravity::operator()(const T *const _p1,
Matrix<T, 5, 1> delta_predicted;
Map<Matrix<T, 2, 1>> dp_predicted(&delta_predicted(0) + 0);
T &dth_predicted = delta_predicted(2);
T & dth_predicted = delta_predicted(2);
Map<Matrix<T, 2, 1>> dv_predicted(&delta_predicted(0) + 3);
imu2d::betweenStatesWithGravity(p1, th1, v1, p2, th2, v2, dt_, g, dp_predicted, dth_predicted, dv_predicted);
......
src/processor/processor_imu_2d.cpp
+ 15
− 15
View file @ dede3645
......@@ -83,13 +83,13 @@ bool ProcessorImu2d::voteForKeyFrame() const
return false;
}
CaptureMotionPtr ProcessorImu2d::emplaceCapture(const FrameBasePtr &_frame_own,
const SensorBasePtr &_sensor,
const TimeStamp &_ts,
const VectorXd &_data,
const MatrixXd &_data_cov,
const VectorXd &_calib,
const VectorXd &_calib_preint,
CaptureMotionPtr ProcessorImu2d::emplaceCapture(const FrameBasePtr & _frame_own,
const SensorBasePtr & _sensor,
const TimeStamp & _ts,
const VectorXd & _data,
const MatrixXd & _data_cov,
const VectorXd & _calib,
const VectorXd & _calib_preint,
const CaptureBasePtr &_capture_origin)
{
auto cap_motion = std::static_pointer_cast<CaptureMotion>(
......@@ -149,9 +149,9 @@ void ProcessorImu2d::computeCurrentDelta(const Eigen::VectorXd &_data,
const Eigen::MatrixXd &_data_cov,
const Eigen::VectorXd &_calib,
const double _dt,
Eigen::VectorXd &_delta,
Eigen::MatrixXd &_delta_cov,
Eigen::MatrixXd &_jac_delta_calib) const
Eigen::VectorXd & _delta,
Eigen::MatrixXd & _delta_cov,
Eigen::MatrixXd & _jac_delta_calib) const
{
using namespace Eigen;
assert(_data.size() == data_size_ && "Wrong data size!");
......@@ -194,7 +194,7 @@ void ProcessorImu2d::computeCurrentDelta(const Eigen::VectorXd &_data,
void ProcessorImu2d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
const Eigen::VectorXd &_delta,
const double _dt,
Eigen::VectorXd &_delta_preint_plus_delta) const
Eigen::VectorXd & _delta_preint_plus_delta) const
{
/* MATHS:
* Simple composition,
......@@ -210,7 +210,7 @@ void ProcessorImu2d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
void ProcessorImu2d::statePlusDelta(const VectorComposite &_x,
const Eigen::VectorXd &_delta,
const double _dt,
VectorComposite &_x_plus_delta) const
VectorComposite & _x_plus_delta) const
{
assert(_x.has("POV") && "Any key missing in _x");
assert(_delta.size() == 5 && "Wrong _delta vector size");
......@@ -233,9 +233,9 @@ void ProcessorImu2d::statePlusDelta(const VectorComposite &_x,
void ProcessorImu2d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
const Eigen::VectorXd &_delta,
const double _dt,
Eigen::VectorXd &_delta_preint_plus_delta,
Eigen::MatrixXd &_jacobian_delta_preint,
Eigen::MatrixXd &_jacobian_delta) const
Eigen::VectorXd & _delta_preint_plus_delta,
Eigen::MatrixXd & _jacobian_delta_preint,
Eigen::MatrixXd & _jacobian_delta) const
{
/*
* Expression of the delta integration step, D' = D (+) d:
......
src/processor/processor_imu_3d.cpp
+ 19
− 18
View file @ dede3645
......@@ -100,13 +100,13 @@ bool ProcessorImu3d::voteForKeyFrame() const
return false;
}
CaptureMotionPtr ProcessorImu3d::emplaceCapture(const FrameBasePtr &_frame_own,
const SensorBasePtr &_sensor,
const TimeStamp &_ts,
const VectorXd &_data,
const MatrixXd &_data_cov,
const VectorXd &_calib,
const VectorXd &_calib_preint,
CaptureMotionPtr ProcessorImu3d::emplaceCapture(const FrameBasePtr & _frame_own,
const SensorBasePtr & _sensor,
const TimeStamp & _ts,
const VectorXd & _data,
const MatrixXd & _data_cov,
const VectorXd & _calib,
const VectorXd & _calib_preint,
const CaptureBasePtr &_capture_origin)
{
auto cap_motion = std::static_pointer_cast<CaptureMotion>(
......@@ -163,7 +163,8 @@ void ProcessorImu3d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, C
_capture_own->getStateBlock('I') !=
_capture_origin->getStateBlock('I')) // make sure it's two different captures
{
auto fac_bias = _capture_own->emplaceDriftFactor(_capture_origin, 'I', false); // false: no loss function for bias drift
auto fac_bias =
_capture_own->emplaceDriftFactor(_capture_origin, 'I', false); // false: no loss function for bias drift
if (bootstrapping_)
{
......@@ -177,9 +178,9 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data,
const Eigen::MatrixXd &_data_cov,
const Eigen::VectorXd &_calib,
const double _dt,
Eigen::VectorXd &_delta,
Eigen::MatrixXd &_delta_cov,
Eigen::MatrixXd &_jac_delta_calib) const
Eigen::VectorXd & _delta,
Eigen::MatrixXd & _delta_cov,
Eigen::MatrixXd & _jac_delta_calib) const
{
assert(_data.size() == data_size_ && "Wrong data size!");
......@@ -218,7 +219,7 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data,
void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
const Eigen::VectorXd &_delta,
const double _dt,
Eigen::VectorXd &_delta_preint_plus_delta) const
Eigen::VectorXd & _delta_preint_plus_delta) const
{
/* MATHS according to Sola-16
* Dp' = Dp + Dv*dt + 1/2*Dq*(a-a_b)*dt^2 = Dp + Dv*dt + Dq*dp if dp =
......@@ -235,7 +236,7 @@ void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
void ProcessorImu3d::statePlusDelta(const VectorComposite &_x,
const Eigen::VectorXd &_delta,
const double _dt,
VectorComposite &_x_plus_delta) const
VectorComposite & _x_plus_delta) const
{
assert(_delta.size() == 10 && "Wrong _delta vector size");
assert(_dt >= 0 && "Time interval _dt is negative!");
......@@ -255,9 +256,9 @@ void ProcessorImu3d::statePlusDelta(const VectorComposite &_x,
void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
const Eigen::VectorXd &_delta,
const double _dt,
Eigen::VectorXd &_delta_preint_plus_delta,
Eigen::MatrixXd &_jacobian_delta_preint,
Eigen::MatrixXd &_jacobian_delta) const
Eigen::VectorXd & _delta_preint_plus_delta,
Eigen::MatrixXd & _jacobian_delta_preint,
Eigen::MatrixXd & _jacobian_delta) const
{
/*
* Expression of the delta integration step, D' = D (+) d:
......@@ -498,8 +499,8 @@ bool ProcessorImu3d::recomputeStates() const
const auto &cap_origin = cap->getOriginCapture();
const auto &frm_origin = cap_origin->getFrame();
const auto &delta = VectorComposite({{'P', ftr->getMeasurement().head<3>()},
{'O', ftr->getMeasurement().segment<4>(3)},
{'V', ftr->getMeasurement().tail<3>()}});
{'O', ftr->getMeasurement().segment<4>(3)},
{'V', ftr->getMeasurement().tail<3>()}});
const auto &x_origin = frm_origin->getState();
auto dt = cap->getTimeStamp() - cap_origin->getTimeStamp();
auto x = imu::composeOverState(x_origin, delta, dt);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment