Skip to content

Commit

Permalink
Last stop: rename "odometry_" for "kiss_icp_"
Browse files Browse the repository at this point in the history
  • Loading branch information
nachovizzo committed Mar 11, 2024
1 parent 20c2d74 commit 8c38132
Show file tree
Hide file tree
Showing 2 changed files with 5 additions and 5 deletions.
8 changes: 4 additions & 4 deletions ros/src/OdometryServer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
// clang-format on

// Construct the main KISS-ICP odometry node
odometry_ = std::make_unique<kiss_icp::pipeline::KissICP>(config_);
kiss_icp_ = std::make_unique<kiss_icp::pipeline::KissICP>(config_);

// Initialize subscribers
pointcloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
Expand Down Expand Up @@ -128,10 +128,10 @@ void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSha
const auto egocentric_estimation = (base_frame_.empty() || base_frame_ == cloud_frame_id);

// Register frame, main entry point to KISS-ICP pipeline
const auto &[frame, keypoints] = odometry_->RegisterFrame(points, timestamps);
const auto &[frame, keypoints] = kiss_icp_->RegisterFrame(points, timestamps);

// Compute the pose using KISS, ego-centric to the LiDAR
const Sophus::SE3d kiss_pose = odometry_->poses().back();
const Sophus::SE3d kiss_pose = kiss_icp_->poses().back();

// If necessary, transform the ego-centric pose to the specified base_link/base_footprint frame
const auto pose = [&]() -> Sophus::SE3d {
Expand Down Expand Up @@ -186,7 +186,7 @@ void OdometryServer::PublishClouds(const std::vector<Eigen::Vector3d> frame,
odom_header.frame_id = odom_frame_;

// Publish map
const auto kiss_map = odometry_->LocalMap();
const auto kiss_map = kiss_icp_->LocalMap();

if (!publish_odom_tf_) {
// debugging happens in an egocentric world
Expand Down
2 changes: 1 addition & 1 deletion ros/src/OdometryServer.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@ class OdometryServer : public rclcpp::Node {
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr traj_publisher_;

/// KISS-ICP
std::unique_ptr<kiss_icp::pipeline::KissICP> odometry_;
std::unique_ptr<kiss_icp::pipeline::KissICP> kiss_icp_;
kiss_icp::pipeline::KISSConfig config_;

/// Global/map coordinate frame.
Expand Down

0 comments on commit 8c38132

Please sign in to comment.