Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

populate the covariance fields using the noise models #333

Merged
merged 5 commits into from
Aug 23, 2023
Merged

populate the covariance fields using the noise models #333

Changes from 2 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
91740c1
populate the covariance fields using the noise models
ejalaa12 Mar 15, 2023
f9885a8
Update src/ImuSensor.cc
ejalaa12 Apr 11, 2023
a0a18e7
use modern casting
ejalaa12 Aug 11, 2023
8a0fa77
Merge branch 'main' into gz-sensors7
mjcarroll Aug 22, 2023
de0e49b
set covariance to 0 by default.
ejalaa12 Aug 23, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 36 additions & 0 deletions src/ImuSensor.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@
#include <gz/transport/Node.hh>

#include "gz/sensors/ImuSensor.hh"
#include "gz/sensors/GaussianNoiseModel.hh"
#include "gz/sensors/Noise.hh"
#include "gz/sensors/SensorFactory.hh"
#include "gz/sensors/SensorTypes.hh"
Expand Down Expand Up @@ -255,6 +256,41 @@ bool ImuSensor::Update(const std::chrono::steady_clock::duration &_now)
frame->set_key("frame_id");
frame->add_value(this->FrameId());

// Populate covariance
for (int i = 0; i < 9; ++i) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

number 9 as a const ?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't see a problem, but wouldn't that be less easy to read ?

const int matrix_3x3_size = 9;
for (int i = 0; i < matrix_3x3_size; ++i)
...

msg.mutable_linear_acceleration_covariance()->add_data(0);
msg.mutable_angular_velocity_covariance()->add_data(0);
msg.mutable_orientation_covariance()->add_data(0);
}

auto getCov = [&](SensorNoiseType noiseType) -> float{
if (this->dataPtr->noises.find(noiseType) != this->dataPtr->noises.end()) {
GaussianNoiseModelPtr gaussian =
std::dynamic_pointer_cast<GaussianNoiseModel>(
this->dataPtr->noises[noiseType]);
ejalaa12 marked this conversation as resolved.
Show resolved Hide resolved
return (float) (gaussian->StdDev() * gaussian->StdDev());
ejalaa12 marked this conversation as resolved.
Show resolved Hide resolved
}
return 0.001f;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

is 0.001f a good default value to return? or 0?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's good question. I'm not entirely sure here.
According to ros imu message definition:

  • if the covariance is known it should be filled in
  • if the covariance is unknown we should set it to 0
  • if we're missing an estimate for one of those values, we should set it to -1.

In this case, I guess 0 is a good default, that is if we want to follow ros conventions in gz-sensors as well.

Copy link
Contributor

@iche033 iche033 Aug 16, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ok let's keep it consistent with ROS and set it to 0 here. Can you also do that for the orientation_covariance below?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Friendly ping @ejalaa12, I would like to get this in for Harmonic.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hey sorry for the late reply.
I just updated the PR and set the default to 0.
Concerning the orientation_covariance, setting it 0. It is important to note that this implies (according ros convention) the noise is always unknown, while in fact, we are just not applying any.
This is fine for robot_localization for example (a small offset will be added), but I wonder what this will do for other localization nodes... Thankfully, this will not break anything as the behavior would be unchanged with this PR :)

On a different tangent, should we consider applying noise to the orientation values (probably a separate PR). That would mean addind a new SensorNoiseType

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Noising up orientation can be tricky as an IMU isn't really directly measuring that, but instead running it through some sort of filter. You end up with things that aren't linear and uncorrelated in a tidy way. I think we can open a ticket to follow up on that though, if you would like.

};

{
msg.mutable_linear_acceleration_covariance()->set_data(
0, getCov(ACCELEROMETER_X_NOISE_M_S_S));
msg.mutable_linear_acceleration_covariance()->set_data(
4, getCov(ACCELEROMETER_Y_NOISE_M_S_S));
msg.mutable_linear_acceleration_covariance()->set_data(
8, getCov(ACCELEROMETER_Z_NOISE_M_S_S));
msg.mutable_angular_velocity_covariance()->set_data(
0, getCov(GYROSCOPE_X_NOISE_RAD_S));
msg.mutable_angular_velocity_covariance()->set_data(
4, getCov(GYROSCOPE_Y_NOISE_RAD_S));
msg.mutable_angular_velocity_covariance()->set_data(
8, getCov(GYROSCOPE_Z_NOISE_RAD_S));
msg.mutable_orientation_covariance()->set_data(0, 0.001);
msg.mutable_orientation_covariance()->set_data(4, 0.001);
msg.mutable_orientation_covariance()->set_data(8, 0.001);
}

if (this->dataPtr->orientationEnabled)
{
// Set the IMU orientation
Expand Down