12345qiupeng
/
oh_my_loam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

opt fitting, cost_function

main
feixyz 2021-02-03 20:50:11 +08:00
parent 08c1a9999b
commit 232d22f783
7 changed files with 114 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
common/math/fitting.h
View File

@ -1,5 +1,6 @@
#pragma once
#include <eigen3/Eigen/Dense>
#include "common/pcl/pcl_types.h"
namespace common {
@ -15,11 +16,15 @@ Eigen::Vector3d FitLine2D(const pcl::PointCloud<PT> &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<Eigen::MatrixX2f> 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<Eigen::Matrix2f> 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<double, 6, 1> FitLine3D(const pcl::PointCloud<PT> &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<Eigen::MatrixX3f> svd(data_centered, Eigen::ComputeFullV);
Eigen::Vector3f direction = svd.matrixV().col(0);
Eigen::Matrix<double, 6, 1> line_coeffs;
line_coeffs.topRows(3) = centroid.transpose().cast<double>();
line_coeffs.bottomRows(3) = direction.cast<double>();
if (score) *score = svd.singularValues()[0] / svd.singularValues()[1];
return line_coeffs;
Eigen::Matrix3f cov_mat = data_centered.transpose() * data_centered;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(cov_mat);
Eigen::Vector3f direction = solver.eigenvectors().col(2);
Eigen::Matrix<double, 6, 1> coeffs;
coeffs.topRows(3) = centroid.transpose().cast<double>();
coeffs.bottomRows(3) = direction.cast<double>();
if (score) {
*score = solver.eigenvalues()[2] / (solver.eigenvalues()[1] + 1e-7);
}
return coeffs;
}
/**
@ -55,14 +63,29 @@ Eigen::Vector4d FitPlane(const pcl::PointCloud<PT> &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<Eigen::MatrixX3f> 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<Eigen::Matrix3f> 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 <typename PT>
// Eigen::Vector4d FitPlane(const pcl::PointCloud<PT> &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

4
oh_my_loam/configs/config.yaml
View File

@ -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]

15
oh_my_loam/mapper/mapper.cc
View File

@ -16,8 +16,8 @@ using LineCoeff = Eigen::Matrix<double, 6, 1>;
bool Mapper::Init() {
const auto &config = YAMLConfig::Instance()->config();
config_ = config["mapper_config"];
is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
verbose_ = config_["vis"].as<bool>();
is_vis_ = config_["vis"].as<bool>();
verbose_ = config_["verbose"].as<bool>();
AINFO << "Mapping visualizer: " << (is_vis_ ? "ON" : "OFF");
map_shape_ = YAMLConfig::GetSeq<int>(config_["map_shape"]);
submap_shape_ = YAMLConfig::GetSeq<int>(config_["submap_shape"]);
@ -68,7 +68,7 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
kdtree_corn.setInputCloud(cloud_corn_map);
pcl::KdTreeFLANN<TPoint> kdtree_surf;
kdtree_surf.setInputCloud(cloud_surf_map);
std::vector<PointLinePair> pl_pairs;
std::vector<PointLineCoeffPair> pl_pairs;
MatchCorn(kdtree_corn, cloud_corn_map, pose_curr2map, &pl_pairs);
std::vector<PointPlaneCoeffPair> 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<TPoint> &kdtree,
const TPointCloudConstPtr &cloud_curr,
const common::Pose3d &pose_curr2map,
std::vector<PointLinePair> *const pairs) const {
std::vector<PointLineCoeffPair> *const pairs) const {
std::vector<int> indices;
std::vector<float> dists;
int nearest_neighbor_k = config_["nearest_neighbor_k"].as<int>();
@ -123,10 +123,7 @@ void Mapper::MatchCorn(const pcl::KdTreeFLANN<TPoint> &kdtree,
double fit_score = 0.0;
LineCoeff coeff = common::FitLine3D(neighbor_pts, &fit_score);
if (fit_score < config_["min_line_fit_score"].as<double>()) 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);
}
}

9
oh_my_loam/mapper/mapper.h
View File

@ -43,7 +43,7 @@ class Mapper {
void MatchCorn(const pcl::KdTreeFLANN<TPoint> &kdtree,
const TPointCloudConstPtr &cloud_curr,
const common::Pose3d &pose_curr2map,
std::vector<PointLinePair> *const pairs) const;
std::vector<PointLineCoeffPair> *const pairs) const;
void MatchSurf(const pcl::KdTreeFLANN<TPoint> &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<PointLinePair> *const pairs) const;
void MatchSurf(const TPointCloudConstPtr &src,
const TCTPointCloudConstPtr &tgt,
std::vector<PointLinePair> *const pairs) const;
// map
mutable std::mutex map_mutex_;
std::unique_ptr<Map> corn_map_;

53
oh_my_loam/solver/cost_function.h
View File

@ -39,6 +39,14 @@ struct PointPlanePair {
: pt(pt), plane(pt1, pt2, pt3) {}
};
struct PointLineCoeffPair {
TPoint pt;
Eigen::Matrix<double, 6, 1> line_coeff;
PointLineCoeffPair(const TPoint &pt,
const Eigen::Matrix<double, 6, 1> &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 <typename T>
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<PointLineCoeffCostFunction, 1, 4, 3>(
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 <typename T>
bool PointLineCoeffCostFunction::operator()(const T *const r_quat,
const T *const t_vec,
T *residual) const {
Eigen::Matrix<T, 3, 1> p(T(pair_.pt.x), T(pair_.pt.y), T(pair_.pt.z));
Eigen::Matrix<T, 6, 1> coeff = pair_.line_coeff.template cast<T>();
Eigen::Matrix<T, 3, 1> p1 = coeff.topRows(3);
Eigen::Matrix<T, 3, 1> dir = coeff.bottomRows(3);
Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
if (time_ <= 1.0 - kEpsilon) {
r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
t = T(time_) * t;
}
Eigen::Matrix<T, 3, 1> p0 = r * p + t;
Eigen::Matrix<T, 3, 1> cross = (p0 - p1).cross(dir);
residual[0] = cross[0];
residual[1] = cross[1];
residual[2] = cross[2];
return true;
}
template <typename T>
bool PointPlaneCoeffCostFunction::operator()(const T *const r_quat,
const T *const t_vec,

7
oh_my_loam/solver/solver.cc
View File

@ -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 =

2
oh_my_loam/solver/solver.h
View File

@ -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,