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

unicycle motion model: add option to rotate the process noise covariance to the current state orientation #325

Open
wants to merge 8 commits into
base: devel
Choose a base branch
from
+185 −8
Open

unicycle motion model: add option to rotate the process noise covariance to the current state orientation #325

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
66cd212
unicycle motion model: add option to rotate the process noise covaria…
fabianhirmann May 9, 2023
7238f0d
fix linter errors for unicycle motion model: rotate process noise cov…
fabianhirmann May 9, 2023
05abcac
unicycle motion model: apply rotation of process noise covariance onl…
fabianhirmann May 10, 2023
555ac04
unicycle motion model: rotate process noise with covariance: fixes fo…
fabianhirmann May 10, 2023
04592ef
unicycle motion model: rotate process noise covariance with state ori…
fabianhirmann May 10, 2023
7f8b97a
unicycle motion model: create unit test for generateMotionModel and n…
fabianhirmann May 15, 2023
ecb11a0
unicycle motion model: revert previous changes which created a separa…
fabianhirmann May 16, 2023
39bbb99
Update test_unicycle_2d to rostest instead of pure gtest after now ro…
fabianhirmann Jul 13, 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
41 changes: 39 additions & 2 deletions fuse_models/include/fuse_models/unicycle_2d.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -123,6 +123,26 @@ class Unicycle2D : public fuse_core::AsyncMotionModel
*/
bool applyCallback(fuse_core::Transaction& transaction) override;

/**
* @brief Generator function for providing to the \c TimestampManager to create a single motion model segment
Copy link
Collaborator

Choose a reason for hiding this comment

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

I think this needs to be reworded, maybe something like:

Suggested change
* @brief Generator function for providing to the \c TimestampManager to create a single motion model segment
* @brief Generator function for providing to the \c TimestampManager a way to create a single motion model segment

* between the specified timestamps.
*
* This function is effectively using \c generateMotionModel.
*
* @param[in] beginning_stamp The beginning timestamp of the motion model constraints to be generated.
* \p beginning_stamp is guaranteed to be less than \p ending_stamp.
* @param[in] ending_stamp The ending timestamp of the motion model constraints to be generated.
* \p ending_stamp is guaranteed to be greater than \p beginning_stamp.
* @param[out] constraints One or more motion model constraints between the requested timestamps.
* @param[out] variables One or more variables at both the \p beginning_stamp and \p ending_stamp. The
* variables should include initial values for the optimizer.
*/
void generateMotionModelGenerator(
const ros::Time& beginning_stamp,
const ros::Time& ending_stamp,
std::vector<fuse_core::Constraint::SharedPtr>& constraints,
std::vector<fuse_core::Variable::SharedPtr>& variables);

