oh_my_loam/src/visualizer_feature_points.h

60 lines
1.9 KiB
C++

#pragma once
#include "feature_extractor_base.h"
#include "visualizer_base.h"
namespace oh_my_loam {
struct FeaturePointsVisFrame : public VisFrame {
std::shared_ptr<const FeaturePoints> feature_pts;
};
class FeaturePointsVisualizer : public Visualizer<FeaturePointsVisFrame> {
public:
explicit FeaturePointsVisualizer(
const std::string &name = "FeaturePointsVisFrame",
size_t max_history_size = 10)
: Visualizer<FeaturePointsVisFrame>(name, max_history_size) {}
protected:
void Draw() override final {
auto frame = GetCurrentFrame();
{ // add raw point cloud
std::string id = "raw point cloud";
DrawPointCloud<Point>(*frame.cloud, WHITE, id, viewer_.get());
rendered_cloud_ids_.push_back(id);
}
{ // add all feature_pts
std::string id = "feature_pts";
DrawPointCloud<IPoint>(frame.feature_pts->feature_pts, "time", id,
viewer_.get());
rendered_cloud_ids_.push_back(id);
}
{ // add flat_surf_pts
std::string id = "flat_surf_pts";
DrawPointCloud(frame.feature_pts->flat_surf_pts, CYAN, id, viewer_.get());
rendered_cloud_ids_.push_back(id);
}
{ // add less_flat_surf_pts
std::string id = "less_flat_surf_pts";
DrawPointCloud(frame.feature_pts->less_flat_surf_pts, GREEN, id,
viewer_.get());
rendered_cloud_ids_.push_back(id);
}
{ // add less_sharp_corner_pts
std::string id = "less_sharp_corner_pts";
DrawPointCloud(frame.feature_pts->less_sharp_corner_pts, ORANGE, id,
viewer_.get());
rendered_cloud_ids_.push_back(id);
}
{ // add sharp_corner_pts
std::string id = "sharp_corner_pts";
DrawPointCloud(frame.feature_pts->sharp_corner_pts, ORANGE, id,
viewer_.get());
rendered_cloud_ids_.push_back(id);
}
}
};
} // namespace oh_my_loam