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

odometer ok

main
feixyz10 2021-01-23 22:11:08 +08:00 committed by feixyz
parent 12054bf10f
commit 38b85ab8f2
16 changed files with 245 additions and 215 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
common/geometry/pose3d.h
View File

@ -77,13 +77,13 @@ class Pose3d {
// const Eigen::Quaterniond& r_quat() const { return r_quat_; } // const Eigen::Quaterniond& r_quat() const { return r_quat_; }
Eigen::Quaterniond r_quat() const { const Eigen::Quaterniond &r_quat() const {
return r_quat_; return r_quat_;
} }
// const Eigen::Vector3d& t_vec() const { return t_vec_; } // const Eigen::Vector3d& t_vec() const { return t_vec_; }
Eigen::Vector3d t_vec() const { const Eigen::Vector3d &t_vec() const {
return t_vec_; return t_vec_;
} }

2
common/math/math_utils.cc
View File

@ -18,7 +18,7 @@ double Rad2Degree(double rad) {
} }
const std::vector<int> Range(int begin, int end, int step) { const std::vector<int> Range(int begin, int end, int step) {
ACHECK(step != 0) << "Step must non-zero"; ACHECK(step != 0) << "Step must be non-zero";
int num = (end - begin) / step; int num = (end - begin) / step;
if (num <= 0) return {}; if (num <= 0) return {};
end = begin + step * num; end = begin + step * num;

37
common/pcl/pcl_utils.h
View File

@ -6,41 +6,41 @@
namespace common { namespace common {
// Distance squred of a point to origin // Distance squred of a point to origin
template <typename PointType> template <typename PT>
inline double DistanceSqure(const PointType &pt) { inline double DistanceSqure(const PT &pt) {
return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z; return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z;
} }
// Distance squred of two points // Distance squred of two points
template <typename PointType> template <typename PT>
inline double DistanceSqure(const PointType &pt1, const PointType &pt2) { inline double DistanceSqure(const PT &pt1, const PT &pt2) {
return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y) + return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y) +
(pt1.z - pt2.z) * (pt1.z - pt2.z); (pt1.z - pt2.z) * (pt1.z - pt2.z);
} }
// Distance of a point to origin // Distance of a point to origin
template <typename PointType> template <typename PT>
inline double Distance(const PointType &pt) { inline double Distance(const PT &pt) {
return std::sqrt(DistanceSqure(pt)); return std::sqrt(DistanceSqure(pt));
} }
// Distance squred of two points // Distance squred of two points
template <typename PointType> template <typename PT>
inline double Distance(const PointType &pt1, const PointType &pt2) { inline double Distance(const PT &pt1, const PT &pt2) {
return std::sqrt(DistanceSqure(pt1, pt2)); return std::sqrt(DistanceSqure(pt1, pt2));
} }
// Check whether is a finite point: neither infinite nor nan // Check whether is a finite point: neither infinite nor nan
template <typename PointType> template <typename PT>
inline double IsFinite(const PointType &pt) { inline double IsFinite(const PT &pt) {
return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z); return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z);
} }
// Remove point if the condition evaluated to true on it // Remove point if the condition evaluated to true on it
template <typename PointType> template <typename PT>
void RemovePoints(const pcl::PointCloud<PointType> &cloud_in, void RemovePoints(const pcl::PointCloud<PT> &cloud_in,
pcl::PointCloud<PointType> *const cloud_out, pcl::PointCloud<PT> *const cloud_out,
std::function<bool(const PointType &)> check, std::function<bool(const PT &)> check,
std::vector<int> *const removed_indices = nullptr) { std::vector<int> *const removed_indices = nullptr) {
if (&cloud_in != cloud_out) { if (&cloud_in != cloud_out) {
cloud_out->header = cloud_in.header; cloud_out->header = cloud_in.header;
@ -62,11 +62,10 @@ void RemovePoints(const pcl::PointCloud<PointType> &cloud_in,
cloud_out->is_dense = true; cloud_out->is_dense = true;
} }
template <typename PointType> template <typename PT>
void VoxelDownSample( void VoxelDownSample(const typename pcl::PointCloud<PT>::ConstPtr &cloud_in,
const typename pcl::PointCloud<PointType>::ConstPtr &cloud_in, pcl::PointCloud<PT> *const cloud_out, double voxel_size) {
pcl::PointCloud<PointType> *const cloud_out, double voxel_size) { pcl::VoxelGrid<PT> filter;
pcl::VoxelGrid<PointType> filter;
filter.setInputCloud(cloud_in); filter.setInputCloud(cloud_in);
filter.setLeafSize(voxel_size, voxel_size, voxel_size); filter.setLeafSize(voxel_size, voxel_size, voxel_size);
filter.filter(*cloud_out); filter.filter(*cloud_out);

20
common/visualizer/lidar_visualizer.h
View File

@ -118,18 +118,18 @@ class LidarVisualizer {
return *static_cast<FrameT *>((*curr_frame_iter_).get()); return *static_cast<FrameT *>((*curr_frame_iter_).get());
} }
template <typename PointType> template <typename PT>
void DrawPointCloud( void DrawPointCloud(const typename pcl::PointCloud<PT>::ConstPtr &cloud,
const typename pcl::PointCloud<PointType>::ConstPtr &cloud, const Color &color, const std::string &id,
const Color &color, const std::string &id, int point_size = 3) { int point_size = 3) {
AddPointCloud<PointType>(cloud, color, id, viewer_.get(), point_size); AddPointCloud<PT>(cloud, color, id, viewer_.get(), point_size);
} }
template <typename PointType> template <typename PT>
void DrawPointCloud( void DrawPointCloud(const typename pcl::PointCloud<PT>::ConstPtr &cloud,
const typename pcl::PointCloud<PointType>::ConstPtr &cloud, const std::string &field, const std::string &id,
const std::string &field, const std::string &id, int point_size = 3) { int point_size = 3) {
AddPointCloud<PointType>(cloud, field, id, viewer_.get(), point_size); AddPointCloud<PT>(cloud, field, id, viewer_.get(), point_size);
} }
// visualizer name // visualizer name

17
common/visualizer/lidar_visualizer_utils.h
View File

@ -17,23 +17,22 @@ using PCLVisualizer = pcl::visualization::PCLVisualizer;
#define PCLColorHandlerGenericField \ #define PCLColorHandlerGenericField \
pcl::visualization::PointCloudColorHandlerGenericField pcl::visualization::PointCloudColorHandlerGenericField
template <typename PointType> template <typename PT>
void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr &cloud, void AddPointCloud(const typename pcl::PointCloud<PT>::ConstPtr &cloud,
const Color &color, const std::string &id, const Color &color, const std::string &id,
PCLVisualizer *const viewer, int point_size = 3) { PCLVisualizer *const viewer, int point_size = 3) {
PCLColorHandlerCustom<PointType> color_handler(cloud, color.r, color.g, PCLColorHandlerCustom<PT> color_handler(cloud, color.r, color.g, color.b);
color.b); viewer->addPointCloud<PT>(cloud, color_handler, id);
viewer->addPointCloud<PointType>(cloud, color_handler, id);
viewer->setPointCloudRenderingProperties( viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id); pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id);
} }
template <typename PointType> template <typename PT>
void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr &cloud, void AddPointCloud(const typename pcl::PointCloud<PT>::ConstPtr &cloud,
const std::string &field, const std::string &id, const std::string &field, const std::string &id,
PCLVisualizer *const viewer, int point_size = 3) { PCLVisualizer *const viewer, int point_size = 3) {
PCLColorHandlerGenericField<PointType> color_handler(cloud, field); PCLColorHandlerGenericField<PT> color_handler(cloud, field);
viewer->addPointCloud<PointType>(cloud, color_handler, id); viewer->addPointCloud<PT>(cloud, color_handler, id);
viewer->setPointCloudRenderingProperties( viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id); pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id);
} }

