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

make extractor better

main
feixyz10 2021-01-14 00:34:13 +08:00 committed by feixyz
parent c71992dcd4
commit da337376f9
12 changed files with 93 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
common/math/math_utils.cc
View File

@ -9,6 +9,10 @@ double NormalizeAngle(double ang) {
return ang - two_pi * std::floor((ang + M_PI) / two_pi); return ang - two_pi * std::floor((ang + M_PI) / two_pi);
} }
double Degree2Rad(double degree) { return degree * M_PI / 180.0; }
double Rad2Degree(double rad) { return rad * 180.0 / M_PI; }
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 non-zero";
int num = (end - begin) / step; int num = (end - begin) / step;

8
common/pcl/pcl_utils.h
View File

@ -40,7 +40,8 @@ inline double IsFinite(const PointType& pt) {
template <typename PointType> template <typename PointType>
void RemovePoints(const pcl::PointCloud<PointType>& cloud_in, void RemovePoints(const pcl::PointCloud<PointType>& cloud_in,
pcl::PointCloud<PointType>* const cloud_out, pcl::PointCloud<PointType>* const cloud_out,
std::function<bool(const PointType&)> check) { std::function<bool(const PointType&)> check,
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;
cloud_out->resize(cloud_in.size()); cloud_out->resize(cloud_in.size());
@ -48,7 +49,10 @@ void RemovePoints(const pcl::PointCloud<PointType>& cloud_in,
size_t j = 0; size_t j = 0;
for (size_t i = 0; i < cloud_in.size(); ++i) { for (size_t i = 0; i < cloud_in.size(); ++i) {
const auto pt = cloud_in.points[i]; const auto pt = cloud_in.points[i];
if (check(pt)) continue; if (check(pt)) {
if (removed_indices) removed_indices->push_back(i);
continue;
}
cloud_out->points[j++] = pt; cloud_out->points[j++] = pt;
} }

2
common/visualizer/lidar_visualizer.h
View File

@ -12,7 +12,7 @@ namespace common {
struct LidarVisFrame { struct LidarVisFrame {
double timestamp = 0.0; double timestamp = 0.0;
PointCloudPtr cloud = nullptr; PointCloudConstPtr cloud = nullptr;
}; };
class LidarVisualizer { class LidarVisualizer {

4
main.cc
View File

@ -16,10 +16,8 @@ void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
OhMyLoam* const slam); OhMyLoam* const slam);
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
auto curr_path = std::filesystem::current_path();
// config // config
YAMLConfig::Instance()->Init( YAMLConfig::Instance()->Init(argv[1]);
(curr_path / "oh_my_loam/configs/config.yaml").string());
bool log_to_file = YAMLConfig::Instance()->Get<bool>("log_to_file"); bool log_to_file = YAMLConfig::Instance()->Get<bool>("log_to_file");
std::string log_path = YAMLConfig::Instance()->Get<std::string>("log_path"); std::string log_path = YAMLConfig::Instance()->Get<std::string>("log_path");
std::string lidar = YAMLConfig::Instance()->Get<std::string>("lidar"); std::string lidar = YAMLConfig::Instance()->Get<std::string>("lidar");

14
oh_my_loam/configs/config.yaml
View File

@ -6,24 +6,24 @@ vis: true
# configs for extractor # configs for extractor
extractor_config: extractor_config:
vis: false vis: true
verbose: true verbose: true
min_point_num: 66 min_point_num: 66
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: 3 ## useless currently surf_point_num: 20 ## useless currently
corner_point_curvature_thres: 0.5 corner_point_curvature_th: 0.5
surf_point_curvature_thres: 0.5 surf_point_curvature_th: 0.5
neighbor_point_dist_thres: 0.05 neighbor_point_dist_th: 0.1
downsample_voxel_size: 0.3 downsample_voxel_size: 0.3
# configs for odometer # configs for odometer
odometer_config: odometer_config:
vis: true vis: false
verbose: false verbose: false
icp_iter_num : 2 icp_iter_num : 2
match_dist_sq_thresh: 1 match_dist_sq_th: 1
# configs for mapper # configs for mapper
mapper_config: mapper_config:

85
oh_my_loam/extractor/extractor.cc
View File

@ -22,7 +22,7 @@ bool Extractor::Init() {
return true; return true;
} }
void Extractor::Process(const PointCloudConstPtr& cloud, void Extractor::Process(double timestamp, const PointCloudConstPtr& cloud,
Feature* const feature) { Feature* const feature) {
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>()) {
@ -34,23 +34,22 @@ void Extractor::Process(const PointCloudConstPtr& cloud,
std::vector<TCTPointCloud> scans; std::vector<TCTPointCloud> scans;
SplitScan(*cloud, &scans); SplitScan(*cloud, &scans);
AINFO_IF(verbose_) << "Extractor::SplitScan: " << BLOCK_TIMER_STOP_FMT; AINFO_IF(verbose_) << "Extractor::SplitScan: " << BLOCK_TIMER_STOP_FMT;
// compute curvature for each point in each scan // compute curvature to each point
for (auto& scan : scans) { for (auto& scan : scans) {
ComputeCurvature(&scan); ComputeCurvature(&scan);
} }
AINFO_IF(verbose_) << "Extractor::ComputeCurvature: " << BLOCK_TIMER_STOP_FMT; AINFO_IF(verbose_) << "Extractor::ComputeCurvature: " << BLOCK_TIMER_STOP_FMT;
// assign type to each point in each scan: FLAT, LESS_FLAT, // assign type to each point: FLAT, LESS_FLAT, NORMAL, LESS_SHARP or SHARP
// NORMAL, LESS_SHARP or SHARP
for (auto& scan : scans) { for (auto& scan : scans) {
AssignType(&scan); AssignType(&scan);
} }
AINFO_IF(verbose_) << "Extractor::AssignType: " << BLOCK_TIMER_STOP_FMT; AINFO_IF(verbose_) << "Extractor::AssignType: " << BLOCK_TIMER_STOP_FMT;
// store points into feature point clouds according to their type // store points into feature point clouds based on their type
for (const auto& scan : scans) { for (const auto& scan : scans) {
GenerateFeature(scan, feature); GenerateFeature(scan, feature);
} }
AINFO << "Extractor::Process: " << BLOCK_TIMER_STOP_FMT; AINFO << "Extractor::Process: " << BLOCK_TIMER_STOP_FMT;
if (is_vis_) Visualize(*cloud, *feature); if (is_vis_) Visualize(cloud, *feature, timestamp);
} }
void Extractor::SplitScan(const PointCloud& cloud, void Extractor::SplitScan(const PointCloud& cloud,
@ -58,7 +57,7 @@ void Extractor::SplitScan(const PointCloud& cloud,
scans->resize(num_scans_); scans->resize(num_scans_);
double yaw_start = -atan2(cloud.points[0].y, cloud.points[0].x); double yaw_start = -atan2(cloud.points[0].y, cloud.points[0].x);
bool half_passed = false; bool half_passed = false;
for (const auto& pt : cloud.points) { 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 = -atan2(pt.y, pt.x);
@ -69,13 +68,14 @@ void Extractor::SplitScan(const PointCloud& cloud,
half_passed = true; half_passed = true;
yaw_start += kTwoPi; yaw_start += kTwoPi;
} }
(*scans)[scan_id].points.emplace_back( (*scans)[scan_id].push_back(
pt.x, pt.y, pt.z, yaw_diff / kTwoPi + scan_id, std::nanf("")); {pt.x, pt.y, pt.z, static_cast<float>(yaw_diff / kTwoPi + scan_id),
std::nanf("")});
} }
} }
void Extractor::ComputeCurvature(TCTPointCloud* const scan) const { void Extractor::ComputeCurvature(TCTPointCloud* const scan) const {
if (scan->size() < 20) return; if (scan->size() <= 10 + kScanSegNum) return;
auto& pts = scan->points; auto& pts = scan->points;
for (size_t i = 5; i < pts.size() - 5; ++i) { for (size_t i = 5; i < pts.size() - 5; ++i) {
float dx = pts[i - 5].x + pts[i - 4].x + pts[i - 3].x + pts[i - 2].x + float dx = pts[i - 5].x + pts[i - 4].x + pts[i - 3].x + pts[i - 2].x +
@ -87,7 +87,7 @@ void Extractor::ComputeCurvature(TCTPointCloud* const scan) const {
float dz = pts[i - 5].z + pts[i - 4].z + pts[i - 3].z + pts[i - 2].z + float dz = pts[i - 5].z + pts[i - 4].z + pts[i - 3].z + pts[i - 2].z +
pts[i - 1].z + pts[i + 1].z + pts[i + 2].z + pts[i + 3].z + pts[i - 1].z + pts[i + 1].z + pts[i + 2].z + pts[i + 3].z +
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::sqrt(dx * dx + dy * dy + dz * dz); pts[i].curvature = std::hypot(dx, dy, dz);
} }
RemovePoints<TCTPoint>(*scan, scan, [](const TCTPoint& pt) { RemovePoints<TCTPoint>(*scan, scan, [](const TCTPoint& pt) {
return !std::isfinite(pt.curvature); return !std::isfinite(pt.curvature);
@ -96,7 +96,7 @@ void Extractor::ComputeCurvature(TCTPointCloud* const scan) const {
void Extractor::AssignType(TCTPointCloud* const scan) const { void Extractor::AssignType(TCTPointCloud* const scan) const {
int pt_num = scan->size(); int pt_num = scan->size();
ACHECK(pt_num >= kScanSegNum); 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 = Range(pt_num);
@ -104,51 +104,50 @@ void Extractor::AssignType(TCTPointCloud* const scan) const {
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>();
int surf_point_num = config_["surf_point_num"].as<int>(); int surf_point_num = config_["surf_point_num"].as<int>();
float corner_point_curvature_thres = float corner_point_curvature_th =
config_["corner_point_curvature_thres"].as<float>(); config_["corner_point_curvature_th"].as<float>();
float surf_point_curvature_thres = float surf_point_curvature_th =
config_["surf_point_curvature_thres"].as<float>(); config_["surf_point_curvature_th"].as<float>();
for (int seg = 0; seg < kScanSegNum; ++seg) { for (int seg = 0; seg < kScanSegNum; ++seg) {
int s = seg * seg_pt_num; int b = seg * seg_pt_num;
int e = std::min((seg + 1) * seg_pt_num, pt_num); int e = std::min((seg + 1) * seg_pt_num, pt_num);
// sort by curvature for each segment: large -> small // sort by curvature for each segment: large -> small
std::sort(indices.begin() + s, indices.begin() + e, [&](int i, int j) { std::sort(indices.begin() + b, indices.begin() + e, [&](int i, int j) {
return scan->at(i).curvature > scan->at(j).curvature; return scan->at(i).curvature > scan->at(j).curvature;
}); });
// pick corner points // pick corner points
int corner_pt_picked_num = 0; int corner_point_picked_num = 0;
for (int i = s; i < e; ++i) { for (int i = b; i < e; ++i) {
size_t ix = indices[i]; size_t ix = indices[i];
if (!picked.at(ix) && if (!picked.at(ix) &&
scan->at(ix).curvature > corner_point_curvature_thres) { scan->at(ix).curvature > corner_point_curvature_th) {
++corner_pt_picked_num; ++corner_point_picked_num;
if (corner_pt_picked_num <= sharp_corner_point_num) { if (corner_point_picked_num <= sharp_corner_point_num) {
scan->at(ix).type = Type::SHARP; scan->at(ix).type = Type::SHARP;
} else if (corner_pt_picked_num <= corner_point_num) { } else if (corner_point_picked_num <= corner_point_num) {
scan->at(ix).type = Type::LESS_SHARP; scan->at(ix).type = Type::LESS_SHARP;
} else { } else {
break; break;
} }
picked.at(ix) = true; picked.at(ix) = true;
SetNeighborsPicked(*scan, ix, &picked); UpdateNeighborsPicked(*scan, ix, &picked);
} }
} }
// pick surface points // pick surface points
int surf_pt_picked_num = 0; int surf_point_picked_num = 0;
for (int i = e - 1; i >= s; --i) { for (int i = e - 1; i >= b; --i) {
size_t ix = indices[i]; size_t ix = indices[i];
if (!picked.at(ix) && if (!picked.at(ix) && scan->at(ix).curvature < surf_point_curvature_th) {
scan->at(ix).curvature < surf_point_curvature_thres) { ++surf_point_picked_num;
++surf_pt_picked_num; if (surf_point_picked_num <= flat_surf_point_num) {
if (surf_pt_picked_num <= flat_surf_point_num) {
scan->at(ix).type = Type::FLAT; scan->at(ix).type = Type::FLAT;
} else if (surf_pt_picked_num <= surf_point_num) { } else if (surf_point_picked_num <= surf_point_num) {
scan->at(ix).type = Type::LESS_FLAT; scan->at(ix).type = Type::LESS_FLAT;
} else { } else {
break; break;
} }
picked.at(ix) = true; picked.at(ix) = true;
SetNeighborsPicked(*scan, ix, &picked); UpdateNeighborsPicked(*scan, ix, &picked);
} }
} }
} }
@ -184,35 +183,31 @@ void Extractor::GenerateFeature(const TCTPointCloud& scan,
feature->cloud_less_flat_surf = dowm_sampled; feature->cloud_less_flat_surf = dowm_sampled;
} }
void Extractor::Visualize(const PointCloud& cloud, const Feature& feature, void Extractor::Visualize(const PointCloudConstPtr& cloud,
double timestamp) { const Feature& feature, double timestamp) {
std::shared_ptr<ExtractorVisFrame> frame(new ExtractorVisFrame); std::shared_ptr<ExtractorVisFrame> frame(new ExtractorVisFrame);
frame->timestamp = timestamp; frame->timestamp = timestamp;
frame->cloud = cloud.makeShared(); frame->cloud = cloud;
frame->feature = feature; frame->feature = feature;
visualizer_->Render(frame); visualizer_->Render(frame);
} }
void Extractor::SetNeighborsPicked(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 DistSqure = [&](int i, int j) -> float {
float dx = scan.at(i).x - scan.at(j).x; return DistanceSqure<TCTPoint>(scan[i], scan[j]);
float dy = scan.at(i).y - scan.at(j).y;
float dz = scan.at(i).z - scan.at(j).z;
return dx * dx + dy * dy + dz * dz;
}; };
float neighbor_point_dist_thres = float neighbor_point_dist_th = config_["neighbor_point_dist_th"].as<float>();
config_["neighbor_point_dist_thres"].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_thres) break; if (DistSqure(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_thres) break; if (DistSqure(ix + i, ix + i - 1) > neighbor_point_dist_th) break;
picked->at(ix + i) = true; picked->at(ix + i) = true;
} }
} }

15
oh_my_loam/extractor/extractor.h
View File

@ -15,15 +15,14 @@ class Extractor {
bool Init(); bool Init();
void Process(const common::PointCloudConstPtr& cloud, Feature* const feature); void Process(double timestamp, const common::PointCloudConstPtr& cloud,
Feature* const feature);
int num_scans() const { return num_scans_; } int num_scans() const { return num_scans_; }
protected: protected:
virtual int GetScanID(const common::Point& pt) const = 0; virtual int GetScanID(const common::Point& pt) const = 0;
YAML::Node config_;
virtual void SplitScan(const common::PointCloud& cloud, virtual void SplitScan(const common::PointCloud& cloud,
std::vector<TCTPointCloud>* const scans) const; std::vector<TCTPointCloud>* const scans) const;
@ -34,17 +33,19 @@ class Extractor {
virtual void GenerateFeature(const TCTPointCloud& scan, virtual void GenerateFeature(const TCTPointCloud& scan,
Feature* const feature) const; Feature* const feature) const;
virtual void Visualize(const common::PointCloudConstPtr& cloud,
const Feature& feature, double timestamp = 0.0);
int num_scans_ = 0; int num_scans_ = 0;
YAML::Node config_;
std::unique_ptr<ExtractorVisualizer> visualizer_{nullptr}; std::unique_ptr<ExtractorVisualizer> visualizer_{nullptr};
bool verbose_ = false; bool verbose_ = false;
private: private:
void Visualize(const common::PointCloud& cloud, const Feature& feature, void UpdateNeighborsPicked(const TCTPointCloud& scan, size_t ix,
double timestamp = 0.0);
void SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
std::vector<bool>* const picked) const; std::vector<bool>* const picked) const;
bool is_vis_ = false; bool is_vis_ = false;

7
oh_my_loam/extractor/extractor_VLP16.cc
View File

@ -1,10 +1,13 @@
#include "extractor_VLP16.h" #include "extractor_VLP16.h"
#include "common/math/math_utils.h"
namespace oh_my_loam { namespace oh_my_loam {
int ExtractorVLP16::GetScanID(const common::Point& pt) const { int ExtractorVLP16::GetScanID(const common::Point& pt) const {
double omega = std::atan2(pt.z, std::hypot(pt.x, pt.y)) * 180 * M_1_PI + 15.0; double theta =
return static_cast<int>(std::round(omega / 2.0) + 1.e-5); common::Rad2Degree(std::atan2(pt.z, std::hypot(pt.x, pt.y))) + 15.0;
return static_cast<int>(std::round(theta / 2.0) + 1.e-5);
}; };
} // namespace oh_my_loam } // namespace oh_my_loam

13
oh_my_loam/odometer/odometer.cc
View File

@ -18,12 +18,13 @@ bool Odometer::Init() {
verbose_ = config_["verbose"].as<bool>(); verbose_ = config_["verbose"].as<bool>();
kdtree_surf_.reset(new pcl::KdTreeFLANN<TPoint>); kdtree_surf_.reset(new pcl::KdTreeFLANN<TPoint>);
kdtree_corn_.reset(new pcl::KdTreeFLANN<TPoint>); kdtree_corn_.reset(new pcl::KdTreeFLANN<TPoint>);
AINFO << "Odometer visualizer: " << (is_vis_ ? "ON" : "OFF"); AINFO << "Odometry visualizer: " << (is_vis_ ? "ON" : "OFF");
if (is_vis_) visualizer_.reset(new OdometerVisualizer); if (is_vis_) visualizer_.reset(new OdometerVisualizer);
return true; return true;
} }
void Odometer::Process(const Feature& feature, Pose3D* const pose) { void Odometer::Process(double timestamp, const Feature& feature,
Pose3D* const pose) {
BLOCK_TIMER_START; BLOCK_TIMER_START;
if (!is_initialized_) { if (!is_initialized_) {
is_initialized_ = true; is_initialized_ = true;
@ -75,7 +76,7 @@ void Odometer::Process(const Feature& feature, Pose3D* const pose) {
void Odometer::MatchCornFeature(const TPointCloud& src, const TPointCloud& tgt, void Odometer::MatchCornFeature(const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointLinePair>* const pairs) const { std::vector<PointLinePair>* const pairs) const {
double dist_sq_thresh = config_["match_dist_sq_thresh"].as<double>(); double dist_sq_thresh = config_["match_dist_sq_th"].as<double>();
for (const auto& q_pt : src) { for (const auto& q_pt : src) {
TPoint query_pt; TPoint query_pt;
TransformToStart(pose_curr2last_, q_pt, &query_pt); TransformToStart(pose_curr2last_, q_pt, &query_pt);
@ -119,7 +120,7 @@ void Odometer::MatchCornFeature(const TPointCloud& src, const TPointCloud& tgt,
void Odometer::MatchSurfFeature( void Odometer::MatchSurfFeature(
const TPointCloud& src, const TPointCloud& tgt, const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointPlanePair>* const pairs) const { std::vector<PointPlanePair>* const pairs) const {
double dist_sq_thresh = config_["match_dist_sq_thresh"].as<double>(); double dist_sq_thresh = config_["match_dist_sq_th"].as<double>();
for (const auto& q_pt : src) { for (const auto& q_pt : src) {
TPoint query_pt; TPoint query_pt;
TransformToStart(pose_curr2last_, q_pt, &query_pt); TransformToStart(pose_curr2last_, q_pt, &query_pt);
@ -193,8 +194,10 @@ void Odometer::UpdatePre(const Feature& feature) {
void Odometer::Visualize(const Feature& feature, void Odometer::Visualize(const Feature& feature,
const std::vector<PointLinePair>& pl_pairs, const std::vector<PointLinePair>& pl_pairs,
const std::vector<PointPlanePair>& pp_pairs) const { const std::vector<PointPlanePair>& pp_pairs,
double timestamp) const {
std::shared_ptr<OdometerVisFrame> frame(new OdometerVisFrame); std::shared_ptr<OdometerVisFrame> frame(new OdometerVisFrame);
frame->timestamp = timestamp;
frame->pl_pairs = pl_pairs; frame->pl_pairs = pl_pairs;
frame->pp_pairs = pp_pairs; frame->pp_pairs = pp_pairs;
frame->pose_curr2last = pose_curr2last_; frame->pose_curr2last = pose_curr2last_;

6
oh_my_loam/odometer/odometer.h
View File

@ -15,7 +15,8 @@ class Odometer {
bool Init(); bool Init();
void Process(const Feature& feature, common::Pose3D* const pose); void Process(double timestamp, const Feature& feature,
common::Pose3D* const pose);
protected: protected:
void UpdatePre(const Feature& feature); void UpdatePre(const Feature& feature);
@ -30,7 +31,8 @@ class Odometer {
void Visualize(const Feature& feature, void Visualize(const Feature& feature,
const std::vector<PointLinePair>& pl_pairs, const std::vector<PointLinePair>& pl_pairs,
const std::vector<PointPlanePair>& pp_pairs) const; const std::vector<PointPlanePair>& pp_pairs,
double timestamp = 0.0) const;
common::Pose3D pose_curr2world_; common::Pose3D pose_curr2world_;
common::Pose3D pose_curr2last_; common::Pose3D pose_curr2last_;

6
oh_my_loam/oh_my_loam.cc
View File

@ -8,7 +8,7 @@ namespace oh_my_loam {
namespace { namespace {
const double kPointMinDist = 0.1; const double kPointMinDist = 0.1;
using namespace common; using namespace common;
} } // namespace
bool OhMyLoam::Init() { bool OhMyLoam::Init() {
YAML::Node config = YAMLConfig::Instance()->config(); YAML::Node config = YAMLConfig::Instance()->config();
@ -34,9 +34,9 @@ void OhMyLoam::Run(double timestamp, const PointCloudConstPtr& cloud_in) {
PointCloudPtr cloud(new PointCloud); PointCloudPtr cloud(new PointCloud);
RemoveOutliers(*cloud_in, cloud.get()); RemoveOutliers(*cloud_in, cloud.get());
Feature feature; Feature feature;
extractor_->Process(cloud, &feature); extractor_->Process(timestamp, cloud, &feature);
Pose3D pose; Pose3D pose;
odometer_->Process(feature, &pose); odometer_->Process(timestamp, feature, &pose);
poses_.emplace_back(pose); poses_.emplace_back(pose);
} }

8
oh_my_loam/visualizer/extractor_visualizer.cc
View File

@ -7,12 +7,12 @@ using namespace common;
void ExtractorVisualizer::Draw() { void ExtractorVisualizer::Draw() {
auto frame = GetCurrentFrame<ExtractorVisFrame>(); auto frame = GetCurrentFrame<ExtractorVisFrame>();
DrawPointCloud<Point>(frame.cloud, WHITE, "cloud_raw"); DrawPointCloud<Point>(frame.cloud, GRAY, "cloud_raw");
DrawPointCloud<TPoint>(frame.feature.cloud_less_flat_surf, CYAN, DrawPointCloud<TPoint>(frame.feature.cloud_less_flat_surf, BLUE,
"cloud_less_flat_surf"); "cloud_less_flat_surf");
DrawPointCloud<TPoint>(frame.feature.cloud_flat_surf, BLUE, DrawPointCloud<TPoint>(frame.feature.cloud_flat_surf, CYAN,
"cloud_flat_surf"); "cloud_flat_surf");
DrawPointCloud<TPoint>(frame.feature.cloud_less_sharp_corner, PURPLE, DrawPointCloud<TPoint>(frame.feature.cloud_less_sharp_corner, YELLOW,
"cloud_less_sharp_corner"); "cloud_less_sharp_corner");
DrawPointCloud<TPoint>(frame.feature.cloud_sharp_corner, RED, DrawPointCloud<TPoint>(frame.feature.cloud_sharp_corner, RED,
"cloud_sharp_corner"); "cloud_sharp_corner");