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

move PointXYZT from common to oh_my_loam

main
feixyz10 2021-01-22 15:53:21 +08:00 committed by feixyz
parent 3c14de4bce
commit 76dfa7f447
12 changed files with 179 additions and 176 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.gitignore vendored
View File

@ -2,3 +2,7 @@
/.log
*.pcd
/build
/.clangd
compile_commands.json
.clang-tidy
.clang-format

6
CMakeLists.txt
View File

@ -18,9 +18,6 @@ find_package(catkin REQUIRED COMPONENTS
rospy
rosbag
std_msgs
image_transport
cv_bridge
tf
)
include_directories(SYSTEM
@ -29,9 +26,6 @@ include_directories(SYSTEM
${G3LOG_INCLUDE_DIRS}
)
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
catkin_package(
CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
DEPENDS EIGEN3 PCL

58
common/pcl/pcl_types.h
View File

@ -17,60 +17,12 @@ 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
};
using Point2D = pcl::PointXY;
using PointCloud2D = pcl::PointCloud<Point2D>;
using PointCloud2DPtr = PointCloud2D::Ptr;
using PointCloud2DConstPtr = PointCloud2D::ConstPtr;
} // 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

18
oh_my_loam/base/feature.h
View File

@ -1,20 +1,20 @@
#pragma once
#include "common/pcl/pcl_types.h"
#include "oh_my_loam/base/types.h"
namespace oh_my_loam {
struct Feature {
common::TPointCloudPtr cloud_corner;
common::TPointCloudPtr cloud_sharp_corner;
common::TPointCloudPtr cloud_surf;
common::TPointCloudPtr cloud_flat_surf;
TPointCloudPtr cloud_corner;
TPointCloudPtr cloud_sharp_corner;
TPointCloudPtr cloud_surf;
TPointCloudPtr cloud_flat_surf;
Feature() {
cloud_corner.reset(new common::TPointCloud);
cloud_sharp_corner.reset(new common::TPointCloud);
cloud_surf.reset(new common::TPointCloud);
cloud_flat_surf.reset(new common::TPointCloud);
cloud_corner.reset(new TPointCloud);
cloud_sharp_corner.reset(new TPointCloud);
cloud_surf.reset(new TPointCloud);
cloud_flat_surf.reset(new TPointCloud);
}
};

2
oh_my_loam/base/helper.cc
View File

@ -4,7 +4,7 @@ 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));
Pose3d pose_interp = Pose3d().Interpolate(pose, pt_in.time);
TransformPoint<TPoint>(pose_interp, pt_in, pt_out);
}

7
oh_my_loam/base/helper.h
View File

