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

adjust visualizer api

main
feixyz10 2020-12-31 20:59:08 +08:00 committed by feixyz
parent 7584eee1f9
commit 907125f775
4 changed files with 69 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/visualizer/base_visualizer.h
View File

@ -60,7 +60,7 @@ class Visualizer {
is_updated_ = true; is_updated_ = true;
} }
std::string Name() { return name_; } std::string Name() const { return name_; }
protected: protected:
void Run() { void Run() {
@ -75,15 +75,10 @@ class Visualizer {
} }
/** /**
* @brief Draw objects. This method should be overrided for customization. * @brief Draw objects, pure virtual function. Example code:
*
* virtual void Draw() { * virtual void Draw() {
* auto frame = GetCurrentFrame<VisFrame>(); * auto frame = GetCurrentFrame<VisFrame>();
* { // Update cloud * AddPointCloud(frame.cloud, {255, 255, 255}, "point cloud");
* std::string id = "point cloud";
* DrawPointCloud(*frame.cloud, {255, 255, 255}, id, viewer_.get());
* rendered_cloud_ids_.push_back(id);
* }
* } * }
*/ */
virtual void Draw() = 0; virtual void Draw() = 0;
@ -134,6 +129,22 @@ class Visualizer {
return *static_cast<FrameT *>((*curr_frame_iter_).get()); 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);
rendered_cloud_ids_.push_back(id);
}
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);
rendered_cloud_ids_.push_back(id);
}
// visualizer name // visualizer name
std::string name_; std::string name_;
size_t max_history_size_; size_t max_history_size_;

38
src/visualizer/extractor_visualizer.cc
View File

@ -4,35 +4,15 @@ namespace oh_my_loam {
void ExtractorVisualizer::Draw() { void ExtractorVisualizer::Draw() {
auto frame = GetCurrentFrame<ExtractorVisFrame>(); auto frame = GetCurrentFrame<ExtractorVisFrame>();
{ // add raw point cloud DrawPointCloud<Point>(frame.cloud, WHITE, "raw_point_cloud");
std::string id = "raw point cloud"; DrawPointCloud<TPoint>(frame.feature_pts.less_flat_surf_pts, CYAN,
DrawPointCloud(*frame.cloud, WHITE, id, viewer_.get()); "less_flat_surf_pts");
rendered_cloud_ids_.push_back(id); DrawPointCloud<TPoint>(frame.feature_pts.flat_surf_pts, BLUE,
} "flat_surf_pts");
{ // add less_flat_surf_pts DrawPointCloud<TPoint>(frame.feature_pts.less_sharp_corner_pts, PURPLE,
std::string id = "less_flat_surf_pts"; "less_sharp_corner_pts");
DrawPointCloud(*frame.feature_pts.less_flat_surf_pts, CYAN, id, DrawPointCloud<TPoint>(frame.feature_pts.sharp_corner_pts, RED,
viewer_.get(), 5); "sharp_corner_pts");
rendered_cloud_ids_.push_back(id);
}
{ // add flat_surf_pts
std::string id = "flat_surf_pts";
DrawPointCloud(*frame.feature_pts.flat_surf_pts, BLUE, id, viewer_.get(),
7);
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, PURPLE, id,
viewer_.get(), 5);
rendered_cloud_ids_.push_back(id);
}
{ // add sharp_corner_pts
std::string id = "sharp_corner_pts";
DrawPointCloud(*frame.feature_pts.sharp_corner_pts, RED, id, viewer_.get(),
7);
rendered_cloud_ids_.push_back(id);
}
}; };
} // namespace oh_my_loam } // namespace oh_my_loam

56
src/visualizer/odometry_visualizer.cc
View File

