diff --git a/common/geometry/pose.cc b/common/geometry/pose.cc index 9e7649b..c246ff8 100644 --- a/common/geometry/pose.cc +++ b/common/geometry/pose.cc @@ -2,17 +2,17 @@ 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); } -Pose3d operator*(const Pose3d& lhs, const Pose3d& rhs) { - return Pose3d(lhs.q() * rhs.q(), lhs.q() * rhs.p() + lhs.p()); +Pose3D operator*(const Pose3D& lhs, const Pose3D& rhs) { + 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; - oss << "[Pose3d] q = (" << q_.coeffs().transpose() << "), p = (" + oss << "[Pose3D] q = (" << q_.coeffs().transpose() << "), p = (" << p_.transpose() << ")"; return oss.str(); } diff --git a/common/geometry/pose.h b/common/geometry/pose.h index 3eef6c8..2c99c29 100644 --- a/common/geometry/pose.h +++ b/common/geometry/pose.h @@ -4,22 +4,22 @@ namespace common { -class Pose3d { +class Pose3D { public: - Pose3d() { + Pose3D() { q_.setIdentity(); p_.setZero(); }; - Pose3d(const Eigen::Quaterniond& q, const Eigen::Vector3d& p) + Pose3D(const Eigen::Quaterniond& q, const Eigen::Vector3d& 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) {} - 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::Vector3d p_inv = q_inv * p_; return {q_inv, -p_inv}; @@ -40,7 +40,7 @@ class Pose3d { Eigen::Vector3d Rotate(const Eigen::Vector3d& vec) const { return q_ * vec; } // 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::Vector3d p_interp = (pose_to.p_ - p_) * t + p_; return {q_interp, p_interp}; @@ -57,10 +57,10 @@ class Pose3d { 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 diff --git a/common/visualizer/lidar_visualizer.h b/common/visualizer/lidar_visualizer.h index 9718e5e..de722d8 100644 --- a/common/visualizer/lidar_visualizer.h +++ b/common/visualizer/lidar_visualizer.h @@ -106,10 +106,7 @@ class LidarVisualizer { } void RemoveRenderedObjects() { - for (const auto &id : rendered_cloud_ids_) { - viewer_->removePointCloud(id); - } - rendered_cloud_ids_.clear(); + viewer_->removeAllPointClouds(); viewer_->removeAllShapes(); } @@ -124,7 +121,6 @@ class LidarVisualizer { const typename pcl::PointCloud::ConstPtr &cloud, const Color &color, const std::string &id, int point_size = 3) { AddPointCloud(cloud, color, id, viewer_.get(), point_size); - rendered_cloud_ids_.push_back(id); } template @@ -132,7 +128,6 @@ class LidarVisualizer { const typename pcl::PointCloud::ConstPtr &cloud, const std::string &field, const std::string &id, int point_size = 3) { AddPointCloud(cloud, field, id, viewer_.get(), point_size); - rendered_cloud_ids_.push_back(id); } // visualizer name @@ -155,9 +150,6 @@ class LidarVisualizer { typename std::deque>::iterator curr_frame_iter_; - // The rendered cloud ids. - std::vector rendered_cloud_ids_; - // thread for visualization std::unique_ptr thread_ = nullptr; diff --git a/common/visualizer/lidar_visualizer_utils.cc b/common/visualizer/lidar_visualizer_utils.cc index 7551c4b..c9fa4b9 100644 --- a/common/visualizer/lidar_visualizer_utils.cc +++ b/common/visualizer/lidar_visualizer_utils.cc @@ -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); } -void DrawSphere(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); } diff --git a/common/visualizer/lidar_visualizer_utils.h b/common/visualizer/lidar_visualizer_utils.h index 92bb734..32f5778 100644 --- a/common/visualizer/lidar_visualizer_utils.h +++ b/common/visualizer/lidar_visualizer_utils.h @@ -42,7 +42,7 @@ void AddLine(const pcl::PointXYZ& pt1, const pcl::PointXYZ& pt2, const Color& color, const std::string& id, PCLVisualizer* const viewer); -void DrawSphere(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 \ No newline at end of file diff --git a/oh_my_loam/base/helper.cc b/oh_my_loam/base/helper.cc index 9372a23..0f8a0d5 100644 --- a/oh_my_loam/base/helper.cc +++ b/oh_my_loam/base/helper.cc @@ -2,13 +2,13 @@ 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) { - Pose3d pose_interp = Pose3d().Interpolate(pose, GetTime(pt_in)); + Pose3D pose_interp = Pose3D().Interpolate(pose, GetTime(pt_in)); TransformPoint(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) { TransformToStart(pose, pt_in, pt_out); TransformPoint(pose.Inv(), *pt_out, pt_out); diff --git a/oh_my_loam/base/helper.h b/oh_my_loam/base/helper.h index cb3da80..1ad7c25 100644 --- a/oh_my_loam/base/helper.h +++ b/oh_my_loam/base/helper.h @@ -6,7 +6,7 @@ namespace oh_my_loam { using common::TPoint; -using common::Pose3d; +using common::Pose3D; using common::Trans3d; 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(pt.time); } template -void TransformPoint(const Pose3d& pose, const PointType& pt_in, +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); @@ -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 */ -void TransformToStart(const Pose3d& pose, const TPoint& pt_in, +void TransformToStart(const Pose3D& pose, const TPoint& pt_in, TPoint* const pt_out); /** * @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, +void TransformToEnd(const Pose3D& pose, const TPoint& pt_in, TPoint* const pt_out); struct PointLinePair { diff --git a/oh_my_loam/mapper/mapper.h b/oh_my_loam/mapper/mapper.h index 4098f30..fb5b9bf 100644 --- a/oh_my_loam/mapper/mapper.h +++ b/oh_my_loam/mapper/mapper.h @@ -18,7 +18,7 @@ class Mapper { void Visualize(); common::TPointCloudPtr map_pts_; - common::Pose3d pose_curr2world_; + common::Pose3D pose_curr2world_; bool is_initialized = false; diff --git a/oh_my_loam/odometer/odometer.cc b/oh_my_loam/odometer/odometer.cc index aaa612b..f6daf83 100644 --- a/oh_my_loam/odometer/odometer.cc +++ b/oh_my_loam/odometer/odometer.cc @@ -23,12 +23,12 @@ bool Odometer::Init() { return true; } -void Odometer::Process(const Feature& feature, Pose3d* const pose) { +void Odometer::Process(const Feature& feature, Pose3D* const pose) { BLOCK_TIMER_START; if (!is_initialized_) { is_initialized_ = true; UpdatePre(feature); - *pose = Pose3d(); + *pose = Pose3D(); AWARN << "Odometer initialized...."; return; } @@ -62,7 +62,7 @@ void Odometer::Process(const Feature& feature, Pose3d* const pose) { solver.AddPointPlanePair(pair, GetTime(pair.pt)); } solver.Solve(); - pose_curr2last_ = Pose3d(q, p); + pose_curr2last_ = Pose3D(q, p); } pose_curr2world_ = pose_curr2world_ * pose_curr2last_; *pose = pose_curr2world_; diff --git a/oh_my_loam/odometer/odometer.h b/oh_my_loam/odometer/odometer.h index dbbd9d7..0119bed 100644 --- a/oh_my_loam/odometer/odometer.h +++ b/oh_my_loam/odometer/odometer.h @@ -15,7 +15,7 @@ class Odometer { bool Init(); - void Process(const Feature& feature, common::Pose3d* const pose); + void Process(const Feature& feature, common::Pose3D* const pose); protected: void UpdatePre(const Feature& feature); @@ -32,8 +32,8 @@ class Odometer { const std::vector& pl_pairs, const std::vector& pp_pairs) const; - common::Pose3d pose_curr2world_; - common::Pose3d pose_curr2last_; + common::Pose3D pose_curr2world_; + common::Pose3D pose_curr2last_; common::TPointCloudPtr corn_pre_{nullptr}; common::TPointCloudPtr surf_pre_{nullptr}; diff --git a/oh_my_loam/oh_my_loam.cc b/oh_my_loam/oh_my_loam.cc index 39a4321..62c5cdb 100644 --- a/oh_my_loam/oh_my_loam.cc +++ b/oh_my_loam/oh_my_loam.cc @@ -35,7 +35,7 @@ void OhMyLoam::Run(double timestamp, const PointCloudConstPtr& cloud_in) { RemoveOutliers(*cloud_in, cloud.get()); Feature feature; extractor_->Process(cloud, &feature); - Pose3d pose; + Pose3D pose; odometer_->Process(feature, &pose); poses_.emplace_back(pose); } diff --git a/oh_my_loam/oh_my_loam.h b/oh_my_loam/oh_my_loam.h index 8d38441..b492e5c 100644 --- a/oh_my_loam/oh_my_loam.h +++ b/oh_my_loam/oh_my_loam.h @@ -25,7 +25,7 @@ class OhMyLoam { // remove outliers: nan and very close points void RemoveOutliers(const common::PointCloud& cloud_in, common::PointCloud* const cloud_out) const; - std::vector poses_; + std::vector poses_; DISALLOW_COPY_AND_ASSIGN(OhMyLoam) }; diff --git a/oh_my_loam/visualizer/odometer_visualizer.cc b/oh_my_loam/visualizer/odometer_visualizer.cc index 2e71bf0..a12107b 100644 --- a/oh_my_loam/visualizer/odometer_visualizer.cc +++ b/oh_my_loam/visualizer/odometer_visualizer.cc @@ -33,9 +33,9 @@ void OdometerVisualizer::Draw() { DrawPointCloud(tgt_corn_pts, BLUE, "tgt_corn_pts", 4); DrawPointCloud(src_surf_pts, PURPLE, "src_surf_pts", 7); DrawPointCloud(tgt_surf_pts, RED, "tgt_surf_pts", 4); - std::vector poses_n; + std::vector poses_n; poses_n.reserve((poses_.size())); - Pose3d pose_inv = frame.pose_curr2world.Inv(); + Pose3D pose_inv = frame.pose_curr2world.Inv(); for (const auto& pose : poses_) { poses_n.emplace_back(pose_inv * pose); }; diff --git a/oh_my_loam/visualizer/odometer_visualizer.h b/oh_my_loam/visualizer/odometer_visualizer.h index 135d8ee..8397320 100644 --- a/oh_my_loam/visualizer/odometer_visualizer.h +++ b/oh_my_loam/visualizer/odometer_visualizer.h @@ -10,8 +10,8 @@ struct OdometerVisFrame : public common::LidarVisFrame { common::TPointCloudPtr corn_pts; std::vector pl_pairs; std::vector pp_pairs; - common::Pose3d pose_curr2last; - common::Pose3d pose_curr2world; + common::Pose3D pose_curr2last; + common::Pose3D pose_curr2world; }; class OdometerVisualizer : public common::LidarVisualizer { @@ -23,7 +23,7 @@ class OdometerVisualizer : public common::LidarVisualizer { private: void Draw() override; - std::deque poses_; + std::deque poses_; }; } // namespace oh_my_loam