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

gtests ok!

parent 84f39951
No related branches found
No related tags found
1 merge request!49Draft: Resolve "Adapt to new sensor constructors in core"
......@@ -26,6 +26,8 @@
// wolf
#include <core/composite/vector_composite.h>
#include <core/capture/capture_base.h>
#include <core/capture/capture_void.h>
#include <core/factor/factor_relative_pose_3d.h>
namespace wolf
{
......@@ -157,7 +159,7 @@ void ProcessorImu3d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, C
// 2. Feature and factor bias -- IMU bias drift for acc and gyro
//---------------------------------------------------------------
// - This factor only if IMU biases are Dynamic (sensor->isStateBlockDynamic('I'))
// - This factor only if IMU biases are Dynamic
// - factor relates bias(last capture) and bias(origin capture)
if (getSensor()->isStateBlockDynamic('I') and
_capture_own->getStateBlock('I') !=
......@@ -348,9 +350,36 @@ void ProcessorImu3d::bootstrap()
WOLF_DEBUG("IMU Keyframe states have been recomputed!");
}
// TODO: add factors for the STATIC strategy:
// - zero-velocity factors (at each KF)
// - zero-displaecement odom3d factors (between KFs)
// Add factors for the STATIC strategy:
auto last_frame = getProblem()->getLastFrame();
while (last_frame)
{
// Fix zero-velocity
last_frame->getV()->setZero();
last_frame->getV()->fix();
// Factor zero-displacement
if (last_frame->getPreviousFrame())
{
auto capture_zero =
CaptureBase::emplace<CaptureVoid>(last_frame, last_frame->getTimeStamp(), nullptr);
auto feature_zero = FeatureBase::emplace<FeatureBase>(
capture_zero, "FeatureZeroOdom", Vector7d::Zero(), Eigen::MatrixXd::Identity(6, 6) * 0.01);
FactorBase::emplace<FactorRelativePose3d>(
feature_zero,
feature_zero->getMeasurement(),
feature_zero->getMeasurementSquareRootInformationUpper(),
last_frame->getPreviousFrame(),
last_frame,
nullptr,
false,
TOP_MOTION);
}
// next
last_frame = last_frame->getPreviousFrame();
}
// Activate factors that were inactive during bootstrap
while (not bootstrap_factor_list_.empty())
......
......@@ -86,9 +86,9 @@ class ProcessorImuStaticInitTest : public testing::Test
KF0_ = problem_->applyFirstFrameOptions(t0);
}
void TestStatic(const Vector6d & body_magnitudes,
const Vector6d & bias_groundtruth,
const Vector6d & bias_initial_guess,
void TestStatic(const Vector6d &body_magnitudes,
const Vector6d &bias_groundtruth,
const Vector6d &bias_initial_guess,
const std::string &test_name,
int testing_var,
bool small_tol)
......@@ -236,9 +236,9 @@ class ProcessorImuStaticInitTest : public testing::Test
#endif
}
void TestStaticZeroOdom(const Vector6d & body_magnitudes,
const Vector6d & bias_groundtruth,
const Vector6d & bias_initial_guess,
void TestStaticZeroOdom(const Vector6d &body_magnitudes,
const Vector6d &bias_groundtruth,
const Vector6d &bias_initial_guess,
const std::string &test_name,
int testing_var,
bool small_tol)
......
......@@ -16,10 +16,8 @@ states:
I:
value: [0, 0, 0, 0, 0, 0]
prior:
mode: factor
factor_std: [0.8, 0.8, 0.8, 0.35, 0.35, 0.35] # Previously named ab_initial_stdev [m/s²] and wb_initial_stdev [rad/sec]
mode: initial_guess
dynamic: false
drift_std: [0.1, 0.1, 0.1, 0.04, 0.04, 0.04] # Previously named ab_rate_stdev [m/s²/sqrt(s)] and wb_rate_stdev [rad/s/sqrt(s)]
a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s²
w_noise: 0.0011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
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