diff --git a/common/log.h b/common/log.h index 2eb6c09..21e5b18 100644 --- a/common/log.h +++ b/common/log.h @@ -6,24 +6,41 @@ #include #include +// heere we define G3LOG instead of use LOG directly, since LOG macro in g3log +// is conflict with LOG macro in glog. Same for G3LOG_IF and G3CHECK +#define G3LOG(level) \ + if (!g3::logLevel(level)) { \ + } else \ + INTERNAL_LOG_MESSAGE(level).stream() + +#define G3LOG_IF(level, boolean_expression) \ + if (false == (boolean_expression) || !g3::logLevel(level)) { \ + } else \ + INTERNAL_LOG_MESSAGE(level).stream() + +#define G3CHECK(boolean_expression) \ + if (true == (boolean_expression)) { \ + } else \ + INTERNAL_CONTRACT_MESSAGE(#boolean_expression).stream() + const LEVELS ERROR{WARNING.value + 100, "ERROR"}; const LEVELS USER(ERROR.value + 100, "USER"); -#define ADEBUG LOG(DEBUG) -#define AINFO LOG(INFO) -#define AWARN LOG(WARNING) -#define AERROR LOG(ERROR) -#define AUSER LOG(USER) -#define AFATAL LOG(FATAL) +#define ADEBUG G3LOG(DEBUG) +#define AINFO G3LOG(INFO) +#define AWARN G3LOG(WARNING) +#define AERROR G3LOG(ERROR) +#define AUSER G3LOG(USER) +#define AFATAL G3LOG(FATAL) // LOG_IF -#define ADEBUG_IF(cond) LOG_IF(DEBUG, cond) -#define AINFO_IF(cond) LOG_IF(INFO, cond) -#define AWARN_IF(cond) LOG_IF(WARNING, cond) -#define AERROR_IF(cond) LOG_IF(ERROR, cond) -#define AUSER_IF(cond) LOG_IF(USER, cond) -#define AFATAL_IF(cond) LOG_IF(FATAL, cond) -#define ACHECK(cond) CHECK(cond) +#define ADEBUG_IF(cond) G3LOG_IF(DEBUG, cond) +#define AINFO_IF(cond) G3LOG_IF(INFO, cond) +#define AWARN_IF(cond) G3LOG_IF(WARNING, cond) +#define AERROR_IF(cond) G3LOG_IF(ERROR, cond) +#define AUSER_IF(cond) G3LOG_IF(USER, cond) +#define AFATAL_IF(cond) G3LOG_IF(FATAL, cond) +#define ACHECK(cond) G3CHECK(cond) namespace g3 { class CustomSink { diff --git a/common/pose.cc b/common/pose.cc index a25a33a..fe0ad14 100644 --- a/common/pose.cc +++ b/common/pose.cc @@ -3,7 +3,7 @@ namespace oh_my_loam { Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t) { - return pose_from.InterPolate(pose_to, t); + return pose_from.Interpolate(pose_to, t); } Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs) { diff --git a/common/pose.h b/common/pose.h index 9223135..39e8efd 100644 --- a/common/pose.h +++ b/common/pose.h @@ -20,9 +20,9 @@ class Pose3D { Pose3D(const double* const q, const double* const p) : q_(q), p_(p) {} Pose3D Inv() const { - auto q_inv = q_.inverse(); - auto p_inv = q_inv * p_; - return {q_inv, p_inv}; + Eigen::Quaterniond q_inv = q_.inverse(); + Eigen::Vector3d p_inv = q_inv * p_; + return {q_inv, -p_inv}; } Eigen::Vector3d Transform(const Eigen::Vector3d& vec) const { @@ -37,9 +37,9 @@ class Pose3D { 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; + pt_n.x = static_cast(vec.x()); + pt_n.y = static_cast(vec.y()); + pt_n.z = static_cast(vec.z()); return pt_n; } @@ -55,9 +55,9 @@ class Pose3D { 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; + Pose3D Interpolate(const Pose3D& pose_to, double t) const { + Eigen::Quaterniond q_interp = q_.slerp(t, pose_to.q_); + Eigen::Vector3d p_interp = (pose_to.p_ - p_) * t + p_; return {q_interp, p_interp}; } diff --git a/common/types.h b/common/types.h index 71970d8..2a2ab3c 100644 --- a/common/types.h +++ b/common/types.h @@ -46,7 +46,12 @@ using TCTPointCloudConstPtr = TCTPointCloud::ConstPtr; struct PointXYZT { PCL_ADD_POINT4D; - float time; + union EIGEN_ALIGN16 { + float time; + // make sure VoxelGrid can work with this custom point type: + // https://github.com/PointCloudLibrary/pcl/issues/2331 + float intensity; + }; PointXYZT() { x = y = z = 0.0f; diff --git a/configs/config.yaml b/configs/config.yaml index f48c5cd..5c65223 100644 --- a/configs/config.yaml +++ b/configs/config.yaml @@ -10,12 +10,12 @@ extractor_config: vis: true sharp_corner_point_num: 2 corner_point_num: 20 - flat_surf_point_num: 3 + 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 - downsample_voxel_size: 0.5 + downsample_voxel_size: 0.3 odometry_config: icp_iter_num : 2 diff --git a/src/extractor/base_extractor.cc b/src/extractor/base_extractor.cc index 88cbba2..fc693f6 100644 --- a/src/extractor/base_extractor.cc +++ b/src/extractor/base_extractor.cc @@ -47,8 +47,8 @@ void Extractor::Process(const PointCloud& cloud, FeaturePoints* const feature) { AINFO << "Time elapsed (ms): scan_split = " << std::setprecision(3) << time_split << ", curvature_compute = " << time_curv - time_split << ", type_assign = " << time_assign - time_curv - << ", store = " << time_store - time_assign - << ", TOTAL = " << time_store; + << ", store = " << time_store - time_assign; + AUSER << "Time elapsed (ms): whole extraction = " << time_store; if (is_vis_) Visualize(cloud, *feature); } @@ -181,15 +181,14 @@ 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); + TPointCloudPtr less_flat_surf_pts(new TPointCloud); for (const auto& pt : scan.points) { switch (pt.type) { case PointType::FLAT: feature->flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, pt.time); // no break: FLAT points are also LESS_FLAT case PointType::LESS_FLAT: - feature->less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, - pt.time); + less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, pt.time); break; case PointType::SHARP: feature->sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z, @@ -201,16 +200,14 @@ void Extractor::GenerateFeaturePoints(const TCTPointCloud& scan, break; default: // all the rest are also LESS_FLAT - feature->less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, - pt.time); + less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, pt.time); break; } } - // 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; + 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/helper/helper.h b/src/helper/helper.h index 089c958..3edc9ea 100644 --- a/src/helper/helper.h +++ b/src/helper/helper.h @@ -10,6 +10,30 @@ inline float GetTime(const TPoint& pt) { inline int GetScanId(const TPoint& pt) { return static_cast(pt.time); } +/** + * @brief Transform a lidar point to the start of the scan + * + * @param pose Relative pose, end scan time w.r.t. start scan time + * @param time Point time relative to the start time of the scan, \in [0, 1] + */ +void TransformToStart(const Pose3D& pose, const TPoint& pt_in, + TPoint* const pt_out) { + Pose3D pose_interp = Pose3D().Interpolate(pose, GetTime(pt_in)); + *pt_out = pose_interp * pt_in; +} + +/** + * @brief Transform a lidar point to the end of the scan + * + * @param pose Relative pose, end scan time w.r.t. start scan time + * @param time Point time relative to the start time of the scan, \in [0, 1] + */ +void TransformToEnd(const Pose3D& pose, const TPoint& pt_in, + TPoint* const pt_out) { + TransformToStart(pose, pt_in, pt_out); + *pt_out = pose.Inv() * (*pt_out); +} + struct PointLinePair { TPoint pt; struct Line { diff --git a/src/odometry/odometry.cc b/src/odometry/odometry.cc index 4b7f172..5344207 100644 --- a/src/odometry/odometry.cc +++ b/src/odometry/odometry.cc @@ -72,13 +72,15 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) { AWARN << "Pose increase: " << pose_curr2last_.ToString(); AWARN << "Pose after: " << pose_curr2world_.ToString(); UpdatePre(feature); - AINFO << "Time elapsed (ms): whole odometry = " << timer_whole.toc(); + AUSER << "Time elapsed (ms): whole odometry = " << timer_whole.toc(); } void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt, std::vector* const pairs, double dist_sq_thresh) const { - for (const auto& query_pt : src) { + for (const auto& q_pt : src) { + TPoint query_pt; + TransformToStart(pose_curr2last_, q_pt, &query_pt); std::vector indices; std::vector dists; kdtree_corn_pts_->nearestKSearch(query_pt, 1, indices, dists); @@ -111,7 +113,7 @@ void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt, } if (pt2_idx >= 0) { TPoint pt2 = tgt.points[pt2_idx]; - pairs->emplace_back(query_pt, pt1, pt2); + pairs->emplace_back(q_pt, pt1, pt2); } } } @@ -119,7 +121,9 @@ void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt, void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt, std::vector* const pairs, double dist_sq_thresh) const { - for (const auto& query_pt : src) { + for (const auto& q_pt : src) { + TPoint query_pt; + TransformToStart(pose_curr2last_, q_pt, &query_pt); std::vector indices; std::vector dists; kdtree_surf_pts_->nearestKSearch(query_pt, 1, indices, dists); @@ -134,7 +138,6 @@ void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt, 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; @@ -150,7 +153,6 @@ void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt, 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) << "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,14 +165,29 @@ void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt, } 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); + pairs->emplace_back(q_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; + if (surf_pts_pre_ == nullptr) { + surf_pts_pre_.reset(new TPointCloud); + } + if (corn_pts_pre_ == nullptr) { + corn_pts_pre_.reset(new TPointCloud); + } + + surf_pts_pre_->resize(feature.less_flat_surf_pts->size()); + for (size_t i = 0; i < feature.less_flat_surf_pts->size(); ++i) { + TransformToEnd(pose_curr2last_, feature.less_flat_surf_pts->points[i], + &surf_pts_pre_->points[i]); + } + corn_pts_pre_->resize(feature.less_sharp_corner_pts->size()); + for (size_t i = 0; i < feature.less_sharp_corner_pts->size(); ++i) { + TransformToEnd(pose_curr2last_, feature.less_sharp_corner_pts->points[i], + &corn_pts_pre_->points[i]); + } kdtree_surf_pts_->setInputCloud(surf_pts_pre_); kdtree_corn_pts_->setInputCloud(corn_pts_pre_); } diff --git a/src/odometry/odometry.h b/src/odometry/odometry.h index cba973c..77ee2e1 100644 --- a/src/odometry/odometry.h +++ b/src/odometry/odometry.h @@ -29,11 +29,11 @@ class Odometry { Pose3D pose_curr2world_; Pose3D pose_curr2last_; - TPointCloudPtr surf_pts_pre_; - TPointCloudPtr corn_pts_pre_; + TPointCloudPtr surf_pts_pre_{nullptr}; + TPointCloudPtr corn_pts_pre_{nullptr}; - pcl::KdTreeFLANN::Ptr kdtree_surf_pts_; - pcl::KdTreeFLANN::Ptr kdtree_corn_pts_; + pcl::KdTreeFLANN::Ptr kdtree_surf_pts_{nullptr}; + pcl::KdTreeFLANN::Ptr kdtree_corn_pts_{nullptr}; bool is_initialized = false; YAML::Node config_; diff --git a/src/visualizer/odometry_visualizer.cc b/src/visualizer/odometry_visualizer.cc new file mode 100644 index 0000000..f03770d --- /dev/null +++ b/src/visualizer/odometry_visualizer.cc @@ -0,0 +1,38 @@ +#include "Odometry_visualizer.h" + +namespace oh_my_loam { + +void OdometryVisualizer::Draw() { + auto frame = GetCurrentFrame(); + { // add raw point cloud + std::string id = "raw point cloud"; + DrawPointCloud(*frame.cloud, WHITE, id, viewer_.get()); + rendered_cloud_ids_.push_back(id); + } + { // add less_flat_surf_pts + std::string id = "less_flat_surf_pts"; + DrawPointCloud(*frame.feature_pts.less_flat_surf_pts, CYAN, id, + viewer_.get(), 5); + rendered_cloud_ids_.push_back(id); + } + { // add flat_surf_pts + std::string id = "flat_surf_pts"; + DrawPointCloud(*frame.feature_pts.flat_surf_pts, BLUE, id, viewer_.get(), + 7); + rendered_cloud_ids_.push_back(id); + } + { // add less_sharp_corner_pts + std::string id = "less_sharp_corner_pts"; + DrawPointCloud(*frame.feature_pts.less_sharp_corner_pts, PURPLE, id, + viewer_.get(), 5); + rendered_cloud_ids_.push_back(id); + } + { // add sharp_corner_pts + std::string id = "sharp_corner_pts"; + DrawPointCloud(*frame.feature_pts.sharp_corner_pts, RED, id, viewer_.get(), + 7); + rendered_cloud_ids_.push_back(id); + } +}; + +} // namespace oh_my_loam diff --git a/src/visualizer/odometry_visualizer.h b/src/visualizer/odometry_visualizer.h new file mode 100644 index 0000000..ea081aa --- /dev/null +++ b/src/visualizer/odometry_visualizer.h @@ -0,0 +1,22 @@ +#pragma once + +#include "base_visualizer.h" +#include "extractor/feature_points.h" + +namespace oh_my_loam { + +struct OdometryVisFrame : public VisFrame { + FeaturePoints feature_pts; +}; + +class OdometryVisualizer : public Visualizer { + public: + explicit OdometryVisualizer(const std::string &name = "OdometryVisualizer", + size_t max_history_size = 10) + : Visualizer(name, max_history_size) {} + + private: + void Draw() override; +}; + +} // namespace oh_my_loam