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

add ros

main
feixyz10 2020-10-12 21:09:32 +08:00 committed by feixyz
parent f94bc39ec6
commit 397abb2bf3
18 changed files with 625 additions and 192 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
CMakeLists.txt
View File

@ -1,23 +1,47 @@
cmake_minimum_required(VERSION 2.8.3)
project(oh_loam)
project(oh_my_loam)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++17")
set(CMAKE_CXX_FLAGS "-std=c++17 -lpthread")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
find_package(PCL QUIET)
if (NOT PCL_FOUND)
message(FATAL_ERROR "PCL not found.")
endif()
find_package(Ceres REQUIRED)
find_package(PCL REQUIRED)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
nav_msgs
sensor_msgs
roscpp
rospy
rosbag
std_msgs
image_transport
cv_bridge
tf
)
include_directories(
${PROJECT_SOURCE_DIR}/common
${PROJECT_SOURCE_DIR}/src
${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${CERES_INCLUDE_DIRS}
src
common
)
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
add_library(${PROJECT_NAME} SHARED
${PROJECT_SOURCE_DIR}/src/base_feature_extractor.cc
catkin_package(
CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
DEPENDS EIGEN3 PCL
INCLUDE_DIRS src common
)
add_subdirectory(common)
add_subdirectory(src)
add_executable(oh_my_loam main.cc)
target_link_libraries(oh_my_loam
${catkin_LIBRARIES}
${PCL_LIBRARIES}
${CERES_LIBRARIES}
OhMyLoam
)

0
common/CMakeLists.txt Normal file
View File

9
common/filter.h
View File

@ -2,7 +2,7 @@
#include "utils.h"
namespace oh_loam {
namespace oh_my_loam {
template <typename PointT>
void RemovePointsIf(const pcl::PointCloud<PointT>& cloud_in,
@ -10,14 +10,15 @@ void RemovePointsIf(const pcl::PointCloud<PointT>& cloud_in,
std::function<bool(const PointT&)> cond) {
if (&cloud_in != cloud_out) {
cloud_out->header = cloud_in.header;
cloud_out->points.resize(cloud_in.points.size());
cloud_out->points.resize(cloud_in.size());
}
size_t j = 0;
for (size_t i = 0; i < cloud_in.points.size(); ++i) {
for (size_t i = 0; i < cloud_in.size(); ++i) {
const auto pt = cloud_in.points[i];
if (cond(pt)) continue;
cloud_out->points[j++] = pt;
}
cloud_out->points.resize(j);
cloud_out->height = 1;
cloud_out->width = static_cast<uint32_t>(j);
@ -40,4 +41,4 @@ void RemoveClosedPoints(const pcl::PointCloud<PointT>& cloud_in,
});
}
} // oh_loam
} // oh_my_loam

4
common/tic_toc.h
View File

@ -4,7 +4,7 @@
#include <cstdlib>
#include <ctime>
namespace oh_loam {
namespace oh_my_loam {
class TicToc {
public:
@ -22,4 +22,4 @@ class TicToc {
std::chrono::time_point<std::chrono::system_clock> start_, end_;
};
} // oh_loam
} // oh_my_loam

53
common/types.h
View File

@ -2,9 +2,11 @@
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/point_cloud_handlers.h>
#include <cmath>
namespace oh_loam {
namespace oh_my_loam {
using Point = pcl::PointXYZ;
using PointCloud = pcl::PointCloud<Point>;
@ -19,11 +21,37 @@ enum class PointType {
SHARP = 2,
};
struct Color {
uint8_t r, g, b;
Color(uint8_t r, uint8_t g, uint8_t b) : r(r), g(g), b(b) {}
};
#define BLACK Color(0, 0, 0)
#define WHITE Color(255, 255, 255)
#define RED Color(255, 0, 0)
#define GREEN Color(0, 255, 0)
#define BLUE Color(0, 0, 255)
#define GRAY Color(50, 50, 50)
#define CYAN Color(0, 255, 255)
#define PURPLE Color(160, 32, 240)
#define ORANGE Color(255, 97, 0)
struct PointXYZTCT;
using IPoint = PointXYZTCT;
using IPointCloud = pcl::PointCloud<IPoint>;
using IPointCloudPtr = IPointCloud::Ptr;
using IPointCloudConstPtr = IPointCloud::ConstPtr;
using PCLVisualizer = pcl::visualization::PCLVisualizer;
#define PCLColorHandlerCustom pcl::visualization::PointCloudColorHandlerCustom
#define PCLColorHandlerGenericField \
pcl::visualization::PointCloudColorHandlerGenericField
struct EIGEN_ALIGN16 PointXYZTCT {
PCL_ADD_POINT4D;
float time = 0.0f;
float curvature = NAN;
PointType type = PointType::NORNAL;
float curvature = std::nanf("");
// PointType label = PointType::NORNAL;
PointXYZTCT() {
x = y = z = 0.0f;
@ -31,8 +59,8 @@ struct EIGEN_ALIGN16 PointXYZTCT {
}
PointXYZTCT(float x, float y, float z, float time = 0.0f,
float curvature = NAN, PointType type = PointType::NORNAL)
: x(x), y(y), z(z), time(time), curvature(curvature), type(type) {
float curvature = NAN)
: x(x), y(y), z(z), time(time), curvature(curvature) {
data[3] = 1.0f;
}
@ -49,16 +77,19 @@ 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
};
using IPoint = PointXYZTCT;
using IPointCloud = pcl::PointCloud<IPoint>;
using IPointCloudPtr = IPointCloud::Ptr;
using IPointCloudConstPtr = IPointCloud::ConstPtr;
} // oh_my_loam
} // oh_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)
)
// clang-format on

40
common/utils.h
View File

@ -2,18 +2,18 @@
#include "types.h"
namespace oh_loam {
template <typename PointT>
inline double Distance(const PointT& pt) {
return std::hypot(pt.x, pt.y, pt.z);
}
namespace oh_my_loam {
template <typename PointT>
inline double DistanceSqure(const PointT& pt) {
return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z;
}
template <typename PointT>
inline double Distance(const PointT& pt) {
return std::sqrt(pt.x * pt.x + pt.y * pt.y + pt.z * pt.z);
}
template <typename PointT>
inline double IsFinite(const PointT& pt) {
return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z);
@ -25,13 +25,25 @@ inline double NormalizeAngle(double ang) {
return ang - two_pi * std::floor((ang + M_PI) / two_pi);
}
std::pair<double, double> GetYawRange(const PointCloud& cloud) {
const auto& pts = cloud.points;
int pt_num = pts.size();
double yaw_start = -atan2(pts[0].y, pts[0].x);
double yaw_end = -atan2(pts[pt_num - 1].y, pts[pt_num - 1].x) + 2 * M_PI;
double yaw_diff = NormalizeAngle(yaw_end - yaw_start);
return {yaw_start, yaw_start + yaw_diff + 2 * M_PI};
template <typename PointT>
void DrawPointCloud(const pcl::PointCloud<PointT>& cloud, const Color& color,
const std::string& id, PCLVisualizer* const viewer,
int pt_size = 3) {
PCLColorHandlerCustom<PointT> color_handler(cloud.makeShared(), color.r,
color.g, color.b);
viewer->addPointCloud<PointT>(cloud.makeShared(), color_handler, id);
viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id);
}
} // oh_loam
template <typename PointT>
void DrawPointCloud(const pcl::PointCloud<PointT>& cloud,
const std::string& field, const std::string& id,
PCLVisualizer* const viewer, int pt_size = 3) {
PCLColorHandlerGenericField<PointT> color_handler(cloud.makeShared(), field);
viewer->addPointCloud<PointT>(cloud.makeShared(), color_handler, id);
viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id);
}
} // oh_my_loam

31
main.cc Normal file
View File

@ -0,0 +1,31 @@
#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,
oh_my_loam::OhMyLoam* const slam);
int main(int argc, char* argv[]) {
oh_my_loam::OhMyLoam slam;
slam.Init();
ros::init(argc, argv, "oh_my_loam");
ros::NodeHandle nh;
ros::Subscriber sub_point_cloud = nh.subscribe<sensor_msgs::PointCloud2>(
"/velodyne_points", 100,
std::bind(PointCloudHandler, std::placeholders::_1, &slam));
ros::spin();
return 0;
}
void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
oh_my_loam::OhMyLoam* const slam) {
oh_my_loam::PointCloud cloud;
pcl::fromROSMsg(*msg, cloud);
ROS_INFO_STREAM("Point num = " << cloud.size()
<< "ts = " << msg->header.stamp.toSec());
slam->Run(cloud, 0.0);
}

39
package.xml Normal file
View File

@ -0,0 +1,39 @@
<?xml version="1.0"?>
<package>
<name>oh_my_loam</name>
<version>0.0.0</version>
<description>
Reimplementation of LOAM.
</description>
<maintainer email="fei-china@139.com">qintong</maintainer>
<license>BSD</license>
<author email="zhangji@cmu.edu">Ji Zhang</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>rosbag</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>image_transport</build_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>rosbag</run_depend>
<run_depend>tf</run_depend>
<run_depend>image_transport</run_depend>
<export>
</export>
</package>

7
src/CMakeLists.txt Normal file
View File

@ -0,0 +1,7 @@
include_directories(
${PROJECT_SOURCE_DIR}/common
)
aux_source_directory(. SRC_FILES)
add_library(OhMyLoam SHARED ${SRC_FILES})

95
src/base_feature_extractor.cc
View File

@ -1,95 +0,0 @@
#include "base_feature_extractor.h"
#include <cmath>
#include "filter.h"
namespace oh_loam {
const double kPointMinDist = 0.1;
const int kScanSegNum = 6;
bool FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
FeaturePoints* const feature) const {
PointCloudPtr cloud(new PointCloud);
RemoveNaNPoint<Point>(cloud_in, cloud.get());
RemoveClosedPoints<Point>(*cloud, cloud.get(), kPointMinDist);
std::vector<IPointCloud> scans;
SplitScan(*cloud, &scans);
for (auto& scan : scans) {
ComputePointCurvature(&scan);
AssignPointType(&scan);
}
for (const auto& scan : scans) {
*(feature->laser_cloud) += scan;
for (const auto& pt : scan.points) {
switch (pt.type) {
case PointType::FLAT:
feature->flat_surf_points->points.emplace_back(pt);
break;
case PointType::LESS_FLAT:
feature->less_flat_surf_points->points.emplace_back(pt);
break;
case PointType::LESS_SHARP:
feature->less_sharp_corner_points->points.emplace_back(pt);
break;
case PointType::SHARP:
feature->sharp_corner_points->points.emplace_back(pt);
break;
default:
break;
}
}
}
}
void FeaturePointsExtractor::SplitScan(
const PointCloud& cloud, std::vector<IPointCloud>* const scans) const {
scans->resize(num_scans_);
double yaw_start, yaw_end;
std::tie(yaw_start, yaw_end) = GetYawRange(cloud);
const double yaw_range = yaw_end - yaw_start;
bool half_passed = false;
for (const auto& pt : cloud.points) {
int scan_id = GetScanID(pt);
if (scan_id >= num_scans_ || scan_id < 0) continue;
double yaw = -atan2(pt.y, pt.x);
double yaw_diff = NormalizeAngle(yaw - yaw_start);
if (yaw_diff > 0) {
if (half_passed) yaw_start += 2 * M_PI;
} else {
half_passed = true;
yaw_start += 2 * M_PI;
}
(*scans)[scan_id].points.emplace_back(pt.x, pt.y, pt.z,
yaw_diff / yaw_range);
}
}
void FeaturePointsExtractor::ComputePointCurvature(
IPointCloud* const scan) const {
auto& pts = scan->points;
for (size_t i = 5; i < pts.size() - 5; ++i) {
float diffX = pts[i - 5].x + pts[i - 4].x + pts[i - 3].x + pts[i - 2].x +
pts[i - 1].x + pts[i + 1].x + pts[i + 2].x + pts[i + 3].x +
pts[i + 4].x + pts[i + 5].x - 10 * pts[i].x;
float diffY = pts[i - 5].y + pts[i - 4].y + pts[i - 3].y + pts[i - 2].y +
pts[i - 1].y + pts[i + 1].y + pts[i + 2].y + pts[i + 3].y +
pts[i + 4].y + pts[i + 5].y - 10 * pts[i].y;
float diffZ = pts[i - 5].z + pts[i - 4].z + pts[i - 3].z + pts[i - 2].z +
pts[i - 1].z + pts[i + 1].z + pts[i + 2].z + pts[i + 3].z +
pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z;
pts[i].curvature = diffX * diffX + diffY * diffY + diffZ * diffZ;
}
}
void FeaturePointsExtractor::AssignPointType(IPointCloud* const scan) const {
int pt_num = scan->size();
int pt_num_seg = pt_num / kScanSegNum;
std::vector<bool> picked(pt_num, false);
for (int i = 0; i < kScanSegNum; ++i) {
int begin = i * pt_num_seg;
int end = i * pt_num_seg + pt_num_seg;
}
}
} // oh_loam

