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

tart writing mapper

main
feixyz10 2021-01-26 00:32:14 +08:00 committed by feixyz
parent 42eaae9acd
commit 4bf3ac5aa5
11 changed files with 78 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
common/config/yaml_config.h
View File

@ -29,6 +29,7 @@ class YAMLConfig {
private:
std::unique_ptr<YAML::Node> config_{nullptr};
DECLARE_SINGLETON(YAMLConfig);
};

10
oh_my_loam/configs/config.yaml
View File

@ -9,10 +9,10 @@ extractor_config:
vis: false
verbose: false
min_point_num: 66
scan_seg_num: 36
scan_seg_num: 12
sharp_corner_point_num: 1
corner_point_num: 4
flat_surf_point_num: 1
corner_point_num: 10
flat_surf_point_num: 2
corner_point_curvature_th: 0.5
surf_point_curvature_th: 0.5
neighbor_point_dist_sq_th: 0.1
@ -26,9 +26,11 @@ odometer_config:
min_correspondence_num: 10
icp_iter_num: 2
solve_iter_num: 4
corn_match_dist_sq_th: 1.0
corn_match_dist_sq_th: 16.0
surf_match_dist_sq_th: 16.0
# configs for mapper
mapper_config:
vis: false
verbose: false
process_period: 0.5

15
oh_my_loam/extractor/extractor.cc
View File

