From 682d8426c1b9b781be44a601a6a3ecbb52819588 Mon Sep 17 00:00:00 2001 From: feixyz10 Date: Sat, 30 Jan 2021 01:19:14 +0800 Subject: [PATCH] writing mapper ... --- common/geometry/pose3d.h | 7 +++ common/pcl/pcl_utils.h | 30 ++++++++++++ oh_my_loam/base/helper.cc | 8 +-- oh_my_loam/base/helper.h | 16 ------ oh_my_loam/configs/config.yaml | 1 + oh_my_loam/mapper/map.cc | 9 +++- oh_my_loam/mapper/map.h | 8 +-- oh_my_loam/mapper/mapper.cc | 90 +++++++++++++++++++++++++++++----- oh_my_loam/mapper/mapper.h | 60 ++++++++++++++++------- 9 files changed, 174 insertions(+), 55 deletions(-) diff --git a/common/geometry/pose3d.h b/common/geometry/pose3d.h index 8b9211a..b443da7 100644 --- a/common/geometry/pose3d.h +++ b/common/geometry/pose3d.h @@ -26,6 +26,13 @@ class Pose3d { t_vec_.setZero(); } + const Eigen::Matrix4d TransMat() const { + Eigen::Matrix4d trans_mat = Eigen::Matrix4d::Identity(); + trans_mat.topLeftCorner<3, 3>() = r_quat_.toRotationMatrix(); + trans_mat.topRightCorner<3, 1>() = t_vec_; + return trans_mat; + } + Pose3d Inv() const { Eigen::Quaterniond r_inv = r_quat_.inverse(); Eigen::Vector3d t_inv = r_inv * t_vec_; diff --git a/common/pcl/pcl_utils.h b/common/pcl/pcl_utils.h index bf9c437..fbba318 100644 --- a/common/pcl/pcl_utils.h +++ b/common/pcl/pcl_utils.h @@ -1,5 +1,10 @@ #pragma once +#include +#include +#include + +#include "common/geometry/pose3d.h" #include "common/log/log.h" #include "pcl_types.h" @@ -36,6 +41,31 @@ inline double IsFinite(const PT &pt) { return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z); } +template +inline void TransformPoint(const Pose3d &pose, const PT &pt_in, + PT *const pt_out) { + *pt_out = pt_in; + Eigen::Vector3d p = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z); + pt_out->x = p.x(); + pt_out->y = p.y(); + pt_out->z = p.z(); +} + +template +inline PT TransformPoint(const Pose3d &pose, const PT &pt_in) { + PT pt_out; + TransformPoint(pose, pt_in, &pt_out); + return pt_out; +} + +template +inline void TransformPointCloud(const Pose3d &pose, + const pcl::PointCloud &cloud_in, + pcl::PointCloud *const cloud_out) { + ACHECK(cloud_out); + pcl::transformPointCloud(cloud_in, *cloud_out, pose.TransMat().cast()); +} + // Remove point if the condition evaluated to true on it template void RemovePoints(const pcl::PointCloud &cloud_in, diff --git a/oh_my_loam/base/helper.cc b/oh_my_loam/base/helper.cc index 0bb6a7c..e4a3f03 100644 --- a/oh_my_loam/base/helper.cc +++ b/oh_my_loam/base/helper.cc @@ -1,11 +1,13 @@ -#include "helper.h" +#include "oh_my_loam/base/helper.h" + +#include "common/pcl/pcl_utils.h" namespace oh_my_loam { void TransformToStart(const Pose3d &pose, const TPoint &pt_in, TPoint *const pt_out) { Pose3d pose_interp = Pose3d().Interpolate(pose, GetTime(pt_in)); - TransformPoint(pose_interp, pt_in, pt_out); + common::TransformPoint(pose_interp, pt_in, pt_out); } TPoint TransformToStart(const Pose3d &pose, const TPoint &pt_in) { @@ -17,7 +19,7 @@ TPoint TransformToStart(const Pose3d &pose, const TPoint &pt_in) { void TransformToEnd(const Pose3d &pose, const TPoint &pt_in, TPoint *const pt_out) { TransformToStart(pose, pt_in, pt_out); - TransformPoint(pose.Inv(), *pt_out, pt_out); + common::TransformPoint(pose.Inv(), *pt_out, pt_out); } TPoint TransformToEnd(const Pose3d &pose, const TPoint &pt_in) { diff --git a/oh_my_loam/base/helper.h b/oh_my_loam/base/helper.h index beed983..f9def1b 100644 --- a/oh_my_loam/base/helper.h +++ b/oh_my_loam/base/helper.h @@ -15,22 +15,6 @@ inline float GetTime(const TPoint &pt) { return pt.time - GetScanId(pt); } -template -void TransformPoint(const Pose3d &pose, const PT &pt_in, PT *const pt_out) { - *pt_out = pt_in; - Eigen::Vector3d p = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z); - pt_out->x = p.x(); - pt_out->y = p.y(); - pt_out->z = p.z(); -} - -template -PT TransformPoint(const Pose3d &pose, const PT &pt_in) { - PT pt_out; - TransformPoint(pose, pt_in, &pt_out); - return pt_out; -} - /** * @brief Transform a lidar point to the start of the scan * diff --git a/oh_my_loam/configs/config.yaml b/oh_my_loam/configs/config.yaml index 393145f..b189366 100644 --- a/oh_my_loam/configs/config.yaml +++ b/oh_my_loam/configs/config.yaml @@ -35,3 +35,4 @@ mapper_config: verbose: false map_shape: [11, 21, 21] map_step_: 50 # meter + submap_shape: 5 diff --git a/oh_my_loam/mapper/map.cc b/oh_my_loam/mapper/map.cc index f73c305..1926337 100644 --- a/oh_my_loam/mapper/map.cc +++ b/oh_my_loam/mapper/map.cc @@ -122,11 +122,16 @@ TPointCloudPtr Map::GetAllPoints() const { return cloud_all; } -void Map::AddPoints(const TPointCloudConstPtr &cloud, IndexSet *const indices) { +void Map::AddPoints(const TPointCloudConstPtr &cloud, + std::vector *const indices) { + std::set index_set; for (const auto &point : *cloud) { Index index = GetIndex(point); this->at(index)->push_back(point); - if (indices) indices->insert(index); + if (indices) index_set.insert(index); + } + if (indices) { + for (const auto &index : index_set) indices->push_back(index); } } diff --git a/oh_my_loam/mapper/map.h b/oh_my_loam/mapper/map.h index 59b40db..0192b18 100644 --- a/oh_my_loam/mapper/map.h +++ b/oh_my_loam/mapper/map.h @@ -22,8 +22,6 @@ struct Index { }; }; -using IndexSet = std::set; - class Map { public: Map(const std::vector &shape, const std::vector &step); @@ -42,6 +40,10 @@ class Map { void ShiftX(int n); + const std::vector Shape() const { + return std::vector(shape_, shape_ + 3); + } + Index GetIndex(const TPoint &point) const; TPointCloudPtr GetSurrPoints(const TPoint &point, int n) const; @@ -49,7 +51,7 @@ class Map { TPointCloudPtr GetAllPoints() const; void AddPoints(const TPointCloudConstPtr &cloud, - IndexSet *const indices = nullptr); + std::vector *const indices = nullptr); void Downsample(double voxel_size); diff --git a/oh_my_loam/mapper/mapper.cc b/oh_my_loam/mapper/mapper.cc index f083188..4e9addd 100644 --- a/oh_my_loam/mapper/mapper.cc +++ b/oh_my_loam/mapper/mapper.cc @@ -2,6 +2,13 @@ #include +#include "common/log/log.h" +#include "common/pcl/pcl_utils.h" +#include "oh_my_loam/base/helper.h" +#include "oh_my_loam/base/types.h" +#include "oh_my_loam/mapper/map.h" +#include "oh_my_loam/solver/solver.h" + namespace oh_my_loam { namespace { @@ -15,8 +22,8 @@ bool Mapper::Init() { verbose_ = config_["vis"].as(); AINFO << "Mapping visualizer: " << (is_vis_ ? "ON" : "OFF"); std::vector shape = YAMLConfig::GetSeq(config_["map_shape"]); - cloud_corn_map_.reset(new Map(shape, config_["map_step"].as())); - cloud_surf_map_.reset(new Map(shape, config_["map_step"].as())); + corn_map_.reset(new Map(shape, config_["map_step"].as())); + surf_map_.reset(new Map(shape, config_["map_step"].as())); return true; } @@ -24,24 +31,83 @@ void Mapper::Reset() {} void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn, const TPointCloudConstPtr &cloud_surf, - common::Pose3d *const pose_out) { + const common::Pose3d &pose_curr2odom, + common::Pose3d *const pose_curr2map) { if (GetState() == UN_INIT) { - cloud_corn_map_->AddPoints(cloud_corn); - cloud_surf_map_->AddPoints(cloud_surf); - pose_out->SetIdentity(); + corn_map_->AddPoints(cloud_corn); + surf_map_->AddPoints(cloud_surf); + pose_curr2map->SetIdentity(); + pose_odom2map_.SetIdentity(); SetState(DONE); return; } if (GetState() == DONE) { - } else { // RUNNING + thread_.reset(new std::thread(&Mapper::Run, this, cloud_corn, cloud_surf, + pose_curr2odom)); + thread_->detach(); } + std::lock_guard lock(state_mutex_); + *pose_curr2map = pose_odom2map_ * pose_curr2odom; } -void Mapper::Run(double timestamp, const TPointCloudConstPtr &cloud_corn, - const TPointCloudConstPtr &cloud_surf) { - TimePose pose; - pose.timestamp = timestamp; - std::lock_guard lock(mutex_); +void Mapper::Run(const TPointCloudConstPtr &cloud_corn_in, + const TPointCloudConstPtr &cloud_surf_in, + const Pose3d &pose_curr2odom) { + SetState(RUNNING); + Pose3d pose_curr2map = pose_odom2map_ * pose_curr2odom; + TPoint cnt(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(), + pose_curr2map.t_vec().z()); + AdjustMap(corn_map_->GetIndex(cnt)); + int submap_shape = config_["submap_shape"].as(); + TPointCloudPtr cloud_corn(new TPointCloud); + TPointCloudPtr cloud_surf(new TPointCloud); + TransformPointCloud(pose_curr2map, *cloud_corn_in, cloud_corn.get()); + TransformPointCloud(pose_curr2map, *cloud_surf_in, cloud_surf.get()); + TPointCloudPtr cloud_corn_map = + corn_map_->GetSurrPoints(cnt, submap_shape / 2); + TPointCloudPtr cloud_surf_map = + corn_map_->GetSurrPoints(cnt, submap_shape / 2); + // MatchCorn(); + // MatchSurf(); + PoseSolver solver(pose_curr2map); + if (!solver.Solve(5, false, &pose_curr2map)) { + AWARN << "Solve: no_convergence"; + } + std::lock_guard lock(state_mutex_); + pose_odom2map_ = pose_curr2map * pose_curr2odom.Inv(); + SetState(DONE); +} + +void Mapper::AdjustMap(const Index &index) { + std::vector map_shape = corn_map_->Shape(); + int min_idx = config_["submap_shape"].as() / 2 + 1; + int max_idx_z = map_shape[0] - min_idx - 1; + if (index.k < min_idx) { + corn_map_->ShiftZ(index.k - min_idx); + surf_map_->ShiftZ(index.k - min_idx); + } + if (index.k > max_idx_z) { + corn_map_->ShiftZ(index.k - max_idx_z); + surf_map_->ShiftZ(index.k - max_idx_z); + } + int max_idx_y = map_shape[1] - min_idx - 1; + if (index.j < min_idx) { + corn_map_->ShiftY(index.j - min_idx); + surf_map_->ShiftY(index.j - min_idx); + } + if (index.j > max_idx_y) { + corn_map_->ShiftY(index.j - max_idx_y); + surf_map_->ShiftY(index.j - max_idx_y); + } + int max_idx_x = map_shape[2] - min_idx - 1; + if (index.i < min_idx) { + corn_map_->ShiftX(index.i - min_idx); + surf_map_->ShiftX(index.i - min_idx); + } + if (index.i > max_idx_x) { + corn_map_->ShiftX(index.i - max_idx_x); + surf_map_->ShiftX(index.i - max_idx_x); + } } void Mapper::Visualize() {} diff --git a/oh_my_loam/mapper/mapper.h b/oh_my_loam/mapper/mapper.h index aaf2b71..165a314 100644 --- a/oh_my_loam/mapper/mapper.h +++ b/oh_my_loam/mapper/mapper.h @@ -3,9 +3,12 @@ #include #include +#include #include #include "common/geometry/pose3d.h" +#include "oh_my_loam/base/helper.h" +#include "oh_my_loam/base/types.h" #include "oh_my_loam/mapper/map.h" namespace oh_my_loam { @@ -18,48 +21,67 @@ class Mapper { void Process(double timestamp, const TPointCloudConstPtr &cloud_corn, const TPointCloudConstPtr &cloud_surf, - common::Pose3d *const pose_out); + const common::Pose3d &pose_curr2odom, + common::Pose3d *const pose_curr2map); + + TPointCloudPtr GetCornMapPoints() const { + std::shared_lock lock(corn_map_mutex_); + return corn_map_->GetAllPoints(); + } + + TPointCloudPtr GetSurfMapPoints() const { + std::shared_lock lock(surf_map_mutex_); + return surf_map_->GetAllPoints(); + } + + TPointCloudPtr GetMapPoints() const { + TPointCloudPtr map_points(new TPointCloud); + *map_points += *GetCornMapPoints(); + *map_points += *GetSurfMapPoints(); + return map_points; + } void Reset(); private: enum State { DONE, RUNNING, UN_INIT }; - void Run(double timestamp, const TPointCloudConstPtr &cloud_corn, - const TPointCloudConstPtr &cloud_surf); + void Run(const TPointCloudConstPtr &cloud_corn, + const TPointCloudConstPtr &cloud_surf, const Pose3d &pose_init); - void TransformUpdate(); + void MatchCorn(const TPointCloudConstPtr &src, const TPointCloudConstPtr &tgt, + std::vector *const pair) const; - void MapUpdate(); + void MatchSurf(const TPointCloudConstPtr &src, const TPointCloudConstPtr &tgt, + std::vector *const pair) const; - State GetState() { - std::lock_guard lock(mutex_); + State GetState() const { + std::lock_guard lock(state_mutex_); return state_; } void SetState(State state) { - std::lock_guard lock(mutex_); + std::lock_guard lock(state_mutex_); state_ = state; } + void AdjustMap(const Index &index); + void Visualize(); YAML::Node config_; - struct TimePose { - double timestamp; - common::Pose3d pose; - }; - - std::unique_ptr cloud_corn_map_; - std::unique_ptr cloud_surf_map_; - - std::mutex mutex_; - std::vector poses_; - std::unique_ptr thread_{nullptr}; + mutable std::shared_mutex corn_map_mutex_; + mutable std::shared_mutex surf_map_mutex_; + std::unique_ptr corn_map_; + std::unique_ptr surf_map_; + mutable std::mutex state_mutex_; + Pose3d pose_odom2map_; State state_ = UN_INIT; + mutable std::unique_ptr thread_{nullptr}; + bool is_vis_ = false; bool verbose_ = false;