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

make extracker even better, start rewrite Pose

main
feixyz10 2021-01-21 01:11:20 +08:00 committed by feixyz
parent da337376f9
commit 1ccebd8b8f
16 changed files with 143 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
CMakeLists.txt
View File

@ -54,9 +54,9 @@ target_link_libraries(main
common common
oh_my_loam oh_my_loam
extractor extractor
odometer # odometer
mapper # mapper
solver # solver
${CERES_LIBRARIES} ${CERES_LIBRARIES}
visualizer visualizer
base base

3
common/color/color.h
View File

@ -12,7 +12,8 @@ struct Color {
#define RED Color(255, 0, 0) #define RED Color(255, 0, 0)
#define GREEN Color(0, 255, 0) #define GREEN Color(0, 255, 0)
#define BLUE Color(0, 0, 255) #define BLUE Color(0, 0, 255)
#define GRAY Color(50, 50, 50) #define DRAK_GRAY Color(60, 60, 60)
#define GRAY Color(128, 128, 128)
#define CYAN Color(0, 255, 255) #define CYAN Color(0, 255, 255)
#define PURPLE Color(160, 32, 240) #define PURPLE Color(160, 32, 240)
#define VIOLET Color(238, 130, 238) #define VIOLET Color(238, 130, 238)

20
common/geometry/pose.cc
View File

@ -1,20 +0,0 @@
#include "pose.h"
namespace common {
Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t) {
return pose_from.Interpolate(pose_to, t);
}
Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs) {
return Pose3D(lhs.q() * rhs.q(), lhs.q() * rhs.p() + lhs.p());
}
std::string Pose3D::ToString() const {
std::ostringstream oss;
oss << "[Pose3D] q = (" << q_.coeffs().transpose() << "), p = ("
<< p_.transpose() << ")";
return oss.str();
}
} // namespace common

83
common/geometry/pose.h
View File