47
src/base_feature_extractor.h
View File

@ -1,47 +0,0 @@
#pragma once
#include "utils.h"
namespace oh_loam {
struct FeaturePoints {
IPointCloudPtr laser_cloud;
IPointCloudPtr sharp_corner_points;
IPointCloudPtr less_sharp_corner_points;
IPointCloudPtr flat_surf_points;
IPointCloudPtr less_flat_surf_points;
FeaturePoints() {
laser_cloud.reset(new IPointCloud);
sharp_corner_points.reset(new IPointCloud);
less_sharp_corner_points.reset(new IPointCloud);
flat_surf_points.reset(new IPointCloud);
less_flat_surf_points.reset(new IPointCloud);
}
};
class FeaturePointsExtractor {
public:
FeaturePointsExtractor() = default;
virtual ~FeaturePointsExtractor() = default;
FeaturePointsExtractor(const FeaturePointsExtractor&) = delete;
FeaturePointsExtractor& operator=(const FeaturePointsExtractor&) = delete;
bool Extract(const PointCloud& cloud_in, FeaturePoints* const feature) const;
int num_scans() const { return num_scans_; }
protected:
virtual int GetScanID(const Point& pt) const = 0;
void SplitScan(const PointCloud& cloud,
std::vector<IPointCloud>* const scans) const;
void ComputePointCurvature(IPointCloud* const scan) const;
void AssignPointType(IPointCloud* const scan) const;
int num_scans_ = 0;
};
} // oh_loam

