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

mapper almost done

main
feixyz10 2021-02-01 21:01:42 +08:00 committed by feixyz
parent 4143d2f67f
commit ed9a75b7d6
15 changed files with 363 additions and 152 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
common/math/fitting.h Normal file
View File

@ -0,0 +1,68 @@
#pragma once
#include "common/pcl/pcl_types.h"
namespace common {
/**
* @return Coefficients representing line equation ax + by + c = 0 (a^2 +
* b^2 = 1)
*/
template <typename PT>
Eigen::Vector3d FitLine2D(const pcl::PointCloud<PT> &cloud,
double *const score = nullptr) {
Eigen::MatrixX2f data(cloud.size(), 2);
size_t i = 0;
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::ComputeThinV);
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};
}
/**
* @return Coefficients with its first three components representing point
* cloud's centroid and last three -- line direction
*/
template <typename PT>
Eigen::Matrix<double, 6, 1> FitLine3D(const pcl::PointCloud<PT> &cloud,
double *const score = nullptr) {
Eigen::MatrixX3f data(cloud.size(), 3);
size_t i = 0;
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::ComputeThinV);
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;
}
/**
* @return Coefficients representing plane equation ax + by + cz + d = 0 (a^2 +
* b^2 + c^2 = 1)
*/
template <typename PT>
Eigen::Vector3d FitPlane(const pcl::PointCloud<PT> &cloud,
double *const score = nullptr) {
Eigen::MatrixX3f data(cloud.size(), 3);
size_t i = 0;
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::ComputeThinV);
Eigen::Vector2f 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};
}
} // namespace common

66
oh_my_loam/base/helper.h
View File

