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

visualization OK

main
feixyz10 2020-10-13 01:07:59 +08:00 committed by feixyz
parent 397abb2bf3
commit 8c4b5976d3
12 changed files with 84 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
common/filter.h
View File

@ -41,4 +41,4 @@ void RemoveClosedPoints(const pcl::PointCloud<PointT>& cloud_in,
}); });
} }
} // oh_my_loam } // namespace oh_my_loam

2
common/tic_toc.h
View File

@ -22,4 +22,4 @@ class TicToc {
std::chrono::time_point<std::chrono::system_clock> start_, end_; std::chrono::time_point<std::chrono::system_clock> start_, end_;
}; };
} // oh_my_loam } // namespace oh_my_loam

31
common/types.h
View File

@ -4,7 +4,12 @@
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h> #include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/point_cloud_handlers.h> #include <pcl/visualization/point_cloud_handlers.h>
#include <cmath> #include <cmath>
// This hpp file should be included if user-defined point type is added, see
// "How are the point types exposed?" section in
// https://pointclouds.org/documentation/tutorials/adding_custom_ptype.html
#include <pcl/visualization/impl/point_cloud_geometry_handlers.hpp>
namespace oh_my_loam { namespace oh_my_loam {
@ -47,11 +52,11 @@ using PCLVisualizer = pcl::visualization::PCLVisualizer;
#define PCLColorHandlerGenericField \ #define PCLColorHandlerGenericField \
pcl::visualization::PointCloudColorHandlerGenericField pcl::visualization::PointCloudColorHandlerGenericField
struct EIGEN_ALIGN16 PointXYZTCT { struct PointXYZTCT {
PCL_ADD_POINT4D; PCL_ADD_POINT4D;
float time = 0.0f; float time = 0.0f;
float curvature = std::nanf(""); float curvature = std::nanf("");
// PointType label = PointType::NORNAL; int8_t type = 0; // -2, -1, 0, 1, or 2
PointXYZTCT() { PointXYZTCT() {
x = y = z = 0.0f; x = y = z = 0.0f;
@ -59,8 +64,8 @@ struct EIGEN_ALIGN16 PointXYZTCT {
} }
PointXYZTCT(float x, float y, float z, float time = 0.0f, PointXYZTCT(float x, float y, float z, float time = 0.0f,
float curvature = NAN) float curvature = NAN, int8_t type = 0)
: x(x), y(y), z(z), time(time), curvature(curvature) { : x(x), y(y), z(z), time(time), curvature(curvature), type(type) {
data[3] = 1.0f; data[3] = 1.0f;
} }
@ -77,19 +82,25 @@ struct EIGEN_ALIGN16 PointXYZTCT {
z = p.z; z = p.z;
time = p.time; time = p.time;
curvature = p.curvature; curvature = p.curvature;
// type = p.type; type = p.type;
data[3] = 1.0f; data[3] = 1.0f;
} }
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PointType Type() const { return static_cast<PointType>(type); }
};
} // oh_my_loam EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
} // namespace oh_my_loam
// clang-format off // clang-format off
POINT_CLOUD_REGISTER_POINT_STRUCT( POINT_CLOUD_REGISTER_POINT_STRUCT(
oh_my_loam::PointXYZTCT, oh_my_loam::PointXYZTCT,
(float, x, x)(float, y, y)(float, z, z) (float, x, x)
(float, time, time)(float, curvature, curvatur) (float, y, y)
(float, z, z)
(float, time, time)
(float, curvature, curvatur)
(int8_t, type, type)
) )
// clang-format on // clang-format on

2
common/utils.h
View File

@ -46,4 +46,4 @@ void DrawPointCloud(const pcl::PointCloud<PointT>& cloud,
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id); pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id);
} }
} // oh_my_loam } // namespace oh_my_loam

2
main.cc
View File

@ -1,7 +1,9 @@
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
#include <functional> #include <functional>
#include "oh_my_loam.h" #include "oh_my_loam.h"
void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg, void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,

6
src/feature_extractor_VLP16.h
View File

@ -17,11 +17,11 @@ class FeatureExtractorVLP16 : public FeaturePointsExtractor {
if (i++ < 10) if (i++ < 10)
std::cout << "OMEGA: " std::cout << "OMEGA: "
<< std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0 << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0
<< " id = " << static_cast<int>(std::round(omega) + 0.01) << " id = " << static_cast<int>(std::round(omega / 2.0) + 0.01)
<< " z = " << pt.z << " " << " z = " << pt.z << " "
<< " d = " << Distance(pt) << std::endl; << " d = " << Distance(pt) << std::endl;
return static_cast<int>(std::round(omega) + 0.01); return static_cast<int>(std::round(omega / 2.0) + 1.e-5);
} }
}; };
} // oh_my_loam } // namespace oh_my_loam

