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

move PointXYZT from common to oh_my_loam

main
feixyz10 2021-01-22 15:53:21 +08:00 committed by feixyz
parent 3c14de4bce
commit 76dfa7f447
12 changed files with 179 additions and 176 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.gitignore vendored
View File

@ -1,4 +1,8 @@
/.vscode
/.log
*.pcd
/build
/build
/.clangd
compile_commands.json
.clang-tidy
.clang-format

6
CMakeLists.txt
View File

@ -18,9 +18,6 @@ find_package(catkin REQUIRED COMPONENTS
rospy
rosbag
std_msgs
image_transport
cv_bridge
tf
)
include_directories(SYSTEM
@ -29,9 +26,6 @@ include_directories(SYSTEM
${G3LOG_INCLUDE_DIRS}
)
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
catkin_package(
CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
DEPENDS EIGEN3 PCL

60
common/pcl/pcl_types.h
View File

@ -17,60 +17,12 @@ using Point = pcl::PointXYZ;
using PointCloud = pcl::PointCloud<Point>;
using PointCloudPtr = PointCloud::Ptr;
using PointCloudConstPtr = PointCloud::ConstPtr;
using Indices = std::vector<int>;
struct PointXYZT;
using TPoint = PointXYZT;
using TPointCloud = pcl::PointCloud<TPoint>;
using TPointCloudPtr = TPointCloud::Ptr;
using TPointCloudConstPtr = TPointCloud::ConstPtr;
using Point2D = pcl::PointXY;
using PointCloud2D = pcl::PointCloud<Point2D>;
using PointCloud2DPtr = PointCloud2D::Ptr;
using PointCloud2DConstPtr = PointCloud2D::ConstPtr;
struct EIGEN_ALIGN16 PointXYZT {
PCL_ADD_POINT4D;
union {
float time;
float intensity;
float data_c[4];
};
PointXYZT() {
x = y = z = 0.0f;
data[3] = 1.0f;
time = 0.0f;
}
PointXYZT(float x, float y, float z, float time = 0.0f)
: x(x), y(y), z(z), time(time) {
data[3] = 1.0f;
}
PointXYZT(const Point& p) {
x = p.x;
y = p.y;
z = p.z;
data[3] = 1.0f;
time = 0.0f;
}
PointXYZT(const PointXYZT& p) {
x = p.x;
y = p.y;
z = p.z;
data[3] = 1.0f;
time = p.time;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace common
// clang-format off
POINT_CLOUD_REGISTER_POINT_STRUCT(
::common::PointXYZT,
(float, x, x)
(float, y, y)
(float, z, z)
(float, time, time)
(float, intensity, intensity)
)
// clang-format on
} // namespace common

18
oh_my_loam/base/feature.h
View File

@ -1,20 +1,20 @@
#pragma once
#include "common/pcl/pcl_types.h"
#include "oh_my_loam/base/types.h"
namespace oh_my_loam {
struct Feature {
common::TPointCloudPtr cloud_corner;
common::TPointCloudPtr cloud_sharp_corner;
common::TPointCloudPtr cloud_surf;
common::TPointCloudPtr cloud_flat_surf;
TPointCloudPtr cloud_corner;
TPointCloudPtr cloud_sharp_corner;
TPointCloudPtr cloud_surf;
TPointCloudPtr cloud_flat_surf;
Feature() {
cloud_corner.reset(new common::TPointCloud);
cloud_sharp_corner.reset(new common::TPointCloud);
cloud_surf.reset(new common::TPointCloud);
cloud_flat_surf.reset(new common::TPointCloud);
cloud_corner.reset(new TPointCloud);
cloud_sharp_corner.reset(new TPointCloud);
cloud_surf.reset(new TPointCloud);
cloud_flat_surf.reset(new TPointCloud);
}
};

14
oh_my_loam/base/helper.cc
View File

@ -2,25 +2,25 @@
namespace oh_my_loam {
void TransformToStart(const Pose3d& pose, const TPoint& pt_in,
TPoint* const pt_out) {
Pose3d pose_interp = Pose3d().Interpolate(pose, GetTime(pt_in));
void TransformToStart(const Pose3d &pose, const TPoint &pt_in,
TPoint *const pt_out) {
Pose3d pose_interp = Pose3d().Interpolate(pose, pt_in.time);
TransformPoint<TPoint>(pose_interp, pt_in, pt_out);
}
TPoint TransformToStart(const Pose3d& pose, const TPoint& pt_in) {
TPoint TransformToStart(const Pose3d &pose, const TPoint &pt_in) {
TPoint pt_out;
TransformToStart(pose, pt_in, &pt_out);
return pt_out;
}
void TransformToEnd(const Pose3d& pose, const TPoint& pt_in,
TPoint* const pt_out) {
void TransformToEnd(const Pose3d &pose, const TPoint &pt_in,
TPoint *const pt_out) {
TransformToStart(pose, pt_in, pt_out);
TransformPoint<TPoint>(pose.Inv(), *pt_out, pt_out);
}
TPoint TransformToEnd(const Pose3d& pose, const TPoint& pt_in) {
TPoint TransformToEnd(const Pose3d &pose, const TPoint &pt_in) {
TPoint pt_out;
TransformToEnd(pose, pt_in, &pt_out);
return pt_out;

39
oh_my_loam/base/helper.h
View File

@ -1,20 +1,15 @@
#pragma once
#include "common/geometry/pose3d.h"
#include "common/pcl/pcl_types.h"
#include "oh_my_loam/base/types.h"
namespace oh_my_loam {
using common::TPoint;
using common::Pose3d;
inline float GetTime(const TPoint& pt) { return pt.time - std::floor(pt.time); }
inline int GetScanId(const TPoint& pt) { return static_cast<int>(pt.time); }
template <typename PointType>
void TransformPoint(const Pose3d& pose, const PointType& pt_in,
PointType* const pt_out) {
void TransformPoint(const Pose3d &pose, const PointType &pt_in,
PointType *const pt_out) {
*pt_out = pt_in;
Eigen::Vector3d pnt = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z);
pt_out->x = pnt.x();
@ -23,7 +18,7 @@ void TransformPoint(const Pose3d& pose, const PointType& pt_in,
}
template <typename PointType>
PointType TransformPoint(const Pose3d& pose, const PointType& pt_in) {
PointType TransformPoint(const Pose3d &pose, const PointType &pt_in) {
PointType pt_out;
TransformPoint<PointType>(pose, pt_in, &pt_out);
return pt_out;
@ -34,31 +29,31 @@ PointType TransformPoint(const Pose3d& pose, const PointType& pt_in) {
*
* @param pose Relative pose, end scan time w.r.t. start scan time
*/
void TransformToStart(const Pose3d& pose, const TPoint& pt_in,
TPoint* const pt_out);
void TransformToStart(const Pose3d &pose, const TPoint &pt_in,
TPoint *const pt_out);
TPoint TransformToStart(const Pose3d& pose, const TPoint& pt_in);
TPoint TransformToStart(const Pose3d &pose, const TPoint &pt_in);
/**
* @brief Transform a lidar point to the end of the scan
*
* @param pose Relative pose, end scan time w.r.t. start scan time
*/
void TransformToEnd(const Pose3d& pose, const TPoint& pt_in,
TPoint* const pt_out);
void TransformToEnd(const Pose3d &pose, const TPoint &pt_in,
TPoint *const pt_out);
TPoint TransformToEnd(const Pose3d& pose, const TPoint& pt_in);
TPoint TransformToEnd(const Pose3d &pose, const TPoint &pt_in);
struct PointLinePair {
TPoint pt;
struct Line {
TPoint pt1, pt2;
Line() = default;
Line(const TPoint& pt1, const TPoint& pt2) : pt1(pt1), pt2(pt2) {}
Line(const TPoint &pt1, const TPoint &pt2) : pt1(pt1), pt2(pt2) {}
};
Line line;
PointLinePair(const TPoint& pt, const Line& line) : pt(pt), line(line) {}
PointLinePair(const TPoint& pt, const TPoint& pt1, const TPoint& pt2)
PointLinePair(const TPoint &pt, const Line &line) : pt(pt), line(line) {}
PointLinePair(const TPoint &pt, const TPoint &pt1, const TPoint &pt2)
: pt(pt), line(pt1, pt2) {}
};
@ -67,13 +62,13 @@ struct PointPlanePair {
struct Plane {
TPoint pt1, pt2, pt3;
Plane() = default;
Plane(const TPoint& pt1, const TPoint& pt2, const TPoint& pt3)
Plane(const TPoint &pt1, const TPoint &pt2, const TPoint &pt3)
: pt1(pt1), pt2(pt2), pt3(pt3) {}
};
Plane plane;
PointPlanePair(const TPoint& pt, const Plane& plane) : pt(pt), plane(plane) {}
PointPlanePair(const TPoint& pt, const TPoint& pt1, const TPoint& pt2,
const TPoint& pt3)
PointPlanePair(const TPoint &pt, const Plane &plane) : pt(pt), plane(plane) {}
PointPlanePair(const TPoint &pt, const TPoint &pt1, const TPoint &pt2,
const TPoint &pt3)
: pt(pt), plane(pt1, pt2, pt3) {}
};

58
oh_my_loam/base/types.h
View File

@ -12,12 +12,55 @@ enum class PType {
SHARP_CORNER = 2
};
struct PointXYZT;
using TPoint = PointXYZT;
using TPointCloud = pcl::PointCloud<TPoint>;
using TPointCloudPtr = TPointCloud::Ptr;
using TPointCloudConstPtr = TPointCloud::ConstPtr;
struct PointXYZTCT;
using TCTPoint = PointXYZTCT;
using TCTPointCloud = pcl::PointCloud<TCTPoint>;
using TCTPointCloudPtr = TCTPointCloud::Ptr;
using TCTPointCloudConstPtr = TCTPointCloud::ConstPtr;
struct EIGEN_ALIGN16 PointXYZT {
PCL_ADD_POINT4D;
union {
float time;
float intensity;
float data_c[4];
};
PointXYZT() {
x = y = z = 0.0f;
data[3] = 1.0f;
time = 0.0f;
}
PointXYZT(float x, float y, float z, float time = 0.0f)
: x(x), y(y), z(z), time(time) {
data[3] = 1.0f;
}
PointXYZT(const common::Point &p) {
x = p.x;
y = p.y;
z = p.z;
data[3] = 1.0f;
time = 0.0f;
}
PointXYZT(const PointXYZT &p) {
x = p.x;
y = p.y;
z = p.z;
data[3] = 1.0f;
time = p.time;
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
struct PointXYZTCT {
PCL_ADD_POINT4D;
union EIGEN_ALIGN16 {
@ -42,7 +85,7 @@ struct PointXYZTCT {
data[3] = 1.0f;
}
PointXYZTCT(const common::Point& p) {
PointXYZTCT(const common::Point &p) {
x = p.x;
y = p.y;
z = p.z;
@ -52,7 +95,7 @@ struct PointXYZTCT {
type = PType::NORNAL;
}
PointXYZTCT(const common::TPoint& p) {
PointXYZTCT(const TPoint &p) {
x = p.x;
y = p.y;
z = p.z;
@ -62,7 +105,7 @@ struct PointXYZTCT {
type = PType::NORNAL;
}
PointXYZTCT(const PointXYZTCT& p) {
PointXYZTCT(const PointXYZTCT &p) {
x = p.x;
y = p.y;
z = p.z;
@ -78,6 +121,15 @@ struct PointXYZTCT {
} // namespace oh_my_loam
// clang-format off
POINT_CLOUD_REGISTER_POINT_STRUCT(
oh_my_loam::PointXYZT,
(float, x, x)
(float, y, y)
(float, z, z)
(float, time, time)
(float, intensity, intensity)
)
POINT_CLOUD_REGISTER_POINT_STRUCT(
oh_my_loam::PointXYZTCT,
(float, x, x)

4
oh_my_loam/mapper/mapper.h
View File

@ -2,7 +2,7 @@
#include "common/common.h"
#include "common/geometry/pose.h"
#include "common/pcl/pcl_types.h"
#include "oh_my_loam/base/types.h"
namespace oh_my_loam {
@ -17,7 +17,7 @@ class Mapper {
private:
void Visualize();
common::TPointCloudPtr map_pts_;
TPointCloudPtr map_pts_;
common::Pose3d pose_curr2world_;
bool is_initialized = false;

92
oh_my_loam/odometer/odometer.cc
View File

@ -12,7 +12,7 @@ using namespace common;
} // namespace
bool Odometer::Init() {
const auto& config = YAMLConfig::Instance()->config();
const auto &config = YAMLConfig::Instance()->config();
config_ = config["odometer_config"];
is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
verbose_ = config_["verbose"].as<bool>();
@ -21,23 +21,25 @@ bool Odometer::Init() {
return true;
}
void Odometer::Process(double timestamp, const std::vector<Feature>& features,
Pose3d* const pose) {
void Odometer::Process(double timestamp, const std::vector<Feature> &features,
Pose3d *const pose) {
BLOCK_TIMER_START;
if (!is_initialized_) {
UpdatePre(features);
is_initialized_ = true;
pose_curr2last_ = Pose3d::Identity();
pose_curr2world_ = Pose3d::Identity();
*pose = Pose3d::Identity();
AWARN << "Odometer initialized....";
return;
}
AINFO << "Pose before: " << pose_curr2world_.ToString();
AINFO_IF(verbose_) << "Pose before: " << pose_curr2world_.ToString();
std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs;
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
for (const auto& feature : features) {
for (const auto &feature : features) {
MatchCorn(*feature.cloud_sharp_corner, &pl_pairs);
// MatchSurf(*feature.cloud_flat_surf, &pp_pairs);
MatchSurf(*feature.cloud_flat_surf, &pp_pairs);
}
AINFO_IF(verbose_) << "Iter " << i
<< ": matched num: point2line = " << pl_pairs.size()
@ -45,32 +47,33 @@ void Odometer::Process(double timestamp, const std::vector<Feature>& features,
if (pl_pairs.size() + pp_pairs.size() < kMinMatchNum) {
AWARN << "Too less correspondence: " << pl_pairs.size() << " + "
<< pp_pairs.size();
continue;
}
PoseSolver solver(pose_curr2last_);
for (const auto& pair : pl_pairs) {
for (const auto &pair : pl_pairs) {
solver.AddPointLinePair(pair, pair.pt.time);
}
for (const auto& pair : pp_pairs) {
for (const auto &pair : pp_pairs) {
solver.AddPointPlanePair(pair, pair.pt.time);
}
solver.Solve(5, verbose_, &pose_curr2last_);
}
pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
*pose = pose_curr2world_;
AWARN_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString();
AWARN_IF(verbose_) << "Pose after: " << pose_curr2world_.ToString();
AINFO_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString();
AINFO_IF(verbose_) << "Pose after: " << pose_curr2world_.ToString();
UpdatePre(features);
if (is_vis_) Visualize(features, pl_pairs, pp_pairs);
AINFO << "Odometer::Porcess: " << BLOCK_TIMER_STOP_FMT;
}
void Odometer::MatchCorn(const TPointCloud& src,
std::vector<PointLinePair>* const pairs) const {
void Odometer::MatchCorn(const TPointCloud &src,
std::vector<PointLinePair> *const pairs) const {
double dist_sq_thresh = config_["match_dist_sq_th"].as<double>();
auto comp = [](const std::vector<float>& v1, const std::vector<float>& v2) {
auto comp = [](const std::vector<float> &v1, const std::vector<float> &v2) {
return v1[0] < v2[0];
};
for (const auto& pt : src) {
for (const auto &pt : src) {
TPoint query_pt = TransformToStart(pose_curr2last_, pt);
std::vector<std::vector<int>> indices;
std::vector<std::vector<float>> dists;
@ -90,20 +93,20 @@ void Odometer::MatchCorn(const TPointCloud& src,
min_dist_pt2_squre = dists[i][0];
}
}
if (pt2_scan_id > 0) {
if (pt2_scan_id >= 0) {
TPoint pt2 = clouds_corn_pre_[pt2_scan_id]->at(indices[pt2_scan_id][0]);
pairs->emplace_back(pt, pt1, pt2);
}
}
}
void Odometer::MatchSurf(const TPointCloud& src,
std::vector<PointPlanePair>* const pairs) const {
void Odometer::MatchSurf(const TPointCloud &src,
std::vector<PointPlanePair> *const pairs) const {
double dist_sq_thresh = config_["match_dist_sq_th"].as<double>();
auto comp = [](const std::vector<float>& v1, const std::vector<float>& v2) {
auto comp = [](const std::vector<float> &v1, const std::vector<float> &v2) {
return v1[0] < v2[0];
};
for (const auto& pt : src) {
for (const auto &pt : src) {
TPoint query_pt = TransformToStart(pose_curr2last_, pt);
std::vector<std::vector<int>> indices;
std::vector<std::vector<float>> dists;
@ -125,64 +128,69 @@ void Odometer::MatchSurf(const TPointCloud& src,
min_dist_pt3_squre = dists[i][0];
}
}
if (pt3_scan_id > 0) {
if (pt3_scan_id >= 0) {
TPoint pt3 = clouds_surf_pre_[pt3_scan_id]->at(indices[pt3_scan_id][0]);
pairs->emplace_back(pt, pt1, pt2, pt3);
}
}
}
void Odometer::UpdatePre(const std::vector<Feature>& features) {
clouds_corn_pre_.resize(features.size(), common::TPointCloud().makeShared());
clouds_surf_pre_.resize(features.size(), common::TPointCloud().makeShared());
void Odometer::UpdatePre(const std::vector<Feature> &features) {
BLOCK_TIMER_START;
clouds_corn_pre_.resize(features.size());
clouds_surf_pre_.resize(features.size());
kdtrees_corn_.resize(features.size());
kdtrees_surf_.resize(features.size());
for (size_t i = 0; i < features.size(); ++i) {
const auto& feature = features[i];
auto& cloud_pre = clouds_corn_pre_[i];
const auto &feature = features[i];
auto &cloud_pre = clouds_corn_pre_[i];
if (!cloud_pre) cloud_pre.reset(new TPointCloud);
cloud_pre->resize(feature.cloud_corner->size());
for (size_t j = 0; j < feature.cloud_corner->size(); ++j) {
TransformToEnd(pose_curr2last_, feature.cloud_corner->at(j),
&cloud_pre->at(j));
}
kdtrees_corn_[i].setInputCloud(clouds_corn_pre_[i]);
kdtrees_corn_[i].setInputCloud(cloud_pre);
}
for (size_t i = 0; i < features.size(); ++i) {
const auto& feature = features[i];
auto& cloud_pre = clouds_surf_pre_[i];
const auto &feature = features[i];
auto &cloud_pre = clouds_surf_pre_[i];
if (!cloud_pre) cloud_pre.reset(new TPointCloud);
cloud_pre->resize(feature.cloud_surf->size());
for (size_t j = 0; j < feature.cloud_surf->size(); ++j) {
TransformToEnd(pose_curr2last_, feature.cloud_surf->at(j),
&cloud_pre->at(j));
}
kdtrees_surf_[i].setInputCloud(clouds_surf_pre_[i]);
kdtrees_surf_[i].setInputCloud(cloud_pre);
}
AINFO << "Odometer::UpdatePre: " << BLOCK_TIMER_STOP_FMT;
}
void Odometer::NearestKSearch(
const std::vector<pcl::KdTreeFLANN<common::TPoint>>& kdtrees,
const TPoint& query_pt, size_t k,
std::vector<std::vector<int>>* const indices,
std::vector<std::vector<float>>* const dists) const {
for (const auto& kdtree : kdtrees) {
const std::vector<pcl::KdTreeFLANN<TPoint>> &kdtrees,
const TPoint &query_pt, int k, std::vector<std::vector<int>> *const indices,
std::vector<std::vector<float>> *const dists) const {
for (const auto &kdtree : kdtrees) {
std::vector<int> index;
std::vector<float> dist;
if (kdtree.getInputCloud()->size() >= k) {
kdtree.nearestKSearch(query_pt, k, index, dist);
} else {
index.resize(k, -1);
dist.resize(k, std::numeric_limits<float>::max());
int n_found = 0;
if (kdtree.getInputCloud()->size() >= static_cast<size_t>(k)) {
n_found = kdtree.nearestKSearch(query_pt, k, index, dist);
}
if (n_found < k) {
std::vector<int>(k, -1).swap(index);
std::vector<float>(k, std::numeric_limits<float>::max()).swap(dist);
}
indices->push_back(std::move(index));
dists->push_back(std::move(dist));
}
}
void Odometer::Visualize(const std::vector<Feature>& features,
const std::vector<PointLinePair>& pl_pairs,
const std::vector<PointPlanePair>& pp_pairs,
void Odometer::Visualize(const std::vector<Feature> &features,
const std::vector<PointLinePair> &pl_pairs,
const std::vector<PointPlanePair> &pp_pairs,
double timestamp) const {
std::shared_ptr<OdometerVisFrame> frame(new OdometerVisFrame);
frame->timestamp = timestamp;

38
oh_my_loam/odometer/odometer.h
View File

@ -15,37 +15,35 @@ class Odometer {
bool Init();
void Process(double timestamp, const std::vector<Feature>& features,
common::Pose3d* const pose);
void Process(double timestamp, const std::vector<Feature> &features,
common::Pose3d *const pose);
protected:
void UpdatePre(const std::vector<Feature>& features);
void UpdatePre(const std::vector<Feature> &features);
void MatchCorn(const common::TPointCloud& src,
std::vector<PointLinePair>* const pairs) const;
void MatchCorn(const TPointCloud &src,
std::vector<PointLinePair> *const pairs) const;
void MatchSurf(const common::TPointCloud& src,
std::vector<PointPlanePair>* const pairs) const;
void MatchSurf(const TPointCloud &src,
std::vector<PointPlanePair> *const pairs) const;
void Visualize(const std::vector<Feature>& features,
const std::vector<PointLinePair>& pl_pairs,
const std::vector<PointPlanePair>& pp_pairs,
void Visualize(const std::vector<Feature> &features,
const std::vector<PointLinePair> &pl_pairs,
const std::vector<PointPlanePair> &pp_pairs,
double timestamp = 0.0) const;
void NearestKSearch(
const std::vector<pcl::KdTreeFLANN<common::TPoint>>& kdtrees,
const TPoint& query_pt, size_t k,
std::vector<std::vector<int>>* const indices,
std::vector<std::vector<float>>* const dists) const;
void NearestKSearch(const std::vector<pcl::KdTreeFLANN<TPoint>> &kdtrees,
const TPoint &query_pt, int k,
std::vector<std::vector<int>> *const indices,
std::vector<std::vector<float>> *const dists) const;
common::Pose3d pose_curr2world_;
common::Pose3d pose_curr2last_;
std::vector<common::TPointCloudPtr> clouds_corn_pre_;
std::vector<common::TPointCloudPtr> clouds_surf_pre_;
std::vector<TPointCloudPtr> clouds_corn_pre_;
std::vector<TPointCloudPtr> clouds_surf_pre_;
std::vector<pcl::KdTreeFLANN<common::TPoint>> kdtrees_surf_;
std::vector<pcl::KdTreeFLANN<common::TPoint>> kdtrees_corn_;
std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_surf_;
std::vector<pcl::KdTreeFLANN<TPoint>> kdtrees_corn_;
YAML::Node config_;

16
oh_my_loam/solver/solver.h
View File

@ -6,22 +6,22 @@
namespace oh_my_loam {
class PoseSolver {
public:
PoseSolver(const common::Pose3d& pose);
public:
PoseSolver(const common::Pose3d &pose);
void AddPointLinePair(const PointLinePair& pair, double time);
void AddPointLinePair(const PointLinePair &pair, double time);
void AddPointPlanePair(const PointPlanePair& pair, double time);
void AddPointPlanePair(const PointPlanePair &pair, double time);
double Solve(int max_iter_num = 5, bool verbose = false,
common::Pose3d* const pose = nullptr);
common::Pose3d *const pose = nullptr);
common::Pose3d GetPose() const;
private:
private:
ceres::Problem problem_;
ceres::LossFunction* loss_function_;
ceres::LossFunction *loss_function_;
// r_quat_: [x, y, z, w], t_vec_: [x, y, z]
double *r_quat_, *t_vec_;
@ -29,4 +29,4 @@ class PoseSolver {
DISALLOW_COPY_AND_ASSIGN(PoseSolver)
};
} // oh_my_loam
} // namespace oh_my_loam

4
oh_my_loam/visualizer/odometer_visualizer.h
View File

@ -6,8 +6,8 @@
namespace oh_my_loam {
struct OdometerVisFrame : public common::LidarVisFrame {
common::TPointCloudPtr cloud_surf;
common::TPointCloudPtr cloud_corner;
TPointCloudPtr cloud_surf;
TPointCloudPtr cloud_corner;
std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs;
common::Pose3d pose_curr2last;