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

Auto 2257 fix double construction kiss icp cpp #294

Merged
merged 7 commits into from
Mar 18, 2024
Merged

Auto 2257 fix double construction kiss icp cpp #294

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
15e20a2
First stop. Remove default constructor
nachovizzo Mar 11, 2024
20c2d74
Second stop: make the only kiss_icp::pipeline::KissICP consumer use ptr
nachovizzo Mar 11, 2024
8c38132
Last stop: rename "odometry_" for "kiss_icp_"
nachovizzo Mar 11, 2024
bc6cfdc
Remove unnecessary class member
nachovizzo Mar 11, 2024
250708a
format
nachovizzo Mar 11, 2024
3fed3a6
Remove unnncesary duplicated call
nachovizzo Mar 11, 2024
533047c
Update utility function to accoomodate for empty timestamps
nachovizzo Mar 11, 2024
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
2 changes: 1 addition & 1 deletion cpp/kiss_icp/pipeline/KissICP.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ namespace kiss_icp::pipeline {
KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vector3d> &frame,
const std::vector<double> &timestamps) {
const auto &deskew_frame = [&]() -> std::vector<Eigen::Vector3d> {
if (!config_.deskew) return frame;
if (!config_.deskew || timestamps.empty()) return frame;
// TODO(Nacho) Add some asserts here to sanitize the timestamps

// If not enough poses for the estimation, do not de-skew
Expand Down
2 changes: 0 additions & 2 deletions cpp/kiss_icp/pipeline/KissICP.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,8 +63,6 @@ class KissICP {
local_map_(config.voxel_size, config.max_range, config.max_points_per_voxel),
adaptive_threshold_(config.initial_threshold, config.min_motion_th, config.max_range) {}

KissICP() : KissICP(KISSConfig{}) {}

public:
Vector3dVectorTuple RegisterFrame(const std::vector<Eigen::Vector3d> &frame);
Vector3dVectorTuple RegisterFrame(const std::vector<Eigen::Vector3d> &frame,
Expand Down
40 changes: 20 additions & 20 deletions ros/src/OdometryServer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,26 +54,29 @@ using utils::PointCloud2ToEigen;

OdometryServer::OdometryServer(const rclcpp::NodeOptions &options)
: rclcpp::Node("odometry_node", options) {
// clang-format off
base_frame_ = declare_parameter<std::string>("base_frame", base_frame_);
odom_frame_ = declare_parameter<std::string>("odom_frame", odom_frame_);
publish_odom_tf_ = declare_parameter<bool>("publish_odom_tf", publish_odom_tf_);
publish_debug_clouds_ = declare_parameter<bool>("visualize", publish_debug_clouds_);
config_.max_range = declare_parameter<double>("max_range", config_.max_range);
config_.min_range = declare_parameter<double>("min_range", config_.min_range);
config_.deskew = declare_parameter<bool>("deskew", config_.deskew);
config_.voxel_size = declare_parameter<double>("voxel_size", config_.max_range / 100.0);
config_.max_points_per_voxel = declare_parameter<int>("max_points_per_voxel", config_.max_points_per_voxel);
config_.initial_threshold = declare_parameter<double>("initial_threshold", config_.initial_threshold);
config_.min_motion_th = declare_parameter<double>("min_motion_th", config_.min_motion_th);
if (config_.max_range < config_.min_range) {
RCLCPP_WARN(get_logger(), "[WARNING] max_range is smaller than min_range, settng min_range to 0.0");
config_.min_range = 0.0;

kiss_icp::pipeline::KISSConfig config;
config.max_range = declare_parameter<double>("max_range", config.max_range);
config.min_range = declare_parameter<double>("min_range", config.min_range);
config.deskew = declare_parameter<bool>("deskew", config.deskew);
config.voxel_size = declare_parameter<double>("voxel_size", config.max_range / 100.0);
config.max_points_per_voxel =
declare_parameter<int>("max_points_per_voxel", config.max_points_per_voxel);
config.initial_threshold =
declare_parameter<double>("initial_threshold", config.initial_threshold);
config.min_motion_th = declare_parameter<double>("min_motion_th", config.min_motion_th);
if (config.max_range < config.min_range) {
RCLCPP_WARN(get_logger(),
"[WARNING] max_range is smaller than min_range, settng min_range to 0.0");
config.min_range = 0.0;
}
// clang-format on

// Construct the main KISS-ICP odometry node
odometry_ = 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 @@ -121,17 +124,14 @@ Sophus::SE3d OdometryServer::LookupTransform(const std::string &target_frame,
void OdometryServer::RegisterFrame(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &msg) {
const auto cloud_frame_id = msg->header.frame_id;
const auto points = PointCloud2ToEigen(msg);
const auto timestamps = [&]() -> std::vector<double> {
if (!config_.deskew) return {};
return GetTimestamps(msg);
}();
const auto timestamps = GetTimestamps(msg);
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
3 changes: 1 addition & 2 deletions ros/src/OdometryServer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,8 +85,7 @@ class OdometryServer : public rclcpp::Node {
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr traj_publisher_;

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

/// Global/map coordinate frame.
std::string odom_frame_{"odom"};
Expand Down
22 changes: 12 additions & 10 deletions ros/src/Utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@
#include <algorithm>
#include <cstddef>
#include <memory>
#include <optional>
#include <regex>
#include <sophus/se3.hpp>
#include <string>
Expand All @@ -35,6 +36,8 @@
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/transform.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/logging.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>

Expand Down Expand Up @@ -88,17 +91,18 @@ inline std::string FixFrameId(const std::string &frame_id) {
return std::regex_replace(frame_id, std::regex("^/"), "");
}

inline auto GetTimestampField(const PointCloud2::ConstSharedPtr msg) {
inline std::optional<PointField> GetTimestampField(const PointCloud2::ConstSharedPtr msg) {
PointField timestamp_field;
for (const auto &field : msg->fields) {
if ((field.name == "t" || field.name == "timestamp" || field.name == "time")) {
timestamp_field = field;
}
}
if (!timestamp_field.count) {
throw std::runtime_error("Field 't', 'timestamp', or 'time' does not exist");
}
return timestamp_field;
if (timestamp_field.count) return timestamp_field;
RCLCPP_WARN_ONCE(rclcpp::get_logger("odometry_node"),
"Field 't', 'timestamp', or 'time' does not exist. "
"Disabling scan deskewing");
return {};
}

// Normalize timestamps from 0.0 to 1.0
Expand All @@ -116,7 +120,7 @@ inline auto NormalizeTimestamps(const std::vector<double> &timestamps) {
}

inline auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg,
const PointField &field) {
const PointField &timestamp_field) {
auto extract_timestamps =
[&msg]<typename T>(sensor_msgs::PointCloud2ConstIterator<T> &&it) -> std::vector<double> {
const size_t n_points = msg->height * msg->width;
Expand All @@ -128,9 +132,6 @@ inline auto ExtractTimestampsFromMsg(const PointCloud2::ConstSharedPtr msg,
return NormalizeTimestamps(timestamps);
};

// Get timestamp field that must be one of the following : {t, timestamp, time}
auto timestamp_field = GetTimestampField(msg);

// According to the type of the timestamp == type, return a PointCloud2ConstIterator<type>
using sensor_msgs::PointCloud2ConstIterator;
if (timestamp_field.datatype == PointField::UINT32) {
Expand Down Expand Up @@ -191,9 +192,10 @@ inline void FillPointCloud2Timestamp(const std::vector<double> &timestamps, Poin

inline std::vector<double> GetTimestamps(const PointCloud2::ConstSharedPtr msg) {
auto timestamp_field = GetTimestampField(msg);
if (!timestamp_field.has_value()) return {};

// Extract timestamps from cloud_msg
std::vector<double> timestamps = ExtractTimestampsFromMsg(msg, timestamp_field);
std::vector<double> timestamps = ExtractTimestampsFromMsg(msg, timestamp_field.value());

return timestamps;
}
Expand Down
Loading