19
src/feature_extractor_VLP16.h
View File

@ -1,20 +1,27 @@
#pragma once
#include "base_feature_extractor.h"
#include "feature_extractor_base.h"
#include "utils.h"
namespace oh_loam {
namespace oh_my_loam {
// for VLP-16
class FeatureExtractorVLP16 : public FeatureExtractor {
class FeatureExtractorVLP16 : public FeaturePointsExtractor {
public:
FeatureExtractorVLP16() { num_scans_ = 16; }
private:
protected:
int GetScanID(const Point& pt) const override final {
static int i = 0;
double omega = std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0;
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)
<< " z = " << pt.z << " "
<< " d = " << Distance(pt) << std::endl;
return static_cast<int>(std::round(omega) + 0.01);
}
}
};
} // oh_loam
} // oh_my_loam

101
src/feature_extractor_base.cc Normal file
View File

@ -0,0 +1,101 @@
#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 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;
RemoveNaNPoint<Point>(cloud_in, cloud.get());
RemoveClosedPoints<Point>(*cloud, cloud.get(), kPointMinDist);
std::cout << "AFTER REMOVE, num = " << cloud->size() << std::endl;
if (cloud->size() < kMinPtsNum) {
return;
}
std::vector<IPointCloud> scans;
SplitScan(*cloud, &scans);
for (auto& scan : scans) {
std::cout << scan.size() << " ";
// ComputePointCurvature(&scan);
// AssignPointType(&scan);
}
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;
// }
// }
}
}
void FeaturePointsExtractor::SplitScan(
const PointCloud& cloud, std::vector<IPointCloud>* const scans) const {
scans->resize(num_scans_);
double yaw_start = -atan2(cloud.points[0].y, cloud.points[0].x);
bool half_passed = false;
for (const auto& pt : cloud.points) {
int scan_id = GetScanID(pt);
if (scan_id >= num_scans_ || scan_id < 0) continue;
double yaw = -atan2(pt.y, pt.x);
double yaw_diff = NormalizeAngle(yaw - yaw_start);
if (yaw_diff > 0) {
if (half_passed) yaw_start += kTwoPi;
} else {
half_passed = true;
yaw_start += kTwoPi;
}
(*scans)[scan_id].points.emplace_back(pt.x, pt.y, pt.z, yaw_diff / kTwoPi);
}
}
// void FeaturePointsExtractor::ComputePointCurvature(
// IPointCloud* const scan) const {
// auto& pts = scan->points;
// for (size_t i = 5; i < pts.size() - 5; ++i) {
// float diffX = pts[i - 5].x + pts[i - 4].x + pts[i - 3].x + pts[i - 2].x +
// pts[i - 1].x + pts[i + 1].x + pts[i + 2].x + pts[i + 3].x +
// pts[i + 4].x + pts[i + 5].x - 10 * pts[i].x;
// float diffY = pts[i - 5].y + pts[i - 4].y + pts[i - 3].y + pts[i - 2].y +
// pts[i - 1].y + pts[i + 1].y + pts[i + 2].y + pts[i + 3].y +
// pts[i + 4].y + pts[i + 5].y - 10 * pts[i].y;
// float diffZ = pts[i - 5].z + pts[i - 4].z + pts[i - 3].z + pts[i - 2].z +
// pts[i - 1].z + pts[i + 1].z + pts[i + 2].z + pts[i + 3].z +
// pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z;
// pts[i].curvature = diffX * diffX + diffY * diffY + diffZ * diffZ;
// }
// }
// void FeaturePointsExtractor::AssignPointType(IPointCloud* const scan) const {
// int pt_num = scan->size();
// int pt_num_seg = pt_num / kScanSegNum;
// std::vector<bool> picked(pt_num, false);
// for (int i = 0; i < kScanSegNum; ++i) {
// int begin = i * pt_num_seg;
// int end = i * pt_num_seg + pt_num_seg;
// }
// }
} // oh_my_loam

