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

simplify lidar visualizer api and rename Pose3d to Pose3D

main
feixyz10 2021-01-07 18:04:56 +08:00 committed by feixyz
parent a706cac8c5
commit c71992dcd4
14 changed files with 41 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
common/geometry/pose.cc
View File

@ -2,17 +2,17 @@
namespace common { namespace common {
Pose3d Interpolate(const Pose3d& pose_from, const Pose3d& pose_to, double t) { Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t) {
return pose_from.Interpolate(pose_to, t); return pose_from.Interpolate(pose_to, t);
} }
Pose3d operator*(const Pose3d& lhs, const Pose3d& rhs) { Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs) {
return Pose3d(lhs.q() * rhs.q(), lhs.q() * rhs.p() + lhs.p()); return Pose3D(lhs.q() * rhs.q(), lhs.q() * rhs.p() + lhs.p());
} }
std::string Pose3d::ToString() const { std::string Pose3D::ToString() const {
std::ostringstream oss; std::ostringstream oss;
oss << "[Pose3d] q = (" << q_.coeffs().transpose() << "), p = (" oss << "[Pose3D] q = (" << q_.coeffs().transpose() << "), p = ("
<< p_.transpose() << ")"; << p_.transpose() << ")";
return oss.str(); return oss.str();
} }

20
common/geometry/pose.h
View File

