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

odometry vis to be done

main
feixyz10 2020-11-02 17:16:42 +08:00 committed by feixyz
parent 469b6a9804
commit 2a052c462e
11 changed files with 171 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
common/log.h
View File

@ -6,24 +6,41 @@
#include <g3log/logworker.hpp>
#include <iostream>
// heere we define G3LOG instead of use LOG directly, since LOG macro in g3log
// is conflict with LOG macro in glog. Same for G3LOG_IF and G3CHECK
#define G3LOG(level) \
if (!g3::logLevel(level)) { \
} else \
INTERNAL_LOG_MESSAGE(level).stream()
#define G3LOG_IF(level, boolean_expression) \
if (false == (boolean_expression) || !g3::logLevel(level)) { \
} else \
INTERNAL_LOG_MESSAGE(level).stream()
#define G3CHECK(boolean_expression) \
if (true == (boolean_expression)) { \
} else \
INTERNAL_CONTRACT_MESSAGE(#boolean_expression).stream()
const LEVELS ERROR{WARNING.value + 100, "ERROR"};
const LEVELS USER(ERROR.value + 100, "USER");
#define ADEBUG LOG(DEBUG)
#define AINFO LOG(INFO)
#define AWARN LOG(WARNING)
#define AERROR LOG(ERROR)
#define AUSER LOG(USER)
#define AFATAL LOG(FATAL)
#define ADEBUG G3LOG(DEBUG)
#define AINFO G3LOG(INFO)
#define AWARN G3LOG(WARNING)
#define AERROR G3LOG(ERROR)
#define AUSER G3LOG(USER)
#define AFATAL G3LOG(FATAL)
// LOG_IF
#define ADEBUG_IF(cond) LOG_IF(DEBUG, cond)
#define AINFO_IF(cond) LOG_IF(INFO, cond)
#define AWARN_IF(cond) LOG_IF(WARNING, cond)
#define AERROR_IF(cond) LOG_IF(ERROR, cond)
#define AUSER_IF(cond) LOG_IF(USER, cond)
#define AFATAL_IF(cond) LOG_IF(FATAL, cond)
#define ACHECK(cond) CHECK(cond)
#define ADEBUG_IF(cond) G3LOG_IF(DEBUG, cond)
#define AINFO_IF(cond) G3LOG_IF(INFO, cond)
#define AWARN_IF(cond) G3LOG_IF(WARNING, cond)
#define AERROR_IF(cond) G3LOG_IF(ERROR, cond)
#define AUSER_IF(cond) G3LOG_IF(USER, cond)
#define AFATAL_IF(cond) G3LOG_IF(FATAL, cond)
#define ACHECK(cond) G3CHECK(cond)
namespace g3 {
class CustomSink {

2
common/pose.cc
View File

@ -3,7 +3,7 @@
namespace oh_my_loam {
Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t) {
return pose_from.InterPolate(pose_to, t);
return pose_from.Interpolate(pose_to, t);
}
Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs) {

18
common/pose.h
View File

@ -20,9 +20,9 @@ class Pose3D {
Pose3D(const double* const q, const double* const p) : q_(q), p_(p) {}
Pose3D Inv() const {
auto q_inv = q_.inverse();
auto p_inv = q_inv * p_;
return {q_inv, p_inv};
Eigen::Quaterniond q_inv = q_.inverse();
Eigen::Vector3d p_inv = q_inv * p_;
return {q_inv, -p_inv};
}
Eigen::Vector3d Transform(const Eigen::Vector3d& vec) const {
@ -37,9 +37,9 @@ class Pose3D {
PointT Transform(const PointT& pt) const {
PointT pt_n = pt;
Eigen::Vector3d vec = Transform(Eigen::Vector3d(pt.x, pt.y, pt.z));
pt_n.x = vec.x;
pt_n.y = vec.y;
pt_n.z = vec.z;
pt_n.x = static_cast<float>(vec.x());
pt_n.y = static_cast<float>(vec.y());
pt_n.z = static_cast<float>(vec.z());
return pt_n;
}
@ -55,9 +55,9 @@ class Pose3D {
Eigen::Vector3d Rotate(const Eigen::Vector3d& vec) const { return q_ * vec; }
// Spherical linear interpolation to `pose_to`, `t` belongs [0, 1]
Pose3D InterPolate(const Pose3D& pose_to, double t) const {
auto q_interp = q_.slerp(t, pose_to.q_);
auto p_interp = (pose_to.p_ - p_) * t;
Pose3D Interpolate(const Pose3D& pose_to, double t) const {
Eigen::Quaterniond q_interp = q_.slerp(t, pose_to.q_);
Eigen::Vector3d p_interp = (pose_to.p_ - p_) * t + p_;
return {q_interp, p_interp};
}

5
common/types.h
View File

@ -46,7 +46,12 @@ using TCTPointCloudConstPtr = TCTPointCloud::ConstPtr;
struct PointXYZT {
PCL_ADD_POINT4D;
union EIGEN_ALIGN16 {
float time;
// make sure VoxelGrid can work with this custom point type:
// https://github.com/PointCloudLibrary/pcl/issues/2331
float intensity;
};
PointXYZT() {
x = y = z = 0.0f;

4
configs/config.yaml
View File

@ -10,12 +10,12 @@ extractor_config:
vis: true
sharp_corner_point_num: 2
corner_point_num: 20
flat_surf_point_num: 3
flat_surf_point_num: 4
surf_point_num: 3 ## useless currently
corner_point_curvature_thres: 0.5
surf_point_curvature_thres: 0.5
neighbor_point_dist_thres: 0.05
downsample_voxel_size: 0.5
downsample_voxel_size: 0.3
odometry_config:
icp_iter_num : 2

21
src/extractor/base_extractor.cc
View File

@ -47,8 +47,8 @@ void Extractor::Process(const PointCloud& cloud, FeaturePoints* const feature) {
AINFO << "Time elapsed (ms): scan_split = " << std::setprecision(3)
<< time_split << ", curvature_compute = " << time_curv - time_split
<< ", type_assign = " << time_assign - time_curv
<< ", store = " << time_store - time_assign
<< ", TOTAL = " << time_store;
<< ", store = " << time_store - time_assign;
AUSER << "Time elapsed (ms): whole extraction = " << time_store;
if (is_vis_) Visualize(cloud, *feature);
}
@ -181,15 +181,14 @@ void Extractor::SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
void Extractor::GenerateFeaturePoints(const TCTPointCloud& scan,
FeaturePoints* const feature) const {
// TPointCloudPtr less_flat_surf_pts(new TPointCloud);
TPointCloudPtr less_flat_surf_pts(new TPointCloud);
for (const auto& pt : scan.points) {
switch (pt.type) {
case PointType::FLAT:
feature->flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, pt.time);
// no break: FLAT points are also LESS_FLAT
case PointType::LESS_FLAT:
feature->less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z,
pt.time);
less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, pt.time);
break;
case PointType::SHARP:
feature->sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z,
@ -201,16 +200,14 @@ void Extractor::GenerateFeaturePoints(const TCTPointCloud& scan,
break;
default:
// all the rest are also LESS_FLAT
feature->less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z,
pt.time);
less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, pt.time);
break;
}
}
// TPointCloudPtr filtered_less_flat_surf_pts(new TPointCloud);
// VoxelDownSample<TPoint>(less_flat_surf_pts,
// filtered_less_flat_surf_pts.get(),
// config_["downsample_voxel_size"].as<double>());
// *feature->less_flat_surf_pts += *filtered_less_flat_surf_pts;
TPointCloudPtr filtered_less_flat_surf_pts(new TPointCloud);
VoxelDownSample<TPoint>(less_flat_surf_pts, filtered_less_flat_surf_pts.get(),
config_["downsample_voxel_size"].as<double>());
*feature->less_flat_surf_pts += *filtered_less_flat_surf_pts;
}
void Extractor::Visualize(const PointCloud& cloud,

24
src/helper/helper.h
View File

@ -10,6 +10,30 @@ inline float GetTime(const TPoint& pt) {
inline int GetScanId(const TPoint& pt) { return static_cast<int>(pt.time); }
/**
* @brief Transform a lidar point to the start of the scan
*
* @param pose Relative pose, end scan time w.r.t. start scan time
* @param time Point time relative to the start time of the scan, \in [0, 1]
*/
void TransformToStart(const Pose3D& pose, const TPoint& pt_in,
TPoint* const pt_out) {
Pose3D pose_interp = Pose3D().Interpolate(pose, GetTime(pt_in));
*pt_out = pose_interp * 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
* @param time Point time relative to the start time of the scan, \in [0, 1]
*/
void TransformToEnd(const Pose3D& pose, const TPoint& pt_in,
TPoint* const pt_out) {
TransformToStart(pose, pt_in, pt_out);
*pt_out = pose.Inv() * (*pt_out);
}
struct PointLinePair {
TPoint pt;
struct Line {

35
src/odometry/odometry.cc
View File

@ -72,13 +72,15 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
AWARN << "Pose increase: " << pose_curr2last_.ToString();
AWARN << "Pose after: " << pose_curr2world_.ToString();
UpdatePre(feature);
AINFO << "Time elapsed (ms): whole odometry = " << timer_whole.toc();
AUSER << "Time elapsed (ms): whole odometry = " << timer_whole.toc();
}
void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointLinePair>* const pairs,
double dist_sq_thresh) const {
for (const auto& query_pt : src) {
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_pts_->nearestKSearch(query_pt, 1, indices, dists);
@ -111,7 +113,7 @@ void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
}
if (pt2_idx >= 0) {
TPoint pt2 = tgt.points[pt2_idx];
pairs->emplace_back(query_pt, pt1, pt2);
pairs->emplace_back(q_pt, pt1, pt2);
}
}
}
@ -119,7 +121,9 @@ void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointPlanePair>* const pairs,
double dist_sq_thresh) const {
for (const auto& query_pt : src) {
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_pts_->nearestKSearch(query_pt, 1, indices, dists);
@ -134,7 +138,6 @@ void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
int scan_id = GetScanId(pt);
if (scan_id > query_pt_scan_id + kNearbyScanNum) break;
double dist_squre = DistanceSqure(query_pt, pt);
// AWARN_IF(scan_id != 0) << "SA" << query_pt_scan_id << ", " << scan_id;
if (scan_id == query_pt_scan_id && dist_squre < min_dist_pt2_squre) {
pt2_idx = i;
min_dist_pt2_squre = dist_squre;
@ -150,7 +153,6 @@ void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
int scan_id = GetScanId(pt);
if (scan_id < query_pt_scan_id - kNearbyScanNum) break;
double dist_squre = DistanceSqure(query_pt, pt);
// AWARN_IF(scan_id != 0) << "SD" << query_pt_scan_id << ", " << scan_id;
if (scan_id == query_pt_scan_id && dist_squre < min_dist_pt2_squre) {
pt2_idx = i;
min_dist_pt2_squre = dist_squre;
@ -163,14 +165,29 @@ void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
}
if (pt2_idx >= 0 && pt3_idx >= 0) {
TPoint pt2 = tgt.points[pt2_idx], pt3 = tgt.points[pt3_idx];
pairs->emplace_back(query_pt, pt1, pt2, pt3);
pairs->emplace_back(q_pt, pt1, pt2, pt3);
}
}
}
void Odometry::UpdatePre(const FeaturePoints& feature) {
surf_pts_pre_ = feature.less_flat_surf_pts;
corn_pts_pre_ = feature.less_sharp_corner_pts;
if (surf_pts_pre_ == nullptr) {
surf_pts_pre_.reset(new TPointCloud);
}
if (corn_pts_pre_ == nullptr) {
corn_pts_pre_.reset(new TPointCloud);
}
surf_pts_pre_->resize(feature.less_flat_surf_pts->size());
for (size_t i = 0; i < feature.less_flat_surf_pts->size(); ++i) {
TransformToEnd(pose_curr2last_, feature.less_flat_surf_pts->points[i],
&surf_pts_pre_->points[i]);
}
corn_pts_pre_->resize(feature.less_sharp_corner_pts->size());
for (size_t i = 0; i < feature.less_sharp_corner_pts->size(); ++i) {
TransformToEnd(pose_curr2last_, feature.less_sharp_corner_pts->points[i],
&corn_pts_pre_->points[i]);
}
kdtree_surf_pts_->setInputCloud(surf_pts_pre_);
kdtree_corn_pts_->setInputCloud(corn_pts_pre_);
}

8
src/odometry/odometry.h
View File

@ -29,11 +29,11 @@ class Odometry {
Pose3D pose_curr2world_;
Pose3D pose_curr2last_;
TPointCloudPtr surf_pts_pre_;
TPointCloudPtr corn_pts_pre_;
TPointCloudPtr surf_pts_pre_{nullptr};
TPointCloudPtr corn_pts_pre_{nullptr};
pcl::KdTreeFLANN<TPoint>::Ptr kdtree_surf_pts_;
pcl::KdTreeFLANN<TPoint>::Ptr kdtree_corn_pts_;
pcl::KdTreeFLANN<TPoint>::Ptr kdtree_surf_pts_{nullptr};
pcl::KdTreeFLANN<TPoint>::Ptr kdtree_corn_pts_{nullptr};
bool is_initialized = false;
YAML::Node config_;

38
src/visualizer/odometry_visualizer.cc Normal file
View File

@ -0,0 +1,38 @@
#include "Odometry_visualizer.h"
namespace oh_my_loam {
void OdometryVisualizer::Draw() {
auto frame = GetCurrentFrame<OdometryVisFrame>();
{ // add raw point cloud
std::string id = "raw point cloud";
DrawPointCloud(*frame.cloud, WHITE, id, viewer_.get());
rendered_cloud_ids_.push_back(id);
}
{ // add less_flat_surf_pts
std::string id = "less_flat_surf_pts";
DrawPointCloud(*frame.feature_pts.less_flat_surf_pts, CYAN, id,
viewer_.get(), 5);
rendered_cloud_ids_.push_back(id);
}
{ // add flat_surf_pts
std::string id = "flat_surf_pts";
DrawPointCloud(*frame.feature_pts.flat_surf_pts, BLUE, id, viewer_.get(),
7);
rendered_cloud_ids_.push_back(id);
}
{ // add less_sharp_corner_pts
std::string id = "less_sharp_corner_pts";
DrawPointCloud(*frame.feature_pts.less_sharp_corner_pts, PURPLE, id,
viewer_.get(), 5);
rendered_cloud_ids_.push_back(id);
}
{ // add sharp_corner_pts
std::string id = "sharp_corner_pts";
DrawPointCloud(*frame.feature_pts.sharp_corner_pts, RED, id, viewer_.get(),
7);
rendered_cloud_ids_.push_back(id);
}
};
} // namespace oh_my_loam

22
src/visualizer/odometry_visualizer.h Normal file
View File

@ -0,0 +1,22 @@
#pragma once
#include "base_visualizer.h"
#include "extractor/feature_points.h"
namespace oh_my_loam {
struct OdometryVisFrame : public VisFrame {
FeaturePoints feature_pts;
};
class OdometryVisualizer : public Visualizer {
public:
explicit OdometryVisualizer(const std::string &name = "OdometryVisualizer",
size_t max_history_size = 10)
: Visualizer(name, max_history_size) {}
private:
void Draw() override;
};
} // namespace oh_my_loam