@ -4,45 +4,31 @@ namespace oh_my_loam {
void OdometryVisualizer::Draw() { void OdometryVisualizer::Draw() {
auto frame = GetCurrentFrame<OdometryVisFrame>(); auto frame = GetCurrentFrame<OdometryVisFrame>();
TPointCloud src_corn_pts, tgt_corn_pts; TPointCloudPtr src_corn_pts(new TPointCloud);
src_corn_pts.resize(frame.pl_pairs.size()); TPointCloudPtr tgt_corn_pts(new TPointCloud);
tgt_corn_pts.resize(frame.pl_pairs.size() * 2); src_corn_pts->resize(frame.pl_pairs.size());
tgt_corn_pts->resize(frame.pl_pairs.size() * 2);
for (size_t i = 0; i < frame.pl_pairs.size(); ++i) { for (size_t i = 0; i < frame.pl_pairs.size(); ++i) {
const auto& pair = frame.pl_pairs[i]; const auto& pair = frame.pl_pairs[i];
TransformToStart(frame.pose_curr2last, pair.pt, &src_corn_pts[i]); TransformToStart(frame.pose_curr2last, pair.pt, &src_corn_pts->points[i]);
tgt_corn_pts[2 * i] = pair.line.pt1; tgt_corn_pts->points[2 * i] = pair.line.pt1;
tgt_corn_pts[2 * i + 1] = pair.line.pt2; tgt_corn_pts->points[2 * i + 1] = pair.line.pt2;
} }
TPointCloud src_surf_pts, tgt_surf_pts; TPointCloudPtr src_surf_pts(new TPointCloud);
src_surf_pts.resize(frame.pp_pairs.size()); TPointCloudPtr tgt_surf_pts(new TPointCloud);
tgt_surf_pts.resize(frame.pp_pairs.size() * 3); src_surf_pts->resize(frame.pp_pairs.size());
tgt_surf_pts->resize(frame.pp_pairs.size() * 3);
for (size_t i = 0; i < frame.pp_pairs.size(); ++i) { for (size_t i = 0; i < frame.pp_pairs.size(); ++i) {
const auto& pair = frame.pp_pairs[i]; const auto& pair = frame.pp_pairs[i];
TransformToStart(frame.pose_curr2last, pair.pt, &src_surf_pts[i]); TransformToStart(frame.pose_curr2last, pair.pt, &src_surf_pts->points[i]);
tgt_surf_pts[3 * i] = pair.plane.pt1; tgt_surf_pts->points[3 * i] = pair.plane.pt1;
tgt_surf_pts[3 * i + 1] = pair.plane.pt2; tgt_surf_pts->points[3 * i + 1] = pair.plane.pt2;
tgt_surf_pts[3 * i + 2] = pair.plane.pt3; tgt_surf_pts->points[3 * i + 2] = pair.plane.pt3;
}
{ // add src_corn_pts
std::string id = "src_corn_pts";
DrawPointCloud(src_corn_pts, CYAN, id, viewer_.get(), 7);
rendered_cloud_ids_.push_back(id);
}
{ // add tgt_corn_pts
std::string id = "tgt_corn_pts";
DrawPointCloud(tgt_corn_pts, BLUE, id, viewer_.get(), 4);
rendered_cloud_ids_.push_back(id);
}
{ // add src_surf_pts
std::string id = "src_surf_pts";
DrawPointCloud(src_surf_pts, PURPLE, id, viewer_.get(), 7);
rendered_cloud_ids_.push_back(id);
}
{ // add tgt_surf_pts
std::string id = "tgt_surf_pts";
DrawPointCloud(tgt_surf_pts, RED, id, viewer_.get(), 4);
rendered_cloud_ids_.push_back(id);
} }
DrawPointCloud<TPoint>(src_corn_pts, CYAN, "src_corn_pts", 7);
DrawPointCloud<TPoint>(tgt_corn_pts, BLUE, "tgt_corn_pts", 4);
DrawPointCloud<TPoint>(src_surf_pts, PURPLE, "src_surf_pts", 7);
DrawPointCloud<TPoint>(tgt_surf_pts, RED, "tgt_surf_pts", 4);
std::vector<Pose3D> poses_n; std::vector<Pose3D> poses_n;
poses_n.reserve((poses_.size())); poses_n.reserve((poses_.size()));
Pose3D pose_inv = frame.pose_curr2world.Inv(); Pose3D pose_inv = frame.pose_curr2world.Inv();
@ -53,10 +39,10 @@ void OdometryVisualizer::Draw() {
Eigen::Vector3f p1 = poses_n[i].p().cast<float>(); Eigen::Vector3f p1 = poses_n[i].p().cast<float>();
if (i < poses_n.size() - 1) { if (i < poses_n.size() - 1) {
Eigen::Vector3f p2 = poses_n[i + 1].p().cast<float>(); Eigen::Vector3f p2 = poses_n[i + 1].p().cast<float>();
DrawLine(Point(p1.x(), p1.y(), p1.z()), Point(p2.x(), p2.y(), p2.z()), AddLine(Point(p1.x(), p1.y(), p1.z()), Point(p2.x(), p2.y(), p2.z()),
WHITE, "line" + std::to_string(i), viewer_.get()); WHITE, "line" + std::to_string(i), viewer_.get());
} else { } else {
DrawLine(Point(p1.x(), p1.y(), p1.z()), Point(0, 0, 0), WHITE, AddLine(Point(p1.x(), p1.y(), p1.z()), Point(0, 0, 0), WHITE,
"line" + std::to_string(i), viewer_.get()); "line" + std::to_string(i), viewer_.get());
} }
} }

32
src/visualizer/utils.h
View File

@ -7,29 +7,29 @@ using PCLVisualizer = pcl::visualization::PCLVisualizer;
#define PCLColorHandlerGenericField \ #define PCLColorHandlerGenericField \
pcl::visualization::PointCloudColorHandlerGenericField pcl::visualization::PointCloudColorHandlerGenericField
template <typename PointT> template <typename PointType>
void DrawPointCloud(const pcl::PointCloud<PointT>& cloud, const Color& color, void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr& cloud,
const std::string& id, PCLVisualizer* const viewer, const Color& color, const std::string& id,
int pt_size = 3) { PCLVisualizer* const viewer, int point_size = 3) {
PCLColorHandlerCustom<PointT> color_handler(cloud.makeShared(), color.r, PCLColorHandlerCustom<PointType> color_handler(cloud, color.r, color.g,
color.g, color.b); color.b);
viewer->addPointCloud<PointT>(cloud.makeShared(), color_handler, id); viewer->addPointCloud<PointType>(cloud, color_handler, id);
viewer->setPointCloudRenderingProperties( viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id); pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id);
} }
template <typename PointT> template <typename PointType>
void DrawPointCloud(const pcl::PointCloud<PointT>& cloud, void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr& cloud,
const std::string& field, const std::string& id, const std::string& field, const std::string& id,
PCLVisualizer* const viewer, int pt_size = 3) { PCLVisualizer* const viewer, int point_size = 3) {
PCLColorHandlerGenericField<PointT> color_handler(cloud.makeShared(), field); PCLColorHandlerGenericField<PointType> color_handler(cloud, field);
viewer->addPointCloud<PointT>(cloud.makeShared(), color_handler, id); viewer->addPointCloud<PointType>(cloud, color_handler, id);
viewer->setPointCloudRenderingProperties( viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id); pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id);
} }
template <typename PointT> template <typename PointType>
void DrawLine(const PointT& pt1, const PointT& pt2, const Color& color, void AddLine(const PointType& pt1, const PointType& pt2, const Color& color,
const std::string& id, PCLVisualizer* const viewer) { const std::string& id, PCLVisualizer* const viewer) {
viewer->addLine(pt1, pt2, color.r, color.g, color.b, id); viewer->addLine(pt1, pt2, color.r, color.g, color.b, id);
} }