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

refactoring: ok

main
feixyz10 2021-01-05 14:33:42 +08:00 committed by feixyz
parent 6c544c56eb
commit a706cac8c5
22 changed files with 266 additions and 236 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
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,32 +32,32 @@ include_directories(SYSTEM
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
# 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 oh_my_loam common
)
include_directories(
${CMAKE_SOURCE_DIR}
${CMAKE_CURRENT_SOURCE_DIR}
)
add_subdirectory(common)
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
odometer
mapper
solver
${CERES_LIBRARIES}
visualizer
base
)

2
common/CMakeLists.txt
View File

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

4
common/pcl/pcl_utils.h
View File

@ -39,8 +39,8 @@ inline double IsFinite(const PointType& pt) {
// Remove point if the condition evaluated to true on it
template <typename PointType>
void RemovePoints(const pcl::PointCloud<PointType>& cloud_in,
std::function<bool(const PointType&)> check,
pcl::PointCloud<PointType>* const cloud_out) {
pcl::PointCloud<PointType>* const cloud_out,
std::function<bool(const PointType&)> check) {
if (&cloud_in != cloud_out) {
cloud_out->header = cloud_in.header;
cloud_out->resize(cloud_in.size());

13
common/time/timer.h
View File

@ -37,8 +37,15 @@ class TimerWrapper {
DISALLOW_COPY_AND_ASSIGN(TimerWrapper);
};
#define TIME_ELAPSED(msg) TimerWrapper(msg)
#define TIME_ELAPSED_EXPECT(msg, max_time_elapsed) \
TimerWrapper(msg, max_time_elapsed)
#define BLOCK_TIMER_START ::common::Timer __timer__
#define BLOCK_TIMER_STOP __timer__.Toc()
#define BLOCK_TIMER_STOP_FMT FMT_TIMESTAMP(__timer__.Toc()) << " ms"
#define BLOCK_TIME_ELAPSED(msg) ::common::TimerWrapper __timer_wrapper__(msg)
#define BLOCK_TIME_ELAPSED_EXPECT(msg, max_time) \
::common::TimerWrapper __timer_wrapper__(msg, max_time)
} // namespace common

5
common/visualizer/lidar_visualizer.h
View File

@ -5,6 +5,7 @@
#include <mutex>
#include <thread>
#include "common/macro/macros.h"
#include "lidar_visualizer_utils.h"
namespace common {
@ -54,7 +55,7 @@ class LidarVisualizer {
is_updated_ = true;
}
std::string Name() const { return name_; }
std::string name() const { return name_; }
protected:
void Run() {
@ -162,6 +163,8 @@ class LidarVisualizer {
// viewer
std::unique_ptr<PCLVisualizer> viewer_ = nullptr;
DISALLOW_COPY_AND_ASSIGN(LidarVisualizer);
};
} // namespace common

26
main.cc
View File

@ -3,25 +3,29 @@
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <filesystem>
#include <functional>
#include "common.h"
#include "src/oh_my_loam.h"
#include "common/common.h"
#include "oh_my_loam/oh_my_loam.h"
using namespace common;
using namespace oh_my_loam;
void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
OhMyLoam* const slam);
int main(int argc, char* argv[]) {
auto curr_path = std::filesystem::current_path();
// config
Config::Instance()->SetConfigFile("configs/config.yaml");
bool log_to_file = Config::Instance()->Get<bool>("log_to_file");
std::string log_path = Config::Instance()->Get<std::string>("log_path");
std::string lidar = Config::Instance()->Get<std::string>("lidar");
YAMLConfig::Instance()->Init(
(curr_path / "oh_my_loam/configs/config.yaml").string());
bool log_to_file = YAMLConfig::Instance()->Get<bool>("log_to_file");
std::string log_path = YAMLConfig::Instance()->Get<std::string>("log_path");
std::string lidar = YAMLConfig::Instance()->Get<std::string>("lidar");
// logging
g3::InitG3Logging(log_to_file, "oh_my_loam_" + lidar, log_path);
InitG3Logging(log_to_file, "oh_my_loam_" + lidar, log_path);
AUSER << "LOAM start..., lidar = " << lidar;
// SLAM system
@ -43,9 +47,9 @@ int main(int argc, char* argv[]) {
void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
OhMyLoam* const slam) {
PointCloud cloud;
pcl::fromROSMsg(*msg, cloud);
PointCloudPtr cloud(new PointCloud);
pcl::fromROSMsg(*msg, *cloud);
double timestamp = msg->header.stamp.toSec();
AINFO << "Timestamp = " << LOG_TIMESTAMP(timestamp);
slam->Run(cloud, timestamp);
AUSER << "Timestamp: " << FMT_TIMESTAMP(timestamp);
slam->Run(timestamp, cloud);
}

18
oh_my_loam/CMakeLists.txt
View File

@ -1,17 +1,11 @@
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)
add_subdirectory(visualizer)
add_subdirectory(odometer)
add_subdirectory(mapper)
add_subdirectory(solver)
# aux_source_directory(. SRC_FILES)
aux_source_directory(. SRC_FILES)
# add_library(oh_my_loam SHARED ${SRC_FILES})
add_library(oh_my_loam SHARED ${SRC_FILES})

12
oh_my_loam/base/helper.cc
View File

@ -2,6 +2,16 @@
namespace oh_my_loam {
//
void TransformToStart(const Pose3d& pose, const TPoint& pt_in,
TPoint* const pt_out) {
Pose3d pose_interp = Pose3d().Interpolate(pose, GetTime(pt_in));
TransformPoint<TPoint>(pose_interp, pt_in, pt_out);
}
void TransformToEnd(const Pose3d& pose, const TPoint& pt_in,
TPoint* const pt_out) {
TransformToStart(pose, pt_in, pt_out);
TransformPoint<TPoint>(pose.Inv(), *pt_out, pt_out);
}
} // namespace oh_my_loam

13
oh_my_loam/base/helper.h
View File

@ -27,25 +27,16 @@ void TransformPoint(const Pose3d& pose, const PointType& pt_in,
* @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,
TPoint* const pt_out) {
Pose3d pose_interp = Pose3d().Interpolate(pose, GetTime(pt_in));
TransformPoint<TPoint>(pose_interp, pt_in, pt_out);
}
TPoint* const pt_out);
/**
* @brief Transform a lidar point to the end 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 TransformToEnd(const Pose3d& pose, const TPoint& pt_in,
TPoint* const pt_out) {
TransformToStart(pose, pt_in, pt_out);
TransformPoint<TPoint>(pose.Inv(), *pt_out, pt_out);
}
TPoint* const pt_out);
struct PointLinePair {
TPoint pt;

11
oh_my_loam/configs/config.yaml
View File

@ -6,9 +6,9 @@ vis: true
# configs for extractor
extractor_config:
min_point_num: 66
vis: false
verbose: false
verbose: true
min_point_num: 66
sharp_corner_point_num: 2
corner_point_num: 20
flat_surf_point_num: 4
@ -18,11 +18,12 @@ extractor_config:
neighbor_point_dist_thres: 0.05
downsample_voxel_size: 0.3
# configs for odometry
odometry_config:
# configs for odometer
odometer_config:
vis: true
verbose: false
icp_iter_num : 2
match_dist_sq_thresh: 1
vis: true
# configs for mapper
mapper_config:

33
oh_my_loam/extractor/extractor.cc
View File

@ -2,7 +2,6 @@
#include <cmath>
#include "base/helper.h"
#include "common/pcl/pcl_utils.h"
namespace oh_my_loam {
@ -25,7 +24,7 @@ bool Extractor::Init() {
void Extractor::Process(const PointCloudConstPtr& cloud,
Feature* const feature) {
TIME_ELAPSED(__PRETTY_FUNCTION__);
BLOCK_TIMER_START;
if (cloud->size() < config_["min_point_num"].as<size_t>()) {
AWARN << "Too few input points: num = " << cloud->size() << " (< "
<< config_["min_point_num"].as<int>() << ")";
@ -34,25 +33,28 @@ void Extractor::Process(const PointCloudConstPtr& cloud,
// split point cloud int scans
std::vector<TCTPointCloud> scans;
SplitScan(*cloud, &scans);
AINFO_IF(verbose_) << "Extractor::SplitScan: " << BLOCK_TIMER_STOP_FMT;
// compute curvature for each point in each scan
for (auto& scan : scans) {
ComputeCurvature(&scan);
}
AINFO_IF(verbose_) << "Extractor::ComputeCurvature: " << BLOCK_TIMER_STOP_FMT;
// assign type to each point in each scan: FLAT, LESS_FLAT,
// NORMAL, LESS_SHARP or SHARP
for (auto& scan : scans) {
AssignType(&scan);
}
AINFO_IF(verbose_) << "Extractor::AssignType: " << BLOCK_TIMER_STOP_FMT;
// store points into feature point clouds according to their type
for (const auto& scan : scans) {
GenerateFeature(scan, feature);
}
if (is_vis_) Visualize(cloud, *feature);
AINFO << "Extractor::Process: " << BLOCK_TIMER_STOP_FMT;
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;
@ -70,12 +72,9 @@ void Extractor::SplitScan(const PointCloud& cloud,
(*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::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,15 +89,12 @@ void Extractor::ComputeCurvature(TCTPointCloud* const scan) const {
pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z;
pts[i].curvature = std::sqrt(dx * dx + dy * dy + dz * dz);
}
RemovePoints<TCTPoint>(
*scan, [](const TCTPoint& pt) { return !std::isfinite(pt.curvature); },
scan);
AINFO_IF(verbose_) << "Extractor::ComputeCurvature: "
<< FMT_TIMESTAMP(timer.Toc()) << " ms";
RemovePoints<TCTPoint>(*scan, scan, [](const TCTPoint& pt) {
return !std::isfinite(pt.curvature);
});
}
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;
@ -156,13 +152,10 @@ void Extractor::AssignType(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);
@ -189,15 +182,13 @@ void Extractor::GenerateFeature(const TCTPointCloud& scan,
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) {
void Extractor::Visualize(const PointCloud& cloud, const Feature& feature,
double timestamp) {
std::shared_ptr<ExtractorVisFrame> frame(new ExtractorVisFrame);
frame->timestamp = timestamp;
frame->cloud = cloud->makeShared();
frame->cloud = cloud.makeShared();
frame->feature = feature;
visualizer_->Render(frame);
}

11
oh_my_loam/extractor/extractor.h
View File

@ -1,9 +1,10 @@
#pragma once
#include "base/feature.h"
#include "base/types.h"
#include "common/common.h"
#include "visualizer/extractor_visualizer.h"
#include "oh_my_loam/base/feature.h"
#include "oh_my_loam/base/helper.h"
#include "oh_my_loam/base/types.h"
#include "oh_my_loam/visualizer/extractor_visualizer.h"
namespace oh_my_loam {
@ -40,8 +41,8 @@ class Extractor {
bool verbose_ = false;
private:
void Visualize(const common::PointCloudConstPtr& cloud,
const Feature& feature, double timestamp = 0.0);
void Visualize(const common::PointCloud& cloud, const Feature& feature,
double timestamp = 0.0);
void SetNeighborsPicked(const TCTPointCloud& scan, size_t ix,
std::vector<bool>* const picked) const;

12
oh_my_loam/mapper/mapper.cc
View File

@ -2,9 +2,15 @@
namespace oh_my_loam {
bool Mapper::Init(const YAML::Node& config) {
config_ = config;
is_vis_ = Config::Instance()->Get<bool>("vis") && config_["vis"].as<bool>();
namespace {
using namespace common;
} // namespace
bool Mapper::Init() {
const auto& config = YAMLConfig::Instance()->config();
config_ = config["mapper_config"];
is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
verbose_ = config_["vis"].as<bool>();
AINFO << "Mapping visualizer: " << (is_vis_ ? "ON" : "OFF");
return true;
}

13
oh_my_loam/mapper/mapper.h
View File

@ -1,6 +1,8 @@
#pragma once
#include "common.h"
#include "common/common.h"
#include "common/geometry/pose.h"
#include "common/pcl/pcl_types.h"
namespace oh_my_loam {
@ -8,19 +10,22 @@ class Mapper {
public:
Mapper() = default;
bool Init(const YAML::Node& config);
bool Init();
void Process();
private:
void Visualize();
TPointCloudPtr map_pts_;
Pose3d pose_curr2world_;
common::TPointCloudPtr map_pts_;
common::Pose3d pose_curr2world_;
bool is_initialized = false;
bool is_vis_ = false;
bool verbose_ = false;
YAML::Node config_;
DISALLOW_COPY_AND_ASSIGN(Mapper)

105
oh_my_loam/odometer/odometer.cc
View File

@ -1,51 +1,49 @@
#include "odometry.h"
#include "odometer.h"
#include "solver/solver.h"
#include "common/pcl/pcl_utils.h"
#include "oh_my_loam/solver/solver.h"
namespace oh_my_loam {
namespace {
int kNearbyScanNum = 2;
size_t kMinMatchNum = 10;
using namespace common;
} // namespace
bool Odometry::Init(const YAML::Node& config) {
config_ = config;
kdtree_surf_pts_.reset(new pcl::KdTreeFLANN<TPoint>);
kdtree_corn_pts_.reset(new pcl::KdTreeFLANN<TPoint>);
is_vis_ = Config::Instance()->Get<bool>("vis") && config_["vis"].as<bool>();
AINFO << "Odometry visualizer: " << (is_vis_ ? "ON" : "OFF");
if (is_vis_) visualizer_.reset(new OdometryVisualizer);
bool Odometer::Init() {
const auto& config = YAMLConfig::Instance()->config();
config_ = config["odometer_config"];
is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
verbose_ = config_["verbose"].as<bool>();
kdtree_surf_.reset(new pcl::KdTreeFLANN<TPoint>);
kdtree_corn_.reset(new pcl::KdTreeFLANN<TPoint>);
AINFO << "Odometer visualizer: " << (is_vis_ ? "ON" : "OFF");
if (is_vis_) visualizer_.reset(new OdometerVisualizer);
return true;
}
void Odometry::Process(const FeaturePoints& feature, Pose3d* const pose) {
void Odometer::Process(const Feature& feature, Pose3d* const pose) {
BLOCK_TIMER_START;
if (!is_initialized_) {
is_initialized_ = true;
UpdatePre(feature);
*pose = pose_curr2world_;
AWARN << "Odometry initialized....";
*pose = Pose3d();
AWARN << "Odometer initialized....";
return;
}
TicToc timer_whole;
double match_dist_sq_thresh = config_["match_dist_sq_thresh"].as<double>();
AINFO << "Pose before: " << pose_curr2world_.ToString();
std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs;
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
TicToc timer;
MatchCornPoints(*feature.sharp_corner_pts, *corn_pts_pre_, &pl_pairs,
match_dist_sq_thresh);
double time_pl_match = timer.toc();
MatchCornFeature(*feature.cloud_sharp_corner, *corn_pre_, &pl_pairs);
AINFO_IF(i == 0) << "PL mathch: src size = "
<< feature.sharp_corner_pts->size()
<< ", tgt size = " << corn_pts_pre_->size();
MatchSurfPoints(*feature.flat_surf_pts, *surf_pts_pre_, &pp_pairs,
match_dist_sq_thresh);
double timer_pp_match = timer.toc();
<< feature.cloud_sharp_corner->size()
<< ", tgt size = " << corn_pre_->size();
MatchSurfFeature(*feature.cloud_flat_surf, *surf_pre_, &pp_pairs);
AINFO_IF(i == 0) << "PP mathch: src size = "
<< feature.flat_surf_pts->size()
<< ", tgt size = " << surf_pts_pre_->size();
<< feature.cloud_flat_surf->size()
<< ", tgt size = " << surf_pre_->size();
AINFO << "Matched num, pl = " << pl_pairs.size()
<< ", pp = " << pp_pairs.size();
if (pl_pairs.size() + pp_pairs.size() < kMinMatchNum) {
@ -65,29 +63,25 @@ void Odometry::Process(const FeaturePoints& feature, Pose3d* const pose) {
}
solver.Solve();
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
<< ", solve = " << time_solve - timer_pp_match;
}
pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
*pose = pose_curr2world_;
AWARN << "Pose increase: " << pose_curr2last_.ToString();
AWARN << "Pose after: " << pose_curr2world_.ToString();
UpdatePre(feature);
AUSER << "Time elapsed (ms): whole odometry = " << timer_whole.toc();
AINFO << "Odometer::Porcess: " << BLOCK_TIMER_STOP_FMT;
if (is_vis_) Visualize(feature, pl_pairs, pp_pairs);
}
void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointLinePair>* const pairs,
double dist_sq_thresh) const {
void Odometer::MatchCornFeature(const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointLinePair>* const pairs) const {
double dist_sq_thresh = config_["match_dist_sq_thresh"].as<double>();
for (const auto& q_pt : src) {
TPoint query_pt;
TransformToStart(pose_curr2last_, q_pt, &query_pt);
std::vector<int> indices;
std::vector<float> dists;
kdtree_corn_pts_->nearestKSearch(query_pt, 1, indices, dists);
kdtree_corn_->nearestKSearch(query_pt, 1, indices, dists);
if (dists[0] >= dist_sq_thresh) continue;
TPoint pt1 = tgt.points[indices[0]];
int pt2_idx = -1;
@ -122,15 +116,16 @@ void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
}
}
void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointPlanePair>* const pairs,
double dist_sq_thresh) const {
void Odometer::MatchSurfFeature(
const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointPlanePair>* const pairs) const {
double dist_sq_thresh = config_["match_dist_sq_thresh"].as<double>();
for (const auto& q_pt : src) {
TPoint query_pt;
TransformToStart(pose_curr2last_, q_pt, &query_pt);
std::vector<int> indices;
std::vector<float> dists;
kdtree_surf_pts_->nearestKSearch(query_pt, 1, indices, dists);
kdtree_surf_->nearestKSearch(query_pt, 1, indices, dists);
if (dists[0] >= dist_sq_thresh) continue;
TPoint pt1 = tgt.points[indices[0]];
int pt2_idx = -1, pt3_idx = -1;
@ -174,32 +169,32 @@ void Odometry::MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
}
}
void Odometry::UpdatePre(const FeaturePoints& feature) {
if (surf_pts_pre_ == nullptr) {
surf_pts_pre_.reset(new TPointCloud);
void Odometer::UpdatePre(const Feature& feature) {
if (!surf_pre_) {
surf_pre_.reset(new TPointCloud);
}
if (corn_pts_pre_ == nullptr) {
corn_pts_pre_.reset(new TPointCloud);
if (!corn_pre_) {
corn_pre_.reset(new TPointCloud);
}
surf_pts_pre_->resize(feature.less_flat_surf_pts->size());
for (size_t i = 0; i < feature.less_flat_surf_pts->size(); ++i) {
TransformToEnd(pose_curr2last_, feature.less_flat_surf_pts->points[i],
&surf_pts_pre_->points[i]);
surf_pre_->resize(feature.cloud_less_flat_surf->size());
for (size_t i = 0; i < feature.cloud_less_flat_surf->size(); ++i) {
TransformToEnd(pose_curr2last_, feature.cloud_less_flat_surf->points[i],
&surf_pre_->points[i]);
}
corn_pts_pre_->resize(feature.less_sharp_corner_pts->size());
for (size_t i = 0; i < feature.less_sharp_corner_pts->size(); ++i) {
TransformToEnd(pose_curr2last_, feature.less_sharp_corner_pts->points[i],
&corn_pts_pre_->points[i]);
corn_pre_->resize(feature.cloud_less_sharp_corner->size());
for (size_t i = 0; i < feature.cloud_less_sharp_corner->size(); ++i) {
TransformToEnd(pose_curr2last_, feature.cloud_less_sharp_corner->points[i],
&corn_pre_->points[i]);
}
kdtree_surf_pts_->setInputCloud(surf_pts_pre_);
kdtree_corn_pts_->setInputCloud(corn_pts_pre_);
kdtree_surf_->setInputCloud(surf_pre_);
kdtree_corn_->setInputCloud(corn_pre_);
}
void Odometry::Visualize(const FeaturePoints& feature,
void Odometer::Visualize(const Feature& feature,
const std::vector<PointLinePair>& pl_pairs,
const std::vector<PointPlanePair>& pp_pairs) const {
std::shared_ptr<OdometryVisFrame> frame(new OdometryVisFrame);
std::shared_ptr<OdometerVisFrame> frame(new OdometerVisFrame);
frame->pl_pairs = pl_pairs;
frame->pp_pairs = pp_pairs;
frame->pose_curr2last = pose_curr2last_;

59
oh_my_loam/odometer/odometer.h
View File

@ -1,52 +1,57 @@
#pragma once
#include "extractor/feature_points.h"
#include "visualizer/odometry_visualizer.h"
#include "helper/helper.h"
#include "common/common.h"
#include "common/geometry/pose.h"
#include "oh_my_loam/base/feature.h"
#include "oh_my_loam/base/helper.h"
#include "oh_my_loam/visualizer/odometer_visualizer.h"
namespace oh_my_loam {
// frame to frame lidar odometry
class Odometry {
// frame to frame lidar Odometer
class Odometer {
public:
Odometry() = default;
Odometer() = default;
bool Init(const YAML::Node& config);
bool Init();
void Process(const FeaturePoints& feature, Pose3d* const pose);
void Process(const Feature& feature, common::Pose3d* const pose);
protected:
void UpdatePre(const FeaturePoints& feature);
void UpdatePre(const Feature& feature);
void MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointLinePair>* const pairs,
double dist_sq_thresh) const;
void MatchCornFeature(const common::TPointCloud& src,
const common::TPointCloud& tgt,
std::vector<PointLinePair>* const pairs) const;
void MatchSurfPoints(const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointPlanePair>* const pairs,
double dist_sq_thresh) const;
void MatchSurfFeature(const common::TPointCloud& src,
const common::TPointCloud& tgt,
std::vector<PointPlanePair>* const pairs) const;
void Visualize(const FeaturePoints& feature,
void Visualize(const Feature& feature,
const std::vector<PointLinePair>& pl_pairs,
const std::vector<PointPlanePair>& pp_pairs) const;
Pose3d pose_curr2world_;
Pose3d pose_curr2last_;
common::Pose3d pose_curr2world_;
common::Pose3d pose_curr2last_;
TPointCloudPtr surf_pts_pre_{nullptr};
TPointCloudPtr corn_pts_pre_{nullptr};
common::TPointCloudPtr corn_pre_{nullptr};
common::TPointCloudPtr surf_pre_{nullptr};
pcl::KdTreeFLANN<TPoint>::Ptr kdtree_surf_pts_{nullptr};
pcl::KdTreeFLANN<TPoint>::Ptr kdtree_corn_pts_{nullptr};
pcl::KdTreeFLANN<common::TPoint>::Ptr kdtree_surf_{nullptr};
pcl::KdTreeFLANN<common::TPoint>::Ptr kdtree_corn_{nullptr};
bool is_initialized_ = false;
bool is_vis_ = false;
YAML::Node config_;
std::unique_ptr<OdometryVisualizer> visualizer_{nullptr};
bool is_initialized_ = false;
DISALLOW_COPY_AND_ASSIGN(Odometry)
bool is_vis_ = false;
bool verbose_ = false;
std::unique_ptr<OdometerVisualizer> visualizer_{nullptr};
DISALLOW_COPY_AND_ASSIGN(Odometer)
};
} // namespace oh_my_loam

35
oh_my_loam/oh_my_loam.cc
View File

@ -1,42 +1,51 @@
#include "oh_my_loam.h"
#include "extractor/extractor_VLP16.h"
#include "common/pcl/pcl_utils.h"
#include "oh_my_loam/extractor/extractor_VLP16.h"
namespace oh_my_loam {
namespace {
const double kPointMinDist = 0.1;
using namespace common;
}
bool OhMyLoam::Init() {
YAML::Node config = Config::Instance()->config();
YAML::Node config = YAMLConfig::Instance()->config();
extractor_.reset(new ExtractorVLP16);
if (!extractor_->Init(config["extractor_config"])) {
if (!extractor_->Init()) {
AERROR << "Failed to initialize extractor";
return false;
}
odometry_.reset(new Odometry);
if (!odometry_->Init(config["odometry_config"])) {
AERROR << "Failed to initialize odometry";
odometer_.reset(new Odometer);
if (!odometer_->Init()) {
AERROR << "Failed to initialize odometer";
return false;
}
mapper_.reset(new Mapper);
if (!mapper_->Init()) {
AERROR << "Failed to initialize mapper";
return false;
}
return true;
}
void OhMyLoam::Run(const PointCloud& cloud_in, double timestamp) {
void OhMyLoam::Run(double timestamp, const PointCloudConstPtr& cloud_in) {
PointCloudPtr cloud(new PointCloud);
RemoveOutliers(cloud_in, cloud.get());
FeaturePoints feature_points;
extractor_->Process(*cloud, &feature_points);
RemoveOutliers(*cloud_in, cloud.get());
Feature feature;
extractor_->Process(cloud, &feature);
Pose3d pose;
odometry_->Process(feature_points, &pose);
odometer_->Process(feature, &pose);
poses_.emplace_back(pose);
}
void OhMyLoam::RemoveOutliers(const PointCloud& cloud_in,
PointCloud* const cloud_out) const {
RemoveNaNPoint<Point>(cloud_in, cloud_out);
RemoveClosedPoints<Point>(*cloud_out, cloud_out, kPointMinDist);
RemovePoints<Point>(cloud_in, cloud_out, [&](const Point& pt) {
return !IsFinite<Point>(pt) ||
DistanceSqure<Point>(pt) < kPointMinDist * kPointMinDist;
});
}
} // namespace oh_my_loam

20
oh_my_loam/oh_my_loam.h
View File

@ -1,8 +1,9 @@
#pragma once
#include "common.h"
#include "extractor/base_extractor.h"
#include "odometry/odometry.h"
#include "common/common.h"
#include "oh_my_loam/extractor/extractor.h"
#include "oh_my_loam/mapper/mapper.h"
#include "oh_my_loam/odometer/odometer.h"
namespace oh_my_loam {
@ -12,16 +13,19 @@ class OhMyLoam {
bool Init();
void Run(const PointCloud& cloud_in, double timestamp);
void Run(double timestamp, const common::PointCloudConstPtr& cloud_in);
private:
std::unique_ptr<Extractor> extractor_{nullptr};
std::unique_ptr<Odometry> odometry_{nullptr};
std::unique_ptr<Odometer> odometer_{nullptr};
std::unique_ptr<Mapper> mapper_{nullptr};
// remove outliers: nan and very close points
void RemoveOutliers(const PointCloud& cloud_in,
PointCloud* const cloud_out) const;
std::vector<Pose3d> poses_;
void RemoveOutliers(const common::PointCloud& cloud_in,
common::PointCloud* const cloud_out) const;
std::vector<common::Pose3d> poses_;
DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
};

7
oh_my_loam/solver/cost_function.h
View File

@ -1,8 +1,9 @@
#pragma once
#include "ceres/ceres.h"
#include "common.h"
#include "helper/helper.h"
#include <ceres/ceres.h>
#include "common/common.h"
#include "oh_my_loam/base/helper.h"
namespace oh_my_loam {

2
oh_my_loam/visualizer/extractor_visualizer.h
View File

@ -1,7 +1,7 @@
#pragma once
#include "base/feature.h"
#include "common/visualizer/lidar_visualizer.h"
#include "oh_my_loam/base/feature.h"
namespace oh_my_loam {

10
oh_my_loam/visualizer/odometer_visualizer.cc
View File

@ -1,9 +1,13 @@
#include "odometry_visualizer.h"
#include "odometer_visualizer.h"
namespace oh_my_loam {
void OdometryVisualizer::Draw() {
auto frame = GetCurrentFrame<OdometryVisFrame>();
namespace {
using namespace common;
} // namespace
void OdometerVisualizer::Draw() {
auto frame = GetCurrentFrame<OdometerVisFrame>();
TPointCloudPtr src_corn_pts(new TPointCloud);
TPointCloudPtr tgt_corn_pts(new TPointCloud);
src_corn_pts->resize(frame.pl_pairs.size());

21
oh_my_loam/visualizer/odometer_visualizer.h
View File

@ -1,28 +1,29 @@
#pragma once
#include "common/visualizer/lidar_visualizer.h"
#include "helper/helper.h"
#include "oh_my_loam/base/helper.h"
namespace oh_my_loam {
struct OdometerVisFrame : public VisFrame {
TPointCloudPtr surf_pts;
TPointCloudPtr corn_pts;
struct OdometerVisFrame : public common::LidarVisFrame {
common::TPointCloudPtr surf_pts;
common::TPointCloudPtr corn_pts;
std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs;
Pose3d pose_curr2last;
Pose3d pose_curr2world;
common::Pose3d pose_curr2last;
common::Pose3d pose_curr2world;
};
class OdometryVisualizer : public Visualizer {
class OdometerVisualizer : public common::LidarVisualizer {
public:
explicit OdometryVisualizer(const std::string &name = "OdometryVisualizer",
explicit OdometerVisualizer(const std::string &name = "OdometerVisualizer",
size_t max_history_size = 10)
: Visualizer(name, max_history_size) {}
: common::LidarVisualizer(name, max_history_size) {}
private:
void Draw() override;
std::deque<Pose3d> poses_;
std::deque<common::Pose3d> poses_;
};
} // namespace oh_my_loam