43
src/feature_extractor_base.cc
View File

@ -1,19 +1,20 @@
#include "feature_extractor_base.h" #include "feature_extractor_base.h"
#include <cmath> #include <cmath>
#include "filter.h" #include "filter.h"
namespace oh_my_loam { namespace oh_my_loam {
const double kPointMinDist = 0.1; const double kPointMinDist = 0.1;
const int kScanSegNum = 6; const int kScanSegNum = 6;
const double kTwoPi = kTwoPi; const double kTwoPi = 2 * M_PI;
const int kMinPtsNum = 100; const int kMinPtsNum = 100;
void FeaturePointsExtractor::Extract(const PointCloud& cloud_in, void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
FeaturePoints* const feature) const { FeaturePoints* const feature) const {
PointCloudPtr cloud(new PointCloud); PointCloudPtr cloud(new PointCloud);
std::cout << "BEFORE REMOVE, num = " << cloud->size() << std::endl; std::cout << "BEFORE REMOVE, num = " << cloud_in.size() << std::endl;
RemoveNaNPoint<Point>(cloud_in, cloud.get()); RemoveNaNPoint<Point>(cloud_in, cloud.get());
RemoveClosedPoints<Point>(*cloud, cloud.get(), kPointMinDist); RemoveClosedPoints<Point>(*cloud, cloud.get(), kPointMinDist);
std::cout << "AFTER REMOVE, num = " << cloud->size() << std::endl; std::cout << "AFTER REMOVE, num = " << cloud->size() << std::endl;
@ -30,24 +31,24 @@ void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
std::cout << std::endl; std::cout << std::endl;
for (const auto& scan : scans) { for (const auto& scan : scans) {
feature->feature_pts += scan; feature->feature_pts += scan;
// for (const auto& pt : scan.points) { for (const auto& pt : scan.points) {
// switch (pt.type) { switch (pt.Type()) {
// case PointType::FLAT: case PointType::FLAT:
// feature->flat_surf_pts->points.emplace_back(pt); feature->flat_surf_pts.points.emplace_back(pt);
// break; break;
// case PointType::LESS_FLAT: case PointType::LESS_FLAT:
// feature->less_flat_surf_pts->points.emplace_back(pt); feature->less_flat_surf_pts.points.emplace_back(pt);
// break; break;
// case PointType::LESS_SHARP: case PointType::LESS_SHARP:
// feature->less_sharp_corner_pts->points.emplace_back(pt); feature->less_sharp_corner_pts.points.emplace_back(pt);
// break; break;
// case PointType::SHARP: case PointType::SHARP:
// feature->sharp_corner_pts->points.emplace_back(pt); feature->sharp_corner_pts.points.emplace_back(pt);
// break; break;
// default: default:
// break; break;
// } }
// } }
} }
} }
@ -98,4 +99,4 @@ void FeaturePointsExtractor::SplitScan(
// } // }
// } // }
} // oh_my_loam } // namespace oh_my_loam

2
src/feature_extractor_base.h
View File

@ -36,4 +36,4 @@ class FeaturePointsExtractor {
int num_scans_ = 0; int num_scans_ = 0;
}; };
} // oh_my_loam } // namespace oh_my_loam

7
src/oh_my_loam.cc
View File

