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_;
};
} // oh_my_loam
} // namespace oh_my_loam

31
common/types.h
View File

@ -4,7 +4,12 @@
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/point_cloud_handlers.h>
#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 {
@ -47,11 +52,11 @@ using PCLVisualizer = pcl::visualization::PCLVisualizer;
#define PCLColorHandlerGenericField \
pcl::visualization::PointCloudColorHandlerGenericField
struct EIGEN_ALIGN16 PointXYZTCT {
struct PointXYZTCT {
PCL_ADD_POINT4D;
float time = 0.0f;
float curvature = std::nanf("");
// PointType label = PointType::NORNAL;
int8_t type = 0; // -2, -1, 0, 1, or 2
PointXYZTCT() {
x = y = z = 0.0f;
@ -59,8 +64,8 @@ struct EIGEN_ALIGN16 PointXYZTCT {
}
PointXYZTCT(float x, float y, float z, float time = 0.0f,
float curvature = NAN)
: x(x), y(y), z(z), time(time), curvature(curvature) {
float curvature = NAN, int8_t type = 0)
: x(x), y(y), z(z), time(time), curvature(curvature), type(type) {
data[3] = 1.0f;
}
@ -77,19 +82,25 @@ struct EIGEN_ALIGN16 PointXYZTCT {
z = p.z;
time = p.time;
curvature = p.curvature;
// type = p.type;
type = p.type;
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
POINT_CLOUD_REGISTER_POINT_STRUCT(
oh_my_loam::PointXYZTCT,
(float, x, x)(float, y, y)(float, z, z)
(float, time, time)(float, curvature, curvatur)
(float, x, x)
(float, y, y)
(float, z, z)
(float, time, time)
(float, curvature, curvatur)
(int8_t, type, type)
)
// 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);
}
} // oh_my_loam
} // namespace oh_my_loam

2
main.cc
View File

@ -1,7 +1,9 @@
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <functional>
#include "oh_my_loam.h"
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)
std::cout << "OMEGA: "
<< 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 << " "
<< " 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 <cmath>
#include "filter.h"
namespace oh_my_loam {
const double kPointMinDist = 0.1;
const int kScanSegNum = 6;
const double kTwoPi = kTwoPi;
const double kTwoPi = 2 * M_PI;
const int kMinPtsNum = 100;
void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
FeaturePoints* const feature) const {
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());
RemoveClosedPoints<Point>(*cloud, cloud.get(), kPointMinDist);
std::cout << "AFTER REMOVE, num = " << cloud->size() << std::endl;
@ -30,24 +31,24 @@ void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
std::cout << std::endl;
for (const auto& scan : scans) {
feature->feature_pts += scan;
// for (const auto& pt : scan.points) {
// switch (pt.type) {
// case PointType::FLAT:
// feature->flat_surf_pts->points.emplace_back(pt);
// break;
// case PointType::LESS_FLAT:
// feature->less_flat_surf_pts->points.emplace_back(pt);
// break;
// case PointType::LESS_SHARP:
// feature->less_sharp_corner_pts->points.emplace_back(pt);
// break;
// case PointType::SHARP:
// feature->sharp_corner_pts->points.emplace_back(pt);
// break;
// default:
// break;
// }
// }
for (const auto& pt : scan.points) {
switch (pt.Type()) {
case PointType::FLAT:
feature->flat_surf_pts.points.emplace_back(pt);
break;
case PointType::LESS_FLAT:
feature->less_flat_surf_pts.points.emplace_back(pt);
break;
case PointType::LESS_SHARP:
feature->less_sharp_corner_pts.points.emplace_back(pt);
break;
case PointType::SHARP:
feature->sharp_corner_pts.points.emplace_back(pt);
break;
default:
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;
};
} // oh_my_loam
} // namespace oh_my_loam

7
src/oh_my_loam.cc
View File

@ -3,7 +3,7 @@
namespace oh_my_loam {
bool OhMyLoam::Init() {
is_vis_ = false;
is_vis_ = true;
feature_extractor_.reset(new FeatureExtractorVLP16);
if (is_vis_) {
visualizer_.reset(new FeaturePointsVisualizer);
@ -12,7 +12,6 @@ bool OhMyLoam::Init() {
}
void OhMyLoam::Run(const PointCloud& cloud, double timestamp) {
pcl::PointCloud<pcl::PointXYZ> cl;
std::shared_ptr<FeaturePoints> feature_pts(new FeaturePoints);
feature_extractor_->Extract(cloud, feature_pts.get());
if (is_vis_) Visualize(cloud, feature_pts, timestamp);
@ -21,11 +20,11 @@ void OhMyLoam::Run(const PointCloud& cloud, double timestamp) {
void OhMyLoam::Visualize(
const PointCloud& cloud,
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->cloud = cloud.makeShared();
frame->feature_pts = feature_pts;
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;
};
} // 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/point_cloud_handlers.h>
#include <atomic>
#include <deque>
#include <mutex>
#include <string>
#include <thread>
#include "types.h"
namespace oh_my_loam {
@ -160,4 +162,4 @@ class Visualizer {
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
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());
rendered_cloud_ids_.push_back(id);
}
// { // add flat_surf_pts
// std::string id = "flat_surf_pts";
// DrawPointCloud(*frame.feature_ptsflat_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_ptsless_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_ptsless_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);
// }
{ // 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);
}
}
};
} // oh_my_loam
} // namespace oh_my_loam