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

refactoring...

main
feixyz10 2021-01-04 21:26:09 +08:00 committed by feixyz
parent cc95fc30e4
commit 64c06113dd
60 changed files with 778 additions and 664 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
CMakeLists.txt
View File

@ -10,21 +10,21 @@ find_package(PCL QUIET)
find_package(g3log REQUIRED)
find_package(yaml-cpp REQUIRED)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
nav_msgs
sensor_msgs
roscpp
rospy
rosbag
std_msgs
image_transport
cv_bridge
tf
)
# find_package(catkin REQUIRED COMPONENTS
# geometry_msgs
# nav_msgs
# sensor_msgs
# roscpp
# rospy
# rosbag
# std_msgs
# image_transport
# cv_bridge
# tf
# )
include_directories(SYSTEM
${catkin_INCLUDE_DIRS}
# ${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${G3LOG_INCLUDE_DIRS}
)
@ -32,33 +32,32 @@ include_directories(SYSTEM
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
include_directories(
src
common
)
# catkin_package(
# CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
# DEPENDS EIGEN3 PCL
# INCLUDE_DIRS src common
# )
catkin_package(
CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
DEPENDS EIGEN3 PCL
INCLUDE_DIRS src common
include_directories(
${CMAKE_SOURCE_DIR}
)
add_subdirectory(common)
add_subdirectory(src)
add_subdirectory(oh_my_loam)
add_executable(main main.cc)
target_link_libraries(main
${catkin_LIBRARIES}
${PCL_LIBRARIES}
${G3LOG_LIBRARIES}
${YAML_CPP_LIBRARIES}
common
oh_my_loam
extractor
odometry
mapper
solver
${CERES_LIBRARIES}
helper
visualizer
)
# add_executable(main main.cc)
# target_link_libraries(main
# ${catkin_LIBRARIES}
# ${PCL_LIBRARIES}
# ${G3LOG_LIBRARIES}
# ${YAML_CPP_LIBRARIES}
# common
# oh_my_loam
# extractor
# odometry
# mapper
# solver
# ${CERES_LIBRARIES}
# helper
# visualizer
# )

6
common/CMakeLists.txt
View File

@ -1,3 +1,7 @@
aux_source_directory(. SRC_FILES)
file(GLOB SRC_FILES **/*.cc)
message(STATUS "common dir: " ${CMAKE_CURRENT_SOURCE_DIR})
# include_directories(${CMAKE_CURRENT_SOURCE_DIR})
add_library(common SHARED ${SRC_FILES})

4
common/color.h → common/color/color.h
View File

@ -1,6 +1,6 @@
#pragma once
namespace oh_my_loam {
namespace common {
struct Color {
unsigned char r, g, b;
@ -20,4 +20,4 @@ struct Color {
#define PINK Color(255, 182, 193)
#define YELLOW Color(255, 255, 0)
} // namespace oh_my_loam
} // namespace common

15
common/common.h
View File

@ -1,11 +1,8 @@
#pragma once
#include "color.h"
#include "config.h"
#include "log.h"
#include "macros.h"
#include "pose.h"
#include "tic_toc.h"
#include "types.h"
#include "utils.h"
#include "yaml-cpp/yaml.h"
#include "color/color.h"
#include "config/yaml_config.h"
#include "log/log.h"
#include "macro/macros.h"
#include "math/math_utils.h"
#include "time/timer.h"

36
common/config.h
View File

@ -1,36 +0,0 @@
#pragma once
#include <yaml-cpp/yaml.h>
#include <string>
#include "log.h"
#include "macros.h"
namespace oh_my_loam {
class Config {
public:
void SetConfigFile(const std::string& file) {
config_.reset(new YAML::Node);
*config_ = YAML::LoadFile(file);
}
template <typename T>
const T Get(const std::string& key) const {
AFATAL_IF(!config_) << "No config exists: please call SetConfigFile.";
return (*config_)[key].as<T>();
}
const YAML::Node& config() const {
AFATAL_IF(!config_) << "No config exists: please call SetConfigFile.";
return *config_;
}
private:
std::shared_ptr<YAML::Node> config_{nullptr};
DECLARE_SINGLETON(Config)
};
} // namespace oh_my_loam

34
common/config/yaml_config.h Normal file
View File

@ -0,0 +1,34 @@
#pragma once
#include <yaml-cpp/yaml.h>
#include <string>
#include "common/log/log.h"
#include "common/macro/macros.h"
namespace common {
class YAMLConfig {
public:
void Init(const std::string& file) {
config_.reset(new YAML::Node);
*config_ = YAML::LoadFile(file);
}
template <typename T>
const T Get(const std::string& key) const {
AFATAL_IF(!config_) << "Not initialized, please call Init first.";
return (*config_)[key].as<T>();
}
const YAML::Node& config() const {
AFATAL_IF(!config_) << "Not initialized, please call Init first.";
return *config_;
}
private:
std::unique_ptr<YAML::Node> config_{nullptr};
DECLARE_SINGLETON(YAMLConfig);
};
} // namespace common

20
common/geometry/pose.cc Normal file
View File

@ -0,0 +1,20 @@
#include "pose.h"
namespace common {
Pose3d Interpolate(const Pose3d& pose_from, const Pose3d& pose_to, double t) {
return pose_from.Interpolate(pose_to, t);
}
Pose3d operator*(const Pose3d& lhs, const Pose3d& rhs) {
return Pose3d(lhs.q() * rhs.q(), lhs.q() * rhs.p() + lhs.p());
}
std::string Pose3d::ToString() const {
std::ostringstream oss;
oss << "[Pose3d] q = (" << q_.coeffs().transpose() << "), p = ("
<< p_.transpose() << ")";
return oss.str();
}
} // namespace common

40
common/pose.h → common/geometry/pose.h
View File

@ -2,24 +2,24 @@
#include <eigen3/Eigen/Dense>
namespace oh_my_loam {
namespace common {
class Pose3D {
class Pose3d {
public:
Pose3D() {
Pose3d() {
q_.setIdentity();
p_.setZero();
};
Pose3D(const Eigen::Quaterniond& q, const Eigen::Vector3d& p)
Pose3d(const Eigen::Quaterniond& q, const Eigen::Vector3d& p)
: q_(q), p_(p) {}
Pose3D(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& p)
Pose3d(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& p)
: q_(r_mat), p_(p) {}
Pose3D(const double* const q, const double* const p) : q_(q), p_(p) {}
Pose3d(const double* const q, const double* const p) : q_(q), p_(p) {}
Pose3D Inv() const {
Pose3d Inv() const {
Eigen::Quaterniond q_inv = q_.inverse();
Eigen::Vector3d p_inv = q_inv * p_;
return {q_inv, -p_inv};
@ -33,21 +33,6 @@ class Pose3D {
return Transform(vec);
}
template <typename PointT>
PointT Transform(const PointT& pt) const {
PointT pt_n = pt;
Eigen::Vector3d vec = Transform(Eigen::Vector3d(pt.x, pt.y, pt.z));
pt_n.x = static_cast<float>(vec.x());
pt_n.y = static_cast<float>(vec.y());
pt_n.z = static_cast<float>(vec.z());
return pt_n;
}
template <typename PointT>
PointT operator*(const PointT& vec) const {
return Transform<PointT>(vec);
}
Eigen::Vector3d Translate(const Eigen::Vector3d& vec) const {
return vec + p_;
}
@ -55,7 +40,7 @@ class Pose3D {
Eigen::Vector3d Rotate(const Eigen::Vector3d& vec) const { return q_ * vec; }
// Spherical linear interpolation to `pose_to`, `t` belongs [0, 1]
Pose3D Interpolate(const Pose3D& pose_to, double t) const {
Pose3d Interpolate(const Pose3d& pose_to, double t) const {
Eigen::Quaterniond q_interp = q_.slerp(t, pose_to.q_);
Eigen::Vector3d p_interp = (pose_to.p_ - p_) * t + p_;
return {q_interp, p_interp};
@ -64,6 +49,7 @@ class Pose3D {
std::string ToString() const;
Eigen::Quaterniond q() const { return q_; }
Eigen::Vector3d p() const { return p_; }
protected:
@ -71,10 +57,10 @@ class Pose3D {
Eigen::Vector3d p_; // position
};
Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t);
Pose3d Interpolate(const Pose3d& pose_from, const Pose3d& pose_to, double t);
Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs);
Pose3d operator*(const Pose3d& lhs, const Pose3d& rhs);
using Trans3D = Pose3D;
using Trans3d = Pose3d;
} // namespace oh_my_loam
} // namespace common

7
common/log.cc → common/log/log.cc
View File

@ -1,6 +1,8 @@
#include "log.h"
#include <filesystem>
namespace common {
namespace g3 {
void InitG3Logging(bool log_to_file, const std::string& prefix,
const std::string& path) {
static std::unique_ptr<g3::LogWorker> worker;
@ -23,4 +25,5 @@ void InitG3Logging(bool log_to_file, const std::string& prefix,
}
g3::initializeLogging(worker.get());
}
}
} // namespace common

8
common/log.h → common/log/log.h
View File

@ -43,6 +43,11 @@ const LEVELS USER(ERROR.value + 100, "USER");
#define AFATAL_IF(cond) G3LOG_IF(FATAL, cond)
#define ACHECK(cond) G3CHECK(cond)
namespace common {
void InitG3Logging(bool log_to_file = false, const std::string& prefix = "",
const std::string& path = "./");
} // namespace common
namespace g3 {
class CustomSink {
public:
@ -114,7 +119,4 @@ class CustomSink {
}
};
void InitG3Logging(bool log_to_file = false, const std::string& prefix = "",
const std::string& path = "./");
} // namespace g3

5
common/macros.h → common/macro/macros.h
View File

@ -4,14 +4,15 @@
#include <memory>
#include <mutex>
#define LOG_TIMESTAMP(timestamp) \
// format timestamp
#define FMT_TIMESTAMP(timestamp) \
std::fixed << std::setprecision(3) << timestamp;
#define DISALLOW_COPY_AND_ASSIGN(classname) \
classname(const classname &) = delete; \
classname &operator=(const classname &) = delete;
// adapted form baidu apollo cyber/common/macros.h, it is thread safe
// adapted form baidu apollo cyber/common/macros.h
#define DECLARE_SINGLETON(classname) \
public: \
static classname *Instance() { \

8
common/utils.cc → common/math/math_utils.cc
View File

@ -1,6 +1,8 @@
#include "utils.h"
#include "math_utils.h"
namespace oh_my_loam {
#include "common/log/log.h"
namespace common {
double NormalizeAngle(double ang) {
const double& two_pi = 2 * M_PI;
@ -19,4 +21,4 @@ const std::vector<int> Range(int begin, int end, int step) {
const std::vector<int> Range(int end) { return Range(0, end, 1); }
} // namespace oh_my_loam
} // namespace common

23
common/math/math_utils.h Normal file
View File

@ -0,0 +1,23 @@
#pragma once
#include <cmath>
#include <vector>
namespace common {
// normalize an angle to [-pi, pi)
double NormalizeAngle(double ang);
// Convert an angle from degree to rad
double Degree2Rad(double degree);
// Convert an angle from rad to degree
double Rad2Degree(double rad);
// like Python built-in range, [begin, end)
const std::vector<int> Range(int begin, int end, int step = 1);
// like Python built-in range, [0, end)
const std::vector<int> Range(int end);
} // namespace common

76
common/pcl/pcl_types.h Normal file
View File

@ -0,0 +1,76 @@
#pragma once
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
// Following 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/filters/impl/voxel_grid.hpp>
#include <pcl/impl/pcl_base.hpp>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
namespace common {
using Point = pcl::PointXYZ;
using PointCloud = pcl::PointCloud<Point>;
using PointCloudPtr = PointCloud::Ptr;
using PointCloudConstPtr = PointCloud::ConstPtr;
using Indices = std::vector<int>;
struct PointXYZT;
using TPoint = PointXYZT;
using TPointCloud = pcl::PointCloud<TPoint>;
using TPointCloudPtr = TPointCloud::Ptr;
using TPointCloudConstPtr = TPointCloud::ConstPtr;
struct EIGEN_ALIGN16 PointXYZT {
PCL_ADD_POINT4D;
union {
float time;
float intensity;
float data_c[4];
};
PointXYZT() {
x = y = z = 0.0f;
data[3] = 1.0f;
time = 0.0f;
}
PointXYZT(float x, float y, float z, float time = 0.0f)
: x(x), y(y), z(z), time(time) {
data[3] = 1.0f;
}
PointXYZT(const Point& p) {
x = p.x;
y = p.y;
z = p.z;
data[3] = 1.0f;
time = 0.0f;
}
PointXYZT(const PointXYZT& p) {
x = p.x;
y = p.y;
z = p.z;
data[3] = 1.0f;
time = p.time;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace common
// clang-format off
POINT_CLOUD_REGISTER_POINT_STRUCT(
::common::PointXYZT,
(float, x, x)
(float, y, y)
(float, z, z)
(float, time, time)
// (float, intensity, intensity)
)
// clang-format on

71
common/pcl/pcl_utils.h Normal file
View File

@ -0,0 +1,71 @@
#pragma once
#include "log/log.h"
#include "pcl_types.h"
namespace common {
// Distance squred of a point to origin
template <typename PointType>
inline double DistanceSqure(const PointType& pt) {
return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z;
}
// Distance squred of two points
template <typename PointType>
inline double DistanceSqure(const PointType& pt1, const PointType& pt2) {
return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y) +
(pt1.z - pt2.z) * (pt1.z - pt2.z);
}
// Distance of a point to origin
template <typename PointType>
inline double Distance(const PointType& pt) {
return std::sqrt(DistanceSqure(pt));
}
// Distance squred of two points
template <typename PointType>
inline double Distance(const PointType& pt1, const PointType& pt2) {
return std::sqrt(DistanceSqure(pt1, pt2));
}
// Check whether is a finite point: neither infinite nor nan
template <typename PointType>
inline double IsFinite(const PointType& pt) {
return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z);
}
// Remove point if the condition evaluated to true on it
template <typename PointType>
void RemovePoints(const typename pcl::PointCloud<PointType>::ConstPtr cloud_in,
std::function<bool(const PointType&)> check,
pcl::PointCloud<PointType>* const cloud_out) {
if (cloud_in.get() != cloud_out) {
cloud_out->header = cloud_in.header;
cloud_out->resize(cloud_in->size());
}
size_t j = 0;
for (size_t i = 0; i < cloud_in->size(); ++i) {
const auto pt = cloud_in->points[i];
if (check(pt)) continue;
cloud_out->points[j++] = pt;
}
cloud_out->points.resize(j);
cloud_out->height = 1;
cloud_out->width = static_cast<uint32_t>(j);
cloud_out->is_dense = true;
}
template <typename PointType>
void VoxelDownSample(
const typename pcl::PointCloud<PointType>::ConstPtr& cloud_in,
pcl::PointCloud<PointType>* const cloud_out, double voxel_size) {
pcl::VoxelGrid<PointType> filter;
filter.setInputCloud(cloud_in);
filter.setLeafSize(voxel_size, voxel_size, voxel_size);
filter.filter(*cloud_out);
}
} // namespace common

20
common/pose.cc
View File

@ -1,20 +0,0 @@
#include <pose.h>
namespace oh_my_loam {
Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t) {
return pose_from.Interpolate(pose_to, t);
}
Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs) {
return Pose3D(lhs.q() * rhs.q(), lhs.q() * rhs.p() + lhs.p());
}
std::string Pose3D::ToString() const {
std::ostringstream oss;
oss << "[Pose3D] q = (" << q_.coeffs().transpose() << "), p = ("
<< p_.transpose() << ")";
return oss.str();
}
} // namespace oh_my_loam

25
common/tic_toc.h
View File

@ -1,25 +0,0 @@
#pragma once
#include <chrono>
#include <cstdlib>
#include <ctime>
namespace oh_my_loam {
class TicToc {
public:
TicToc() { tic(); }
void tic() { start_ = std::chrono::system_clock::now(); }
double toc() {
end_ = std::chrono::system_clock::now();
std::chrono::duration<double> elapsed_seconds = end_ - start_;
return elapsed_seconds.count() * 1000; // ms
}
private:
std::chrono::time_point<std::chrono::system_clock> start_, end_;
};
} // namespace oh_my_loam

28
common/time/timer.cc Normal file
View File

@ -0,0 +1,28 @@
#include "timer.h"
#include "common/log/log.h"
namespace common {
double Timer::Toc(char unit) {
ACHECK(unit == 's' || unit == 'm' || unit == 'u')
<< "Only 's'(second), 'm'(millisecond) and 'u'(microsecond) are "
"supported";
double factor = 1.0;
if (unit == 'm') factor = 1.0e3;
if (unit == 'u') factor = 1.0e6;
end_ = std::chrono::system_clock::now();
std::chrono::duration<double> elapsed_seconds = end_ - start_;
return elapsed_seconds.count() * factor;
}
TimerWrapper::~TimerWrapper() {
double duration = timer_.Toc();
if (duration_ms_ < 0) {
AINFO << msg_ << ": time elapsed (ms): " << FMT_TIMESTAMP(duration);
}
if (duration_ms_ > 0 && duration > duration_ms_) {
AWARN << msg_ << ": time elapsed (ms): " << FMT_TIMESTAMP(duration);
}
}
} // namespace common

43
common/time/timer.h Normal file
View File

@ -0,0 +1,43 @@
#pragma once
#include <chrono>
#include <string>
#include "common/macro/macros.h"
namespace common {
class Timer {
public:
Timer() { Tic(); }
void Tic() { start_ = std::chrono::system_clock::now(); }
// unit: 's' = second, 'm' = millisecond, 'u' = microsecond
double Toc(char unit = 'm');
private:
std::chrono::time_point<std::chrono::system_clock> start_, end_;
DISALLOW_COPY_AND_ASSIGN(Timer);
};
class TimerWrapper {
public:
explicit TimerWrapper(const std::string& msg, double duration_ms = -1.0)
: msg_(msg), duration_ms_(duration_ms) {
timer_.Tic();
};
~TimerWrapper();
private:
std::string msg_;
double duration_ms_;
Timer timer_;
DISALLOW_COPY_AND_ASSIGN(TimerWrapper);
};
#define TIME_ELAPSED(msg) TimerWrapper(msg)
#define TIME_ELAPSED_EXPECT(msg, max_time_elapsed) \
TimerWrapper(msg, max_time_elapsed)
} // namespace common

144
common/types.h
View File

@ -1,144 +0,0 @@
#pragma once
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#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>
// Thses 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/filters/impl/voxel_grid.hpp>
#include <pcl/impl/pcl_base.hpp>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <pcl/visualization/impl/pcl_visualizer.hpp>
#include <pcl/visualization/impl/point_cloud_geometry_handlers.hpp>
namespace oh_my_loam {
using Point = pcl::PointXYZ;
using PointCloud = pcl::PointCloud<Point>;
using PointCloudPtr = PointCloud::Ptr;
using PointCloudConstPtr = PointCloud::ConstPtr;
enum class PointType {
FLAT = -2,
LESS_FLAT = -1,
NORNAL = 0,
LESS_SHARP = 1,
SHARP = 2,
};
struct PointXYZT;
using TPoint = PointXYZT;
using TPointCloud = pcl::PointCloud<TPoint>;
using TPointCloudPtr = TPointCloud::Ptr;
using TPointCloudConstPtr = TPointCloud::ConstPtr;
struct PointXYZTCT;
using TCTPoint = PointXYZTCT;
using TCTPointCloud = pcl::PointCloud<TCTPoint>;
using TCTPointCloudPtr = TCTPointCloud::Ptr;
using TCTPointCloudConstPtr = TCTPointCloud::ConstPtr;
struct PointXYZT {
PCL_ADD_POINT4D;
union EIGEN_ALIGN16 {
float time;
// make sure VoxelGrid can work with this custom point type:
// https://github.com/PointCloudLibrary/pcl/issues/2331
float intensity;
};
PointXYZT() {
x = y = z = 0.0f;
time = 0.0f;
}
PointXYZT(float x, float y, float z, float time = 0.0f)
: x(x), y(y), z(z), time(time) {}
PointXYZT(const Point& p) {
x = p.x;
y = p.y;
z = p.z;
time = 0.0f;
}
PointXYZT(const PointXYZT& p) {
x = p.x;
y = p.y;
z = p.z;
time = p.time;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
struct PointXYZTCT {
PCL_ADD_POINT4D;
union EIGEN_ALIGN16 {
float data_c[4];
struct {
float time;
float curvature;
PointType type;
};
};
PointXYZTCT() {
x = y = z = 0.0f;
time = 0.0f;
curvature = std::nanf("");
type = PointType::NORNAL;
}
PointXYZTCT(float x, float y, float z, float time = 0.0f,
float curvature = std::nanf(""),
PointType type = PointType::NORNAL)
: x(x), y(y), z(z), time(time), curvature(curvature), type(type) {}
PointXYZTCT(const Point& p) {
x = p.x;
y = p.y;
z = p.z;
time = 0.0f;
curvature = std::nanf("");
type = PointType::NORNAL;
}
PointXYZTCT(const PointXYZTCT& p) {
x = p.x;
y = p.y;
z = p.z;
time = p.time;
curvature = p.curvature;
type = p.type;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
} // namespace oh_my_loam
// clang-format off
POINT_CLOUD_REGISTER_POINT_STRUCT(
oh_my_loam::PointXYZT,
(float, x, x)
(float, y, y)
(float, z, z)
(float, time, time)
)
POINT_CLOUD_REGISTER_POINT_STRUCT(
oh_my_loam::PointXYZTCT,
(float, x, x)
(float, y, y)
(float, z, z)
(float, time, time)
(float, curvature, curvature)
)
// clang-format on

89
common/utils.h
View File

@ -1,89 +0,0 @@
#pragma once
#include "color.h"
#include "log.h"
#include "types.h"
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 DistanceSqure(const PointT& pt1, const PointT& pt2) {
return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y) +
(pt1.z - pt2.z) * (pt1.z - pt2.z);
}
template <typename PointT>
inline double Distance(const PointT& pt) {
return std::sqrt(DistanceSqure(pt));
}
template <typename PointT>
inline double Distance(const PointT& pt1, const PointT& pt2) {
return std::sqrt(DistanceSqure(pt1, pt2));
}
template <typename PointT>
inline double IsFinite(const PointT& pt) {
return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z);
}
// normalize an angle to [-pi, pi)
double NormalizeAngle(double ang);
// like Python built-in range, [begin, end)
const std::vector<int> Range(int begin, int end, int step = 1);
const std::vector<int> Range(int end); // [0, end)
template <typename PointT>
void RemovePointsIf(const pcl::PointCloud<PointT>& cloud_in,
pcl::PointCloud<PointT>* const cloud_out,
std::function<bool(const PointT&)> cond) {
if (&cloud_in != cloud_out) {
cloud_out->header = cloud_in.header;
cloud_out->points.resize(cloud_in.size());
}
size_t j = 0;
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);
cloud_out->is_dense = true;
}
template <typename PointT>
void RemoveNaNPoint(const pcl::PointCloud<PointT>& cloud_in,
pcl::PointCloud<PointT>* const cloud_out) {
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<PointT>(cloud_in, cloud_out, [&](const PointT& pt) {
return DistanceSqure(pt) < min_dist * min_dist;
});
}
template <typename PointT>
void VoxelDownSample(const typename pcl::PointCloud<PointT>::ConstPtr& cloud_in,
pcl::PointCloud<PointT>* const cloud_out,
double voxel_size) {
pcl::VoxelGrid<PointT> filter;
filter.setInputCloud(cloud_in);
filter.setLeafSize(voxel_size, voxel_size, voxel_size);
filter.filter(*cloud_out);
}
} // namespace oh_my_loam

54
src/visualizer/base_visualizer.h → common/visualizer/lidar_visualizer.h
View File

@ -1,39 +1,33 @@
#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 "common.h"
#include "utils.h"
#include "lidar_visualizer_utils.h"
namespace oh_my_loam {
namespace common {
struct VisFrame {
double timestamp = std::nanf("");
struct LidarVisFrame {
double timestamp = 0.0;
PointCloudPtr cloud = nullptr;
};
class Visualizer {
class LidarVisualizer {
public:
Visualizer(const std::string &name, size_t max_history_size)
explicit LidarVisualizer(const std::string &name,
size_t max_history_size = 10)
: 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.
// Set background color: black
viewer_->setBackgroundColor(0, 0, 0);
// 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.
// Register keyboard callback
viewer_->registerKeyboardCallback(
[&](const pcl::visualization::KeyboardEvent &event) -> void {
KeyboardEventCallback(event);
@ -42,7 +36,7 @@ class Visualizer {
}));
}
virtual ~Visualizer() {
virtual ~LidarVisualizer() {
is_stopped_ = true;
viewer_->close();
if (thread_->joinable()) {
@ -50,7 +44,7 @@ class Visualizer {
}
}
void Render(const std::shared_ptr<VisFrame> &frame) {
void Render(const std::shared_ptr<LidarVisFrame> &frame) {
std::lock_guard<std::mutex> lock(mutex_);
while (history_frames_.size() > max_history_size_) {
history_frames_.pop_back();
@ -75,13 +69,12 @@ class Visualizer {
}
/**
* @brief Draw objects, pure virtual function. Example code:
* virtual void Draw() {
* auto frame = GetCurrentFrame<VisFrame>();
* AddPointCloud(frame.cloud, {255, 255, 255}, "point cloud");
* }
* @brief Draw objects. This method should be overrided for customization
*/
virtual void Draw() = 0;
virtual void Draw() {
auto frame = GetCurrentFrame<LidarVisFrame>();
DrawPointCloud(frame.cloud, {255, 255, 255}, "point cloud");
}
/**
* @brief Keyboard event callback function, This method should be overrided
@ -111,10 +104,6 @@ class Visualizer {
}
}
/**
* @brief Remove all old rendered point clouds
*
*/
void RemoveRenderedObjects() {
for (const auto &id : rendered_cloud_ids_) {
viewer_->removePointCloud(id);
@ -159,10 +148,11 @@ class Visualizer {
std::atomic_bool is_updated_{false};
// The visualizer frame list
std::deque<std::shared_ptr<VisFrame>> history_frames_;
std::deque<std::shared_ptr<LidarVisFrame>> history_frames_;
// The current displayed frame iter.
typename std::deque<std::shared_ptr<VisFrame>>::iterator curr_frame_iter_;
typename std::deque<std::shared_ptr<LidarVisFrame>>::iterator
curr_frame_iter_;
// The rendered cloud ids.
std::vector<std::string> rendered_cloud_ids_;
@ -174,4 +164,4 @@ class Visualizer {
std::unique_ptr<PCLVisualizer> viewer_ = nullptr;
};
} // namespace oh_my_loam
} // namespace common

16
common/visualizer/lidar_visualizer_utils.cc Normal file
View File

@ -0,0 +1,16 @@
#include "lidar_visualizer_utils.h"
namespace common {
void AddLine(const pcl::PointXYZ& pt1, const pcl::PointXYZ& pt2,
const Color& color, const std::string& id,
PCLVisualizer* const viewer) {
viewer->addLine(pt1, pt2, color.r, color.g, color.b, id);
}
void DrawSphere(const pcl::PointXYZ& center, double radius, const Color& color,
const std::string& id, PCLVisualizer* const viewer) {
viewer->addSphere(center, radius, color.r, color.g, color.b, id);
}
} // namespace common

24
src/visualizer/utils.h → common/visualizer/lidar_visualizer_utils.h
View File

@ -1,6 +1,15 @@
#pragma once
namespace oh_my_loam {
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/point_cloud_handlers.h>
#include <pcl/visualization/impl/pcl_visualizer.hpp>
#include <pcl/visualization/impl/point_cloud_geometry_handlers.hpp>
#include <string>
#include "common/color/color.h"
#include "common/pcl/pcl_types.h"
namespace common {
using PCLVisualizer = pcl::visualization::PCLVisualizer;
#define PCLColorHandlerCustom pcl::visualization::PointCloudColorHandlerCustom
@ -28,10 +37,11 @@ void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr& cloud,
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id);
}
template <typename PointType>
void AddLine(const PointType& pt1, const PointType& pt2, const Color& color,
const std::string& id, PCLVisualizer* const viewer) {
viewer->addLine(pt1, pt2, color.r, color.g, color.b, id);
}
void AddLine(const pcl::PointXYZ& pt1, const pcl::PointXYZ& pt2,
const Color& color, const std::string& id,
PCLVisualizer* const viewer);
} // namespace oh_my_loam
void DrawSphere(const pcl::PointXYZ& center, double radius, const Color& color,
const std::string& id, PCLVisualizer* const viewer);
} // namespace common

17
oh_my_loam/CMakeLists.txt Normal file
View File

@ -0,0 +1,17 @@
message(STATUS "oh_my_loam dir: " ${CMAKE_CURRENT_SOURCE_DIR})
include_directories(
${CMAKE_CURRENT_SOURCE_DIR}
)
add_subdirectory(base)
add_subdirectory(extractor)
# add_subdirectory(visualizer)
# add_subdirectory(odometry)
# add_subdirectory(mapper)
# add_subdirectory(solver)
# aux_source_directory(. SRC_FILES)
# add_library(oh_my_loam SHARED ${SRC_FILES})

3
oh_my_loam/base/CMakeLists.txt Normal file
View File

@ -0,0 +1,3 @@
aux_source_directory(. SRC_FILES)
add_library(base SHARED ${SRC_FILES})

21
oh_my_loam/base/feature.h Normal file
View File

@ -0,0 +1,21 @@
#pragma once
#include "common/pcl/pcl_types.h"
namespace oh_my_loam {
struct Feature {
common::TPointCloudPtr cloud_sharp_corner;
common::TPointCloudPtr cloud_less_sharp_corner;
common::TPointCloudPtr cloud_flat_surf;
common::TPointCloudPtr cloud_less_flat_surf;
FeaturePoints() {
cloud_sharp_corner.reset(new common::TPointCloud);
cloud_less_sharp_corner.reset(new common::TPointCloud);
cloud_flat_surf.reset(new common::TPointCloud);
cloud_less_flat_surf.reset(new common::TPointCloud);
}
};
} // namespace oh_my_loam

0
src/helper/helper.cc → oh_my_loam/base/helper.cc
View File

31
src/helper/helper.h → oh_my_loam/base/helper.h
View File

@ -1,25 +1,38 @@
#pragma once
#include "common.h"
#include "common/geometry/pose.h"
#include "common/pcl/pcl_types.h"
namespace oh_my_loam {
inline float GetTime(const TPoint& pt) {
return pt.time - static_cast<int>(pt.time);
}
using common::TPoint;
using common::Pose3d;
using common::Trans3d;
inline float GetTime(const TPoint& pt) { return pt.time - std::floor(pt.time); }
inline int GetScanId(const TPoint& pt) { return static_cast<int>(pt.time); }
template <typename PointType>
void TransformPoint(const Pose3d& pose, const PointType& pt_in,
PointType* const pt_out) {
*pt_out = pt_in;
Eigen::Vector3d pnt = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z);
pt_out->x = pnt.x();
pt_out->y = pnt.y();
pt_out->z = pnt.z();
}
/**
* @brief Transform a lidar point to the start of the scan
*
* @param pose Relative pose, end scan time w.r.t. start scan time
* @param time Point time relative to the start time of the scan, \in [0, 1]
*/
void TransformToStart(const Pose3D& pose, const TPoint& pt_in,
void TransformToStart(const Pose3d& pose, const TPoint& pt_in,
TPoint* const pt_out) {
Pose3D pose_interp = Pose3D().Interpolate(pose, GetTime(pt_in));
*pt_out = pose_interp * pt_in;
Pose3d pose_interp = Pose3d().Interpolate(pose, GetTime(pt_in));
TransformPoint<TPoint>(pose_interp, pt_in, pt_out);
}
/**
@ -28,10 +41,10 @@ void TransformToStart(const Pose3D& pose, const TPoint& pt_in,
* @param pose Relative pose, end scan time w.r.t. start scan time
* @param time Point time relative to the start time of the scan, \in [0, 1]
*/
void TransformToEnd(const Pose3D& pose, const TPoint& pt_in,
void TransformToEnd(const Pose3d& pose, const TPoint& pt_in,
TPoint* const pt_out) {
TransformToStart(pose, pt_in, pt_out);
*pt_out = pose.Inv() * (*pt_out);
TransformPoint<TPoint>(pose.Inv(), *pt_out, pt_out);
}
struct PointLinePair {

89
oh_my_loam/base/types.h Normal file
View File

@ -0,0 +1,89 @@
#pragma once
#include "common/pcl/pcl_types.h"
namespace oh_my_loam {
enum class Type {
FLAT = -2,
LESS_FLAT = -1,
NORNAL = 0,
LESS_SHARP = 1,
SHARP = 2,
};
struct PointXYZTCT;
using TCTPoint = PointXYZTCT;
using TCTPointCloud = pcl::PointCloud<TCTPoint>;
using TCTPointCloudPtr = TCTPointCloud::Ptr;
using TCTPointCloudConstPtr = TCTPointCloud::ConstPtr;
struct PointXYZTCT {
PCL_ADD_POINT4D;
union EIGEN_ALIGN16 {
struct {
float time;
float curvature;
Type type;
};
float data_c[4];
};
PointXYZTCT() {
x = y = z = 0.0f;
data[3] = 1.0f;
time = curvature = 0.0f;
type = Type::NORNAL;
}
PointXYZTCT(float x, float y, float z, float time = 0.0f,
float curvature = 0.0f, Type type = Type::NORNAL)
: x(x), y(y), z(z), time(time), curvature(curvature), type(type) {
data[3] = 1.0f;
}
PointXYZTCT(const common::Point& p) {
x = p.x;
y = p.y;
z = p.z;
data[3] = 1.0f;
time = 0.0f;
curvature = 0.0f;
type = Type::NORNAL;
}
PointXYZTCT(const common::TPoint& p) {
x = p.x;
y = p.y;
z = p.z;
data[3] = 1.0f;
time = p.time;
curvature = 0.0f;
type = Type::NORNAL;
}
PointXYZTCT(const PointXYZTCT& p) {
x = p.x;
y = p.y;
z = p.z;
data[3] = 1.0f;
time = p.time;
curvature = p.curvature;
type = p.type;
}
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, curvature)
)
// clang-format on

3
configs/config.yaml → oh_my_loam/configs/config.yaml
View File

@ -8,6 +8,7 @@ vis: true
extractor_config:
min_point_num: 66
vis: false
verbose: false
sharp_corner_point_num: 2
corner_point_num: 20
flat_surf_point_num: 4
@ -24,5 +25,5 @@ odometry_config:
vis: true
# configs for mapper
odometry_config:
mapper_config:
vis: false

2
src/extractor/CMakeLists.txt → oh_my_loam/extractor/CMakeLists.txt
View File

@ -1,3 +1,5 @@
aux_source_directory(. SRC_FILES)
message(STATUS ${SRC_FILES})
add_library(extractor SHARED ${SRC_FILES})

144
src/extractor/base_extractor.cc → oh_my_loam/extractor/extractor.cc
View File

@ -1,4 +1,6 @@
#include "base_extractor.h"
#include "extractor.h"
#include "base/helper.h"
#include <cmath>
@ -7,53 +9,49 @@ namespace oh_my_loam {
namespace {
const int kScanSegNum = 6;
const double kTwoPi = 2 * M_PI;
using namespace common;
} // namespace
bool Extractor::Init(const YAML::Node& config) {
config_ = config;
is_vis_ = Config::Instance()->Get<bool>("vis") && config_["vis"].as<bool>();
bool Extractor::Init() {
const auto& config = YAMLConfig::Instance()->config();
config_ = config["extractor_config"];
is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
verbose_ = config_["verbose"].as<bool>();
AINFO << "Extraction visualizer: " << (is_vis_ ? "ON" : "OFF");
if (is_vis_) visualizer_.reset(new ExtractorVisualizer);
return true;
}
void Extractor::Process(const PointCloud& cloud, FeaturePoints* const feature) {
if (cloud.size() < config_["min_point_num"].as<size_t>()) {
AWARN << "Too few points ( < " << config_["min_point_num"].as<int>()
<< " ) after remove: " << cloud.size();
void Extractor::Process(const PointCloudConstPtr& cloud,
Feature* const feature) {
TIME_ELAPSED(__PRETTY_FUNCTION__);
if (cloud->size() < config_["min_point_num"].as<size_t>()) {
AWARN << "Too few input points: num = " << cloud->size() << " (< "
<< config_["min_point_num"].as<int>() << ")";
return;
}
TicToc timer;
// split point cloud int scans
std::vector<TCTPointCloud> scans;
SplitScan(cloud, &scans);
double time_split = timer.toc();
SplitScan(*cloud, &scans);
// compute curvature for each point in each scan
for (auto& scan : scans) {
ComputePointCurvature(&scan);
}
double time_curv = timer.toc();
// assign type for each point in each scan, five types: FLAT, LESS_FLAT,
// NORMAL, LESS_SHARP, SHARP
// assign type to each point in each scan: FLAT, LESS_FLAT,
// NORMAL, LESS_SHARP or SHARP
for (auto& scan : scans) {
AssignPointType(&scan);
}
double time_assign = timer.toc();
// store points into feature point clouds according to their type
for (const auto& scan : scans) {
GenerateFeaturePoints(scan, feature);
GenerateFeature(scan, feature);
}
double time_store = timer.toc();
AINFO << "Time elapsed (ms): scan_split = " << std::setprecision(3)
<< time_split << ", curvature_compute = " << time_curv - time_split
<< ", type_assign = " << time_assign - time_curv
<< ", store = " << time_store - time_assign;
AUSER << "Time elapsed (ms): whole extraction = " << time_store;
if (is_vis_) Visualize(cloud, *feature);
}
void Extractor::SplitScan(const PointCloud& cloud,
std::vector<TCTPointCloud>* const scans) const {
Timer timer;
scans->resize(num_scans_);
double yaw_start = -atan2(cloud.points[0].y, cloud.points[0].x);
bool half_passed = false;
@ -68,14 +66,15 @@ void Extractor::SplitScan(const PointCloud& cloud,
half_passed = true;
yaw_start += kTwoPi;
}
(*scans)[scan_id].points.emplace_back(pt.x, pt.y, pt.z,
yaw_diff / kTwoPi + scan_id);
(*scans)[scan_id].points.emplace_back(
pt.x, pt.y, pt.z, yaw_diff / kTwoPi + scan_id, std::nanf(""));
}
AINFO_IF(verbose_) << "Extractor::SplitScan: " << FMT_TIMESTAMP(timer.Toc())
<< " ms";
}
//
void Extractor::ComputePointCurvature(TCTPointCloud* const scan,
bool remove_nan) const {
void Extractor::ComputeCurvature(TCTPointCloud* const scan) const {
Timer timer;
if (scan->size() < 20) return;
auto& pts = scan->points;
for (size_t i = 5; i < pts.size() - 5; ++i) {
@ -90,12 +89,15 @@ void Extractor::ComputePointCurvature(TCTPointCloud* const scan,
pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z;
pts[i].curvature = std::sqrt(dx * dx + dy * dy + dz * dz);
}
RemovePointsIf<TCTPoint>(*scan, scan, [](const TCTPoint& pt) {
RemovePoints<TCTPoint>(*scan, scan, [](const TCTPoint& pt) {
return !std::isfinite(pt.curvature);
});
AINFO_IF(verbose_) << "Extractor::ComputeCurvature: "
<< FMT_TIMESTAMP(timer.Toc()) << " ms";
}
void Extractor::AssignPointType(TCTPointCloud* const scan) const {
void Extractor::AssignType(TCTPointCloud* const scan) const {
Timer timer;
int pt_num = scan->size();
ACHECK(pt_num >= kScanSegNum);
int seg_pt_num = (pt_num - 1) / kScanSegNum + 1;
@ -153,6 +155,50 @@ void Extractor::AssignPointType(TCTPointCloud* const scan) const {
}
}
}
AINFO_IF(verbose_) << "Extractor::AssignType: " << FMT_TIMESTAMP(timer.Toc())
<< " ms";
}
void Extractor::GenerateFeature(const TCTPointCloud& scan,
Feature* const feature) const {
Timer timer;
TPointCloudPtr cloud_less_flat_surf(new TPointCloud);
for (const auto& pt : scan.points) {
TPoint point(pt.x, pt, y, pt.z, pt.time);
switch (pt.type) {
case PointType::FLAT:
feature->cloud_flat_surf->points.emplace_back(point);
// no break: FLAT points are also LESS_FLAT
case PointType::LESS_FLAT:
cloud_less_flat_surf->points.emplace_back(point);
break;
case PointType::SHARP:
feature->cloud_sharp_corner->points.emplace_back(point);
// no break: SHARP points are also LESS_SHARP
case PointType::LESS_SHARP:
feature->cloud_less_sharp_corner->points.emplace_back(point);
break;
default:
// all the rest are also LESS_FLAT
cloud_less_flat_surf->points.emplace_back(pt.x, pt.y, pt.z, pt.time);
break;
}
}
TPointCloudPtr dowm_sampled(new TPointCloud);
VoxelDownSample<TPoint>(cloud_less_flat_surf, dowm_sampled.get(),
config_["downsample_voxel_size"].as<double>());
feature->cloud_less_flat_surf = dowm_sampled;
AINFO_IF(verbose_) << "Extractor::GenerateFeature: "
<< FMT_TIMESTAMP(timer.Toc()) << " ms";
}
void Extractor::Visualize(const PointCloudConstPtr& cloud,
const Feature& feature, double timestamp) {
std::shared_ptr<ExtractorVisFrame> frame(new ExtractorVisFrame);
frame->timestamp = timestamp;
frame->cloud = cloud;
frame->feature = feature;
visualizer_->Render(frame);
}
void Extractor::SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
@ -179,44 +225,4 @@ void Extractor::SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
}
}
void Extractor::GenerateFeaturePoints(const TCTPointCloud& scan,
FeaturePoints* const feature) const {
TPointCloudPtr less_flat_surf_pts(new TPointCloud);
for (const auto& pt : scan.points) {
switch (pt.type) {
case PointType::FLAT:
feature->flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, pt.time);
// no break: FLAT points are also LESS_FLAT
case PointType::LESS_FLAT:
less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, pt.time);
break;
case PointType::SHARP:
feature->sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z,
pt.time);
// no break: SHARP points are also LESS_SHARP
case PointType::LESS_SHARP:
feature->less_sharp_corner_pts->points.emplace_back(pt.x, pt.y, pt.z,
pt.time);
break;
default:
// all the rest are also LESS_FLAT
less_flat_surf_pts->points.emplace_back(pt.x, pt.y, pt.z, pt.time);
break;
}
}
TPointCloudPtr filtered_less_flat_surf_pts(new TPointCloud);
VoxelDownSample<TPoint>(less_flat_surf_pts, filtered_less_flat_surf_pts.get(),
config_["downsample_voxel_size"].as<double>());
*feature->less_flat_surf_pts += *filtered_less_flat_surf_pts;
}
void Extractor::Visualize(const PointCloud& cloud,
const FeaturePoints& feature_pts, double timestamp) {
std::shared_ptr<ExtractorVisFrame> frame(new ExtractorVisFrame);
frame->timestamp = timestamp;
frame->cloud = cloud.makeShared();
frame->feature_pts = feature_pts;
visualizer_->Render(frame);
}
} // namespace oh_my_loam

54
oh_my_loam/extractor/extractor.h Normal file
View File

@ -0,0 +1,54 @@
#pragma once
#include "base/feature.h"
#include "base/types.h"
#include "common/common.h"
#include "visualizer/extractor_visualizer.h"
namespace oh_my_loam {
class Extractor {
public:
Extractor() = default;
virtual ~Extractor() = default;
bool Init();
void Process(const common::PointCloudConstPtr& cloud, Feature* const feature);
int num_scans() const { return num_scans_; }
protected:
virtual int GetScanID(const Point& pt) const = 0;
YAML::Node config_;
virtual void SplitScan(const common::PointCloud& cloud,
std::vector<TCTPointCloud>* const scans) const;
virtual void ComputeCurvature(TCTPointCloud* const scan) const;
virtual void AssignType(TCTPointCloud* const scan) const;
virtual void GenerateFeature(const TCTPointCloud& scan,
Feature* const feature) const;
int num_scans_ = 0;
std::unique_ptr<ExtractorVisualizer> visualizer_{nullptr};
bool verbose_ = false;
private:
void Visualize(const common::PointCloudConstPtr& cloud,
const Feature& feature, double timestamp = 0.0);
void SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
std::vector<bool>* const picked) const;
bool is_vis_ = false;
DISALLOW_COPY_AND_ASSIGN(Extractor);
};
} // namespace oh_my_loam

0
src/extractor/extractor_VLP16.cc → oh_my_loam/extractor/extractor_VLP16.cc
View File

2
src/extractor/extractor_VLP16.h → oh_my_loam/extractor/extractor_VLP16.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "base_extractor.h"
#include "extractor.h"
namespace oh_my_loam {

0
src/mapper/CMakeLists.txt → oh_my_loam/mapper/CMakeLists.txt
View File

0
src/mapper/mapper.cc → oh_my_loam/mapper/mapper.cc
View File

2
src/mapper/mapper.h → oh_my_loam/mapper/mapper.h
View File

@ -16,7 +16,7 @@ class Mapper {
void Visualize();
TPointCloudPtr map_pts_;
Pose3D pose_curr2world_;
Pose3d pose_curr2world_;
bool is_initialized = false;
bool is_vis_ = false;

3
oh_my_loam/odometer/CMakeLists.txt Normal file
View File

@ -0,0 +1,3 @@
aux_source_directory(. SRC_FILES)
add_library(odometer SHARED ${SRC_FILES})

4
src/odometry/odometry.cc → oh_my_loam/odometer/odometer.cc
View File

@ -19,7 +19,7 @@ bool Odometry::Init(const YAML::Node& config) {
return true;
}
void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
void Odometry::Process(const FeaturePoints& feature, Pose3d* const pose) {
if (!is_initialized_) {
is_initialized_ = true;
UpdatePre(feature);
@ -64,7 +64,7 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
solver.AddPointPlanePair(pair, GetTime(pair.pt));
}
solver.Solve();
pose_curr2last_ = Pose3D(q, p);
pose_curr2last_ = Pose3d(q, p);
double time_solve = timer.toc();
AINFO << "Time elapsed (ms), pl_match = " << time_pl_match
<< ", pp_match = " << timer_pp_match - time_pl_match

7
src/odometry/odometry.h → oh_my_loam/odometer/odometer.h
View File

@ -3,7 +3,6 @@
#include "extractor/feature_points.h"
#include "visualizer/odometry_visualizer.h"
#include "common.h"
#include "helper/helper.h"
namespace oh_my_loam {
@ -15,7 +14,7 @@ class Odometry {
bool Init(const YAML::Node& config);
void Process(const FeaturePoints& feature, Pose3D* const pose);
void Process(const FeaturePoints& feature, Pose3d* const pose);
protected:
void UpdatePre(const FeaturePoints& feature);
@ -32,8 +31,8 @@ class Odometry {
const std::vector<PointLinePair>& pl_pairs,
const std::vector<PointPlanePair>& pp_pairs) const;
Pose3D pose_curr2world_;
Pose3D pose_curr2last_;
Pose3d pose_curr2world_;
Pose3d pose_curr2last_;
TPointCloudPtr surf_pts_pre_{nullptr};
TPointCloudPtr corn_pts_pre_{nullptr};

2
src/oh_my_loam.cc → oh_my_loam/oh_my_loam.cc
View File

@ -28,7 +28,7 @@ void OhMyLoam::Run(const PointCloud& cloud_in, double timestamp) {
RemoveOutliers(cloud_in, cloud.get());
FeaturePoints feature_points;
extractor_->Process(*cloud, &feature_points);
Pose3D pose;
Pose3d pose;
odometry_->Process(feature_points, &pose);
poses_.emplace_back(pose);
}

2
src/oh_my_loam.h → oh_my_loam/oh_my_loam.h
View File

@ -21,7 +21,7 @@ class OhMyLoam {
// remove outliers: nan and very close points
void RemoveOutliers(const PointCloud& cloud_in,
PointCloud* const cloud_out) const;
std::vector<Pose3D> poses_;
std::vector<Pose3d> poses_;
DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
};

0
src/solver/CMakeLists.txt → oh_my_loam/solver/CMakeLists.txt
View File

0
src/solver/cost_function.h → oh_my_loam/solver/cost_function.h
View File

0
src/solver/solver.cc → oh_my_loam/solver/solver.cc
View File

0
src/solver/solver.h → oh_my_loam/solver/solver.h
View File

0
src/visualizer/CMakeLists.txt → oh_my_loam/visualizer/CMakeLists.txt
View File

21
oh_my_loam/visualizer/extractor_visualizer.cc Normal file
View File

@ -0,0 +1,21 @@
#include "extractor_visualizer.h"
namespace oh_my_loam {
namespace {
using namespace common;
} // namespace
void ExtractorVisualizer::Draw() {
auto frame = GetCurrentFrame<ExtractorVisFrame>();
DrawPointCloud<Point>(frame.cloud, WHITE, "cloud_raw");
DrawPointCloud<TPoint>(frame.feature.cloud_less_flat_surf, CYAN,
"cloud_less_flat_surf");
DrawPointCloud<TPoint>(frame.feature.cloud_flat_surf, BLUE,
"cloud_flat_surf");
DrawPointCloud<TPoint>(frame.feature.cloud_less_sharp_corner, PURPLE,
"cloud_less_sharp_corner");
DrawPointCloud<TPoint>(frame.feature.cloud_sharp_corner, RED,
"cloud_sharp_corner");
};
} // namespace oh_my_loam

22
oh_my_loam/visualizer/extractor_visualizer.h Normal file
View File

@ -0,0 +1,22 @@
#pragma once
#include "base/feature.h"
#include "common/visualizer/lidar_visualizer.h"
namespace oh_my_loam {
struct ExtractorVisFrame : public common::LidarVisFrame {
Feature feature;
};
class ExtractorVisualizer : public common::LidarVisualizer {
public:
explicit ExtractorVisualizer(const std::string &name = "ExtractorVisualizer",
size_t max_history_size = 10)
: common::LidarVisualizer(name, max_history_size) {}
private:
void Draw() override;
};
} // namespace oh_my_loam

4
src/visualizer/odometry_visualizer.cc → oh_my_loam/visualizer/odometer_visualizer.cc
View File

@ -29,9 +29,9 @@ void OdometryVisualizer::Draw() {
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()));
Pose3D pose_inv = frame.pose_curr2world.Inv();
Pose3d pose_inv = frame.pose_curr2world.Inv();
for (const auto& pose : poses_) {
poses_n.emplace_back(pose_inv * pose);
};

10
src/visualizer/odometry_visualizer.h → oh_my_loam/visualizer/odometer_visualizer.h
View File

@ -1,17 +1,17 @@
#pragma once
#include "base_visualizer.h"
#include "common/visualizer/lidar_visualizer.h"
#include "helper/helper.h"
namespace oh_my_loam {
struct OdometryVisFrame : public VisFrame {
struct OdometerVisFrame : public VisFrame {
TPointCloudPtr surf_pts;
TPointCloudPtr corn_pts;
std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs;
Pose3D pose_curr2last;
Pose3D pose_curr2world;
Pose3d pose_curr2last;
Pose3d pose_curr2world;
};
class OdometryVisualizer : public Visualizer {
@ -22,7 +22,7 @@ class OdometryVisualizer : public Visualizer {
private:
void Draw() override;
std::deque<Pose3D> poses_;
std::deque<Pose3d> poses_;
};
} // namespace oh_my_loam

11
src/CMakeLists.txt
View File

@ -1,11 +0,0 @@
add_subdirectory(extractor)
add_subdirectory(visualizer)
add_subdirectory(odometry)
add_subdirectory(mapper)
add_subdirectory(helper)
add_subdirectory(solver)
aux_source_directory(. SRC_FILES)
add_library(oh_my_loam SHARED ${SRC_FILES})

50
src/extractor/base_extractor.h
View File

@ -1,50 +0,0 @@
#pragma once
#include "feature_points.h"
#include "visualizer/extractor_visualizer.h"
namespace oh_my_loam {
class Extractor {
public:
Extractor() = default;
virtual ~Extractor() = default;
bool Init(const YAML::Node& config);
void Process(const PointCloud& cloud, FeaturePoints* const feature);
int num_scans() const { return num_scans_; }
protected:
virtual int GetScanID(const Point& pt) const = 0;
int num_scans_ = 0;
YAML::Node config_;
private:
void Visualize(const PointCloud& cloud, const FeaturePoints& feature_pts,
double timestamp = std::nan(""));
void SplitScan(const PointCloud& cloud,
std::vector<TCTPointCloud>* const scans) const;
void ComputePointCurvature(TCTPointCloud* const scan,
bool remove_nan = true) const;
void AssignPointType(TCTPointCloud* const scan) const;
void SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
std::vector<bool>* const picked) const;
void GenerateFeaturePoints(const TCTPointCloud& scan,
FeaturePoints* const feature) const;
bool is_vis_ = false;
std::unique_ptr<ExtractorVisualizer> visualizer_{nullptr};
DISALLOW_COPY_AND_ASSIGN(Extractor);
};
} // namespace oh_my_loam

21
src/extractor/feature_points.h
View File

@ -1,21 +0,0 @@
#pragma once
#include "types.h"
namespace oh_my_loam {
struct FeaturePoints {
TPointCloudPtr sharp_corner_pts;
TPointCloudPtr less_sharp_corner_pts;
TPointCloudPtr flat_surf_pts;
TPointCloudPtr less_flat_surf_pts;
FeaturePoints() {
sharp_corner_pts.reset(new TPointCloud);
less_sharp_corner_pts.reset(new TPointCloud);
flat_surf_pts.reset(new TPointCloud);
less_flat_surf_pts.reset(new TPointCloud);
}
};
} // namespace oh_my_loam

3
src/helper/CMakeLists.txt
View File

@ -1,3 +0,0 @@
aux_source_directory(. SRC_FILES)
add_library(helper SHARED ${SRC_FILES})

3
src/odometry/CMakeLists.txt
View File

@ -1,3 +0,0 @@
aux_source_directory(. SRC_FILES)
add_library(odometry SHARED ${SRC_FILES})

18
src/visualizer/extractor_visualizer.cc
View File

@ -1,18 +0,0 @@
#include "extractor_visualizer.h"
namespace oh_my_loam {
void ExtractorVisualizer::Draw() {
auto frame = GetCurrentFrame<ExtractorVisFrame>();
DrawPointCloud<Point>(frame.cloud, WHITE, "raw_point_cloud");
DrawPointCloud<TPoint>(frame.feature_pts.less_flat_surf_pts, CYAN,
"less_flat_surf_pts");
DrawPointCloud<TPoint>(frame.feature_pts.flat_surf_pts, BLUE,
"flat_surf_pts");
DrawPointCloud<TPoint>(frame.feature_pts.less_sharp_corner_pts, PURPLE,
"less_sharp_corner_pts");
DrawPointCloud<TPoint>(frame.feature_pts.sharp_corner_pts, RED,
"sharp_corner_pts");
};
} // namespace oh_my_loam

22
src/visualizer/extractor_visualizer.h
View File

@ -1,22 +0,0 @@
#pragma once
#include "base_visualizer.h"
#include "extractor/feature_points.h"
namespace oh_my_loam {
struct ExtractorVisFrame : public VisFrame {
FeaturePoints feature_pts;
};
class ExtractorVisualizer : public Visualizer {
public:
explicit ExtractorVisualizer(const std::string &name = "ExtractorVisualizer",
size_t max_history_size = 10)
: Visualizer(name, max_history_size) {}
private:
void Draw() override;
};
} // namespace oh_my_loam