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

add CMakeLists.txt and fix bug: build OK

main
feixyz10 2020-10-10 20:59:26 +08:00 committed by feixyz
parent 82eee40dc8
commit f94bc39ec6
8 changed files with 58 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.gitignore vendored
View File

@ -1 +1,3 @@
/.vscode
/.vscode
/lib
/build

23
CMakeLists.txt Normal file
View File

@ -0,0 +1,23 @@
cmake_minimum_required(VERSION 2.8.3)
project(oh_loam)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++17")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
find_package(PCL QUIET)
if (NOT PCL_FOUND)
message(FATAL_ERROR "PCL not found.")
endif()
include_directories(
${PROJECT_SOURCE_DIR}/common
${PROJECT_SOURCE_DIR}/src
${PCL_INCLUDE_DIRS}
)
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
add_library(${PROJECT_NAME} SHARED
${PROJECT_SOURCE_DIR}/src/base_feature_extractor.cc
)

10
common/filter.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "common/utils.h"
#include "utils.h"
namespace oh_loam {
@ -20,22 +20,22 @@ void RemovePointsIf(const pcl::PointCloud<PointT>& cloud_in,
}
cloud_out->points.resize(j);
cloud_out->height = 1;
cloud_out->widht = static_cast<uint32_t>(j);
cloud_out->width = static_cast<uint32_t>(j);
cloud_out->is_dense = true;
}
template <typename PointT>
void RemoveNaNPoint(const pcl::PointCloud<PointT>& cloud_in,
pcl::PointCloud<PointT>* const cloud_out) {
RemovePointsIf(cloud_in, cloud_out,
[](const PointT& pt) { return !IsFinite(pt); });
RemovePointsIf<PointT>(cloud_in, cloud_out,
[](const PointT& pt) { return !IsFinite(pt); });
}
template <typename PointT>
void RemoveClosedPoints(const pcl::PointCloud<PointT>& cloud_in,
pcl::PointCloud<PointT>* const cloud_out,
double min_dist = 0.1) {
RemovePointsIf(cloud_in, cloud_out, [](const PointT& pt) {
RemovePointsIf<PointT>(cloud_in, cloud_out, [&](const PointT& pt) {
return DistanceSqure(pt) < min_dist * min_dist;
});
}

7
common/types.h
View File

@ -1,5 +1,6 @@
#pragma once
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <cmath>
@ -16,13 +17,13 @@ enum class PointType {
NORNAL = 0,
LESS_SHARP = 1,
SHARP = 2,
}
};
struct EIGEN_ALIGN16 PointXYZTCT {
PCL_ADD_POINT4D;
float time = 0.0f;
float curvature = NAN;
char type = PointType::NORNAL;
PointType type = PointType::NORNAL;
PointXYZTCT() {
x = y = z = 0.0f;
@ -55,7 +56,7 @@ struct EIGEN_ALIGN16 PointXYZTCT {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
using IPoint = pcl::PointXYZTCT;
using IPoint = PointXYZTCT;
using IPointCloud = pcl::PointCloud<IPoint>;
using IPointCloudPtr = IPointCloud::Ptr;
using IPointCloudConstPtr = IPointCloud::ConstPtr;

4
common/utils.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "common/types.h"
#include "types.h"
namespace oh_loam {
@ -28,7 +28,7 @@ inline double NormalizeAngle(double ang) {
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, pt[0].x);
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};

32
src/base_feature_extractor.cc
View File

@ -1,7 +1,7 @@
#include "src/base_feature_extractor.h"
#include "base_feature_extractor.h"
#include <cmath>
#include "common/filter.h"
#include "filter.h"
namespace oh_loam {
@ -14,26 +14,26 @@ bool FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
RemoveNaNPoint<Point>(cloud_in, cloud.get());
RemoveClosedPoints<Point>(*cloud, cloud.get(), kPointMinDist);
std::vector<IPointCloud> scans;
ScanSplit(*cloud, &scans);
for (auto& scan : sccans) {
ComputeCurvature(&scan);
AssignType(&scan);
SplitScan(*cloud, &scans);
for (auto& scan : scans) {
ComputePointCurvature(&scan);
AssignPointType(&scan);
}
for (const auto& scan : sccans) {
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.emplace_back(pt);
feature->flat_surf_points->points.emplace_back(pt);
break;
case PointType::LESS_FLAT:
feature->less_flat_surf_points.emplace_back(pt);
feature->less_flat_surf_points->points.emplace_back(pt);
break;
case PointType::LESS_SHARP:
feature->less_sharp_corner_points.emplace_back(pt);
feature->less_sharp_corner_points->points.emplace_back(pt);
break;
case PointType::SHARP:
feature->sharp_corner_points.emplace_back(pt);
feature->sharp_corner_points->points.emplace_back(pt);
break;
default:
break;
@ -44,8 +44,9 @@ bool FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
void FeaturePointsExtractor::SplitScan(
const PointCloud& cloud, std::vector<IPointCloud>* const scans) const {
scans.resize(num_scans_);
const auto & [ yaw_start, yaw_end ] = GetYawRange(cloud);
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) {
@ -59,14 +60,15 @@ void FeaturePointsExtractor::SplitScan(
half_passed = true;
yaw_start += 2 * M_PI;
}
scans[scan_id].emplace_back(pt.x, pt.y, pt.z, yaw_diff / yaw_range);
(*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 (int i = 5; i < pts.size() - 5; ++i) {
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;

4
src/base_feature_extractor.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "common/utils.h"
#include "utils.h"
namespace oh_loam {
@ -32,7 +32,7 @@ class FeaturePointsExtractor {
int num_scans() const { return num_scans_; }
protected:
virtual GetScanID(const Point& pt) const = 0;
virtual int GetScanID(const Point& pt) const = 0;
void SplitScan(const PointCloud& cloud,
std::vector<IPointCloud>* const scans) const;

4
src/feature_extractor_VLP16.h
View File

@ -1,7 +1,7 @@
#pragma once
#include "common/utils.h"
#include "src/base_feature_extractor.h"
#include "base_feature_extractor.h"
#include "utils.h"
namespace oh_loam {