diff --git a/CMakeLists.txt b/CMakeLists.txt index c160cf8..5f51a41 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 2.8.3) project(oh_my_loam) +set(CMAKE_CXX_FLAGS "-std=c++17 -Wall -lpthread") set(CMAKE_BUILD_TYPE "Release") -set(CMAKE_CXX_FLAGS "-std=c++17 -lpthread") -set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall") +set(CMAKE_CXX_FLAGS_RELEASE "-O3") find_package(Ceres REQUIRED) find_package(PCL QUIET) diff --git a/common/macros.h b/common/macros.h index 48b54d9..e957a70 100644 --- a/common/macros.h +++ b/common/macros.h @@ -4,8 +4,6 @@ #include #include -// #define LOG_TIMESTAMP(timestamp, precision) -// std::fixed << std::precision(precision) << timestamp; #define LOG_TIMESTAMP(timestamp) \ std::fixed << std::setprecision(3) << timestamp; @@ -17,7 +15,7 @@ #define DECLARE_SINGLETON(classname) \ public: \ static classname *Instance() { \ - static std::unique_ptr instance = nullptr; \ + static std::unique_ptr instance{nullptr}; \ if (!instance) { \ static std::once_flag flag; \ std::call_once(flag, \ diff --git a/common/utils.h b/common/utils.h index 8c11676..815d32e 100644 --- a/common/utils.h +++ b/common/utils.h @@ -77,11 +77,11 @@ void RemoveClosedPoints(const pcl::PointCloud& cloud_in, } template -void VoxelDownSample(const pcl::PointCloud& cloud_in, +void VoxelDownSample(const typename pcl::PointCloud::ConstPtr& cloud_in, pcl::PointCloud* const cloud_out, double voxel_size) { pcl::VoxelGrid filter; - filter.setInputCloud(cloud_in.makeShared()); + filter.setInputCloud(cloud_in); filter.setLeafSize(voxel_size, voxel_size, voxel_size); filter.filter(*cloud_out); } diff --git a/configs/config.yaml b/configs/config.yaml index 41e61e6..f48c5cd 100644 --- a/configs/config.yaml +++ b/configs/config.yaml @@ -10,13 +10,14 @@ extractor_config: vis: true sharp_corner_point_num: 2 corner_point_num: 20 - flat_surf_point_num: 4 - surf_point_num: 20 ## useless currently + flat_surf_point_num: 3 + surf_point_num: 3 ## useless currently corner_point_curvature_thres: 0.5 surf_point_curvature_thres: 0.5 neighbor_point_dist_thres: 0.05 - downsample_voxel_size: 0.2 + downsample_voxel_size: 0.5 odometry_config: icp_iter_num : 2 - match_dist_thresh: 0.5 \ No newline at end of file + match_dist_sq_thresh: 1 + vis: false \ No newline at end of file diff --git a/src/extractor/base_extractor.cc b/src/extractor/base_extractor.cc index a9a71d6..88cbba2 100644 --- a/src/extractor/base_extractor.cc +++ b/src/extractor/base_extractor.cc @@ -40,18 +40,9 @@ void Extractor::Process(const PointCloud& cloud, FeaturePoints* const feature) { } double time_assign = timer.toc(); // store points into feature point clouds according to their type - std::ostringstream oss; - oss << "Feature point num: "; for (const auto& scan : scans) { - FeaturePoints scan_feature; - GenerateFeaturePoints(scan, &scan_feature); - feature->Add(scan_feature); - oss << scan.size() << ":" << scan_feature.sharp_corner_pts->size() << ":" - << scan_feature.less_sharp_corner_pts->size() << ":" - << scan_feature.flat_surf_pts->size() << ":" - << scan_feature.less_flat_surf_pts->size() << " "; + GenerateFeaturePoints(scan, feature); } - ADEBUG << oss.str(); double time_store = timer.toc(); AINFO << "Time elapsed (ms): scan_split = " << std::setprecision(3) << time_split << ", curvature_compute = " << time_curv - time_split @@ -190,6 +181,7 @@ void Extractor::SetNeighborsPicked(const TCTPointCloud& scan, size_t ix, void Extractor::GenerateFeaturePoints(const TCTPointCloud& scan, FeaturePoints* const feature) const { + // TPointCloudPtr less_flat_surf_pts(new TPointCloud); for (const auto& pt : scan.points) { switch (pt.type) { case PointType::FLAT: @@ -214,11 +206,11 @@ void Extractor::GenerateFeaturePoints(const TCTPointCloud& scan, break; } } - TPointCloudPtr filtered_less_flat_surf_pts(new TPointCloud); - VoxelDownSample(*feature->less_flat_surf_pts, - filtered_less_flat_surf_pts.get(), - config_["downsample_voxel_size"].as()); - feature->less_flat_surf_pts = filtered_less_flat_surf_pts; + // TPointCloudPtr filtered_less_flat_surf_pts(new TPointCloud); + // VoxelDownSample(less_flat_surf_pts, + // filtered_less_flat_surf_pts.get(), + // config_["downsample_voxel_size"].as()); + // *feature->less_flat_surf_pts += *filtered_less_flat_surf_pts; } void Extractor::Visualize(const PointCloud& cloud, diff --git a/src/extractor/extractor_VLP16.cc b/src/extractor/extractor_VLP16.cc index afc14c5..74dfca8 100644 --- a/src/extractor/extractor_VLP16.cc +++ b/src/extractor/extractor_VLP16.cc @@ -3,14 +3,15 @@ namespace oh_my_loam { int ExtractorVLP16::GetScanID(const Point& pt) const { - static int i = 0; double omega = std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0; - if (i++ < 10) { - ADEBUG << "OMEGA: " << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0 - << " id = " << static_cast(std::round(omega / 2.0) + 0.01) - << " z = " << pt.z << " " - << " d = " << Distance(pt) << std::endl; - } + // static int i = 0; + // if (i++ < 10) { + // ADEBUG << "OMEGA: " << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + // + 15.0 + // << " id = " << static_cast(std::round(omega / 2.0) + 0.01) + // << " z = " << pt.z << " " + // << " d = " << Distance(pt) << std::endl; + // } return static_cast(std::round(omega / 2.0) + 1.e-5); }; diff --git a/src/extractor/feature_points.h b/src/extractor/feature_points.h index f3d7bbb..6b03e51 100644 --- a/src/extractor/feature_points.h +++ b/src/extractor/feature_points.h @@ -16,13 +16,6 @@ struct FeaturePoints { flat_surf_pts.reset(new TPointCloud); less_flat_surf_pts.reset(new TPointCloud); } - - void Add(const FeaturePoints& rhs) { - *sharp_corner_pts += *rhs.sharp_corner_pts; - *less_sharp_corner_pts += *rhs.less_sharp_corner_pts; - *flat_surf_pts += *rhs.flat_surf_pts; - *less_flat_surf_pts += *rhs.less_flat_surf_pts; - } }; } // namespace oh_my_loam diff --git a/src/helper/helper.cc b/src/helper/helper.cc index 76d9686..c5bd5e0 100644 --- a/src/helper/helper.cc +++ b/src/helper/helper.cc @@ -2,8 +2,6 @@ namespace oh_my_loam { -float GetTime(const TPoint& pt) { return pt.time - GetScanId(pt); } +// -int GetScanId(const TPoint& pt) { return static_cast(pt.time); } - -} // oh_my_loam \ No newline at end of file +} // namespace oh_my_loam \ No newline at end of file diff --git a/src/helper/helper.h b/src/helper/helper.h index 75c19f9..089c958 100644 --- a/src/helper/helper.h +++ b/src/helper/helper.h @@ -4,9 +4,11 @@ namespace oh_my_loam { -float GetTime(const TPoint& pt); +inline float GetTime(const TPoint& pt) { + return pt.time - static_cast(pt.time); +} -int GetScanId(const TPoint& pt); +inline int GetScanId(const TPoint& pt) { return static_cast(pt.time); } struct PointLinePair { TPoint pt; @@ -36,4 +38,4 @@ struct PointPlanePair { : pt(pt), plane(pt1, pt2, pt3) {} }; -} // oh_my_loam \ No newline at end of file +} // namespace oh_my_loam \ No newline at end of file diff --git a/src/odometry/odometry.cc b/src/odometry/odometry.cc index 73a0176..4b7f172 100644 --- a/src/odometry/odometry.cc +++ b/src/odometry/odometry.cc @@ -1,4 +1,5 @@ #include "odometry.h" + #include "solver/solver.h" namespace oh_my_loam { @@ -20,21 +21,25 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) { is_initialized = true; UpdatePre(feature); *pose = pose_curr2world_; - AINFO << "Odometry initialized...."; + AWARN << "Odometry initialized...."; return; } - double match_dist_thresh = config_["match_dist_thresh"].as(); + TicToc timer_whole; + double match_dist_sq_thresh = config_["match_dist_sq_thresh"].as(); AINFO << "Pose before: " << pose_curr2world_.ToString(); for (int i = 0; i < config_["icp_iter_num"].as(); ++i) { + TicToc timer; std::vector pl_pairs; std::vector pp_pairs; MatchCornPoints(*feature.sharp_corner_pts, *corn_pts_pre_, &pl_pairs, - match_dist_thresh); + match_dist_sq_thresh); + double time_pl_match = timer.toc(); AINFO_IF(i == 0) << "PL mathch: src size = " << feature.sharp_corner_pts->size() << ", tgt size = " << corn_pts_pre_->size(); MatchSurfPoints(*feature.flat_surf_pts, *surf_pts_pre_, &pp_pairs, - match_dist_thresh); + match_dist_sq_thresh); + double timer_pp_match = timer.toc(); AINFO_IF(i == 0) << "PP mathch: src size = " << feature.flat_surf_pts->size() << ", tgt size = " << surf_pts_pre_->size(); @@ -44,8 +49,10 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) { AWARN << "Too less correspondence: num = " << pl_pairs.size() + pp_pairs.size(); } - double* q = pose_curr2last_.q().coeffs().data(); - double* p = pose_curr2last_.p().data(); + Eigen::Vector4d q_vec = pose_curr2last_.q().coeffs(); + Eigen::Vector3d p_vec = pose_curr2last_.p(); + double* q = q_vec.data(); + double* p = p_vec.data(); PoseSolver solver(q, p); for (const auto& pair : pl_pairs) { solver.AddPointLinePair(pair, GetTime(pair.pt)); @@ -55,27 +62,30 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) { } solver.Solve(); pose_curr2last_ = Pose3D(q, p); + double time_solve = timer.toc(); + AINFO << "Time elapsed (ms), pl_match = " << time_pl_match + << ", pp_match = " << timer_pp_match - time_pl_match + << ", solve = " << time_solve - timer_pp_match; } pose_curr2world_ = pose_curr2world_ * pose_curr2last_; *pose = pose_curr2world_; AWARN << "Pose increase: " << pose_curr2last_.ToString(); AWARN << "Pose after: " << pose_curr2world_.ToString(); UpdatePre(feature); + AINFO << "Time elapsed (ms): whole odometry = " << timer_whole.toc(); } void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt, std::vector* const pairs, - double dist_thresh) const { - kdtree_corn_pts_->setInputCloud(tgt.makeShared()); - + double dist_sq_thresh) const { for (const auto& query_pt : src) { std::vector indices; std::vector dists; kdtree_corn_pts_->nearestKSearch(query_pt, 1, indices, dists); - if (dists[0] >= dist_thresh) continue; + if (dists[0] >= dist_sq_thresh) continue; TPoint pt1 = tgt.points[indices[0]]; int pt2_idx = -1; - double min_dist_pt2_squre = dist_thresh * dist_thresh; + double min_dist_pt2_squre = dist_sq_thresh; int query_pt_scan_id = GetScanId(query_pt); for (int i = indices[0] + 1; i < static_cast(tgt.size()); ++i) { const auto pt = tgt.points[i]; @@ -108,25 +118,23 @@ void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt, void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt, std::vector* const pairs, - double dist_thresh) const { - kdtree_surf_pts_->setInputCloud(tgt.makeShared()); - AINFO << "Point to plane mathch: src size = " << src.size() - << ", tgt size = " << tgt.size(); + double dist_sq_thresh) const { for (const auto& query_pt : src) { std::vector indices; std::vector dists; kdtree_surf_pts_->nearestKSearch(query_pt, 1, indices, dists); - if (dists[0] >= dist_thresh) continue; + if (dists[0] >= dist_sq_thresh) continue; TPoint pt1 = tgt.points[indices[0]]; int pt2_idx = -1, pt3_idx = -1; - double min_dist_pt2_squre = dist_thresh * dist_thresh; - double min_dist_pt3_squre = dist_thresh * dist_thresh; + double min_dist_pt2_squre = dist_sq_thresh; + double min_dist_pt3_squre = dist_sq_thresh; int query_pt_scan_id = GetScanId(query_pt); for (int i = indices[0] + 1; i < static_cast(tgt.size()); ++i) { const auto pt = tgt.points[i]; int scan_id = GetScanId(pt); if (scan_id > query_pt_scan_id + kNearbyScanNum) break; double dist_squre = DistanceSqure(query_pt, pt); + // AWARN_IF(scan_id != 0) << "SA" << query_pt_scan_id << ", " << scan_id; if (scan_id == query_pt_scan_id && dist_squre < min_dist_pt2_squre) { pt2_idx = i; min_dist_pt2_squre = dist_squre; @@ -140,9 +148,9 @@ void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt, for (int i = indices[0] - 1; i >= 0; --i) { const auto pt = tgt.points[i]; int scan_id = GetScanId(pt); - if (scan_id >= query_pt_scan_id) continue; if (scan_id < query_pt_scan_id - kNearbyScanNum) break; double dist_squre = DistanceSqure(query_pt, pt); + // AWARN_IF(scan_id != 0) << "SD" << query_pt_scan_id << ", " << scan_id; if (scan_id == query_pt_scan_id && dist_squre < min_dist_pt2_squre) { pt2_idx = i; min_dist_pt2_squre = dist_squre; @@ -163,6 +171,8 @@ void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt, void Odometry::UpdatePre(const FeaturePoints& feature) { surf_pts_pre_ = feature.less_flat_surf_pts; corn_pts_pre_ = feature.less_sharp_corner_pts; + kdtree_surf_pts_->setInputCloud(surf_pts_pre_); + kdtree_corn_pts_->setInputCloud(corn_pts_pre_); } } // namespace oh_my_loam diff --git a/src/odometry/odometry.h b/src/odometry/odometry.h index c196168..cba973c 100644 --- a/src/odometry/odometry.h +++ b/src/odometry/odometry.h @@ -20,11 +20,11 @@ class Odometry { void UpdatePre(const FeaturePoints& feature); void MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt, std::vector* const pairs, - double dist_thresh) const; + double dist_sq_thresh) const; void MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt, std::vector* const pairs, - double dist_thresh) const; + double dist_sq_thresh) const; Pose3D pose_curr2world_; Pose3D pose_curr2last_; diff --git a/src/oh_my_loam.cc b/src/oh_my_loam.cc index 76793af..28a036f 100644 --- a/src/oh_my_loam.cc +++ b/src/oh_my_loam.cc @@ -26,8 +26,6 @@ bool OhMyLoam::Init() { void OhMyLoam::Run(const PointCloud& cloud_in, double timestamp) { PointCloudPtr cloud(new PointCloud); RemoveOutliers(cloud_in, cloud.get()); - ADEBUG << "After remove, point num: " << cloud_in.size() << " -> " - << cloud->size(); FeaturePoints feature_points; extractor_->Process(*cloud, &feature_points); Pose3D pose;