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

[skip ci] applied clang format

parent b96cd347
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
src/processor/processor_imu_3d.cpp
+ 17
− 17
View file @ 9b0cd257
...@@ -102,13 +102,13 @@ bool ProcessorImu3d::voteForKeyFrame() const ...@@ -102,13 +102,13 @@ bool ProcessorImu3d::voteForKeyFrame() const
return false; return false;
} }
CaptureMotionPtr ProcessorImu3d::emplaceCapture(const FrameBasePtr &_frame_own, CaptureMotionPtr ProcessorImu3d::emplaceCapture(const FrameBasePtr & _frame_own,
const SensorBasePtr &_sensor, const SensorBasePtr & _sensor,
const TimeStamp &_ts, const TimeStamp & _ts,
const VectorXd &_data, const VectorXd & _data,
const MatrixXd &_data_cov, const MatrixXd & _data_cov,
const VectorXd &_calib, const VectorXd & _calib,
const VectorXd &_calib_preint, const VectorXd & _calib_preint,
const CaptureBasePtr &_capture_origin) const CaptureBasePtr &_capture_origin)
{ {
auto cap_motion = std::static_pointer_cast<CaptureMotion>( auto cap_motion = std::static_pointer_cast<CaptureMotion>(
...@@ -180,9 +180,9 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data, ...@@ -180,9 +180,9 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data,
const Eigen::MatrixXd &_data_cov, const Eigen::MatrixXd &_data_cov,
const Eigen::VectorXd &_calib, const Eigen::VectorXd &_calib,
const double _dt, const double _dt,
Eigen::VectorXd &_delta, Eigen::VectorXd & _delta,
Eigen::MatrixXd &_delta_cov, Eigen::MatrixXd & _delta_cov,
Eigen::MatrixXd &_jac_delta_calib) const Eigen::MatrixXd & _jac_delta_calib) const
{ {
assert(_data.size() == data_size_ && "Wrong data size!"); assert(_data.size() == data_size_ && "Wrong data size!");
...@@ -221,7 +221,7 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data, ...@@ -221,7 +221,7 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data,
void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint, void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
const Eigen::VectorXd &_delta, const Eigen::VectorXd &_delta,
const double _dt, const double _dt,
Eigen::VectorXd &_delta_preint_plus_delta) const Eigen::VectorXd & _delta_preint_plus_delta) const
{ {
/* MATHS according to Sola-16 /* MATHS according to Sola-16
* Dp' = Dp + Dv*dt + 1/2*Dq*(a-a_b)*dt^2 = Dp + Dv*dt + Dq*dp if dp = * Dp' = Dp + Dv*dt + 1/2*Dq*(a-a_b)*dt^2 = Dp + Dv*dt + Dq*dp if dp =
...@@ -238,7 +238,7 @@ void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint, ...@@ -238,7 +238,7 @@ void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
void ProcessorImu3d::statePlusDelta(const VectorComposite &_x, void ProcessorImu3d::statePlusDelta(const VectorComposite &_x,
const Eigen::VectorXd &_delta, const Eigen::VectorXd &_delta,
const double _dt, const double _dt,
VectorComposite &_x_plus_delta) const VectorComposite & _x_plus_delta) const
{ {
assert(_delta.size() == 10 && "Wrong _delta vector size"); assert(_delta.size() == 10 && "Wrong _delta vector size");
assert(_dt >= 0 && "Time interval _dt is negative!"); assert(_dt >= 0 && "Time interval _dt is negative!");
...@@ -258,9 +258,9 @@ void ProcessorImu3d::statePlusDelta(const VectorComposite &_x, ...@@ -258,9 +258,9 @@ void ProcessorImu3d::statePlusDelta(const VectorComposite &_x,
void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint, void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
const Eigen::VectorXd &_delta, const Eigen::VectorXd &_delta,
const double _dt, const double _dt,
Eigen::VectorXd &_delta_preint_plus_delta, Eigen::VectorXd & _delta_preint_plus_delta,
Eigen::MatrixXd &_jacobian_delta_preint, Eigen::MatrixXd & _jacobian_delta_preint,
Eigen::MatrixXd &_jacobian_delta) const Eigen::MatrixXd & _jacobian_delta) const
{ {
/* /*
* Expression of the delta integration step, D' = D (+) d: * Expression of the delta integration step, D' = D (+) d:
...@@ -528,8 +528,8 @@ bool ProcessorImu3d::recomputeStates() const ...@@ -528,8 +528,8 @@ bool ProcessorImu3d::recomputeStates() const
const auto &cap_origin = cap->getOriginCapture(); const auto &cap_origin = cap->getOriginCapture();
const auto &frm_origin = cap_origin->getFrame(); const auto &frm_origin = cap_origin->getFrame();
const auto &delta = VectorComposite({{'P', ftr->getMeasurement().head<3>()}, const auto &delta = VectorComposite({{'P', ftr->getMeasurement().head<3>()},
{'O', ftr->getMeasurement().segment<4>(3)}, {'O', ftr->getMeasurement().segment<4>(3)},
{'V', ftr->getMeasurement().tail<3>()}}); {'V', ftr->getMeasurement().tail<3>()}});
const auto &x_origin = frm_origin->getState(); const auto &x_origin = frm_origin->getState();
auto dt = cap->getTimeStamp() - cap_origin->getTimeStamp(); auto dt = cap->getTimeStamp() - cap_origin->getTimeStamp();
auto x = imu::composeOverState(x_origin, delta, dt); auto x = imu::composeOverState(x_origin, delta, dt);
......
src/sensor/sensor_compass.cpp
+ 5
− 1
View file @ 9b0cd257
...@@ -23,7 +23,11 @@ ...@@ -23,7 +23,11 @@
namespace wolf namespace wolf
{ {
SensorCompass::SensorCompass(const YAML::Node &_params) SensorCompass::SensorCompass(const YAML::Node &_params)
: SensorBase("SensorCompass", 0, TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}, {'I', "StateParams3"}, {'F', "StateParams3"}}, _params) : SensorBase(
"SensorCompass",
0,
TypeComposite{{'P', "StatePoint3d"}, {'O', "StateQuaternion"}, {'I', "StateParams3"}, {'F', "StateParams3"}},
_params)
{ {
stdev_noise_ = _params["stdev_noise"].as<double>(); stdev_noise_ = _params["stdev_noise"].as<double>();
} }
......
test/gtest_feature_imu_3d.cpp
+ 1
− 1
View file @ 9b0cd257
...@@ -104,7 +104,7 @@ TEST_F(FeatureImu3d_test, check_frame) ...@@ -104,7 +104,7 @@ TEST_F(FeatureImu3d_test, check_frame)
{ {
// set variables // set variables
FrameBasePtr left_frame = feat_imu->getFrame(); FrameBasePtr left_frame = feat_imu->getFrame();
TimeStamp t = ts; TimeStamp t = ts;
left_frame->getTimeStamp(t); left_frame->getTimeStamp(t);
origin_frame->getTimeStamp(ts); origin_frame->getTimeStamp(ts);
......
test/gtest_imu_3d_static_init.cpp
+ 6
− 6
View file @ 9b0cd257
...@@ -86,9 +86,9 @@ class ProcessorImuStaticInitTest : public testing::Test ...@@ -86,9 +86,9 @@ class ProcessorImuStaticInitTest : public testing::Test
KF0_ = problem_->applyFirstFrameOptions(t0); KF0_ = problem_->applyFirstFrameOptions(t0);
} }
void TestStatic(const Vector6d &body_magnitudes, void TestStatic(const Vector6d & body_magnitudes,
const Vector6d &bias_groundtruth, const Vector6d & bias_groundtruth,
const Vector6d &bias_initial_guess, const Vector6d & bias_initial_guess,
const std::string &test_name, const std::string &test_name,
int testing_var, int testing_var,
bool small_tol) bool small_tol)
...@@ -236,9 +236,9 @@ class ProcessorImuStaticInitTest : public testing::Test ...@@ -236,9 +236,9 @@ class ProcessorImuStaticInitTest : public testing::Test
#endif #endif
} }
void TestStaticZeroOdom(const Vector6d &body_magnitudes, void TestStaticZeroOdom(const Vector6d & body_magnitudes,
const Vector6d &bias_groundtruth, const Vector6d & bias_groundtruth,
const Vector6d &bias_initial_guess, const Vector6d & bias_initial_guess,
const std::string &test_name, const std::string &test_name,
int testing_var, int testing_var,
bool small_tol) bool small_tol)
......
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