13
oh_my_loam/base/helper.h
View File

@ -7,9 +7,8 @@ namespace oh_my_loam {
using common::Pose3d; using common::Pose3d;
template <typename PointType> template <typename PT>
void TransformPoint(const Pose3d &pose, const PointType &pt_in, void TransformPoint(const Pose3d &pose, const PT &pt_in, PT *const pt_out) {
PointType *const pt_out) {
*pt_out = pt_in; *pt_out = pt_in;
Eigen::Vector3d pnt = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z); Eigen::Vector3d pnt = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z);
pt_out->x = pnt.x(); pt_out->x = pnt.x();
@ -17,10 +16,10 @@ void TransformPoint(const Pose3d &pose, const PointType &pt_in,
pt_out->z = pnt.z(); pt_out->z = pnt.z();
} }
template <typename PointType> template <typename PT>
PointType TransformPoint(const Pose3d &pose, const PointType &pt_in) { PT TransformPoint(const Pose3d &pose, const PT &pt_in) {
PointType pt_out; PT pt_out;
TransformPoint<PointType>(pose, pt_in, &pt_out); TransformPoint<PT>(pose, pt_in, &pt_out);
return pt_out; return pt_out;
} }

12
oh_my_loam/base/types.h
View File

@ -4,7 +4,7 @@
namespace oh_my_loam { namespace oh_my_loam {
enum class PType { enum class PointType {
FLAT_SURF = -2, FLAT_SURF = -2,
SURF = -1, SURF = -1,
NORNAL = 0, NORNAL = 0,
@ -67,7 +67,7 @@ struct PointXYZTCT {
struct { struct {
float time; float time;
float curvature; float curvature;
PType type; PointType type;
}; };
float data_c[4]; float data_c[4];
}; };
@ -76,11 +76,11 @@ struct PointXYZTCT {
x = y = z = 0.0f; x = y = z = 0.0f;
data[3] = 1.0f; data[3] = 1.0f;
time = curvature = 0.0f; time = curvature = 0.0f;
type = PType::NORNAL; type = PointType::NORNAL;
} }
PointXYZTCT(float x, float y, float z, float time = 0.0f, PointXYZTCT(float x, float y, float z, float time = 0.0f,
float curvature = 0.0f, PType type = PType::NORNAL) float curvature = 0.0f, PointType type = PointType::NORNAL)
: x(x), y(y), z(z), time(time), curvature(curvature), type(type) { : x(x), y(y), z(z), time(time), curvature(curvature), type(type) {
data[3] = 1.0f; data[3] = 1.0f;
} }
@ -92,7 +92,7 @@ struct PointXYZTCT {
data[3] = 1.0f; data[3] = 1.0f;
time = 0.0f; time = 0.0f;
curvature = 0.0f; curvature = 0.0f;
type = PType::NORNAL; type = PointType::NORNAL;
} }
PointXYZTCT(const TPoint &p) { PointXYZTCT(const TPoint &p) {
@ -102,7 +102,7 @@ struct PointXYZTCT {
data[3] = 1.0f; data[3] = 1.0f;
time = p.time; time = p.time;
curvature = 0.0f; curvature = 0.0f;
type = PType::NORNAL; type = PointType::NORNAL;
} }
PointXYZTCT(const PointXYZTCT &p) { PointXYZTCT(const PointXYZTCT &p) {

5
oh_my_loam/configs/config.yaml
View File

@ -12,7 +12,7 @@ extractor_config:
sharp_corner_point_num: 2 sharp_corner_point_num: 2
corner_point_num: 20 corner_point_num: 20
flat_surf_point_num: 4 flat_surf_point_num: 4
surf_point_num: 50 surf_point_num: 30
corner_point_curvature_th: 0.5 corner_point_curvature_th: 0.5
surf_point_curvature_th: 0.5 surf_point_curvature_th: 0.5
neighbor_point_dist_th: 0.1 neighbor_point_dist_th: 0.1
@ -21,8 +21,9 @@ extractor_config:
# configs for odometer # configs for odometer
odometer_config: odometer_config:
vis: true vis: true
verbose: false verbose: true
icp_iter_num: 2 icp_iter_num: 2
solve_iter_num: 5
match_dist_sq_th: 1 match_dist_sq_th: 1
# configs for mapper # configs for mapper

70
oh_my_loam/extractor/extractor.cc
View File

@ -9,11 +9,10 @@ namespace oh_my_loam {
namespace { namespace {
const int kScanSegNum = 6; const int kScanSegNum = 6;
const double kTwoPi = 2 * M_PI; const double kTwoPi = 2 * M_PI;
using namespace common;
} // namespace } // namespace
bool Extractor::Init() { bool Extractor::Init() {
const auto &config = YAMLConfig::Instance()->config(); const auto &config = common::YAMLConfig::Instance()->config();
config_ = config["extractor_config"]; config_ = config["extractor_config"];
is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>(); is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
verbose_ = config_["verbose"].as<bool>(); verbose_ = config_["verbose"].as<bool>();
@ -22,7 +21,8 @@ bool Extractor::Init() {
return true; return true;
} }
void Extractor::Process(double timestamp, const PointCloudConstPtr &cloud, void Extractor::Process(double timestamp,
const common::PointCloudConstPtr &cloud,
std::vector<Feature> *const features) { std::vector<Feature> *const features) {
BLOCK_TIMER_START; BLOCK_TIMER_START;
if (cloud->size() < config_["min_point_num"].as<size_t>()) { if (cloud->size() < config_["min_point_num"].as<size_t>()) {
@ -54,25 +54,25 @@ void Extractor::Process(double timestamp, const PointCloudConstPtr &cloud,
if (is_vis_) Visualize(cloud, *features, timestamp); if (is_vis_) Visualize(cloud, *features, timestamp);
} }
void Extractor::SplitScan(const PointCloud &cloud, void Extractor::SplitScan(const common::PointCloud &cloud,
std::vector<TCTPointCloud> *const scans) const { std::vector<TCTPointCloud> *const scans) const {
scans->resize(num_scans_); scans->resize(num_scans_);
double yaw_start = -atan2(cloud.points[0].y, cloud.points[0].x); double yaw_start = -std::atan2(cloud.points[0].y, cloud.points[0].x);
bool half_passed = false; bool half_passed = false;
for (const auto &pt : cloud) { for (const auto &pt : cloud) {
int scan_id = GetScanID(pt); int scan_id = GetScanID(pt);
if (scan_id >= num_scans_ || scan_id < 0) continue; if (scan_id >= num_scans_ || scan_id < 0) continue;
double yaw = -atan2(pt.y, pt.x); double yaw = -std::atan2(pt.y, pt.x);
double yaw_diff = NormalizeAngle(yaw - yaw_start); double yaw_diff = common::NormalizeAngle(yaw - yaw_start);
if (yaw_diff > 0) { if (yaw_diff > 0) {
if (half_passed) yaw_start += kTwoPi; if (half_passed) yaw_start += kTwoPi;
} else { } else {
half_passed = true; half_passed = true;
yaw_start += kTwoPi; yaw_start += kTwoPi;
} }
(*scans)[scan_id].push_back({pt.x, pt.y, pt.z, double time = std::min(yaw_diff / kTwoPi, 1.0) + scan_id;
static_cast<float>(yaw_diff / kTwoPi), scans->at(scan_id).push_back(
std::nanf("")}); {pt.x, pt.y, pt.z, static_cast<float>(time), std::nanf("")});
} }
} }
@ -91,7 +91,7 @@ void Extractor::ComputeCurvature(TCTPointCloud *const scan) const {
pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z; pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z;
pts[i].curvature = std::hypot(dx, dy, dz); pts[i].curvature = std::hypot(dx, dy, dz);
} }
RemovePoints<TCTPoint>(*scan, scan, [](const TCTPoint &pt) { common::RemovePoints<TCTPoint>(*scan, scan, [](const TCTPoint &pt) {
return !std::isfinite(pt.curvature); return !std::isfinite(pt.curvature);
}); });
} }
@ -101,7 +101,7 @@ void Extractor::AssignType(TCTPointCloud *const scan) const {
if (pt_num < kScanSegNum) return; if (pt_num < kScanSegNum) return;
int seg_pt_num = (pt_num - 1) / kScanSegNum + 1; int seg_pt_num = (pt_num - 1) / kScanSegNum + 1;
std::vector<bool> picked(pt_num, false); std::vector<bool> picked(pt_num, false);
std::vector<int> indices = Range(pt_num); std::vector<int> indices = common::Range(pt_num);
int sharp_corner_point_num = config_["sharp_corner_point_num"].as<int>(); int sharp_corner_point_num = config_["sharp_corner_point_num"].as<int>();
int corner_point_num = config_["corner_point_num"].as<int>(); int corner_point_num = config_["corner_point_num"].as<int>();
int flat_surf_point_num = config_["flat_surf_point_num"].as<int>(); int flat_surf_point_num = config_["flat_surf_point_num"].as<int>();
@ -125,9 +125,9 @@ void Extractor::AssignType(TCTPointCloud *const scan) const {
scan->at(ix).curvature > corner_point_curvature_th) { scan->at(ix).curvature > corner_point_curvature_th) {
++corner_point_picked_num; ++corner_point_picked_num;
if (corner_point_picked_num <= sharp_corner_point_num) { if (corner_point_picked_num <= sharp_corner_point_num) {
scan->at(ix).type = PType::SHARP_CORNER; scan->at(ix).type = PointType::SHARP_CORNER;
} else if (corner_point_picked_num <= corner_point_num) { } else if (corner_point_picked_num <= corner_point_num) {
scan->at(ix).type = PType::CORNER; scan->at(ix).type = PointType::CORNER;
} else { } else {
break; break;
} }
@ -142,9 +142,9 @@ void Extractor::AssignType(TCTPointCloud *const scan) const {
if (!picked.at(ix) && scan->at(ix).curvature < surf_point_curvature_th) { if (!picked.at(ix) && scan->at(ix).curvature < surf_point_curvature_th) {
++surf_point_picked_num; ++surf_point_picked_num;
if (surf_point_picked_num <= flat_surf_point_num) { if (surf_point_picked_num <= flat_surf_point_num) {
scan->at(ix).type = PType::FLAT_SURF; scan->at(ix).type = PointType::FLAT_SURF;
} else if (surf_point_picked_num <= surf_point_num) { } else if (surf_point_picked_num <= surf_point_num) {
scan->at(ix).type = PType::SURF; scan->at(ix).type = PointType::SURF;
} else { } else {
break; break;
} }
@ -160,31 +160,33 @@ void Extractor::GenerateFeature(const TCTPointCloud &scan,
for (const auto &pt : scan) { for (const auto &pt : scan) {
TPoint point(pt.x, pt.y, pt.z, pt.time); TPoint point(pt.x, pt.y, pt.z, pt.time);
switch (pt.type) { switch (pt.type) {
case PType::FLAT_SURF: case PointType::FLAT_SURF:
feature->cloud_flat_surf->points.emplace_back(point); feature->cloud_flat_surf->push_back(point);
// no break: FLAT_SURF points are also SURF points // no break: FLAT_SURF points are also SURF points
case PType::SURF: case PointType::SURF:
feature->cloud_surf->points.emplace_back(point); feature->cloud_surf->push_back(point);
break; break;
case PType::SHARP_CORNER: case PointType::SHARP_CORNER:
feature->cloud_sharp_corner->points.emplace_back(point); feature->cloud_sharp_corner->push_back(point);
// no break: SHARP_CORNER points are also CORNER points // no break: SHARP_CORNER points are also CORNER points
case PType::CORNER: case PointType::CORNER:
feature->cloud_corner->points.emplace_back(point); feature->cloud_corner->push_back(point);
break; break;
default: default:
feature->cloud_surf->push_back(point);
break; break;
} }
} }
TPointCloudPtr dowm_sampled(new TPointCloud); TPointCloudPtr dowm_sampled(new TPointCloud);
VoxelDownSample<TPoint>(feature->cloud_surf, dowm_sampled.get(), common::VoxelDownSample<TPoint>(
config_["downsample_voxel_size"].as<double>()); feature->cloud_surf, dowm_sampled.get(),
feature->cloud_surf->swap(*dowm_sampled); config_["downsample_voxel_size"].as<double>());
feature->cloud_surf = dowm_sampled;
} }
void Extractor::Visualize(const PointCloudConstPtr &cloud, void Extractor::Visualize(const common::PointCloudConstPtr &cloud,
const std::vector<Feature> &features, const std::vector<Feature> &features,
double timestamp) { double timestamp) const {
std::shared_ptr<ExtractorVisFrame> frame(new ExtractorVisFrame); std::shared_ptr<ExtractorVisFrame> frame(new ExtractorVisFrame);
frame->timestamp = timestamp; frame->timestamp = timestamp;
frame->cloud = cloud; frame->cloud = cloud;
@ -194,20 +196,20 @@ void Extractor::Visualize(const PointCloudConstPtr &cloud,
void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix, void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix,
std::vector<bool> *const picked) const { std::vector<bool> *const picked) const {
auto DistSqure = [&](int i, int j) -> float { auto dist_sq = [&](size_t i, size_t j) -> double {
return DistanceSqure<TCTPoint>(scan[i], scan[j]); return common::DistanceSqure<TCTPoint>(scan[i], scan[j]);
}; };
float neighbor_point_dist_th = config_["neighbor_point_dist_th"].as<float>(); double neighbor_point_dist_th = config_["neighbor_point_dist_th"].as<float>();
for (size_t i = 1; i <= 5; ++i) { for (size_t i = 1; i <= 5; ++i) {
if (ix < i) break; if (ix < i) break;
if (picked->at(ix - i)) continue; if (picked->at(ix - i)) continue;
if (DistSqure(ix - i, ix - i + 1) > neighbor_point_dist_th) break; if (dist_sq(ix - i, ix - i + 1) > neighbor_point_dist_th) break;
picked->at(ix - i) = true; picked->at(ix - i) = true;
} }
for (size_t i = 1; i <= 5; ++i) { for (size_t i = 1; i <= 5; ++i) {
if (ix + i >= scan.size()) break; if (ix + i >= scan.size()) break;
if (picked->at(ix + i)) continue; if (picked->at(ix + i)) continue;
if (DistSqure(ix + i, ix + i - 1) > neighbor_point_dist_th) break; if (dist_sq(ix + i, ix + i - 1) > neighbor_point_dist_th) break;
picked->at(ix + i) = true; picked->at(ix + i) = true;
} }
} }

2
oh_my_loam/extractor/extractor.h
View File

@ -37,7 +37,7 @@ class Extractor {
virtual void Visualize(const common::PointCloudConstPtr &cloud, virtual void Visualize(const common::PointCloudConstPtr &cloud,
const std::vector<Feature> &features, const std::vector<Feature> &features,
double timestamp = 0.0); double timestamp = 0.0) const;
int num_scans_ = 0; int num_scans_ = 0;

217
oh_my_loam/odometer/odometer.cc
View File

@ -1,6 +1,11 @@
#include "odometer.h" #include "odometer.h"
#include <algorithm>
#include "common/log/log.h"
#include "common/pcl/pcl_utils.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" #include "oh_my_loam/solver/solver.h"
namespace oh_my_loam { namespace oh_my_loam {
@ -8,11 +13,16 @@ namespace oh_my_loam {
namespace { namespace {
int kNearScanN = 2; int kNearScanN = 2;
size_t kMinMatchNum = 10; size_t kMinMatchNum = 10;
using namespace common; int GetScanId(const TPoint &point) {
return static_cast<int>(point.time);
}
float GetTime(const TPoint &point) {
return point.time - GetScanId(point);
}
} // namespace } // namespace
bool Odometer::Init() { bool Odometer::Init() {
const auto &config = YAMLConfig::Instance()->config(); const auto &config = common::YAMLConfig::Instance()->config();
config_ = config["odometer_config"]; config_ = config["odometer_config"];
is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>(); is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
verbose_ = config_["verbose"].as<bool>(); verbose_ = config_["verbose"].as<bool>();
@ -23,7 +33,6 @@ bool Odometer::Init() {
void Odometer::Process(double timestamp, const std::vector<Feature> &features, void Odometer::Process(double timestamp, const std::vector<Feature> &features,
Pose3d *const pose_out) { Pose3d *const pose_out) {
BLOCK_TIMER_START;
if (!is_initialized_) { if (!is_initialized_) {
UpdatePre(features); UpdatePre(features);
is_initialized_ = true; is_initialized_ = true;
@ -33,9 +42,13 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
AWARN << "Odometer initialized...."; AWARN << "Odometer initialized....";
return; return;
} }
AINFO_IF(verbose_) << "Pose before: " << pose_curr2world_.ToString(); BLOCK_TIMER_START;
AINFO << "Pose before: " << pose_curr2world_.ToString();
std::vector<PointLinePair> pl_pairs; std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs; std::vector<PointPlanePair> pp_pairs;
AINFO_IF(verbose_) << "Points to be matched: "
<< kdtree_corn_.getInputCloud()->size() << " + "
<< kdtree_surf_.getInputCloud()->size();
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) { for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
for (const auto &feature : features) { for (const auto &feature : features) {
MatchCorn(*feature.cloud_sharp_corner, &pl_pairs); MatchCorn(*feature.cloud_sharp_corner, &pl_pairs);
@ -56,136 +69,148 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
for (const auto &pair : pp_pairs) { for (const auto &pair : pp_pairs) {
solver.AddPointPlanePair(pair, pair.pt.time); solver.AddPointPlanePair(pair, pair.pt.time);
} }
solver.Solve(5, verbose_, &pose_curr2last_); solver.Solve(config_["solve_iter_num"].as<int>(), verbose_,
&pose_curr2last_);
AINFO << "Odometer::ICP: iter_" << i << ": " << BLOCK_TIMER_STOP_FMT;
} }
pose_curr2world_ = pose_curr2world_ * pose_curr2last_; pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
*pose_out = pose_curr2world_; *pose_out = pose_curr2world_;
AINFO_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString(); AINFO << "Pose increase: " << pose_curr2last_.ToString();
AINFO_IF(verbose_) << "Pose after: " << pose_curr2world_.ToString(); AINFO << "Pose after: " << pose_curr2world_.ToString();
UpdatePre(features); UpdatePre(features);
if (is_vis_) Visualize(features, pl_pairs, pp_pairs);
AINFO << "Odometer::Porcess: " << BLOCK_TIMER_STOP_FMT; AINFO << "Odometer::Porcess: " << BLOCK_TIMER_STOP_FMT;
if (is_vis_) Visualize(features, pl_pairs, pp_pairs);
} }
void Odometer::MatchCorn(const TPointCloud &src, void Odometer::MatchCorn(const TPointCloud &src,
std::vector<PointLinePair> *const pairs) const { std::vector<PointLinePair> *const pairs) const {
double dist_sq_thresh = config_["match_dist_sq_th"].as<double>(); double dist_sq_thresh = config_["match_dist_sq_th"].as<double>();
auto comp = [](const std::vector<float> &v1, const std::vector<float> &v2) { for (const auto &point : src) {
return v1[0] < v2[0]; TPoint query_pt = TransformToStart(pose_curr2last_, point);
}; std::vector<int> indices(1);
for (const auto &pt : src) { std::vector<float> dists(1);
TPoint query_pt = TransformToStart(pose_curr2last_, pt); if (kdtree_corn_.nearestKSearch(query_pt, 1, indices, dists) != 1) {
std::vector<std::vector<int>> indices; continue;
std::vector<std::vector<float>> dists; }
NearestKSearch(kdtrees_corn_, query_pt, 1, &indices, &dists); if (dists[0] >= dist_sq_thresh) continue;
int pt1_scan_id = TPoint pt1 = kdtree_corn_.getInputCloud()->at(indices[0]);
std::min_element(dists.begin(), dists.end(), comp) - dists.begin(); int pt1_scan_id = GetScanId(pt1);
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; bool pt2_fount = false;
int pt2_scan_id = -1; float min_pt2_dist_squre = dist_sq_thresh;
TPoint pt2;
int i_begin = std::max<int>(0, pt1_scan_id - kNearScanN); int i_begin = std::max<int>(0, pt1_scan_id - kNearScanN);
int i_end = std::min<int>(indices.size(), pt1_scan_id + kNearScanN + 1); int i_end =
std::min<int>(kdtrees_scan_corn_.size(), pt1_scan_id + kNearScanN + 1);
for (int i = i_begin; i < i_end && i != pt1_scan_id; ++i) { for (int i = i_begin; i < i_end && i != pt1_scan_id; ++i) {
if (dists[i][0] < min_dist_pt2_squre) { const auto &kdtree = kdtrees_scan_corn_[i];
pt2_scan_id = i; if (kdtree.nearestKSearch(query_pt, 1, indices, dists) != 0) {
min_dist_pt2_squre = dists[i][0]; continue;
}
if (dists[0] < min_pt2_dist_squre) {
pt2_fount = true;
pt2 = kdtree.getInputCloud()->at(indices[0]);
min_pt2_dist_squre = dists[0];
} }
} }
if (pt2_scan_id >= 0) { if (!pt2_fount) continue;
TPoint pt2 = clouds_corn_pre_[pt2_scan_id]->at(indices[pt2_scan_id][0]);
pairs->emplace_back(pt, pt1, pt2); TPoint pt(point.x, point.y, point.z, GetTime(point));
} pt1.time = GetTime(pt1);
pt2.time = GetTime(pt2);
pairs->emplace_back(pt, pt1, pt2);
} }
} }
void Odometer::MatchSurf(const TPointCloud &src, void Odometer::MatchSurf(const TPointCloud &src,
std::vector<PointPlanePair> *const pairs) const { std::vector<PointPlanePair> *const pairs) const {
double dist_sq_thresh = config_["match_dist_sq_th"].as<double>(); double dist_sq_thresh = config_["match_dist_sq_th"].as<double>();
auto comp = [](const std::vector<float> &v1, const std::vector<float> &v2) { for (const auto &point : src) {
return v1[0] < v2[0]; TPoint query_pt = TransformToStart(pose_curr2last_, point);
}; std::vector<int> indices;
for (const auto &pt : src) { std::vector<float> dists;
TPoint query_pt = TransformToStart(pose_curr2last_, pt); if (kdtree_surf_.nearestKSearch(query_pt, 2, indices, dists) != 2) {
std::vector<std::vector<int>> indices; continue;
std::vector<std::vector<float>> dists; }
NearestKSearch(kdtrees_surf_, query_pt, 2, &indices, &dists); if (dists[0] >= dist_sq_thresh || dists[1] >= dist_sq_thresh) continue;
int pt1_scan_id = TPoint pt1 = kdtree_surf_.getInputCloud()->at(indices[0]);
std::min_element(dists.begin(), dists.end(), comp) - dists.begin(); int pt1_scan_id = GetScanId(pt1);
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; TPoint pt2 = kdtree_surf_.getInputCloud()->at(indices[1]), pt3;
int pt3_scan_id = -1; bool pt2_found = true, pt3_found = false;
int i_begin = std::max<int>(0, pt1_scan_id - kNearScanN); int pt2_scan_id = GetScanId(pt2);
int i_end = std::min<int>(indices.size(), pt1_scan_id + kNearScanN + 1); if (pt2_scan_id != pt1_scan_id) {
for (int i = i_begin; i < i_end && i != pt1_scan_id; ++i) { pt2_found = false;
if (dists[i][0] < min_dist_pt3_squre) { if (std::abs(pt2_scan_id - pt1_scan_id) <= kNearScanN) {
pt3_scan_id = i; pt3 = pt2;
min_dist_pt3_squre = dists[i][0]; pt3_found = true;
} }
} }
if (pt3_scan_id >= 0) {
TPoint pt3 = clouds_surf_pre_[pt3_scan_id]->at(indices[pt3_scan_id][0]); if (!pt2_found) {
pairs->emplace_back(pt, pt1, pt2, pt3); const auto &kdtree = kdtrees_scan_surf_[pt1_scan_id];
if (kdtree.nearestKSearch(query_pt, 2, indices, dists) == 2) {
if (dists[1] < dist_sq_thresh) {
pt2 = kdtree.getInputCloud()->at(indices[1]);
pt2_found = true;
}
}
} }
if (!pt2_found) continue;
if (!pt3_found) {
float min_pt3_dist_squre = dist_sq_thresh;
int i_begin = std::max<int>(0, pt1_scan_id - kNearScanN);
int i_end = std::min<int>(kdtrees_scan_corn_.size(),
pt1_scan_id + kNearScanN + 1);
for (int i = i_begin; i < i_end && i != pt1_scan_id; ++i) {
const auto &kdtree = kdtrees_scan_surf_[i];
if (kdtree.nearestKSearch(query_pt, 1, indices, dists) != 1) {
continue;
}
if (dists[0] < min_pt3_dist_squre) {
pt3 = kdtree.getInputCloud()->at(indices[0]);
pt3_found = true;
min_pt3_dist_squre = dists[0];
}
}
}
if (!pt3_found) continue;
TPoint pt(point.x, point.y, point.z, GetTime(point));
pt1.time = GetTime(pt1);
pt2.time = GetTime(pt2);
pt3.time = GetTime(pt3);
pairs->emplace_back(pt, pt1, pt2, pt3);
} }
} }
void Odometer::UpdatePre(const std::vector<Feature> &features) { void Odometer::UpdatePre(const std::vector<Feature> &features) {
BLOCK_TIMER_START; kdtrees_scan_corn_.resize(features.size());
clouds_corn_pre_.resize(features.size()); kdtrees_scan_surf_.resize(features.size());
clouds_surf_pre_.resize(features.size()); TPointCloudPtr corn_pre(new TPointCloud);
kdtrees_corn_.resize(features.size()); TPointCloudPtr surf_pre(new TPointCloud);
kdtrees_surf_.resize(features.size()); TPointCloudPtr scan_corn_pre(new TPointCloud);
TPointCloudPtr scan_surf_pre(new TPointCloud);
for (size_t i = 0; i < features.size(); ++i) { for (size_t i = 0; i < features.size(); ++i) {
const auto &feature = features[i]; const auto &feature = features[i];
auto &cloud_pre = clouds_corn_pre_[i]; scan_corn_pre->resize(feature.cloud_corner->size());
if (!cloud_pre) cloud_pre.reset(new TPointCloud); scan_surf_pre->resize(feature.cloud_surf->size());
cloud_pre->resize(feature.cloud_corner->size());
for (size_t j = 0; j < feature.cloud_corner->size(); ++j) { for (size_t j = 0; j < feature.cloud_corner->size(); ++j) {
TransformToEnd(pose_curr2last_, feature.cloud_corner->at(j), TransformToEnd(pose_curr2last_, feature.cloud_corner->at(j),
&cloud_pre->at(j)); &scan_corn_pre->at(j));
} }
kdtrees_corn_[i].setInputCloud(cloud_pre);
}
for (size_t i = 0; i < features.size(); ++i) {
const auto &feature = features[i];
auto &cloud_pre = clouds_surf_pre_[i];
if (!cloud_pre) cloud_pre.reset(new TPointCloud);
cloud_pre->resize(feature.cloud_surf->size());
for (size_t j = 0; j < feature.cloud_surf->size(); ++j) { for (size_t j = 0; j < feature.cloud_surf->size(); ++j) {
TransformToEnd(pose_curr2last_, feature.cloud_surf->at(j), TransformToEnd(pose_curr2last_, feature.cloud_surf->at(j),
&cloud_pre->at(j)); &scan_surf_pre->at(j));
} }
kdtrees_surf_[i].setInputCloud(cloud_pre); *corn_pre += *scan_corn_pre;
} *surf_pre += *scan_surf_pre;
AINFO << "Odometer::UpdatePre: " << BLOCK_TIMER_STOP_FMT; kdtrees_scan_corn_[i].setInputCloud(scan_corn_pre);
} kdtrees_scan_surf_[i].setInputCloud(scan_surf_pre);
void Odometer::NearestKSearch(
const std::vector<pcl::KdTreeFLANN<TPoint>> &kdtrees,
const TPoint &query_pt, int 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;
int n_found = 0;
if (kdtree.getInputCloud()->size() >= static_cast<size_t>(k)) {
n_found = kdtree.nearestKSearch(query_pt, k, index, dist);
}
if (n_found < k) {
std::vector<int>(k, -1).swap(index);
std::vector<float>(k, std::numeric_limits<float>::max()).swap(dist);
}
indices->push_back(std::move(index));
dists->push_back(std::move(dist));
} }
kdtree_corn_.setInputCloud(corn_pre);
kdtree_surf_.setInputCloud(surf_pre);
} }
void Odometer::Visualize(const std::vector<Feature> &features, void Odometer::Visualize(const std::vector<Feature> &features,

19
oh_my_loam/odometer/odometer.h
View File

@ -31,21 +31,18 @@ class Odometer {
const std::vector<PointLinePair> &pl_pairs, const std::vector<PointLinePair> &pl_pairs,
const std::vector<PointPlanePair> &pp_pairs, const std::vector<PointPlanePair> &pp_pairs,
double timestamp = 0.0) const; double timestamp = 0.0) const;
void NearestKSearch(const std::vector<pcl::KdTreeFLANN<TPoint>> &kdtrees,
const TPoint &query_pt, int k, bool NearestSearch(const pcl::KdTreeFLANN<TPoint> &kdtree,
std::vector<std::vector<int>> *const indices, const TPoint &query_pt, int k, float dist_sq_th,
std::vector<std::vector<float>> *const dists) const; std::vector<int> *const indices) const;
common::Pose3d pose_curr2world_; common::Pose3d pose_curr2world_;
common::Pose3d pose_curr2last_; common::Pose3d pose_curr2last_;
std::vector<TPointCloudPtr> clouds_corn_pre_; std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_scan_surf_;
std::vector<TPointCloudPtr> clouds_surf_pre_; std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_scan_corn_;
TPointCloudPtr corn_pre_; pcl::KdTreeFLANN<TPoint> kdtree_corn_;
TPointCloudPtr surf_pre_; pcl::KdTreeFLANN<TPoint> kdtree_surf_;
std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_surf_;
std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_corn_;
YAML::Node config_; YAML::Node config_;

2
oh_my_loam/solver/cost_function.h
View File

@ -90,4 +90,4 @@ bool PointPlaneCostFunction::operator()(const T *const q, const T *const p,
return true; return true;
} }
} // oh_my_loam } // namespace oh_my_loam

21
oh_my_loam/solver/solver.cc
View File

@ -1,30 +1,37 @@
#include "solver.h" #include "solver.h"
#include <Eigen/src/Core/Matrix.h>
#include <ceres/loss_function.h>
#include "common/log/log.h"
namespace oh_my_loam { namespace oh_my_loam {
namespace { namespace {
double kHuberLossScale = 0.1; double kHuberLossScale = 0.1;
} }
PoseSolver::PoseSolver(const common::Pose3d &pose) PoseSolver::PoseSolver(const common::Pose3d &pose) {
: r_quat_(pose.r_quat().coeffs().data()), t_vec_(pose.t_vec().data()) { std::copy_n(pose.r_quat().coeffs().data(), 4, r_quat_);
std::copy_n(pose.t_vec().data(), 3, t_vec_);
loss_function_ = new ceres::HuberLoss(kHuberLossScale); loss_function_ = new ceres::HuberLoss(kHuberLossScale);
problem_.AddParameterBlock(r_quat_, 4, problem_.AddParameterBlock(r_quat_, 4,
new ceres::EigenQuaternionParameterization()); new ceres::EigenQuaternionParameterization());
problem_.AddParameterBlock(t_vec_, 3); problem_.AddParameterBlock(t_vec_, 3);
} }
double PoseSolver::Solve(int max_iter_num, bool verbose, bool PoseSolver::Solve(int max_iter_num, bool verbose,
common::Pose3d *const pose) { common::Pose3d *const pose) {
ceres::Solver::Options options; ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR; options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = max_iter_num; options.max_num_iterations = max_iter_num;
options.minimizer_progress_to_stdout = verbose; options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary; ceres::Solver::Summary summary;
ceres::Solve(options, &problem_, &summary); ceres::Solve(options, &problem_, &summary);
AINFO_IF(verbose) << summary.BriefReport(); AINFO_IF(verbose) << summary.BriefReport();
AWARN_IF(!summary.IsSolutionUsable()) << "Solution may be unusable";
if (pose) *pose = common::Pose3d(r_quat_, t_vec_); if (pose) *pose = common::Pose3d(r_quat_, t_vec_);
return summary.final_cost; return summary.IsSolutionUsable();
} }
void PoseSolver::AddPointLinePair(const PointLinePair &pair, double time) { void PoseSolver::AddPointLinePair(const PointLinePair &pair, double time) {
@ -43,4 +50,4 @@ common::Pose3d PoseSolver::GetPose() const {
return common::Pose3d(r_quat_, t_vec_); return common::Pose3d(r_quat_, t_vec_);
} }
} // oh_my_loam } // namespace oh_my_loam

10
oh_my_loam/solver/solver.h
View File

@ -1,20 +1,20 @@
#pragma once #pragma once
#include "common/geometry/pose3d.h" #include "common/geometry/pose3d.h"
#include "cost_function.h" #include "oh_my_loam/solver/cost_function.h"
namespace oh_my_loam { namespace oh_my_loam {
class PoseSolver { class PoseSolver {
public: public:
PoseSolver(const common::Pose3d &pose); explicit PoseSolver(const common::Pose3d &pose);
void AddPointLinePair(const PointLinePair &pair, double time); void AddPointLinePair(const PointLinePair &pair, double time);
void AddPointPlanePair(const PointPlanePair &pair, double time); void AddPointPlanePair(const PointPlanePair &pair, double time);
double Solve(int max_iter_num = 5, bool verbose = false, bool Solve(int max_iter_num = 5, bool verbose = false,
common::Pose3d *const pose = nullptr); common::Pose3d *const pose = nullptr);
common::Pose3d GetPose() const; common::Pose3d GetPose() const;
@ -24,7 +24,7 @@ class PoseSolver {
ceres::LossFunction *loss_function_; ceres::LossFunction *loss_function_;
// r_quat_: [x, y, z, w], t_vec_: [x, y, z] // r_quat_: [x, y, z, w], t_vec_: [x, y, z]
double *r_quat_, *t_vec_; double r_quat_[4], t_vec_[3];
DISALLOW_COPY_AND_ASSIGN(PoseSolver) DISALLOW_COPY_AND_ASSIGN(PoseSolver)
}; };

9
oh_my_loam/visualizer/extractor_visualizer.cc
View File

@ -11,13 +11,14 @@ void ExtractorVisualizer::Draw() {
for (size_t i = 0; i < frame.features.size(); ++i) { for (size_t i = 0; i < frame.features.size(); ++i) {
const auto &feature = frame.features[i]; const auto &feature = frame.features[i];
std::string id_suffix = std::to_string(i); std::string id_suffix = std::to_string(i);
DrawPointCloud<TPoint>(feature.cloud_surf, BLUE, "cloud_surf" + id_suffix); DrawPointCloud<TPoint>(feature.cloud_surf, BLUE, "cloud_surf" + id_suffix,
5);
DrawPointCloud<TPoint>(feature.cloud_flat_surf, CYAN, DrawPointCloud<TPoint>(feature.cloud_flat_surf, CYAN,
"cloud_flat_surf" + id_suffix); "cloud_flat_surf" + id_suffix, 7);
DrawPointCloud<TPoint>(feature.cloud_corner, YELLOW, DrawPointCloud<TPoint>(feature.cloud_corner, YELLOW,
"cloud_corner" + id_suffix); "cloud_corner" + id_suffix, 5);
DrawPointCloud<TPoint>(feature.cloud_sharp_corner, RED, DrawPointCloud<TPoint>(feature.cloud_sharp_corner, RED,
"cloud_sharp_corner" + id_suffix); "cloud_sharp_corner" + id_suffix, 7);
} }
}; };