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.

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

Add empty pipeline constructor #332

Closed
wants to merge 5 commits into from
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
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
15 changes: 9 additions & 6 deletions cpp/kiss_icp/core/Registration.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -167,15 +167,18 @@ LinearSystem BuildLinearSystem(const Correspondences &correspondences, const dou

namespace kiss_icp {

// Need to provide a definition to the static attribute

Registration::Registration(int max_num_iteration, double convergence_criterion, int max_num_threads)
: max_num_iterations_(max_num_iteration),
convergence_criterion_(convergence_criterion),
// Only manipulate the number of threads if the user specifies something greater than 0
max_num_threads_(max_num_threads > 0 ? max_num_threads : tbb::info::default_concurrency()) {
// This global variable requires static duration storage to be able to manipulate the max
// concurrency from TBB across the entire class
static const auto tbb_control_settings = tbb::global_control(
tbb::global_control::max_allowed_parallelism, static_cast<size_t>(max_num_threads_));
tbb_control_settings(tbb::global_control::max_allowed_parallelism,
static_cast<size_t>(tbb::info::default_concurrency())) {
// Only manipulate the number of threads if the user specifies something greater than 0
if (max_num_threads > 0) {
tbb_control_settings = tbb::global_control(tbb::global_control::max_allowed_parallelism,
static_cast<size_t>(max_num_threads));
}
}

Sophus::SE3d Registration::AlignPointsToMap(const std::vector<Eigen::Vector3d> &frame,
Expand Down
7 changes: 6 additions & 1 deletion cpp/kiss_icp/core/Registration.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,9 @@
// SOFTWARE.
#pragma once

#include <tbb/global_control.h>
#include <tbb/info.h>

#include <Eigen/Core>
#include <sophus/se3.hpp>
#include <vector>
Expand All @@ -41,6 +44,8 @@ struct Registration {

int max_num_iterations_;
double convergence_criterion_;
int max_num_threads_;
// This attribute is needed to be able to manipulate the max
// concurrency from TBB this class
tbb::global_control tbb_control_settings;
};
} // namespace kiss_icp
2 changes: 2 additions & 0 deletions cpp/kiss_icp/pipeline/KissICP.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@ class KissICP {
using Vector3dVectorTuple = std::tuple<Vector3dVector, Vector3dVector>;

public:
explicit KissICP() : KissICP(KISSConfig()) {}
benemer marked this conversation as resolved.
Show resolved Hide resolved
explicit KissICP(const KISSConfig &config)
: config_(config),
registration_(
Expand All @@ -74,6 +75,7 @@ class KissICP {
std::vector<Eigen::Vector3d> LocalMap() const { return local_map_.Pointcloud(); };
Sophus::SE3d pose() const { return last_pose_; }
Sophus::SE3d delta() const { return last_delta_; }
const KISSConfig &config() const { return config_; }
benemer marked this conversation as resolved.
Show resolved Hide resolved

private:
Sophus::SE3d last_pose_;
Expand Down
2 changes: 1 addition & 1 deletion ros/src/OdometryServer.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@ class OdometryServer : public rclcpp::Node {
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr map_publisher_;

/// KISS-ICP
std::unique_ptr<kiss_icp::pipeline::KissICP> kiss_icp_;
std::unique_ptr<kiss_icp::pipeline::KissICP> kiss_icp_ = nullptr;

/// Global/map coordinate frame.
std::string odom_frame_{"odom"};
Expand Down
Loading