@ -4,63 +4,82 @@
namespace common { namespace common {
class Pose3D { template <typename T>
class Pose3 {
public: public:
Pose3D() { Pose3() {
q_.setIdentity(); r_quat_.setIdentity();
p_.setZero(); t_vec_.setZero();
}; };
Pose3D(const Eigen::Quaterniond& q, const Eigen::Vector3d& p) Pose3(const Eigen::Quaterniond& r_quat, const Eigen::Matrix<T, 3, 1>& t_vec)
: q_(q), p_(p) {} : r_quat_(q), t_vec_(p) {}
Pose3D(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& p) Pose3(const Eigen::Matrix3d& r_mat, const Eigen::Matrix<T, 3, 1>& t_vec)
: q_(r_mat), p_(p) {} : r_quat_(r_mat), t_vec_(t_vec) {}
Pose3D(const double* const q, const double* const p) : q_(q), p_(p) {} Pose3(const double* const r_quat, const double* const t_vec)
: r_quat_(r_quat), t_vec_(t_vec) {}
Pose3D Inv() const { Pose3 Inv() const {
Eigen::Quaterniond q_inv = q_.inverse(); Eigen::Quaternion<T> r_inv = r_quat_.inverse();
Eigen::Vector3d p_inv = q_inv * p_; Eigen::Matrix<T, 3, 1> t_inv = -r_inv * t_vec_;
return {q_inv, -p_inv}; return {r_inv, t_inv};
} }
Eigen::Vector3d Transform(const Eigen::Vector3d& vec) const { Eigen::Matrix<T, 3, 1> Transform(const Eigen::Matrix<T, 3, 1>& point) const {
return q_ * vec + p_; return r_quat_ * point + t_vec_;
} }
Eigen::Vector3d operator*(const Eigen::Vector3d& vec) const { Eigen::Matrix<T, 3, 1> operator*(const Eigen::Matrix<T, 3, 1>& point) const {
return Transform(vec); return Transform(point);
} }
Eigen::Vector3d Translate(const Eigen::Vector3d& vec) const { Eigen::Matrix<T, 3, 1> Translate(const Eigen::Matrix<T, 3, 1>& vec) const {
return vec + p_; return vec + t_vec_;
} }
Eigen::Vector3d Rotate(const Eigen::Vector3d& vec) const { return q_ * vec; } Eigen::Matrix<T, 3, 1> Rotate(const Eigen::Matrix<T, 3, 1>& vec) const {
return r_quat_ * vec;
}
// Spherical linear interpolation to `pose_to`, `t` belongs [0, 1] // Spherical linear interpolation to `pose_to`
Pose3D Interpolate(const Pose3D& pose_to, double t) const { Pose3 Interpolate(const Pose3& pose_to, double t) const {
Eigen::Quaterniond q_interp = q_.slerp(t, pose_to.q_); Pose3 pose_dst;
Eigen::Vector3d p_interp = (pose_to.p_ - p_) * t + p_; Eigen::Quaternion<T> q_interp = r_quat_.slerp(t, pose_to.r_quat_);
Eigen::Matrix<T, 3, 1> p_interp = (pose_to.t_vec_ - t_vec_) * t + t_vec_;
return {q_interp, p_interp}; return {q_interp, p_interp};
} }
std::string ToString() const; std::string ToString() const {
std::ostringstream oss;
oss << "[Pose3D] q = (" << q_.coeffs().transpose() << "), p = ("
<< p_.transpose() << ")";
return oss.str();
}
Eigen::Quaterniond q() const { return q_; } Eigen::Quaternion<T> r_quat() const { return r_quat_; }
Eigen::Vector3d p() const { return p_; } Eigen::Matrix<T, 3, 1> t_vec() const { return t_vec_; }
protected: protected:
Eigen::Quaterniond q_; // orientation Eigen::Quaternion<T> r_quat_; // orientation/rotation
Eigen::Vector3d p_; // position Eigen::Matrix<T, 3, 1> t_vec_; // positon/translation
}; };
Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t); template <typename T>
Pose3<T> Interpolate(const Pose3<T>& pose_from, const Pose3<T>& pose_to,
double t) {
return pose_from.Interpolate(pose_to, t);
}
Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs); template <typename T>
Pose3<T> operator*(const Pose3<T>& lhs, const Pose3<T>& rhs) {
return Pose3<T>(lhs.q() * rhs.q(), lhs.q() * rhs.p() + lhs.p());
}
using Trans3d = Pose3D; using Pose3d = Pose3<double>;
using Pose3f = Pose3<float>;
} // namespace common } // namespace common

6
oh_my_loam/CMakeLists.txt
View File

@ -1,9 +1,9 @@
add_subdirectory(base) add_subdirectory(base)
add_subdirectory(extractor) add_subdirectory(extractor)
add_subdirectory(visualizer) add_subdirectory(visualizer)
add_subdirectory(odometer) # add_subdirectory(odometer)
add_subdirectory(mapper) # add_subdirectory(mapper)
add_subdirectory(solver) # add_subdirectory(solver)
aux_source_directory(. SRC_FILES) aux_source_directory(. SRC_FILES)

8
oh_my_loam/base/feature.h
View File