@ -4,22 +4,22 @@
namespace common { namespace common {
class Pose3d { class Pose3D {
public: public:
Pose3d() { Pose3D() {
q_.setIdentity(); q_.setIdentity();
p_.setZero(); p_.setZero();
}; };
Pose3d(const Eigen::Quaterniond& q, const Eigen::Vector3d& p) Pose3D(const Eigen::Quaterniond& q, const Eigen::Vector3d& p)
: q_(q), p_(p) {} : q_(q), p_(p) {}
Pose3d(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& p) Pose3D(const Eigen::Matrix3d& r_mat, const Eigen::Vector3d& p)
: q_(r_mat), p_(p) {} : q_(r_mat), p_(p) {}
Pose3d(const double* const q, const double* const p) : q_(q), p_(p) {} Pose3D(const double* const q, const double* const p) : q_(q), p_(p) {}
Pose3d Inv() const { Pose3D Inv() const {
Eigen::Quaterniond q_inv = q_.inverse(); Eigen::Quaterniond q_inv = q_.inverse();
Eigen::Vector3d p_inv = q_inv * p_; Eigen::Vector3d p_inv = q_inv * p_;
return {q_inv, -p_inv}; return {q_inv, -p_inv};
@ -40,7 +40,7 @@ class Pose3d {
Eigen::Vector3d Rotate(const Eigen::Vector3d& vec) const { return q_ * vec; } Eigen::Vector3d Rotate(const Eigen::Vector3d& vec) const { return q_ * vec; }
// Spherical linear interpolation to `pose_to`, `t` belongs [0, 1] // Spherical linear interpolation to `pose_to`, `t` belongs [0, 1]
Pose3d Interpolate(const Pose3d& pose_to, double t) const { Pose3D Interpolate(const Pose3D& pose_to, double t) const {
Eigen::Quaterniond q_interp = q_.slerp(t, pose_to.q_); Eigen::Quaterniond q_interp = q_.slerp(t, pose_to.q_);
Eigen::Vector3d p_interp = (pose_to.p_ - p_) * t + p_; Eigen::Vector3d p_interp = (pose_to.p_ - p_) * t + p_;
return {q_interp, p_interp}; return {q_interp, p_interp};
@ -57,10 +57,10 @@ class Pose3d {
Eigen::Vector3d p_; // position Eigen::Vector3d p_; // position
}; };
Pose3d Interpolate(const Pose3d& pose_from, const Pose3d& pose_to, double t); Pose3D Interpolate(const Pose3D& pose_from, const Pose3D& pose_to, double t);
Pose3d operator*(const Pose3d& lhs, const Pose3d& rhs); Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs);
using Trans3d = Pose3d; using Trans3d = Pose3D;
} // namespace common } // namespace common

10
common/visualizer/lidar_visualizer.h
View File

@ -106,10 +106,7 @@ class LidarVisualizer {
} }
void RemoveRenderedObjects() { void RemoveRenderedObjects() {
for (const auto &id : rendered_cloud_ids_) { viewer_->removeAllPointClouds();
viewer_->removePointCloud(id);
}
rendered_cloud_ids_.clear();
viewer_->removeAllShapes(); viewer_->removeAllShapes();
} }
@ -124,7 +121,6 @@ class LidarVisualizer {
const typename pcl::PointCloud<PointType>::ConstPtr &cloud, const typename pcl::PointCloud<PointType>::ConstPtr &cloud,
const Color &color, const std::string &id, int point_size = 3) { const Color &color, const std::string &id, int point_size = 3) {
AddPointCloud<PointType>(cloud, color, id, viewer_.get(), point_size); AddPointCloud<PointType>(cloud, color, id, viewer_.get(), point_size);
rendered_cloud_ids_.push_back(id);
} }
template <typename PointType> template <typename PointType>
@ -132,7 +128,6 @@ class LidarVisualizer {
const typename pcl::PointCloud<PointType>::ConstPtr &cloud, const typename pcl::PointCloud<PointType>::ConstPtr &cloud,
const std::string &field, const std::string &id, int point_size = 3) { const std::string &field, const std::string &id, int point_size = 3) {
AddPointCloud<PointType>(cloud, field, id, viewer_.get(), point_size); AddPointCloud<PointType>(cloud, field, id, viewer_.get(), point_size);
rendered_cloud_ids_.push_back(id);
} }
// visualizer name // visualizer name
@ -155,9 +150,6 @@ class LidarVisualizer {
typename std::deque<std::shared_ptr<LidarVisFrame>>::iterator typename std::deque<std::shared_ptr<LidarVisFrame>>::iterator
curr_frame_iter_; curr_frame_iter_;
// The rendered cloud ids.
std::vector<std::string> rendered_cloud_ids_;
// thread for visualization // thread for visualization
std::unique_ptr<std::thread> thread_ = nullptr; std::unique_ptr<std::thread> thread_ = nullptr;

4
common/visualizer/lidar_visualizer_utils.cc
View File

@ -8,8 +8,8 @@ void AddLine(const pcl::PointXYZ& pt1, const pcl::PointXYZ& pt2,
viewer->addLine(pt1, pt2, color.r, color.g, color.b, id); viewer->addLine(pt1, pt2, color.r, color.g, color.b, id);
} }
void DrawSphere(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);
} }

4
common/visualizer/lidar_visualizer_utils.h
View File

@ -42,7 +42,7 @@ 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 DrawSphere(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

6
oh_my_loam/base/helper.cc
View File

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

8
oh_my_loam/base/helper.h
View File

@ -6,7 +6,7 @@
namespace oh_my_loam { namespace oh_my_loam {
using common::TPoint; using common::TPoint;
using common::Pose3d; using common::Pose3D;
using common::Trans3d; using common::Trans3d;
inline float GetTime(const TPoint& pt) { return pt.time - std::floor(pt.time); } inline float GetTime(const TPoint& pt) { return pt.time - std::floor(pt.time); }
@ -14,7 +14,7 @@ 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); } inline int GetScanId(const TPoint& pt) { return static_cast<int>(pt.time); }
template <typename PointType> template <typename PointType>
void TransformPoint(const Pose3d& pose, const PointType& pt_in, void TransformPoint(const Pose3D& pose, const PointType& pt_in,
PointType* const pt_out) { PointType* const pt_out) {
*pt_out = pt_in; *pt_out = pt_in;
Eigen::Vector3d pnt = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z); Eigen::Vector3d pnt = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z);
@ -28,14 +28,14 @@ void TransformPoint(const Pose3d& pose, const PointType& pt_in,
* *
* @param pose Relative pose, end scan time w.r.t. start scan time * @param pose Relative pose, end scan time w.r.t. start scan time
*/ */
void TransformToStart(const Pose3d& pose, const TPoint& pt_in, void TransformToStart(const Pose3D& pose, const TPoint& pt_in,
TPoint* const pt_out); TPoint* const pt_out);
/** /**
* @brief Transform a lidar point to the end of the scan * @brief Transform a lidar point to the end of the scan
* *
* @param pose Relative pose, end scan time w.r.t. start scan time * @param pose Relative pose, end scan time w.r.t. start scan time
*/ */
void TransformToEnd(const Pose3d& pose, const TPoint& pt_in, void TransformToEnd(const Pose3D& pose, const TPoint& pt_in,
TPoint* const pt_out); TPoint* const pt_out);
struct PointLinePair { struct PointLinePair {

2
oh_my_loam/mapper/mapper.h
View File

@ -18,7 +18,7 @@ class Mapper {
void Visualize(); void Visualize();
common::TPointCloudPtr map_pts_; common::TPointCloudPtr map_pts_;
common::Pose3d pose_curr2world_; common::Pose3D pose_curr2world_;
bool is_initialized = false; bool is_initialized = false;

6
oh_my_loam/odometer/odometer.cc
View File

@ -23,12 +23,12 @@ bool Odometer::Init() {
return true; return true;
} }
void Odometer::Process(const Feature& feature, Pose3d* const pose) { void Odometer::Process(const Feature& feature, Pose3D* const pose) {
BLOCK_TIMER_START; BLOCK_TIMER_START;
if (!is_initialized_) { if (!is_initialized_) {
is_initialized_ = true; is_initialized_ = true;
UpdatePre(feature); UpdatePre(feature);
*pose = Pose3d(); *pose = Pose3D();
AWARN << "Odometer initialized...."; AWARN << "Odometer initialized....";
return; return;
} }
@ -62,7 +62,7 @@ void Odometer::Process(const Feature& feature, Pose3d* const pose) {
solver.AddPointPlanePair(pair, GetTime(pair.pt)); solver.AddPointPlanePair(pair, GetTime(pair.pt));
} }
solver.Solve(); solver.Solve();
pose_curr2last_ = Pose3d(q, p); pose_curr2last_ = Pose3D(q, p);
} }
pose_curr2world_ = pose_curr2world_ * pose_curr2last_; pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
*pose = pose_curr2world_; *pose = pose_curr2world_;

6
oh_my_loam/odometer/odometer.h
View File

@ -15,7 +15,7 @@ class Odometer {
bool Init(); bool Init();
void Process(const Feature& feature, common::Pose3d* const pose); void Process(const Feature& feature, common::Pose3D* const pose);
protected: protected:
void UpdatePre(const Feature& feature); void UpdatePre(const Feature& feature);
@ -32,8 +32,8 @@ class Odometer {
const std::vector<PointLinePair>& pl_pairs, const std::vector<PointLinePair>& pl_pairs,
const std::vector<PointPlanePair>& pp_pairs) const; const std::vector<PointPlanePair>& pp_pairs) const;
common::Pose3d pose_curr2world_; common::Pose3D pose_curr2world_;
common::Pose3d pose_curr2last_; common::Pose3D pose_curr2last_;
common::TPointCloudPtr corn_pre_{nullptr}; common::TPointCloudPtr corn_pre_{nullptr};
common::TPointCloudPtr surf_pre_{nullptr}; common::TPointCloudPtr surf_pre_{nullptr};

2
oh_my_loam/oh_my_loam.cc
View File

@ -35,7 +35,7 @@ void OhMyLoam::Run(double timestamp, const PointCloudConstPtr& cloud_in) {
RemoveOutliers(*cloud_in, cloud.get()); RemoveOutliers(*cloud_in, cloud.get());
Feature feature; Feature feature;
extractor_->Process(cloud, &feature); extractor_->Process(cloud, &feature);
Pose3d pose; Pose3D pose;
odometer_->Process(feature, &pose); odometer_->Process(feature, &pose);
poses_.emplace_back(pose); poses_.emplace_back(pose);
} }

2
oh_my_loam/oh_my_loam.h
View File

@ -25,7 +25,7 @@ class OhMyLoam {
// 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_;
DISALLOW_COPY_AND_ASSIGN(OhMyLoam) DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
}; };

4
oh_my_loam/visualizer/odometer_visualizer.cc
View File

@ -33,9 +33,9 @@ void OdometerVisualizer::Draw() {
DrawPointCloud<TPoint>(tgt_corn_pts, BLUE, "tgt_corn_pts", 4); DrawPointCloud<TPoint>(tgt_corn_pts, BLUE, "tgt_corn_pts", 4);
DrawPointCloud<TPoint>(src_surf_pts, PURPLE, "src_surf_pts", 7); DrawPointCloud<TPoint>(src_surf_pts, PURPLE, "src_surf_pts", 7);
DrawPointCloud<TPoint>(tgt_surf_pts, RED, "tgt_surf_pts", 4); DrawPointCloud<TPoint>(tgt_surf_pts, RED, "tgt_surf_pts", 4);
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);
}; };

6
oh_my_loam/visualizer/odometer_visualizer.h
View File

@ -10,8 +10,8 @@ struct OdometerVisFrame : public common::LidarVisFrame {
common::TPointCloudPtr corn_pts; common::TPointCloudPtr corn_pts;
std::vector<PointLinePair> pl_pairs; std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs; std::vector<PointPlanePair> pp_pairs;
common::Pose3d pose_curr2last; common::Pose3D pose_curr2last;
common::Pose3d pose_curr2world; common::Pose3D pose_curr2world;
}; };
class OdometerVisualizer : public common::LidarVisualizer { class OdometerVisualizer : public common::LidarVisualizer {
@ -23,7 +23,7 @@ class OdometerVisualizer : public common::LidarVisualizer {
private: private:
void Draw() override; void Draw() override;
std::deque<common::Pose3d> poses_; std::deque<common::Pose3D> poses_;
}; };
} // namespace oh_my_loam } // namespace oh_my_loam