39
src/feature_extractor_base.h Normal file
View File

@ -0,0 +1,39 @@
#pragma once
#include "utils.h"
namespace oh_my_loam {
struct FeaturePoints {
IPointCloud feature_pts; // all feature points
IPointCloud sharp_corner_pts;
IPointCloud less_sharp_corner_pts;
IPointCloud flat_surf_pts;
IPointCloud less_flat_surf_pts;
};
class FeaturePointsExtractor {
public:
FeaturePointsExtractor() = default;
virtual ~FeaturePointsExtractor() = default;
FeaturePointsExtractor(const FeaturePointsExtractor&) = delete;
FeaturePointsExtractor& operator=(const FeaturePointsExtractor&) = delete;
void Extract(const PointCloud& cloud_in, FeaturePoints* const feature) const;
int num_scans() const { return num_scans_; }
protected:
virtual int GetScanID(const Point& pt) const = 0;
void SplitScan(const PointCloud& cloud,
std::vector<IPointCloud>* const scans) const;
// void ComputePointCurvature(IPointCloud* const scan) const;
// void AssignPointType(IPointCloud* const scan) const;
int num_scans_ = 0;
};
} // oh_my_loam

31
src/oh_my_loam.cc Normal file
View File

@ -0,0 +1,31 @@
#include "oh_my_loam.h"
namespace oh_my_loam {
bool OhMyLoam::Init() {
is_vis_ = false;
feature_extractor_.reset(new FeatureExtractorVLP16);
if (is_vis_) {
visualizer_.reset(new FeaturePointsVisualizer);
}
return true;
}
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);
}
void OhMyLoam::Visualize(
const PointCloud& cloud,
const std::shared_ptr<const FeaturePoints>& feature_pts, double timestamp) {
std::shared_ptr<FeaturePointsVisFrame> frame;
frame->timestamp = timestamp;
frame->cloud = cloud.makeShared();
frame->feature_pts = feature_pts;
visualizer_->Render(frame);
}
} // oh_my_loam