/**
* @brief Generate a single motion model segment between the specified timestamps.
*
Expand All @@ -133,13 +153,30 @@ class Unicycle2D : public fuse_core::AsyncMotionModel
* \p beginning_stamp is guaranteed to be less than \p ending_stamp.
* @param[in] ending_stamp The ending timestamp of the motion model constraints to be generated.
* \p ending_stamp is guaranteed to be greater than \p beginning_stamp.
* @param[in] state_history The state history object to be updated
* @param[in] device_id The UUID of the device to be published
* @param[in] name The unique name for this motion model instance
* @param[in] process_noise_covariance Process noise covariance matrix
* @param[in] scale_process_noise Whether to scale the process noise covariance pose by the norm
of the current state twist.
* @param[in] velocity_norm_min The minimum velocity/twist norm allowed when scaling the
process noise covariance
* @param[in] disable_checks Whether to disable the validation checks for the current and predicted state,
including the process noise covariance after it is scaled and multiplied by dt.
* @param[out] constraints One or more motion model constraints between the requested timestamps.
* @param[out] variables One or more variables at both the \p beginning_stamp and \p ending_stamp. The
* variables should include initial values for the optimizer.
*/
void generateMotionModel(
static void generateMotionModel(
Copy link
Collaborator

Choose a reason for hiding this comment

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

I wonder if this breaks existing code because it might break the API for children classes. 🤔

Copy link
Collaborator

Choose a reason for hiding this comment

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

Why did you have to make this static?

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 made this function static because I am honestly not that familiar with gtest and setting the private variables process_noise_covariance_, state_history_, etc. seemed complicated otherwise. Also Unicycle2D::onInit() gets some ROS parameters that again seemed more complicated to handle. Defining that function as static which I can then directly use seemed easier for me.
However, I just thought about it again and with another way I came around this issue and there is no need to set this static and make many changes in the class itself (and also many comments from you are no more relevant then). If you feel like having a static function is still better, I can just revert my changes again. I look forward to your feedback on it.

const ros::Time& beginning_stamp,
const ros::Time& ending_stamp,
StateHistory& state_history,
const fuse_core::UUID& device_id,
const std::string name,
fuse_core::Matrix8d process_noise_covariance,
const bool scale_process_noise,
const double velocity_norm_min,
const bool disable_checks,
std::vector<fuse_core::Constraint::SharedPtr>& constraints,
std::vector<fuse_core::Variable::SharedPtr>& variables);

Expand Down Expand Up @@ -178,7 +215,7 @@ class Unicycle2D : public fuse_core::AsyncMotionModel
*
* @param[in] state1 The first/oldest state
* @param[in] state2 The second/newest state
* @param[in] process_noise_covariance The process noise covariance, after it is scaled and multiplied by dt
* @param[in] process_noise_covariance The process noise covariance, after it is rotated, scaled and multiplied by dt
*/
static void validateMotionModel(const StateHistoryElement& state1, const StateHistoryElement& state2,
const fuse_core::Matrix8d& process_noise_covariance);
Expand Down
83 changes: 54 additions & 29 deletions fuse_models/src/unicycle_2d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,7 +119,7 @@ Unicycle2D::Unicycle2D() :
fuse_core::AsyncMotionModel(1),
buffer_length_(ros::DURATION_MAX),
device_id_(fuse_core::uuid::NIL),
timestamp_manager_(&Unicycle2D::generateMotionModel, this, ros::DURATION_MAX)
timestamp_manager_(&Unicycle2D::generateMotionModelGenerator, this, ros::DURATION_MAX)
{
}

Expand Down Expand Up @@ -204,6 +204,7 @@ void Unicycle2D::onInit()
process_noise_covariance_ = fuse_core::Vector8d(process_noise_diagonal.data()).asDiagonal();

private_node_handle_.param("scale_process_noise", scale_process_noise_, scale_process_noise_);

Copy link
Collaborator

Choose a reason for hiding this comment

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

I think the extra lines for the others are only because they need extra work other than just calling param, so I wouldn't add a new line here:

Suggested change

private_node_handle_.param("velocity_norm_min", velocity_norm_min_, velocity_norm_min_);

private_node_handle_.param("disable_checks", disable_checks_, disable_checks_);
Expand All @@ -228,23 +229,41 @@ void Unicycle2D::onStart()
state_history_.clear();
}

void Unicycle2D::generateMotionModelGenerator(
const ros::Time& beginning_stamp,
const ros::Time& ending_stamp,
std::vector<fuse_core::Constraint::SharedPtr>& constraints,
std::vector<fuse_core::Variable::SharedPtr>& variables)
{
generateMotionModel(beginning_stamp, ending_stamp, state_history_, device_id_, name(),
process_noise_covariance_, scale_process_noise_, velocity_norm_min_, disable_checks_,
constraints, variables);
}

void Unicycle2D::generateMotionModel(
const ros::Time& beginning_stamp,
const ros::Time& ending_stamp,
StateHistory& state_history,
const fuse_core::UUID& device_id,
const std::string name,
fuse_core::Matrix8d process_noise_covariance,
Copy link
Collaborator

Choose a reason for hiding this comment

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

Do you really need to copy here? Why don't you do the following?:

Suggested change
const std::string name,
fuse_core::Matrix8d process_noise_covariance,
const std::string& name,
const fuse_core::Matrix8d& process_noise_covariance,

It looks like you're relying on making a copy of the process noise covariance, so the input doesn't change. I believe it's cleaner to just create a local copy below because there are cases where we return even before using the process nosie covariance, so there's no need to copy always it.

const bool scale_process_noise,
const double velocity_norm_min,
const bool disable_checks,
std::vector<fuse_core::Constraint::SharedPtr>& constraints,
std::vector<fuse_core::Variable::SharedPtr>& variables)
{
assert(beginning_stamp < ending_stamp || (beginning_stamp == ending_stamp && state_history_.empty()));
assert(beginning_stamp < ending_stamp || (beginning_stamp == ending_stamp && state_history.empty()));

StateHistoryElement base_state;
ros::Time base_time;

// Find an entry that is > beginning_stamp
// The entry that is <= will be the one before it
auto base_state_pair_it = state_history_.upper_bound(beginning_stamp);
if (base_state_pair_it == state_history_.begin())
auto base_state_pair_it = state_history.upper_bound(beginning_stamp);
if (base_state_pair_it == state_history.begin())
{
ROS_WARN_STREAM_COND_NAMED(!state_history_.empty(), "UnicycleModel", "Unable to locate a state in this history "
ROS_WARN_STREAM_COND_NAMED(!state_history.empty(), "UnicycleModel", "Unable to locate a state in this history "
"with stamp <= " << beginning_stamp << ". Variables will all be initialized to 0.");
base_time = beginning_stamp;
}
Expand Down Expand Up @@ -281,13 +300,13 @@ void Unicycle2D::generateMotionModel(

if (dt == 0.0)
{
state1.position_uuid = fuse_variables::Position2DStamped(beginning_stamp, device_id_).uuid();
state1.yaw_uuid = fuse_variables::Orientation2DStamped(beginning_stamp, device_id_).uuid();
state1.vel_linear_uuid = fuse_variables::VelocityLinear2DStamped(beginning_stamp, device_id_).uuid();
state1.vel_yaw_uuid = fuse_variables::VelocityAngular2DStamped(beginning_stamp, device_id_).uuid();
state1.acc_linear_uuid = fuse_variables::AccelerationLinear2DStamped(beginning_stamp, device_id_).uuid();
state1.position_uuid = fuse_variables::Position2DStamped(beginning_stamp, device_id).uuid();
state1.yaw_uuid = fuse_variables::Orientation2DStamped(beginning_stamp, device_id).uuid();
state1.vel_linear_uuid = fuse_variables::VelocityLinear2DStamped(beginning_stamp, device_id).uuid();
state1.vel_yaw_uuid = fuse_variables::VelocityAngular2DStamped(beginning_stamp, device_id).uuid();
state1.acc_linear_uuid = fuse_variables::AccelerationLinear2DStamped(beginning_stamp, device_id).uuid();

state_history_.emplace(beginning_stamp, std::move(state1));
state_history.emplace(beginning_stamp, std::move(state1));

return;
}
Expand All @@ -306,16 +325,16 @@ void Unicycle2D::generateMotionModel(
state2.acceleration_linear);

// Define the fuse variables required for this constraint
auto position1 = fuse_variables::Position2DStamped::make_shared(beginning_stamp, device_id_);
auto yaw1 = fuse_variables::Orientation2DStamped::make_shared(beginning_stamp, device_id_);
auto velocity_linear1 = fuse_variables::VelocityLinear2DStamped::make_shared(beginning_stamp, device_id_);
auto velocity_yaw1 = fuse_variables::VelocityAngular2DStamped::make_shared(beginning_stamp, device_id_);
auto acceleration_linear1 = fuse_variables::AccelerationLinear2DStamped::make_shared(beginning_stamp, device_id_);
auto position2 = fuse_variables::Position2DStamped::make_shared(ending_stamp, device_id_);
auto yaw2 = fuse_variables::Orientation2DStamped::make_shared(ending_stamp, device_id_);
auto velocity_linear2 = fuse_variables::VelocityLinear2DStamped::make_shared(ending_stamp, device_id_);
auto velocity_yaw2 = fuse_variables::VelocityAngular2DStamped::make_shared(ending_stamp, device_id_);
auto acceleration_linear2 = fuse_variables::AccelerationLinear2DStamped::make_shared(ending_stamp, device_id_);
auto position1 = fuse_variables::Position2DStamped::make_shared(beginning_stamp, device_id);
auto yaw1 = fuse_variables::Orientation2DStamped::make_shared(beginning_stamp, device_id);
auto velocity_linear1 = fuse_variables::VelocityLinear2DStamped::make_shared(beginning_stamp, device_id);
auto velocity_yaw1 = fuse_variables::VelocityAngular2DStamped::make_shared(beginning_stamp, device_id);
auto acceleration_linear1 = fuse_variables::AccelerationLinear2DStamped::make_shared(beginning_stamp, device_id);
auto position2 = fuse_variables::Position2DStamped::make_shared(ending_stamp, device_id);
auto yaw2 = fuse_variables::Orientation2DStamped::make_shared(ending_stamp, device_id);
auto velocity_linear2 = fuse_variables::VelocityLinear2DStamped::make_shared(ending_stamp, device_id);
auto velocity_yaw2 = fuse_variables::VelocityAngular2DStamped::make_shared(ending_stamp, device_id);
auto acceleration_linear2 = fuse_variables::AccelerationLinear2DStamped::make_shared(ending_stamp, device_id);

position1->data()[fuse_variables::Position2DStamped::X] = state1.pose.x();
position1->data()[fuse_variables::Position2DStamped::Y] = state1.pose.y();
Expand Down Expand Up @@ -345,36 +364,42 @@ void Unicycle2D::generateMotionModel(
state2.vel_yaw_uuid = velocity_yaw2->uuid();
state2.acc_linear_uuid = acceleration_linear2->uuid();

state_history_.emplace(beginning_stamp, std::move(state1));
state_history_.emplace(ending_stamp, std::move(state2));
state_history.emplace(beginning_stamp, std::move(state1));
state_history.emplace(ending_stamp, std::move(state2));

// Rotate the process noise covariance with the yaw angle of the current state orientation
const auto rotation_matrix = Eigen::Rotation2Dd(state1.pose.yaw()).toRotationMatrix();
// Apply only to x and y position as the other state variables are already along the
// current state orientation
process_noise_covariance.topLeftCorner<2, 2>() =
rotation_matrix * process_noise_covariance.topLeftCorner<2, 2>() * rotation_matrix.transpose();
Comment on lines +352 to +357
Copy link
Contributor

Choose a reason for hiding this comment

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

I'm a little confused with this. The actual residual that is computed for the unicycle constraints is just the position difference. That difference is expressed in the world/map frame here, so rotating the process noise into the frame doesn't make sense to me. This would make sense if that residual is also expressed in the first poses frame.

Copy link
Contributor

Choose a reason for hiding this comment

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

@svwilliams Has your team had a chance to validate or test this?

Copy link
Contributor

Choose a reason for hiding this comment

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

I looked at some point, but haven't had time to actually fix it.
From what I remember, I agree with the assessment that the current covariance is not represented in the same frame as the error. However, I believe the correct thing to do is change the error calculation to be in the frame of Pose1 rather than to spin the covariance to be in the global frame.

Copy link
Contributor

Choose a reason for hiding this comment

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

Yeah that makes more sense to me. I can work on a PR that does that


// Scale process noise covariance pose by the norm of the current state twist
auto process_noise_covariance = process_noise_covariance_;
if (scale_process_noise_)
if (scale_process_noise)
{
common::scaleProcessNoiseCovariance(process_noise_covariance, state1.velocity_linear, state1.velocity_yaw,
velocity_norm_min_);
velocity_norm_min);
}

// Validate
process_noise_covariance *= dt;

if (!disable_checks_)
if (!disable_checks)
{
try
{
validateMotionModel(state1, state2, process_noise_covariance);
}
catch (const std::runtime_error& ex)
{
ROS_ERROR_STREAM_THROTTLE(10.0, "Invalid '" << name_ << "' motion model: " << ex.what());
ROS_ERROR_STREAM_THROTTLE(10.0, "Invalid '" << name << "' motion model: " << ex.what());
return;
}
}

// Create the constraints for this motion model segment
auto constraint = fuse_models::Unicycle2DStateKinematicConstraint::make_shared(
name(),
name,
*position1,
*yaw1,
*velocity_linear1,
Expand Down
Loading