feat: added Reset initial pose through ros service #241

RamanaBotta · 2023-10-12T12:30:00Z

Issue

What?

kiss-icp is not consistent for global localization, so we need to have a reset_pose feature when working with GPS or other global localization systems.
So this pull request adds such feature to provide such a feature to the ros1 version of kiss-icp.

Why?

Since kiss-icp is prone to accumulation of positional errors over time, having something like a service call to reset odometry would really help when working with other sensors, especially sensors like GPS (globally consistent).

How?

Created a service file in ros.
Added a New method, reset_pose at kiss_icp::pipeline::Reset Here
And calling this method once service is called from ros.

Testing?

roslaunch kiss_icp odometry.launch
rosservice call /reset_pose "{x: 0.0, y: 0.0, z: 0.0, R: 0.0, P: 0.0, Y: 0.0}"

Repository:

https://github.com/RamanaBotta/kiss-icp

Branch to be tested:

Reset-initial-pose-through-ros-service

Observations

When /reset_pose service was called, kiss-icp odometry should start from the same position as requested in the service call.
In My local system, ros-noetic, Ubuntu 20.04, Results are as expected.

Expected Results

When /reset_pose service was called, kiss-icp odometry should start from the same position as requested in the service call.

Screenshots (optional)

Changelog (anytopics changed/major changes)

Anything Else?

- clears local map - clears path (trajectory)

nachovizzo · 2023-10-12T13:44:46Z

Please fix the ci! Install ore commit and run it locally. You can also check the email you got about the error

nachovizzo · 2023-10-12T21:40:18Z

cpp/kiss_icp/pipeline/KissICP.cpp

@@ -107,4 +107,11 @@ bool KissICP::HasMoved() {
    return motion > 5.0 * config_.min_motion_th;
 }

