From 38b85ab8f29b5a58d0dfec2dc0111758e9d542f7 Mon Sep 17 00:00:00 2001
From: feixyz10 <fei-china@139.com>
Date: Sat, 23 Jan 2021 22:11:08 +0800
Subject: [PATCH] odometer ok
---
common/geometry/pose3d.h | 4 +-
common/math/math_utils.cc | 2 +-
common/pcl/pcl_utils.h | 37 ++-
common/visualizer/lidar_visualizer.h | 20 +-
common/visualizer/lidar_visualizer_utils.h | 17 +-
oh_my_loam/base/helper.h | 13 +-
oh_my_loam/base/types.h | 12 +-
oh_my_loam/configs/config.yaml | 5 +-
oh_my_loam/extractor/extractor.cc | 70 +++---
oh_my_loam/extractor/extractor.h | 2 +-
oh_my_loam/odometer/odometer.cc | 217 ++++++++++--------
oh_my_loam/odometer/odometer.h | 19 +-
oh_my_loam/solver/cost_function.h | 2 +-
oh_my_loam/solver/solver.cc | 21 +-
oh_my_loam/solver/solver.h | 10 +-
oh_my_loam/visualizer/extractor_visualizer.cc | 9 +-
16 files changed, 245 insertions(+), 215 deletions(-)
diff --git a/common/geometry/pose3d.h b/common/geometry/pose3d.h
index c2ff21b..a3d8224 100644
--- a/common/geometry/pose3d.h
+++ b/common/geometry/pose3d.h
@@ -77,13 +77,13 @@ class Pose3d {
// const Eigen::Quaterniond& r_quat() const { return r_quat_; }
- Eigen::Quaterniond r_quat() const {
+ const Eigen::Quaterniond &r_quat() const {
return r_quat_;
}
// const Eigen::Vector3d& t_vec() const { return t_vec_; }
- Eigen::Vector3d t_vec() const {
+ const Eigen::Vector3d &t_vec() const {
return t_vec_;
}
diff --git a/common/math/math_utils.cc b/common/math/math_utils.cc
index a637a1b..f7a5d70 100644
--- a/common/math/math_utils.cc
+++ b/common/math/math_utils.cc
@@ -18,7 +18,7 @@ double Rad2Degree(double rad) {
}
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;
if (num <= 0) return {};
end = begin + step * num;
diff --git a/common/pcl/pcl_utils.h b/common/pcl/pcl_utils.h
index 1540cb4..10f5895 100644
--- a/common/pcl/pcl_utils.h
+++ b/common/pcl/pcl_utils.h
@@ -6,41 +6,41 @@
namespace common {
// Distance squred of a point to origin
-template <typename PointType>
-inline double DistanceSqure(const PointType &pt) {
+template <typename PT>
+inline double DistanceSqure(const PT &pt) {
return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z;
}
// Distance squred of two points
-template <typename PointType>
-inline double DistanceSqure(const PointType &pt1, const PointType &pt2) {
+template <typename PT>
+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) +
(pt1.z - pt2.z) * (pt1.z - pt2.z);
}
// Distance of a point to origin
-template <typename PointType>
-inline double Distance(const PointType &pt) {
+template <typename PT>
+inline double Distance(const PT &pt) {
return std::sqrt(DistanceSqure(pt));
}
// Distance squred of two points
-template <typename PointType>
-inline double Distance(const PointType &pt1, const PointType &pt2) {
+template <typename PT>
+inline double Distance(const PT &pt1, const PT &pt2) {
return std::sqrt(DistanceSqure(pt1, pt2));
}
// Check whether is a finite point: neither infinite nor nan
-template <typename PointType>
-inline double IsFinite(const PointType &pt) {
+template <typename PT>
+inline double IsFinite(const PT &pt) {
return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z);
}
// Remove point if the condition evaluated to true on it
-template <typename PointType>
-void RemovePoints(const pcl::PointCloud<PointType> &cloud_in,
- pcl::PointCloud<PointType> *const cloud_out,
- std::function<bool(const PointType &)> check,
+template <typename PT>
+void RemovePoints(const pcl::PointCloud<PT> &cloud_in,
+ pcl::PointCloud<PT> *const cloud_out,
+ std::function<bool(const PT &)> check,
std::vector<int> *const removed_indices = nullptr) {
if (&cloud_in != cloud_out) {
cloud_out->header = cloud_in.header;
@@ -62,11 +62,10 @@ void RemovePoints(const pcl::PointCloud<PointType> &cloud_in,
cloud_out->is_dense = true;
}
-template <typename PointType>
-void VoxelDownSample(
- const typename pcl::PointCloud<PointType>::ConstPtr &cloud_in,
- pcl::PointCloud<PointType> *const cloud_out, double voxel_size) {
- pcl::VoxelGrid<PointType> filter;
+template <typename PT>
+void VoxelDownSample(const typename pcl::PointCloud<PT>::ConstPtr &cloud_in,
+ pcl::PointCloud<PT> *const cloud_out, double voxel_size) {
+ pcl::VoxelGrid<PT> filter;
filter.setInputCloud(cloud_in);
filter.setLeafSize(voxel_size, voxel_size, voxel_size);
filter.filter(*cloud_out);
diff --git a/common/visualizer/lidar_visualizer.h b/common/visualizer/lidar_visualizer.h
index 1fcf920..8bc671c 100644
--- a/common/visualizer/lidar_visualizer.h
+++ b/common/visualizer/lidar_visualizer.h
@@ -118,18 +118,18 @@ class LidarVisualizer {
return *static_cast<FrameT *>((*curr_frame_iter_).get());
}
- template <typename PointType>
- void DrawPointCloud(
- const typename pcl::PointCloud<PointType>::ConstPtr &cloud,
- const Color &color, const std::string &id, int point_size = 3) {
- AddPointCloud<PointType>(cloud, color, id, viewer_.get(), point_size);
+ template <typename PT>
+ void DrawPointCloud(const typename pcl::PointCloud<PT>::ConstPtr &cloud,
+ const Color &color, const std::string &id,
+ int point_size = 3) {
+ AddPointCloud<PT>(cloud, color, id, viewer_.get(), point_size);
}
- template <typename PointType>
- void DrawPointCloud(
- const typename pcl::PointCloud<PointType>::ConstPtr &cloud,
- const std::string &field, const std::string &id, int point_size = 3) {
- AddPointCloud<PointType>(cloud, field, id, viewer_.get(), point_size);
+ template <typename PT>
+ void DrawPointCloud(const typename pcl::PointCloud<PT>::ConstPtr &cloud,
+ const std::string &field, const std::string &id,
+ int point_size = 3) {
+ AddPointCloud<PT>(cloud, field, id, viewer_.get(), point_size);
}
// visualizer name
diff --git a/common/visualizer/lidar_visualizer_utils.h b/common/visualizer/lidar_visualizer_utils.h
index 626a2b1..4175b2f 100644
--- a/common/visualizer/lidar_visualizer_utils.h
+++ b/common/visualizer/lidar_visualizer_utils.h
@@ -17,23 +17,22 @@ using PCLVisualizer = pcl::visualization::PCLVisualizer;
#define PCLColorHandlerGenericField \
pcl::visualization::PointCloudColorHandlerGenericField
-template <typename PointType>
-void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr &cloud,
+template <typename PT>
+void AddPointCloud(const typename pcl::PointCloud<PT>::ConstPtr &cloud,
const Color &color, const std::string &id,
PCLVisualizer *const viewer, int point_size = 3) {
- PCLColorHandlerCustom<PointType> color_handler(cloud, color.r, color.g,
- color.b);
- viewer->addPointCloud<PointType>(cloud, color_handler, id);
+ PCLColorHandlerCustom<PT> color_handler(cloud, color.r, color.g, color.b);
+ viewer->addPointCloud<PT>(cloud, color_handler, id);
viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id);
}
-template <typename PointType>
-void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr &cloud,
+template <typename PT>
+void AddPointCloud(const typename pcl::PointCloud<PT>::ConstPtr &cloud,
const std::string &field, const std::string &id,
PCLVisualizer *const viewer, int point_size = 3) {
- PCLColorHandlerGenericField<PointType> color_handler(cloud, field);
- viewer->addPointCloud<PointType>(cloud, color_handler, id);
+ PCLColorHandlerGenericField<PT> color_handler(cloud, field);
+ viewer->addPointCloud<PT>(cloud, color_handler, id);
viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id);
}
diff --git a/oh_my_loam/base/helper.h b/oh_my_loam/base/helper.h
index 658de7b..3aae436 100644
--- a/oh_my_loam/base/helper.h
+++ b/oh_my_loam/base/helper.h
@@ -7,9 +7,8 @@ namespace oh_my_loam {
using common::Pose3d;
-template <typename PointType>
-void TransformPoint(const Pose3d &pose, const PointType &pt_in,
- PointType *const pt_out) {
+template <typename PT>
+void TransformPoint(const Pose3d &pose, const PT &pt_in, PT *const pt_out) {
*pt_out = pt_in;
Eigen::Vector3d pnt = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z);
pt_out->x = pnt.x();
@@ -17,10 +16,10 @@ 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);
+template <typename PT>
+PT TransformPoint(const Pose3d &pose, const PT &pt_in) {
+ PT pt_out;
+ TransformPoint<PT>(pose, pt_in, &pt_out);
return pt_out;
}
diff --git a/oh_my_loam/base/types.h b/oh_my_loam/base/types.h
index eb2ebcf..08d7b5c 100644
--- a/oh_my_loam/base/types.h
+++ b/oh_my_loam/base/types.h
@@ -4,7 +4,7 @@
namespace oh_my_loam {
-enum class PType {
+enum class PointType {
FLAT_SURF = -2,
SURF = -1,
NORNAL = 0,
@@ -67,7 +67,7 @@ struct PointXYZTCT {
struct {
float time;
float curvature;
- PType type;
+ PointType type;
};
float data_c[4];
};
@@ -76,11 +76,11 @@ struct PointXYZTCT {
x = y = z = 0.0f;
data[3] = 1.0f;
time = curvature = 0.0f;
- type = PType::NORNAL;
+ type = PointType::NORNAL;
}
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) {
data[3] = 1.0f;
}
@@ -92,7 +92,7 @@ struct PointXYZTCT {
data[3] = 1.0f;
time = 0.0f;
curvature = 0.0f;
- type = PType::NORNAL;
+ type = PointType::NORNAL;
}
PointXYZTCT(const TPoint &p) {
@@ -102,7 +102,7 @@ struct PointXYZTCT {
data[3] = 1.0f;
time = p.time;
curvature = 0.0f;
- type = PType::NORNAL;
+ type = PointType::NORNAL;
}
PointXYZTCT(const PointXYZTCT &p) {
diff --git a/oh_my_loam/configs/config.yaml b/oh_my_loam/configs/config.yaml
index a4ab950..cf66638 100644
--- a/oh_my_loam/configs/config.yaml
+++ b/oh_my_loam/configs/config.yaml
@@ -12,7 +12,7 @@ extractor_config:
sharp_corner_point_num: 2
corner_point_num: 20
flat_surf_point_num: 4
- surf_point_num: 50
+ surf_point_num: 30
corner_point_curvature_th: 0.5
surf_point_curvature_th: 0.5
neighbor_point_dist_th: 0.1
@@ -21,8 +21,9 @@ extractor_config:
# configs for odometer
odometer_config:
vis: true
- verbose: false
+ verbose: true
icp_iter_num: 2
+ solve_iter_num: 5
match_dist_sq_th: 1
# configs for mapper
diff --git a/oh_my_loam/extractor/extractor.cc b/oh_my_loam/extractor/extractor.cc
index 19127c7..a56d174 100644
--- a/oh_my_loam/extractor/extractor.cc
+++ b/oh_my_loam/extractor/extractor.cc
@@ -9,11 +9,10 @@ namespace oh_my_loam {
namespace {
const int kScanSegNum = 6;
const double kTwoPi = 2 * M_PI;
-using namespace common;
} // namespace
bool Extractor::Init() {
- const auto &config = YAMLConfig::Instance()->config();
+ const auto &config = common::YAMLConfig::Instance()->config();
config_ = config["extractor_config"];
is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
verbose_ = config_["verbose"].as<bool>();
@@ -22,7 +21,8 @@ bool Extractor::Init() {
return true;
}
-void Extractor::Process(double timestamp, const PointCloudConstPtr &cloud,
+void Extractor::Process(double timestamp,
+ const common::PointCloudConstPtr &cloud,
std::vector<Feature> *const features) {
BLOCK_TIMER_START;
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);
}
-void Extractor::SplitScan(const PointCloud &cloud,
+void Extractor::SplitScan(const common::PointCloud &cloud,
std::vector<TCTPointCloud> *const scans) const {
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;
for (const auto &pt : cloud) {
int scan_id = GetScanID(pt);
if (scan_id >= num_scans_ || scan_id < 0) continue;
- double yaw = -atan2(pt.y, pt.x);
- double yaw_diff = NormalizeAngle(yaw - yaw_start);
+ double yaw = -std::atan2(pt.y, pt.x);
+ double yaw_diff = common::NormalizeAngle(yaw - yaw_start);
if (yaw_diff > 0) {
if (half_passed) yaw_start += kTwoPi;
} else {
half_passed = true;
yaw_start += kTwoPi;
}
- (*scans)[scan_id].push_back({pt.x, pt.y, pt.z,
- static_cast<float>(yaw_diff / kTwoPi),
- std::nanf("")});
+ double time = std::min(yaw_diff / kTwoPi, 1.0) + scan_id;
+ scans->at(scan_id).push_back(
+ {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].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);
});
}
@@ -101,7 +101,7 @@ void Extractor::AssignType(TCTPointCloud *const scan) const {
if (pt_num < kScanSegNum) return;
int seg_pt_num = (pt_num - 1) / kScanSegNum + 1;
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 corner_point_num = config_["corner_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) {
++corner_point_picked_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) {
- scan->at(ix).type = PType::CORNER;
+ scan->at(ix).type = PointType::CORNER;
} else {
break;
}
@@ -142,9 +142,9 @@ void Extractor::AssignType(TCTPointCloud *const scan) const {
if (!picked.at(ix) && scan->at(ix).curvature < surf_point_curvature_th) {
++surf_point_picked_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) {
- scan->at(ix).type = PType::SURF;
+ scan->at(ix).type = PointType::SURF;
} else {
break;
}
@@ -160,31 +160,33 @@ void Extractor::GenerateFeature(const TCTPointCloud &scan,
for (const auto &pt : scan) {
TPoint point(pt.x, pt.y, pt.z, pt.time);
switch (pt.type) {
- case PType::FLAT_SURF:
- feature->cloud_flat_surf->points.emplace_back(point);
+ case PointType::FLAT_SURF:
+ feature->cloud_flat_surf->push_back(point);
// no break: FLAT_SURF points are also SURF points
- case PType::SURF:
- feature->cloud_surf->points.emplace_back(point);
+ case PointType::SURF:
+ feature->cloud_surf->push_back(point);
break;
- case PType::SHARP_CORNER:
- feature->cloud_sharp_corner->points.emplace_back(point);
+ case PointType::SHARP_CORNER:
+ feature->cloud_sharp_corner->push_back(point);
// no break: SHARP_CORNER points are also CORNER points
- case PType::CORNER:
- feature->cloud_corner->points.emplace_back(point);
+ case PointType::CORNER:
+ feature->cloud_corner->push_back(point);
break;
default:
+ feature->cloud_surf->push_back(point);
break;
}
}
TPointCloudPtr dowm_sampled(new TPointCloud);
- VoxelDownSample<TPoint>(feature->cloud_surf, dowm_sampled.get(),
- config_["downsample_voxel_size"].as<double>());
- feature->cloud_surf->swap(*dowm_sampled);
+ common::VoxelDownSample<TPoint>(
+ feature->cloud_surf, dowm_sampled.get(),
+ 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,
- double timestamp) {
+ double timestamp) const {
std::shared_ptr<ExtractorVisFrame> frame(new ExtractorVisFrame);
frame->timestamp = timestamp;
frame->cloud = cloud;
@@ -194,20 +196,20 @@ void Extractor::Visualize(const PointCloudConstPtr &cloud,
void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix,
std::vector<bool> *const picked) const {
- auto DistSqure = [&](int i, int j) -> float {
- return DistanceSqure<TCTPoint>(scan[i], scan[j]);
+ auto dist_sq = [&](size_t i, size_t j) -> double {
+ 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) {
if (ix < i) break;
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;
}
for (size_t i = 1; i <= 5; ++i) {
if (ix + i >= scan.size()) break;
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;
}
}
diff --git a/oh_my_loam/extractor/extractor.h b/oh_my_loam/extractor/extractor.h
index 00faf63..0c9e901 100644
--- a/oh_my_loam/extractor/extractor.h
+++ b/oh_my_loam/extractor/extractor.h
@@ -37,7 +37,7 @@ class Extractor {
virtual void Visualize(const common::PointCloudConstPtr &cloud,
const std::vector<Feature> &features,
- double timestamp = 0.0);
+ double timestamp = 0.0) const;
int num_scans_ = 0;
diff --git a/oh_my_loam/odometer/odometer.cc b/oh_my_loam/odometer/odometer.cc
index 1c3a059..6fcb67b 100644
--- a/oh_my_loam/odometer/odometer.cc
+++ b/oh_my_loam/odometer/odometer.cc
@@ -1,6 +1,11 @@
#include "odometer.h"
+#include <algorithm>
+
+#include "common/log/log.h"
#include "common/pcl/pcl_utils.h"
+#include "common/time/timer.h"
+#include "oh_my_loam/base/types.h"
#include "oh_my_loam/solver/solver.h"
namespace oh_my_loam {
@@ -8,11 +13,16 @@ namespace oh_my_loam {
namespace {
int kNearScanN = 2;
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
bool Odometer::Init() {
- const auto &config = YAMLConfig::Instance()->config();
+ const auto &config = common::YAMLConfig::Instance()->config();
config_ = config["odometer_config"];
is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
verbose_ = config_["verbose"].as<bool>();
@@ -23,7 +33,6 @@ bool Odometer::Init() {
void Odometer::Process(double timestamp, const std::vector<Feature> &features,
Pose3d *const pose_out) {
- BLOCK_TIMER_START;
if (!is_initialized_) {
UpdatePre(features);
is_initialized_ = true;
@@ -33,9 +42,13 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
AWARN << "Odometer initialized....";
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<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 (const auto &feature : features) {
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) {
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_out = pose_curr2world_;
- AINFO_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString();
- AINFO_IF(verbose_) << "Pose after: " << pose_curr2world_.ToString();
+ AINFO << "Pose increase: " << pose_curr2last_.ToString();
+ AINFO << "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(features, pl_pairs, pp_pairs);
}
void Odometer::MatchCorn(const TPointCloud &src,
std::vector<PointLinePair> *const pairs) const {
double dist_sq_thresh = config_["match_dist_sq_th"].as<double>();
- 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]);
+ for (const auto &point : src) {
+ TPoint query_pt = TransformToStart(pose_curr2last_, point);
+ std::vector<int> indices(1);
+ std::vector<float> dists(1);
+ if (kdtree_corn_.nearestKSearch(query_pt, 1, indices, dists) != 1) {
+ continue;
+ }
+ if (dists[0] >= dist_sq_thresh) continue;
+ TPoint pt1 = kdtree_corn_.getInputCloud()->at(indices[0]);
+ int pt1_scan_id = GetScanId(pt1);
- double min_dist_pt2_squre = dist_sq_thresh;
- int pt2_scan_id = -1;
+ bool pt2_fount = false;
+ float min_pt2_dist_squre = dist_sq_thresh;
+ TPoint pt2;
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) {
- if (dists[i][0] < min_dist_pt2_squre) {
- pt2_scan_id = i;
- min_dist_pt2_squre = dists[i][0];
+ const auto &kdtree = kdtrees_scan_corn_[i];
+ if (kdtree.nearestKSearch(query_pt, 1, indices, dists) != 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) {
- TPoint pt2 = clouds_corn_pre_[pt2_scan_id]->at(indices[pt2_scan_id][0]);
- pairs->emplace_back(pt, pt1, pt2);
- }
+ if (!pt2_fount) continue;
+
+ 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,
std::vector<PointPlanePair> *const pairs) const {
double dist_sq_thresh = config_["match_dist_sq_th"].as<double>();
- 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]);
+ for (const auto &point : src) {
+ TPoint query_pt = TransformToStart(pose_curr2last_, point);
+ std::vector<int> indices;
+ std::vector<float> dists;
+ if (kdtree_surf_.nearestKSearch(query_pt, 2, indices, dists) != 2) {
+ continue;
+ }
+ if (dists[0] >= dist_sq_thresh || dists[1] >= dist_sq_thresh) continue;
+ TPoint pt1 = kdtree_surf_.getInputCloud()->at(indices[0]);
+ int pt1_scan_id = GetScanId(pt1);
- double min_dist_pt3_squre = dist_sq_thresh;
- 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];
+ TPoint pt2 = kdtree_surf_.getInputCloud()->at(indices[1]), pt3;
+ bool pt2_found = true, pt3_found = false;
+ int pt2_scan_id = GetScanId(pt2);
+ if (pt2_scan_id != pt1_scan_id) {
+ pt2_found = false;
+ if (std::abs(pt2_scan_id - pt1_scan_id) <= kNearScanN) {
+ pt3 = pt2;
+ pt3_found = true;
}
}
- 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);
+
+ if (!pt2_found) {
+ 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) {
- BLOCK_TIMER_START;
- clouds_corn_pre_.resize(features.size());
- clouds_surf_pre_.resize(features.size());
- kdtrees_corn_.resize(features.size());
- kdtrees_surf_.resize(features.size());
-
+ kdtrees_scan_corn_.resize(features.size());
+ kdtrees_scan_surf_.resize(features.size());
+ TPointCloudPtr corn_pre(new TPointCloud);
+ TPointCloudPtr surf_pre(new TPointCloud);
+ TPointCloudPtr scan_corn_pre(new TPointCloud);
+ TPointCloudPtr scan_surf_pre(new TPointCloud);
for (size_t i = 0; i < features.size(); ++i) {
const auto &feature = features[i];
- auto &cloud_pre = clouds_corn_pre_[i];
- if (!cloud_pre) cloud_pre.reset(new TPointCloud);
- cloud_pre->resize(feature.cloud_corner->size());
+ scan_corn_pre->resize(feature.cloud_corner->size());
+ scan_surf_pre->resize(feature.cloud_surf->size());
for (size_t j = 0; j < feature.cloud_corner->size(); ++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) {
TransformToEnd(pose_curr2last_, feature.cloud_surf->at(j),
- &cloud_pre->at(j));
+ &scan_surf_pre->at(j));
}
- kdtrees_surf_[i].setInputCloud(cloud_pre);
- }
- AINFO << "Odometer::UpdatePre: " << BLOCK_TIMER_STOP_FMT;
-}
-
-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));
+ *corn_pre += *scan_corn_pre;
+ *surf_pre += *scan_surf_pre;
+ kdtrees_scan_corn_[i].setInputCloud(scan_corn_pre);
+ kdtrees_scan_surf_[i].setInputCloud(scan_surf_pre);
}
+ kdtree_corn_.setInputCloud(corn_pre);
+ kdtree_surf_.setInputCloud(surf_pre);
}
void Odometer::Visualize(const std::vector<Feature> &features,
diff --git a/oh_my_loam/odometer/odometer.h b/oh_my_loam/odometer/odometer.h
index b89bf4a..b1592bc 100644
--- a/oh_my_loam/odometer/odometer.h
+++ b/oh_my_loam/odometer/odometer.h
@@ -31,21 +31,18 @@ class Odometer {
const std::vector<PointLinePair> &pl_pairs,
const std::vector<PointPlanePair> &pp_pairs,
double timestamp = 0.0) const;
- void 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;
+
+ bool NearestSearch(const pcl::KdTreeFLANN<TPoint> &kdtree,
+ const TPoint &query_pt, int k, float dist_sq_th,
+ std::vector<int> *const indices) const;
common::Pose3d pose_curr2world_;
common::Pose3d pose_curr2last_;
- std::vector<TPointCloudPtr> clouds_corn_pre_;
- std::vector<TPointCloudPtr> clouds_surf_pre_;
- TPointCloudPtr corn_pre_;
- TPointCloudPtr surf_pre_;
-
- std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_surf_;
- std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_corn_;
+ std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_scan_surf_;
+ std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_scan_corn_;
+ pcl::KdTreeFLANN<TPoint> kdtree_corn_;
+ pcl::KdTreeFLANN<TPoint> kdtree_surf_;
YAML::Node config_;
diff --git a/oh_my_loam/solver/cost_function.h b/oh_my_loam/solver/cost_function.h
index ade3208..41af71f 100644
--- a/oh_my_loam/solver/cost_function.h
+++ b/oh_my_loam/solver/cost_function.h
@@ -90,4 +90,4 @@ bool PointPlaneCostFunction::operator()(const T *const q, const T *const p,
return true;
}
-} // oh_my_loam
\ No newline at end of file
+} // namespace oh_my_loam
\ No newline at end of file
diff --git a/oh_my_loam/solver/solver.cc b/oh_my_loam/solver/solver.cc
index 21054c0..2e42d10 100644
--- a/oh_my_loam/solver/solver.cc
+++ b/oh_my_loam/solver/solver.cc
@@ -1,30 +1,37 @@
#include "solver.h"
+#include <Eigen/src/Core/Matrix.h>
+#include <ceres/loss_function.h>
+
+#include "common/log/log.h"
+
namespace oh_my_loam {
namespace {
double kHuberLossScale = 0.1;
}
-PoseSolver::PoseSolver(const common::Pose3d &pose)
- : r_quat_(pose.r_quat().coeffs().data()), t_vec_(pose.t_vec().data()) {
+PoseSolver::PoseSolver(const common::Pose3d &pose) {
+ 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);
problem_.AddParameterBlock(r_quat_, 4,
new ceres::EigenQuaternionParameterization());
problem_.AddParameterBlock(t_vec_, 3);
}
-double PoseSolver::Solve(int max_iter_num, bool verbose,
- common::Pose3d *const pose) {
+bool 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;
- options.minimizer_progress_to_stdout = verbose;
+ options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem_, &summary);
AINFO_IF(verbose) << summary.BriefReport();
+ AWARN_IF(!summary.IsSolutionUsable()) << "Solution may be unusable";
if (pose) *pose = common::Pose3d(r_quat_, t_vec_);
- return summary.final_cost;
+ return summary.IsSolutionUsable();
}
void PoseSolver::AddPointLinePair(const PointLinePair &pair, double time) {
@@ -43,4 +50,4 @@ common::Pose3d PoseSolver::GetPose() const {
return common::Pose3d(r_quat_, t_vec_);
}
-} // oh_my_loam
\ No newline at end of file
+} // namespace oh_my_loam
\ No newline at end of file
diff --git a/oh_my_loam/solver/solver.h b/oh_my_loam/solver/solver.h
index 9f91a8b..20a860d 100644
--- a/oh_my_loam/solver/solver.h
+++ b/oh_my_loam/solver/solver.h
@@ -1,20 +1,20 @@
#pragma once
#include "common/geometry/pose3d.h"
-#include "cost_function.h"
+#include "oh_my_loam/solver/cost_function.h"
namespace oh_my_loam {
class PoseSolver {
public:
- PoseSolver(const common::Pose3d &pose);
+ explicit PoseSolver(const common::Pose3d &pose);
void AddPointLinePair(const PointLinePair &pair, double time);
void AddPointPlanePair(const PointPlanePair &pair, double time);
- double Solve(int max_iter_num = 5, bool verbose = false,
- common::Pose3d *const pose = nullptr);
+ bool Solve(int max_iter_num = 5, bool verbose = false,
+ common::Pose3d *const pose = nullptr);
common::Pose3d GetPose() const;
@@ -24,7 +24,7 @@ class PoseSolver {
ceres::LossFunction *loss_function_;
// 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)
};
diff --git a/oh_my_loam/visualizer/extractor_visualizer.cc b/oh_my_loam/visualizer/extractor_visualizer.cc
index c148740..9823b01 100644
--- a/oh_my_loam/visualizer/extractor_visualizer.cc
+++ b/oh_my_loam/visualizer/extractor_visualizer.cc
@@ -11,13 +11,14 @@ void ExtractorVisualizer::Draw() {
for (size_t i = 0; i < frame.features.size(); ++i) {
const auto &feature = frame.features[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,
- "cloud_flat_surf" + id_suffix);
+ "cloud_flat_surf" + id_suffix, 7);
DrawPointCloud<TPoint>(feature.cloud_corner, YELLOW,
- "cloud_corner" + id_suffix);
+ "cloud_corner" + id_suffix, 5);
DrawPointCloud<TPoint>(feature.cloud_sharp_corner, RED,
- "cloud_sharp_corner" + id_suffix);
+ "cloud_sharp_corner" + id_suffix, 7);
}
};