@ -3,7 +3,7 @@
namespace oh_my_loam { namespace oh_my_loam {
bool OhMyLoam::Init() { bool OhMyLoam::Init() {
is_vis_ = false; is_vis_ = true;
feature_extractor_.reset(new FeatureExtractorVLP16); feature_extractor_.reset(new FeatureExtractorVLP16);
if (is_vis_) { if (is_vis_) {
visualizer_.reset(new FeaturePointsVisualizer); visualizer_.reset(new FeaturePointsVisualizer);
@ -12,7 +12,6 @@ bool OhMyLoam::Init() {
} }
void OhMyLoam::Run(const PointCloud& cloud, double timestamp) { void OhMyLoam::Run(const PointCloud& cloud, double timestamp) {
pcl::PointCloud<pcl::PointXYZ> cl;
std::shared_ptr<FeaturePoints> feature_pts(new FeaturePoints); std::shared_ptr<FeaturePoints> feature_pts(new FeaturePoints);
feature_extractor_->Extract(cloud, feature_pts.get()); feature_extractor_->Extract(cloud, feature_pts.get());
if (is_vis_) Visualize(cloud, feature_pts, timestamp); if (is_vis_) Visualize(cloud, feature_pts, timestamp);
@ -21,11 +20,11 @@ void OhMyLoam::Run(const PointCloud& cloud, double timestamp) {
void OhMyLoam::Visualize( void OhMyLoam::Visualize(
const PointCloud& cloud, const PointCloud& cloud,
const std::shared_ptr<const FeaturePoints>& feature_pts, double timestamp) { const std::shared_ptr<const FeaturePoints>& feature_pts, double timestamp) {
std::shared_ptr<FeaturePointsVisFrame> frame; std::shared_ptr<FeaturePointsVisFrame> frame(new FeaturePointsVisFrame);
frame->timestamp = timestamp; frame->timestamp = timestamp;
frame->cloud = cloud.makeShared(); frame->cloud = cloud.makeShared();
frame->feature_pts = feature_pts; frame->feature_pts = feature_pts;
visualizer_->Render(frame); visualizer_->Render(frame);
} }
} // oh_my_loam } // namespace oh_my_loam

2
src/oh_my_loam.h
View File

@ -26,4 +26,4 @@ class OhMyLoam {
std::unique_ptr<FeaturePointsVisualizer> visualizer_ = nullptr; std::unique_ptr<FeaturePointsVisualizer> visualizer_ = nullptr;
}; };
} // oh_my_loam } // namespace oh_my_loam

4
src/visualizer_base.h
View File

@ -2,11 +2,13 @@
#include <pcl/visualization/pcl_visualizer.h> #include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/point_cloud_handlers.h> #include <pcl/visualization/point_cloud_handlers.h>
#include <atomic> #include <atomic>
#include <deque> #include <deque>
#include <mutex> #include <mutex>
#include <string> #include <string>
#include <thread> #include <thread>
#include "types.h" #include "types.h"
namespace oh_my_loam { namespace oh_my_loam {
@ -160,4 +162,4 @@ class Visualizer {
std::unique_ptr<PCLVisualizer> viewer_ = nullptr; std::unique_ptr<PCLVisualizer> viewer_ = nullptr;
}; };
} // oh_my_loam } // namespace oh_my_loam

51
src/visualizer_feature_points.h
View File

@ -26,35 +26,34 @@ class FeaturePointsVisualizer : public Visualizer<FeaturePointsVisFrame> {
} }
{ // add all feature_pts { // add all feature_pts
std::string id = "feature_pts"; std::string id = "feature_pts";
DrawPointCloud<IPoint>(frame.feature_pts->feature_pts, GRAY, id, DrawPointCloud<IPoint>(frame.feature_pts->feature_pts, "time", id,
viewer_.get()); viewer_.get());
rendered_cloud_ids_.push_back(id); rendered_cloud_ids_.push_back(id);
} }
// { // add flat_surf_pts { // add flat_surf_pts
// std::string id = "flat_surf_pts"; std::string id = "flat_surf_pts";
// DrawPointCloud(*frame.feature_ptsflat_surf_pts, CYAN, id, DrawPointCloud(frame.feature_pts->flat_surf_pts, CYAN, id, viewer_.get());
// viewer_.get()); rendered_cloud_ids_.push_back(id);
// rendered_cloud_ids_.push_back(id); }
// } { // add less_flat_surf_pts
// { // add less_flat_surf_pts std::string id = "less_flat_surf_pts";
// std::string id = "less_flat_surf_pts"; DrawPointCloud(frame.feature_pts->less_flat_surf_pts, GREEN, id,
// DrawPointCloud(*frame.feature_ptsless_flat_surf_pts, GREEN, id, viewer_.get());
// viewer_.get()); rendered_cloud_ids_.push_back(id);
// rendered_cloud_ids_.push_back(id); }
// } { // add less_sharp_corner_pts
// { // add less_sharp_corner_pts std::string id = "less_sharp_corner_pts";
// std::string id = "less_sharp_corner_pts"; DrawPointCloud(frame.feature_pts->less_sharp_corner_pts, ORANGE, id,
// DrawPointCloud(*frame.feature_ptsless_sharp_corner_pts, ORANGE, id, viewer_.get());
// viewer_.get()); rendered_cloud_ids_.push_back(id);
// rendered_cloud_ids_.push_back(id); }
// } { // add sharp_corner_pts
// { // add sharp_corner_pts std::string id = "sharp_corner_pts";
// std::string id = "sharp_corner_pts"; DrawPointCloud(frame.feature_pts->sharp_corner_pts, ORANGE, id,
// DrawPointCloud(*frame.feature_pts.sharp_corner_pts, ORANGE, id, viewer_.get());
// viewer_.get()); rendered_cloud_ids_.push_back(id);
// rendered_cloud_ids_.push_back(id); }
// }
} }
}; };
} // oh_my_loam } // namespace oh_my_loam