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

feat: added Reset initial pose through ros service #241

Closed
wants to merge 7 commits into from
Closed

feat: added Reset initial pose through ros service #241

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
8fff56b
feat: added Reset initial pose through ros service
RamanaBotta Oct 12, 2023
8f68153
chore : precommit checks fixes
RamanaBotta Oct 12, 2023
3df4be5
Merge branch 'main' into Reset-initial-pose-through-ros-service
RamanaBotta Oct 12, 2023
295da42
format commits
RamanaBotta Oct 12, 2023
1995df8
chore : added sophus::SE3d as argument to Reset method
RamanaBotta Oct 13, 2023
6f17fcd
chore : attempt to local-map-transform-to-new-ref-frame
RamanaBotta Oct 13, 2023
b3ac61b
Merge pull request #1 from RamanaBotta/local-map-transform-to-new-ref…
RamanaBotta Nov 22, 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
14 changes: 14 additions & 0 deletions cpp/kiss_icp/pipeline/KissICP.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,4 +107,18 @@ bool KissICP::HasMoved() {
return motion > 5.0 * config_.min_motion_th;
}

void KissICP::Reset(Sophus::SE3d se3d_transform) {
// getting local map
std::vector<Eigen::Vector3d> local_cloud = local_map_.Pointcloud();
// clearing poses
poses_.clear();
// adding new start pose
poses_.push_back(se3d_transform);
// clearing the previous map (wrt previus frame of reference)
local_map_.Clear();
Copy link
Collaborator

Choose a reason for hiding this comment

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

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

Copy link
Member

Choose a reason for hiding this comment

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

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.

Copy link
Author

Choose a reason for hiding this comment

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

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.

Copy link
Author

@RamanaBotta RamanaBotta Oct 13, 2023 •
edited
Loading

Choose a reason for hiding this comment

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

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.
Screenshot from 2023-10-14 00-13-14

Help

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

Copy link
Member

Choose a reason for hiding this comment

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

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.

Copy link
Member

Choose a reason for hiding this comment

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

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.

Copy link
Author

@RamanaBotta RamanaBotta Oct 25, 2023 •
edited
Loading

Choose a reason for hiding this comment

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

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

Copy link
Collaborator

Choose a reason for hiding this comment

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

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.

// Updating local_map_ with the transformed map wrt new frame
local_map_.Update(local_cloud, se3d_transform);

}

} // namespace kiss_icp::pipeline
1 change: 1 addition & 0 deletions cpp/kiss_icp/pipeline/KissICP.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,7 @@ class KissICP {
double GetAdaptiveThreshold();
Sophus::SE3d GetPredictionModel() const;
bool HasMoved();
void Reset(Sophus::SE3d);

public:
// Extra C++ API to facilitate ROS debugging
Expand Down
5 changes: 4 additions & 1 deletion ros/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,10 @@ if("$ENV{ROS_VERSION}" STREQUAL "1")
rosbag
std_msgs
tf2
tf2_ros)
tf2_ros
message_generation)
add_service_files(FILES set_pose.srv)
generate_messages(DEPENDENCIES std_msgs)
catkin_package()

# ROS 1 node
Expand Down
3 changes: 2 additions & 1 deletion ros/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,8 @@
<depend condition="$ROS_VERSION == 2">rclcpp</depend>
<depend condition="$ROS_VERSION == 2">rclcpp_components</depend>
<exec_depend condition="$ROS_VERSION == 2">ros2launch</exec_depend>

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<!-- ROS1/2 dependencies -->
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
Expand Down
16 changes: 16 additions & 0 deletions ros/ros1/OdometryServer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,7 @@ OdometryServer::OdometryServer(const ros::NodeHandle &nh, const ros::NodeHandle
// Initialize subscribers
pointcloud_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>("pointcloud_topic", queue_size_,
&OdometryServer::RegisterFrame, this);
service = nh_.advertiseService("reset_pose", &OdometryServer::reset_pose, this);

// Initialize publishers
odom_publisher_ = pnh_.advertise<nav_msgs::Odometry>("odometry", queue_size_);
Expand Down Expand Up @@ -172,6 +173,21 @@ void OdometryServer::RegisterFrame(const sensor_msgs::PointCloud2::ConstPtr &msg
local_map_header.frame_id = odom_frame_;
map_publisher_.publish(*std::move(EigenToPointCloud2(odometry_.LocalMap(), local_map_header)));
}

bool OdometryServer::reset_pose(kiss_icp::set_pose::Request &req,
kiss_icp::set_pose::Response &res) {
Eigen::Quaterniond quaternion =
Eigen::Quaterniond(Eigen::AngleAxisd(req.R, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(req.P, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(req.Y, Eigen::Vector3d::UnitZ()));
Eigen::Vector3d translation(req.x, req.y, req.z);
Sophus::SE3d se3d_transform(quaternion, translation);
odometry_.Reset(se3d_transform);
path_msg_.poses.clear();
res.done = true;
return true;
}

} // namespace kiss_icp_ros

int main(int argc, char **argv) {
Expand Down
4 changes: 4 additions & 0 deletions ros/ros1/OdometryServer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,8 @@
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/transform_broadcaster.h>

#include "kiss_icp/set_pose.h"

namespace kiss_icp_ros {

class OdometryServer {
Expand All @@ -41,6 +43,7 @@ class OdometryServer {
private:
/// Register new frame
void RegisterFrame(const sensor_msgs::PointCloud2::ConstPtr &msg);
bool reset_pose(kiss_icp::set_pose::Request &req, kiss_icp::set_pose::Response &res);

/// Ros node stuff
ros::NodeHandle nh_;
Expand All @@ -62,6 +65,7 @@ class OdometryServer {
ros::Publisher frame_publisher_;
ros::Publisher kpoints_publisher_;
ros::Publisher map_publisher_;
ros::ServiceServer service;

/// KISS-ICP
kiss_icp::pipeline::KissICP odometry_;
Expand Down
10 changes: 10 additions & 0 deletions ros/srv/set_pose.srv
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
# Translation XYZ [meters]
float32 x
float32 y
float32 z
# Orientation RPY [radians]
float32 R
float32 P
float32 Y
---
bool done