diff --git a/common/math/fitting.h b/common/math/fitting.h index e3017d6..9c2e756 100644 --- a/common/math/fitting.h +++ b/common/math/fitting.h @@ -1,5 +1,6 @@ #pragma once +#include #include "common/pcl/pcl_types.h" namespace common { @@ -15,11 +16,15 @@ Eigen::Vector3d FitLine2D(const pcl::PointCloud &cloud, for (const auto &p : cloud) data.row(i++) << p.x, p.y; Eigen::RowVector2f centroid = data.colwise().mean(); Eigen::MatrixX2f data_centered = data.rowwise() - centroid; - Eigen::JacobiSVD svd(data_centered, Eigen::ComputeFullV); - Eigen::Vector2f normal = svd.matrixV().col(1); - float c = -centroid * normal; - if (score) *score = svd.singularValues()[0] / svd.singularValues()[1]; - return {normal.x(), normal.y(), c}; + Eigen::Matrix2f cov_mat = data_centered.transpose() * data_centered; + Eigen::SelfAdjointEigenSolver solver(cov_mat); + Eigen::Vector2f normal = solver.eigenvectors().col(0); + double c = -centroid * normal; + Eigen::Vector3d coeffs(normal(0), normal(1), c); + if (score) { + *score = solver.eigenvalues()[1] / (solver.eigenvalues()[0] + 1e-7); + } + return coeffs; } /** @@ -34,13 +39,16 @@ Eigen::Matrix FitLine3D(const pcl::PointCloud &cloud, for (const auto &p : cloud) data.row(i++) << p.x, p.y, p.z; Eigen::RowVector3f centroid = data.colwise().mean(); Eigen::MatrixX3f data_centered = data.rowwise() - centroid; - Eigen::JacobiSVD svd(data_centered, Eigen::ComputeFullV); - Eigen::Vector3f direction = svd.matrixV().col(0); - Eigen::Matrix line_coeffs; - line_coeffs.topRows(3) = centroid.transpose().cast(); - line_coeffs.bottomRows(3) = direction.cast(); - if (score) *score = svd.singularValues()[0] / svd.singularValues()[1]; - return line_coeffs; + Eigen::Matrix3f cov_mat = data_centered.transpose() * data_centered; + Eigen::SelfAdjointEigenSolver solver(cov_mat); + Eigen::Vector3f direction = solver.eigenvectors().col(2); + Eigen::Matrix coeffs; + coeffs.topRows(3) = centroid.transpose().cast(); + coeffs.bottomRows(3) = direction.cast(); + if (score) { + *score = solver.eigenvalues()[2] / (solver.eigenvalues()[1] + 1e-7); + } + return coeffs; } /** @@ -55,14 +63,29 @@ Eigen::Vector4d FitPlane(const pcl::PointCloud &cloud, for (const auto &p : cloud) data.row(i++) << p.x, p.y, p.z; Eigen::RowVector3f centroid = data.colwise().mean(); Eigen::MatrixX3f data_centered = data.rowwise() - centroid; - Eigen::JacobiSVD svd(data_centered, Eigen::ComputeFullV); - Eigen::Vector3f normal = svd.matrixV().col(2); - float d = -centroid * normal; - if (score) { - *score = svd.singularValues()[1] * svd.singularValues()[1] / - (svd.singularValues()[0] * svd.singularValues()[2]); - } - return {normal.x(), normal.y(), normal.z(), d}; + Eigen::Matrix3f cov_mat = data_centered.transpose() * data_centered; + Eigen::SelfAdjointEigenSolver solver(cov_mat); + Eigen::Vector3f normal = solver.eigenvectors().col(0); + double d = -centroid * normal; + Eigen::Vector4d coeffs(normal(0), normal(1), normal(2), d); + if (score) + *score = solver.eigenvalues()[1] * solver.eigenvalues()[1] / + (solver.eigenvalues()[2] * solver.eigenvalues()[0] + 1e-7); + return coeffs; } +// template +// Eigen::Vector4d FitPlane(const pcl::PointCloud &cloud) { +// Eigen::MatrixX3f A(cloud.size(), 3); // NOLINT +// Eigen::VectorXf b(cloud.size()); +// b.setConstant(-1.0); +// size_t i = 0; +// for (const auto &p : cloud) { +// A.row(i++) << p.x, p.y, p.z; +// } +// Eigen::Vector3f sol = A.colPivHouseholderQr().solve(b); +// Eigen::Vector4d coeff(sol(0), sol(1), sol(2), 1.0); +// return coeff / sol.norm(); +// } + } // namespace common \ No newline at end of file diff --git a/oh_my_loam/configs/config.yaml b/oh_my_loam/configs/config.yaml index d2f8ab3..5ea2fd1 100644 --- a/oh_my_loam/configs/config.yaml +++ b/oh_my_loam/configs/config.yaml @@ -21,7 +21,7 @@ extractor_config: # configs for odometer odometer_config: vis: false - verbose: true + verbose: false nearby_scan_num: 1 min_correspondence_num: 10 icp_iter_num: 4 @@ -32,7 +32,7 @@ odometer_config: # configs for mapper mapper_config: vis: false - verbose: false + verbose: true map_shape: [3, 21, 21] map_step: 50 submap_shape: [1, 5, 5] diff --git a/oh_my_loam/mapper/mapper.cc b/oh_my_loam/mapper/mapper.cc index fdb94bc..48de0c2 100644 --- a/oh_my_loam/mapper/mapper.cc +++ b/oh_my_loam/mapper/mapper.cc @@ -16,8 +16,8 @@ using LineCoeff = Eigen::Matrix; bool Mapper::Init() { const auto &config = YAMLConfig::Instance()->config(); config_ = config["mapper_config"]; - is_vis_ = config["vis"].as() && config_["vis"].as(); - verbose_ = config_["vis"].as(); + is_vis_ = config_["vis"].as(); + verbose_ = config_["verbose"].as(); AINFO << "Mapping visualizer: " << (is_vis_ ? "ON" : "OFF"); map_shape_ = YAMLConfig::GetSeq(config_["map_shape"]); submap_shape_ = YAMLConfig::GetSeq(config_["submap_shape"]); @@ -68,7 +68,7 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn, kdtree_corn.setInputCloud(cloud_corn_map); pcl::KdTreeFLANN kdtree_surf; kdtree_surf.setInputCloud(cloud_surf_map); - std::vector pl_pairs; + std::vector pl_pairs; MatchCorn(kdtree_corn, cloud_corn_map, pose_curr2map, &pl_pairs); std::vector pp_pairs; MatchSurf(kdtree_surf, cloud_surf_map, pose_curr2map, &pp_pairs); @@ -83,7 +83,7 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn, } PoseSolver solver(pose_curr2map); for (const auto &pair : pl_pairs) { - solver.AddPointLinePair(pair, 1.0); + solver.AddPointLineCoeffPair(pair, 1.0); } for (const auto &pair : pp_pairs) { solver.AddPointPlaneCoeffPair(pair, 1.0); @@ -105,7 +105,7 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn, void Mapper::MatchCorn(const pcl::KdTreeFLANN &kdtree, const TPointCloudConstPtr &cloud_curr, const common::Pose3d &pose_curr2map, - std::vector *const pairs) const { + std::vector *const pairs) const { std::vector indices; std::vector dists; int nearest_neighbor_k = config_["nearest_neighbor_k"].as(); @@ -123,10 +123,7 @@ void Mapper::MatchCorn(const pcl::KdTreeFLANN &kdtree, double fit_score = 0.0; LineCoeff coeff = common::FitLine3D(neighbor_pts, &fit_score); if (fit_score < config_["min_line_fit_score"].as()) continue; - TPoint pt1(coeff[0], coeff[1], coeff[2], 0.0); - TPoint pt2(coeff[0] + 0.1 * coeff[3], coeff[1] + 0.1 * coeff[4], - coeff[2] + 0.1 * coeff[5], 0.0); - pairs->emplace_back(pt, pt1, pt2); + pairs->emplace_back(pt, coeff); } } diff --git a/oh_my_loam/mapper/mapper.h b/oh_my_loam/mapper/mapper.h index cc4c569..1b243c6 100644 --- a/oh_my_loam/mapper/mapper.h +++ b/oh_my_loam/mapper/mapper.h @@ -43,7 +43,7 @@ class Mapper { void MatchCorn(const pcl::KdTreeFLANN &kdtree, const TPointCloudConstPtr &cloud_curr, const common::Pose3d &pose_curr2map, - std::vector *const pairs) const; + std::vector *const pairs) const; void MatchSurf(const pcl::KdTreeFLANN &kdtree, const TPointCloudConstPtr &cloud_curr, @@ -60,13 +60,6 @@ class Mapper { const TPointCloudConstPtr &cloud_corn = nullptr, const TPointCloudConstPtr &cloud_surf = nullptr); - void MatchCorn(const TPointCloudConstPtr &src, - const TCTPointCloudConstPtr &tgt, - std::vector *const pairs) const; - - void MatchSurf(const TPointCloudConstPtr &src, - const TCTPointCloudConstPtr &tgt, - std::vector *const pairs) const; // map mutable std::mutex map_mutex_; std::unique_ptr corn_map_; diff --git a/oh_my_loam/solver/cost_function.h b/oh_my_loam/solver/cost_function.h index ff418da..2364b6c 100644 --- a/oh_my_loam/solver/cost_function.h +++ b/oh_my_loam/solver/cost_function.h @@ -39,6 +39,14 @@ struct PointPlanePair { : pt(pt), plane(pt1, pt2, pt3) {} }; +struct PointLineCoeffPair { + TPoint pt; + Eigen::Matrix line_coeff; + PointLineCoeffPair(const TPoint &pt, + const Eigen::Matrix &line_coeff) + : pt(pt), line_coeff(line_coeff) {} +}; + struct PointPlaneCoeffPair { TPoint pt; Eigen::Vector4d plane_coeff; @@ -87,6 +95,27 @@ class PointPlaneCostFunction { DISALLOW_COPY_AND_ASSIGN(PointPlaneCostFunction) }; +class PointLineCoeffCostFunction { + public: + PointLineCoeffCostFunction(const PointLineCoeffPair &pair, double time) + : pair_(pair), time_(time){}; + + template + bool operator()(const T *const r_quat, const T *const t_vec, + T *residual) const; + + static ceres::CostFunction *Create(const PointLineCoeffPair &pair, + double time) { + return new ceres::AutoDiffCostFunction( + new PointLineCoeffCostFunction(pair, time)); + } + + private: + PointLineCoeffPair pair_; + double time_; + DISALLOW_COPY_AND_ASSIGN(PointLineCoeffCostFunction) +}; + class PointPlaneCoeffCostFunction { public: PointPlaneCoeffCostFunction(const PointPlaneCoeffPair &pair, double time) @@ -159,6 +188,30 @@ bool PointPlaneCostFunction::operator()(const T *const r_quat, return true; } +template +bool PointLineCoeffCostFunction::operator()(const T *const r_quat, + const T *const t_vec, + T *residual) const { + Eigen::Matrix p(T(pair_.pt.x), T(pair_.pt.y), T(pair_.pt.z)); + Eigen::Matrix coeff = pair_.line_coeff.template cast(); + Eigen::Matrix p1 = coeff.topRows(3); + Eigen::Matrix dir = coeff.bottomRows(3); + + Eigen::Quaternion r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]); + Eigen::Matrix t(t_vec[0], t_vec[1], t_vec[2]); + if (time_ <= 1.0 - kEpsilon) { + r = Eigen::Quaternion::Identity().slerp(T(time_), r); + t = T(time_) * t; + } + Eigen::Matrix p0 = r * p + t; + + Eigen::Matrix cross = (p0 - p1).cross(dir); + residual[0] = cross[0]; + residual[1] = cross[1]; + residual[2] = cross[2]; + return true; +} + template bool PointPlaneCoeffCostFunction::operator()(const T *const r_quat, const T *const t_vec, diff --git a/oh_my_loam/solver/solver.cc b/oh_my_loam/solver/solver.cc index 571019e..8629236 100644 --- a/oh_my_loam/solver/solver.cc +++ b/oh_my_loam/solver/solver.cc @@ -46,6 +46,13 @@ void PoseSolver::AddPointPlanePair(const PointPlanePair &pair, double time) { problem_.AddResidualBlock(cost_function, loss_function_, r_quat_, t_vec_); } +void PoseSolver::AddPointLineCoeffPair(const PointLineCoeffPair &pair, + double time) { + ceres::CostFunction *cost_function = + PointLineCoeffCostFunction::Create(pair, time); + problem_.AddResidualBlock(cost_function, loss_function_, r_quat_, t_vec_); +} + void PoseSolver::AddPointPlaneCoeffPair(const PointPlaneCoeffPair &pair, double time) { ceres::CostFunction *cost_function = diff --git a/oh_my_loam/solver/solver.h b/oh_my_loam/solver/solver.h index e8c4306..0ff3b1c 100644 --- a/oh_my_loam/solver/solver.h +++ b/oh_my_loam/solver/solver.h @@ -13,6 +13,8 @@ class PoseSolver { void AddPointPlanePair(const PointPlanePair &pair, double time); + void AddPointLineCoeffPair(const PointLineCoeffPair &pair, double time); + void AddPointPlaneCoeffPair(const PointPlaneCoeffPair &pair, double time); bool Solve(int max_iter_num = 5, bool verbose = false,