diff --git a/common/geometry/pose3d.h b/common/geometry/pose3d.h index c2ff21b..a3d8224 100644 --- a/common/geometry/pose3d.h +++ b/common/geometry/pose3d.h @@ -77,13 +77,13 @@ class Pose3d { // const Eigen::Quaterniond& r_quat() const { return r_quat_; } - Eigen::Quaterniond r_quat() const { + const Eigen::Quaterniond &r_quat() const { return r_quat_; } // const Eigen::Vector3d& t_vec() const { return t_vec_; } - Eigen::Vector3d t_vec() const { + const Eigen::Vector3d &t_vec() const { return t_vec_; } diff --git a/common/math/math_utils.cc b/common/math/math_utils.cc index a637a1b..f7a5d70 100644 --- a/common/math/math_utils.cc +++ b/common/math/math_utils.cc @@ -18,7 +18,7 @@ double Rad2Degree(double rad) { } const std::vector Range(int begin, int end, int step) { - ACHECK(step != 0) << "Step must non-zero"; + ACHECK(step != 0) << "Step must be non-zero"; int num = (end - begin) / step; if (num <= 0) return {}; end = begin + step * num; diff --git a/common/pcl/pcl_utils.h b/common/pcl/pcl_utils.h index 1540cb4..10f5895 100644 --- a/common/pcl/pcl_utils.h +++ b/common/pcl/pcl_utils.h @@ -6,41 +6,41 @@ namespace common { // Distance squred of a point to origin -template -inline double DistanceSqure(const PointType &pt) { +template +inline double DistanceSqure(const PT &pt) { return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z; } // Distance squred of two points -template -inline double DistanceSqure(const PointType &pt1, const PointType &pt2) { +template +inline double DistanceSqure(const PT &pt1, const PT &pt2) { return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y) + (pt1.z - pt2.z) * (pt1.z - pt2.z); } // Distance of a point to origin -template -inline double Distance(const PointType &pt) { +template +inline double Distance(const PT &pt) { return std::sqrt(DistanceSqure(pt)); } // Distance squred of two points -template -inline double Distance(const PointType &pt1, const PointType &pt2) { +template +inline double Distance(const PT &pt1, const PT &pt2) { return std::sqrt(DistanceSqure(pt1, pt2)); } // Check whether is a finite point: neither infinite nor nan -template -inline double IsFinite(const PointType &pt) { +template +inline double IsFinite(const PT &pt) { return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z); } // Remove point if the condition evaluated to true on it -template -void RemovePoints(const pcl::PointCloud &cloud_in, - pcl::PointCloud *const cloud_out, - std::function check, +template +void RemovePoints(const pcl::PointCloud &cloud_in, + pcl::PointCloud *const cloud_out, + std::function check, std::vector *const removed_indices = nullptr) { if (&cloud_in != cloud_out) { cloud_out->header = cloud_in.header; @@ -62,11 +62,10 @@ void RemovePoints(const pcl::PointCloud &cloud_in, cloud_out->is_dense = true; } -template -void VoxelDownSample( - const typename pcl::PointCloud::ConstPtr &cloud_in, - pcl::PointCloud *const cloud_out, double voxel_size) { - pcl::VoxelGrid filter; +template +void VoxelDownSample(const typename pcl::PointCloud::ConstPtr &cloud_in, + pcl::PointCloud *const cloud_out, double voxel_size) { + pcl::VoxelGrid filter; filter.setInputCloud(cloud_in); filter.setLeafSize(voxel_size, voxel_size, voxel_size); filter.filter(*cloud_out); diff --git a/common/visualizer/lidar_visualizer.h b/common/visualizer/lidar_visualizer.h index 1fcf920..8bc671c 100644 --- a/common/visualizer/lidar_visualizer.h +++ b/common/visualizer/lidar_visualizer.h @@ -118,18 +118,18 @@ class LidarVisualizer { return *static_cast((*curr_frame_iter_).get()); } - template - void DrawPointCloud( - const typename pcl::PointCloud::ConstPtr &cloud, - const Color &color, const std::string &id, int point_size = 3) { - AddPointCloud(cloud, color, id, viewer_.get(), point_size); + template + void DrawPointCloud(const typename pcl::PointCloud::ConstPtr &cloud, + const Color &color, const std::string &id, + int point_size = 3) { + AddPointCloud(cloud, color, id, viewer_.get(), point_size); } - template - void DrawPointCloud( - const typename pcl::PointCloud::ConstPtr &cloud, - const std::string &field, const std::string &id, int point_size = 3) { - AddPointCloud(cloud, field, id, viewer_.get(), point_size); + template + void DrawPointCloud(const typename pcl::PointCloud::ConstPtr &cloud, + const std::string &field, const std::string &id, + int point_size = 3) { + AddPointCloud(cloud, field, id, viewer_.get(), point_size); } // visualizer name diff --git a/common/visualizer/lidar_visualizer_utils.h b/common/visualizer/lidar_visualizer_utils.h index 626a2b1..4175b2f 100644 --- a/common/visualizer/lidar_visualizer_utils.h +++ b/common/visualizer/lidar_visualizer_utils.h @@ -17,23 +17,22 @@ using PCLVisualizer = pcl::visualization::PCLVisualizer; #define PCLColorHandlerGenericField \ pcl::visualization::PointCloudColorHandlerGenericField -template -void AddPointCloud(const typename pcl::PointCloud::ConstPtr &cloud, +template +void AddPointCloud(const typename pcl::PointCloud::ConstPtr &cloud, const Color &color, const std::string &id, PCLVisualizer *const viewer, int point_size = 3) { - PCLColorHandlerCustom color_handler(cloud, color.r, color.g, - color.b); - viewer->addPointCloud(cloud, color_handler, id); + PCLColorHandlerCustom color_handler(cloud, color.r, color.g, color.b); + viewer->addPointCloud(cloud, color_handler, id); viewer->setPointCloudRenderingProperties( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id); } -template -void AddPointCloud(const typename pcl::PointCloud::ConstPtr &cloud, +template +void AddPointCloud(const typename pcl::PointCloud::ConstPtr &cloud, const std::string &field, const std::string &id, PCLVisualizer *const viewer, int point_size = 3) { - PCLColorHandlerGenericField color_handler(cloud, field); - viewer->addPointCloud(cloud, color_handler, id); + PCLColorHandlerGenericField color_handler(cloud, field); + viewer->addPointCloud(cloud, color_handler, id); viewer->setPointCloudRenderingProperties( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id); } diff --git a/oh_my_loam/base/helper.h b/oh_my_loam/base/helper.h index 658de7b..3aae436 100644 --- a/oh_my_loam/base/helper.h +++ b/oh_my_loam/base/helper.h @@ -7,9 +7,8 @@ namespace oh_my_loam { using common::Pose3d; -template -void TransformPoint(const Pose3d &pose, const PointType &pt_in, - PointType *const pt_out) { +template +void TransformPoint(const Pose3d &pose, const PT &pt_in, PT *const pt_out) { *pt_out = pt_in; Eigen::Vector3d pnt = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z); pt_out->x = pnt.x(); @@ -17,10 +16,10 @@ void TransformPoint(const Pose3d &pose, const PointType &pt_in, pt_out->z = pnt.z(); } -template -PointType TransformPoint(const Pose3d &pose, const PointType &pt_in) { - PointType pt_out; - TransformPoint(pose, pt_in, &pt_out); +template +PT TransformPoint(const Pose3d &pose, const PT &pt_in) { + PT pt_out; + TransformPoint(pose, pt_in, &pt_out); return pt_out; } diff --git a/oh_my_loam/base/types.h b/oh_my_loam/base/types.h index eb2ebcf..08d7b5c 100644 --- a/oh_my_loam/base/types.h +++ b/oh_my_loam/base/types.h @@ -4,7 +4,7 @@ namespace oh_my_loam { -enum class PType { +enum class PointType { FLAT_SURF = -2, SURF = -1, NORNAL = 0, @@ -67,7 +67,7 @@ struct PointXYZTCT { struct { float time; float curvature; - PType type; + PointType type; }; float data_c[4]; }; @@ -76,11 +76,11 @@ struct PointXYZTCT { x = y = z = 0.0f; data[3] = 1.0f; time = curvature = 0.0f; - type = PType::NORNAL; + type = PointType::NORNAL; } PointXYZTCT(float x, float y, float z, float time = 0.0f, - float curvature = 0.0f, PType type = PType::NORNAL) + float curvature = 0.0f, PointType type = PointType::NORNAL) : x(x), y(y), z(z), time(time), curvature(curvature), type(type) { data[3] = 1.0f; } @@ -92,7 +92,7 @@ struct PointXYZTCT { data[3] = 1.0f; time = 0.0f; curvature = 0.0f; - type = PType::NORNAL; + type = PointType::NORNAL; } PointXYZTCT(const TPoint &p) { @@ -102,7 +102,7 @@ struct PointXYZTCT { data[3] = 1.0f; time = p.time; curvature = 0.0f; - type = PType::NORNAL; + type = PointType::NORNAL; } PointXYZTCT(const PointXYZTCT &p) { diff --git a/oh_my_loam/configs/config.yaml b/oh_my_loam/configs/config.yaml index a4ab950..cf66638 100644 --- a/oh_my_loam/configs/config.yaml +++ b/oh_my_loam/configs/config.yaml @@ -12,7 +12,7 @@ extractor_config: sharp_corner_point_num: 2 corner_point_num: 20 flat_surf_point_num: 4 - surf_point_num: 50 + surf_point_num: 30 corner_point_curvature_th: 0.5 surf_point_curvature_th: 0.5 neighbor_point_dist_th: 0.1 @@ -21,8 +21,9 @@ extractor_config: # configs for odometer odometer_config: vis: true - verbose: false + verbose: true icp_iter_num: 2 + solve_iter_num: 5 match_dist_sq_th: 1 # configs for mapper diff --git a/oh_my_loam/extractor/extractor.cc b/oh_my_loam/extractor/extractor.cc index 19127c7..a56d174 100644 --- a/oh_my_loam/extractor/extractor.cc +++ b/oh_my_loam/extractor/extractor.cc @@ -9,11 +9,10 @@ namespace oh_my_loam { namespace { const int kScanSegNum = 6; const double kTwoPi = 2 * M_PI; -using namespace common; } // namespace bool Extractor::Init() { - const auto &config = YAMLConfig::Instance()->config(); + const auto &config = common::YAMLConfig::Instance()->config(); config_ = config["extractor_config"]; is_vis_ = config["vis"].as() && config_["vis"].as(); verbose_ = config_["verbose"].as(); @@ -22,7 +21,8 @@ bool Extractor::Init() { return true; } -void Extractor::Process(double timestamp, const PointCloudConstPtr &cloud, +void Extractor::Process(double timestamp, + const common::PointCloudConstPtr &cloud, std::vector *const features) { BLOCK_TIMER_START; if (cloud->size() < config_["min_point_num"].as()) { @@ -54,25 +54,25 @@ void Extractor::Process(double timestamp, const PointCloudConstPtr &cloud, if (is_vis_) Visualize(cloud, *features, timestamp); } -void Extractor::SplitScan(const PointCloud &cloud, +void Extractor::SplitScan(const common::PointCloud &cloud, std::vector *const scans) const { scans->resize(num_scans_); - double yaw_start = -atan2(cloud.points[0].y, cloud.points[0].x); + double yaw_start = -std::atan2(cloud.points[0].y, cloud.points[0].x); bool half_passed = false; 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); - double yaw_diff = NormalizeAngle(yaw - yaw_start); + double yaw = -std::atan2(pt.y, pt.x); + double yaw_diff = common::NormalizeAngle(yaw - yaw_start); if (yaw_diff > 0) { if (half_passed) yaw_start += kTwoPi; } else { half_passed = true; yaw_start += kTwoPi; } - (*scans)[scan_id].push_back({pt.x, pt.y, pt.z, - static_cast(yaw_diff / kTwoPi), - std::nanf("")}); + double time = std::min(yaw_diff / kTwoPi, 1.0) + scan_id; + scans->at(scan_id).push_back( + {pt.x, pt.y, pt.z, static_cast(time), std::nanf("")}); } } @@ -91,7 +91,7 @@ void Extractor::ComputeCurvature(TCTPointCloud *const scan) const { pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z; pts[i].curvature = std::hypot(dx, dy, dz); } - RemovePoints(*scan, scan, [](const TCTPoint &pt) { + common::RemovePoints(*scan, scan, [](const TCTPoint &pt) { return !std::isfinite(pt.curvature); }); } @@ -101,7 +101,7 @@ void Extractor::AssignType(TCTPointCloud *const scan) const { 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); + std::vector indices = common::Range(pt_num); int sharp_corner_point_num = config_["sharp_corner_point_num"].as(); int corner_point_num = config_["corner_point_num"].as(); int flat_surf_point_num = config_["flat_surf_point_num"].as(); @@ -125,9 +125,9 @@ void Extractor::AssignType(TCTPointCloud *const scan) const { 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 = PType::SHARP_CORNER; + scan->at(ix).type = PointType::SHARP_CORNER; } else if (corner_point_picked_num <= corner_point_num) { - scan->at(ix).type = PType::CORNER; + scan->at(ix).type = PointType::CORNER; } else { break; } @@ -142,9 +142,9 @@ void Extractor::AssignType(TCTPointCloud *const scan) const { 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 = PType::FLAT_SURF; + scan->at(ix).type = PointType::FLAT_SURF; } else if (surf_point_picked_num <= surf_point_num) { - scan->at(ix).type = PType::SURF; + scan->at(ix).type = PointType::SURF; } else { break; } @@ -160,31 +160,33 @@ void Extractor::GenerateFeature(const TCTPointCloud &scan, for (const auto &pt : scan) { TPoint point(pt.x, pt.y, pt.z, pt.time); switch (pt.type) { - case PType::FLAT_SURF: - feature->cloud_flat_surf->points.emplace_back(point); + case PointType::FLAT_SURF: + feature->cloud_flat_surf->push_back(point); // no break: FLAT_SURF points are also SURF points - case PType::SURF: - feature->cloud_surf->points.emplace_back(point); + case PointType::SURF: + feature->cloud_surf->push_back(point); break; - case PType::SHARP_CORNER: - feature->cloud_sharp_corner->points.emplace_back(point); + case PointType::SHARP_CORNER: + feature->cloud_sharp_corner->push_back(point); // no break: SHARP_CORNER points are also CORNER points - case PType::CORNER: - feature->cloud_corner->points.emplace_back(point); + case PointType::CORNER: + feature->cloud_corner->push_back(point); break; default: + feature->cloud_surf->push_back(point); break; } } TPointCloudPtr dowm_sampled(new TPointCloud); - VoxelDownSample(feature->cloud_surf, dowm_sampled.get(), - config_["downsample_voxel_size"].as()); - feature->cloud_surf->swap(*dowm_sampled); + common::VoxelDownSample( + feature->cloud_surf, dowm_sampled.get(), + config_["downsample_voxel_size"].as()); + feature->cloud_surf = dowm_sampled; } -void Extractor::Visualize(const PointCloudConstPtr &cloud, +void Extractor::Visualize(const common::PointCloudConstPtr &cloud, const std::vector &features, - double timestamp) { + double timestamp) const { std::shared_ptr frame(new ExtractorVisFrame); frame->timestamp = timestamp; frame->cloud = cloud; @@ -194,20 +196,20 @@ void Extractor::Visualize(const PointCloudConstPtr &cloud, void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix, std::vector *const picked) const { - auto DistSqure = [&](int i, int j) -> float { - return DistanceSqure(scan[i], scan[j]); + auto dist_sq = [&](size_t i, size_t j) -> double { + return common::DistanceSqure(scan[i], scan[j]); }; - float neighbor_point_dist_th = config_["neighbor_point_dist_th"].as(); + double 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_th) break; + if (dist_sq(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_th) break; + if (dist_sq(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 00faf63..0c9e901 100644 --- a/oh_my_loam/extractor/extractor.h +++ b/oh_my_loam/extractor/extractor.h @@ -37,7 +37,7 @@ class Extractor { virtual void Visualize(const common::PointCloudConstPtr &cloud, const std::vector &features, - double timestamp = 0.0); + double timestamp = 0.0) const; int num_scans_ = 0; diff --git a/oh_my_loam/odometer/odometer.cc b/oh_my_loam/odometer/odometer.cc index 1c3a059..6fcb67b 100644 --- a/oh_my_loam/odometer/odometer.cc +++ b/oh_my_loam/odometer/odometer.cc @@ -1,6 +1,11 @@ #include "odometer.h" +#include + +#include "common/log/log.h" #include "common/pcl/pcl_utils.h" +#include "common/time/timer.h" +#include "oh_my_loam/base/types.h" #include "oh_my_loam/solver/solver.h" namespace oh_my_loam { @@ -8,11 +13,16 @@ namespace oh_my_loam { namespace { int kNearScanN = 2; size_t kMinMatchNum = 10; -using namespace common; +int GetScanId(const TPoint &point) { + return static_cast(point.time); +} +float GetTime(const TPoint &point) { + return point.time - GetScanId(point); +} } // namespace bool Odometer::Init() { - const auto &config = YAMLConfig::Instance()->config(); + const auto &config = common::YAMLConfig::Instance()->config(); config_ = config["odometer_config"]; is_vis_ = config["vis"].as() && config_["vis"].as(); verbose_ = config_["verbose"].as(); @@ -23,7 +33,6 @@ bool Odometer::Init() { void Odometer::Process(double timestamp, const std::vector &features, Pose3d *const pose_out) { - BLOCK_TIMER_START; if (!is_initialized_) { UpdatePre(features); is_initialized_ = true; @@ -33,9 +42,13 @@ void Odometer::Process(double timestamp, const std::vector &features, AWARN << "Odometer initialized...."; return; } - AINFO_IF(verbose_) << "Pose before: " << pose_curr2world_.ToString(); + BLOCK_TIMER_START; + AINFO << "Pose before: " << pose_curr2world_.ToString(); std::vector pl_pairs; std::vector pp_pairs; + AINFO_IF(verbose_) << "Points to be matched: " + << kdtree_corn_.getInputCloud()->size() << " + " + << kdtree_surf_.getInputCloud()->size(); for (int i = 0; i < config_["icp_iter_num"].as(); ++i) { for (const auto &feature : features) { MatchCorn(*feature.cloud_sharp_corner, &pl_pairs); @@ -56,136 +69,148 @@ void Odometer::Process(double timestamp, const std::vector &features, for (const auto &pair : pp_pairs) { solver.AddPointPlanePair(pair, pair.pt.time); } - solver.Solve(5, verbose_, &pose_curr2last_); + solver.Solve(config_["solve_iter_num"].as(), verbose_, + &pose_curr2last_); + AINFO << "Odometer::ICP: iter_" << i << ": " << BLOCK_TIMER_STOP_FMT; } pose_curr2world_ = pose_curr2world_ * pose_curr2last_; *pose_out = pose_curr2world_; - AINFO_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString(); - AINFO_IF(verbose_) << "Pose after: " << pose_curr2world_.ToString(); + AINFO << "Pose increase: " << pose_curr2last_.ToString(); + AINFO << "Pose after: " << pose_curr2world_.ToString(); UpdatePre(features); - if (is_vis_) Visualize(features, pl_pairs, pp_pairs); AINFO << "Odometer::Porcess: " << BLOCK_TIMER_STOP_FMT; + if (is_vis_) Visualize(features, pl_pairs, pp_pairs); } void Odometer::MatchCorn(const TPointCloud &src, std::vector *const pairs) const { double dist_sq_thresh = config_["match_dist_sq_th"].as(); - auto comp = [](const std::vector &v1, const std::vector &v2) { - return v1[0] < v2[0]; - }; - for (const auto &pt : src) { - TPoint query_pt = TransformToStart(pose_curr2last_, pt); - std::vector> indices; - std::vector> dists; - NearestKSearch(kdtrees_corn_, query_pt, 1, &indices, &dists); - int pt1_scan_id = - std::min_element(dists.begin(), dists.end(), comp) - dists.begin(); - if (dists[pt1_scan_id][0] >= dist_sq_thresh) continue; - TPoint pt1 = clouds_corn_pre_[pt1_scan_id]->at(indices[pt1_scan_id][0]); + for (const auto &point : src) { + TPoint query_pt = TransformToStart(pose_curr2last_, point); + std::vector indices(1); + std::vector dists(1); + if (kdtree_corn_.nearestKSearch(query_pt, 1, indices, dists) != 1) { + continue; + } + if (dists[0] >= dist_sq_thresh) continue; + TPoint pt1 = kdtree_corn_.getInputCloud()->at(indices[0]); + int pt1_scan_id = GetScanId(pt1); - double min_dist_pt2_squre = dist_sq_thresh; - int pt2_scan_id = -1; + bool pt2_fount = false; + float min_pt2_dist_squre = dist_sq_thresh; + TPoint pt2; int i_begin = std::max(0, pt1_scan_id - kNearScanN); - int i_end = std::min(indices.size(), pt1_scan_id + kNearScanN + 1); + int i_end = + std::min(kdtrees_scan_corn_.size(), pt1_scan_id + kNearScanN + 1); for (int i = i_begin; i < i_end && i != pt1_scan_id; ++i) { - if (dists[i][0] < min_dist_pt2_squre) { - pt2_scan_id = i; - min_dist_pt2_squre = dists[i][0]; + const auto &kdtree = kdtrees_scan_corn_[i]; + if (kdtree.nearestKSearch(query_pt, 1, indices, dists) != 0) { + continue; + } + if (dists[0] < min_pt2_dist_squre) { + pt2_fount = true; + pt2 = kdtree.getInputCloud()->at(indices[0]); + min_pt2_dist_squre = dists[0]; } } - if (pt2_scan_id >= 0) { - TPoint pt2 = clouds_corn_pre_[pt2_scan_id]->at(indices[pt2_scan_id][0]); - pairs->emplace_back(pt, pt1, pt2); - } + if (!pt2_fount) continue; + + TPoint pt(point.x, point.y, point.z, GetTime(point)); + pt1.time = GetTime(pt1); + pt2.time = GetTime(pt2); + pairs->emplace_back(pt, pt1, pt2); } } void Odometer::MatchSurf(const TPointCloud &src, std::vector *const pairs) const { double dist_sq_thresh = config_["match_dist_sq_th"].as(); - auto comp = [](const std::vector &v1, const std::vector &v2) { - return v1[0] < v2[0]; - }; - for (const auto &pt : src) { - TPoint query_pt = TransformToStart(pose_curr2last_, pt); - std::vector> indices; - std::vector> dists; - NearestKSearch(kdtrees_surf_, query_pt, 2, &indices, &dists); - int pt1_scan_id = - std::min_element(dists.begin(), dists.end(), comp) - dists.begin(); - if (dists[pt1_scan_id][0] >= dist_sq_thresh) continue; - if (dists[pt1_scan_id][1] >= dist_sq_thresh) continue; - TPoint pt1 = clouds_surf_pre_[pt1_scan_id]->at(indices[pt1_scan_id][0]); - TPoint pt2 = clouds_surf_pre_[pt1_scan_id]->at(indices[pt1_scan_id][1]); + for (const auto &point : src) { + TPoint query_pt = TransformToStart(pose_curr2last_, point); + std::vector indices; + std::vector dists; + if (kdtree_surf_.nearestKSearch(query_pt, 2, indices, dists) != 2) { + continue; + } + if (dists[0] >= dist_sq_thresh || dists[1] >= dist_sq_thresh) continue; + TPoint pt1 = kdtree_surf_.getInputCloud()->at(indices[0]); + int pt1_scan_id = GetScanId(pt1); - double min_dist_pt3_squre = dist_sq_thresh; - int pt3_scan_id = -1; - int i_begin = std::max(0, pt1_scan_id - kNearScanN); - int i_end = std::min(indices.size(), pt1_scan_id + kNearScanN + 1); - for (int i = i_begin; i < i_end && i != pt1_scan_id; ++i) { - if (dists[i][0] < min_dist_pt3_squre) { - pt3_scan_id = i; - min_dist_pt3_squre = dists[i][0]; + TPoint pt2 = kdtree_surf_.getInputCloud()->at(indices[1]), pt3; + bool pt2_found = true, pt3_found = false; + int pt2_scan_id = GetScanId(pt2); + if (pt2_scan_id != pt1_scan_id) { + pt2_found = false; + if (std::abs(pt2_scan_id - pt1_scan_id) <= kNearScanN) { + pt3 = pt2; + pt3_found = true; } } - if (pt3_scan_id >= 0) { - TPoint pt3 = clouds_surf_pre_[pt3_scan_id]->at(indices[pt3_scan_id][0]); - pairs->emplace_back(pt, pt1, pt2, pt3); + + if (!pt2_found) { + const auto &kdtree = kdtrees_scan_surf_[pt1_scan_id]; + if (kdtree.nearestKSearch(query_pt, 2, indices, dists) == 2) { + if (dists[1] < dist_sq_thresh) { + pt2 = kdtree.getInputCloud()->at(indices[1]); + pt2_found = true; + } + } } + if (!pt2_found) continue; + + if (!pt3_found) { + float min_pt3_dist_squre = dist_sq_thresh; + int i_begin = std::max(0, pt1_scan_id - kNearScanN); + int i_end = std::min(kdtrees_scan_corn_.size(), + pt1_scan_id + kNearScanN + 1); + for (int i = i_begin; i < i_end && i != pt1_scan_id; ++i) { + const auto &kdtree = kdtrees_scan_surf_[i]; + if (kdtree.nearestKSearch(query_pt, 1, indices, dists) != 1) { + continue; + } + if (dists[0] < min_pt3_dist_squre) { + pt3 = kdtree.getInputCloud()->at(indices[0]); + pt3_found = true; + min_pt3_dist_squre = dists[0]; + } + } + } + if (!pt3_found) continue; + + TPoint pt(point.x, point.y, point.z, GetTime(point)); + pt1.time = GetTime(pt1); + pt2.time = GetTime(pt2); + pt3.time = GetTime(pt3); + pairs->emplace_back(pt, pt1, pt2, pt3); } } void Odometer::UpdatePre(const std::vector &features) { - BLOCK_TIMER_START; - clouds_corn_pre_.resize(features.size()); - clouds_surf_pre_.resize(features.size()); - kdtrees_corn_.resize(features.size()); - kdtrees_surf_.resize(features.size()); - + kdtrees_scan_corn_.resize(features.size()); + kdtrees_scan_surf_.resize(features.size()); + TPointCloudPtr corn_pre(new TPointCloud); + TPointCloudPtr surf_pre(new TPointCloud); + TPointCloudPtr scan_corn_pre(new TPointCloud); + TPointCloudPtr scan_surf_pre(new TPointCloud); for (size_t i = 0; i < features.size(); ++i) { const auto &feature = features[i]; - auto &cloud_pre = clouds_corn_pre_[i]; - if (!cloud_pre) cloud_pre.reset(new TPointCloud); - cloud_pre->resize(feature.cloud_corner->size()); + scan_corn_pre->resize(feature.cloud_corner->size()); + scan_surf_pre->resize(feature.cloud_surf->size()); for (size_t j = 0; j < feature.cloud_corner->size(); ++j) { TransformToEnd(pose_curr2last_, feature.cloud_corner->at(j), - &cloud_pre->at(j)); + &scan_corn_pre->at(j)); } - kdtrees_corn_[i].setInputCloud(cloud_pre); - } - - for (size_t i = 0; i < features.size(); ++i) { - const auto &feature = features[i]; - auto &cloud_pre = clouds_surf_pre_[i]; - if (!cloud_pre) cloud_pre.reset(new TPointCloud); - cloud_pre->resize(feature.cloud_surf->size()); for (size_t j = 0; j < feature.cloud_surf->size(); ++j) { TransformToEnd(pose_curr2last_, feature.cloud_surf->at(j), - &cloud_pre->at(j)); + &scan_surf_pre->at(j)); } - kdtrees_surf_[i].setInputCloud(cloud_pre); - } - AINFO << "Odometer::UpdatePre: " << BLOCK_TIMER_STOP_FMT; -} - -void Odometer::NearestKSearch( - const std::vector> &kdtrees, - const TPoint &query_pt, int k, std::vector> *const indices, - std::vector> *const dists) const { - for (const auto &kdtree : kdtrees) { - std::vector index; - std::vector dist; - int n_found = 0; - if (kdtree.getInputCloud()->size() >= static_cast(k)) { - n_found = kdtree.nearestKSearch(query_pt, k, index, dist); - } - if (n_found < k) { - std::vector(k, -1).swap(index); - std::vector(k, std::numeric_limits::max()).swap(dist); - } - indices->push_back(std::move(index)); - dists->push_back(std::move(dist)); + *corn_pre += *scan_corn_pre; + *surf_pre += *scan_surf_pre; + kdtrees_scan_corn_[i].setInputCloud(scan_corn_pre); + kdtrees_scan_surf_[i].setInputCloud(scan_surf_pre); } + kdtree_corn_.setInputCloud(corn_pre); + kdtree_surf_.setInputCloud(surf_pre); } void Odometer::Visualize(const std::vector &features, diff --git a/oh_my_loam/odometer/odometer.h b/oh_my_loam/odometer/odometer.h index b89bf4a..b1592bc 100644 --- a/oh_my_loam/odometer/odometer.h +++ b/oh_my_loam/odometer/odometer.h @@ -31,21 +31,18 @@ class Odometer { const std::vector &pl_pairs, const std::vector &pp_pairs, double timestamp = 0.0) const; - void NearestKSearch(const std::vector> &kdtrees, - const TPoint &query_pt, int k, - std::vector> *const indices, - std::vector> *const dists) const; + + bool NearestSearch(const pcl::KdTreeFLANN &kdtree, + const TPoint &query_pt, int k, float dist_sq_th, + std::vector *const indices) const; common::Pose3d pose_curr2world_; common::Pose3d pose_curr2last_; - std::vector clouds_corn_pre_; - std::vector clouds_surf_pre_; - TPointCloudPtr corn_pre_; - TPointCloudPtr surf_pre_; - - std::vector> kdtrees_surf_; - std::vector> kdtrees_corn_; + std::vector> kdtrees_scan_surf_; + std::vector> kdtrees_scan_corn_; + pcl::KdTreeFLANN kdtree_corn_; + pcl::KdTreeFLANN kdtree_surf_; YAML::Node config_; diff --git a/oh_my_loam/solver/cost_function.h b/oh_my_loam/solver/cost_function.h index ade3208..41af71f 100644 --- a/oh_my_loam/solver/cost_function.h +++ b/oh_my_loam/solver/cost_function.h @@ -90,4 +90,4 @@ bool PointPlaneCostFunction::operator()(const T *const q, const T *const p, return true; } -} // oh_my_loam \ No newline at end of file +} // namespace oh_my_loam \ No newline at end of file diff --git a/oh_my_loam/solver/solver.cc b/oh_my_loam/solver/solver.cc index 21054c0..2e42d10 100644 --- a/oh_my_loam/solver/solver.cc +++ b/oh_my_loam/solver/solver.cc @@ -1,30 +1,37 @@ #include "solver.h" +#include +#include + +#include "common/log/log.h" + namespace oh_my_loam { namespace { double kHuberLossScale = 0.1; } -PoseSolver::PoseSolver(const common::Pose3d &pose) - : r_quat_(pose.r_quat().coeffs().data()), t_vec_(pose.t_vec().data()) { +PoseSolver::PoseSolver(const common::Pose3d &pose) { + std::copy_n(pose.r_quat().coeffs().data(), 4, r_quat_); + std::copy_n(pose.t_vec().data(), 3, t_vec_); loss_function_ = new ceres::HuberLoss(kHuberLossScale); problem_.AddParameterBlock(r_quat_, 4, new ceres::EigenQuaternionParameterization()); problem_.AddParameterBlock(t_vec_, 3); } -double PoseSolver::Solve(int max_iter_num, bool verbose, - common::Pose3d *const pose) { +bool PoseSolver::Solve(int max_iter_num, bool verbose, + common::Pose3d *const pose) { ceres::Solver::Options options; options.linear_solver_type = ceres::DENSE_QR; options.max_num_iterations = max_iter_num; - options.minimizer_progress_to_stdout = verbose; + options.minimizer_progress_to_stdout = false; ceres::Solver::Summary summary; ceres::Solve(options, &problem_, &summary); AINFO_IF(verbose) << summary.BriefReport(); + AWARN_IF(!summary.IsSolutionUsable()) << "Solution may be unusable"; if (pose) *pose = common::Pose3d(r_quat_, t_vec_); - return summary.final_cost; + return summary.IsSolutionUsable(); } void PoseSolver::AddPointLinePair(const PointLinePair &pair, double time) { @@ -43,4 +50,4 @@ common::Pose3d PoseSolver::GetPose() const { return common::Pose3d(r_quat_, t_vec_); } -} // oh_my_loam \ No newline at end of file +} // namespace oh_my_loam \ No newline at end of file diff --git a/oh_my_loam/solver/solver.h b/oh_my_loam/solver/solver.h index 9f91a8b..20a860d 100644 --- a/oh_my_loam/solver/solver.h +++ b/oh_my_loam/solver/solver.h @@ -1,20 +1,20 @@ #pragma once #include "common/geometry/pose3d.h" -#include "cost_function.h" +#include "oh_my_loam/solver/cost_function.h" namespace oh_my_loam { class PoseSolver { public: - PoseSolver(const common::Pose3d &pose); + explicit PoseSolver(const common::Pose3d &pose); void AddPointLinePair(const PointLinePair &pair, double time); void AddPointPlanePair(const PointPlanePair &pair, double time); - double Solve(int max_iter_num = 5, bool verbose = false, - common::Pose3d *const pose = nullptr); + bool Solve(int max_iter_num = 5, bool verbose = false, + common::Pose3d *const pose = nullptr); common::Pose3d GetPose() const; @@ -24,7 +24,7 @@ class PoseSolver { ceres::LossFunction *loss_function_; // r_quat_: [x, y, z, w], t_vec_: [x, y, z] - double *r_quat_, *t_vec_; + double r_quat_[4], t_vec_[3]; DISALLOW_COPY_AND_ASSIGN(PoseSolver) }; diff --git a/oh_my_loam/visualizer/extractor_visualizer.cc b/oh_my_loam/visualizer/extractor_visualizer.cc index c148740..9823b01 100644 --- a/oh_my_loam/visualizer/extractor_visualizer.cc +++ b/oh_my_loam/visualizer/extractor_visualizer.cc @@ -11,13 +11,14 @@ void ExtractorVisualizer::Draw() { for (size_t i = 0; i < frame.features.size(); ++i) { const auto &feature = frame.features[i]; std::string id_suffix = std::to_string(i); - DrawPointCloud(feature.cloud_surf, BLUE, "cloud_surf" + id_suffix); + DrawPointCloud(feature.cloud_surf, BLUE, "cloud_surf" + id_suffix, + 5); DrawPointCloud(feature.cloud_flat_surf, CYAN, - "cloud_flat_surf" + id_suffix); + "cloud_flat_surf" + id_suffix, 7); DrawPointCloud(feature.cloud_corner, YELLOW, - "cloud_corner" + id_suffix); + "cloud_corner" + id_suffix, 5); DrawPointCloud(feature.cloud_sharp_corner, RED, - "cloud_sharp_corner" + id_suffix); + "cloud_sharp_corner" + id_suffix, 7); } };