diff --git a/CMakeLists.txt b/CMakeLists.txt index 0d4debf..056c1e5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,6 +54,8 @@ target_link_libraries(main g3log common oh_my_loam + # helper + # solver extractor visualizer # odometry diff --git a/common/types.h b/common/types.h index b49b376..d3eb8f1 100644 --- a/common/types.h +++ b/common/types.h @@ -64,8 +64,6 @@ struct PointXYZT { y = p.y; z = p.z; time = p.time; - curvature = p.curvature; - type = p.type; } EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 74b67b2..e750139 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,6 +1,8 @@ add_subdirectory(extractor) add_subdirectory(visualizer) # add_subdirectory(odometry) +add_subdirectory(helper) +add_subdirectory(solver) aux_source_directory(. SRC_FILES) diff --git a/src/helper/CMakeLists.txt b/src/helper/CMakeLists.txt new file mode 100644 index 0000000..877a5d1 --- /dev/null +++ b/src/helper/CMakeLists.txt @@ -0,0 +1,3 @@ +aux_source_directory(. SRC_FILES) + +add_library(helper SHARED ${SRC_FILES}) diff --git a/src/helper/helper.cc b/src/helper/helper.cc new file mode 100644 index 0000000..2641778 --- /dev/null +++ b/src/helper/helper.cc @@ -0,0 +1,13 @@ +#include "helper.h" + +namespace oh_my_loam { + +float GetTime(const TPoint& pt) { return pt.time - GetScanId(pt); } + +int GetScanId(const TPoint& pt) { return static_cast(pt.time); } + +double PointLinePair::DistPointToLine() const { return 0.0; } + +double PointPlanePair::DistPointToPlane() const { return 0.0; } + +} // oh_my_loam \ No newline at end of file diff --git a/src/helper/helper.h b/src/helper/helper.h new file mode 100644 index 0000000..20f853c --- /dev/null +++ b/src/helper/helper.h @@ -0,0 +1,36 @@ +#pragma once + +#include "common.h" + +namespace oh_my_loam { + +float GetTime(const TPoint& pt); + +int GetScanId(const TPoint& pt); + +struct PointLinePair { + TPoint pt; + struct Line { + TPoint pt1, pt2; + Line() = default; + Line(const TPoint& pt1, const TPoint& pt2) : pt1(pt1), pt2(pt2) {} + }; + Line line; + PointLinePair(const TPoint& pt, const Line& line) : pt(pt), line(line) {} + double DistPointToLine() const; +}; + +struct PointPlanePair { + TPoint pt; + struct Plane { + TPoint pt1, pt2, pt3; + Plane() = default; + Plane(const TPoint& pt1, const TPoint& pt2, const TPoint& pt3) + : pt1(pt1), pt2(pt2), pt3(pt3) {} + }; + Plane plane; + PointPlanePair(const TPoint& pt, const Plane& plane) : pt(pt), plane(plane) {} + double DistPointToPlane() const; +}; + +} // oh_my_loam \ No newline at end of file diff --git a/src/odometry/odometry.cc b/src/odometry/odometry.cc index 0ff1625..525824b 100644 --- a/src/odometry/odometry.cc +++ b/src/odometry/odometry.cc @@ -2,8 +2,15 @@ namespace oh_my_loam { +namespace { +int kNearbyScanNum = 2; +double kDistSquareThresh = 25; +} + bool Odometry::Init(const YAML::Node& config) { config_ = config; + kdtree_surf_pts.reset(new pcl::KdTreeFLANN); + kdtree_corn_pts.reset(new pcl::KdTreeFLANN); return true; } @@ -13,40 +20,115 @@ void Odometry::Process(const FeaturePoints& feature) { UpdatePre(feature); return; } - AssociateCornPoints(); - AssociateSurfPoints(); + std::vector pl_pairs; + std::vector pp_pairs; + AssociateCornPoints(*feature.sharp_corner_pts, corn_pts_pre_, pl_pairs, + kDistSquareThresh); + AssociateSurfPoints(*feature.flat_surf_pts, surf_pts_pre_, pp_pairs, + kDistSquareThresh); UpdatePre(feature); } -void Odometry::AssociateCornPoints( - const TPointCloud& sharp_corn_pts_curr, - std::vector* const pairs) const { - for (const pt : sharp_corn_pts_curr) { - Point p1, p2; - - pairs->emplace_back(pt, {p1, p2}); +void Odometry::AssociateCornPoints(const TPointCloud& src, + const TPointCloud& tgt, + std::vector* const pairs, + double dist_thresh) const { + kdtree_corn_pts_->setInputCloud(tgt); + for (const query_pt : src) { + std::vector indices; + std::vector dists; + kdtree_corn_pts->nearestKSearch(query_pt, 1, indices, dists); + if (dists[0] >= dist_thresh) continue; + Point pt1 = tgt.points[indices[0]]; + int pt2_idx = -1; + double min_dist_pt2_squre = dist_thresh * dist_thresh; + int query_pt_scan_id = GetScanId(query_pt); + for (int i = indices[0] + 1; i < tgt.size(); ++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); + if (dist_squre < min_dist_pt2_squre) { + pt2_idx = i; + min_dist_pt2_squre = dist_squre; + } + } + 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); + if (dist_squre < min_dist_pt2_squre) { + pt2_idx = i; + min_dist_pt2_squre = dist_squre; + } + } + if (pt2_idx >= 0) { + TPoint pt2 = tgt.points[pt2_idx]; + pairs->emplace_back(query_pt, {pt1, pt2}); + } } } -void Odometry::AssociateSurfPoints( - const TPointCloud& flat_surf_pts_curr, - std::vector* const pairs) const { - for (const pt : flat_surf_pts_curr) { - Point p1, p2, p3; - pairs->emplace_back(pt, {p1, p2, p3}); +void Odometry::AssociateSurfPoints(const TPointCloud& src, + const TPointCloud& tgt, + std::vector* const pairs, + double dist_thresh) const { + kdtree_surf_pts_->setInputCloud(tgt); + for (const query_pt : src) { + std::vector indices; + std::vector dists; + kdtree_corn_pts->nearestKSearch(query_pt, 1, indices, dists); + if (dists[0] >= dist_thresh) continue; + Point 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; + int query_pt_scan_id = GetScanId(query_pt); + for (int i = indices[0] + 1; i < 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); + if (scan_id == query_pt_scan_id && dist_squre < min_dist_pt2_squre) { + pt2_idx = i; + min_dist_pt2_squre = dist_squre; + } + if (scan_id > query_pt_scan_id && dist_squre < min_dist_pt2_squre) { + pt3_idx = i; + min_dist_pt3_squre = dist_squre; + } + if (pt2_idx == -1 && scan_id > query_pt_scan_id) break; + } + 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); + if (scan_id == query_pt_scan_id && dist_squre < min_dist_pt2_squre) { + pt2_idx = i; + min_dist_pt2_squre = dist_squre; + } + if (scan_id < query_pt_scan_id && dist_squre < min_dist_pt2_squre) { + pt3_idx = i; + min_dist_pt3_squre = dist_squre; + } + if (pt2_idx == -1 && scan_id < query_pt_scan_id) break; + } + if (pt2_idx >= 0 && pt3_idx >= 0) { + TPoint pt2 = tgt.points[pt2_idx], pt3 = tgt.points[pt3_idx]; + pairs->emplace_back(query_pt, {pt1, pt2, pt3}); + } } } 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_); } -float GetTime(const TPoint& pt) { return pt.time - GetScanId(pt); } - -int GetScanId(const TPoint& pt) { return static_cast(pt.time); } - } // namespace oh_my_loam diff --git a/src/odometry/odometry.h b/src/odometry/odometry.h index d5bfc86..efa0cd4 100644 --- a/src/odometry/odometry.h +++ b/src/odometry/odometry.h @@ -5,35 +5,10 @@ #include #include "common.h" +#include "helper/helper.h" namespace oh_my_loam { -float GetTime(const TPoint& pt); -int GetScanId(const TPoint& pt); - -struct PointLinePair { - Point pt; - struct Line { - Point pt1, pt2; - Line(const Point& pt1, const Point& pt2) : pt1(pt1), pt2(pt2) {} - }; - Line line; - PointLinePair(const Point& pt, const Line& line) : pt(pt), line(line) {} - double PointToLineDist() { return 0.0; } -}; - -struct PointPlanePair { - Point pt; - struct Plane { - Point pt1, pt2, pt3; - Plane(const Point& pt1, const Point& pt2, const Point& pt3) - : pt1(pt1), pt2(pt2), pt3(pt3) {} - }; - Plane plane; - PointLinePair(const Point& pt, const Plane& plane) : pt(pt), plane(plane) {} - double PointToPlaneDist() { return 0.0; } -}; - // frame to frame lidar odometry class Odometry { public: @@ -45,21 +20,23 @@ class Odometry { protected: void UpdatePre(const FeaturePoints& feature); - void AssociateCornPoints(const TPointCloud& sharp_corn_pts_curr, - std::vector* const pairs) const; + void AssociateCornPoints(const TPointCloud& src, const TPointCloud& tgt, + std::vector* const pairs, + double dist_thresh) const; - void AssociateSurfPoints(const TPointCloud& flat_surf_pts_curr, - std::vector* const pairs) const; - - YAML::Node config_; - - bool is_initialized = false; + void AssociateSurfPoints(const TPointCloud& src, const TPointCloud& tgt, + std::vector* const pairs, + double dist_thresh) const; TPointCloudPtr surf_pts_pre_; TPointCloudPtr corn_pts_pre_; - pcl::KdTreeFLANN::Ptr kdtree_surf_pts(new pcl::KdTreeFLANN); - pcl::KdTreeFLANN::Ptr kdtree_corn_pts(new pcl::KdTreeFLANN); + pcl::KdTreeFLANN::Ptr kdtree_surf_pts; + pcl::KdTreeFLANN::Ptr kdtree_corn_pts; + + bool is_initialized = false; + + YAML::Node config_; }; } // namespace oh_my_loam diff --git a/src/solver/CMakeLists.txt b/src/solver/CMakeLists.txt new file mode 100644 index 0000000..8cb2180 --- /dev/null +++ b/src/solver/CMakeLists.txt @@ -0,0 +1,3 @@ +aux_source_directory(. SRC_FILES) + +add_library(solver SHARED ${SRC_FILES}) diff --git a/src/solver/solver.cc b/src/solver/solver.cc new file mode 100644 index 0000000..e50a28b --- /dev/null +++ b/src/solver/solver.cc @@ -0,0 +1,7 @@ +#include "solver.h" + +namespace oh_my_loam { + +// + +} // oh_my_loam \ No newline at end of file diff --git a/src/solver/solver.h b/src/solver/solver.h new file mode 100644 index 0000000..582fffd --- /dev/null +++ b/src/solver/solver.h @@ -0,0 +1,19 @@ +#pragma once + +#include "ceres/ceres.h" + +namespace oh_my_loam { + +class Solver { + public: + Solver() = default; + + void SetInitialGuess(double* param_q, double* param_p); + + protected: + std::unique_ptr problem_; + ceres::LocalParameterization* parameterization_; + ceres::LossFunction* loss_function_; +}; + +} // oh_my_loam \ No newline at end of file