@ -5,16 +5,16 @@
namespace oh_my_loam { namespace oh_my_loam {
struct Feature { struct Feature {
common::TPointCloudPtr cloud_corner;
common::TPointCloudPtr cloud_sharp_corner; common::TPointCloudPtr cloud_sharp_corner;
common::TPointCloudPtr cloud_less_sharp_corner; common::TPointCloudPtr cloud_surf;
common::TPointCloudPtr cloud_flat_surf; common::TPointCloudPtr cloud_flat_surf;
common::TPointCloudPtr cloud_less_flat_surf;
Feature() { Feature() {
cloud_corner.reset(new common::TPointCloud);
cloud_sharp_corner.reset(new common::TPointCloud); cloud_sharp_corner.reset(new common::TPointCloud);
cloud_less_sharp_corner.reset(new common::TPointCloud); cloud_surf.reset(new common::TPointCloud);
cloud_flat_surf.reset(new common::TPointCloud); cloud_flat_surf.reset(new common::TPointCloud);
cloud_less_flat_surf.reset(new common::TPointCloud);
} }
}; };

20
oh_my_loam/base/types.h
View File

@ -4,12 +4,12 @@
namespace oh_my_loam { namespace oh_my_loam {
enum class Type { enum class PType {
FLAT = -2, FLAT_SURF = -2,
LESS_FLAT = -1, SURF = -1,
NORNAL = 0, NORNAL = 0,
LESS_SHARP = 1, CORNER = 1,
SHARP = 2, SHARP_CORNER = 2
}; };
struct PointXYZTCT; struct PointXYZTCT;
@ -24,7 +24,7 @@ struct PointXYZTCT {
struct { struct {
float time; float time;
float curvature; float curvature;
Type type; PType type;
}; };
float data_c[4]; float data_c[4];
}; };
@ -33,11 +33,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 = Type::NORNAL; type = PType::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, Type type = Type::NORNAL) float curvature = 0.0f, PType type = PType::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;
} }
@ -49,7 +49,7 @@ struct PointXYZTCT {
data[3] = 1.0f; data[3] = 1.0f;
time = 0.0f; time = 0.0f;
curvature = 0.0f; curvature = 0.0f;
type = Type::NORNAL; type = PType::NORNAL;
} }
PointXYZTCT(const common::TPoint& p) { PointXYZTCT(const common::TPoint& p) {
@ -59,7 +59,7 @@ struct PointXYZTCT {
data[3] = 1.0f; data[3] = 1.0f;
time = p.time; time = p.time;
curvature = 0.0f; curvature = 0.0f;
type = Type::NORNAL; type = PType::NORNAL;
} }
PointXYZTCT(const PointXYZTCT& p) { PointXYZTCT(const PointXYZTCT& p) {

2
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: 20 ## useless currently surf_point_num: 20
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

54
oh_my_loam/extractor/extractor.cc
View File

@ -23,7 +23,7 @@ bool Extractor::Init() {
} }
void Extractor::Process(double timestamp, const PointCloudConstPtr& cloud, void Extractor::Process(double timestamp, const PointCloudConstPtr& cloud,
Feature* const feature) { 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>()) {
AWARN << "Too few input points: num = " << cloud->size() << " (< " AWARN << "Too few input points: num = " << cloud->size() << " (< "
@ -45,11 +45,13 @@ void Extractor::Process(double timestamp, const PointCloudConstPtr& cloud,
} }
AINFO_IF(verbose_) << "Extractor::AssignType: " << BLOCK_TIMER_STOP_FMT; AINFO_IF(verbose_) << "Extractor::AssignType: " << BLOCK_TIMER_STOP_FMT;
// store points into feature point clouds based on their type // store points into feature point clouds based on their type
for (const auto& scan : scans) { for (size_t i = 0; i < scans.size(); ++i) {
GenerateFeature(scan, feature); Feature feature;
GenerateFeature(scans[i], &feature);
features->push_back(std::move(feature));
} }
AINFO << "Extractor::Process: " << BLOCK_TIMER_STOP_FMT; AINFO << "Extractor::Process: " << BLOCK_TIMER_STOP_FMT;
if (is_vis_) Visualize(cloud, *feature, timestamp); if (is_vis_) Visualize(cloud, *features, timestamp);
} }
void Extractor::SplitScan(const PointCloud& cloud, void Extractor::SplitScan(const PointCloud& cloud,
@ -68,9 +70,9 @@ void Extractor::SplitScan(const PointCloud& cloud,
half_passed = true; half_passed = true;
yaw_start += kTwoPi; yaw_start += kTwoPi;
} }
(*scans)[scan_id].push_back( (*scans)[scan_id].push_back({pt.x, pt.y, pt.z,
{pt.x, pt.y, pt.z, static_cast<float>(yaw_diff / kTwoPi + scan_id), static_cast<float>(yaw_diff / kTwoPi),
std::nanf("")}); std::nanf("")});
} }
} }
@ -123,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 = Type::SHARP; scan->at(ix).type = PType::SHARP_CORNER;
} else if (corner_point_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 = PType::CORNER;
} else { } else {
break; break;
} }
@ -140,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 = Type::FLAT; scan->at(ix).type = PType::FLAT_SURF;
} else if (surf_point_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 = PType::SURF;
} else { } else {
break; break;
} }
@ -155,40 +157,38 @@ void Extractor::AssignType(TCTPointCloud* const scan) const {
void Extractor::GenerateFeature(const TCTPointCloud& scan, void Extractor::GenerateFeature(const TCTPointCloud& scan,
Feature* const feature) const { Feature* const feature) const {
TPointCloudPtr cloud_less_flat_surf(new TPointCloud); for (const auto& pt : scan) {
for (const auto& pt : scan.points) {
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 Type::FLAT: case PType::FLAT_SURF:
feature->cloud_flat_surf->points.emplace_back(point); feature->cloud_flat_surf->points.emplace_back(point);
// no break: FLAT points are also LESS_FLAT // no break: FLAT_SURF points are also SURF points
case Type::LESS_FLAT: case PType::SURF:
cloud_less_flat_surf->points.emplace_back(point); feature->cloud_surf->points.emplace_back(point);
break; break;
case Type::SHARP: case PType::SHARP_CORNER:
feature->cloud_sharp_corner->points.emplace_back(point); feature->cloud_sharp_corner->points.emplace_back(point);
// no break: SHARP points are also LESS_SHARP // no break: SHARP_CORNER points are also CORNER points
case Type::LESS_SHARP: case PType::CORNER:
feature->cloud_less_sharp_corner->points.emplace_back(point); feature->cloud_corner->points.emplace_back(point);
break; break;
default: default:
// all the rest are also LESS_FLAT
cloud_less_flat_surf->points.emplace_back(pt.x, pt.y, pt.z, pt.time);
break; break;
} }
} }
TPointCloudPtr dowm_sampled(new TPointCloud); TPointCloudPtr dowm_sampled(new TPointCloud);
VoxelDownSample<TPoint>(cloud_less_flat_surf, dowm_sampled.get(), VoxelDownSample<TPoint>(feature->cloud_surf, dowm_sampled.get(),
config_["downsample_voxel_size"].as<double>()); config_["downsample_voxel_size"].as<double>());
feature->cloud_less_flat_surf = dowm_sampled; feature->cloud_surf->swap(*dowm_sampled);
} }
void Extractor::Visualize(const PointCloudConstPtr& cloud, void Extractor::Visualize(const PointCloudConstPtr& cloud,
const Feature& feature, double timestamp) { const std::vector<Feature>& features,
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; frame->cloud = cloud;
frame->feature = feature; frame->features = features;
visualizer_->Render(frame); visualizer_->Render(frame);
} }

