[WIP] Covariance Estimation #312
Conversation
* Fix this madness Simplify implementation by debugging in an ego-centric way always. Since the KISS-ICP internal map is on the global coordinates, fetch the last ego-centric pose and apply it to the map. Seen from the cloud_frame_id (which is the sensor frame) everything should always work in terms of visualization, no matter the multi-sensor setup. * Now is safe to disable this by default * Simplify, borrow the header from the input pointcloud msg This actually makes the visualization closer to the Python visualizer * Disable path/odom visualization by default In the case where we are not computing the poses in an egocentric world (base_frame != "") and we are not publishing to the TF tree, then the visualization wouldn't make sense. Therefore, disable it by default * Changed my mind If someone doesn't have that particular frame defined, then the visualization won't work. Leave this default * Move responsability of handling tf frames out of Registration (#288) * Move responsability of handling tf frames out of Registration Since with this new changes PublishOdometry is the only member that requieres to know the user specified target-frame, it is not necesary to handle all this bits. This makes the implementation cleaner, easier to read and reduces the lines of code. Now RegisterFrame is a simple callback as few months ago one can read in seconds. * typo * Easier to read * We need this for LookupTransform * Remove unused variable * Revert "Remove unused variable" This reverts commit 424ee90. * Remove unnecessary check * Remove unnecessary exposed utility function from ROS API With this change this function is not exposed (which was never the intention to) to the header. This way we can also "hide" this into a private unnamed namesapces and benefit from inlining the simple function into the translation unit * Revert "Remove unnecessary exposed utility function from ROS API" This reverts commit 23cd7ef. * Revert "Remove unnecessary check" This reverts commit d1dcb48. * merge conflicts :0 * Remove unnecessary exposed utility function from ROS API (#289) * Move responsability of handling tf frames out of Registration Since with this new changes PublishOdometry is the only member that requieres to know the user specified target-frame, it is not necesary to handle all this bits. This makes the implementation cleaner, easier to read and reduces the lines of code. Now RegisterFrame is a simple callback as few months ago one can read in seconds. * typo * Easier to read * We need this for LookupTransform * Remove unused variable * Revert "Remove unused variable" This reverts commit 424ee90. * Remove unnecessary check * Remove unnecessary exposed utility function from ROS API With this change this function is not exposed (which was never the intention to) to the header. This way we can also "hide" this into a private unnamed namesapces and benefit from inlining the simple function into the translation unit * Revert "Remove unnecessary exposed utility function from ROS API" This reverts commit 23cd7ef. * Remove unnecessary exposed utility function from ROS API With this change this function is not exposed (which was never the intention to) to the header. This way we can also "hide" this into a private unnamed namesapces and benefit from inlining the simple function into the translation unit * Revert "Remove unnecessary check" This reverts commit d1dcb48. --------- Co-authored-by: tizianoGuadagnino <frevo93@gmail.com> * too many merges * Merge Rviz and Python colors * Just make the default construction more clear --------- Co-authored-by: tizianoGuadagnino <frevo93@gmail.com>
…-covariance' into 296-autocompute-icp-covariance
tizianoGuadagnino
changed the title
| @@ -79,10 +81,6 @@ class OdometryServer : public rclcpp::Node { | |||
| /// Global/map coordinate frame. | |||
| std::string odom_frame_{"odom"}; | |||
| std::string base_frame_{}; | |||
|
|
|||
| /// Covariance diagonal | |||
There was a problem hiding this comment.
Not something to tackle right away. But I'd prefer if we keep this fixed covariance as an optional parameter. Similar to how we do (only in python) for the adaptive threshold. I believe many people would like to fuse the KISS ICP output and just give it a constant "weight"
There was a problem hiding this comment.
Make sense ;)
| icp_correction.pose = estimation * icp_correction.pose; | ||
| // Follow Ollila et.al. https://arxiv.org/pdf/2006.10005.pdf - Eq. (2) for sample covariance | ||
| // using an M estimator | ||
| icp_correction.covariance = 1.0 / static_cast<double>(associations.size()) * JTJ_inverse; |
There was a problem hiding this comment.
What happens if associations is empty 🤔
There was a problem hiding this comment.
The linear system will explode so also dx will be garbage.
| icp_correction.pose = estimation * icp_correction.pose; | ||
| // Follow Ollila et.al. https://arxiv.org/pdf/2006.10005.pdf - Eq. (2) for sample covariance | ||
| // using an M estimator | ||
| icp_correction.covariance = 1.0 / static_cast<double>(associations.size()) * JTJ_inverse; | ||
| // Equation (12) |
There was a problem hiding this comment.
Shall we remove all this equation comments ?
There was a problem hiding this comment.
I wanted to keep it there until we don't decide to merge as a reference. I don't want this on main, but for now I think it is useful during the development if some of us wants to go a bit deeper in the computation.
| if (N < 2) return pred; | ||
| return poses_[N - 2].inverse() * poses_[N - 1]; | ||
| Estimate KissICP::GetPredictionModel() const { | ||
| Estimate prediction; |
There was a problem hiding this comment.
I think renaming Estimate will also help here, because Estimate prediction looks a bit weird
| @@ -56,21 +56,22 @@ def register_frame(self, frame, timestamps): | |||
|
|
|||
| # Compute initial_guess for ICP | |||
| prediction = self.get_prediction_model() | |||
| last_pose = self.poses[-1] if self.poses else np.eye(4) | |||
| initial_guess = last_pose @ prediction | |||
| initial_guess = self.estimates[-1] if self.estimates else Estimate() | |||
There was a problem hiding this comment.
Maybe better last_estimate? Since it is not yet the initial guess
|
Hi Guys, any news on this? |
Unfortunately not, I don't feel we are ready for this change, as in general, I am not convinced that the benefits of adding this "uncertainty" estimation (notice the quotes) are worth this change; this will require more discussions with the KISS core team, so for now we postponed this at the moment. |
I am not necessarily disagreeing, (nor am I agreeing), but if you have a chance, can you please share some of the concerns and critiques of this approach? It seems like maybe I missed something. |
I introduce the covariance estimation in the KISS pipeline (python/cpp/ROS).
The main change is the introduction of a
struct Estimateto contain both the pose and the system's covariance for a certain scan. Thanks to some operator overloading and some basicpybind, this change should be "invisible" outside of theRegistration.hpp/cppfiles.The code is still not as clean as I would like it to be, but I think it is time for @nachovizzo and @benemer to come into the game as well as I work out all the math.
Other than that, give this a look. I will ask you, @nachovizzo, to intensively test this on ROS2 data, as I think you have tons of that.
I have a small mental bug regarding the math, as the covariance will continue to increase as the run goes further, I am studying the problem to see if there is a way to "regularize" this, I will keep you updated.
Let me know what do you think.