From 12054bf10f63598d57de77f215fa97638fa9e824 Mon Sep 17 00:00:00 2001 From: feixyz10 Date: Fri, 22 Jan 2021 16:33:55 +0800 Subject: [PATCH] format all .h .cc files --- common/config/yaml_config.h | 6 +-- common/geometry/pose3d.h | 32 ++++++++-------- common/log/log.cc | 4 +- common/log/log.h | 12 +++--- common/math/math_utils.cc | 14 +++++-- common/pcl/pcl_utils.h | 22 +++++------ common/time/timer.h | 10 +++-- common/visualizer/lidar_visualizer.h | 4 +- common/visualizer/lidar_visualizer_utils.cc | 10 ++--- common/visualizer/lidar_visualizer_utils.h | 22 +++++------ main.cc | 10 ++--- oh_my_loam/configs/config.yaml | 2 +- oh_my_loam/extractor/extractor.cc | 38 +++++++++---------- oh_my_loam/extractor/extractor.h | 30 ++++++++------- oh_my_loam/extractor/extractor_VLP16.cc | 2 +- oh_my_loam/extractor/extractor_VLP16.h | 6 ++- oh_my_loam/mapper/mapper.cc | 2 +- oh_my_loam/odometer/odometer.cc | 10 ++--- oh_my_loam/odometer/odometer.h | 4 +- oh_my_loam/oh_my_loam.cc | 8 ++-- oh_my_loam/oh_my_loam.h | 6 +-- oh_my_loam/solver/cost_function.h | 20 +++++----- oh_my_loam/solver/solver.h | 6 +-- oh_my_loam/visualizer/extractor_visualizer.cc | 2 +- oh_my_loam/visualizer/odometer_visualizer.cc | 6 +-- 25 files changed, 154 insertions(+), 134 deletions(-) diff --git a/common/config/yaml_config.h b/common/config/yaml_config.h index b186a65..a2378bb 100644 --- a/common/config/yaml_config.h +++ b/common/config/yaml_config.h @@ -11,18 +11,18 @@ namespace common { class YAMLConfig { public: - void Init(const std::string& file) { + void Init(const std::string &file) { config_.reset(new YAML::Node); *config_ = YAML::LoadFile(file); } template - 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."; return (*config_)[key].as(); } - const YAML::Node& config() const { + const YAML::Node &config() const { AFATAL_IF(!config_) << "Not initialized, please call Init first."; return *config_; } diff --git a/common/geometry/pose3d.h b/common/geometry/pose3d.h index b2d58eb..c2ff21b 100644 --- a/common/geometry/pose3d.h +++ b/common/geometry/pose3d.h @@ -12,18 +12,16 @@ class Pose3d { 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) {} - 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) {} - 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) {} - static Pose3d Identity() { return {}; } - - void setIdentity() { + void SetIdentity() { r_quat_.setIdentity(); t_vec_.setZero(); } @@ -34,30 +32,30 @@ class Pose3d { 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_); } - Pose3d& operator*=(const Pose3d& rhs) { + Pose3d &operator*=(const Pose3d &rhs) { t_vec_ += r_quat_ * rhs.t_vec_; r_quat_ *= rhs.r_quat_; return *this; } - Eigen::Vector3d Transform(const Eigen::Vector3d& point) const { + Eigen::Vector3d Transform(const Eigen::Vector3d &point) const { 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); } - Eigen::Vector3d Rotate(const Eigen::Vector3d& vec) const { + Eigen::Vector3d Rotate(const Eigen::Vector3d &vec) const { return r_quat_ * vec; } // 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); t = t > 0 ? t : -t; int whole = std::floor(t); @@ -79,18 +77,22 @@ class Pose3d { // 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_; } - Eigen::Vector3d t_vec() const { return t_vec_; } + Eigen::Vector3d t_vec() const { + return t_vec_; + } protected: Eigen::Quaterniond r_quat_; // orientation/rotation 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) { return pose_from.Interpolate(pose_to, t); } diff --git a/common/log/log.cc b/common/log/log.cc index a9bc7c3..73ded48 100644 --- a/common/log/log.cc +++ b/common/log/log.cc @@ -3,8 +3,8 @@ namespace common { -void InitG3Logging(bool log_to_file, const std::string& prefix, - const std::string& path) { +void InitG3Logging(bool log_to_file, const std::string &prefix, + const std::string &path) { static std::unique_ptr worker; if (worker != nullptr) return; worker = std::move(g3::LogWorker::createLogWorker()); diff --git a/common/log/log.h b/common/log/log.h index 42dcb4d..013d728 100644 --- a/common/log/log.h +++ b/common/log/log.h @@ -44,8 +44,8 @@ const LEVELS USER(ERROR.value + 100, "USER"); #define ACHECK(cond) G3CHECK(cond) namespace common { -void InitG3Logging(bool log_to_file = false, const std::string& prefix = "", - const std::string& path = "./"); +void InitG3Logging(bool log_to_file = false, const std::string &prefix = "", + const std::string &path = "./"); } // namespace common namespace g3 { @@ -53,7 +53,7 @@ class CustomSink { public: 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) { ofs_.reset(new std::ofstream(log_file_name)); } @@ -86,20 +86,20 @@ class CustomSink { bool log_to_file_{false}; std::unique_ptr ofs_{nullptr}; - std::string FormatedMessage(const g3::LogMessage& msg) const { + std::string FormatedMessage(const g3::LogMessage &msg) const { std::ostringstream oss; oss << "[" << msg.level()[0] << msg.timestamp("%Y%m%d %H:%M:%S.%f3") << " " << msg.file() << ":" << msg.line() << "] " << msg.message(); return oss.str(); } - std::string ColorFormatedMessage(const g3::LogMessage& msg) const { + std::string ColorFormatedMessage(const g3::LogMessage &msg) const { std::ostringstream oss; oss << GetColorCode(msg._level) << FormatedMessage(msg) << "\033[m"; return oss.str(); } - std::string GetColorCode(const LEVELS& level) const { + std::string GetColorCode(const LEVELS &level) const { if (level.value == WARNING.value) { return "\033[33m"; // yellow } diff --git a/common/math/math_utils.cc b/common/math/math_utils.cc index b13b06b..a637a1b 100644 --- a/common/math/math_utils.cc +++ b/common/math/math_utils.cc @@ -5,13 +5,17 @@ namespace common { 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); } -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 Range(int begin, int end, int step) { ACHECK(step != 0) << "Step must non-zero"; @@ -23,6 +27,8 @@ const std::vector Range(int begin, int end, int step) { return seq; } -const std::vector Range(int end) { return Range(0, end, 1); } +const std::vector Range(int end) { + return Range(0, end, 1); +} } // namespace common \ No newline at end of file diff --git a/common/pcl/pcl_utils.h b/common/pcl/pcl_utils.h index 9a4a46c..1540cb4 100644 --- a/common/pcl/pcl_utils.h +++ b/common/pcl/pcl_utils.h @@ -7,41 +7,41 @@ namespace common { // Distance squred of a point to origin template -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; } // Distance squred of two points template -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) + (pt1.z - pt2.z) * (pt1.z - pt2.z); } // Distance of a point to origin template -inline double Distance(const PointType& pt) { +inline double Distance(const PointType &pt) { return std::sqrt(DistanceSqure(pt)); } // Distance squred of two points template -inline double Distance(const PointType& pt1, const PointType& pt2) { +inline double Distance(const PointType &pt1, const PointType &pt2) { return std::sqrt(DistanceSqure(pt1, pt2)); } // Check whether is a finite point: neither infinite nor nan template -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); } // Remove point if the condition evaluated to true on it template -void RemovePoints(const pcl::PointCloud& cloud_in, - pcl::PointCloud* const cloud_out, - std::function check, - std::vector* const removed_indices = nullptr) { +void RemovePoints(const pcl::PointCloud &cloud_in, + pcl::PointCloud *const cloud_out, + std::function check, + std::vector *const removed_indices = nullptr) { if (&cloud_in != cloud_out) { cloud_out->header = cloud_in.header; cloud_out->resize(cloud_in.size()); @@ -64,8 +64,8 @@ void RemovePoints(const pcl::PointCloud& cloud_in, template void VoxelDownSample( - const typename pcl::PointCloud::ConstPtr& cloud_in, - pcl::PointCloud* const cloud_out, double voxel_size) { + const typename pcl::PointCloud::ConstPtr &cloud_in, + pcl::PointCloud *const cloud_out, double voxel_size) { pcl::VoxelGrid filter; filter.setInputCloud(cloud_in); filter.setLeafSize(voxel_size, voxel_size, voxel_size); diff --git a/common/time/timer.h b/common/time/timer.h index b49a162..833f2b3 100644 --- a/common/time/timer.h +++ b/common/time/timer.h @@ -9,9 +9,13 @@ namespace common { class Timer { 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 double Toc(char unit = 'm'); @@ -23,7 +27,7 @@ class Timer { class TimerWrapper { 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) { timer_.Tic(); }; diff --git a/common/visualizer/lidar_visualizer.h b/common/visualizer/lidar_visualizer.h index 0edefdc..1fcf920 100644 --- a/common/visualizer/lidar_visualizer.h +++ b/common/visualizer/lidar_visualizer.h @@ -55,7 +55,9 @@ class LidarVisualizer { is_updated_ = true; } - std::string name() const { return name_; } + std::string name() const { + return name_; + } protected: void Run() { diff --git a/common/visualizer/lidar_visualizer_utils.cc b/common/visualizer/lidar_visualizer_utils.cc index c9fa4b9..fcefcc6 100644 --- a/common/visualizer/lidar_visualizer_utils.cc +++ b/common/visualizer/lidar_visualizer_utils.cc @@ -2,14 +2,14 @@ namespace common { -void AddLine(const pcl::PointXYZ& pt1, const pcl::PointXYZ& pt2, - const Color& color, const std::string& id, - PCLVisualizer* const viewer) { +void AddLine(const pcl::PointXYZ &pt1, const pcl::PointXYZ &pt2, + const Color &color, const std::string &id, + PCLVisualizer *const viewer) { viewer->addLine(pt1, pt2, color.r, color.g, color.b, id); } -void AddSphere(const pcl::PointXYZ& center, double radius, const Color& color, - const std::string& id, PCLVisualizer* const viewer) { +void AddSphere(const pcl::PointXYZ ¢er, double radius, const Color &color, + const std::string &id, PCLVisualizer *const viewer) { viewer->addSphere(center, radius, color.r, color.g, color.b, id); } diff --git a/common/visualizer/lidar_visualizer_utils.h b/common/visualizer/lidar_visualizer_utils.h index 32f5778..626a2b1 100644 --- a/common/visualizer/lidar_visualizer_utils.h +++ b/common/visualizer/lidar_visualizer_utils.h @@ -18,9 +18,9 @@ using PCLVisualizer = pcl::visualization::PCLVisualizer; pcl::visualization::PointCloudColorHandlerGenericField template -void AddPointCloud(const typename pcl::PointCloud::ConstPtr& cloud, - const Color& color, const std::string& id, - PCLVisualizer* const viewer, int point_size = 3) { +void AddPointCloud(const typename pcl::PointCloud::ConstPtr &cloud, + const Color &color, const std::string &id, + PCLVisualizer *const viewer, int point_size = 3) { PCLColorHandlerCustom color_handler(cloud, color.r, color.g, color.b); viewer->addPointCloud(cloud, color_handler, id); @@ -29,20 +29,20 @@ void AddPointCloud(const typename pcl::PointCloud::ConstPtr& cloud, } template -void AddPointCloud(const typename pcl::PointCloud::ConstPtr& cloud, - const std::string& field, const std::string& id, - PCLVisualizer* const viewer, int point_size = 3) { +void AddPointCloud(const typename pcl::PointCloud::ConstPtr &cloud, + const std::string &field, const std::string &id, + PCLVisualizer *const viewer, int point_size = 3) { PCLColorHandlerGenericField color_handler(cloud, field); viewer->addPointCloud(cloud, color_handler, id); viewer->setPointCloudRenderingProperties( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id); } -void AddLine(const pcl::PointXYZ& pt1, const pcl::PointXYZ& pt2, - const Color& color, const std::string& id, - PCLVisualizer* const viewer); +void AddLine(const pcl::PointXYZ &pt1, const pcl::PointXYZ &pt2, + const Color &color, const std::string &id, + PCLVisualizer *const viewer); -void AddSphere(const pcl::PointXYZ& center, double radius, const Color& color, - const std::string& id, PCLVisualizer* const viewer); +void AddSphere(const pcl::PointXYZ ¢er, double radius, const Color &color, + const std::string &id, PCLVisualizer *const viewer); } // namespace common \ No newline at end of file diff --git a/main.cc b/main.cc index 6caa7a8..079e1ba 100644 --- a/main.cc +++ b/main.cc @@ -12,10 +12,10 @@ using namespace common; using namespace oh_my_loam; -void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg, - OhMyLoam* const slam); +void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr &msg, + OhMyLoam *const slam); -int main(int argc, char* argv[]) { +int main(int argc, char *argv[]) { // config YAMLConfig::Instance()->Init(argv[1]); bool log_to_file = YAMLConfig::Instance()->Get("log_to_file"); @@ -43,8 +43,8 @@ int main(int argc, char* argv[]) { return 0; } -void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg, - OhMyLoam* const slam) { +void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr &msg, + OhMyLoam *const slam) { PointCloudPtr cloud(new PointCloud); pcl::fromROSMsg(*msg, *cloud); double timestamp = msg->header.stamp.toSec(); diff --git a/oh_my_loam/configs/config.yaml b/oh_my_loam/configs/config.yaml index b66f6c0..a4ab950 100644 --- a/oh_my_loam/configs/config.yaml +++ b/oh_my_loam/configs/config.yaml @@ -12,7 +12,7 @@ extractor_config: sharp_corner_point_num: 2 corner_point_num: 20 flat_surf_point_num: 4 - surf_point_num: 20 + surf_point_num: 50 corner_point_curvature_th: 0.5 surf_point_curvature_th: 0.5 neighbor_point_dist_th: 0.1 diff --git a/oh_my_loam/extractor/extractor.cc b/oh_my_loam/extractor/extractor.cc index ca05953..19127c7 100644 --- a/oh_my_loam/extractor/extractor.cc +++ b/oh_my_loam/extractor/extractor.cc @@ -13,7 +13,7 @@ using namespace common; } // namespace bool Extractor::Init() { - const auto& config = YAMLConfig::Instance()->config(); + const auto &config = YAMLConfig::Instance()->config(); config_ = config["extractor_config"]; is_vis_ = config["vis"].as() && config_["vis"].as(); verbose_ = config_["verbose"].as(); @@ -22,8 +22,8 @@ bool Extractor::Init() { return true; } -void Extractor::Process(double timestamp, const PointCloudConstPtr& cloud, - std::vector* const features) { +void Extractor::Process(double timestamp, const PointCloudConstPtr &cloud, + std::vector *const features) { BLOCK_TIMER_START; if (cloud->size() < config_["min_point_num"].as()) { AWARN << "Too few input points: num = " << cloud->size() << " (< " @@ -35,12 +35,12 @@ void Extractor::Process(double timestamp, const PointCloudConstPtr& cloud, SplitScan(*cloud, &scans); AINFO_IF(verbose_) << "Extractor::SplitScan: " << BLOCK_TIMER_STOP_FMT; // compute curvature to each point - for (auto& scan : scans) { + for (auto &scan : scans) { ComputeCurvature(&scan); } AINFO_IF(verbose_) << "Extractor::ComputeCurvature: " << BLOCK_TIMER_STOP_FMT; // assign type to each point: FLAT, LESS_FLAT, NORMAL, LESS_SHARP or SHARP - for (auto& scan : scans) { + for (auto &scan : scans) { AssignType(&scan); } 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); } -void Extractor::SplitScan(const PointCloud& cloud, - std::vector* const scans) const { +void Extractor::SplitScan(const PointCloud &cloud, + std::vector *const scans) const { scans->resize(num_scans_); double yaw_start = -atan2(cloud.points[0].y, cloud.points[0].x); bool half_passed = false; - for (const auto& pt : cloud) { + for (const auto &pt : cloud) { int scan_id = GetScanID(pt); if (scan_id >= num_scans_ || scan_id < 0) continue; 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; - auto& pts = scan->points; + auto &pts = scan->points; 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 + 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].curvature = std::hypot(dx, dy, dz); } - RemovePoints(*scan, scan, [](const TCTPoint& pt) { + RemovePoints(*scan, scan, [](const TCTPoint &pt) { return !std::isfinite(pt.curvature); }); } -void Extractor::AssignType(TCTPointCloud* const scan) const { +void Extractor::AssignType(TCTPointCloud *const scan) const { int pt_num = scan->size(); if (pt_num < kScanSegNum) return; 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, - Feature* const feature) const { - for (const auto& pt : scan) { +void Extractor::GenerateFeature(const TCTPointCloud &scan, + Feature *const feature) const { + for (const auto &pt : scan) { TPoint point(pt.x, pt.y, pt.z, pt.time); switch (pt.type) { case PType::FLAT_SURF: @@ -182,8 +182,8 @@ void Extractor::GenerateFeature(const TCTPointCloud& scan, feature->cloud_surf->swap(*dowm_sampled); } -void Extractor::Visualize(const PointCloudConstPtr& cloud, - const std::vector& features, +void Extractor::Visualize(const PointCloudConstPtr &cloud, + const std::vector &features, double timestamp) { std::shared_ptr frame(new ExtractorVisFrame); frame->timestamp = timestamp; @@ -192,8 +192,8 @@ void Extractor::Visualize(const PointCloudConstPtr& cloud, visualizer_->Render(frame); } -void Extractor::UpdateNeighborsPicked(const TCTPointCloud& scan, size_t ix, - std::vector* const picked) const { +void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix, + std::vector *const picked) const { auto DistSqure = [&](int i, int j) -> float { return DistanceSqure(scan[i], scan[j]); }; diff --git a/oh_my_loam/extractor/extractor.h b/oh_my_loam/extractor/extractor.h index b18c8b7..00faf63 100644 --- a/oh_my_loam/extractor/extractor.h +++ b/oh_my_loam/extractor/extractor.h @@ -15,26 +15,28 @@ class Extractor { bool Init(); - void Process(double timestamp, const common::PointCloudConstPtr& cloud, - std::vector* const features); + void Process(double timestamp, const common::PointCloudConstPtr &cloud, + std::vector *const features); - int num_scans() const { return num_scans_; } + int num_scans() const { + return num_scans_; + } 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, - std::vector* const scans) const; + virtual void SplitScan(const common::PointCloud &cloud, + std::vector *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, - Feature* const feature) const; + virtual void GenerateFeature(const TCTPointCloud &scan, + Feature *const feature) const; - virtual void Visualize(const common::PointCloudConstPtr& cloud, - const std::vector& features, + virtual void Visualize(const common::PointCloudConstPtr &cloud, + const std::vector &features, double timestamp = 0.0); int num_scans_ = 0; @@ -46,8 +48,8 @@ class Extractor { bool verbose_ = false; private: - void UpdateNeighborsPicked(const TCTPointCloud& scan, size_t ix, - std::vector* const picked) const; + void UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix, + std::vector *const picked) const; bool is_vis_ = false; diff --git a/oh_my_loam/extractor/extractor_VLP16.cc b/oh_my_loam/extractor/extractor_VLP16.cc index 9b0c3d3..dc419d7 100644 --- a/oh_my_loam/extractor/extractor_VLP16.cc +++ b/oh_my_loam/extractor/extractor_VLP16.cc @@ -4,7 +4,7 @@ namespace oh_my_loam { -int ExtractorVLP16::GetScanID(const common::Point& pt) const { +int ExtractorVLP16::GetScanID(const common::Point &pt) const { double theta = common::Rad2Degree(std::atan2(pt.z, std::hypot(pt.x, pt.y))) + 15.0; return static_cast(std::round(theta / 2.0) + 1.e-5); diff --git a/oh_my_loam/extractor/extractor_VLP16.h b/oh_my_loam/extractor/extractor_VLP16.h index 31ed5f4..de5b44f 100644 --- a/oh_my_loam/extractor/extractor_VLP16.h +++ b/oh_my_loam/extractor/extractor_VLP16.h @@ -7,10 +7,12 @@ namespace oh_my_loam { // for VLP-16 class ExtractorVLP16 : public Extractor { public: - ExtractorVLP16() { num_scans_ = 16; } + ExtractorVLP16() { + num_scans_ = 16; + } private: - int GetScanID(const common::Point& pt) const override; + int GetScanID(const common::Point &pt) const override; }; } // namespace oh_my_loam \ No newline at end of file diff --git a/oh_my_loam/mapper/mapper.cc b/oh_my_loam/mapper/mapper.cc index ee7939d..9b9f2b4 100644 --- a/oh_my_loam/mapper/mapper.cc +++ b/oh_my_loam/mapper/mapper.cc @@ -7,7 +7,7 @@ using namespace common; } // namespace bool Mapper::Init() { - const auto& config = YAMLConfig::Instance()->config(); + const auto &config = YAMLConfig::Instance()->config(); config_ = config["mapper_config"]; is_vis_ = config["vis"].as() && config_["vis"].as(); verbose_ = config_["vis"].as(); diff --git a/oh_my_loam/odometer/odometer.cc b/oh_my_loam/odometer/odometer.cc index 9da0f78..1c3a059 100644 --- a/oh_my_loam/odometer/odometer.cc +++ b/oh_my_loam/odometer/odometer.cc @@ -22,14 +22,14 @@ bool Odometer::Init() { } void Odometer::Process(double timestamp, const std::vector &features, - Pose3d *const pose) { + Pose3d *const pose_out) { BLOCK_TIMER_START; if (!is_initialized_) { UpdatePre(features); is_initialized_ = true; - pose_curr2last_ = Pose3d::Identity(); - pose_curr2world_ = Pose3d::Identity(); - *pose = Pose3d::Identity(); + pose_curr2last_.SetIdentity(); + pose_curr2world_.SetIdentity(); + pose_out->SetIdentity(); AWARN << "Odometer initialized...."; return; } @@ -59,7 +59,7 @@ void Odometer::Process(double timestamp, const std::vector &features, solver.Solve(5, verbose_, &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 after: " << pose_curr2world_.ToString(); UpdatePre(features); diff --git a/oh_my_loam/odometer/odometer.h b/oh_my_loam/odometer/odometer.h index 5cf02b1..b89bf4a 100644 --- a/oh_my_loam/odometer/odometer.h +++ b/oh_my_loam/odometer/odometer.h @@ -16,7 +16,7 @@ class Odometer { bool Init(); void Process(double timestamp, const std::vector &features, - common::Pose3d *const pose); + common::Pose3d *const pose_out); protected: void UpdatePre(const std::vector &features); @@ -41,6 +41,8 @@ class Odometer { std::vector clouds_corn_pre_; std::vector clouds_surf_pre_; + TPointCloudPtr corn_pre_; + TPointCloudPtr surf_pre_; std::vector> kdtrees_surf_; std::vector> kdtrees_corn_; diff --git a/oh_my_loam/oh_my_loam.cc b/oh_my_loam/oh_my_loam.cc index b1d21e4..dc1f32e 100644 --- a/oh_my_loam/oh_my_loam.cc +++ b/oh_my_loam/oh_my_loam.cc @@ -30,7 +30,7 @@ bool OhMyLoam::Init() { return true; } -void OhMyLoam::Run(double timestamp, const PointCloudConstPtr& cloud_in) { +void OhMyLoam::Run(double timestamp, const PointCloudConstPtr &cloud_in) { PointCloudPtr cloud(new PointCloud); RemoveOutliers(*cloud_in, cloud.get()); std::vector features; @@ -40,9 +40,9 @@ void OhMyLoam::Run(double timestamp, const PointCloudConstPtr& cloud_in) { poses_.emplace_back(pose); } -void OhMyLoam::RemoveOutliers(const PointCloud& cloud_in, - PointCloud* const cloud_out) const { - RemovePoints(cloud_in, cloud_out, [&](const Point& pt) { +void OhMyLoam::RemoveOutliers(const PointCloud &cloud_in, + PointCloud *const cloud_out) const { + RemovePoints(cloud_in, cloud_out, [&](const Point &pt) { return !IsFinite(pt) || DistanceSqure(pt) < kPointMinDist * kPointMinDist; }); diff --git a/oh_my_loam/oh_my_loam.h b/oh_my_loam/oh_my_loam.h index 2d13c04..187441d 100644 --- a/oh_my_loam/oh_my_loam.h +++ b/oh_my_loam/oh_my_loam.h @@ -13,7 +13,7 @@ class OhMyLoam { bool Init(); - void Run(double timestamp, const common::PointCloudConstPtr& cloud_in); + void Run(double timestamp, const common::PointCloudConstPtr &cloud_in); private: std::unique_ptr extractor_{nullptr}; @@ -23,8 +23,8 @@ class OhMyLoam { // std::unique_ptr mapper_{nullptr}; // remove outliers: nan and very close points - void RemoveOutliers(const common::PointCloud& cloud_in, - common::PointCloud* const cloud_out) const; + void RemoveOutliers(const common::PointCloud &cloud_in, + common::PointCloud *const cloud_out) const; std::vector poses_; diff --git a/oh_my_loam/solver/cost_function.h b/oh_my_loam/solver/cost_function.h index 2b20097..ade3208 100644 --- a/oh_my_loam/solver/cost_function.h +++ b/oh_my_loam/solver/cost_function.h @@ -9,13 +9,13 @@ namespace oh_my_loam { class PointLineCostFunction { public: - PointLineCostFunction(const PointLinePair& pair, double time) + PointLineCostFunction(const PointLinePair &pair, double time) : pair_(pair), time_(time){}; template - 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( new PointLineCostFunction(pair, time)); } @@ -29,13 +29,13 @@ class PointLineCostFunction { class PointPlaneCostFunction { public: - PointPlaneCostFunction(const PointPlanePair& pair, double time) + PointPlaneCostFunction(const PointPlanePair &pair, double time) : pair_(pair), time_(time){}; template - 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( new PointPlaneCostFunction(pair, time)); } @@ -47,8 +47,8 @@ class PointPlaneCostFunction { }; template -bool PointLineCostFunction::operator()(const T* const q, const T* const p, - T* residual) const { +bool PointLineCostFunction::operator()(const T *const q, const T *const p, + T *residual) const { const auto pt = pair_.pt, pt1 = pair_.line.pt1, pt2 = pair_.line.pt2; Eigen::Matrix pnt(T(pt.x), T(pt.y), T(pt.z)); Eigen::Matrix 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 -bool PointPlaneCostFunction::operator()(const T* const q, const T* const p, - T* residual) const { +bool PointPlaneCostFunction::operator()(const T *const q, const T *const p, + T *residual) const { const auto &pt = pair_.pt, &pt1 = pair_.plane.pt1, &pt2 = pair_.plane.pt2, &pt3 = pair_.plane.pt3; Eigen::Matrix pnt(T(pt.x), T(pt.y), T(pt.z)); diff --git a/oh_my_loam/solver/solver.h b/oh_my_loam/solver/solver.h index da92071..9f91a8b 100644 --- a/oh_my_loam/solver/solver.h +++ b/oh_my_loam/solver/solver.h @@ -6,7 +6,7 @@ namespace oh_my_loam { class PoseSolver { -public: + public: PoseSolver(const common::Pose3d &pose); void AddPointLinePair(const PointLinePair &pair, double time); @@ -18,7 +18,7 @@ public: common::Pose3d GetPose() const; -private: + private: ceres::Problem problem_; ceres::LossFunction *loss_function_; @@ -29,4 +29,4 @@ private: DISALLOW_COPY_AND_ASSIGN(PoseSolver) }; -} // namespace oh_my_loam \ No newline at end of file +} // namespace oh_my_loam \ No newline at end of file diff --git a/oh_my_loam/visualizer/extractor_visualizer.cc b/oh_my_loam/visualizer/extractor_visualizer.cc index ae451b9..c148740 100644 --- a/oh_my_loam/visualizer/extractor_visualizer.cc +++ b/oh_my_loam/visualizer/extractor_visualizer.cc @@ -9,7 +9,7 @@ void ExtractorVisualizer::Draw() { auto frame = GetCurrentFrame(); DrawPointCloud(frame.cloud, GRAY, "cloud_raw"); 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); DrawPointCloud(feature.cloud_surf, BLUE, "cloud_surf" + id_suffix); DrawPointCloud(feature.cloud_flat_surf, CYAN, diff --git a/oh_my_loam/visualizer/odometer_visualizer.cc b/oh_my_loam/visualizer/odometer_visualizer.cc index 8dfff95..b6bd80c 100644 --- a/oh_my_loam/visualizer/odometer_visualizer.cc +++ b/oh_my_loam/visualizer/odometer_visualizer.cc @@ -13,7 +13,7 @@ void OdometerVisualizer::Draw() { src_corn_pts->resize(frame.pl_pairs.size()); tgt_corn_pts->resize(frame.pl_pairs.size() * 2); 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]); tgt_corn_pts->points[2 * i] = pair.line.pt1; 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()); tgt_surf_pts->resize(frame.pp_pairs.size() * 3); 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]); tgt_surf_pts->points[3 * i] = pair.plane.pt1; tgt_surf_pts->points[3 * i + 1] = pair.plane.pt2; @@ -36,7 +36,7 @@ void OdometerVisualizer::Draw() { std::vector poses_n; poses_n.reserve((poses_.size())); Pose3d pose_inv = frame.pose_curr2world.Inv(); - for (const auto& pose : poses_) { + for (const auto &pose : poses_) { poses_n.emplace_back(pose_inv * pose); }; for (size_t i = 0; i < poses_n.size(); ++i) {