diff --git a/common/math/math_utils.cc b/common/math/math_utils.cc index adcaed7..b13b06b 100644 --- a/common/math/math_utils.cc +++ b/common/math/math_utils.cc @@ -9,6 +9,10 @@ double NormalizeAngle(double ang) { return ang - two_pi * std::floor((ang + M_PI) / two_pi); } +double Degree2Rad(double degree) { return degree * M_PI / 180.0; } + +double Rad2Degree(double rad) { return rad * 180.0 / M_PI; } + const std::vector Range(int begin, int end, int step) { ACHECK(step != 0) << "Step must non-zero"; int num = (end - begin) / step; diff --git a/common/pcl/pcl_utils.h b/common/pcl/pcl_utils.h index ea57b2c..9a4a46c 100644 --- a/common/pcl/pcl_utils.h +++ b/common/pcl/pcl_utils.h @@ -40,7 +40,8 @@ inline double IsFinite(const PointType& pt) { template void RemovePoints(const pcl::PointCloud& cloud_in, pcl::PointCloud* const cloud_out, - std::function check) { + std::function check, + std::vector* const removed_indices = nullptr) { if (&cloud_in != cloud_out) { cloud_out->header = cloud_in.header; cloud_out->resize(cloud_in.size()); @@ -48,7 +49,10 @@ void RemovePoints(const pcl::PointCloud& cloud_in, size_t j = 0; for (size_t i = 0; i < cloud_in.size(); ++i) { const auto pt = cloud_in.points[i]; - if (check(pt)) continue; + if (check(pt)) { + if (removed_indices) removed_indices->push_back(i); + continue; + } cloud_out->points[j++] = pt; } diff --git a/common/visualizer/lidar_visualizer.h b/common/visualizer/lidar_visualizer.h index de722d8..0edefdc 100644 --- a/common/visualizer/lidar_visualizer.h +++ b/common/visualizer/lidar_visualizer.h @@ -12,7 +12,7 @@ namespace common { struct LidarVisFrame { double timestamp = 0.0; - PointCloudPtr cloud = nullptr; + PointCloudConstPtr cloud = nullptr; }; class LidarVisualizer { diff --git a/main.cc b/main.cc index 62dec94..6caa7a8 100644 --- a/main.cc +++ b/main.cc @@ -16,10 +16,8 @@ void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg, OhMyLoam* const slam); int main(int argc, char* argv[]) { - auto curr_path = std::filesystem::current_path(); // config - YAMLConfig::Instance()->Init( - (curr_path / "oh_my_loam/configs/config.yaml").string()); + YAMLConfig::Instance()->Init(argv[1]); bool log_to_file = YAMLConfig::Instance()->Get("log_to_file"); std::string log_path = YAMLConfig::Instance()->Get("log_path"); std::string lidar = YAMLConfig::Instance()->Get("lidar"); diff --git a/oh_my_loam/configs/config.yaml b/oh_my_loam/configs/config.yaml index a559174..89e2cfa 100644 --- a/oh_my_loam/configs/config.yaml +++ b/oh_my_loam/configs/config.yaml @@ -6,24 +6,24 @@ vis: true # configs for extractor extractor_config: - vis: false + vis: true verbose: true min_point_num: 66 sharp_corner_point_num: 2 corner_point_num: 20 flat_surf_point_num: 4 - surf_point_num: 3 ## useless currently - corner_point_curvature_thres: 0.5 - surf_point_curvature_thres: 0.5 - neighbor_point_dist_thres: 0.05 + surf_point_num: 20 ## useless currently + corner_point_curvature_th: 0.5 + surf_point_curvature_th: 0.5 + neighbor_point_dist_th: 0.1 downsample_voxel_size: 0.3 # configs for odometer odometer_config: - vis: true + vis: false verbose: false icp_iter_num : 2 - match_dist_sq_thresh: 1 + match_dist_sq_th: 1 # configs for mapper mapper_config: diff --git a/oh_my_loam/extractor/extractor.cc b/oh_my_loam/extractor/extractor.cc index 18e235f..6a871cb 100644 --- a/oh_my_loam/extractor/extractor.cc +++ b/oh_my_loam/extractor/extractor.cc @@ -22,7 +22,7 @@ bool Extractor::Init() { return true; } -void Extractor::Process(const PointCloudConstPtr& cloud, +void Extractor::Process(double timestamp, const PointCloudConstPtr& cloud, Feature* const feature) { BLOCK_TIMER_START; if (cloud->size() < config_["min_point_num"].as()) { @@ -34,23 +34,22 @@ void Extractor::Process(const PointCloudConstPtr& cloud, std::vector scans; SplitScan(*cloud, &scans); AINFO_IF(verbose_) << "Extractor::SplitScan: " << BLOCK_TIMER_STOP_FMT; - // compute curvature for each point in each scan + // compute curvature to each point for (auto& scan : scans) { ComputeCurvature(&scan); } AINFO_IF(verbose_) << "Extractor::ComputeCurvature: " << BLOCK_TIMER_STOP_FMT; - // assign type to each point in each scan: FLAT, LESS_FLAT, - // NORMAL, LESS_SHARP or SHARP + // assign type to each point: FLAT, LESS_FLAT, NORMAL, LESS_SHARP or SHARP for (auto& scan : scans) { AssignType(&scan); } AINFO_IF(verbose_) << "Extractor::AssignType: " << BLOCK_TIMER_STOP_FMT; - // store points into feature point clouds according to their type + // store points into feature point clouds based on their type for (const auto& scan : scans) { GenerateFeature(scan, feature); } AINFO << "Extractor::Process: " << BLOCK_TIMER_STOP_FMT; - if (is_vis_) Visualize(*cloud, *feature); + if (is_vis_) Visualize(cloud, *feature, timestamp); } void Extractor::SplitScan(const PointCloud& cloud, @@ -58,7 +57,7 @@ void Extractor::SplitScan(const PointCloud& cloud, scans->resize(num_scans_); double yaw_start = -atan2(cloud.points[0].y, cloud.points[0].x); bool half_passed = false; - for (const auto& pt : cloud.points) { + for (const auto& pt : cloud) { int scan_id = GetScanID(pt); if (scan_id >= num_scans_ || scan_id < 0) continue; double yaw = -atan2(pt.y, pt.x); @@ -69,13 +68,14 @@ void Extractor::SplitScan(const PointCloud& cloud, half_passed = true; yaw_start += kTwoPi; } - (*scans)[scan_id].points.emplace_back( - pt.x, pt.y, pt.z, yaw_diff / kTwoPi + scan_id, std::nanf("")); + (*scans)[scan_id].push_back( + {pt.x, pt.y, pt.z, static_cast(yaw_diff / kTwoPi + scan_id), + std::nanf("")}); } } void Extractor::ComputeCurvature(TCTPointCloud* const scan) const { - if (scan->size() < 20) return; + if (scan->size() <= 10 + kScanSegNum) return; auto& pts = scan->points; for (size_t i = 5; i < pts.size() - 5; ++i) { float dx = pts[i - 5].x + pts[i - 4].x + pts[i - 3].x + pts[i - 2].x + @@ -87,7 +87,7 @@ void Extractor::ComputeCurvature(TCTPointCloud* const scan) const { float dz = pts[i - 5].z + pts[i - 4].z + pts[i - 3].z + pts[i - 2].z + pts[i - 1].z + pts[i + 1].z + pts[i + 2].z + pts[i + 3].z + pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z; - pts[i].curvature = std::sqrt(dx * dx + dy * dy + dz * dz); + pts[i].curvature = std::hypot(dx, dy, dz); } RemovePoints(*scan, scan, [](const TCTPoint& pt) { return !std::isfinite(pt.curvature); @@ -96,7 +96,7 @@ void Extractor::ComputeCurvature(TCTPointCloud* const scan) const { void Extractor::AssignType(TCTPointCloud* const scan) const { int pt_num = scan->size(); - ACHECK(pt_num >= kScanSegNum); + if (pt_num < kScanSegNum) return; int seg_pt_num = (pt_num - 1) / kScanSegNum + 1; std::vector picked(pt_num, false); std::vector indices = Range(pt_num); @@ -104,51 +104,50 @@ void Extractor::AssignType(TCTPointCloud* const scan) const { int corner_point_num = config_["corner_point_num"].as(); int flat_surf_point_num = config_["flat_surf_point_num"].as(); int surf_point_num = config_["surf_point_num"].as(); - float corner_point_curvature_thres = - config_["corner_point_curvature_thres"].as(); - float surf_point_curvature_thres = - config_["surf_point_curvature_thres"].as(); + float corner_point_curvature_th = + config_["corner_point_curvature_th"].as(); + float surf_point_curvature_th = + config_["surf_point_curvature_th"].as(); for (int seg = 0; seg < kScanSegNum; ++seg) { - int s = seg * seg_pt_num; + int b = seg * seg_pt_num; int e = std::min((seg + 1) * seg_pt_num, pt_num); // sort by curvature for each segment: large -> small - std::sort(indices.begin() + s, indices.begin() + e, [&](int i, int j) { + std::sort(indices.begin() + b, indices.begin() + e, [&](int i, int j) { return scan->at(i).curvature > scan->at(j).curvature; }); // pick corner points - int corner_pt_picked_num = 0; - for (int i = s; i < e; ++i) { + int corner_point_picked_num = 0; + for (int i = b; i < e; ++i) { size_t ix = indices[i]; if (!picked.at(ix) && - scan->at(ix).curvature > corner_point_curvature_thres) { - ++corner_pt_picked_num; - if (corner_pt_picked_num <= sharp_corner_point_num) { + scan->at(ix).curvature > corner_point_curvature_th) { + ++corner_point_picked_num; + if (corner_point_picked_num <= sharp_corner_point_num) { scan->at(ix).type = Type::SHARP; - } else if (corner_pt_picked_num <= corner_point_num) { + } else if (corner_point_picked_num <= corner_point_num) { scan->at(ix).type = Type::LESS_SHARP; } else { break; } picked.at(ix) = true; - SetNeighborsPicked(*scan, ix, &picked); + UpdateNeighborsPicked(*scan, ix, &picked); } } // pick surface points - int surf_pt_picked_num = 0; - for (int i = e - 1; i >= s; --i) { + int surf_point_picked_num = 0; + for (int i = e - 1; i >= b; --i) { size_t ix = indices[i]; - if (!picked.at(ix) && - scan->at(ix).curvature < surf_point_curvature_thres) { - ++surf_pt_picked_num; - if (surf_pt_picked_num <= flat_surf_point_num) { + 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) { scan->at(ix).type = Type::FLAT; - } else if (surf_pt_picked_num <= surf_point_num) { + } else if (surf_point_picked_num <= surf_point_num) { scan->at(ix).type = Type::LESS_FLAT; } else { break; } picked.at(ix) = true; - SetNeighborsPicked(*scan, ix, &picked); + UpdateNeighborsPicked(*scan, ix, &picked); } } } @@ -184,35 +183,31 @@ void Extractor::GenerateFeature(const TCTPointCloud& scan, feature->cloud_less_flat_surf = dowm_sampled; } -void Extractor::Visualize(const PointCloud& cloud, const Feature& feature, - double timestamp) { +void Extractor::Visualize(const PointCloudConstPtr& cloud, + const Feature& feature, double timestamp) { std::shared_ptr frame(new ExtractorVisFrame); frame->timestamp = timestamp; - frame->cloud = cloud.makeShared(); + frame->cloud = cloud; frame->feature = feature; visualizer_->Render(frame); } -void Extractor::SetNeighborsPicked(const TCTPointCloud& scan, size_t ix, - std::vector* const picked) const { +void Extractor::UpdateNeighborsPicked(const TCTPointCloud& scan, size_t ix, + std::vector* const picked) const { auto DistSqure = [&](int i, int j) -> float { - float dx = scan.at(i).x - scan.at(j).x; - float dy = scan.at(i).y - scan.at(j).y; - float dz = scan.at(i).z - scan.at(j).z; - return dx * dx + dy * dy + dz * dz; + return DistanceSqure(scan[i], scan[j]); }; - float neighbor_point_dist_thres = - config_["neighbor_point_dist_thres"].as(); + float neighbor_point_dist_th = config_["neighbor_point_dist_th"].as(); for (size_t i = 1; i <= 5; ++i) { if (ix < i) break; if (picked->at(ix - i)) continue; - if (DistSqure(ix - i, ix - i + 1) > neighbor_point_dist_thres) break; + if (DistSqure(ix - i, ix - i + 1) > neighbor_point_dist_th) break; picked->at(ix - i) = true; } for (size_t i = 1; i <= 5; ++i) { if (ix + i >= scan.size()) break; if (picked->at(ix + i)) continue; - if (DistSqure(ix + i, ix + i - 1) > neighbor_point_dist_thres) break; + if (DistSqure(ix + i, ix + i - 1) > neighbor_point_dist_th) break; picked->at(ix + i) = true; } } diff --git a/oh_my_loam/extractor/extractor.h b/oh_my_loam/extractor/extractor.h index ec8d67b..16dafac 100644 --- a/oh_my_loam/extractor/extractor.h +++ b/oh_my_loam/extractor/extractor.h @@ -15,15 +15,14 @@ class Extractor { bool Init(); - void Process(const common::PointCloudConstPtr& cloud, Feature* const feature); + void Process(double timestamp, const common::PointCloudConstPtr& cloud, + Feature* const feature); int num_scans() const { return num_scans_; } protected: virtual int GetScanID(const common::Point& pt) const = 0; - YAML::Node config_; - virtual void SplitScan(const common::PointCloud& cloud, std::vector* const scans) const; @@ -34,18 +33,20 @@ class Extractor { virtual void GenerateFeature(const TCTPointCloud& scan, Feature* const feature) const; + virtual void Visualize(const common::PointCloudConstPtr& cloud, + const Feature& feature, double timestamp = 0.0); + int num_scans_ = 0; + YAML::Node config_; + std::unique_ptr visualizer_{nullptr}; bool verbose_ = false; private: - void Visualize(const common::PointCloud& cloud, const Feature& feature, - double timestamp = 0.0); - - void SetNeighborsPicked(const TCTPointCloud& scan, size_t ix, - std::vector* const picked) const; + void UpdateNeighborsPicked(const TCTPointCloud& scan, size_t ix, + std::vector* const picked) const; bool is_vis_ = false; diff --git a/oh_my_loam/extractor/extractor_VLP16.cc b/oh_my_loam/extractor/extractor_VLP16.cc index a0d35f1..9b0c3d3 100644 --- a/oh_my_loam/extractor/extractor_VLP16.cc +++ b/oh_my_loam/extractor/extractor_VLP16.cc @@ -1,10 +1,13 @@ #include "extractor_VLP16.h" +#include "common/math/math_utils.h" + namespace oh_my_loam { int ExtractorVLP16::GetScanID(const common::Point& pt) const { - double omega = std::atan2(pt.z, std::hypot(pt.x, pt.y)) * 180 * M_1_PI + 15.0; - return static_cast(std::round(omega / 2.0) + 1.e-5); + double theta = + common::Rad2Degree(std::atan2(pt.z, std::hypot(pt.x, pt.y))) + 15.0; + return static_cast(std::round(theta / 2.0) + 1.e-5); }; } // namespace oh_my_loam \ No newline at end of file diff --git a/oh_my_loam/odometer/odometer.cc b/oh_my_loam/odometer/odometer.cc index f6daf83..3eb32cf 100644 --- a/oh_my_loam/odometer/odometer.cc +++ b/oh_my_loam/odometer/odometer.cc @@ -18,12 +18,13 @@ bool Odometer::Init() { verbose_ = config_["verbose"].as(); kdtree_surf_.reset(new pcl::KdTreeFLANN); kdtree_corn_.reset(new pcl::KdTreeFLANN); - AINFO << "Odometer visualizer: " << (is_vis_ ? "ON" : "OFF"); + AINFO << "Odometry visualizer: " << (is_vis_ ? "ON" : "OFF"); if (is_vis_) visualizer_.reset(new OdometerVisualizer); return true; } -void Odometer::Process(const Feature& feature, Pose3D* const pose) { +void Odometer::Process(double timestamp, const Feature& feature, + Pose3D* const pose) { BLOCK_TIMER_START; if (!is_initialized_) { is_initialized_ = true; @@ -75,7 +76,7 @@ void Odometer::Process(const Feature& feature, Pose3D* const pose) { void Odometer::MatchCornFeature(const TPointCloud& src, const TPointCloud& tgt, std::vector* const pairs) const { - double dist_sq_thresh = config_["match_dist_sq_thresh"].as(); + double dist_sq_thresh = config_["match_dist_sq_th"].as(); for (const auto& q_pt : src) { TPoint query_pt; TransformToStart(pose_curr2last_, q_pt, &query_pt); @@ -119,7 +120,7 @@ void Odometer::MatchCornFeature(const TPointCloud& src, const TPointCloud& tgt, void Odometer::MatchSurfFeature( const TPointCloud& src, const TPointCloud& tgt, std::vector* const pairs) const { - double dist_sq_thresh = config_["match_dist_sq_thresh"].as(); + double dist_sq_thresh = config_["match_dist_sq_th"].as(); for (const auto& q_pt : src) { TPoint query_pt; TransformToStart(pose_curr2last_, q_pt, &query_pt); @@ -193,8 +194,10 @@ void Odometer::UpdatePre(const Feature& feature) { void Odometer::Visualize(const Feature& feature, const std::vector& pl_pairs, - const std::vector& pp_pairs) const { + const std::vector& pp_pairs, + double timestamp) const { std::shared_ptr frame(new OdometerVisFrame); + frame->timestamp = timestamp; frame->pl_pairs = pl_pairs; frame->pp_pairs = pp_pairs; frame->pose_curr2last = pose_curr2last_; diff --git a/oh_my_loam/odometer/odometer.h b/oh_my_loam/odometer/odometer.h index 0119bed..578e73b 100644 --- a/oh_my_loam/odometer/odometer.h +++ b/oh_my_loam/odometer/odometer.h @@ -15,7 +15,8 @@ class Odometer { bool Init(); - void Process(const Feature& feature, common::Pose3D* const pose); + void Process(double timestamp, const Feature& feature, + common::Pose3D* const pose); protected: void UpdatePre(const Feature& feature); @@ -30,7 +31,8 @@ class Odometer { void Visualize(const Feature& feature, const std::vector& pl_pairs, - const std::vector& pp_pairs) const; + const std::vector& pp_pairs, + double timestamp = 0.0) const; common::Pose3D pose_curr2world_; common::Pose3D pose_curr2last_; diff --git a/oh_my_loam/oh_my_loam.cc b/oh_my_loam/oh_my_loam.cc index 62c5cdb..42677c3 100644 --- a/oh_my_loam/oh_my_loam.cc +++ b/oh_my_loam/oh_my_loam.cc @@ -8,7 +8,7 @@ namespace oh_my_loam { namespace { const double kPointMinDist = 0.1; using namespace common; -} +} // namespace bool OhMyLoam::Init() { YAML::Node config = YAMLConfig::Instance()->config(); @@ -34,9 +34,9 @@ void OhMyLoam::Run(double timestamp, const PointCloudConstPtr& cloud_in) { PointCloudPtr cloud(new PointCloud); RemoveOutliers(*cloud_in, cloud.get()); Feature feature; - extractor_->Process(cloud, &feature); + extractor_->Process(timestamp, cloud, &feature); Pose3D pose; - odometer_->Process(feature, &pose); + odometer_->Process(timestamp, feature, &pose); poses_.emplace_back(pose); } diff --git a/oh_my_loam/visualizer/extractor_visualizer.cc b/oh_my_loam/visualizer/extractor_visualizer.cc index f794ad0..16f8191 100644 --- a/oh_my_loam/visualizer/extractor_visualizer.cc +++ b/oh_my_loam/visualizer/extractor_visualizer.cc @@ -7,12 +7,12 @@ using namespace common; void ExtractorVisualizer::Draw() { auto frame = GetCurrentFrame(); - DrawPointCloud(frame.cloud, WHITE, "cloud_raw"); - DrawPointCloud(frame.feature.cloud_less_flat_surf, CYAN, + DrawPointCloud(frame.cloud, GRAY, "cloud_raw"); + DrawPointCloud(frame.feature.cloud_less_flat_surf, BLUE, "cloud_less_flat_surf"); - DrawPointCloud(frame.feature.cloud_flat_surf, BLUE, + DrawPointCloud(frame.feature.cloud_flat_surf, CYAN, "cloud_flat_surf"); - DrawPointCloud(frame.feature.cloud_less_sharp_corner, PURPLE, + DrawPointCloud(frame.feature.cloud_less_sharp_corner, YELLOW, "cloud_less_sharp_corner"); DrawPointCloud(frame.feature.cloud_sharp_corner, RED, "cloud_sharp_corner");