PM getState() and time_tolerance
There is a rare error (dt negative) in ProcessorMotion::getState()
(it crashed in called methods from getState).
I discovered that with a wrong time_tolerance_
this error occurs in a test (gtest_imu_mocap_fusion).
The origin of this error is that first, we call ProcessorMotion::findCaptureContainingTimeStamp(_ts)
and this function uses the time_tolerance_
. This means that it can return a capture that is within ts_
+/- time_tolerance_
.
If time_tolerance_
is bigger than half the capture's period, it is obvious that there will be errors in finding captures according to timestamp. But from the user point of view, this is not a comprehensive way of complaining about wrong time_tolerance_
value. I already added an ERROR message when dt is negative ("maybe it is due to wrong value...").
But, even if time_tolerance_
is correctly specified, ProcessorMotion::findCaptureContainingTimeStamp(_ts)
may return a capture that is older than ts_
(less than time_tolerance_
before). This will produce a negative dt
.
The code of ProcessorMotion::getState()
is the following (already with my error message and the fix for dt<0):
VectorComposite ProcessorMotion::getState(const TimeStamp& _ts, const StateKeys& _keys) const
{
assert(_ts.ok());
const StateKeys& keys = (_keys == "" ? getStateKeys() : _keys);
// We need to search for the capture containing a motion buffer with the queried time stamp
auto capture_motion = findCaptureContainingTimeStamp(_ts);
if (capture_motion == nullptr) // we do not have any info of where to find a valid state
{
WOLF_DEBUG("Processor has no state. Returning an empty VectorComposite with no blocks");
return VectorComposite(); // return empty state
}
else // We found a CaptureMotion whose buffer contains the time stamp
{
// if buffer is empty --> we did not advance from origin!
// this may happen when in the very first frame where the capture has no motion info --> empty buffer
if (capture_motion->getBuffer().empty())
{
return capture_motion->getFrame()->getState(keys);
}
/* Doing this:
*
* x_t = x_0 [+] ( D_0t (+) J_D_c * ( c - c_preint ) )
*
* or, put in code form:
*
* _state = state_origin [+] (delta_preint (+) Jac_delta_calib * (calib - calib_preint) )
*
* with
* [+] : group composition
* (+) : block-wise plus
*/
// Get state of origin
auto cap_orig = capture_motion->getOriginCapture();
auto x_origin = cap_orig->getFrame()->getState(getStateKeys());
// dt
double dt = _ts - cap_orig->getTimeStamp();
if (dt < 0)
{
WOLF_ERROR("ProcessorMotion::getState: negative dt!",
"\ttimestamp requested: ",
_ts,
"\tCapture found id:",
capture_motion->id(),
" ts = ",
capture_motion->getTimeStamp(),
"\tCapture origin id:",
cap_orig->id(),
" ts = ",
cap_orig->getTimeStamp());
WOLF_ERROR("THIS ERROR MAY BE CAUSED BY A WRONG 'time_tolerance' VALUE OF THIS PROCESSOR: ",
getName(),
"\nIt should be lower than 1/2 of the period of the captures arribal.");
dt = 0;
}
// Get motion at time stamp
const auto& motion = capture_motion->getBuffer().getMotion(_ts);
// Get delta preintegrated up to time stamp
const auto& delta_preint = motion.delta_integr_;
// Get calibration preint -- stored in last capture
auto calib_preint = capture_motion->getCalibrationPreint();
VectorComposite state;
if (hasCalibration())
{
// Get current calibration -- from origin capture
auto calib = getCalibration(cap_orig);
// get Jacobian of delta wrt calibration
const auto& J_delta_calib = motion.jacobian_calib_;
// compute delta change
const auto& delta_step = J_delta_calib * (calib - calib_preint);
// correct delta // this is (+)
auto delta_corrected = correctDelta(delta_preint, delta_step);
// compute current state // this is [+]
statePlusDelta(x_origin, delta_corrected, dt, state);
}
else
{
statePlusDelta(x_origin, delta_preint, dt, state);
}
// Return the requested keys
if (_keys == "")
{
return state;
}
else
{
// remove states not requested by structure
auto pair_key_vec_it = state.begin();
while (pair_key_vec_it != state.end())
{
if (_keys.find(pair_key_vec_it->first) == std::string::npos)
pair_key_vec_it = state.erase(pair_key_vec_it);
else
pair_key_vec_it++;
}
return state;
}
}
}