+void KissICP::Reset(Eigen::Quaterniond quaternion, Eigen::Vector3d translation) {


This method does not reset the poses but instead injects a new one... I'd split this into two methods. What do you think?

Alright, My understanding from your comment,
1 . Reset to be done when we Reset to the origin position.
2 . Since This feature is setting the starting position, we need to have a method SetPose.
Is that what you are referring to?

nachovizzo · 2023-10-12T21:44:20Z

@RamanaBotta , I'm adding @benemer and @tizianoGuadagnino to join the review party.

As a general comment, the draft looks ok. But it's far from our standards. KISS-ICP is a multi-framework project. This PR is only affecting the ROS 1 side; what you would be missing is the following:

Implement this in the Python API
Implement this in the ROS 2 API

I know it's too much to ask, but we will never merge this in the current state. We spend a lot of time and effort to make the system what it is right now. I hope you understand 😃

What do you think? Would you be able to do the extra effort and complete the integration?

RamanaBotta · 2023-10-13T06:17:48Z

Hello @nachovizzo, Thank you for assigning reviewers.
@tizianoGuadagnino, @benemer Welcome to this pull request.

@nachovizzo, I can add support for ROS2 API. But I need some support on Python API.
I am happy to put in extra effort and complete the integration with your support.

tizianoGuadagnino · 2023-10-13T09:11:56Z

cpp/kiss_icp/pipeline/KissICP.hpp

@@ -67,6 +67,7 @@ class KissICP {
    double GetAdaptiveThreshold();
    Sophus::SE3d GetPredictionModel() const;
    bool HasMoved();
+    void Reset(Eigen::Quaterniond, Eigen::Vector3d);


Please use Sophus::SE3d as argument. Quaternion&Vector is more of a ROS thing and I will like to keep the cpp API Sophus only.

tizianoGuadagnino · 2023-10-13T09:16:10Z

cpp/kiss_icp/pipeline/KissICP.cpp

+    poses_.clear();
+    Sophus::SE3d predpred(quaternion, translation);
+    poses_.push_back(predpred);
+    local_map_.Clear();


Instead of resetting the local map here, would in make more sense to transformed it the new reference frame so that KISS can continue his "job" with the old map points?

What do you think ? @nachovizzo @benemer @RamanaBotta

I agree, we shold transform the local map and the poses to the new frame. Otherwise, every time Reset is called, the constant velocity model can not be applied and the map is empty. So you lose a lot of the nice properties of KISS.

Instead of resetting the local map here, would in make more sense to transformed it the new reference frame so that KISS can continue his "job" with the old map points?

What do you think ? @nachovizzo @benemer @RamanaBotta

I agree, It makes sense to transform the local map to the new reference frame.

Hello @tizianoGuadagnino @benemer and @nachovizzo ,
I have tried transforming the local map to the new reference frame. Please look here for implementation.

Issue

After resetting to the new frame, the Local map is not looking good, It has previous ref frame points too.

Help

Please consider looking into implementation here, please let me know any corrections or suggestions.

You are applying the wrong transformation, because `local_cloud is not expressed in the sensor frame. We need to keep track of the frame of the map since this will change when it is reset. Initially, this frame is the first pose of the sensor which is assumed to be identity.

Therefore, you have to apply first the inverse of the current map's frame and then transform it using the desired new reference.

Can you please push your changes to this branch? This makes it easier to discuss it.

For example, you are still clearing the poses when resetting the map. This will prevent the system from de-skewing and using a constant velocity model because the velocity can not be estimated without prior pose information. If it is desired to reset the list of estimated poses, I would at least transform and keep the last two.

Hi @benemer @tizianoGuadagnino, Can you Please reiterate this suggestion on transforming the local map, without this also things are working fine.

Hi @RamanaBotta, for us it is not just important that it works but it also needs to be correct from the theoretical perspective. Once the pose has been reset, it just makes sense to transform the local map in the new reference frame, so that KISS can use the previous map. Please fix this.

…-frame chore : attempt to local-map-transform-to-new-ref-frame

nachovizzo · 2023-11-23T18:02:47Z

Hello, thanks for the amazing work. This branch got a bit outdated and the PR is getting a bit bigger than expected. I'm closing the PR now in favor of opening a new one. But please try to do so once the code has been tested, and it's in a final state. Otherwise, the collaboration from our side turns to be a bit challenging since we none of us is working full-time in this project.

Thanks a lot!

RamanaBotta · 2023-11-24T03:44:22Z

Okay @nachovizzo ,
Can you please tell me, How to get support from you without the pull request?

feat: added Reset initial pose through ros service

8fff56b

- clears local map - clears path (trajectory)

RamanaBotta added 3 commits October 12, 2023 21:58

chore : precommit checks fixes

8f68153

Merge branch 'main' into Reset-initial-pose-through-ros-service

3df4be5

format commits

295da42

nachovizzo reviewed Oct 12, 2023

View reviewed changes

nachovizzo requested review from tizianoGuadagnino and benemer October 12, 2023 21:41

tizianoGuadagnino reviewed Oct 13, 2023

View reviewed changes

RamanaBotta and others added 3 commits October 13, 2023 23:24

chore : added sophus::SE3d as argument to Reset method

1995df8

chore : attempt to local-map-transform-to-new-ref-frame

6f17fcd

Merge pull request #1 from RamanaBotta/local-map-transform-to-new-ref…

b3ac61b

…-frame chore : attempt to local-map-transform-to-new-ref-frame

nachovizzo closed this Nov 23, 2023

benemer mentioned this pull request Apr 18, 2024

Adjust Global Frame in odometry pipeline #331

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat: added Reset initial pose through ros service #241

feat: added Reset initial pose through ros service #241

RamanaBotta commented Oct 12, 2023 •

edited

Loading

nachovizzo commented Oct 12, 2023

nachovizzo Oct 12, 2023

RamanaBotta Oct 13, 2023

nachovizzo commented Oct 12, 2023

RamanaBotta commented Oct 13, 2023

tizianoGuadagnino Oct 13, 2023

RamanaBotta Oct 13, 2023

RamanaBotta Oct 13, 2023

tizianoGuadagnino Oct 13, 2023

benemer Oct 13, 2023

RamanaBotta Oct 13, 2023

RamanaBotta Oct 13, 2023 •

edited

Loading

benemer Oct 16, 2023

benemer Oct 16, 2023

RamanaBotta Oct 25, 2023 •

edited

Loading

tizianoGuadagnino Oct 26, 2023

nachovizzo commented Nov 23, 2023

RamanaBotta commented Nov 24, 2023

feat: added Reset initial pose through ros service #241

feat: added Reset initial pose through ros service #241

Conversation

RamanaBotta commented Oct 12, 2023 • edited Loading

Issue

What?

Why?

How?

Testing?

Repository:

Branch to be tested:

Observations

Expected Results

Screenshots (optional)

Changelog (anytopics changed/major changes)

Anything Else?

nachovizzo commented Oct 12, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

nachovizzo commented Oct 12, 2023

RamanaBotta commented Oct 13, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

RamanaBotta Oct 13, 2023 • edited Loading

Choose a reason for hiding this comment

Issue

Help

Choose a reason for hiding this comment

Choose a reason for hiding this comment

RamanaBotta Oct 25, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

nachovizzo commented Nov 23, 2023

RamanaBotta commented Nov 24, 2023

RamanaBotta commented Oct 12, 2023 •

edited

Loading

RamanaBotta Oct 13, 2023 •

edited

Loading

RamanaBotta Oct 25, 2023 •

edited

Loading