From de9a8c57a9016fc9d78a6cd0bf08d097eab652b9 Mon Sep 17 00:00:00 2001 From: Kenny Chen Date: Tue, 11 Jan 2022 18:02:24 -0800 Subject: [PATCH] Release v1.2.0 --- cfg/dlo.yaml | 2 +- include/dlo/odom.h | 1 - include/nano_gicp/impl/nano_gicp_impl.hpp | 14 +++++- include/nano_gicp/nano_gicp.hpp | 3 ++ package.xml | 2 +- src/dlo/odom.cc | 52 ++++++++++++----------- 6 files changed, 44 insertions(+), 30 deletions(-) diff --git a/cfg/dlo.yaml b/cfg/dlo.yaml index 54d627e..7b923db 100644 --- a/cfg/dlo.yaml +++ b/cfg/dlo.yaml @@ -9,7 +9,7 @@ dlo: - version: 1.1.0 + version: 1.2.0 adaptiveParams: true diff --git a/include/dlo/odom.h b/include/dlo/odom.h index dd478f4..1a925df 100644 --- a/include/dlo/odom.h +++ b/include/dlo/odom.h @@ -82,7 +82,6 @@ class dlo::OdomNode { std::atomic dlo_initialized; std::atomic imu_calibrated; - std::atomic normals_initialized; std::string odom_frame; std::string child_frame; diff --git a/include/nano_gicp/impl/nano_gicp_impl.hpp b/include/nano_gicp/impl/nano_gicp_impl.hpp index ca7b8f8..bc17e54 100644 --- a/include/nano_gicp/impl/nano_gicp_impl.hpp +++ b/include/nano_gicp/impl/nano_gicp_impl.hpp @@ -148,13 +148,23 @@ void NanoGICP::setTargetCovariances(const std::vector< target_covs_ = covs; } +template +bool NanoGICP::calculateSourceCovariances() { + return calculate_covariances(input_, *source_kdtree_, source_covs_); +} + +template +bool NanoGICP::calculateTargetCovariances() { + return calculate_covariances(target_, *target_kdtree_, target_covs_); +} + template void NanoGICP::computeTransformation(PointCloudSource& output, const Matrix4& guess) { if (source_covs_.size() != input_->size()) { - calculate_covariances(input_, *source_kdtree_, source_covs_); + calculateSourceCovariances(); } if (target_covs_.size() != target_->size()) { - calculate_covariances(target_, *target_kdtree_, target_covs_); + calculateTargetCovariances(); } LsqRegistration::computeTransformation(output, guess); diff --git a/include/nano_gicp/nano_gicp.hpp b/include/nano_gicp/nano_gicp.hpp index 6dbc8ae..e12f860 100644 --- a/include/nano_gicp/nano_gicp.hpp +++ b/include/nano_gicp/nano_gicp.hpp @@ -94,6 +94,9 @@ class NanoGICP : public LsqRegistration { virtual void registerInputSource(const PointCloudSourceConstPtr& cloud); + virtual bool calculateSourceCovariances(); + virtual bool calculateTargetCovariances(); + const std::vector>& getSourceCovariances() const { return source_covs_; } diff --git a/package.xml b/package.xml index 02e984e..5658604 100644 --- a/package.xml +++ b/package.xml @@ -3,7 +3,7 @@ direct_lidar_odometry - 1.0.0 + 1.2.0 Direct LiDAR Odometry: Fast Localization with Dense Point Clouds Kenny J. Chen diff --git a/src/dlo/odom.cc b/src/dlo/odom.cc index e324c08..93482ff 100644 --- a/src/dlo/odom.cc +++ b/src/dlo/odom.cc @@ -27,7 +27,6 @@ dlo::OdomNode::OdomNode(ros::NodeHandle node_handle) : nh(node_handle) { this->dlo_initialized = false; this->imu_calibrated = false; - this->normals_initialized = false; this->icp_sub = this->nh.subscribe("pointcloud", 1, &dlo::OdomNode::icpCB, this); this->imu_sub = this->nh.subscribe("imu", 1, &dlo::OdomNode::imuCB, this); @@ -464,6 +463,7 @@ void dlo::OdomNode::initializeInputTarget() { this->target_cloud = pcl::PointCloud::Ptr (new pcl::PointCloud); this->target_cloud = this->current_scan; this->gicp_s2s.setInputTarget(this->target_cloud); + this->gicp_s2s.calculateTargetCovariances(); // initialize keyframes pcl::PointCloud::Ptr first_keyframe (new pcl::PointCloud); @@ -477,10 +477,14 @@ void dlo::OdomNode::initializeInputTarget() { // keep history of keyframes this->keyframes.push_back(std::make_pair(std::make_pair(this->pose, this->rotq), first_keyframe)); - *this->keyframes_cloud += *first_keyframe; *this->keyframe_cloud = *first_keyframe; + // compute kdtree and keyframe normals (use gicp_s2s input source as temporary storage because it will be overwritten by setInputSources()) + this->gicp_s2s.setInputSource(this->keyframe_cloud); + this->gicp_s2s.calculateSourceCovariances(); + this->keyframe_normals.push_back(this->gicp_s2s.getSourceCovariances()); + this->publish_keyframe_thread = std::thread( &dlo::OdomNode::publishKeyframe, this ); this->publish_keyframe_thread.detach(); @@ -787,12 +791,6 @@ void dlo::OdomNode::getNextPose() { this->gicp_s2s.align(*aligned); } - // if first iteration, store target covariance into keyframe normals - if (!this->normals_initialized) { - this->keyframe_normals.push_back(this->gicp_s2s.getTargetCovariances()); - this->normals_initialized = true; - } - // Get the local S2S transform Eigen::Matrix4f T_S2S = this->gicp_s2s.getFinalTransformation(); @@ -1153,7 +1151,9 @@ void dlo::OdomNode::updateKeyframes() { // update keyframe vector this->keyframes.push_back(std::make_pair(std::make_pair(this->pose, this->rotq), this->current_scan_t)); - // update keyframe normals + // compute kdtree and keyframe normals (use gicp_s2s input source as temporary storage because it will be overwritten by setInputSources()) + this->gicp_s2s.setInputSource(this->keyframe_cloud); + this->gicp_s2s.calculateSourceCovariances(); this->keyframe_normals.push_back(this->gicp_s2s.getSourceCovariances()); *this->keyframes_cloud += *this->current_scan_t; @@ -1225,10 +1225,6 @@ void dlo::OdomNode::pushSubmapIndices(std::vector dists, int k) { void dlo::OdomNode::getSubmapKeyframes() { - // reinitialize submap cloud and normals - this->submap_cloud = pcl::PointCloud::Ptr (new pcl::PointCloud); - this->submap_normals.clear(); - // clear vector of keyframe indices to use for submap this->submap_kf_idx_curr.clear(); @@ -1286,28 +1282,34 @@ void dlo::OdomNode::getSubmapKeyframes() { std::sort(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end()); auto last = std::unique(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end()); this->submap_kf_idx_curr.erase(last, this->submap_kf_idx_curr.end()); - for (auto k : this->submap_kf_idx_curr) { - - // create current submap cloud - *this->submap_cloud += *this->keyframes[k].second; - - // grab corresponding submap cloud's normals - this->submap_normals.insert( std::end(this->submap_normals), std::begin(this->keyframe_normals[k]), std::end(this->keyframe_normals[k]) ); - - } // sort current and previous submap kf list of indices std::sort(this->submap_kf_idx_curr.begin(), this->submap_kf_idx_curr.end()); std::sort(this->submap_kf_idx_prev.begin(), this->submap_kf_idx_prev.end()); // check if submap has changed from previous iteration - if(this->submap_kf_idx_curr == this->submap_kf_idx_prev){ + if (this->submap_kf_idx_curr == this->submap_kf_idx_prev){ this->submap_hasChanged = false; - } else{ + } else { this->submap_hasChanged = true; + + // reinitialize submap cloud, normals + pcl::PointCloud::Ptr submap_cloud_ (boost::make_shared>()); + this->submap_normals.clear(); + + for (auto k : this->submap_kf_idx_curr) { + + // create current submap cloud + *submap_cloud_ += *this->keyframes[k].second; + + // grab corresponding submap cloud's normals + this->submap_normals.insert( std::end(this->submap_normals), std::begin(this->keyframe_normals[k]), std::end(this->keyframe_normals[k]) ); + } + + this->submap_cloud = submap_cloud_; + this->submap_kf_idx_prev = this->submap_kf_idx_curr; } - this->submap_kf_idx_prev = this->submap_kf_idx_curr; }