From 69135ebe4859915744ce72a953703c50d62ea549 Mon Sep 17 00:00:00 2001 From: feixyz10 Date: Wed, 28 Oct 2020 02:09:59 +0800 Subject: [PATCH] odometry: writing... --- CMakeLists.txt | 1 + common/pose.cc | 9 +++++ common/pose.h | 62 ++++++++++++++++++++++++++++++ common/types.h | 56 ++++++++++++++++++++++----- common/utils.h | 34 ++++++----------- src/CMakeLists.txt | 1 + src/extractor/base_extractor.cc | 16 ++++---- src/extractor/base_extractor.h | 12 +++--- src/odometry/CMakeLists.txt | 3 ++ src/odometry/odometry.cc | 52 +++++++++++++++++++++++++ src/odometry/odometry.h | 65 ++++++++++++++++++++++++++++++++ src/oh_my_loam.cc | 2 +- src/visualizer/base_visualizer.h | 3 +- src/visualizer/utils.h | 31 +++++++++++++++ 14 files changed, 300 insertions(+), 47 deletions(-) create mode 100644 common/pose.cc create mode 100644 common/pose.h create mode 100644 src/odometry/CMakeLists.txt create mode 100644 src/odometry/odometry.cc create mode 100644 src/odometry/odometry.h create mode 100644 src/visualizer/utils.h diff --git a/CMakeLists.txt b/CMakeLists.txt index ad7662c..0d4debf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -56,4 +56,5 @@ target_link_libraries(main oh_my_loam extractor visualizer + # odometry ) diff --git a/common/pose.cc b/common/pose.cc new file mode 100644 index 0000000..d682c71 --- /dev/null +++ b/common/pose.cc @@ -0,0 +1,9 @@ +#include + +namespace oh_my_loam { + +Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t) { + return pose_from.InterPolate(pose_to, t); +} + +} // namespace oh_my_loam diff --git a/common/pose.h b/common/pose.h new file mode 100644 index 0000000..d639521 --- /dev/null +++ b/common/pose.h @@ -0,0 +1,62 @@ +#pragma once + +#include + +namespace oh_my_loam { + +class Pose3D { + public: + Pose3D() { + q_.setIdentity(); + p_.setZero(); + }; + + Pose3D(const Eigen::Quaterniond& q, const Eigen::Vector3d& p) + : q_(q), p_(p) {} + + Pose3D(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& p) { + q_ = Eigen::Quaterniond(r_mat); + p_ = p; + } + + Pose3D Inv() const { + auto q_inv = q_.inverse(); + auto p_inv = q_inv * p_; + return {q_inv, p_inv}; + } + + Eigen::Vector3d Transform(const Eigen::Vector3d& vec) const { + return q_ * vec + p_; + } + + template + PointT Transform(const PointT& pt) const { + PointT pt_n = pt; + Eigen::Vector3d vec = Transform(Eigen::Vector3d(pt.x, pt.y, pt.z)); + pt_n.x = vec.x; + pt_n.y = vec.y; + pt_n.z = vec.z; + return pt_n; + } + + Eigen::Vector3d Translate(const Eigen::Vector3d& vec) const { + return vec + p_; + } + + Eigen::Vector3d Rotate(const Eigen::Vector3d& vec) const { return q_ * vec; } + + // Spherical linear interpolation to `pose_to`, `t` belongs [0, 1] + Pose3D InterPolate(const Pose3D& pose_to, double t) const { + auto q_interp = q_.slerp(t, pose_to.q_); + auto p_interp = (pose_to.p_ - p_) * t; + return {q_interp, p_interp}; + } + + protected: + Eigen::Quaterniond q_; // orientation + Eigen::Vector3d p_; // position +}; + +Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t); + +} // namespace oh_my_loam diff --git a/common/types.h b/common/types.h index a19c9c3..b49b376 100644 --- a/common/types.h +++ b/common/types.h @@ -28,17 +28,48 @@ enum class PointType { SHARP = 2, }; +struct PointXYZT; +using TPoint = PointXYZT; +using TPointCloud = pcl::PointCloud; +using TPointCloudPtr = TPointCloud::Ptr; +using TPointCloudConstPtr = TPointCloud::ConstPtr; + struct PointXYZTCT; +using TCTPoint = PointXYZTCT; +using TCTPointCloud = pcl::PointCloud; +using TCTPointCloudPtr = TCTPointCloud::Ptr; +using TCTPointCloudConstPtr = TCTPointCloud::ConstPtr; -using IPoint = PointXYZTCT; -using IPointCloud = pcl::PointCloud; -using IPointCloudPtr = IPointCloud::Ptr; -using IPointCloudConstPtr = IPointCloud::ConstPtr; +struct PointXYZT { + PCL_ADD_POINT4D; + float time; -using PCLVisualizer = pcl::visualization::PCLVisualizer; -#define PCLColorHandlerCustom pcl::visualization::PointCloudColorHandlerCustom -#define PCLColorHandlerGenericField \ - pcl::visualization::PointCloudColorHandlerGenericField + PointXYZT() { + x = y = z = 0.0f; + time = 0.0f; + } + + PointXYZT(float x, float y, float z, float time = 0.0f) + : x(x), y(y), z(z), time(time) {} + + PointXYZT(const Point& p) { + x = p.x; + y = p.y; + z = p.z; + time = 0.0f; + } + + PointXYZT(const PointXYZT& p) { + x = p.x; + y = p.y; + z = p.z; + time = p.time; + curvature = p.curvature; + type = p.type; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; struct PointXYZTCT { PCL_ADD_POINT4D; @@ -87,6 +118,14 @@ struct PointXYZTCT { } // namespace oh_my_loam // clang-format off +POINT_CLOUD_REGISTER_POINT_STRUCT( + oh_my_loam::PointXYZT, + (float, x, x) + (float, y, y) + (float, z, z) + (float, time, time) +) + POINT_CLOUD_REGISTER_POINT_STRUCT( oh_my_loam::PointXYZTCT, (float, x, x) @@ -94,6 +133,5 @@ POINT_CLOUD_REGISTER_POINT_STRUCT( (float, z, z) (float, time, time) (float, curvature, curvature) - // (int8_t, type, type) ) // clang-format on \ No newline at end of file diff --git a/common/utils.h b/common/utils.h index 13185b8..8c11676 100644 --- a/common/utils.h +++ b/common/utils.h @@ -11,9 +11,20 @@ inline double DistanceSqure(const PointT& pt) { return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z; } +template +inline double DistanceSqure(const PointT& pt1, const PointT& 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); +} + template inline double Distance(const PointT& pt) { - return std::sqrt(pt.x * pt.x + pt.y * pt.y + pt.z * pt.z); + return std::sqrt(DistanceSqure(pt)); +} + +template +inline double Distance(const PointT& pt1, const PointT& pt2) { + return std::sqrt(DistanceSqure(pt1, pt2)); } template @@ -28,27 +39,6 @@ double NormalizeAngle(double ang); const std::vector Range(int begin, int end, int step = 1); const std::vector Range(int end); // [0, end) -template -void DrawPointCloud(const pcl::PointCloud& cloud, const Color& color, - const std::string& id, PCLVisualizer* const viewer, - int pt_size = 3) { - PCLColorHandlerCustom color_handler(cloud.makeShared(), color.r, - color.g, color.b); - viewer->addPointCloud(cloud.makeShared(), color_handler, id); - viewer->setPointCloudRenderingProperties( - pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id); -} - -template -void DrawPointCloud(const pcl::PointCloud& cloud, - const std::string& field, const std::string& id, - PCLVisualizer* const viewer, int pt_size = 3) { - PCLColorHandlerGenericField color_handler(cloud.makeShared(), field); - viewer->addPointCloud(cloud.makeShared(), color_handler, id); - viewer->setPointCloudRenderingProperties( - pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id); -} - template void RemovePointsIf(const pcl::PointCloud& cloud_in, pcl::PointCloud* const cloud_out, diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 32b7833..74b67b2 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -1,5 +1,6 @@ add_subdirectory(extractor) add_subdirectory(visualizer) +# add_subdirectory(odometry) aux_source_directory(. SRC_FILES) diff --git a/src/extractor/base_extractor.cc b/src/extractor/base_extractor.cc index c4d68d4..f6c3ce2 100644 --- a/src/extractor/base_extractor.cc +++ b/src/extractor/base_extractor.cc @@ -17,7 +17,7 @@ bool Extractor::Init(const YAML::Node& config) { return true; } -void Extractor::Extract(const PointCloud& cloud, FeaturePoints* const feature) { +void Extractor::Process(const PointCloud& cloud, FeaturePoints* const feature) { if (cloud.size() < config_["min_point_num"].as()) { AWARN << "Too few points ( < " << config_["min_point_num"].as() << " ) after remove: " << cloud.size(); @@ -25,7 +25,7 @@ void Extractor::Extract(const PointCloud& cloud, FeaturePoints* const feature) { } TicToc timer; // split point cloud int scans - std::vector scans; + std::vector scans; SplitScan(cloud, &scans); double time_split = timer.toc(); // compute curvature for each point in each scan @@ -62,7 +62,7 @@ void Extractor::Extract(const PointCloud& cloud, FeaturePoints* const feature) { } void Extractor::SplitScan(const PointCloud& cloud, - std::vector* const scans) const { + std::vector* const scans) const { scans->resize(num_scans_); double yaw_start = -atan2(cloud.points[0].y, cloud.points[0].x); bool half_passed = false; @@ -82,7 +82,7 @@ void Extractor::SplitScan(const PointCloud& cloud, } // -void Extractor::ComputePointCurvature(IPointCloud* const scan, +void Extractor::ComputePointCurvature(TCTPointCloud* const scan, bool remove_nan) const { if (scan->size() < 20) return; auto& pts = scan->points; @@ -98,12 +98,12 @@ void Extractor::ComputePointCurvature(IPointCloud* const scan, pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z; pts[i].curvature = std::sqrt(dx * dx + dy * dy + dz * dz); } - RemovePointsIf(*scan, scan, [](const IPoint& pt) { + RemovePointsIf(*scan, scan, [](const TCTPoint& pt) { return !std::isfinite(pt.curvature); }); } -void Extractor::AssignPointType(IPointCloud* const scan) const { +void Extractor::AssignPointType(TCTPointCloud* const scan) const { int pt_num = scan->size(); ACHECK(pt_num >= kScanSegNum); int seg_pt_num = (pt_num - 1) / kScanSegNum + 1; @@ -163,7 +163,7 @@ void Extractor::AssignPointType(IPointCloud* const scan) const { } } -void Extractor::SetNeighborsPicked(const IPointCloud& scan, size_t ix, +void Extractor::SetNeighborsPicked(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; @@ -187,7 +187,7 @@ void Extractor::SetNeighborsPicked(const IPointCloud& scan, size_t ix, } } -void Extractor::StoreToFeaturePoints(const IPointCloud& scan, +void Extractor::StoreToFeaturePoints(const TCTPointCloud& scan, FeaturePoints* const feature) const { for (const auto& pt : scan.points) { switch (pt.type) { diff --git a/src/extractor/base_extractor.h b/src/extractor/base_extractor.h index 103320c..0a1c6d7 100644 --- a/src/extractor/base_extractor.h +++ b/src/extractor/base_extractor.h @@ -11,7 +11,7 @@ class Extractor { virtual ~Extractor() = default; bool Init(const YAML::Node& config); - void Extract(const PointCloud& cloud, FeaturePoints* const feature); + void Process(const PointCloud& cloud, FeaturePoints* const feature); int num_scans() const { return num_scans_; } @@ -27,17 +27,17 @@ class Extractor { double timestamp = std::nan("")); void SplitScan(const PointCloud& cloud, - std::vector* const scans) const; + std::vector* const scans) const; - void ComputePointCurvature(IPointCloud* const scan, + void ComputePointCurvature(TCTPointCloud* const scan, bool remove_nan = true) const; - void AssignPointType(IPointCloud* const scan) const; + void AssignPointType(TCTPointCloud* const scan) const; - void SetNeighborsPicked(const IPointCloud& scan, size_t ix, + void SetNeighborsPicked(const TCTPointCloud& scan, size_t ix, std::vector* const picked) const; - void StoreToFeaturePoints(const IPointCloud& scan, + void StoreToFeaturePoints(const TCTPointCloud& scan, FeaturePoints* const feature) const; bool is_vis_ = false; diff --git a/src/odometry/CMakeLists.txt b/src/odometry/CMakeLists.txt new file mode 100644 index 0000000..e3f9472 --- /dev/null +++ b/src/odometry/CMakeLists.txt @@ -0,0 +1,3 @@ +aux_source_directory(. SRC_FILES) + +add_library(odometry SHARED ${SRC_FILES}) diff --git a/src/odometry/odometry.cc b/src/odometry/odometry.cc new file mode 100644 index 0000000..0ff1625 --- /dev/null +++ b/src/odometry/odometry.cc @@ -0,0 +1,52 @@ +#include "odometry.h" + +namespace oh_my_loam { + +bool Odometry::Init(const YAML::Node& config) { + config_ = config; + return true; +} + +void Odometry::Process(const FeaturePoints& feature) { + if (!is_initialized) { + is_initialized = true; + UpdatePre(feature); + return; + } + AssociateCornPoints(); + AssociateSurfPoints(); + + 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::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::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 new file mode 100644 index 0000000..d5bfc86 --- /dev/null +++ b/src/odometry/odometry.h @@ -0,0 +1,65 @@ +#pragma once + +#include + +#include + +#include "common.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: + Odometry() = default; + + bool Init(const YAML::Node& config); + + void Process(const FeaturePoints& feature); + + protected: + void UpdatePre(const FeaturePoints& feature); + void AssociateCornPoints(const TPointCloud& sharp_corn_pts_curr, + std::vector* const pairs) const; + + void AssociateSurfPoints(const TPointCloud& flat_surf_pts_curr, + std::vector* const pairs) const; + + YAML::Node config_; + + bool is_initialized = false; + + 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); +}; + +} // namespace oh_my_loam diff --git a/src/oh_my_loam.cc b/src/oh_my_loam.cc index 777abac..cacf540 100644 --- a/src/oh_my_loam.cc +++ b/src/oh_my_loam.cc @@ -24,7 +24,7 @@ void OhMyLoam::Run(const PointCloud& cloud_in, double timestamp) { ADEBUG << "After remove, point num: " << cloud_in.size() << " -> " << cloud->size(); FeaturePoints feature_points; - extractor_->Extract(*cloud, &feature_points); + extractor_->Process(*cloud, &feature_points); } void OhMyLoam::RemoveOutliers(const PointCloud& cloud_in, diff --git a/src/visualizer/base_visualizer.h b/src/visualizer/base_visualizer.h index bd585b6..5d1b087 100644 --- a/src/visualizer/base_visualizer.h +++ b/src/visualizer/base_visualizer.h @@ -10,6 +10,7 @@ #include #include "common.h" +#include "utils.h" namespace oh_my_loam { @@ -77,7 +78,7 @@ class Visualizer { * @brief Draw objects. This method should be overrided for customization. * * virtual void Draw() { - * VisFrame frame* = GetCurrentFrame(); + * auto frame = GetCurrentFrame(); * { // Update cloud * std::string id = "point cloud"; * DrawPointCloud(*frame.cloud, {255, 255, 255}, id, viewer_.get()); diff --git a/src/visualizer/utils.h b/src/visualizer/utils.h new file mode 100644 index 0000000..a8471ab --- /dev/null +++ b/src/visualizer/utils.h @@ -0,0 +1,31 @@ +#pragma once + +namespace oh_my_loam { + +using PCLVisualizer = pcl::visualization::PCLVisualizer; +#define PCLColorHandlerCustom pcl::visualization::PointCloudColorHandlerCustom +#define PCLColorHandlerGenericField \ + pcl::visualization::PointCloudColorHandlerGenericField + +template +void DrawPointCloud(const pcl::PointCloud& cloud, const Color& color, + const std::string& id, PCLVisualizer* const viewer, + int pt_size = 3) { + PCLColorHandlerCustom color_handler(cloud.makeShared(), color.r, + color.g, color.b); + viewer->addPointCloud(cloud.makeShared(), color_handler, id); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id); +} + +template +void DrawPointCloud(const pcl::PointCloud& cloud, + const std::string& field, const std::string& id, + PCLVisualizer* const viewer, int pt_size = 3) { + PCLColorHandlerGenericField color_handler(cloud.makeShared(), field); + viewer->addPointCloud(cloud.makeShared(), color_handler, id); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id); +} + +} // namespace oh_my_loam \ No newline at end of file