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

make extracker even better, start rewrite Pose

main
feixyz10 2021-01-21 01:11:20 +08:00 committed by feixyz
parent da337376f9
commit 1ccebd8b8f
16 changed files with 143 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
CMakeLists.txt
View File

@ -54,9 +54,9 @@ target_link_libraries(main
common
oh_my_loam
extractor
odometer
mapper
solver
# odometer
# mapper
# solver
${CERES_LIBRARIES}
visualizer
base

3
common/color/color.h
View File

@ -12,7 +12,8 @@ struct Color {
#define RED Color(255, 0, 0)
#define GREEN Color(0, 255, 0)
#define BLUE Color(0, 0, 255)
#define GRAY Color(50, 50, 50)
#define DRAK_GRAY Color(60, 60, 60)
#define GRAY Color(128, 128, 128)
#define CYAN Color(0, 255, 255)
#define PURPLE Color(160, 32, 240)
#define VIOLET Color(238, 130, 238)

20
common/geometry/pose.cc
View File

@ -1,20 +0,0 @@
#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

83
common/geometry/pose.h
View File

@ -4,63 +4,82 @@
namespace common {
class Pose3D {
template <typename T>
class Pose3 {
public:
Pose3D() {
q_.setIdentity();
p_.setZero();
Pose3() {
r_quat_.setIdentity();
t_vec_.setZero();
};
Pose3D(const Eigen::Quaterniond& q, const Eigen::Vector3d& p)
: q_(q), p_(p) {}
Pose3(const Eigen::Quaterniond& r_quat, const Eigen::Matrix<T, 3, 1>& t_vec)
: r_quat_(q), t_vec_(p) {}
Pose3D(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& p)
: q_(r_mat), p_(p) {}
Pose3(const Eigen::Matrix3d& r_mat, const Eigen::Matrix<T, 3, 1>& t_vec)
: r_quat_(r_mat), t_vec_(t_vec) {}
Pose3D(const double* const q, const double* const p) : q_(q), p_(p) {}
Pose3(const double* const r_quat, const double* const t_vec)
: r_quat_(r_quat), t_vec_(t_vec) {}
Pose3D Inv() const {
Eigen::Quaterniond q_inv = q_.inverse();
Eigen::Vector3d p_inv = q_inv * p_;
return {q_inv, -p_inv};
Pose3 Inv() const {
Eigen::Quaternion<T> r_inv = r_quat_.inverse();
Eigen::Matrix<T, 3, 1> t_inv = -r_inv * t_vec_;
return {r_inv, t_inv};
}
Eigen::Vector3d Transform(const Eigen::Vector3d& vec) const {
return q_ * vec + p_;
Eigen::Matrix<T, 3, 1> Transform(const Eigen::Matrix<T, 3, 1>& point) const {
return r_quat_ * point + t_vec_;
}
Eigen::Vector3d operator*(const Eigen::Vector3d& vec) const {
return Transform(vec);
Eigen::Matrix<T, 3, 1> operator*(const Eigen::Matrix<T, 3, 1>& point) const {
return Transform(point);
}
Eigen::Vector3d Translate(const Eigen::Vector3d& vec) const {
return vec + p_;
Eigen::Matrix<T, 3, 1> Translate(const Eigen::Matrix<T, 3, 1>& vec) const {
return vec + t_vec_;
}
Eigen::Vector3d Rotate(const Eigen::Vector3d& vec) const { return q_ * vec; }
Eigen::Matrix<T, 3, 1> Rotate(const Eigen::Matrix<T, 3, 1>& vec) const {
return r_quat_ * vec;
}
// Spherical linear interpolation to `pose_to`, `t` belongs [0, 1]
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_;
// Spherical linear interpolation to `pose_to`
Pose3 Interpolate(const Pose3& pose_to, double t) const {
Pose3 pose_dst;
Eigen::Quaternion<T> q_interp = r_quat_.slerp(t, pose_to.r_quat_);
Eigen::Matrix<T, 3, 1> p_interp = (pose_to.t_vec_ - t_vec_) * t + t_vec_;
return {q_interp, p_interp};
}
std::string ToString() const;
std::string ToString() const {
std::ostringstream oss;
oss << "[Pose3D] q = (" << q_.coeffs().transpose() << "), p = ("
<< p_.transpose() << ")";
return oss.str();
}
Eigen::Quaterniond q() const { return q_; }
Eigen::Quaternion<T> r_quat() const { return r_quat_; }
Eigen::Vector3d p() const { return p_; }
Eigen::Matrix<T, 3, 1> t_vec() const { return t_vec_; }
protected:
Eigen::Quaterniond q_; // orientation
Eigen::Vector3d p_; // position
Eigen::Quaternion<T> r_quat_; // orientation/rotation
Eigen::Matrix<T, 3, 1> t_vec_; // positon/translation
};
Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t);
template <typename T>
Pose3<T> Interpolate(const Pose3<T>& pose_from, const Pose3<T>& pose_to,
double t) {
return pose_from.Interpolate(pose_to, t);
}
Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs);
template <typename T>
Pose3<T> operator*(const Pose3<T>& lhs, const Pose3<T>& rhs) {
return Pose3<T>(lhs.q() * rhs.q(), lhs.q() * rhs.p() + lhs.p());
}
using Trans3d = Pose3D;
using Pose3d = Pose3<double>;
using Pose3f = Pose3<float>;
} // namespace common

6
oh_my_loam/CMakeLists.txt
View File

@ -1,9 +1,9 @@
add_subdirectory(base)
add_subdirectory(extractor)
add_subdirectory(visualizer)
add_subdirectory(odometer)
add_subdirectory(mapper)
add_subdirectory(solver)
# add_subdirectory(odometer)
# add_subdirectory(mapper)
# add_subdirectory(solver)
aux_source_directory(. SRC_FILES)

8
oh_my_loam/base/feature.h
View File

@ -5,16 +5,16 @@
namespace oh_my_loam {
struct Feature {
common::TPointCloudPtr cloud_corner;
common::TPointCloudPtr cloud_sharp_corner;
common::TPointCloudPtr cloud_less_sharp_corner;
common::TPointCloudPtr cloud_surf;
common::TPointCloudPtr cloud_flat_surf;
common::TPointCloudPtr cloud_less_flat_surf;
Feature() {
cloud_corner.reset(new common::TPointCloud);
cloud_sharp_corner.reset(new common::TPointCloud);
cloud_less_sharp_corner.reset(new common::TPointCloud);
cloud_surf.reset(new common::TPointCloud);
cloud_flat_surf.reset(new common::TPointCloud);
cloud_less_flat_surf.reset(new common::TPointCloud);
}
};

20
oh_my_loam/base/types.h
View File

@ -4,12 +4,12 @@
namespace oh_my_loam {
enum class Type {
FLAT = -2,
LESS_FLAT = -1,
enum class PType {
FLAT_SURF = -2,
SURF = -1,
NORNAL = 0,
LESS_SHARP = 1,
SHARP = 2,
CORNER = 1,
SHARP_CORNER = 2
};
struct PointXYZTCT;
@ -24,7 +24,7 @@ struct PointXYZTCT {
struct {
float time;
float curvature;
Type type;
PType type;
};
float data_c[4];
};
@ -33,11 +33,11 @@ struct PointXYZTCT {
x = y = z = 0.0f;
data[3] = 1.0f;
time = curvature = 0.0f;
type = Type::NORNAL;
type = PType::NORNAL;
}
PointXYZTCT(float x, float y, float z, float time = 0.0f,
float curvature = 0.0f, Type type = Type::NORNAL)
float curvature = 0.0f, PType type = PType::NORNAL)
: x(x), y(y), z(z), time(time), curvature(curvature), type(type) {
data[3] = 1.0f;
}
@ -49,7 +49,7 @@ struct PointXYZTCT {
data[3] = 1.0f;
time = 0.0f;
curvature = 0.0f;
type = Type::NORNAL;
type = PType::NORNAL;
}
PointXYZTCT(const common::TPoint& p) {
@ -59,7 +59,7 @@ struct PointXYZTCT {
data[3] = 1.0f;
time = p.time;
curvature = 0.0f;
type = Type::NORNAL;
type = PType::NORNAL;
}
PointXYZTCT(const PointXYZTCT& p) {

2
oh_my_loam/configs/config.yaml
View File

@ -12,7 +12,7 @@ extractor_config:
sharp_corner_point_num: 2
corner_point_num: 20
flat_surf_point_num: 4
surf_point_num: 20 ## useless currently
surf_point_num: 20
corner_point_curvature_th: 0.5
surf_point_curvature_th: 0.5
neighbor_point_dist_th: 0.1

54
oh_my_loam/extractor/extractor.cc
View File

@ -23,7 +23,7 @@ bool Extractor::Init() {
}
void Extractor::Process(double timestamp, const PointCloudConstPtr& cloud,
Feature* const feature) {
std::vector<Feature>* const features) {
BLOCK_TIMER_START;
if (cloud->size() < config_["min_point_num"].as<size_t>()) {
AWARN << "Too few input points: num = " << cloud->size() << " (< "
@ -45,11 +45,13 @@ void Extractor::Process(double timestamp, const PointCloudConstPtr& cloud,
}
AINFO_IF(verbose_) << "Extractor::AssignType: " << BLOCK_TIMER_STOP_FMT;
// store points into feature point clouds based on their type
for (const auto& scan : scans) {
GenerateFeature(scan, feature);
for (size_t i = 0; i < scans.size(); ++i) {
Feature feature;
GenerateFeature(scans[i], &feature);
features->push_back(std::move(feature));
}
AINFO << "Extractor::Process: " << BLOCK_TIMER_STOP_FMT;
if (is_vis_) Visualize(cloud, *feature, timestamp);
if (is_vis_) Visualize(cloud, *features, timestamp);
}
void Extractor::SplitScan(const PointCloud& cloud,
@ -68,9 +70,9 @@ void Extractor::SplitScan(const PointCloud& cloud,
half_passed = true;
yaw_start += kTwoPi;
}
(*scans)[scan_id].push_back(
{pt.x, pt.y, pt.z, static_cast<float>(yaw_diff / kTwoPi + scan_id),
std::nanf("")});
(*scans)[scan_id].push_back({pt.x, pt.y, pt.z,
static_cast<float>(yaw_diff / kTwoPi),
std::nanf("")});
}
}
@ -123,9 +125,9 @@ void Extractor::AssignType(TCTPointCloud* const scan) const {
scan->at(ix).curvature > corner_point_curvature_th) {
++corner_point_picked_num;
if (corner_point_picked_num <= sharp_corner_point_num) {
scan->at(ix).type = Type::SHARP;
scan->at(ix).type = PType::SHARP_CORNER;
} else if (corner_point_picked_num <= corner_point_num) {
scan->at(ix).type = Type::LESS_SHARP;
scan->at(ix).type = PType::CORNER;
} else {
break;
}
@ -140,9 +142,9 @@ void Extractor::AssignType(TCTPointCloud* const scan) const {
if (!picked.at(ix) && scan->at(ix).curvature < surf_point_curvature_th) {
++surf_point_picked_num;
if (surf_point_picked_num <= flat_surf_point_num) {
scan->at(ix).type = Type::FLAT;
scan->at(ix).type = PType::FLAT_SURF;
} else if (surf_point_picked_num <= surf_point_num) {
scan->at(ix).type = Type::LESS_FLAT;
scan->at(ix).type = PType::SURF;
} else {
break;
}
@ -155,40 +157,38 @@ void Extractor::AssignType(TCTPointCloud* const scan) const {
void Extractor::GenerateFeature(const TCTPointCloud& scan,
Feature* const feature) const {
TPointCloudPtr cloud_less_flat_surf(new TPointCloud);
for (const auto& pt : scan.points) {
for (const auto& pt : scan) {
TPoint point(pt.x, pt.y, pt.z, pt.time);
switch (pt.type) {
case Type::FLAT:
case PType::FLAT_SURF:
feature->cloud_flat_surf->points.emplace_back(point);
// no break: FLAT points are also LESS_FLAT
case Type::LESS_FLAT:
cloud_less_flat_surf->points.emplace_back(point);
// no break: FLAT_SURF points are also SURF points
case PType::SURF:
feature->cloud_surf->points.emplace_back(point);
break;
case Type::SHARP:
case PType::SHARP_CORNER:
feature->cloud_sharp_corner->points.emplace_back(point);
// no break: SHARP points are also LESS_SHARP
case Type::LESS_SHARP:
feature->cloud_less_sharp_corner->points.emplace_back(point);
// no break: SHARP_CORNER points are also CORNER points
case PType::CORNER:
feature->cloud_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(),
VoxelDownSample<TPoint>(feature->cloud_surf, dowm_sampled.get(),
config_["downsample_voxel_size"].as<double>());
feature->cloud_less_flat_surf = dowm_sampled;
feature->cloud_surf->swap(*dowm_sampled);
}
void Extractor::Visualize(const PointCloudConstPtr& cloud,
const Feature& feature, double timestamp) {
const std::vector<Feature>& features,
double timestamp) {
std::shared_ptr<ExtractorVisFrame> frame(new ExtractorVisFrame);
frame->timestamp = timestamp;
frame->cloud = cloud;
frame->feature = feature;
frame->features = features;
visualizer_->Render(frame);
}

5
oh_my_loam/extractor/extractor.h
View File

@ -16,7 +16,7 @@ class Extractor {
bool Init();
void Process(double timestamp, const common::PointCloudConstPtr& cloud,
Feature* const feature);
std::vector<Feature>* const features);
int num_scans() const { return num_scans_; }
@ -34,7 +34,8 @@ class Extractor {
Feature* const feature) const;
virtual void Visualize(const common::PointCloudConstPtr& cloud,
const Feature& feature, double timestamp = 0.0);
const std::vector<Feature>& features,
double timestamp = 0.0);
int num_scans_ = 0;

10
oh_my_loam/odometer/odometer.h
View File

@ -15,11 +15,11 @@ class Odometer {
bool Init();
void Process(double timestamp, const Feature& feature,
void Process(double timestamp, const std::vector<Feature>& features,
common::Pose3D* const pose);
protected:
void UpdatePre(const Feature& feature);
void UpdatePre(const std::vector<Feature>& feature) s;
void MatchCornFeature(const common::TPointCloud& src,
const common::TPointCloud& tgt,
@ -29,7 +29,7 @@ class Odometer {
const common::TPointCloud& tgt,
std::vector<PointPlanePair>* const pairs) const;
void Visualize(const Feature& feature,
void Visualize(const std::vector<Feature>& features,
const std::vector<PointLinePair>& pl_pairs,
const std::vector<PointPlanePair>& pp_pairs,
double timestamp = 0.0) const;
@ -37,8 +37,8 @@ class Odometer {
common::Pose3D pose_curr2world_;
common::Pose3D pose_curr2last_;
common::TPointCloudPtr corn_pre_{nullptr};
common::TPointCloudPtr surf_pre_{nullptr};
common::TPointCloudPtr cloud_corn_pre_{nullptr};
common::TPointCloudPtr cloud_surf_pre_{nullptr};
pcl::KdTreeFLANN<common::TPoint>::Ptr kdtree_surf_{nullptr};
pcl::KdTreeFLANN<common::TPoint>::Ptr kdtree_corn_{nullptr};

30
oh_my_loam/oh_my_loam.cc
View File

@ -17,27 +17,27 @@ bool OhMyLoam::Init() {
AERROR << "Failed to initialize extractor";
return false;
}
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;
}
// 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(double timestamp, const PointCloudConstPtr& cloud_in) {
PointCloudPtr cloud(new PointCloud);
RemoveOutliers(*cloud_in, cloud.get());
Feature feature;
extractor_->Process(timestamp, cloud, &feature);
Pose3D pose;
odometer_->Process(timestamp, feature, &pose);
poses_.emplace_back(pose);
std::vector<Feature> features;
extractor_->Process(timestamp, cloud, &features);
// Pose3D pose;
// odometer_->Process(timestamp, feature, &pose);
// poses_.emplace_back(pose);
}
void OhMyLoam::RemoveOutliers(const PointCloud& cloud_in,

9
oh_my_loam/oh_my_loam.h
View File

@ -2,8 +2,8 @@
#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"
// #include "oh_my_loam/mapper/mapper.h"
// #include "oh_my_loam/odometer/odometer.h"
namespace oh_my_loam {
@ -18,13 +18,14 @@ class OhMyLoam {
private:
std::unique_ptr<Extractor> extractor_{nullptr};
std::unique_ptr<Odometer> odometer_{nullptr};
// std::unique_ptr<Odometer> odometer_{nullptr};
std::unique_ptr<Mapper> mapper_{nullptr};
// std::unique_ptr<Mapper> mapper_{nullptr};
// remove outliers: nan and very close points
void RemoveOutliers(const common::PointCloud& cloud_in,
common::PointCloud* const cloud_out) const;
std::vector<common::Pose3D> poses_;
DISALLOW_COPY_AND_ASSIGN(OhMyLoam)

19
oh_my_loam/visualizer/extractor_visualizer.cc
View File

@ -8,14 +8,17 @@ using namespace common;
void ExtractorVisualizer::Draw() {
auto frame = GetCurrentFrame<ExtractorVisFrame>();
DrawPointCloud<Point>(frame.cloud, GRAY, "cloud_raw");
DrawPointCloud<TPoint>(frame.feature.cloud_less_flat_surf, BLUE,
"cloud_less_flat_surf");
DrawPointCloud<TPoint>(frame.feature.cloud_flat_surf, CYAN,
"cloud_flat_surf");
DrawPointCloud<TPoint>(frame.feature.cloud_less_sharp_corner, YELLOW,
"cloud_less_sharp_corner");
DrawPointCloud<TPoint>(frame.feature.cloud_sharp_corner, RED,
"cloud_sharp_corner");
for (size_t i = 0; i < frame.features.size(); ++i) {
const auto& feature = frame.features[i];
std::string id_suffix = std::to_string(i);
DrawPointCloud<TPoint>(feature.cloud_surf, BLUE, "cloud_surf" + id_suffix);
DrawPointCloud<TPoint>(feature.cloud_flat_surf, CYAN,
"cloud_flat_surf" + id_suffix);
DrawPointCloud<TPoint>(feature.cloud_corner, YELLOW,
"cloud_corner" + id_suffix);
DrawPointCloud<TPoint>(feature.cloud_sharp_corner, RED,
"cloud_sharp_corner" + id_suffix);
}
};
} // namespace oh_my_loam

2
oh_my_loam/visualizer/extractor_visualizer.h
View File

@ -6,7 +6,7 @@
namespace oh_my_loam {
struct ExtractorVisFrame : public common::LidarVisFrame {
Feature feature;
std::vector<Feature> features;
};
class ExtractorVisualizer : public common::LidarVisualizer {

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 surf_pts;
common::TPointCloudPtr corn_pts;
common::TPointCloudPtr cloud_surf;
common::TPointCloudPtr cloud_corner;
std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs;
common::Pose3D pose_curr2last;