@ -1,66 +0,0 @@
#pragma once
#include "common/geometry/pose3d.h"
#include "oh_my_loam/base/types.h"
namespace oh_my_loam {
using common::Pose3d;
inline int GetScanId(const TPoint &pt) {
return static_cast<int>(pt.time);
}
inline float GetTime(const TPoint &pt) {
return pt.time - GetScanId(pt);
}
/**
* @brief Transform a lidar point to the start of the scan
*
* @param pose Relative pose, end scan time w.r.t. start scan time
*/
void TransformToStart(const Pose3d &pose, const TPoint &pt_in,
TPoint *const pt_out);
TPoint TransformToStart(const Pose3d &pose, const TPoint &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
*/
void TransformToEnd(const Pose3d &pose, const TPoint &pt_in,
TPoint *const pt_out);
TPoint TransformToEnd(const Pose3d &pose, const TPoint &pt_in);
struct PointLinePair {
TPoint pt;
struct Line {
TPoint pt1, pt2;
Line() = default;
Line(const TPoint &pt1, const TPoint &pt2) : pt1(pt1), pt2(pt2) {}
};
Line line;
PointLinePair(const TPoint &pt, const Line &line) : pt(pt), line(line) {}
PointLinePair(const TPoint &pt, const TPoint &pt1, const TPoint &pt2)
: pt(pt), line(pt1, pt2) {}
};
struct PointPlanePair {
TPoint pt;
struct Plane {
TPoint pt1, pt2, pt3;
Plane() = default;
Plane(const TPoint &pt1, const TPoint &pt2, const TPoint &pt3)
: pt1(pt1), pt2(pt2), pt3(pt3) {}
};
Plane plane;
PointPlanePair(const TPoint &pt, const Plane &plane) : pt(pt), plane(plane) {}
PointPlanePair(const TPoint &pt, const TPoint &pt1, const TPoint &pt2,
const TPoint &pt3)
: pt(pt), plane(pt1, pt2, pt3) {}
};
} // namespace oh_my_loam

0
oh_my_loam/base/helper.cc → oh_my_loam/base/utils.cc
View File

36
oh_my_loam/base/utils.h Normal file
View File

@ -0,0 +1,36 @@
#pragma once
#include "common/geometry/pose3d.h"
#include "oh_my_loam/base/types.h"
namespace oh_my_loam {
inline int GetScanId(const TPoint &pt) {
return static_cast<int>(pt.time);
}
inline float GetTime(const TPoint &pt) {
return pt.time - GetScanId(pt);
}
/**
* @brief Transform a lidar point to the start of the scan
*
* @param pose Relative pose, end scan time w.r.t. start scan time
*/
void TransformToStart(const common::Pose3d &pose, const TPoint &pt_in,
TPoint *const pt_out);
TPoint TransformToStart(const common::Pose3d &pose, const TPoint &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
*/
void TransformToEnd(const common::Pose3d &pose, const TPoint &pt_in,
TPoint *const pt_out);
TPoint TransformToEnd(const common::Pose3d &pose, const TPoint &pt_in);
} // namespace oh_my_loam

6
oh_my_loam/configs/config.yaml
View File

@ -34,5 +34,9 @@ mapper_config:
vis: false
verbose: false
map_shape: [3, 21, 21]
map_step_: 50 # meter
map_step: 50 # meter
submap_shape: [1, 5, 5]
nearest_neighbor_k: 5
neighbor_point_dist_sq_th: 1.0
min_line_fit_score: 2
min_plane_fit_score: 2

2
oh_my_loam/extractor/extractor.h
View File

@ -2,7 +2,7 @@
#include "common/common.h"
#include "oh_my_loam/base/feature.h"
#include "oh_my_loam/base/helper.h"
#include "oh_my_loam/base/utils.h"
#include "oh_my_loam/base/types.h"
#include "oh_my_loam/visualizer/extractor_visualizer.h"

15
oh_my_loam/mapper/map.cc
View File

@ -108,9 +108,10 @@ Index Map::GetIndex(const TPoint &point) const {
return index;
}
TPointCloudPtr Map::GetSurrPoints(const TPoint &point, int n) const {
TPointCloudPtr Map::GetSurrPoints(const TPoint &point,
const std::vector<int> &surr_shapes) const {
TPointCloudPtr cloud_all(new TPointCloud);
for (const auto &index : GetSurrIndices(point, n)) {
for (const auto &index : GetSurrIndices(point, surr_shapes)) {
*cloud_all += *this->at(index);
}
return cloud_all;
@ -165,12 +166,14 @@ const Row &Map::at(int z_idx, int y_idx) const {
return map_.at(z_idx).at(y_idx);
}
std::vector<Index> Map::GetSurrIndices(const TPoint &point, int n) const {
std::vector<Index> Map::GetSurrIndices(
const TPoint &point, const std::vector<int> &surr_shapes) const {
std::vector<Index> indices;
Index index = GetIndex(point);
for (int k = -n; k <= n; ++k) {
for (int j = -n; j <= n; ++j) {
for (int i = -n; i <= n; ++i) {
int nz = surr_shapes[0] / 2, ny = surr_shapes[1] / 2, nx = surr_shapes[2] / 2;
for (int k = -nz; k <= nz; ++k) {
for (int j = -ny; j <= ny; ++j) {
for (int i = -nx; i <= nx; ++i) {
indices.emplace_back(index.k + k, index.j + j, index.i + i);
}
}

6
oh_my_loam/mapper/map.h
View File

@ -46,7 +46,8 @@ class Map {
Index GetIndex(const TPoint &point) const;
TPointCloudPtr GetSurrPoints(const TPoint &point, int n) const;
TPointCloudPtr GetSurrPoints(const TPoint &point,
const std::vector<int> &surr_shapes) const;
TPointCloudPtr GetAllPoints() const;
@ -70,7 +71,8 @@ class Map {
const TPointCloudPtr &at(int z_idx, int y_idx, int x_idx) const;
std::vector<Index> GetSurrIndices(const TPoint &point, int n) const;
std::vector<Index> GetSurrIndices(const TPoint &point,
const std::vector<int> &surr_shapes) const;
std::vector<Index> GetAllIndices() const;

133
oh_my_loam/mapper/mapper.cc
View File

@ -3,16 +3,14 @@
#include <mutex>
#include "common/log/log.h"
#include "common/math/fitting.h"
#include "common/pcl/pcl_utils.h"
#include "oh_my_loam/base/helper.h"
#include "oh_my_loam/base/types.h"
#include "oh_my_loam/mapper/map.h"
#include "oh_my_loam/solver/solver.h"
namespace oh_my_loam {
namespace {
using namespace common;
using common::YAMLConfig;
using LineCoeff = Eigen::Matrix<double, 6, 1>;
} // namespace
bool Mapper::Init() {
@ -29,7 +27,9 @@ bool Mapper::Init() {
return true;
}
void Mapper::Reset() {}
void Mapper::Reset() {
SetState(UN_INIT);
}
void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf,
@ -46,32 +46,37 @@ void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
if (GetState() == DONE) {
thread_.reset(new std::thread(&Mapper::Run, this, cloud_corn, cloud_surf,
pose_curr2odom));
thread_->detach();
if (thread_->joinable()) thread_->detach();
}
std::lock_guard<std::mutex> lock(state_mutex_);
*pose_curr2map = pose_odom2map_ * pose_curr2odom;
}
void Mapper::Run(const TPointCloudConstPtr &cloud_corn_in,
const TPointCloudConstPtr &cloud_surf_in,
const Pose3d &pose_curr2odom) {
void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf,
const common::Pose3d &pose_curr2odom) {
SetState(RUNNING);
Pose3d pose_curr2map = pose_odom2map_ * pose_curr2odom;
common::Pose3d pose_curr2map = pose_odom2map_ * pose_curr2odom;
TPoint cnt(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(),
pose_curr2map.t_vec().z());
AdjustMap(corn_map_->GetIndex(cnt));
int submap_shape = config_["submap_shape"].as<int>();
TPointCloudPtr cloud_corn(new TPointCloud);
TPointCloudPtr cloud_surf(new TPointCloud);
TransformPointCloud(pose_curr2map, *cloud_corn_in, cloud_corn.get());
TransformPointCloud(pose_curr2map, *cloud_surf_in, cloud_surf.get());
TPointCloudPtr cloud_corn_map =
corn_map_->GetSurrPoints(cnt, submap_shape / 2);
TPointCloudPtr cloud_surf_map =
corn_map_->GetSurrPoints(cnt, submap_shape / 2);
// MatchCorn();
// MatchSurf();
AdjustMap(cnt);
TPointCloudPtr cloud_corn_map = corn_map_->GetSurrPoints(cnt, submap_shape_);
TPointCloudPtr cloud_surf_map = corn_map_->GetSurrPoints(cnt, submap_shape_);
pcl::KdTreeFLANN<TPoint> kdtree_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<PointPlaneCoeffPair> pp_pairs;
MatchCorn(kdtree_corn, cloud_corn_map, pose_curr2map, &pl_pairs);
MatchSurf(kdtree_surf, cloud_surf_map, pose_curr2map, &pp_pairs);
PoseSolver solver(pose_curr2map);
for (const auto &pair : pl_pairs) {
solver.AddPointLinePair(pair, 0.0);
}
for (const auto &pair : pp_pairs) {
solver.AddPointPlaneCoeffPair(pair, 0.0);
}
if (!solver.Solve(5, false, &pose_curr2map)) {
AWARN << "Solve: no_convergence";
}
@ -80,7 +85,62 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn_in,
SetState(DONE);
}
void Mapper::AdjustMap(const Index &index) {
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<int> indices;
std::vector<float> dists;
int nearest_neighbor_k = config_["nearest_neighbor_k"].as<int>();
float neighbor_point_dist_sq_th =
config_["neighbor_point_dist_sq_th"].as<float>();
for (const auto &pt : *cloud_curr) {
TPoint pt_query = common::TransformPoint(pose_curr2map, pt);
if (kdtree.nearestKSearch(pt_query, nearest_neighbor_k, indices, dists) !=
nearest_neighbor_k) {
continue;
}
if (dists.back() > neighbor_point_dist_sq_th) continue;
TPointCloud neighbor_pts;
pcl::copyPointCloud(*cloud_curr, indices, neighbor_pts);
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);
}
}
void Mapper::MatchSurf(const pcl::KdTreeFLANN<TPoint> &kdtree,
const TPointCloudConstPtr &cloud_curr,
const common::Pose3d &pose_curr2map,
std::vector<PointPlaneCoeffPair> *const pairs) const {
std::vector<int> indices;
std::vector<float> dists;
int nearest_neighbor_k = config_["nearest_neighbor_k"].as<int>();
float neighbor_point_dist_sq_th =
config_["neighbor_point_dist_sq_th"].as<float>();
for (const auto &pt : *cloud_curr) {
TPoint pt_query = common::TransformPoint(pose_curr2map, pt);
if (kdtree.nearestKSearch(pt_query, nearest_neighbor_k, indices, dists) !=
nearest_neighbor_k) {
continue;
}
if (dists.back() > neighbor_point_dist_sq_th) continue;
TPointCloud neighbor_pts;
pcl::copyPointCloud(*cloud_curr, indices, neighbor_pts);
double fit_score = 0.0;
Eigen::Vector4d coeff = common::FitPlane(neighbor_pts, &fit_score);
if (fit_score < config_["min_plane_fit_score"].as<double>()) continue;
TPoint pt1(coeff[0], coeff[1], coeff[2], 0.0);
pairs->emplace_back(pt, coeff);
}
}
void Mapper::AdjustMap(const TPoint &center) {
Index index = corn_map_->GetIndex(center);
int min_idx_z = submap_shape_[0] / 2 + 1;
int max_idx_z = map_shape_[0] - min_idx_z - 1;
if (index.k < min_idx_z) {
@ -113,6 +173,31 @@ void Mapper::AdjustMap(const Index &index) {
}
}
TPointCloudPtr Mapper::GetCornMapPoints() const {
return corn_map_->GetAllPoints();
}
TPointCloudPtr Mapper::GetSurfMapPoints() const {
return surf_map_->GetAllPoints();
}
TPointCloudPtr Mapper::GetMapPoints() const {
TPointCloudPtr map_points(new TPointCloud);
*map_points += *GetCornMapPoints();
*map_points += *GetSurfMapPoints();
return map_points;
}
Mapper::State Mapper::GetState() const {
std::lock_guard<std::mutex> lock(state_mutex_);
return state_;
}
void Mapper::SetState(State state) {
std::lock_guard<std::mutex> lock(state_mutex_);
state_ = state;
}
void Mapper::Visualize() {}
} // namespace oh_my_loam

61
oh_my_loam/mapper/mapper.h
View File

@ -7,9 +7,10 @@
#include <vector>
#include "common/geometry/pose3d.h"
#include "oh_my_loam/base/helper.h"
#include "oh_my_loam/base/types.h"
#include "oh_my_loam/base/utils.h"
#include "oh_my_loam/mapper/map.h"
#include "oh_my_loam/solver/solver.h"
namespace oh_my_loam {
@ -24,22 +25,11 @@ class Mapper {
const common::Pose3d &pose_curr2odom,
common::Pose3d *const pose_curr2map);
TPointCloudPtr GetCornMapPoints() const {
std::shared_lock<std::shared_mutex> lock(corn_map_mutex_);
return corn_map_->GetAllPoints();
}
TPointCloudPtr GetCornMapPoints() const;
TPointCloudPtr GetSurfMapPoints() const {
std::shared_lock<std::shared_mutex> lock(surf_map_mutex_);
return surf_map_->GetAllPoints();
}
TPointCloudPtr GetSurfMapPoints() const;
TPointCloudPtr GetMapPoints() const {
TPointCloudPtr map_points(new TPointCloud);
*map_points += *GetCornMapPoints();
*map_points += *GetSurfMapPoints();
return map_points;
}
TPointCloudPtr GetMapPoints() const;
void Reset();
@ -47,41 +37,44 @@ class Mapper {
enum State { DONE, RUNNING, UN_INIT };
void Run(const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf, const Pose3d &pose_init);
const TPointCloudConstPtr &cloud_surf,
const common::Pose3d &pose_init);
void MatchCorn(const TPointCloudConstPtr &src, const TPointCloudConstPtr &tgt,
std::vector<PointLinePair> *const pair) const;
void MatchCorn(const pcl::KdTreeFLANN<TPoint> &kdtree,
const TPointCloudConstPtr &cloud_curr,
const common::Pose3d &pose_curr2map,
std::vector<PointLinePair> *const pairs) const;
void MatchSurf(const TPointCloudConstPtr &src, const TPointCloudConstPtr &tgt,
std::vector<PointPlanePair> *const pair) const;
void MatchSurf(const pcl::KdTreeFLANN<TPoint> &kdtree,
const TPointCloudConstPtr &cloud_curr,
const common::Pose3d &pose_curr2map,
std::vector<PointPlaneCoeffPair> *const pairs) const;
State GetState() const {
std::lock_guard<std::mutex> lock(state_mutex_);
return state_;
}
State GetState() const;
void SetState(State state) {
std::lock_guard<std::mutex> lock(state_mutex_);
state_ = state;
}
void SetState(State state);
void AdjustMap(const Index &index);
void AdjustMap(const TPoint &center);
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;
void Visualize();
YAML::Node config_;
mutable std::shared_mutex corn_map_mutex_;
mutable std::shared_mutex surf_map_mutex_;
std::vector<int> map_shape_, submap_shape_;
double map_step_;
std::unique_ptr<Map> corn_map_;
std::unique_ptr<Map> surf_map_;
pcl::KdTreeFLANN<TPoint> kdtree_corn_map_;
pcl::KdTreeFLANN<TPoint> kdtree_surf_map_;
mutable std::mutex state_mutex_;
Pose3d pose_odom2map_;
common::Pose3d pose_odom2map_;
State state_ = UN_INIT;
mutable std::unique_ptr<std::thread> thread_{nullptr};

2
oh_my_loam/odometer/odometer.cc
View File

@ -5,8 +5,6 @@
#include "common/log/log.h"
#include "common/pcl/pcl_utils.h"
#include "common/time/timer.h"
#include "oh_my_loam/base/types.h"
#include "oh_my_loam/solver/solver.h"
namespace oh_my_loam {

4
oh_my_loam/odometer/odometer.h
View File

@ -1,9 +1,9 @@
#pragma once
#include "common/common.h"
#include "common/geometry/pose3d.h"
#include "oh_my_loam/base/feature.h"
#include "oh_my_loam/base/helper.h"
#include "oh_my_loam/base/utils.h"
#include "oh_my_loam/solver/solver.h"
#include "oh_my_loam/visualizer/odometer_visualizer.h"
namespace oh_my_loam {

107
oh_my_loam/solver/cost_function.h
View File

@ -2,14 +2,50 @@
#include <ceres/ceres.h>
#include <eigen3/Eigen/Dense>
#include <type_traits>
#include "common/common.h"
#include "oh_my_loam/base/helper.h"
#include "oh_my_loam/base/utils.h"
namespace oh_my_loam {
namespace {
const double kEpsilon = 1.0e-6;
}
struct PointLinePair {
TPoint pt;
struct Line {
TPoint pt1, pt2;
Line() = default;
Line(const TPoint &pt1, const TPoint &pt2) : pt1(pt1), pt2(pt2) {}
};
Line line;
PointLinePair(const TPoint &pt, const Line &line) : pt(pt), line(line) {}
PointLinePair(const TPoint &pt, const TPoint &pt1, const TPoint &pt2)
: pt(pt), line(pt1, pt2) {}
};
struct PointPlanePair {
TPoint pt;
struct Plane {
TPoint pt1, pt2, pt3;
Plane() = default;
Plane(const TPoint &pt1, const TPoint &pt2, const TPoint &pt3)
: pt1(pt1), pt2(pt2), pt3(pt3) {}
};
Plane plane;
PointPlanePair(const TPoint &pt, const Plane &plane) : pt(pt), plane(plane) {}
PointPlanePair(const TPoint &pt, const TPoint &pt1, const TPoint &pt2,
const TPoint &pt3)
: pt(pt), plane(pt1, pt2, pt3) {}
};
struct PointPlaneCoeffPair {
TPoint pt;
Eigen::Vector4d plane_coeff;
PointPlaneCoeffPair(const TPoint &pt, const Eigen::Vector4d &plane_coeff)
: pt(pt), plane_coeff(plane_coeff) {}
};
class PointLineCostFunction {
public:
PointLineCostFunction(const PointLinePair &pair, double time)
@ -51,6 +87,28 @@ class PointPlaneCostFunction {
DISALLOW_COPY_AND_ASSIGN(PointPlaneCostFunction)
};
class PointPlaneCoeffCostFunction {
public:
PointPlaneCoeffCostFunction(const PointPlaneCoeffPair &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 PointPlaneCoeffPair &pair,
double time) {
return new ceres::AutoDiffCostFunction<PointPlaneCoeffCostFunction, 1, 4,
3>(
new PointPlaneCoeffCostFunction(pair, time));
}
private:
PointPlaneCoeffPair pair_;
double time_;
DISALLOW_COPY_AND_ASSIGN(PointPlaneCoeffCostFunction)
};
template <typename T>
bool PointLineCostFunction::operator()(const T *const r_quat,
const T *const t_vec,
@ -61,11 +119,12 @@ bool PointLineCostFunction::operator()(const T *const r_quat,
Eigen::Matrix<T, 3, 1> p2(T(pt2.x), T(pt2.y), T(pt2.z));
Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
Eigen::Quaternion<T> r_interp =
Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
Eigen::Matrix<T, 3, 1> t(T(time_) * t_vec[0], T(time_) * t_vec[1],
T(time_) * t_vec[2]);
Eigen::Matrix<T, 3, 1> p0 = r_interp * p + t;
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;
// norm of cross product: triangle area x 2
Eigen::Matrix<T, 3, 1> area = (p0 - p1).cross(p0 - p2) * 0.5;
@ -88,15 +147,35 @@ bool PointPlaneCostFunction::operator()(const T *const r_quat,
Eigen::Matrix<T, 3, 1> p3(T(pt3.x), T(pt3.y), T(pt3.z));
Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
Eigen::Quaternion<T> r_interp =
Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
Eigen::Matrix<T, 3, 1> t(T(time_) * t_vec[0], T(time_) * t_vec[1],
T(time_) * t_vec[2]);
Eigen::Matrix<T, 3, 1> p0 = r_interp * p + t;
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> normal = (p2 - p1).cross(p3 - p1).normalized();
residual[0] = (p0 - p1).dot(normal);
return true;
}
template <typename T>
bool PointPlaneCoeffCostFunction::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, 4, 1> coeff = pair_.plane_coeff.template cast<T>();
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;
residual[0] = coeff.topRows(3).transpose() * p + coeff[3];
return true;
}
} // namespace oh_my_loam

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::AddPointPlaneCoeffPair(const PointPlaneCoeffPair &pair,
double time) {
ceres::CostFunction *cost_function =
PointPlaneCoeffCostFunction::Create(pair, time);
problem_.AddResidualBlock(cost_function, loss_function_, r_quat_, t_vec_);
}
common::Pose3d PoseSolver::GetPose() const {
return common::Pose3d(r_quat_, t_vec_);
}

2
oh_my_loam/solver/solver.h
View File

@ -13,6 +13,8 @@ class PoseSolver {
void AddPointPlanePair(const PointPlanePair &pair, double time);
void AddPointPlaneCoeffPair(const PointPlaneCoeffPair &pair, double time);
bool Solve(int max_iter_num = 5, bool verbose = false,
common::Pose3d *const pose = nullptr);