@ -1,17 +1,12 @@
#pragma once
#include "common/geometry/pose3d.h"
#include "common/pcl/pcl_types.h"
#include "oh_my_loam/base/types.h"
namespace oh_my_loam {
using common::TPoint;
using common::Pose3d;
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) {

54
oh_my_loam/base/types.h
View File

@ -12,12 +12,55 @@ enum class PType {
SHARP_CORNER = 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 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 common::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
};
struct PointXYZTCT {
PCL_ADD_POINT4D;
union EIGEN_ALIGN16 {
@ -52,7 +95,7 @@ struct PointXYZTCT {
type = PType::NORNAL;
}
PointXYZTCT(const common::TPoint& p) {
PointXYZTCT(const TPoint &p) {
x = p.x;
y = p.y;
z = p.z;
@ -78,6 +121,15 @@ struct PointXYZTCT {
} // 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)
(float, intensity, intensity)
)
POINT_CLOUD_REGISTER_POINT_STRUCT(
oh_my_loam::PointXYZTCT,
(float, x, x)

4
oh_my_loam/mapper/mapper.h
View File

@ -2,7 +2,7 @@
#include "common/common.h"
#include "common/geometry/pose.h"
#include "common/pcl/pcl_types.h"
#include "oh_my_loam/base/types.h"
namespace oh_my_loam {
@ -17,7 +17,7 @@ class Mapper {
private:
void Visualize();
common::TPointCloudPtr map_pts_;
TPointCloudPtr map_pts_;
common::Pose3d pose_curr2world_;
bool is_initialized = false;

44
oh_my_loam/odometer/odometer.cc
View File

@ -27,17 +27,19 @@ void Odometer::Process(double timestamp, const std::vector<Feature>& features,
if (!is_initialized_) {
UpdatePre(features);
is_initialized_ = true;
pose_curr2last_ = Pose3d::Identity();
pose_curr2world_ = Pose3d::Identity();
*pose = Pose3d::Identity();
AWARN << "Odometer initialized....";
return;
}
AINFO << "Pose before: " << pose_curr2world_.ToString();
AINFO_IF(verbose_) << "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) {
for (const auto &feature : features) {
MatchCorn(*feature.cloud_sharp_corner, &pl_pairs);
// MatchSurf(*feature.cloud_flat_surf, &pp_pairs);
MatchSurf(*feature.cloud_flat_surf, &pp_pairs);
}
AINFO_IF(verbose_) << "Iter " << i
<< ": matched num: point2line = " << pl_pairs.size()
@ -45,6 +47,7 @@ void Odometer::Process(double timestamp, const std::vector<Feature>& features,
if (pl_pairs.size() + pp_pairs.size() < kMinMatchNum) {
AWARN << "Too less correspondence: " << pl_pairs.size() << " + "
<< pp_pairs.size();
continue;
}
PoseSolver solver(pose_curr2last_);
for (const auto &pair : pl_pairs) {
@ -57,8 +60,8 @@ void Odometer::Process(double timestamp, const std::vector<Feature>& features,
}
pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
*pose = pose_curr2world_;
AWARN_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString();
AWARN_IF(verbose_) << "Pose after: " << pose_curr2world_.ToString();
AINFO_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString();
AINFO_IF(verbose_) << "Pose after: " << pose_curr2world_.ToString();
UpdatePre(features);
if (is_vis_) Visualize(features, pl_pairs, pp_pairs);
AINFO << "Odometer::Porcess: " << BLOCK_TIMER_STOP_FMT;
@ -90,7 +93,7 @@ void Odometer::MatchCorn(const TPointCloud& src,
min_dist_pt2_squre = dists[i][0];
}
}
if (pt2_scan_id > 0) {
if (pt2_scan_id >= 0) {
TPoint pt2 = clouds_corn_pre_[pt2_scan_id]->at(indices[pt2_scan_id][0]);
pairs->emplace_back(pt, pt1, pt2);
}
@ -125,7 +128,7 @@ void Odometer::MatchSurf(const TPointCloud& src,
min_dist_pt3_squre = dists[i][0];
}
}
if (pt3_scan_id > 0) {
if (pt3_scan_id >= 0) {
TPoint pt3 = clouds_surf_pre_[pt3_scan_id]->at(indices[pt3_scan_id][0]);
pairs->emplace_back(pt, pt1, pt2, pt3);
}
@ -133,47 +136,52 @@ void Odometer::MatchSurf(const TPointCloud& src,
}
void Odometer::UpdatePre(const std::vector<Feature> &features) {
clouds_corn_pre_.resize(features.size(), common::TPointCloud().makeShared());
clouds_surf_pre_.resize(features.size(), common::TPointCloud().makeShared());
BLOCK_TIMER_START;
clouds_corn_pre_.resize(features.size());
clouds_surf_pre_.resize(features.size());
kdtrees_corn_.resize(features.size());
kdtrees_surf_.resize(features.size());
for (size_t i = 0; i < features.size(); ++i) {
const auto &feature = features[i];
auto &cloud_pre = clouds_corn_pre_[i];
if (!cloud_pre) cloud_pre.reset(new TPointCloud);
cloud_pre->resize(feature.cloud_corner->size());
for (size_t j = 0; j < feature.cloud_corner->size(); ++j) {
TransformToEnd(pose_curr2last_, feature.cloud_corner->at(j),
&cloud_pre->at(j));
}
kdtrees_corn_[i].setInputCloud(clouds_corn_pre_[i]);
kdtrees_corn_[i].setInputCloud(cloud_pre);
}
for (size_t i = 0; i < features.size(); ++i) {
const auto &feature = features[i];
auto &cloud_pre = clouds_surf_pre_[i];
if (!cloud_pre) cloud_pre.reset(new TPointCloud);
cloud_pre->resize(feature.cloud_surf->size());
for (size_t j = 0; j < feature.cloud_surf->size(); ++j) {
TransformToEnd(pose_curr2last_, feature.cloud_surf->at(j),
&cloud_pre->at(j));
}
kdtrees_surf_[i].setInputCloud(clouds_surf_pre_[i]);
kdtrees_surf_[i].setInputCloud(cloud_pre);
}
AINFO << "Odometer::UpdatePre: " << BLOCK_TIMER_STOP_FMT;
}
void Odometer::NearestKSearch(
const std::vector<pcl::KdTreeFLANN<common::TPoint>>& kdtrees,
const TPoint& query_pt, size_t k,
std::vector<std::vector<int>>* const indices,
const std::vector<pcl::KdTreeFLANN<TPoint>> &kdtrees,
const TPoint &query_pt, int k, std::vector<std::vector<int>> *const indices,
std::vector<std::vector<float>> *const dists) const {
for (const auto &kdtree : kdtrees) {
std::vector<int> index;
std::vector<float> dist;
if (kdtree.getInputCloud()->size() >= k) {
kdtree.nearestKSearch(query_pt, k, index, dist);
} else {
index.resize(k, -1);
dist.resize(k, std::numeric_limits<float>::max());
int n_found = 0;
if (kdtree.getInputCloud()->size() >= static_cast<size_t>(k)) {
n_found = kdtree.nearestKSearch(query_pt, k, index, dist);
}
if (n_found < k) {
std::vector<int>(k, -1).swap(index);
std::vector<float>(k, std::numeric_limits<float>::max()).swap(dist);
}
indices->push_back(std::move(index));
dists->push_back(std::move(dist));

18
oh_my_loam/odometer/odometer.h
View File

@ -21,31 +21,29 @@ class Odometer {
protected:
void UpdatePre(const std::vector<Feature> &features);
void MatchCorn(const common::TPointCloud& src,
void MatchCorn(const TPointCloud &src,
std::vector<PointLinePair> *const pairs) const;
void MatchSurf(const common::TPointCloud& src,
void MatchSurf(const TPointCloud &src,
std::vector<PointPlanePair> *const pairs) const;
void Visualize(const std::vector<Feature> &features,
const std::vector<PointLinePair> &pl_pairs,
const std::vector<PointPlanePair> &pp_pairs,
double timestamp = 0.0) const;
void NearestKSearch(
const std::vector<pcl::KdTreeFLANN<common::TPoint>>& kdtrees,
const TPoint& query_pt, size_t k,
void NearestKSearch(const std::vector<pcl::KdTreeFLANN<TPoint>> &kdtrees,
const TPoint &query_pt, int k,
std::vector<std::vector<int>> *const indices,
std::vector<std::vector<float>> *const dists) const;
common::Pose3d pose_curr2world_;
common::Pose3d pose_curr2last_;
std::vector<common::TPointCloudPtr> clouds_corn_pre_;
std::vector<common::TPointCloudPtr> clouds_surf_pre_;
std::vector<TPointCloudPtr> clouds_corn_pre_;
std::vector<TPointCloudPtr> clouds_surf_pre_;
std::vector<pcl::KdTreeFLANN<common::TPoint>> kdtrees_surf_;
std::vector<pcl::KdTreeFLANN<common::TPoint>> kdtrees_corn_;
std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_surf_;
std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_corn_;
YAML::Node config_;

2
oh_my_loam/solver/solver.h
View File

@ -29,4 +29,4 @@ class PoseSolver {
DISALLOW_COPY_AND_ASSIGN(PoseSolver)
};
} // oh_my_loam
} // namespace oh_my_loam

4
oh_my_loam/visualizer/odometer_visualizer.h
View File

@ -6,8 +6,8 @@
namespace oh_my_loam {
struct OdometerVisFrame : public common::LidarVisFrame {
common::TPointCloudPtr cloud_surf;
common::TPointCloudPtr cloud_corner;
TPointCloudPtr cloud_surf;
TPointCloudPtr cloud_corner;
std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs;
common::Pose3d pose_curr2last;