5
oh_my_loam/extractor/extractor.h
View File

@ -16,7 +16,7 @@ class Extractor {
bool Init(); bool Init();
void Process(double timestamp, const common::PointCloudConstPtr& cloud, void Process(double timestamp, const common::PointCloudConstPtr& cloud,
Feature* const feature); std::vector<Feature>* const features);
int num_scans() const { return num_scans_; } int num_scans() const { return num_scans_; }
@ -34,7 +34,8 @@ class Extractor {
Feature* const feature) const; Feature* const feature) const;
virtual void Visualize(const common::PointCloudConstPtr& cloud, virtual void Visualize(const common::PointCloudConstPtr& cloud,
const Feature& feature, double timestamp = 0.0); const std::vector<Feature>& features,
double timestamp = 0.0);
int num_scans_ = 0; int num_scans_ = 0;

10
oh_my_loam/odometer/odometer.h
View File

@ -15,11 +15,11 @@ class Odometer {
bool Init(); bool Init();
void Process(double timestamp, const Feature& feature, void Process(double timestamp, const std::vector<Feature>& features,
common::Pose3D* const pose); common::Pose3D* const pose);
protected: protected:
void UpdatePre(const Feature& feature); void UpdatePre(const std::vector<Feature>& feature) s;
void MatchCornFeature(const common::TPointCloud& src, void MatchCornFeature(const common::TPointCloud& src,
const common::TPointCloud& tgt, const common::TPointCloud& tgt,
@ -29,7 +29,7 @@ class Odometer {
const common::TPointCloud& tgt, const common::TPointCloud& tgt,
std::vector<PointPlanePair>* const pairs) const; std::vector<PointPlanePair>* const pairs) const;
void Visualize(const Feature& feature, void Visualize(const std::vector<Feature>& features,
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;
@ -37,8 +37,8 @@ class Odometer {
common::Pose3D pose_curr2world_; common::Pose3D pose_curr2world_;
common::Pose3D pose_curr2last_; common::Pose3D pose_curr2last_;
common::TPointCloudPtr corn_pre_{nullptr}; common::TPointCloudPtr cloud_corn_pre_{nullptr};
common::TPointCloudPtr surf_pre_{nullptr}; common::TPointCloudPtr cloud_surf_pre_{nullptr};
pcl::KdTreeFLANN<common::TPoint>::Ptr kdtree_surf_{nullptr}; pcl::KdTreeFLANN<common::TPoint>::Ptr kdtree_surf_{nullptr};
pcl::KdTreeFLANN<common::TPoint>::Ptr kdtree_corn_{nullptr}; pcl::KdTreeFLANN<common::TPoint>::Ptr kdtree_corn_{nullptr};

30
oh_my_loam/oh_my_loam.cc
View File

@ -17,27 +17,27 @@ bool OhMyLoam::Init() {
AERROR << "Failed to initialize extractor"; AERROR << "Failed to initialize extractor";
return false; return false;
} }
odometer_.reset(new Odometer); // odometer_.reset(new Odometer);
if (!odometer_->Init()) { // if (!odometer_->Init()) {
AERROR << "Failed to initialize odometer"; // AERROR << "Failed to initialize odometer";
return false; // return false;
} // }
mapper_.reset(new Mapper); // mapper_.reset(new Mapper);
if (!mapper_->Init()) { // if (!mapper_->Init()) {
AERROR << "Failed to initialize mapper"; // AERROR << "Failed to initialize mapper";
return false; // return false;
} // }
return true; return true;
} }
void OhMyLoam::Run(double timestamp, const PointCloudConstPtr& cloud_in) { 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; std::vector<Feature> features;
extractor_->Process(timestamp, cloud, &feature); extractor_->Process(timestamp, cloud, &features);
Pose3D pose; // Pose3D pose;
odometer_->Process(timestamp, feature, &pose); // odometer_->Process(timestamp, feature, &pose);
poses_.emplace_back(pose); // poses_.emplace_back(pose);
} }
void OhMyLoam::RemoveOutliers(const PointCloud& cloud_in, void OhMyLoam::RemoveOutliers(const PointCloud& cloud_in,

9
oh_my_loam/oh_my_loam.h
View File

@ -2,8 +2,8 @@
#include "common/common.h" #include "common/common.h"
#include "oh_my_loam/extractor/extractor.h" #include "oh_my_loam/extractor/extractor.h"
#include "oh_my_loam/mapper/mapper.h" // #include "oh_my_loam/mapper/mapper.h"
#include "oh_my_loam/odometer/odometer.h" // #include "oh_my_loam/odometer/odometer.h"
namespace oh_my_loam { namespace oh_my_loam {
@ -18,13 +18,14 @@ class OhMyLoam {
private: private:
std::unique_ptr<Extractor> extractor_{nullptr}; std::unique_ptr<Extractor> extractor_{nullptr};
std::unique_ptr<Odometer> odometer_{nullptr}; // std::unique_ptr<Odometer> odometer_{nullptr};
std::unique_ptr<Mapper> mapper_{nullptr}; // std::unique_ptr<Mapper> mapper_{nullptr};
// remove outliers: nan and very close points // remove outliers: nan and very close points
void RemoveOutliers(const common::PointCloud& cloud_in, void RemoveOutliers(const common::PointCloud& cloud_in,
common::PointCloud* const cloud_out) const; common::PointCloud* const cloud_out) const;
std::vector<common::Pose3D> poses_; std::vector<common::Pose3D> poses_;
DISALLOW_COPY_AND_ASSIGN(OhMyLoam) DISALLOW_COPY_AND_ASSIGN(OhMyLoam)

19
oh_my_loam/visualizer/extractor_visualizer.cc
View File

@ -8,14 +8,17 @@ using namespace common;
void ExtractorVisualizer::Draw() { void ExtractorVisualizer::Draw() {
auto frame = GetCurrentFrame<ExtractorVisFrame>(); auto frame = GetCurrentFrame<ExtractorVisFrame>();
DrawPointCloud<Point>(frame.cloud, GRAY, "cloud_raw"); DrawPointCloud<Point>(frame.cloud, GRAY, "cloud_raw");
DrawPointCloud<TPoint>(frame.feature.cloud_less_flat_surf, BLUE, for (size_t i = 0; i < frame.features.size(); ++i) {
"cloud_less_flat_surf"); const auto& feature = frame.features[i];
DrawPointCloud<TPoint>(frame.feature.cloud_flat_surf, CYAN, std::string id_suffix = std::to_string(i);
"cloud_flat_surf"); DrawPointCloud<TPoint>(feature.cloud_surf, BLUE, "cloud_surf" + id_suffix);
DrawPointCloud<TPoint>(frame.feature.cloud_less_sharp_corner, YELLOW, DrawPointCloud<TPoint>(feature.cloud_flat_surf, CYAN,
"cloud_less_sharp_corner"); "cloud_flat_surf" + id_suffix);
DrawPointCloud<TPoint>(frame.feature.cloud_sharp_corner, RED, DrawPointCloud<TPoint>(feature.cloud_corner, YELLOW,
"cloud_sharp_corner"); "cloud_corner" + id_suffix);
DrawPointCloud<TPoint>(feature.cloud_sharp_corner, RED,
"cloud_sharp_corner" + id_suffix);
}
}; };
} // namespace oh_my_loam } // namespace oh_my_loam

2
oh_my_loam/visualizer/extractor_visualizer.h
View File

@ -6,7 +6,7 @@
namespace oh_my_loam { namespace oh_my_loam {
struct ExtractorVisFrame : public common::LidarVisFrame { struct ExtractorVisFrame : public common::LidarVisFrame {
Feature feature; std::vector<Feature> features;
}; };
class ExtractorVisualizer : public common::LidarVisualizer { class ExtractorVisualizer : public common::LidarVisualizer {

4
oh_my_loam/visualizer/odometer_visualizer.h
View File

@ -6,8 +6,8 @@
namespace oh_my_loam { namespace oh_my_loam {
struct OdometerVisFrame : public common::LidarVisFrame { struct OdometerVisFrame : public common::LidarVisFrame {
common::TPointCloudPtr surf_pts; common::TPointCloudPtr cloud_surf;
common::TPointCloudPtr corn_pts; common::TPointCloudPtr cloud_corner;
std::vector<PointLinePair> pl_pairs; std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs; std::vector<PointPlanePair> pp_pairs;
common::Pose3D pose_curr2last; common::Pose3D pose_curr2last;