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

minor changes

main
feixyz10 2020-11-02 01:12:53 +08:00 committed by feixyz
parent 7a56d2337c
commit 469b6a9804
12 changed files with 63 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -1,9 +1,9 @@
cmake_minimum_required(VERSION 2.8.3)
project(oh_my_loam)
set(CMAKE_CXX_FLAGS "-std=c++17 -Wall -lpthread")
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++17 -lpthread")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall")
set(CMAKE_CXX_FLAGS_RELEASE "-O3")
find_package(Ceres REQUIRED)
find_package(PCL QUIET)

4
common/macros.h
View File

@ -4,8 +4,6 @@
#include <memory>
#include <mutex>
// #define LOG_TIMESTAMP(timestamp, precision)
// std::fixed << std::precision(precision) << timestamp;
#define LOG_TIMESTAMP(timestamp) \
std::fixed << std::setprecision(3) << timestamp;
@ -17,7 +15,7 @@
#define DECLARE_SINGLETON(classname) \
public: \
static classname *Instance() { \
static std::unique_ptr<classname> instance = nullptr; \
static std::unique_ptr<classname> instance{nullptr}; \
if (!instance) { \
static std::once_flag flag; \
std::call_once(flag, \

4
common/utils.h
View File

@ -77,11 +77,11 @@ void RemoveClosedPoints(const pcl::PointCloud<PointT>& cloud_in,
}
template <typename PointT>
void VoxelDownSample(const pcl::PointCloud<PointT>& cloud_in,
void VoxelDownSample(const typename pcl::PointCloud<PointT>::ConstPtr& cloud_in,
pcl::PointCloud<PointT>* const cloud_out,
double voxel_size) {
pcl::VoxelGrid<PointT> filter;
filter.setInputCloud(cloud_in.makeShared());
filter.setInputCloud(cloud_in);
filter.setLeafSize(voxel_size, voxel_size, voxel_size);
filter.filter(*cloud_out);
}

9
configs/config.yaml
View File

@ -10,13 +10,14 @@ extractor_config:
vis: true
sharp_corner_point_num: 2
corner_point_num: 20
flat_surf_point_num: 4
surf_point_num: 20 ## useless currently
flat_surf_point_num: 3
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.2
downsample_voxel_size: 0.5
odometry_config:
icp_iter_num : 2
match_dist_thresh: 0.5
match_dist_sq_thresh: 1
vis: false

22
src/extractor/base_extractor.cc
View File

@ -40,18 +40,9 @@ void Extractor::Process(const PointCloud& cloud, FeaturePoints* const feature) {
}
double time_assign = timer.toc();
// store points into feature point clouds according to their type
std::ostringstream oss;
oss << "Feature point num: ";
for (const auto& scan : scans) {
FeaturePoints scan_feature;
GenerateFeaturePoints(scan, &scan_feature);
feature->Add(scan_feature);
oss << scan.size() << ":" << scan_feature.sharp_corner_pts->size() << ":"
<< scan_feature.less_sharp_corner_pts->size() << ":"
<< scan_feature.flat_surf_pts->size() << ":"
<< scan_feature.less_flat_surf_pts->size() << " ";
GenerateFeaturePoints(scan, feature);
}
ADEBUG << oss.str();
double time_store = timer.toc();
AINFO << "Time elapsed (ms): scan_split = " << std::setprecision(3)
<< time_split << ", curvature_compute = " << time_curv - time_split
@ -190,6 +181,7 @@ 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);
for (const auto& pt : scan.points) {
switch (pt.type) {
case PointType::FLAT:
@ -214,11 +206,11 @@ void Extractor::GenerateFeaturePoints(const TCTPointCloud& scan,
break;
}
}
TPointCloudPtr filtered_less_flat_surf_pts(new TPointCloud);
VoxelDownSample(*feature->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,

15
src/extractor/extractor_VLP16.cc
View File

@ -3,14 +3,15 @@
namespace oh_my_loam {
int ExtractorVLP16::GetScanID(const Point& pt) const {
static int i = 0;
double omega = std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0;
if (i++ < 10) {
ADEBUG << "OMEGA: " << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0
<< " id = " << static_cast<int>(std::round(omega / 2.0) + 0.01)
<< " z = " << pt.z << " "
<< " d = " << Distance(pt) << std::endl;
}
// static int i = 0;
// if (i++ < 10) {
// ADEBUG << "OMEGA: " << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI
// + 15.0
// << " id = " << static_cast<int>(std::round(omega / 2.0) + 0.01)
// << " z = " << pt.z << " "
// << " d = " << Distance(pt) << std::endl;
// }
return static_cast<int>(std::round(omega / 2.0) + 1.e-5);
};

7
src/extractor/feature_points.h
View File

@ -16,13 +16,6 @@ struct FeaturePoints {
flat_surf_pts.reset(new TPointCloud);
less_flat_surf_pts.reset(new TPointCloud);
}
void Add(const FeaturePoints& rhs) {
*sharp_corner_pts += *rhs.sharp_corner_pts;
*less_sharp_corner_pts += *rhs.less_sharp_corner_pts;
*flat_surf_pts += *rhs.flat_surf_pts;
*less_flat_surf_pts += *rhs.less_flat_surf_pts;
}
};
} // namespace oh_my_loam

6
src/helper/helper.cc
View File

@ -2,8 +2,6 @@
namespace oh_my_loam {
float GetTime(const TPoint& pt) { return pt.time - GetScanId(pt); }
//
int GetScanId(const TPoint& pt) { return static_cast<int>(pt.time); }
} // oh_my_loam
} // namespace oh_my_loam

8
src/helper/helper.h
View File

@ -4,9 +4,11 @@
namespace oh_my_loam {
float GetTime(const TPoint& pt);
inline float GetTime(const TPoint& pt) {
return pt.time - static_cast<int>(pt.time);
}
int GetScanId(const TPoint& pt);
inline int GetScanId(const TPoint& pt) { return static_cast<int>(pt.time); }
struct PointLinePair {
TPoint pt;
@ -36,4 +38,4 @@ struct PointPlanePair {
: pt(pt), plane(pt1, pt2, pt3) {}
};
} // oh_my_loam
} // namespace oh_my_loam

48
src/odometry/odometry.cc
View File

@ -1,4 +1,5 @@
#include "odometry.h"
#include "solver/solver.h"
namespace oh_my_loam {
@ -20,21 +21,25 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
is_initialized = true;
UpdatePre(feature);
*pose = pose_curr2world_;
AINFO << "Odometry initialized....";
AWARN << "Odometry initialized....";
return;
}
double match_dist_thresh = config_["match_dist_thresh"].as<double>();
TicToc timer_whole;
double match_dist_sq_thresh = config_["match_dist_sq_thresh"].as<double>();
AINFO << "Pose before: " << pose_curr2world_.ToString();
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
TicToc timer;
std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs;
MatchCornPoints(*feature.sharp_corner_pts, *corn_pts_pre_, &pl_pairs,
match_dist_thresh);
match_dist_sq_thresh);
double time_pl_match = timer.toc();
AINFO_IF(i == 0) << "PL mathch: src size = "
<< feature.sharp_corner_pts->size()
<< ", tgt size = " << corn_pts_pre_->size();
MatchSurfPoints(*feature.flat_surf_pts, *surf_pts_pre_, &pp_pairs,
match_dist_thresh);
match_dist_sq_thresh);
double timer_pp_match = timer.toc();
AINFO_IF(i == 0) << "PP mathch: src size = "
<< feature.flat_surf_pts->size()
<< ", tgt size = " << surf_pts_pre_->size();
@ -44,8 +49,10 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
AWARN << "Too less correspondence: num = "
<< pl_pairs.size() + pp_pairs.size();
}
double* q = pose_curr2last_.q().coeffs().data();
double* p = pose_curr2last_.p().data();
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);
for (const auto& pair : pl_pairs) {
solver.AddPointLinePair(pair, GetTime(pair.pt));
@ -55,27 +62,30 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
}
solver.Solve();
pose_curr2last_ = Pose3D(q, p);
double time_solve = timer.toc();
AINFO << "Time elapsed (ms), pl_match = " << time_pl_match
<< ", pp_match = " << timer_pp_match - time_pl_match
<< ", solve = " << time_solve - timer_pp_match;
}
pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
*pose = pose_curr2world_;
AWARN << "Pose increase: " << pose_curr2last_.ToString();
AWARN << "Pose after: " << pose_curr2world_.ToString();
UpdatePre(feature);
AINFO << "Time elapsed (ms): whole odometry = " << timer_whole.toc();
}
void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointLinePair>* const pairs,
double dist_thresh) const {
kdtree_corn_pts_->setInputCloud(tgt.makeShared());
double dist_sq_thresh) const {
for (const auto& query_pt : src) {
std::vector<int> indices;
std::vector<float> dists;
kdtree_corn_pts_->nearestKSearch(query_pt, 1, indices, dists);
if (dists[0] >= dist_thresh) continue;
if (dists[0] >= dist_sq_thresh) continue;
TPoint pt1 = tgt.points[indices[0]];
int pt2_idx = -1;
double min_dist_pt2_squre = dist_thresh * dist_thresh;
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];
@ -108,25 +118,23 @@ 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_thresh) const {
kdtree_surf_pts_->setInputCloud(tgt.makeShared());
AINFO << "Point to plane mathch: src size = " << src.size()
<< ", tgt size = " << tgt.size();
double dist_sq_thresh) const {
for (const auto& query_pt : src) {
std::vector<int> indices;
std::vector<float> dists;
kdtree_surf_pts_->nearestKSearch(query_pt, 1, indices, dists);
if (dists[0] >= dist_thresh) continue;
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_thresh * dist_thresh;
double min_dist_pt3_squre = dist_thresh * dist_thresh;
double min_dist_pt2_squre = dist_sq_thresh;
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);
// 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;
@ -140,9 +148,9 @@ void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
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);
// 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,6 +171,8 @@ void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
void Odometry::UpdatePre(const FeaturePoints& feature) {
surf_pts_pre_ = feature.less_flat_surf_pts;
corn_pts_pre_ = feature.less_sharp_corner_pts;
kdtree_surf_pts_->setInputCloud(surf_pts_pre_);
kdtree_corn_pts_->setInputCloud(corn_pts_pre_);
}
} // namespace oh_my_loam

4
src/odometry/odometry.h
View File

@ -20,11 +20,11 @@ class Odometry {
void UpdatePre(const FeaturePoints& feature);
void MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointLinePair>* const pairs,
double dist_thresh) const;
double dist_sq_thresh) const;
void MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointPlanePair>* const pairs,
double dist_thresh) const;
double dist_sq_thresh) const;
Pose3D pose_curr2world_;
Pose3D pose_curr2last_;

2
src/oh_my_loam.cc
View File

@ -26,8 +26,6 @@ bool OhMyLoam::Init() {
void OhMyLoam::Run(const PointCloud& cloud_in, double timestamp) {
PointCloudPtr cloud(new PointCloud);
RemoveOutliers(cloud_in, cloud.get());
ADEBUG << "After remove, point num: " << cloud_in.size() << " -> "
<< cloud->size();
FeaturePoints feature_points;
extractor_->Process(*cloud, &feature_points);
Pose3D pose;