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

odometer almost done

main
feixyz10 2021-01-21 20:56:23 +08:00 committed by feixyz
parent 1ccebd8b8f
commit 3c14de4bce
16 changed files with 330 additions and 290 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -54,9 +54,9 @@ target_link_libraries(main
common
oh_my_loam
extractor
# odometer
odometer
# mapper
# solver
solver
${CERES_LIBRARIES}
visualizer
base

85
common/geometry/pose.h
View File

@ -1,85 +0,0 @@
#pragma once
#include <eigen3/Eigen/Dense>
namespace common {
template <typename T>
class Pose3 {
public:
Pose3() {
r_quat_.setIdentity();
t_vec_.setZero();
};
Pose3(const Eigen::Quaterniond& r_quat, const Eigen::Matrix<T, 3, 1>& t_vec)
: r_quat_(q), t_vec_(p) {}
Pose3(const Eigen::Matrix3d& r_mat, const Eigen::Matrix<T, 3, 1>& t_vec)
: r_quat_(r_mat), t_vec_(t_vec) {}
Pose3(const double* const r_quat, const double* const t_vec)
: r_quat_(r_quat), t_vec_(t_vec) {}
Pose3 Inv() const {
Eigen::Quaternion<T> r_inv = r_quat_.inverse();
Eigen::Matrix<T, 3, 1> t_inv = -r_inv * t_vec_;
return {r_inv, t_inv};
}
Eigen::Matrix<T, 3, 1> Transform(const Eigen::Matrix<T, 3, 1>& point) const {
return r_quat_ * point + t_vec_;
}
Eigen::Matrix<T, 3, 1> operator*(const Eigen::Matrix<T, 3, 1>& point) const {
return Transform(point);
}
Eigen::Matrix<T, 3, 1> Translate(const Eigen::Matrix<T, 3, 1>& vec) const {
return vec + t_vec_;
}
Eigen::Matrix<T, 3, 1> Rotate(const Eigen::Matrix<T, 3, 1>& vec) const {
return r_quat_ * vec;
}
// Spherical linear interpolation to `pose_to`
Pose3 Interpolate(const Pose3& pose_to, double t) const {
Pose3 pose_dst;
Eigen::Quaternion<T> q_interp = r_quat_.slerp(t, pose_to.r_quat_);
Eigen::Matrix<T, 3, 1> p_interp = (pose_to.t_vec_ - t_vec_) * t + t_vec_;
return {q_interp, p_interp};
}
std::string ToString() const {
std::ostringstream oss;
oss << "[Pose3D] q = (" << q_.coeffs().transpose() << "), p = ("
<< p_.transpose() << ")";
return oss.str();
}
Eigen::Quaternion<T> r_quat() const { return r_quat_; }
Eigen::Matrix<T, 3, 1> t_vec() const { return t_vec_; }
protected:
Eigen::Quaternion<T> r_quat_; // orientation/rotation
Eigen::Matrix<T, 3, 1> t_vec_; // positon/translation
};
template <typename T>
Pose3<T> Interpolate(const Pose3<T>& pose_from, const Pose3<T>& pose_to,
double t) {
return pose_from.Interpolate(pose_to, t);
}
template <typename T>
Pose3<T> operator*(const Pose3<T>& lhs, const Pose3<T>& rhs) {
return Pose3<T>(lhs.q() * rhs.q(), lhs.q() * rhs.p() + lhs.p());
}
using Pose3d = Pose3<double>;
using Pose3f = Pose3<float>;
} // namespace common

98
common/geometry/pose3d.h Normal file
View File

@ -0,0 +1,98 @@
#pragma once
#include <eigen3/Eigen/Dense>
#include <iomanip>
namespace common {
class Pose3d {
public:
Pose3d() {
r_quat_.setIdentity();
t_vec_.setZero();
};
Pose3d(const Eigen::Quaterniond& r_quat, const Eigen::Vector3d& t_vec)
: r_quat_(r_quat), t_vec_(t_vec) {}
Pose3d(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& t_vec)
: r_quat_(r_mat), t_vec_(t_vec) {}
Pose3d(const double* const r_quat, const double* const t_vec)
: r_quat_(r_quat), t_vec_(t_vec) {}
static Pose3d Identity() { return {}; }
void setIdentity() {
r_quat_.setIdentity();
t_vec_.setZero();
}
Pose3d Inv() const {
Eigen::Quaterniond r_inv = r_quat_.inverse();
Eigen::Vector3d t_inv = r_inv * t_vec_;
return {r_inv, -t_inv};
}
Pose3d operator*(const Pose3d& rhs) const {
return Pose3d(r_quat_ * rhs.r_quat_, r_quat_ * rhs.t_vec_ + t_vec_);
}
Pose3d& operator*=(const Pose3d& rhs) {
t_vec_ += r_quat_ * rhs.t_vec_;
r_quat_ *= rhs.r_quat_;
return *this;
}
Eigen::Vector3d Transform(const Eigen::Vector3d& point) const {
return r_quat_ * point + t_vec_;
}
Eigen::Vector3d operator*(const Eigen::Vector3d& point) const {
return Transform(point);
}
Eigen::Vector3d Rotate(const Eigen::Vector3d& vec) const {
return r_quat_ * vec;
}
// Spherical linear interpolation to `pose_to`
Pose3d Interpolate(const Pose3d& pose_to, double t) const {
Pose3d pose_dst = t > 0 ? pose_to : (*this) * pose_to.Inv() * (*this);
t = t > 0 ? t : -t;
int whole = std::floor(t);
double frac = t - whole;
Eigen::Quaterniond r_interp = r_quat_.slerp(frac, pose_dst.r_quat_);
while (whole--) r_interp *= pose_dst.r_quat_;
Eigen::Vector3d t_interp = (pose_dst.t_vec_ - t_vec_) * t + t_vec_;
return {r_interp, t_interp};
}
std::string ToString() const {
std::ostringstream oss;
double w = r_quat_.w(), a = r_quat_.x(), b = r_quat_.y(), c = r_quat_.z();
double x = t_vec_.x(), y = t_vec_.y(), z = t_vec_.z();
oss << std::setprecision(3) << "[Pose3d] r_quat = (" << w << " " << a << " "
<< b << " " << c << "), p = (" << x << " " << y << " " << z << ")";
return oss.str();
}
// const Eigen::Quaterniond& r_quat() const { return r_quat_; }
Eigen::Quaterniond r_quat() const { return r_quat_; }
// const Eigen::Vector3d& t_vec() const { return t_vec_; }
Eigen::Vector3d t_vec() const { return t_vec_; }
protected:
Eigen::Quaterniond r_quat_; // orientation/rotation
Eigen::Vector3d t_vec_; // positon/translation
};
inline Pose3d Interpolate(const Pose3d& pose_from, const Pose3d& pose_to,
double t) {
return pose_from.Interpolate(pose_to, t);
}
} // namespace common

4
oh_my_loam/CMakeLists.txt
View File

@ -1,9 +1,9 @@
add_subdirectory(base)
add_subdirectory(extractor)
add_subdirectory(visualizer)
# add_subdirectory(odometer)
add_subdirectory(odometer)
# add_subdirectory(mapper)
# add_subdirectory(solver)
add_subdirectory(solver)
aux_source_directory(. SRC_FILES)

18
oh_my_loam/base/helper.cc
View File

@ -2,16 +2,28 @@
namespace oh_my_loam {
void TransformToStart(const Pose3D& pose, const TPoint& pt_in,
void TransformToStart(const Pose3d& pose, const TPoint& pt_in,
TPoint* const pt_out) {
Pose3D pose_interp = Pose3D().Interpolate(pose, GetTime(pt_in));
Pose3d pose_interp = Pose3d().Interpolate(pose, GetTime(pt_in));
TransformPoint<TPoint>(pose_interp, pt_in, pt_out);
}
void TransformToEnd(const Pose3D& pose, const TPoint& pt_in,
TPoint TransformToStart(const 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,
TPoint* const pt_out) {
TransformToStart(pose, pt_in, pt_out);
TransformPoint<TPoint>(pose.Inv(), *pt_out, pt_out);
}
TPoint TransformToEnd(const Pose3d& pose, const TPoint& pt_in) {
TPoint pt_out;
TransformToEnd(pose, pt_in, &pt_out);
return pt_out;
}
} // namespace oh_my_loam

23
oh_my_loam/base/helper.h
View File

@ -1,20 +1,19 @@
#pragma once
#include "common/geometry/pose.h"
#include "common/geometry/pose3d.h"
#include "common/pcl/pcl_types.h"
namespace oh_my_loam {
using common::TPoint;
using common::Pose3D;
using common::Trans3d;
using common::Pose3d;
inline float GetTime(const TPoint& pt) { return pt.time - std::floor(pt.time); }
inline int GetScanId(const TPoint& pt) { return static_cast<int>(pt.time); }
template <typename PointType>
void TransformPoint(const Pose3D& pose, const PointType& pt_in,
void TransformPoint(const Pose3d& pose, const PointType& pt_in,
PointType* const pt_out) {
*pt_out = pt_in;
Eigen::Vector3d pnt = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z);
@ -23,21 +22,33 @@ void TransformPoint(const Pose3D& pose, const PointType& pt_in,
pt_out->z = pnt.z();
}
template <typename PointType>
PointType TransformPoint(const Pose3d& pose, const PointType& pt_in) {
PointType pt_out;
TransformPoint<PointType>(pose, pt_in, &pt_out);
return pt_out;
}
/**
* @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,
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,
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 {

6
oh_my_loam/configs/config.yaml
View File

@ -6,7 +6,7 @@ vis: true
# configs for extractor
extractor_config:
vis: true
vis: false
verbose: true
min_point_num: 66
sharp_corner_point_num: 2
@ -20,9 +20,9 @@ extractor_config:
# configs for odometer
odometer_config:
vis: false
vis: true
verbose: false
icp_iter_num : 2
icp_iter_num: 2
match_dist_sq_th: 1
# configs for mapper

2
oh_my_loam/mapper/mapper.h
View File

@ -18,7 +18,7 @@ class Mapper {
void Visualize();
common::TPointCloudPtr map_pts_;
common::Pose3D pose_curr2world_;
common::Pose3d pose_curr2world_;
bool is_initialized = false;

251
oh_my_loam/odometer/odometer.cc
View File

@ -6,7 +6,7 @@
namespace oh_my_loam {
namespace {
int kNearbyScanNum = 2;
int kNearScanN = 2;
size_t kMinMatchNum = 10;
using namespace common;
} // namespace
@ -16,20 +16,18 @@ bool Odometer::Init() {
config_ = config["odometer_config"];
is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
verbose_ = config_["verbose"].as<bool>();
kdtree_surf_.reset(new pcl::KdTreeFLANN<TPoint>);
kdtree_corn_.reset(new pcl::KdTreeFLANN<TPoint>);
AINFO << "Odometry visualizer: " << (is_vis_ ? "ON" : "OFF");
if (is_vis_) visualizer_.reset(new OdometerVisualizer);
return true;
}
void Odometer::Process(double timestamp, const Feature& feature,
Pose3D* const pose) {
void Odometer::Process(double timestamp, const std::vector<Feature>& features,
Pose3d* const pose) {
BLOCK_TIMER_START;
if (!is_initialized_) {
UpdatePre(features);
is_initialized_ = true;
UpdatePre(feature);
*pose = Pose3D();
*pose = Pose3d::Identity();
AWARN << "Odometer initialized....";
return;
}
@ -37,162 +35,152 @@ void Odometer::Process(double timestamp, const Feature& feature,
std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs;
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
MatchCornFeature(*feature.cloud_sharp_corner, *corn_pre_, &pl_pairs);
AINFO_IF(i == 0) << "PL mathch: src size = "
<< feature.cloud_sharp_corner->size()
<< ", tgt size = " << corn_pre_->size();
MatchSurfFeature(*feature.cloud_flat_surf, *surf_pre_, &pp_pairs);
AINFO_IF(i == 0) << "PP mathch: src size = "
<< feature.cloud_flat_surf->size()
<< ", tgt size = " << surf_pre_->size();
AINFO << "Matched num, pl = " << pl_pairs.size()
<< ", pp = " << pp_pairs.size();
if (pl_pairs.size() + pp_pairs.size() < kMinMatchNum) {
AWARN << "Too less correspondence: num = "
<< pl_pairs.size() + pp_pairs.size();
for (const auto& feature : features) {
MatchCorn(*feature.cloud_sharp_corner, &pl_pairs);
// MatchSurf(*feature.cloud_flat_surf, &pp_pairs);
}
Eigen::Vector4d q_vec = pose_curr2last_.q().coeffs();
Eigen::Vector3d p_vec = pose_curr2last_.p();
double* q = q_vec.data();
double* p = p_vec.data();
PoseSolver solver(q, p);
AINFO_IF(verbose_) << "Iter " << i
<< ": matched num: point2line = " << pl_pairs.size()
<< ", point2plane = " << pp_pairs.size();
if (pl_pairs.size() + pp_pairs.size() < kMinMatchNum) {
AWARN << "Too less correspondence: " << pl_pairs.size() << " + "
<< pp_pairs.size();
}
PoseSolver solver(pose_curr2last_);
for (const auto& pair : pl_pairs) {
solver.AddPointLinePair(pair, GetTime(pair.pt));
solver.AddPointLinePair(pair, pair.pt.time);
}
for (const auto& pair : pp_pairs) {
solver.AddPointPlanePair(pair, GetTime(pair.pt));
solver.AddPointPlanePair(pair, pair.pt.time);
}
solver.Solve();
pose_curr2last_ = Pose3D(q, p);
solver.Solve(5, verbose_, &pose_curr2last_);
}
pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
*pose = pose_curr2world_;
AWARN << "Pose increase: " << pose_curr2last_.ToString();
AWARN << "Pose after: " << pose_curr2world_.ToString();
UpdatePre(feature);
AWARN_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString();
AWARN_IF(verbose_) << "Pose after: " << pose_curr2world_.ToString();
UpdatePre(features);
if (is_vis_) Visualize(features, pl_pairs, pp_pairs);
AINFO << "Odometer::Porcess: " << BLOCK_TIMER_STOP_FMT;
if (is_vis_) Visualize(feature, pl_pairs, pp_pairs);
}
void Odometer::MatchCornFeature(const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointLinePair>* const pairs) const {
void Odometer::MatchCorn(const TPointCloud& src,
std::vector<PointLinePair>* const pairs) const {
double dist_sq_thresh = config_["match_dist_sq_th"].as<double>();
for (const auto& q_pt : src) {
TPoint query_pt;
TransformToStart(pose_curr2last_, q_pt, &query_pt);
std::vector<int> indices;
std::vector<float> dists;
kdtree_corn_->nearestKSearch(query_pt, 1, indices, dists);
if (dists[0] >= dist_sq_thresh) continue;
TPoint pt1 = tgt.points[indices[0]];
int pt2_idx = -1;
auto comp = [](const std::vector<float>& v1, const std::vector<float>& v2) {
return v1[0] < v2[0];
};
for (const auto& pt : src) {
TPoint query_pt = TransformToStart(pose_curr2last_, pt);
std::vector<std::vector<int>> indices;
std::vector<std::vector<float>> dists;
NearestKSearch(kdtrees_corn_, query_pt, 1, &indices, &dists);
int pt1_scan_id =
std::min_element(dists.begin(), dists.end(), comp) - dists.begin();
if (dists[pt1_scan_id][0] >= dist_sq_thresh) continue;
TPoint pt1 = clouds_corn_pre_[pt1_scan_id]->at(indices[pt1_scan_id][0]);
double min_dist_pt2_squre = dist_sq_thresh;
int query_pt_scan_id = GetScanId(query_pt);
for (int i = indices[0] + 1; i < static_cast<int>(tgt.size()); ++i) {
const auto pt = tgt.points[i];
int scan_id = GetScanId(pt);
if (scan_id <= query_pt_scan_id) continue;
if (scan_id > query_pt_scan_id + kNearbyScanNum) break;
double dist_squre = DistanceSqure(query_pt, pt);
if (dist_squre < min_dist_pt2_squre) {
pt2_idx = i;
min_dist_pt2_squre = dist_squre;
int pt2_scan_id = -1;
int i_begin = std::max<int>(0, pt1_scan_id - kNearScanN);
int i_end = std::min<int>(indices.size(), pt1_scan_id + kNearScanN + 1);
for (int i = i_begin; i < i_end && i != pt1_scan_id; ++i) {
if (dists[i][0] < min_dist_pt2_squre) {
pt2_scan_id = i;
min_dist_pt2_squre = dists[i][0];
}
}
for (int i = indices[0] - 1; i >= 0; --i) {
const auto pt = tgt.points[i];
int scan_id = GetScanId(pt);
if (scan_id >= query_pt_scan_id) continue;
if (scan_id < query_pt_scan_id - kNearbyScanNum) break;
double dist_squre = DistanceSqure(query_pt, pt);
if (dist_squre < min_dist_pt2_squre) {
pt2_idx = i;
min_dist_pt2_squre = dist_squre;
}
}
if (pt2_idx >= 0) {
TPoint pt2 = tgt.points[pt2_idx];
pairs->emplace_back(q_pt, pt1, pt2);
if (pt2_scan_id > 0) {
TPoint pt2 = clouds_corn_pre_[pt2_scan_id]->at(indices[pt2_scan_id][0]);
pairs->emplace_back(pt, pt1, pt2);
}
}
}
void Odometer::MatchSurfFeature(
const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointPlanePair>* const pairs) const {
void Odometer::MatchSurf(const TPointCloud& src,
std::vector<PointPlanePair>* const pairs) const {
double dist_sq_thresh = config_["match_dist_sq_th"].as<double>();
for (const auto& q_pt : src) {
TPoint query_pt;
TransformToStart(pose_curr2last_, q_pt, &query_pt);
std::vector<int> indices;
std::vector<float> dists;
kdtree_surf_->nearestKSearch(query_pt, 1, indices, dists);
if (dists[0] >= dist_sq_thresh) continue;
TPoint pt1 = tgt.points[indices[0]];
int pt2_idx = -1, pt3_idx = -1;
double min_dist_pt2_squre = dist_sq_thresh;
auto comp = [](const std::vector<float>& v1, const std::vector<float>& v2) {
return v1[0] < v2[0];
};
for (const auto& pt : src) {
TPoint query_pt = TransformToStart(pose_curr2last_, pt);
std::vector<std::vector<int>> indices;
std::vector<std::vector<float>> dists;
NearestKSearch(kdtrees_surf_, query_pt, 2, &indices, &dists);
int pt1_scan_id =
std::min_element(dists.begin(), dists.end(), comp) - dists.begin();
if (dists[pt1_scan_id][0] >= dist_sq_thresh) continue;
if (dists[pt1_scan_id][1] >= dist_sq_thresh) continue;
TPoint pt1 = clouds_surf_pre_[pt1_scan_id]->at(indices[pt1_scan_id][0]);
TPoint pt2 = clouds_surf_pre_[pt1_scan_id]->at(indices[pt1_scan_id][1]);
double min_dist_pt3_squre = dist_sq_thresh;
int query_pt_scan_id = GetScanId(query_pt);
for (int i = indices[0] + 1; i < static_cast<int>(tgt.size()); ++i) {
const auto pt = tgt.points[i];
int scan_id = GetScanId(pt);
if (scan_id > query_pt_scan_id + kNearbyScanNum) break;
double dist_squre = DistanceSqure(query_pt, pt);
if (scan_id == query_pt_scan_id && dist_squre < min_dist_pt2_squre) {
pt2_idx = i;
min_dist_pt2_squre = dist_squre;
int pt3_scan_id = -1;
int i_begin = std::max<int>(0, pt1_scan_id - kNearScanN);
int i_end = std::min<int>(indices.size(), pt1_scan_id + kNearScanN + 1);
for (int i = i_begin; i < i_end && i != pt1_scan_id; ++i) {
if (dists[i][0] < min_dist_pt3_squre) {
pt3_scan_id = i;
min_dist_pt3_squre = dists[i][0];
}
if (scan_id > query_pt_scan_id && dist_squre < min_dist_pt3_squre) {
pt3_idx = i;
min_dist_pt3_squre = dist_squre;
}
if (pt2_idx == -1 && scan_id > query_pt_scan_id) break;
}
for (int i = indices[0] - 1; i >= 0; --i) {
const auto pt = tgt.points[i];
int scan_id = GetScanId(pt);
if (scan_id < query_pt_scan_id - kNearbyScanNum) break;
double dist_squre = DistanceSqure(query_pt, pt);
if (scan_id == query_pt_scan_id && dist_squre < min_dist_pt2_squre) {
pt2_idx = i;
min_dist_pt2_squre = dist_squre;
}
if (scan_id < query_pt_scan_id && dist_squre < min_dist_pt3_squre) {
pt3_idx = i;
min_dist_pt3_squre = dist_squre;
}
if (pt2_idx == -1 && scan_id < query_pt_scan_id) break;
}
if (pt2_idx >= 0 && pt3_idx >= 0) {
TPoint pt2 = tgt.points[pt2_idx], pt3 = tgt.points[pt3_idx];
pairs->emplace_back(q_pt, pt1, pt2, pt3);
if (pt3_scan_id > 0) {
TPoint pt3 = clouds_surf_pre_[pt3_scan_id]->at(indices[pt3_scan_id][0]);
pairs->emplace_back(pt, pt1, pt2, pt3);
}
}
}
void Odometer::UpdatePre(const Feature& feature) {
if (!surf_pre_) {
surf_pre_.reset(new TPointCloud);
}
if (!corn_pre_) {
corn_pre_.reset(new TPointCloud);
void Odometer::UpdatePre(const std::vector<Feature>& features) {
clouds_corn_pre_.resize(features.size(), common::TPointCloud().makeShared());
clouds_surf_pre_.resize(features.size(), common::TPointCloud().makeShared());
kdtrees_corn_.resize(features.size());
kdtrees_surf_.resize(features.size());
for (size_t i = 0; i < features.size(); ++i) {
const auto& feature = features[i];
auto& cloud_pre = clouds_corn_pre_[i];
cloud_pre->resize(feature.cloud_corner->size());
for (size_t j = 0; j < feature.cloud_corner->size(); ++j) {
TransformToEnd(pose_curr2last_, feature.cloud_corner->at(j),
&cloud_pre->at(j));
}
kdtrees_corn_[i].setInputCloud(clouds_corn_pre_[i]);
}
surf_pre_->resize(feature.cloud_less_flat_surf->size());
for (size_t i = 0; i < feature.cloud_less_flat_surf->size(); ++i) {
TransformToEnd(pose_curr2last_, feature.cloud_less_flat_surf->points[i],
&surf_pre_->points[i]);
for (size_t i = 0; i < features.size(); ++i) {
const auto& feature = features[i];
auto& cloud_pre = clouds_surf_pre_[i];
cloud_pre->resize(feature.cloud_surf->size());
for (size_t j = 0; j < feature.cloud_surf->size(); ++j) {
TransformToEnd(pose_curr2last_, feature.cloud_surf->at(j),
&cloud_pre->at(j));
}
kdtrees_surf_[i].setInputCloud(clouds_surf_pre_[i]);
}
corn_pre_->resize(feature.cloud_less_sharp_corner->size());
for (size_t i = 0; i < feature.cloud_less_sharp_corner->size(); ++i) {
TransformToEnd(pose_curr2last_, feature.cloud_less_sharp_corner->points[i],
&corn_pre_->points[i]);
}
kdtree_surf_->setInputCloud(surf_pre_);
kdtree_corn_->setInputCloud(corn_pre_);
}
void Odometer::Visualize(const Feature& feature,
void Odometer::NearestKSearch(
const std::vector<pcl::KdTreeFLANN<common::TPoint>>& kdtrees,
const TPoint& query_pt, size_t k,
std::vector<std::vector<int>>* const indices,
std::vector<std::vector<float>>* const dists) const {
for (const auto& kdtree : kdtrees) {
std::vector<int> index;
std::vector<float> dist;
if (kdtree.getInputCloud()->size() >= k) {
kdtree.nearestKSearch(query_pt, k, index, dist);
} else {
index.resize(k, -1);
dist.resize(k, std::numeric_limits<float>::max());
}
indices->push_back(std::move(index));
dists->push_back(std::move(dist));
}
}
void Odometer::Visualize(const std::vector<Feature>& features,
const std::vector<PointLinePair>& pl_pairs,
const std::vector<PointPlanePair>& pp_pairs,
double timestamp) const {
@ -204,5 +192,4 @@ void Odometer::Visualize(const Feature& feature,
frame->pose_curr2world = pose_curr2world_;
visualizer_->Render(frame);
}
} // namespace oh_my_loam

34
oh_my_loam/odometer/odometer.h
View File

@ -1,7 +1,7 @@
#pragma once
#include "common/common.h"
#include "common/geometry/pose.h"
#include "common/geometry/pose3d.h"
#include "oh_my_loam/base/feature.h"
#include "oh_my_loam/base/helper.h"
#include "oh_my_loam/visualizer/odometer_visualizer.h"
@ -16,32 +16,36 @@ class Odometer {
bool Init();
void Process(double timestamp, const std::vector<Feature>& features,
common::Pose3D* const pose);
common::Pose3d* const pose);
protected:
void UpdatePre(const std::vector<Feature>& feature) s;
void UpdatePre(const std::vector<Feature>& features);
void MatchCornFeature(const common::TPointCloud& src,
const common::TPointCloud& tgt,
std::vector<PointLinePair>* const pairs) const;
void MatchCorn(const common::TPointCloud& src,
std::vector<PointLinePair>* const pairs) const;
void MatchSurfFeature(const common::TPointCloud& src,
const common::TPointCloud& tgt,
std::vector<PointPlanePair>* const pairs) const;
void MatchSurf(const common::TPointCloud& src,
std::vector<PointPlanePair>* const pairs) const;
void Visualize(const std::vector<Feature>& features,
const std::vector<PointLinePair>& pl_pairs,
const std::vector<PointPlanePair>& pp_pairs,
double timestamp = 0.0) const;
common::Pose3D pose_curr2world_;
common::Pose3D pose_curr2last_;
void NearestKSearch(
const std::vector<pcl::KdTreeFLANN<common::TPoint>>& kdtrees,
const TPoint& query_pt, size_t k,
std::vector<std::vector<int>>* const indices,
std::vector<std::vector<float>>* const dists) const;
common::TPointCloudPtr cloud_corn_pre_{nullptr};
common::TPointCloudPtr cloud_surf_pre_{nullptr};
common::Pose3d pose_curr2world_;
common::Pose3d pose_curr2last_;
pcl::KdTreeFLANN<common::TPoint>::Ptr kdtree_surf_{nullptr};
pcl::KdTreeFLANN<common::TPoint>::Ptr kdtree_corn_{nullptr};
std::vector<common::TPointCloudPtr> clouds_corn_pre_;
std::vector<common::TPointCloudPtr> clouds_surf_pre_;
std::vector<pcl::KdTreeFLANN<common::TPoint>> kdtrees_surf_;
std::vector<pcl::KdTreeFLANN<common::TPoint>> kdtrees_corn_;
YAML::Node config_;

16
oh_my_loam/oh_my_loam.cc
View File

@ -17,11 +17,11 @@ bool OhMyLoam::Init() {
AERROR << "Failed to initialize extractor";
return false;
}
// odometer_.reset(new Odometer);
// if (!odometer_->Init()) {
// AERROR << "Failed to initialize odometer";
// return false;
// }
odometer_.reset(new Odometer);
if (!odometer_->Init()) {
AERROR << "Failed to initialize odometer";
return false;
}
// mapper_.reset(new Mapper);
// if (!mapper_->Init()) {
// AERROR << "Failed to initialize mapper";
@ -35,9 +35,9 @@ void OhMyLoam::Run(double timestamp, const PointCloudConstPtr& cloud_in) {
RemoveOutliers(*cloud_in, cloud.get());
std::vector<Feature> features;
extractor_->Process(timestamp, cloud, &features);
// Pose3D pose;
// odometer_->Process(timestamp, feature, &pose);
// poses_.emplace_back(pose);
Pose3d pose;
odometer_->Process(timestamp, features, &pose);
poses_.emplace_back(pose);
}
void OhMyLoam::RemoveOutliers(const PointCloud& cloud_in,

6
oh_my_loam/oh_my_loam.h
View File

@ -3,7 +3,7 @@
#include "common/common.h"
#include "oh_my_loam/extractor/extractor.h"
// #include "oh_my_loam/mapper/mapper.h"
// #include "oh_my_loam/odometer/odometer.h"
#include "oh_my_loam/odometer/odometer.h"
namespace oh_my_loam {
@ -18,7 +18,7 @@ class OhMyLoam {
private:
std::unique_ptr<Extractor> extractor_{nullptr};
// std::unique_ptr<Odometer> odometer_{nullptr};
std::unique_ptr<Odometer> odometer_{nullptr};
// std::unique_ptr<Mapper> mapper_{nullptr};
@ -26,7 +26,7 @@ class OhMyLoam {
void RemoveOutliers(const common::PointCloud& cloud_in,
common::PointCloud* const cloud_out) const;
std::vector<common::Pose3D> poses_;
std::vector<common::Pose3d> poses_;
DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
};

40
oh_my_loam/solver/solver.cc
View File

@ -6,26 +6,16 @@ namespace {
double kHuberLossScale = 0.1;
}
PoseSolver::PoseSolver(double *q, double *p) : q_(q), p_(p) {
PoseSolver::PoseSolver(const common::Pose3d &pose)
: r_quat_(pose.r_quat().coeffs().data()), t_vec_(pose.t_vec().data()) {
loss_function_ = new ceres::HuberLoss(kHuberLossScale);
problem_.AddParameterBlock(q_, 4,
problem_.AddParameterBlock(r_quat_, 4,
new ceres::EigenQuaternionParameterization());
problem_.AddParameterBlock(p_, 3);
problem_.AddParameterBlock(t_vec_, 3);
}
void PoseSolver::AddPointLinePair(const PointLinePair &pair, double time) {
ceres::CostFunction *cost_function =
PointLineCostFunction::Create(pair, time);
problem_.AddResidualBlock(cost_function, loss_function_, q_, p_);
}
void PoseSolver::AddPointPlanePair(const PointPlanePair &pair, double time) {
ceres::CostFunction *cost_function =
PointPlaneCostFunction::Create(pair, time);
problem_.AddResidualBlock(cost_function, loss_function_, q_, p_);
}
void PoseSolver::Solve(int max_iter_num, bool verbose) {
double PoseSolver::Solve(int max_iter_num, bool verbose,
common::Pose3d *const pose) {
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = max_iter_num;
@ -33,6 +23,24 @@ void PoseSolver::Solve(int max_iter_num, bool verbose) {
ceres::Solver::Summary summary;
ceres::Solve(options, &problem_, &summary);
AINFO_IF(verbose) << summary.BriefReport();
if (pose) *pose = common::Pose3d(r_quat_, t_vec_);
return summary.final_cost;
}
void PoseSolver::AddPointLinePair(const PointLinePair &pair, double time) {
ceres::CostFunction *cost_function =
PointLineCostFunction::Create(pair, time);
problem_.AddResidualBlock(cost_function, loss_function_, r_quat_, t_vec_);
}
void PoseSolver::AddPointPlanePair(const PointPlanePair &pair, double time) {
ceres::CostFunction *cost_function =
PointPlaneCostFunction::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_);
}
} // oh_my_loam

11
oh_my_loam/solver/solver.h
View File

@ -1,25 +1,30 @@
#pragma once
#include "common/geometry/pose3d.h"
#include "cost_function.h"
namespace oh_my_loam {
class PoseSolver {
public:
PoseSolver(double* q, double* p);
PoseSolver(const common::Pose3d& pose);
void AddPointLinePair(const PointLinePair& pair, double time);
void AddPointPlanePair(const PointPlanePair& pair, double time);
void Solve(int max_iter_num = 5, bool verbose = false);
double Solve(int max_iter_num = 5, bool verbose = false,
common::Pose3d* const pose = nullptr);
common::Pose3d GetPose() const;
private:
ceres::Problem problem_;
ceres::LossFunction* loss_function_;
double *q_, *p_;
// r_quat_: [x, y, z, w], t_vec_: [x, y, z]
double *r_quat_, *t_vec_;
DISALLOW_COPY_AND_ASSIGN(PoseSolver)
};

8
oh_my_loam/visualizer/odometer_visualizer.cc
View File

@ -33,16 +33,16 @@ void OdometerVisualizer::Draw() {
DrawPointCloud<TPoint>(tgt_corn_pts, BLUE, "tgt_corn_pts", 4);
DrawPointCloud<TPoint>(src_surf_pts, PURPLE, "src_surf_pts", 7);
DrawPointCloud<TPoint>(tgt_surf_pts, RED, "tgt_surf_pts", 4);
std::vector<Pose3D> poses_n;
std::vector<Pose3d> poses_n;
poses_n.reserve((poses_.size()));
Pose3D pose_inv = frame.pose_curr2world.Inv();
Pose3d pose_inv = frame.pose_curr2world.Inv();
for (const auto& pose : poses_) {
poses_n.emplace_back(pose_inv * pose);
};
for (size_t i = 0; i < poses_n.size(); ++i) {
Eigen::Vector3f p1 = poses_n[i].p().cast<float>();
Eigen::Vector3f p1 = poses_n[i].t_vec().cast<float>();
if (i < poses_n.size() - 1) {
Eigen::Vector3f p2 = poses_n[i + 1].p().cast<float>();
Eigen::Vector3f p2 = poses_n[i + 1].t_vec().cast<float>();
AddLine(Point(p1.x(), p1.y(), p1.z()), Point(p2.x(), p2.y(), p2.z()),
WHITE, "line" + std::to_string(i), viewer_.get());
} else {

6
oh_my_loam/visualizer/odometer_visualizer.h
View File

@ -10,8 +10,8 @@ struct OdometerVisFrame : public common::LidarVisFrame {
common::TPointCloudPtr cloud_corner;
std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs;
common::Pose3D pose_curr2last;
common::Pose3D pose_curr2world;
common::Pose3d pose_curr2last;
common::Pose3d pose_curr2world;
};
class OdometerVisualizer : public common::LidarVisualizer {
@ -23,7 +23,7 @@ class OdometerVisualizer : public common::LidarVisualizer {
private:
void Draw() override;
std::deque<common::Pose3D> poses_;
std::deque<common::Pose3d> poses_;
};
} // namespace oh_my_loam