29
src/oh_my_loam.h Normal file
View File

@ -0,0 +1,29 @@
#pragma once
#include "feature_extractor_VLP16.h"
#include "visualizer_feature_points.h"
namespace oh_my_loam {
class OhMyLoam {
public:
OhMyLoam() = default;
~OhMyLoam() = default;
OhMyLoam(const OhMyLoam&) = delete;
OhMyLoam& operator=(const OhMyLoam&) = delete;
bool Init();
void Run(const PointCloud& cloud, double timestamp);
private:
void Visualize(const PointCloud& cloud,
const std::shared_ptr<const FeaturePoints>& feature_pts,
double timestamp = std::nanf(""));
bool is_vis_ = false;
std::unique_ptr<FeaturePointsExtractor> feature_extractor_ = nullptr;
std::unique_ptr<FeaturePointsVisualizer> visualizer_ = nullptr;
};
} // oh_my_loam

163
src/visualizer_base.h Normal file
View File

@ -0,0 +1,163 @@
#pragma once
#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 {
struct VisFrame {
double timestamp = std::nanf("");
PointCloudPtr cloud = nullptr;
};
template <typename FrameT>
class Visualizer {
public:
Visualizer(const std::string &name, size_t max_history_size)
: name_(name), max_history_size_(max_history_size) {
// Start the thread to draw
thread_.reset(new std::thread([&]() {
viewer_.reset(new PCLVisualizer(name_));
// Set background color
const Color &bg_color = {0, 0, 0};
viewer_->setBackgroundColor(bg_color.r, bg_color.g, bg_color.b);
// Set camera position.
viewer_->setCameraPosition(0, 0, 200, 0, 0, 0, 1, 0, 0, 0);
viewer_->setSize(2500, 1500);
viewer_->addCoordinateSystem(1.0);
// Add mouse and keyboard callback.
viewer_->registerKeyboardCallback(
[&](const pcl::visualization::KeyboardEvent &event) -> void {
KeyboardEventCallback(event);
});
Run();
}));
}
virtual ~Visualizer() {
is_stopped_ = true;
viewer_->close();
if (thread_->joinable()) {
thread_->join();
}
}
void Render(const std::shared_ptr<FrameT> &frame) {
std::lock_guard<std::mutex> lock(mutex_);
while (history_frames_.size() > max_history_size_) {
history_frames_.pop_back();
}
history_frames_.emplace_front(frame);
curr_frame_iter_ = history_frames_.begin();
is_updated_ = true;
}
std::string Name() { return name_; }
protected:
void Run() {
while (!is_stopped_) {
if (is_updated_) {
RemoveRenderedObjects();
Draw();
is_updated_ = false;
}
viewer_->spinOnce(20); // ms
}
}
/**
* @brief Draw objects. This method should be overrided for customization.
*
* virtual void Draw() {
* FrameT frame = GetCurrentFrame();
* { // Update 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;
/**
* @brief Keyboard event callback function, This method should be overrided
* for customization
*
* @param event Keyboard event
*/
virtual void KeyboardEventCallback(
const pcl::visualization::KeyboardEvent &event) {
if (event.getKeySym() == "p" && event.keyDown()) {
std::lock_guard<std::mutex> lock(mutex_);
++curr_frame_iter_;
if (curr_frame_iter_ == history_frames_.end()) {
--curr_frame_iter_;
}
is_updated_ = true;
} else if (event.getKeySym() == "n" && event.keyDown()) {
std::lock_guard<std::mutex> lock(mutex_);
if (curr_frame_iter_ != history_frames_.begin()) {
--curr_frame_iter_;
}
is_updated_ = true;
} else if (event.getKeySym() == "r" && event.keyDown()) {
viewer_->setCameraPosition(0, 0, 200, 0, 0, 0, 1, 0, 0, 0);
viewer_->setSize(2500, 1500);
is_updated_ = true;
}
}
/**
* @brief Remove all old rendered point clouds
*
*/
void RemoveRenderedObjects() {
for (const auto &id : rendered_cloud_ids_) {
viewer_->removePointCloud(id);
}
rendered_cloud_ids_.clear();
viewer_->removeAllShapes();
}
FrameT GetCurrentFrame() const {
std::lock_guard<std::mutex> lock(mutex_);
return *(*curr_frame_iter_);
}
// visualizer name
std::string name_;
size_t max_history_size_;
// atomic bool variable
std::atomic_bool is_stopped_{false};
// mutex for visualizer_frame.
mutable std::mutex mutex_;
// bool flag indicates whether the Visualizer frame is updated.
std::atomic_bool is_updated_{false};
// The visualizer frame list
std::deque<std::shared_ptr<FrameT>> history_frames_;
// The current displayed frame iter.
typename std::deque<std::shared_ptr<FrameT>>::const_iterator curr_frame_iter_;
// The rendered cloud ids.
std::vector<std::string> rendered_cloud_ids_;
// thread for visualization
std::unique_ptr<std::thread> thread_ = nullptr;
// viewer
std::unique_ptr<PCLVisualizer> viewer_ = nullptr;
};
} // oh_my_loam

60
src/visualizer_feature_points.h Normal file
View File

@ -0,0 +1,60 @@
#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, GRAY, 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);
// }
}
};
} // oh_my_loam