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

format all .h .cc files

main
feixyz10 2021-01-22 16:33:55 +08:00 committed by feixyz
parent 76dfa7f447
commit 12054bf10f
25 changed files with 154 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
common/config/yaml_config.h
View File

@ -11,18 +11,18 @@ namespace common {
class YAMLConfig { class YAMLConfig {
public: public:
void Init(const std::string& file) { void Init(const std::string &file) {
config_.reset(new YAML::Node); config_.reset(new YAML::Node);
*config_ = YAML::LoadFile(file); *config_ = YAML::LoadFile(file);
} }
template <typename T> template <typename T>
const T Get(const std::string& key) const { const T Get(const std::string &key) const {
AFATAL_IF(!config_) << "Not initialized, please call Init first."; AFATAL_IF(!config_) << "Not initialized, please call Init first.";
return (*config_)[key].as<T>(); return (*config_)[key].as<T>();
} }
const YAML::Node& config() const { const YAML::Node &config() const {
AFATAL_IF(!config_) << "Not initialized, please call Init first."; AFATAL_IF(!config_) << "Not initialized, please call Init first.";
return *config_; return *config_;
} }

32
common/geometry/pose3d.h
View File

@ -12,18 +12,16 @@ class Pose3d {
t_vec_.setZero(); t_vec_.setZero();
}; };
Pose3d(const Eigen::Quaterniond& r_quat, const Eigen::Vector3d& t_vec) Pose3d(const Eigen::Quaterniond &r_quat, const Eigen::Vector3d &t_vec)
: r_quat_(r_quat), t_vec_(t_vec) {} : r_quat_(r_quat), t_vec_(t_vec) {}
Pose3d(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& t_vec) Pose3d(const Eigen::Matrix3d &r_mat, const Eigen::Vector3d &t_vec)
: r_quat_(r_mat), t_vec_(t_vec) {} : r_quat_(r_mat), t_vec_(t_vec) {}
Pose3d(const double* const r_quat, const double* const t_vec) Pose3d(const double *const r_quat, const double *const t_vec)
: r_quat_(r_quat), t_vec_(t_vec) {} : r_quat_(r_quat), t_vec_(t_vec) {}
static Pose3d Identity() { return {}; } void SetIdentity() {
void setIdentity() {
r_quat_.setIdentity(); r_quat_.setIdentity();
t_vec_.setZero(); t_vec_.setZero();
} }
@ -34,30 +32,30 @@ class Pose3d {
return {r_inv, -t_inv}; return {r_inv, -t_inv};
} }
Pose3d operator*(const Pose3d& rhs) const { Pose3d operator*(const Pose3d &rhs) const {
return Pose3d(r_quat_ * rhs.r_quat_, r_quat_ * rhs.t_vec_ + t_vec_); return Pose3d(r_quat_ * rhs.r_quat_, r_quat_ * rhs.t_vec_ + t_vec_);
} }
Pose3d& operator*=(const Pose3d& rhs) { Pose3d &operator*=(const Pose3d &rhs) {
t_vec_ += r_quat_ * rhs.t_vec_; t_vec_ += r_quat_ * rhs.t_vec_;
r_quat_ *= rhs.r_quat_; r_quat_ *= rhs.r_quat_;
return *this; return *this;
} }
Eigen::Vector3d Transform(const Eigen::Vector3d& point) const { Eigen::Vector3d Transform(const Eigen::Vector3d &point) const {
return r_quat_ * point + t_vec_; return r_quat_ * point + t_vec_;
} }
Eigen::Vector3d operator*(const Eigen::Vector3d& point) const { Eigen::Vector3d operator*(const Eigen::Vector3d &point) const {
return Transform(point); return Transform(point);
} }
Eigen::Vector3d Rotate(const Eigen::Vector3d& vec) const { Eigen::Vector3d Rotate(const Eigen::Vector3d &vec) const {
return r_quat_ * vec; return r_quat_ * vec;
} }
// Spherical linear interpolation to `pose_to` // Spherical linear interpolation to `pose_to`
Pose3d Interpolate(const Pose3d& pose_to, double t) const { Pose3d Interpolate(const Pose3d &pose_to, double t) const {
Pose3d pose_dst = t > 0 ? pose_to : (*this) * pose_to.Inv() * (*this); Pose3d pose_dst = t > 0 ? pose_to : (*this) * pose_to.Inv() * (*this);
t = t > 0 ? t : -t; t = t > 0 ? t : -t;
int whole = std::floor(t); int whole = std::floor(t);
@ -79,18 +77,22 @@ class Pose3d {
// const Eigen::Quaterniond& r_quat() const { return r_quat_; } // const Eigen::Quaterniond& r_quat() const { return r_quat_; }
Eigen::Quaterniond r_quat() const { return r_quat_; } Eigen::Quaterniond r_quat() const {
return r_quat_;
}
// const Eigen::Vector3d& t_vec() const { return t_vec_; } // const Eigen::Vector3d& t_vec() const { return t_vec_; }
Eigen::Vector3d t_vec() const { return t_vec_; } Eigen::Vector3d t_vec() const {
return t_vec_;
}
protected: protected:
Eigen::Quaterniond r_quat_; // orientation/rotation Eigen::Quaterniond r_quat_; // orientation/rotation
Eigen::Vector3d t_vec_; // positon/translation Eigen::Vector3d t_vec_; // positon/translation
}; };
inline Pose3d Interpolate(const Pose3d& pose_from, const Pose3d& pose_to, inline Pose3d Interpolate(const Pose3d &pose_from, const Pose3d &pose_to,
double t) { double t) {
return pose_from.Interpolate(pose_to, t); return pose_from.Interpolate(pose_to, t);
} }

4
common/log/log.cc
View File

@ -3,8 +3,8 @@
namespace common { namespace common {
void InitG3Logging(bool log_to_file, const std::string& prefix, void InitG3Logging(bool log_to_file, const std::string &prefix,
const std::string& path) { const std::string &path) {
static std::unique_ptr<g3::LogWorker> worker; static std::unique_ptr<g3::LogWorker> worker;
if (worker != nullptr) return; if (worker != nullptr) return;
worker = std::move(g3::LogWorker::createLogWorker()); worker = std::move(g3::LogWorker::createLogWorker());

12
common/log/log.h
View File

@ -44,8 +44,8 @@ const LEVELS USER(ERROR.value + 100, "USER");
#define ACHECK(cond) G3CHECK(cond) #define ACHECK(cond) G3CHECK(cond)
namespace common { namespace common {
void InitG3Logging(bool log_to_file = false, const std::string& prefix = "", void InitG3Logging(bool log_to_file = false, const std::string &prefix = "",
const std::string& path = "./"); const std::string &path = "./");
} // namespace common } // namespace common
namespace g3 { namespace g3 {
@ -53,7 +53,7 @@ class CustomSink {
public: public:
CustomSink() = default; CustomSink() = default;
explicit CustomSink(const std::string& log_file_name) explicit CustomSink(const std::string &log_file_name)
: log_file_name_(log_file_name), log_to_file_(true) { : log_file_name_(log_file_name), log_to_file_(true) {
ofs_.reset(new std::ofstream(log_file_name)); ofs_.reset(new std::ofstream(log_file_name));
} }
@ -86,20 +86,20 @@ class CustomSink {
bool log_to_file_{false}; bool log_to_file_{false};
std::unique_ptr<std::ofstream> ofs_{nullptr}; std::unique_ptr<std::ofstream> ofs_{nullptr};
std::string FormatedMessage(const g3::LogMessage& msg) const { std::string FormatedMessage(const g3::LogMessage &msg) const {
std::ostringstream oss; std::ostringstream oss;
oss << "[" << msg.level()[0] << msg.timestamp("%Y%m%d %H:%M:%S.%f3") << " " oss << "[" << msg.level()[0] << msg.timestamp("%Y%m%d %H:%M:%S.%f3") << " "
<< msg.file() << ":" << msg.line() << "] " << msg.message(); << msg.file() << ":" << msg.line() << "] " << msg.message();
return oss.str(); return oss.str();
} }
std::string ColorFormatedMessage(const g3::LogMessage& msg) const { std::string ColorFormatedMessage(const g3::LogMessage &msg) const {
std::ostringstream oss; std::ostringstream oss;
oss << GetColorCode(msg._level) << FormatedMessage(msg) << "\033[m"; oss << GetColorCode(msg._level) << FormatedMessage(msg) << "\033[m";
return oss.str(); return oss.str();
} }
std::string GetColorCode(const LEVELS& level) const { std::string GetColorCode(const LEVELS &level) const {
if (level.value == WARNING.value) { if (level.value == WARNING.value) {
return "\033[33m"; // yellow return "\033[33m"; // yellow
} }

14
common/math/math_utils.cc
View File

@ -5,13 +5,17 @@
namespace common { namespace common {
double NormalizeAngle(double ang) { double NormalizeAngle(double ang) {
const double& two_pi = 2 * M_PI; const double &two_pi = 2 * M_PI;
return ang - two_pi * std::floor((ang + M_PI) / two_pi); return ang - two_pi * std::floor((ang + M_PI) / two_pi);
} }
double Degree2Rad(double degree) { return degree * M_PI / 180.0; } double Degree2Rad(double degree) {
return degree * M_PI / 180.0;
}
double Rad2Degree(double rad) { return rad * 180.0 / M_PI; } double Rad2Degree(double rad) {
return rad * 180.0 / M_PI;
}
const std::vector<int> Range(int begin, int end, int step) { const std::vector<int> Range(int begin, int end, int step) {
ACHECK(step != 0) << "Step must non-zero"; ACHECK(step != 0) << "Step must non-zero";
@ -23,6 +27,8 @@ const std::vector<int> Range(int begin, int end, int step) {
return seq; return seq;
} }
const std::vector<int> Range(int end) { return Range(0, end, 1); } const std::vector<int> Range(int end) {
return Range(0, end, 1);
}
} // namespace common } // namespace common

22
common/pcl/pcl_utils.h
View File

@ -7,41 +7,41 @@ namespace common {
// Distance squred of a point to origin // Distance squred of a point to origin
template <typename PointType> template <typename PointType>
inline double DistanceSqure(const PointType& pt) { inline double DistanceSqure(const PointType &pt) {
return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z; return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z;
} }
// Distance squred of two points // Distance squred of two points
template <typename PointType> template <typename PointType>
inline double DistanceSqure(const PointType& pt1, const PointType& pt2) { 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) + 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); (pt1.z - pt2.z) * (pt1.z - pt2.z);
} }
// Distance of a point to origin // Distance of a point to origin
template <typename PointType> template <typename PointType>
inline double Distance(const PointType& pt) { inline double Distance(const PointType &pt) {
return std::sqrt(DistanceSqure(pt)); return std::sqrt(DistanceSqure(pt));
} }
// Distance squred of two points // Distance squred of two points
template <typename PointType> template <typename PointType>
inline double Distance(const PointType& pt1, const PointType& pt2) { inline double Distance(const PointType &pt1, const PointType &pt2) {
return std::sqrt(DistanceSqure(pt1, pt2)); return std::sqrt(DistanceSqure(pt1, pt2));
} }
// Check whether is a finite point: neither infinite nor nan // Check whether is a finite point: neither infinite nor nan
template <typename PointType> template <typename PointType>
inline double IsFinite(const PointType& pt) { inline double IsFinite(const PointType &pt) {
return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z); return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z);
} }
// Remove point if the condition evaluated to true on it // Remove point if the condition evaluated to true on it
template <typename PointType> template <typename PointType>
void RemovePoints(const pcl::PointCloud<PointType>& cloud_in, void RemovePoints(const pcl::PointCloud<PointType> &cloud_in,
pcl::PointCloud<PointType>* const cloud_out, pcl::PointCloud<PointType> *const cloud_out,
std::function<bool(const PointType&)> check, std::function<bool(const PointType &)> check,
std::vector<int>* const removed_indices = nullptr) { std::vector<int> *const removed_indices = nullptr) {
if (&cloud_in != cloud_out) { if (&cloud_in != cloud_out) {
cloud_out->header = cloud_in.header; cloud_out->header = cloud_in.header;
cloud_out->resize(cloud_in.size()); cloud_out->resize(cloud_in.size());
@ -64,8 +64,8 @@ void RemovePoints(const pcl::PointCloud<PointType>& cloud_in,
template <typename PointType> template <typename PointType>
void VoxelDownSample( void VoxelDownSample(
const typename pcl::PointCloud<PointType>::ConstPtr& cloud_in, const typename pcl::PointCloud<PointType>::ConstPtr &cloud_in,
pcl::PointCloud<PointType>* const cloud_out, double voxel_size) { pcl::PointCloud<PointType> *const cloud_out, double voxel_size) {
pcl::VoxelGrid<PointType> filter; pcl::VoxelGrid<PointType> filter;
filter.setInputCloud(cloud_in); filter.setInputCloud(cloud_in);
filter.setLeafSize(voxel_size, voxel_size, voxel_size); filter.setLeafSize(voxel_size, voxel_size, voxel_size);

10
common/time/timer.h
View File

@ -9,9 +9,13 @@ namespace common {
class Timer { class Timer {
public: public:
Timer() { Tic(); } Timer() {
Tic();
}
void Tic() { start_ = std::chrono::system_clock::now(); } void Tic() {
start_ = std::chrono::system_clock::now();
}
// unit: 's' = second, 'm' = millisecond, 'u' = microsecond // unit: 's' = second, 'm' = millisecond, 'u' = microsecond
double Toc(char unit = 'm'); double Toc(char unit = 'm');
@ -23,7 +27,7 @@ class Timer {
class TimerWrapper { class TimerWrapper {
public: public:
explicit TimerWrapper(const std::string& msg, double duration_ms = -1.0) explicit TimerWrapper(const std::string &msg, double duration_ms = -1.0)
: msg_(msg), duration_ms_(duration_ms) { : msg_(msg), duration_ms_(duration_ms) {
timer_.Tic(); timer_.Tic();
}; };

4
common/visualizer/lidar_visualizer.h
View File

@ -55,7 +55,9 @@ class LidarVisualizer {
is_updated_ = true; is_updated_ = true;
} }
std::string name() const { return name_; } std::string name() const {
return name_;
}
protected: protected:
void Run() { void Run() {

10
common/visualizer/lidar_visualizer_utils.cc
View File

@ -2,14 +2,14 @@
namespace common { namespace common {
void AddLine(const pcl::PointXYZ& pt1, const pcl::PointXYZ& pt2, void AddLine(const pcl::PointXYZ &pt1, const pcl::PointXYZ &pt2,
const Color& color, const std::string& id, const Color &color, const std::string &id,
PCLVisualizer* const viewer) { PCLVisualizer *const viewer) {
viewer->addLine(pt1, pt2, color.r, color.g, color.b, id); viewer->addLine(pt1, pt2, color.r, color.g, color.b, id);
} }
void AddSphere(const pcl::PointXYZ& center, double radius, const Color& color, void AddSphere(const pcl::PointXYZ &center, double radius, const Color &color,
const std::string& id, PCLVisualizer* const viewer) { const std::string &id, PCLVisualizer *const viewer) {
viewer->addSphere(center, radius, color.r, color.g, color.b, id); viewer->addSphere(center, radius, color.r, color.g, color.b, id);
} }

22
common/visualizer/lidar_visualizer_utils.h
View File

@ -18,9 +18,9 @@ using PCLVisualizer = pcl::visualization::PCLVisualizer;
pcl::visualization::PointCloudColorHandlerGenericField pcl::visualization::PointCloudColorHandlerGenericField
template <typename PointType> template <typename PointType>
void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr& cloud, void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr &cloud,
const Color& color, const std::string& id, const Color &color, const std::string &id,
PCLVisualizer* const viewer, int point_size = 3) { PCLVisualizer *const viewer, int point_size = 3) {
PCLColorHandlerCustom<PointType> color_handler(cloud, color.r, color.g, PCLColorHandlerCustom<PointType> color_handler(cloud, color.r, color.g,
color.b); color.b);
viewer->addPointCloud<PointType>(cloud, color_handler, id); viewer->addPointCloud<PointType>(cloud, color_handler, id);
@ -29,20 +29,20 @@ void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr& cloud,
} }
template <typename PointType> template <typename PointType>
void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr& cloud, void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr &cloud,
const std::string& field, const std::string& id, const std::string &field, const std::string &id,
PCLVisualizer* const viewer, int point_size = 3) { PCLVisualizer *const viewer, int point_size = 3) {
PCLColorHandlerGenericField<PointType> color_handler(cloud, field); PCLColorHandlerGenericField<PointType> color_handler(cloud, field);
viewer->addPointCloud<PointType>(cloud, color_handler, id); viewer->addPointCloud<PointType>(cloud, color_handler, id);
viewer->setPointCloudRenderingProperties( viewer->setPointCloudRenderingProperties(
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id); pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id);
} }
void AddLine(const pcl::PointXYZ& pt1, const pcl::PointXYZ& pt2, void AddLine(const pcl::PointXYZ &pt1, const pcl::PointXYZ &pt2,
const Color& color, const std::string& id, const Color &color, const std::string &id,
PCLVisualizer* const viewer); PCLVisualizer *const viewer);
void AddSphere(const pcl::PointXYZ& center, double radius, const Color& color, void AddSphere(const pcl::PointXYZ &center, double radius, const Color &color,
const std::string& id, PCLVisualizer* const viewer); const std::string &id, PCLVisualizer *const viewer);
} // namespace common } // namespace common

10
main.cc
View File

@ -12,10 +12,10 @@
using namespace common; using namespace common;
using namespace oh_my_loam; using namespace oh_my_loam;
void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg, void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr &msg,
OhMyLoam* const slam); OhMyLoam *const slam);
int main(int argc, char* argv[]) { int main(int argc, char *argv[]) {
// config // config
YAMLConfig::Instance()->Init(argv[1]); YAMLConfig::Instance()->Init(argv[1]);
bool log_to_file = YAMLConfig::Instance()->Get<bool>("log_to_file"); bool log_to_file = YAMLConfig::Instance()->Get<bool>("log_to_file");
@ -43,8 +43,8 @@ int main(int argc, char* argv[]) {
return 0; return 0;
} }
void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg, void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr &msg,
OhMyLoam* const slam) { OhMyLoam *const slam) {
PointCloudPtr cloud(new PointCloud); PointCloudPtr cloud(new PointCloud);
pcl::fromROSMsg(*msg, *cloud); pcl::fromROSMsg(*msg, *cloud);
double timestamp = msg->header.stamp.toSec(); double timestamp = msg->header.stamp.toSec();

2
oh_my_loam/configs/config.yaml
View File

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

38
oh_my_loam/extractor/extractor.cc
View File

@ -13,7 +13,7 @@ using namespace common;
} // namespace } // namespace
bool Extractor::Init() { bool Extractor::Init() {
const auto& config = YAMLConfig::Instance()->config(); const auto &config = YAMLConfig::Instance()->config();
config_ = config["extractor_config"]; config_ = config["extractor_config"];
is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>(); is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
verbose_ = config_["verbose"].as<bool>(); verbose_ = config_["verbose"].as<bool>();
@ -22,8 +22,8 @@ bool Extractor::Init() {
return true; return true;
} }
void Extractor::Process(double timestamp, const PointCloudConstPtr& cloud, void Extractor::Process(double timestamp, const PointCloudConstPtr &cloud,
std::vector<Feature>* const features) { std::vector<Feature> *const features) {
BLOCK_TIMER_START; BLOCK_TIMER_START;
if (cloud->size() < config_["min_point_num"].as<size_t>()) { if (cloud->size() < config_["min_point_num"].as<size_t>()) {
AWARN << "Too few input points: num = " << cloud->size() << " (< " AWARN << "Too few input points: num = " << cloud->size() << " (< "
@ -35,12 +35,12 @@ void Extractor::Process(double timestamp, const PointCloudConstPtr& cloud,
SplitScan(*cloud, &scans); SplitScan(*cloud, &scans);
AINFO_IF(verbose_) << "Extractor::SplitScan: " << BLOCK_TIMER_STOP_FMT; AINFO_IF(verbose_) << "Extractor::SplitScan: " << BLOCK_TIMER_STOP_FMT;
// compute curvature to each point // compute curvature to each point
for (auto& scan : scans) { for (auto &scan : scans) {
ComputeCurvature(&scan); ComputeCurvature(&scan);
} }
AINFO_IF(verbose_) << "Extractor::ComputeCurvature: " << BLOCK_TIMER_STOP_FMT; AINFO_IF(verbose_) << "Extractor::ComputeCurvature: " << BLOCK_TIMER_STOP_FMT;
// assign type to each point: FLAT, LESS_FLAT, NORMAL, LESS_SHARP or SHARP // assign type to each point: FLAT, LESS_FLAT, NORMAL, LESS_SHARP or SHARP
for (auto& scan : scans) { for (auto &scan : scans) {
AssignType(&scan); AssignType(&scan);
} }
AINFO_IF(verbose_) << "Extractor::AssignType: " << BLOCK_TIMER_STOP_FMT; AINFO_IF(verbose_) << "Extractor::AssignType: " << BLOCK_TIMER_STOP_FMT;
@ -54,12 +54,12 @@ void Extractor::Process(double timestamp, const PointCloudConstPtr& cloud,
if (is_vis_) Visualize(cloud, *features, timestamp); if (is_vis_) Visualize(cloud, *features, timestamp);
} }
void Extractor::SplitScan(const PointCloud& cloud, void Extractor::SplitScan(const PointCloud &cloud,
std::vector<TCTPointCloud>* const scans) const { std::vector<TCTPointCloud> *const scans) const {
scans->resize(num_scans_); scans->resize(num_scans_);
double yaw_start = -atan2(cloud.points[0].y, cloud.points[0].x); double yaw_start = -atan2(cloud.points[0].y, cloud.points[0].x);
bool half_passed = false; bool half_passed = false;
for (const auto& pt : cloud) { for (const auto &pt : cloud) {
int scan_id = GetScanID(pt); int scan_id = GetScanID(pt);
if (scan_id >= num_scans_ || scan_id < 0) continue; if (scan_id >= num_scans_ || scan_id < 0) continue;
double yaw = -atan2(pt.y, pt.x); double yaw = -atan2(pt.y, pt.x);
@ -76,9 +76,9 @@ void Extractor::SplitScan(const PointCloud& cloud,
} }
} }
void Extractor::ComputeCurvature(TCTPointCloud* const scan) const { void Extractor::ComputeCurvature(TCTPointCloud *const scan) const {
if (scan->size() <= 10 + kScanSegNum) return; if (scan->size() <= 10 + kScanSegNum) return;
auto& pts = scan->points; auto &pts = scan->points;
for (size_t i = 5; i < pts.size() - 5; ++i) { for (size_t i = 5; i < pts.size() - 5; ++i) {
float dx = pts[i - 5].x + pts[i - 4].x + pts[i - 3].x + pts[i - 2].x + float dx = pts[i - 5].x + pts[i - 4].x + pts[i - 3].x + pts[i - 2].x +
pts[i - 1].x + pts[i + 1].x + pts[i + 2].x + pts[i + 3].x + pts[i - 1].x + pts[i + 1].x + pts[i + 2].x + pts[i + 3].x +
@ -91,12 +91,12 @@ void Extractor::ComputeCurvature(TCTPointCloud* const scan) const {
pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z; pts[i + 4].z + pts[i + 5].z - 10 * pts[i].z;
pts[i].curvature = std::hypot(dx, dy, dz); pts[i].curvature = std::hypot(dx, dy, dz);
} }
RemovePoints<TCTPoint>(*scan, scan, [](const TCTPoint& pt) { RemovePoints<TCTPoint>(*scan, scan, [](const TCTPoint &pt) {
return !std::isfinite(pt.curvature); return !std::isfinite(pt.curvature);
}); });
} }
void Extractor::AssignType(TCTPointCloud* const scan) const { void Extractor::AssignType(TCTPointCloud *const scan) const {
int pt_num = scan->size(); int pt_num = scan->size();
if (pt_num < kScanSegNum) return; if (pt_num < kScanSegNum) return;
int seg_pt_num = (pt_num - 1) / kScanSegNum + 1; int seg_pt_num = (pt_num - 1) / kScanSegNum + 1;
@ -155,9 +155,9 @@ void Extractor::AssignType(TCTPointCloud* const scan) const {
} }
} }
void Extractor::GenerateFeature(const TCTPointCloud& scan, void Extractor::GenerateFeature(const TCTPointCloud &scan,
Feature* const feature) const { Feature *const feature) const {
for (const auto& pt : scan) { for (const auto &pt : scan) {
TPoint point(pt.x, pt.y, pt.z, pt.time); TPoint point(pt.x, pt.y, pt.z, pt.time);
switch (pt.type) { switch (pt.type) {
case PType::FLAT_SURF: case PType::FLAT_SURF:
@ -182,8 +182,8 @@ void Extractor::GenerateFeature(const TCTPointCloud& scan,
feature->cloud_surf->swap(*dowm_sampled); feature->cloud_surf->swap(*dowm_sampled);
} }
void Extractor::Visualize(const PointCloudConstPtr& cloud, void Extractor::Visualize(const PointCloudConstPtr &cloud,
const std::vector<Feature>& features, const std::vector<Feature> &features,
double timestamp) { double timestamp) {
std::shared_ptr<ExtractorVisFrame> frame(new ExtractorVisFrame); std::shared_ptr<ExtractorVisFrame> frame(new ExtractorVisFrame);
frame->timestamp = timestamp; frame->timestamp = timestamp;
@ -192,8 +192,8 @@ void Extractor::Visualize(const PointCloudConstPtr& cloud,
visualizer_->Render(frame); visualizer_->Render(frame);
} }
void Extractor::UpdateNeighborsPicked(const TCTPointCloud& scan, size_t ix, void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix,
std::vector<bool>* const picked) const { std::vector<bool> *const picked) const {
auto DistSqure = [&](int i, int j) -> float { auto DistSqure = [&](int i, int j) -> float {
return DistanceSqure<TCTPoint>(scan[i], scan[j]); return DistanceSqure<TCTPoint>(scan[i], scan[j]);
}; };

30
oh_my_loam/extractor/extractor.h
View File

@ -15,26 +15,28 @@ class Extractor {
bool Init(); bool Init();
void Process(double timestamp, const common::PointCloudConstPtr& cloud, void Process(double timestamp, const common::PointCloudConstPtr &cloud,
std::vector<Feature>* const features); std::vector<Feature> *const features);
int num_scans() const { return num_scans_; } int num_scans() const {
return num_scans_;
}
protected: protected:
virtual int GetScanID(const common::Point& pt) const = 0; virtual int GetScanID(const common::Point &pt) const = 0;
virtual void SplitScan(const common::PointCloud& cloud, virtual void SplitScan(const common::PointCloud &cloud,
std::vector<TCTPointCloud>* const scans) const; std::vector<TCTPointCloud> *const scans) const;
virtual void ComputeCurvature(TCTPointCloud* const scan) const; virtual void ComputeCurvature(TCTPointCloud *const scan) const;
virtual void AssignType(TCTPointCloud* const scan) const; virtual void AssignType(TCTPointCloud *const scan) const;
virtual void GenerateFeature(const TCTPointCloud& scan, virtual void GenerateFeature(const TCTPointCloud &scan,
Feature* const feature) const; Feature *const feature) const;
virtual void Visualize(const common::PointCloudConstPtr& cloud, virtual void Visualize(const common::PointCloudConstPtr &cloud,
const std::vector<Feature>& features, const std::vector<Feature> &features,
double timestamp = 0.0); double timestamp = 0.0);
int num_scans_ = 0; int num_scans_ = 0;
@ -46,8 +48,8 @@ class Extractor {
bool verbose_ = false; bool verbose_ = false;
private: private:
void UpdateNeighborsPicked(const TCTPointCloud& scan, size_t ix, void UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix,
std::vector<bool>* const picked) const; std::vector<bool> *const picked) const;
bool is_vis_ = false; bool is_vis_ = false;

2
oh_my_loam/extractor/extractor_VLP16.cc
View File

@ -4,7 +4,7 @@
namespace oh_my_loam { namespace oh_my_loam {
int ExtractorVLP16::GetScanID(const common::Point& pt) const { int ExtractorVLP16::GetScanID(const common::Point &pt) const {
double theta = double theta =
common::Rad2Degree(std::atan2(pt.z, std::hypot(pt.x, pt.y))) + 15.0; common::Rad2Degree(std::atan2(pt.z, std::hypot(pt.x, pt.y))) + 15.0;
return static_cast<int>(std::round(theta / 2.0) + 1.e-5); return static_cast<int>(std::round(theta / 2.0) + 1.e-5);

6
oh_my_loam/extractor/extractor_VLP16.h
View File

@ -7,10 +7,12 @@ namespace oh_my_loam {
// for VLP-16 // for VLP-16
class ExtractorVLP16 : public Extractor { class ExtractorVLP16 : public Extractor {
public: public:
ExtractorVLP16() { num_scans_ = 16; } ExtractorVLP16() {
num_scans_ = 16;
}
private: private:
int GetScanID(const common::Point& pt) const override; int GetScanID(const common::Point &pt) const override;
}; };
} // namespace oh_my_loam } // namespace oh_my_loam

2
oh_my_loam/mapper/mapper.cc
View File

@ -7,7 +7,7 @@ using namespace common;
} // namespace } // namespace
bool Mapper::Init() { bool Mapper::Init() {
const auto& config = YAMLConfig::Instance()->config(); const auto &config = YAMLConfig::Instance()->config();
config_ = config["mapper_config"]; config_ = config["mapper_config"];
is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>(); is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
verbose_ = config_["vis"].as<bool>(); verbose_ = config_["vis"].as<bool>();

10
oh_my_loam/odometer/odometer.cc
View File

@ -22,14 +22,14 @@ bool Odometer::Init() {
} }
void Odometer::Process(double timestamp, const std::vector<Feature> &features, void Odometer::Process(double timestamp, const std::vector<Feature> &features,
Pose3d *const pose) { Pose3d *const pose_out) {
BLOCK_TIMER_START; BLOCK_TIMER_START;
if (!is_initialized_) { if (!is_initialized_) {
UpdatePre(features); UpdatePre(features);
is_initialized_ = true; is_initialized_ = true;
pose_curr2last_ = Pose3d::Identity(); pose_curr2last_.SetIdentity();
pose_curr2world_ = Pose3d::Identity(); pose_curr2world_.SetIdentity();
*pose = Pose3d::Identity(); pose_out->SetIdentity();
AWARN << "Odometer initialized...."; AWARN << "Odometer initialized....";
return; return;
} }
@ -59,7 +59,7 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
solver.Solve(5, verbose_, &pose_curr2last_); solver.Solve(5, verbose_, &pose_curr2last_);
} }
pose_curr2world_ = pose_curr2world_ * pose_curr2last_; pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
*pose = pose_curr2world_; *pose_out = pose_curr2world_;
AINFO_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString(); AINFO_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString();
AINFO_IF(verbose_) << "Pose after: " << pose_curr2world_.ToString(); AINFO_IF(verbose_) << "Pose after: " << pose_curr2world_.ToString();
UpdatePre(features); UpdatePre(features);

4
oh_my_loam/odometer/odometer.h
View File

@ -16,7 +16,7 @@ class Odometer {
bool Init(); bool Init();
void Process(double timestamp, const std::vector<Feature> &features, void Process(double timestamp, const std::vector<Feature> &features,
common::Pose3d *const pose); common::Pose3d *const pose_out);
protected: protected:
void UpdatePre(const std::vector<Feature> &features); void UpdatePre(const std::vector<Feature> &features);
@ -41,6 +41,8 @@ class Odometer {
std::vector<TPointCloudPtr> clouds_corn_pre_; std::vector<TPointCloudPtr> clouds_corn_pre_;
std::vector<TPointCloudPtr> clouds_surf_pre_; std::vector<TPointCloudPtr> clouds_surf_pre_;
TPointCloudPtr corn_pre_;
TPointCloudPtr surf_pre_;
std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_surf_; std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_surf_;
std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_corn_; std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_corn_;

8
oh_my_loam/oh_my_loam.cc
View File

@ -30,7 +30,7 @@ bool OhMyLoam::Init() {
return true; return true;
} }
void OhMyLoam::Run(double timestamp, const PointCloudConstPtr& cloud_in) { void OhMyLoam::Run(double timestamp, const PointCloudConstPtr &cloud_in) {
PointCloudPtr cloud(new PointCloud); PointCloudPtr cloud(new PointCloud);
RemoveOutliers(*cloud_in, cloud.get()); RemoveOutliers(*cloud_in, cloud.get());
std::vector<Feature> features; std::vector<Feature> features;
@ -40,9 +40,9 @@ void OhMyLoam::Run(double timestamp, const PointCloudConstPtr& cloud_in) {
poses_.emplace_back(pose); poses_.emplace_back(pose);
} }
void OhMyLoam::RemoveOutliers(const PointCloud& cloud_in, void OhMyLoam::RemoveOutliers(const PointCloud &cloud_in,
PointCloud* const cloud_out) const { PointCloud *const cloud_out) const {
RemovePoints<Point>(cloud_in, cloud_out, [&](const Point& pt) { RemovePoints<Point>(cloud_in, cloud_out, [&](const Point &pt) {
return !IsFinite<Point>(pt) || return !IsFinite<Point>(pt) ||
DistanceSqure<Point>(pt) < kPointMinDist * kPointMinDist; DistanceSqure<Point>(pt) < kPointMinDist * kPointMinDist;
}); });

6
oh_my_loam/oh_my_loam.h
View File

@ -13,7 +13,7 @@ class OhMyLoam {
bool Init(); bool Init();
void Run(double timestamp, const common::PointCloudConstPtr& cloud_in); void Run(double timestamp, const common::PointCloudConstPtr &cloud_in);
private: private:
std::unique_ptr<Extractor> extractor_{nullptr}; std::unique_ptr<Extractor> extractor_{nullptr};
@ -23,8 +23,8 @@ class OhMyLoam {
// std::unique_ptr<Mapper> mapper_{nullptr}; // std::unique_ptr<Mapper> mapper_{nullptr};
// remove outliers: nan and very close points // remove outliers: nan and very close points
void RemoveOutliers(const common::PointCloud& cloud_in, void RemoveOutliers(const common::PointCloud &cloud_in,
common::PointCloud* const cloud_out) const; common::PointCloud *const cloud_out) const;
std::vector<common::Pose3d> poses_; std::vector<common::Pose3d> poses_;

20
oh_my_loam/solver/cost_function.h
View File

@ -9,13 +9,13 @@ namespace oh_my_loam {
class PointLineCostFunction { class PointLineCostFunction {
public: public:
PointLineCostFunction(const PointLinePair& pair, double time) PointLineCostFunction(const PointLinePair &pair, double time)
: pair_(pair), time_(time){}; : pair_(pair), time_(time){};
template <typename T> template <typename T>
bool operator()(const T* const q, const T* const p, T* residual) const; bool operator()(const T *const q, const T *const p, T *residual) const;
static ceres::CostFunction* Create(const PointLinePair& pair, double time) { static ceres::CostFunction *Create(const PointLinePair &pair, double time) {
return new ceres::AutoDiffCostFunction<PointLineCostFunction, 3, 4, 3>( return new ceres::AutoDiffCostFunction<PointLineCostFunction, 3, 4, 3>(
new PointLineCostFunction(pair, time)); new PointLineCostFunction(pair, time));
} }
@ -29,13 +29,13 @@ class PointLineCostFunction {
class PointPlaneCostFunction { class PointPlaneCostFunction {
public: public:
PointPlaneCostFunction(const PointPlanePair& pair, double time) PointPlaneCostFunction(const PointPlanePair &pair, double time)
: pair_(pair), time_(time){}; : pair_(pair), time_(time){};
template <typename T> template <typename T>
bool operator()(const T* const q, const T* const p, T* residual) const; bool operator()(const T *const q, const T *const p, T *residual) const;
static ceres::CostFunction* Create(const PointPlanePair& pair, double time) { static ceres::CostFunction *Create(const PointPlanePair &pair, double time) {
return new ceres::AutoDiffCostFunction<PointPlaneCostFunction, 1, 4, 3>( return new ceres::AutoDiffCostFunction<PointPlaneCostFunction, 1, 4, 3>(
new PointPlaneCostFunction(pair, time)); new PointPlaneCostFunction(pair, time));
} }
@ -47,8 +47,8 @@ class PointPlaneCostFunction {
}; };
template <typename T> template <typename T>
bool PointLineCostFunction::operator()(const T* const q, const T* const p, bool PointLineCostFunction::operator()(const T *const q, const T *const p,
T* residual) const { T *residual) const {
const auto pt = pair_.pt, pt1 = pair_.line.pt1, pt2 = pair_.line.pt2; const auto pt = pair_.pt, pt1 = pair_.line.pt1, pt2 = pair_.line.pt2;
Eigen::Matrix<T, 3, 1> pnt(T(pt.x), T(pt.y), T(pt.z)); Eigen::Matrix<T, 3, 1> pnt(T(pt.x), T(pt.y), T(pt.z));
Eigen::Matrix<T, 3, 1> pnt1(T(pt1.x), T(pt1.y), T(pt1.z)); Eigen::Matrix<T, 3, 1> pnt1(T(pt1.x), T(pt1.y), T(pt1.z));
@ -70,8 +70,8 @@ bool PointLineCostFunction::operator()(const T* const q, const T* const p,
} }
template <typename T> template <typename T>
bool PointPlaneCostFunction::operator()(const T* const q, const T* const p, bool PointPlaneCostFunction::operator()(const T *const q, const T *const p,
T* residual) const { T *residual) const {
const auto &pt = pair_.pt, &pt1 = pair_.plane.pt1, &pt2 = pair_.plane.pt2, const auto &pt = pair_.pt, &pt1 = pair_.plane.pt1, &pt2 = pair_.plane.pt2,
&pt3 = pair_.plane.pt3; &pt3 = pair_.plane.pt3;
Eigen::Matrix<T, 3, 1> pnt(T(pt.x), T(pt.y), T(pt.z)); Eigen::Matrix<T, 3, 1> pnt(T(pt.x), T(pt.y), T(pt.z));

6
oh_my_loam/solver/solver.h
View File

@ -6,7 +6,7 @@
namespace oh_my_loam { namespace oh_my_loam {
class PoseSolver { class PoseSolver {
public: public:
PoseSolver(const common::Pose3d &pose); PoseSolver(const common::Pose3d &pose);
void AddPointLinePair(const PointLinePair &pair, double time); void AddPointLinePair(const PointLinePair &pair, double time);
@ -18,7 +18,7 @@ public:
common::Pose3d GetPose() const; common::Pose3d GetPose() const;
private: private:
ceres::Problem problem_; ceres::Problem problem_;
ceres::LossFunction *loss_function_; ceres::LossFunction *loss_function_;
@ -29,4 +29,4 @@ private:
DISALLOW_COPY_AND_ASSIGN(PoseSolver) DISALLOW_COPY_AND_ASSIGN(PoseSolver)
}; };
} // namespace oh_my_loam } // namespace oh_my_loam

2
oh_my_loam/visualizer/extractor_visualizer.cc
View File

@ -9,7 +9,7 @@ void ExtractorVisualizer::Draw() {
auto frame = GetCurrentFrame<ExtractorVisFrame>(); auto frame = GetCurrentFrame<ExtractorVisFrame>();
DrawPointCloud<Point>(frame.cloud, GRAY, "cloud_raw"); DrawPointCloud<Point>(frame.cloud, GRAY, "cloud_raw");
for (size_t i = 0; i < frame.features.size(); ++i) { for (size_t i = 0; i < frame.features.size(); ++i) {
const auto& feature = frame.features[i]; const auto &feature = frame.features[i];
std::string id_suffix = std::to_string(i); std::string id_suffix = std::to_string(i);
DrawPointCloud<TPoint>(feature.cloud_surf, BLUE, "cloud_surf" + id_suffix); DrawPointCloud<TPoint>(feature.cloud_surf, BLUE, "cloud_surf" + id_suffix);
DrawPointCloud<TPoint>(feature.cloud_flat_surf, CYAN, DrawPointCloud<TPoint>(feature.cloud_flat_surf, CYAN,

6
oh_my_loam/visualizer/odometer_visualizer.cc
View File

@ -13,7 +13,7 @@ void OdometerVisualizer::Draw() {
src_corn_pts->resize(frame.pl_pairs.size()); src_corn_pts->resize(frame.pl_pairs.size());
tgt_corn_pts->resize(frame.pl_pairs.size() * 2); tgt_corn_pts->resize(frame.pl_pairs.size() * 2);
for (size_t i = 0; i < frame.pl_pairs.size(); ++i) { for (size_t i = 0; i < frame.pl_pairs.size(); ++i) {
const auto& pair = frame.pl_pairs[i]; const auto &pair = frame.pl_pairs[i];
TransformToStart(frame.pose_curr2last, pair.pt, &src_corn_pts->points[i]); TransformToStart(frame.pose_curr2last, pair.pt, &src_corn_pts->points[i]);
tgt_corn_pts->points[2 * i] = pair.line.pt1; tgt_corn_pts->points[2 * i] = pair.line.pt1;
tgt_corn_pts->points[2 * i + 1] = pair.line.pt2; tgt_corn_pts->points[2 * i + 1] = pair.line.pt2;
@ -23,7 +23,7 @@ void OdometerVisualizer::Draw() {
src_surf_pts->resize(frame.pp_pairs.size()); src_surf_pts->resize(frame.pp_pairs.size());
tgt_surf_pts->resize(frame.pp_pairs.size() * 3); tgt_surf_pts->resize(frame.pp_pairs.size() * 3);
for (size_t i = 0; i < frame.pp_pairs.size(); ++i) { for (size_t i = 0; i < frame.pp_pairs.size(); ++i) {
const auto& pair = frame.pp_pairs[i]; const auto &pair = frame.pp_pairs[i];
TransformToStart(frame.pose_curr2last, pair.pt, &src_surf_pts->points[i]); TransformToStart(frame.pose_curr2last, pair.pt, &src_surf_pts->points[i]);
tgt_surf_pts->points[3 * i] = pair.plane.pt1; tgt_surf_pts->points[3 * i] = pair.plane.pt1;
tgt_surf_pts->points[3 * i + 1] = pair.plane.pt2; tgt_surf_pts->points[3 * i + 1] = pair.plane.pt2;
@ -36,7 +36,7 @@ void OdometerVisualizer::Draw() {
std::vector<Pose3d> poses_n; std::vector<Pose3d> poses_n;
poses_n.reserve((poses_.size())); poses_n.reserve((poses_.size()));
Pose3d pose_inv = frame.pose_curr2world.Inv(); Pose3d pose_inv = frame.pose_curr2world.Inv();
for (const auto& pose : poses_) { for (const auto &pose : poses_) {
poses_n.emplace_back(pose_inv * pose); poses_n.emplace_back(pose_inv * pose);
}; };
for (size_t i = 0; i < poses_n.size(); ++i) { for (size_t i = 0; i < poses_n.size(); ++i) {