@ -107,8 +107,6 @@ void Extractor::AssignType(TCTPointCloud *const scan) const {
int seg_num = config_["scan_seg_num"].as<int>();
if (pt_num < seg_num) return;
int seg_pt_num = (pt_num - 1) / seg_num + 1;
AERROR << pt_num << " " << seg_num << " " << seg_pt_num;
std::vector<bool> picked(pt_num, false);
std::vector<int> indices = common::Range(pt_num);
int sharp_corner_point_num = config_["sharp_corner_point_num"].as<int>();
@ -121,6 +119,7 @@ void Extractor::AssignType(TCTPointCloud *const scan) const {
for (int seg = 0; seg < seg_num; ++seg) {
int b = seg * seg_pt_num;
int e = std::min((seg + 1) * seg_pt_num, pt_num);
if (b >= e) break;
// sort by curvature for each segment: large -> small
std::sort(indices.begin() + b, indices.begin() + e, [&](int i, int j) {
return scan->at(i).curvature > scan->at(j).curvature;
@ -128,7 +127,7 @@ void Extractor::AssignType(TCTPointCloud *const scan) const {
// pick corner points
int corner_point_picked_num = 0;
for (int i = b; i < e; ++i) {
size_t ix = indices[i];
int ix = indices[i];
if (!picked.at(ix) &&
scan->at(ix).curvature > corner_point_curvature_th) {
++corner_point_picked_num;
@ -146,7 +145,7 @@ void Extractor::AssignType(TCTPointCloud *const scan) const {
// pick surface points
int surf_point_picked_num = 0;
for (int i = e - 1; i >= b; --i) {
size_t ix = indices[i];
int ix = indices[i];
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) {
@ -200,21 +199,21 @@ void Extractor::Visualize(const common::PointCloudConstPtr &cloud,
visualizer_->Render(frame);
}
void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix,
void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, int ix,
std::vector<bool> *const picked) const {
auto dist_sq = [&](size_t i, size_t j) -> double {
return common::DistanceSquare<TCTPoint>(scan[i], scan[j]);
};
double neighbor_point_dist_sq_th =
config_["neighbor_point_dist_sq_th"].as<float>();
for (size_t i = 1; i <= 5; ++i) {
for (int i = 1; i <= 5; ++i) {
if (ix - i < 0) break;
if (picked->at(ix - i)) continue;
if (dist_sq(ix - i, ix - i + 1) > neighbor_point_dist_sq_th) break;
picked->at(ix - i) = true;
}
for (size_t i = 1; i <= 5; ++i) {
if (ix + i >= scan.size()) break;
for (int i = 1; i <= 5; ++i) {
if (static_cast<size_t>(ix + i) >= scan.size()) break;
if (picked->at(ix + i)) continue;
if (dist_sq(ix + i, ix + i - 1) > neighbor_point_dist_sq_th) break;
picked->at(ix + i) = true;

4
oh_my_loam/extractor/extractor.h
View File

@ -22,6 +22,8 @@ class Extractor {
return num_scans_;
}
virtual void Reset() {}
protected:
virtual int GetScanID(const common::Point &pt) const = 0;
@ -48,7 +50,7 @@ class Extractor {
bool verbose_ = false;
private:
void UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix,
void UpdateNeighborsPicked(const TCTPointCloud &scan, int ix,
std::vector<bool> *const picked) const;
bool is_vis_ = false;

3
oh_my_loam/mapper/mapper.cc
View File

@ -15,7 +15,8 @@ bool Mapper::Init() {
return true;
}
void Mapper::Process() {}
void Mapper::Process(double timestamp, const std::vector<Feature> &features,
Pose3d *const pose_out) {}
void Mapper::Visualize() {}

21
oh_my_loam/mapper/mapper.h
View File

@ -1,7 +1,10 @@
#pragma once
#include <vector>
#include "common/common.h"
#include "common/geometry/pose.h"
#include "common/geometry/pose3d.h"
#include "oh_my_loam/base/feature.h"
#include "oh_my_loam/base/types.h"
namespace oh_my_loam {
@ -12,22 +15,30 @@ class Mapper {
bool Init();
void Process();
void Process(double timestamp, const std::vector<Feature> &features,
common::Pose3d *const pose_out);
common::PointCloudConstPtr map() const {
return cloud_map_;
}
void Reset();
private:
void Visualize();
TPointCloudPtr map_pts_;
common::PointCloudPtr cloud_map_;
common::Pose3d pose_curr2world_;
YAML::Node config_;
bool is_initialized = false;
bool is_vis_ = false;
bool verbose_ = false;
YAML::Node config_;
DISALLOW_COPY_AND_ASSIGN(Mapper)
};

4
oh_my_loam/odometer/odometer.cc
View File

@ -20,6 +20,10 @@ bool Odometer::Init() {
return true;
}
void Odometer::Reset() {
is_initialized_ = false;
}
void Odometer::Process(double timestamp, const std::vector<Feature> &features,
Pose3d *const pose_out) {
if (!is_initialized_) {

2
oh_my_loam/odometer/odometer.h
View File

@ -18,6 +18,8 @@ class Odometer {
void Process(double timestamp, const std::vector<Feature> &features,
common::Pose3d *const pose_out);
void Reset();
protected:
void UpdatePre(const std::vector<Feature> &features);

30
oh_my_loam/oh_my_loam.cc
View File

@ -1,6 +1,9 @@
#include "oh_my_loam.h"
#include <vector>
#include "common/pcl/pcl_utils.h"
#include "oh_my_loam/base/helper.h"
#include "oh_my_loam/extractor/extractor_VLP16.h"
namespace oh_my_loam {
@ -10,7 +13,7 @@ const double kPointMinDist = 0.1;
} // namespace
bool OhMyLoam::Init() {
YAML::Node config = common::YAMLConfig::Instance()->config();
config_ = common::YAMLConfig::Instance()->config();
extractor_.reset(new ExtractorVLP16);
if (!extractor_->Init()) {
AERROR << "Failed to initialize extractor";
@ -21,14 +24,22 @@ bool OhMyLoam::Init() {
AERROR << "Failed to initialize odometer";
return false;
}
// mapper_.reset(new Mapper);
// if (!mapper_->Init()) {
// AERROR << "Failed to initialize mapper";
// return false;
// }
mapper_.reset(new Mapper);
if (!mapper_->Init()) {
AERROR << "Failed to initialize mapper";
return false;
}
return true;
}
void OhMyLoam::Reset() {
timestamp_last_ = timestamp_last_mapping_ = 0.0;
extractor_->Reset();
odometer_->Reset();
mapper_->Reset();
std::vector<common::Pose3d>().swap(poses_);
}
void OhMyLoam::Run(double timestamp,
const common::PointCloudConstPtr &cloud_in) {
common::PointCloudPtr cloud(new common::PointCloud);
@ -38,6 +49,8 @@ void OhMyLoam::Run(double timestamp,
Pose3d pose;
odometer_->Process(timestamp, features, &pose);
poses_.emplace_back(pose);
if (!IsMapping(timestamp)) return;
mapper_->Process();
}
void OhMyLoam::RemoveOutliers(const common::PointCloud &cloud_in,
@ -49,4 +62,9 @@ void OhMyLoam::RemoveOutliers(const common::PointCloud &cloud_in,
});
}
bool OhMyLoam::IsMapping(double timestamp) const {
return std::abs(timestamp - timestamp_last_mapping_) >=
config_["mapper_config"]["process_period"].as<double>();
}
} // namespace oh_my_loam

16
oh_my_loam/oh_my_loam.h
View File

@ -1,8 +1,10 @@
#pragma once
#include <yaml-cpp/node/node.h>
#include "common/common.h"
#include "oh_my_loam/extractor/extractor.h"
// #include "oh_my_loam/mapper/mapper.h"
#include "oh_my_loam/mapper/mapper.h"
#include "oh_my_loam/odometer/odometer.h"
namespace oh_my_loam {
@ -16,18 +18,26 @@ class OhMyLoam {
void Run(double timestamp, const common::PointCloudConstPtr &cloud_in);
private:
void Reset();
bool IsMapping(double timestamp) const;
std::unique_ptr<Extractor> extractor_{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
// remove outliers: nan or very close points
void RemoveOutliers(const common::PointCloud &cloud_in,
common::PointCloud *const cloud_out) const;
std::vector<common::Pose3d> poses_;
YAML::Node config_;
double timestamp_last_ = 0.0, timestamp_last_mapping_ = 0.0;
DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
};

15
oh_my_loam/solver/cost_function.h
View File

@ -59,22 +59,10 @@ bool PointLineCostFunction::operator()(const T *const r_quat,
Eigen::Matrix<T, 3, 1> p(T(pt.x), T(pt.y), T(pt.z));
Eigen::Matrix<T, 3, 1> p1(T(pt1.x), T(pt1.y), T(pt1.z));
Eigen::Matrix<T, 3, 1> p2(T(pt2.x), T(pt2.y), T(pt2.z));
// if constexpr (!std::is_arithmetic<T>::value) {
// AERROR << p.x().a << ", " << p.y().a << ", " << p.z().a;
// AERROR << p1.x().a << ", " << p1.y().a << ", " << p1.z().a;
// AERROR << p2.x().a << ", " << p2.y().a << ", " << p2.z().a;
// }
Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
Eigen::Quaternion<T> r_interp =
Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
// if constexpr (!std::is_arithmetic<T>::value) {
// AERROR << time_;
// AERROR << r.w().a << " " << r.x().a << ", " << r.y().a << ", " <<
// r.z().a; AERROR << r_interp.w().a << " " << r_interp.x().a << ", " <<
// r_interp.y().a
// << ", " << r_interp.z().a;
// }
Eigen::Matrix<T, 3, 1> t(T(time_) * t_vec[0], T(time_) * t_vec[1],
T(time_) * t_vec[2]);
Eigen::Matrix<T, 3, 1> p0 = r_interp * p + t;
@ -85,9 +73,6 @@ bool PointLineCostFunction::operator()(const T *const r_quat,
residual[0] = area[0] / base_length;
residual[1] = area[1] / base_length;
residual[2] = area[2] / base_length;
// if constexpr (!std::is_arithmetic<T>::value) {
// AERROR << base_length.a;
// }
return true;
}