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 {
public:
void Init(const std::string& file) {
void Init(const std::string &file) {
config_.reset(new YAML::Node);
*config_ = YAML::LoadFile(file);
}
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.";
return (*config_)[key].as<T>();
}
const YAML::Node& config() const {
const YAML::Node &config() const {
AFATAL_IF(!config_) << "Not initialized, please call Init first.";
return *config_;
}

32
common/geometry/pose3d.h
View File

@ -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);
}

4
common/log/log.cc
View File

@ -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<g3::LogWorker> worker;
if (worker != nullptr) return;
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)
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<std::ofstream> 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
}

14
common/math/math_utils.cc
View File

@ -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<int> Range(int begin, int end, int step) {
ACHECK(step != 0) << "Step must non-zero";
@ -23,6 +27,8 @@ const std::vector<int> Range(int begin, int end, int step) {
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

22
common/pcl/pcl_utils.h
View File

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

10
common/time/timer.h
View File

@ -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();
};

4
common/visualizer/lidar_visualizer.h
View File

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

10
common/visualizer/lidar_visualizer_utils.cc
View File

@ -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 &center, double radius, const Color &color,
const std::string &id, PCLVisualizer *const viewer) {
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
template <typename PointType>
void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr& cloud,
const Color& color, const std::string& id,
PCLVisualizer* const viewer, int point_size = 3) {
void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr &cloud,
const Color &color, const std::string &id,
PCLVisualizer *const viewer, int point_size = 3) {
PCLColorHandlerCustom<PointType> color_handler(cloud, color.r, color.g,
color.b);
viewer->addPointCloud<PointType>(cloud, color_handler, id);
@ -29,20 +29,20 @@ void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr& cloud,
}
template <typename PointType>
void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr& cloud,
const std::string& field, const std::string& id,
PCLVisualizer* const viewer, int point_size = 3) {
void AddPointCloud(const typename pcl::PointCloud<PointType>::ConstPtr &cloud,
const std::string &field, const std::string &id,
PCLVisualizer *const viewer, int point_size = 3) {
PCLColorHandlerGenericField<PointType> color_handler(cloud, field);
viewer->addPointCloud<PointType>(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 &center, double radius, const Color &color,
const std::string &id, PCLVisualizer *const viewer);
} // namespace common

10
main.cc
View File

@ -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<bool>("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();

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
surf_point_num: 50
corner_point_curvature_th: 0.5
surf_point_curvature_th: 0.5
neighbor_point_dist_th: 0.1

38
oh_my_loam/extractor/extractor.cc
View File

@ -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<bool>() && config_["vis"].as<bool>();
verbose_ = config_["verbose"].as<bool>();
@ -22,8 +22,8 @@ bool Extractor::Init() {
return true;
}
void Extractor::Process(double timestamp, const PointCloudConstPtr& cloud,
std::vector<Feature>* const features) {
void Extractor::Process(double timestamp, const PointCloudConstPtr &cloud,
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() << " (< "
@ -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<TCTPointCloud>* const scans) const {
void Extractor::SplitScan(const PointCloud &cloud,
std::vector<TCTPointCloud> *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<TCTPoint>(*scan, scan, [](const TCTPoint& pt) {
RemovePoints<TCTPoint>(*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<Feature>& features,
void Extractor::Visualize(const PointCloudConstPtr &cloud,
const std::vector<Feature> &features,
double timestamp) {
std::shared_ptr<ExtractorVisFrame> 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<bool>* const picked) const {
void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix,
std::vector<bool> *const picked) const {
auto DistSqure = [&](int i, int j) -> float {
return DistanceSqure<TCTPoint>(scan[i], scan[j]);
};

30
oh_my_loam/extractor/extractor.h
View File

@ -15,26 +15,28 @@ class Extractor {
bool Init();
void Process(double timestamp, const common::PointCloudConstPtr& cloud,
std::vector<Feature>* const features);
void Process(double timestamp, const common::PointCloudConstPtr &cloud,
std::vector<Feature> *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<TCTPointCloud>* const scans) const;
virtual void SplitScan(const common::PointCloud &cloud,
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,
Feature* const feature) const;
virtual void GenerateFeature(const TCTPointCloud &scan,
Feature *const feature) const;
virtual void Visualize(const common::PointCloudConstPtr& cloud,
const std::vector<Feature>& features,
virtual void Visualize(const common::PointCloudConstPtr &cloud,
const std::vector<Feature> &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<bool>* const picked) const;
void UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix,
std::vector<bool> *const picked) const;
bool is_vis_ = false;

2
oh_my_loam/extractor/extractor_VLP16.cc
View File

@ -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<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
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

2
oh_my_loam/mapper/mapper.cc
View File

@ -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<bool>() && 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,
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<Feature> &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);

4
oh_my_loam/odometer/odometer.h
View File

@ -16,7 +16,7 @@ class Odometer {
bool Init();
void Process(double timestamp, const std::vector<Feature> &features,
common::Pose3d *const pose);
common::Pose3d *const pose_out);
protected:
void UpdatePre(const std::vector<Feature> &features);
@ -41,6 +41,8 @@ class Odometer {
std::vector<TPointCloudPtr> clouds_corn_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_corn_;

8
oh_my_loam/oh_my_loam.cc
View File

@ -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<Feature> 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<Point>(cloud_in, cloud_out, [&](const Point& pt) {
void OhMyLoam::RemoveOutliers(const PointCloud &cloud_in,
PointCloud *const cloud_out) const {
RemovePoints<Point>(cloud_in, cloud_out, [&](const Point &pt) {
return !IsFinite<Point>(pt) ||
DistanceSqure<Point>(pt) < kPointMinDist * kPointMinDist;
});

6
oh_my_loam/oh_my_loam.h
View File

@ -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> extractor_{nullptr};
@ -23,8 +23,8 @@ class OhMyLoam {
// 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;
void RemoveOutliers(const common::PointCloud &cloud_in,
common::PointCloud *const cloud_out) const;
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 {
public:
PointLineCostFunction(const PointLinePair& pair, double time)
PointLineCostFunction(const PointLinePair &pair, double time)
: pair_(pair), time_(time){};
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>(
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 <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>(
new PointPlaneCostFunction(pair, time));
}
@ -47,8 +47,8 @@ class PointPlaneCostFunction {
};
template <typename T>
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<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));
@ -70,8 +70,8 @@ bool PointLineCostFunction::operator()(const T* const q, const T* const p,
}
template <typename T>
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<T, 3, 1> pnt(T(pt.x), T(pt.y), T(pt.z));

4
oh_my_loam/solver/solver.h
View File

@ -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_;

2
oh_my_loam/visualizer/extractor_visualizer.cc
View File

@ -9,7 +9,7 @@ void ExtractorVisualizer::Draw() {
auto frame = GetCurrentFrame<ExtractorVisFrame>();
DrawPointCloud<Point>(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<TPoint>(feature.cloud_surf, BLUE, "cloud_surf" + id_suffix);
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());
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<Pose3d> 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) {