From 1b0919d1b76cf83ae8beecb279d271b10597298d Mon Sep 17 00:00:00 2001 From: feixyz10 Date: Tue, 2 Feb 2021 00:26:49 +0800 Subject: [PATCH] mapper compile ok --- common/math/fitting.h | 4 +- oh_my_loam/base/utils.cc | 13 ++++--- oh_my_loam/mapper/map.h | 2 + oh_my_loam/mapper/mapper.cc | 12 +++--- oh_my_loam/mapper/mapper.h | 10 ++--- oh_my_loam/odometer/odometer.cc | 2 +- oh_my_loam/odometer/odometer.h | 4 +- oh_my_loam/oh_my_loam.cc | 43 ++++++++------------- oh_my_loam/oh_my_loam.h | 6 +-- oh_my_loam/solver/cost_function.h | 3 +- oh_my_loam/visualizer/odometer_visualizer.h | 8 ++-- 11 files changed, 50 insertions(+), 57 deletions(-) diff --git a/common/math/fitting.h b/common/math/fitting.h index 96de221..3e69742 100644 --- a/common/math/fitting.h +++ b/common/math/fitting.h @@ -48,7 +48,7 @@ Eigen::Matrix FitLine3D(const pcl::PointCloud &cloud, * b^2 + c^2 = 1) */ template -Eigen::Vector3d FitPlane(const pcl::PointCloud &cloud, +Eigen::Vector4d FitPlane(const pcl::PointCloud &cloud, double *const score = nullptr) { Eigen::MatrixX3f data(cloud.size(), 3); size_t i = 0; @@ -56,7 +56,7 @@ Eigen::Vector3d FitPlane(const pcl::PointCloud &cloud, Eigen::RowVector3f centroid = data.colwise().mean(); Eigen::MatrixX3f data_centered = data.rowwise() - centroid; Eigen::JacobiSVD svd(data_centered, Eigen::ComputeThinV); - Eigen::Vector2f normal = svd.matrixV().col(2); + Eigen::Vector3f normal = svd.matrixV().col(2); float d = -centroid * normal; if (score) { *score = svd.singularValues()[1] * svd.singularValues()[1] / diff --git a/oh_my_loam/base/utils.cc b/oh_my_loam/base/utils.cc index e4a3f03..754dd97 100644 --- a/oh_my_loam/base/utils.cc +++ b/oh_my_loam/base/utils.cc @@ -1,28 +1,29 @@ -#include "oh_my_loam/base/helper.h" +#include "oh_my_loam/base/utils.h" #include "common/pcl/pcl_utils.h" namespace oh_my_loam { -void TransformToStart(const Pose3d &pose, const TPoint &pt_in, +void TransformToStart(const common::Pose3d &pose, const TPoint &pt_in, TPoint *const pt_out) { - Pose3d pose_interp = Pose3d().Interpolate(pose, GetTime(pt_in)); + common::Pose3d pose_interp = + common::Pose3d().Interpolate(pose, GetTime(pt_in)); common::TransformPoint(pose_interp, pt_in, pt_out); } -TPoint TransformToStart(const Pose3d &pose, const TPoint &pt_in) { +TPoint TransformToStart(const common::Pose3d &pose, const TPoint &pt_in) { TPoint pt_out; TransformToStart(pose, pt_in, &pt_out); return pt_out; } -void TransformToEnd(const Pose3d &pose, const TPoint &pt_in, +void TransformToEnd(const common::Pose3d &pose, const TPoint &pt_in, TPoint *const pt_out) { TransformToStart(pose, pt_in, pt_out); common::TransformPoint(pose.Inv(), *pt_out, pt_out); } -TPoint TransformToEnd(const Pose3d &pose, const TPoint &pt_in) { +TPoint TransformToEnd(const common::Pose3d &pose, const TPoint &pt_in) { TPoint pt_out; TransformToEnd(pose, pt_in, &pt_out); return pt_out; diff --git a/oh_my_loam/mapper/map.h b/oh_my_loam/mapper/map.h index 6ca4e28..a85e0ef 100644 --- a/oh_my_loam/mapper/map.h +++ b/oh_my_loam/mapper/map.h @@ -13,6 +13,8 @@ class Grid; struct Index { int k, j, i; + Index() = default; + Index(int k, int j, int i) : k(k), j(j), i(i) {} struct Comp { bool operator()(const Index &idx1, const Index &idx2) const { diff --git a/oh_my_loam/mapper/mapper.cc b/oh_my_loam/mapper/mapper.cc index 77a9e6d..8823ae1 100644 --- a/oh_my_loam/mapper/mapper.cc +++ b/oh_my_loam/mapper/mapper.cc @@ -173,18 +173,18 @@ void Mapper::AdjustMap(const TPoint ¢er) { } } -TPointCloudPtr Mapper::GetCornMapPoints() const { +TPointCloudPtr Mapper::GetMapCloudCorn() const { return corn_map_->GetAllPoints(); } -TPointCloudPtr Mapper::GetSurfMapPoints() const { +TPointCloudPtr Mapper::GetMapCloudSurf() const { return surf_map_->GetAllPoints(); } -TPointCloudPtr Mapper::GetMapPoints() const { +TPointCloudPtr Mapper::GetMapCloud() const { TPointCloudPtr map_points(new TPointCloud); - *map_points += *GetCornMapPoints(); - *map_points += *GetSurfMapPoints(); + *map_points += *GetMapCloudCorn(); + *map_points += *GetMapCloudSurf(); return map_points; } @@ -198,6 +198,4 @@ void Mapper::SetState(State state) { state_ = state; } -void Mapper::Visualize() {} - } // namespace oh_my_loam \ No newline at end of file diff --git a/oh_my_loam/mapper/mapper.h b/oh_my_loam/mapper/mapper.h index a860e07..f0aa277 100644 --- a/oh_my_loam/mapper/mapper.h +++ b/oh_my_loam/mapper/mapper.h @@ -25,11 +25,11 @@ class Mapper { const common::Pose3d &pose_curr2odom, common::Pose3d *const pose_curr2map); - TPointCloudPtr GetCornMapPoints() const; + TPointCloudPtr GetMapCloudCorn() const; - TPointCloudPtr GetSurfMapPoints() const; + TPointCloudPtr GetMapCloudSurf() const; - TPointCloudPtr GetMapPoints() const; + TPointCloudPtr GetMapCloud() const; void Reset(); @@ -64,8 +64,6 @@ class Mapper { const TCTPointCloudConstPtr &tgt, std::vector *const pairs) const; - void Visualize(); - YAML::Node config_; std::vector map_shape_, submap_shape_; @@ -74,8 +72,8 @@ class Mapper { std::unique_ptr surf_map_; mutable std::mutex state_mutex_; - common::Pose3d pose_odom2map_; State state_ = UN_INIT; + common::Pose3d pose_odom2map_; mutable std::unique_ptr thread_{nullptr}; diff --git a/oh_my_loam/odometer/odometer.cc b/oh_my_loam/odometer/odometer.cc index 3e3ea5a..e1ec3c7 100644 --- a/oh_my_loam/odometer/odometer.cc +++ b/oh_my_loam/odometer/odometer.cc @@ -23,7 +23,7 @@ void Odometer::Reset() { } void Odometer::Process(double timestamp, const std::vector &features, - Pose3d *const pose_out) { + common::Pose3d *const pose_out) { if (!is_initialized_) { UpdatePre(features); is_initialized_ = true; diff --git a/oh_my_loam/odometer/odometer.h b/oh_my_loam/odometer/odometer.h index 655469c..b6d30c5 100644 --- a/oh_my_loam/odometer/odometer.h +++ b/oh_my_loam/odometer/odometer.h @@ -18,11 +18,11 @@ class Odometer { void Process(double timestamp, const std::vector &features, common::Pose3d *const pose_out); - TPointCloudConstPtr cloud_corn() const { + TPointCloudConstPtr GetCloudCorn() const { return kdtree_corn_.getInputCloud(); } - TPointCloudConstPtr cloud_surf() const { + TPointCloudConstPtr GetCloudSurf() const { return kdtree_surf_.getInputCloud(); } diff --git a/oh_my_loam/oh_my_loam.cc b/oh_my_loam/oh_my_loam.cc index e1036bd..2f36f1a 100644 --- a/oh_my_loam/oh_my_loam.cc +++ b/oh_my_loam/oh_my_loam.cc @@ -2,8 +2,8 @@ #include +#include "common/common.h" #include "common/pcl/pcl_utils.h" -#include "oh_my_loam/base/helper.h" #include "oh_my_loam/extractor/extractor_VLP16.h" namespace oh_my_loam { @@ -33,6 +33,7 @@ bool OhMyLoam::Init() { } void OhMyLoam::Reset() { + AWARN << "OhMySlam RESET"; extractor_->Reset(); odometer_->Reset(); mapper_->Reset(); @@ -44,37 +45,27 @@ void OhMyLoam::Run(double timestamp, RemoveOutliers(*cloud_in, cloud.get()); std::vector features; extractor_->Process(timestamp, cloud, &features); - FusionOdometryMapping(); - auto pose_odom = - poses_curr2world_.empty() ? TimePose() : poses_curr2world_.back(); - odometer_->Process(timestamp, features, &pose_odom.pose); - poses_curr2world_.push_back(pose_odom); - const auto &cloud_corn = odometer_->cloud_corn(); - const auto &cloud_surf = odometer_->cloud_surf(); - - if (!pose_mapping_updated_) return; - mapping_thread_.reset(new std::thread(&OhMyLoam::MappingProcess, this, - timestamp, cloud_corn, cloud_surf)); - if (mapping_thread_->joinable()) mapping_thread_->detach(); -} - -void OhMyLoam::FusionOdometryMapping() { - std::lock_guard lock(mutex_); - TimePose pose_m = pose_mapping_; - pose_mapping_updated_ = false; - for (;;) { - } + common::Pose3d pose_curr2odom; + odometer_->Process(timestamp, features, &pose_curr2odom); + common::Pose3d pose_curr2map; + const auto &cloud_corn = odometer_->GetCloudCorn(); + const auto &cloud_surf = odometer_->GetCloudSurf(); + mapper_->Process(timestamp, cloud_corn, cloud_surf, pose_curr2odom, + &pose_curr2map); + poses_curr2odom_.push_back(pose_curr2odom); + poses_curr2world_.push_back(pose_curr2map); + if (is_vis_) Visualize(timestamp); } void OhMyLoam::Visualize(double timestamp) {} void OhMyLoam::RemoveOutliers(const common::PointCloud &cloud_in, common::PointCloud *const cloud_out) const { - common::RemovePoints(cloud_in, cloud_out, [&](const auto &pt) { - return !common::IsFinite(pt) || - common::DistanceSquare(pt) < - kPointMinDist * kPointMinDist; - }); + common::RemovePoints( + cloud_in, cloud_out, [&](const common::Point &pt) { + return !common::IsFinite(pt) || + common::DistanceSquare(pt) < kPointMinDist * kPointMinDist; + }); } } // namespace oh_my_loam \ No newline at end of file diff --git a/oh_my_loam/oh_my_loam.h b/oh_my_loam/oh_my_loam.h index 40ee88d..6620b09 100644 --- a/oh_my_loam/oh_my_loam.h +++ b/oh_my_loam/oh_my_loam.h @@ -20,8 +20,6 @@ class OhMyLoam { private: void Reset(); - void FusionOdometryMapping(); - void Visualize(double timestamp = 0.0); std::unique_ptr extractor_{nullptr}; @@ -34,11 +32,13 @@ class OhMyLoam { void RemoveOutliers(const common::PointCloud &cloud_in, common::PointCloud *const cloud_out) const; + std::vector poses_curr2odom_; std::vector poses_curr2world_; YAML::Node config_; + bool is_vis_ = false; - DISALLOW_COPY_AND_ASSIGN(OhMyLoam) + DISALLOW_COPY_AND_ASSIGN(OhMyLoam); }; } // namespace oh_my_loam \ No newline at end of file diff --git a/oh_my_loam/solver/cost_function.h b/oh_my_loam/solver/cost_function.h index cd3cc44..ff418da 100644 --- a/oh_my_loam/solver/cost_function.h +++ b/oh_my_loam/solver/cost_function.h @@ -174,7 +174,8 @@ bool PointPlaneCoeffCostFunction::operator()(const T *const r_quat, } Eigen::Matrix p0 = r * p + t; - residual[0] = coeff.topRows(3).transpose() * p + coeff[3]; + residual[0] = coeff.topRows(3).transpose() * p0; + residual[0] += coeff(3); return true; } diff --git a/oh_my_loam/visualizer/odometer_visualizer.h b/oh_my_loam/visualizer/odometer_visualizer.h index 3b87b02..148f7d1 100644 --- a/oh_my_loam/visualizer/odometer_visualizer.h +++ b/oh_my_loam/visualizer/odometer_visualizer.h @@ -1,7 +1,7 @@ #pragma once #include "common/visualizer/lidar_visualizer.h" -#include "oh_my_loam/base/helper.h" +#include "oh_my_loam/solver/cost_function.h" namespace oh_my_loam { @@ -23,9 +23,11 @@ class OdometerVisualizer : public common::LidarVisualizer { private: void Draw() override; - void DrawCorn(const Pose3d &pose, const std::vector &pairs); + void DrawCorn(const common::Pose3d &pose, + const std::vector &pairs); - void DrawSurf(const Pose3d &pose, const std::vector &pairs); + void DrawSurf(const common::Pose3d &pose, + const std::vector &pairs); void DrawTrajectory();