Skip to content
Snippets Groups Projects
Commit 84f39951 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

[skip ci] fixing tests

parent d74b6c94
No related branches found
No related tags found
1 merge request!49Draft: Resolve "Adapt to new sensor constructors in core"
......@@ -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>(
......@@ -178,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!");
......@@ -219,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 =
......@@ -236,13 +236,13 @@ 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!");
const auto &delta =
VectorComposite({{'P', _delta.head<3>()}, {'O', _delta.segment<3>(4)}, {'V', _delta.tail<3>()}});
VectorComposite({{'P', _delta.head<3>()}, {'O', _delta.segment<4>(3)}, {'V', _delta.tail<3>()}});
/* MATH according to Sola-16
*
......@@ -256,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:
......@@ -499,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);
......
......@@ -104,7 +104,7 @@ TEST_F(FeatureImu3d_test, check_frame)
{
// set variables
FrameBasePtr left_frame = feat_imu->getFrame();
TimeStamp t;
TimeStamp t = ts;
left_frame->getTimeStamp(t);
origin_frame->getTimeStamp(ts);
......@@ -119,8 +119,9 @@ TEST_F(FeatureImu3d_test, check_frame)
left_v = left_frame->getV();
ASSERT_MATRIX_APPROX(origin_p->getState(), left_p->getState(), Constants::EPS_SMALL);
Map<const Quaterniond> origin_Quat(origin_o->getState().data()), left_Quat(left_o->getState().data());
ASSERT_QUATERNION_APPROX(origin_Quat, left_Quat, Constants::EPS_SMALL);
// Map<const Quaterniond> origin_Quat(origin_o->getState().data()), left_Quat(left_o->getState().data());
// ASSERT_QUATERNION_APPROX(origin_Quat, left_Quat, Constants::EPS_SMALL);
ASSERT_QUATERNION_VECTOR_APPROX(origin_o->getState(), left_o->getState(), Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(origin_v->getState(), left_v->getState(), Constants::EPS_SMALL);
ASSERT_EQ(origin_frame->id(), left_frame->id());
......
......@@ -5,7 +5,7 @@ problem:
value: [0,0]
prior:
mode: "factor"
factor_std: [.001, .001]
factor_std: [.001, .001]
O:
value: [0]
prior:
......
......@@ -5,12 +5,12 @@ problem:
value: [0, 0, 0]
prior:
mode: "factor"
factor_std: [.001, .001, .001]
factor_std: [.001, .001, .001]
O:
value: [0, 0, 0, 1]
prior:
mode: "factor"
factor_std: [.001, .001, .001]
factor_std: [.001, .001, .001]
V:
value: [0, 0, 0]
prior:
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment