Adjust Global Frame in odometry pipeline

[tizianoGuadagnino]

Me and @benemer added this change so that it is possible to adjust the global reference frame of KISS. This can be useful if one needs to adjust the pose estimate based on a new global pose measurement (coming from GPS, April tag localization system, or any indoor global localization system out there). Notice that this change only affects the CPP API, as a user can already do this manually in Python. For the ROS "API", this highly depends on the global measurement used, which we can probably also leave to the users in their custom nodes.

[benemer]

Short notice: We already discussed this on the ROS side in #241, but the PR got closed. The main issue was that the map was cleared instead of being transformed as done here.

Actually, since we no longer keep track of the trajectory, this functionality is now easier to implement and also feels more natural.

[nachovizzo]

I also did something similar, but mainly for resetting the poses. I need to think about this one in particular, as calling that added method "spiritually" breaks the odometry assumption.... which is also not that hard given we already really on a local representation of the environment.

Let's say I'm using this odometry source to path plan around a local map, if suddenly I get a pose from whatever source. then this will make the state of the robot jump immediately, even when the pose is being corrected to a global map.

I'm not saying we shouldn't add this, as it doesn't hurt just leaving it there. And since the only consumer of this pipeline is ROS, this won't break anything. But I'm trying to think if this is worth adding here from a conceptual point of view.

What's your view 🧘 ?

[nachovizzo]

Adding REP-105 as reference, which is what most people is using to stick to as a common referennce

odom

The coordinate frame called odom is a world-fixed frame. The pose of a mobile platform in the odom frame can drift over time, without any bounds. This drift makes the odom frame useless as a long-term global reference. However, the pose of a robot in the odom frame is guaranteed to be continuous, meaning that the pose of a mobile platform in the odom frame always evolves in a smooth way, without discrete jumps.

In a typical setup the odom frame is computed based on an odometry source, such as wheel odometry, visual odometry or an inertial measurement unit.

The odom frame is useful as an accurate, short-term local reference, but drift makes it a poor frame for long-term reference.

map
The coordinate frame called map is a world fixed frame, with its Z-axis pointing upwards. The pose of a mobile platform, relative to the map frame, should not significantly drift over time. The map frame is not continuous, meaning the pose of a mobile platform in the map frame can change in discrete jumps at any time.

In a typical setup, a localization component constantly re-computes the robot pose in the map frame based on sensor observations, therefore eliminating drift, but causing discrete jumps when new sensor information arrives.

The map frame is useful as a long-term global reference, but discrete jumps in position estimators make it a poor reference frame for local sensing and acting.

[tizianoGuadagnino]

I see your point, and from a ROS-centric perspective, this is right as, typically, the map -> odom frame is managed by the tfs so that the odometry estimate is (locally) continuous. The problem is that if, for whatever reason, you are not in a ROS environment, you have no control over the reference frame in which the odometry is expressed. This is a potential limitation that can pop up later. In python that is not a problem as the pose and the map are public so that the user can manipulate them.

A better solution philosophically will be to make the map and the last_pose public in the KissICP C++ pipeline, so that the user can play around with this outside of ROS and we do not pollute the API with this concept.

what do you think? @nachovizzo @benemer

[tizianoGuadagnino]

I got a proposal code now, check also the commit message

[benemer]

I see your point @nachovizzo. The new solution is a good compromise then, we at least give the user access to the members such that they can implement their own AdjustGlobalFrame, ResetOdometry, SetPose, or WhatEver :)

[nachovizzo]

Great solution. This is exactly what I had in mind: providing const and non-const references. If in a downstream application, the user decides to mess up with the internal map representation, it would be explicit in the code and make more sense to me.

Another example I was thinking to support this idea was the example of having a wheel odometry node that converts the encoder readings from the wheels of a robot in odometry messages. It would be strange to provide an API entry point to inject a reading from outside this context. But if I want to write a wheel odometry manager and do crazy stuff with it would be nice to have the opportunity.

From the const point of view, it would be better not allow to modify the internal map representation, but this would force copying data, something that was already happening in the original proposal:

void KissICP::adjustGlobalFrame(const Sophus::SE3d &global_frame_correction) {
    last_pose_ = global_frame_correction * last_pose_;
    KissICP::Vector3dVector map_points = local_map_.Pointcloud();
    local_map_.Clear();
    std::transform(map_points.cbegin(), map_points.cend(), map_points.begin(),
                   [&](const auto &p) { return global_frame_correction * p; });
    local_map_.AddPoints(map_points);
}

This might be the weak point of this nonconst ref API. but I believe it is worth taking the risk of this "unsafe" API to simplify consumption of the pipeline