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

4
common/geometry/pose3d.h
View File

@ -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_;
}

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) {
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;

37
common/pcl/pcl_utils.h
View File

@ -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);

20
common/visualizer/lidar_visualizer.h
View File

@ -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

17
common/visualizer/lidar_visualizer_utils.h
View File

@ -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);
}

13
oh_my_loam/base/helper.h
View File

@ -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;
}

12
oh_my_loam/base/types.h
View File

@ -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) {

5
oh_my_loam/configs/config.yaml
View File

@ -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

70
oh_my_loam/extractor/extractor.cc
View File

@ -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;
}
}

2
oh_my_loam/extractor/extractor.h
View File

@ -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;

217
oh_my_loam/odometer/odometer.cc
View File

@ -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,

19
oh_my_loam/odometer/odometer.h
View File

@ -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_;

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;
}
} // oh_my_loam
} // namespace oh_my_loam

21
oh_my_loam/solver/solver.cc
View File

@ -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
} // namespace oh_my_loam

10
oh_my_loam/solver/solver.h
View File

@ -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)
};

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) {
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);
}
};