From 4bf3ac5aa54bb6bd9dee2ee6945a4d94cf805660 Mon Sep 17 00:00:00 2001 From: feixyz10 Date: Tue, 26 Jan 2021 00:32:14 +0800 Subject: [PATCH] tart writing mapper --- common/config/yaml_config.h | 1 + oh_my_loam/configs/config.yaml | 10 ++++++---- oh_my_loam/extractor/extractor.cc | 15 +++++++-------- oh_my_loam/extractor/extractor.h | 4 +++- oh_my_loam/mapper/mapper.cc | 3 ++- oh_my_loam/mapper/mapper.h | 21 ++++++++++++++++----- oh_my_loam/odometer/odometer.cc | 4 ++++ oh_my_loam/odometer/odometer.h | 2 ++ oh_my_loam/oh_my_loam.cc | 30 ++++++++++++++++++++++++------ oh_my_loam/oh_my_loam.h | 16 +++++++++++++--- oh_my_loam/solver/cost_function.h | 15 --------------- 11 files changed, 78 insertions(+), 43 deletions(-) diff --git a/common/config/yaml_config.h b/common/config/yaml_config.h index a2378bb..e2eea25 100644 --- a/common/config/yaml_config.h +++ b/common/config/yaml_config.h @@ -29,6 +29,7 @@ class YAMLConfig { private: std::unique_ptr config_{nullptr}; + DECLARE_SINGLETON(YAMLConfig); }; diff --git a/oh_my_loam/configs/config.yaml b/oh_my_loam/configs/config.yaml index 57f0f6d..9e3a6ed 100644 --- a/oh_my_loam/configs/config.yaml +++ b/oh_my_loam/configs/config.yaml @@ -9,10 +9,10 @@ extractor_config: vis: false verbose: false min_point_num: 66 - scan_seg_num: 36 + scan_seg_num: 12 sharp_corner_point_num: 1 - corner_point_num: 4 - flat_surf_point_num: 1 + corner_point_num: 10 + flat_surf_point_num: 2 corner_point_curvature_th: 0.5 surf_point_curvature_th: 0.5 neighbor_point_dist_sq_th: 0.1 @@ -26,9 +26,11 @@ odometer_config: min_correspondence_num: 10 icp_iter_num: 2 solve_iter_num: 4 - corn_match_dist_sq_th: 1.0 + corn_match_dist_sq_th: 16.0 surf_match_dist_sq_th: 16.0 # configs for mapper mapper_config: vis: false + verbose: false + process_period: 0.5 diff --git a/oh_my_loam/extractor/extractor.cc b/oh_my_loam/extractor/extractor.cc index 4649548..bb1e4b2 100644 --- a/oh_my_loam/extractor/extractor.cc +++ b/oh_my_loam/extractor/extractor.cc @@ -107,8 +107,6 @@ void Extractor::AssignType(TCTPointCloud *const scan) const { int seg_num = config_["scan_seg_num"].as(); if (pt_num < seg_num) return; int seg_pt_num = (pt_num - 1) / seg_num + 1; - AERROR << pt_num << " " << seg_num << " " << seg_pt_num; - std::vector picked(pt_num, false); std::vector indices = common::Range(pt_num); int sharp_corner_point_num = config_["sharp_corner_point_num"].as(); @@ -121,6 +119,7 @@ void Extractor::AssignType(TCTPointCloud *const scan) const { for (int seg = 0; seg < seg_num; ++seg) { int b = seg * seg_pt_num; int e = std::min((seg + 1) * seg_pt_num, pt_num); + if (b >= e) break; // sort by curvature for each segment: large -> small std::sort(indices.begin() + b, indices.begin() + e, [&](int i, int j) { return scan->at(i).curvature > scan->at(j).curvature; @@ -128,7 +127,7 @@ void Extractor::AssignType(TCTPointCloud *const scan) const { // pick corner points int corner_point_picked_num = 0; for (int i = b; i < e; ++i) { - size_t ix = indices[i]; + int ix = indices[i]; if (!picked.at(ix) && scan->at(ix).curvature > corner_point_curvature_th) { ++corner_point_picked_num; @@ -146,7 +145,7 @@ void Extractor::AssignType(TCTPointCloud *const scan) const { // pick surface points int surf_point_picked_num = 0; for (int i = e - 1; i >= b; --i) { - size_t ix = indices[i]; + int ix = indices[i]; if (!picked.at(ix) && scan->at(ix).curvature < surf_point_curvature_th) { ++surf_point_picked_num; if (surf_point_picked_num <= flat_surf_point_num) { @@ -200,21 +199,21 @@ void Extractor::Visualize(const common::PointCloudConstPtr &cloud, visualizer_->Render(frame); } -void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix, +void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, int ix, std::vector *const picked) const { auto dist_sq = [&](size_t i, size_t j) -> double { return common::DistanceSquare(scan[i], scan[j]); }; double neighbor_point_dist_sq_th = config_["neighbor_point_dist_sq_th"].as(); - for (size_t i = 1; i <= 5; ++i) { + for (int i = 1; i <= 5; ++i) { if (ix - i < 0) break; if (picked->at(ix - i)) continue; if (dist_sq(ix - i, ix - i + 1) > neighbor_point_dist_sq_th) break; picked->at(ix - i) = true; } - for (size_t i = 1; i <= 5; ++i) { - if (ix + i >= scan.size()) break; + for (int i = 1; i <= 5; ++i) { + if (static_cast(ix + i) >= scan.size()) break; if (picked->at(ix + i)) continue; if (dist_sq(ix + i, ix + i - 1) > neighbor_point_dist_sq_th) break; picked->at(ix + i) = true; diff --git a/oh_my_loam/extractor/extractor.h b/oh_my_loam/extractor/extractor.h index 0c9e901..2ce098a 100644 --- a/oh_my_loam/extractor/extractor.h +++ b/oh_my_loam/extractor/extractor.h @@ -22,6 +22,8 @@ class Extractor { return num_scans_; } + virtual void Reset() {} + protected: virtual int GetScanID(const common::Point &pt) const = 0; @@ -48,7 +50,7 @@ class Extractor { bool verbose_ = false; private: - void UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix, + void UpdateNeighborsPicked(const TCTPointCloud &scan, int ix, std::vector *const picked) const; bool is_vis_ = false; diff --git a/oh_my_loam/mapper/mapper.cc b/oh_my_loam/mapper/mapper.cc index 9b9f2b4..4fab450 100644 --- a/oh_my_loam/mapper/mapper.cc +++ b/oh_my_loam/mapper/mapper.cc @@ -15,7 +15,8 @@ bool Mapper::Init() { return true; } -void Mapper::Process() {} +void Mapper::Process(double timestamp, const std::vector &features, + Pose3d *const pose_out) {} void Mapper::Visualize() {} diff --git a/oh_my_loam/mapper/mapper.h b/oh_my_loam/mapper/mapper.h index e349640..3261002 100644 --- a/oh_my_loam/mapper/mapper.h +++ b/oh_my_loam/mapper/mapper.h @@ -1,7 +1,10 @@ #pragma once +#include + #include "common/common.h" -#include "common/geometry/pose.h" +#include "common/geometry/pose3d.h" +#include "oh_my_loam/base/feature.h" #include "oh_my_loam/base/types.h" namespace oh_my_loam { @@ -12,22 +15,30 @@ class Mapper { bool Init(); - void Process(); + void Process(double timestamp, const std::vector &features, + common::Pose3d *const pose_out); + + common::PointCloudConstPtr map() const { + return cloud_map_; + } + + void Reset(); private: void Visualize(); - TPointCloudPtr map_pts_; + common::PointCloudPtr cloud_map_; + common::Pose3d pose_curr2world_; + YAML::Node config_; + bool is_initialized = false; bool is_vis_ = false; bool verbose_ = false; - YAML::Node config_; - DISALLOW_COPY_AND_ASSIGN(Mapper) }; diff --git a/oh_my_loam/odometer/odometer.cc b/oh_my_loam/odometer/odometer.cc index afd2f81..40798c8 100644 --- a/oh_my_loam/odometer/odometer.cc +++ b/oh_my_loam/odometer/odometer.cc @@ -20,6 +20,10 @@ bool Odometer::Init() { return true; } +void Odometer::Reset() { + is_initialized_ = false; +} + void Odometer::Process(double timestamp, const std::vector &features, Pose3d *const pose_out) { if (!is_initialized_) { diff --git a/oh_my_loam/odometer/odometer.h b/oh_my_loam/odometer/odometer.h index cd9ae3d..8c02769 100644 --- a/oh_my_loam/odometer/odometer.h +++ b/oh_my_loam/odometer/odometer.h @@ -18,6 +18,8 @@ class Odometer { void Process(double timestamp, const std::vector &features, common::Pose3d *const pose_out); + void Reset(); + protected: void UpdatePre(const std::vector &features); diff --git a/oh_my_loam/oh_my_loam.cc b/oh_my_loam/oh_my_loam.cc index eca1a34..73b4503 100644 --- a/oh_my_loam/oh_my_loam.cc +++ b/oh_my_loam/oh_my_loam.cc @@ -1,6 +1,9 @@ #include "oh_my_loam.h" +#include + #include "common/pcl/pcl_utils.h" +#include "oh_my_loam/base/helper.h" #include "oh_my_loam/extractor/extractor_VLP16.h" namespace oh_my_loam { @@ -10,7 +13,7 @@ const double kPointMinDist = 0.1; } // namespace bool OhMyLoam::Init() { - YAML::Node config = common::YAMLConfig::Instance()->config(); + config_ = common::YAMLConfig::Instance()->config(); extractor_.reset(new ExtractorVLP16); if (!extractor_->Init()) { AERROR << "Failed to initialize extractor"; @@ -21,14 +24,22 @@ bool OhMyLoam::Init() { AERROR << "Failed to initialize odometer"; return false; } - // mapper_.reset(new Mapper); - // if (!mapper_->Init()) { - // AERROR << "Failed to initialize mapper"; - // return false; - // } + mapper_.reset(new Mapper); + if (!mapper_->Init()) { + AERROR << "Failed to initialize mapper"; + return false; + } return true; } +void OhMyLoam::Reset() { + timestamp_last_ = timestamp_last_mapping_ = 0.0; + extractor_->Reset(); + odometer_->Reset(); + mapper_->Reset(); + std::vector().swap(poses_); +} + void OhMyLoam::Run(double timestamp, const common::PointCloudConstPtr &cloud_in) { common::PointCloudPtr cloud(new common::PointCloud); @@ -38,6 +49,8 @@ void OhMyLoam::Run(double timestamp, Pose3d pose; odometer_->Process(timestamp, features, &pose); poses_.emplace_back(pose); + if (!IsMapping(timestamp)) return; + mapper_->Process(); } void OhMyLoam::RemoveOutliers(const common::PointCloud &cloud_in, @@ -49,4 +62,9 @@ void OhMyLoam::RemoveOutliers(const common::PointCloud &cloud_in, }); } +bool OhMyLoam::IsMapping(double timestamp) const { + return std::abs(timestamp - timestamp_last_mapping_) >= + config_["mapper_config"]["process_period"].as(); +} + } // namespace oh_my_loam \ No newline at end of file diff --git a/oh_my_loam/oh_my_loam.h b/oh_my_loam/oh_my_loam.h index 187441d..56001d0 100644 --- a/oh_my_loam/oh_my_loam.h +++ b/oh_my_loam/oh_my_loam.h @@ -1,8 +1,10 @@ #pragma once +#include + #include "common/common.h" #include "oh_my_loam/extractor/extractor.h" -// #include "oh_my_loam/mapper/mapper.h" +#include "oh_my_loam/mapper/mapper.h" #include "oh_my_loam/odometer/odometer.h" namespace oh_my_loam { @@ -16,18 +18,26 @@ class OhMyLoam { void Run(double timestamp, const common::PointCloudConstPtr &cloud_in); private: + void Reset(); + + bool IsMapping(double timestamp) const; + std::unique_ptr extractor_{nullptr}; std::unique_ptr odometer_{nullptr}; - // std::unique_ptr mapper_{nullptr}; + std::unique_ptr mapper_{nullptr}; - // remove outliers: nan and very close points + // remove outliers: nan or very close points void RemoveOutliers(const common::PointCloud &cloud_in, common::PointCloud *const cloud_out) const; std::vector poses_; + YAML::Node config_; + + double timestamp_last_ = 0.0, timestamp_last_mapping_ = 0.0; + DISALLOW_COPY_AND_ASSIGN(OhMyLoam) }; diff --git a/oh_my_loam/solver/cost_function.h b/oh_my_loam/solver/cost_function.h index 8e371fa..911971a 100644 --- a/oh_my_loam/solver/cost_function.h +++ b/oh_my_loam/solver/cost_function.h @@ -59,22 +59,10 @@ bool PointLineCostFunction::operator()(const T *const r_quat, Eigen::Matrix p(T(pt.x), T(pt.y), T(pt.z)); Eigen::Matrix p1(T(pt1.x), T(pt1.y), T(pt1.z)); Eigen::Matrix p2(T(pt2.x), T(pt2.y), T(pt2.z)); - // if constexpr (!std::is_arithmetic::value) { - // AERROR << p.x().a << ", " << p.y().a << ", " << p.z().a; - // AERROR << p1.x().a << ", " << p1.y().a << ", " << p1.z().a; - // AERROR << p2.x().a << ", " << p2.y().a << ", " << p2.z().a; - // } Eigen::Quaternion r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]); Eigen::Quaternion r_interp = Eigen::Quaternion::Identity().slerp(T(time_), r); - // if constexpr (!std::is_arithmetic::value) { - // AERROR << time_; - // AERROR << r.w().a << " " << r.x().a << ", " << r.y().a << ", " << - // r.z().a; AERROR << r_interp.w().a << " " << r_interp.x().a << ", " << - // r_interp.y().a - // << ", " << r_interp.z().a; - // } Eigen::Matrix t(T(time_) * t_vec[0], T(time_) * t_vec[1], T(time_) * t_vec[2]); Eigen::Matrix p0 = r_interp * p + t; @@ -85,9 +73,6 @@ bool PointLineCostFunction::operator()(const T *const r_quat, residual[0] = area[0] / base_length; residual[1] = area[1] / base_length; residual[2] = area[2] / base_length; - // if constexpr (!std::is_arithmetic::value) { - // AERROR << base_length.a; - // } return true; }