compensator assert killing the process

[bertaveira]

I have been converting this to ROS2 and noticed that the assert in the compensator upsample function was killing the process. Upon further investigation, I think this assert is a bug.
Line: https://github.com/Huguet57/LIMO-Velo/blob/main/src/Modules/Compensator.cpp#L72

As it says in the "path" function the states are between "just before t1 to t2" and the imus are between "first state to just after t2".

Then in the assert we check if imus.front().time <= states.front().time which should always throw an error since the oldest imu should not be older than the older state.

I had to comment this line and then it works.

[Huguet57]

Weird! As seen in lines 108-112 of the Accumulator initialization:

LIMO-Velo/src/Modules/Accumulator.cpp

Lines 108 to 112 in 57f3205

	if (this->enough_imus()) {
	this->set_initial_time();
	Localizator::getInstance().initialize(this->initial_time);
	return this->is_ready = true;
	}

It's supposed to initialize when enough IMU samples have been gathered - and then set initial_time (which in turn, sets initial_state's time).

Making the first IMU trivially older than the first State. Weird that it happens... And I don't think you delete IMU data anywhere...

[bertaveira]

But upsample is called by the Compensator::path() which will only get a portion of IMUs and States.

If I am not mistaken this piece of code ensures that the oldest state passed to the Compensator::upsample() is older than the older IMU passed to it as well (when doing states.push_front(accum.get_prev_state(t1));). It also ensures that the latest IMU is later than the latest State

LIMO-Velo/src/Modules/Compensator.cpp

Lines 36 to 49 in 57f3205

	States Compensator::path(double t1, double t2) {
	// Call Accumulator
	Accumulator& accum = Accumulator::getInstance();
	// Get states just before t1 to t2
	States states = accum.get_states(t1, t2);
	states.push_front(accum.get_prev_state(t1));
	// Get imus from first state to just after t2
	IMUs imus = accum.get_imus(states.front().time, t2);
	imus.push_back(accum.get_next_imu(t2));
	return this->upsample(states, imus);
	}

It fells like this assert should have the first inequality changed from <= to >=

LIMO-Velo/src/Modules/Compensator.cpp

Lines 71 to 73 in 57f3205

	States Compensator::upsample(const States& states, const IMUs& imus) {
	assert (imus.front().time <= states.front().time and states.back().time <= imus.back().time);

[Huguet57]

Now I see that the get method gets from t1 to t2 included:

LIMO-Velo/include/Headers/Accumulator.hpp

Line 69 in 543d92e

if (t1 > cnt.time) break;

So, in principle, states.front().time and imus.front().time should be the same - if the state has been derived from an IMU timestamp (which I think always happens). Weird that it crashed your code.

I would vote against changing the inequality and more in favor of just removing (?) the first part. The idea was actually that the IMU stream spanned the State stream because then we can get a continuous upsampling (no large chunk from first state to first IMU).

It is more interesting to investigate why it's crashing the code. For now, commenting it doesn't seem to be dangerous.