From f2993b40b86baf820c3cd6a27b492f6ad0d37d82 Mon Sep 17 00:00:00 2001 From: feixyz10 Date: Mon, 8 Feb 2021 17:09:52 +0800 Subject: [PATCH] mapper nice --- common/geometry/trajectory.h | 64 +++++++++++++++++++++ common/pcl/pcl_utils.h | 8 ++- common/visualizer/lidar_visualizer_utils.cc | 14 ++++- common/visualizer/lidar_visualizer_utils.h | 4 ++ ggg.txt | 42 ++++++++++++++ oh_my_loam/configs/config.yaml | 4 +- oh_my_loam/mapper/mapper.cc | 50 ++++++++++------ oh_my_loam/mapper/mapper.h | 2 + oh_my_loam/oh_my_loam.cc | 4 +- oh_my_loam/solver/cost_function.h | 12 ++-- oh_my_loam/visualizer/mapper_visualizer.cc | 63 +++++--------------- oh_my_loam/visualizer/mapper_visualizer.h | 10 ++-- 12 files changed, 191 insertions(+), 86 deletions(-) create mode 100644 common/geometry/trajectory.h create mode 100644 ggg.txt diff --git a/common/geometry/trajectory.h b/common/geometry/trajectory.h new file mode 100644 index 0000000..f969f0a --- /dev/null +++ b/common/geometry/trajectory.h @@ -0,0 +1,64 @@ +#pragma once + +#include +#include "common/geometry/pose3d.h" + +namespace common { + +class Trajectory { + public: + typedef std::shared_ptr Ptr; + typedef std::shared_ptr ConstPtr; + + Trajectory() = default; + + explicit Trajectory(const std::vector &poses) { + poses_ = poses; + } + + size_t size() const { + return poses_.size(); + } + + bool empty() const { + return poses_.empty(); + } + + const Pose3d &at(size_t i) const { + return poses_.at(i); + } + + void AddPose(const Pose3d &pose) { + poses_.push_back(pose); + } + + Trajectory Copy(bool last_as_origin = false) const { + Trajectory traj; + if (empty()) return traj; + if (!last_as_origin) return *this; + const Pose3d &last_inv = poses_.back().Inv(); + for (const auto &pose : poses_) { + traj.AddPose(last_inv * pose); + } + return traj; + } + + std::vector GetPointSeq(bool last_as_origin = false) const { + std::vector point_seq; + if (empty()) return point_seq; + const Pose3d &last_inv = poses_.back().Inv(); + for (const auto &pose : poses_) { + if (last_as_origin) { + point_seq.push_back((last_inv * pose).t_vec()); + } else { + point_seq.push_back(pose.t_vec()); + } + } + return point_seq; + }; + + private: + std::vector poses_; +}; + +} // namespace common \ No newline at end of file diff --git a/common/pcl/pcl_utils.h b/common/pcl/pcl_utils.h index fbba318..fe64b4c 100644 --- a/common/pcl/pcl_utils.h +++ b/common/pcl/pcl_utils.h @@ -98,7 +98,13 @@ void VoxelDownSample(const typename pcl::PointCloud::ConstPtr &cloud_in, pcl::VoxelGrid filter; filter.setInputCloud(cloud_in); filter.setLeafSize(voxel_size, voxel_size, voxel_size); - filter.filter(*cloud_out); + if (cloud_out == cloud_in.get()) { + pcl::PointCloud cloud_downsampled; + filter.filter(cloud_downsampled); + cloud_out->swap(cloud_downsampled); + } else { + filter.filter(*cloud_out); + } } } // namespace common \ No newline at end of file diff --git a/common/visualizer/lidar_visualizer_utils.cc b/common/visualizer/lidar_visualizer_utils.cc index 6272475..ae336b3 100644 --- a/common/visualizer/lidar_visualizer_utils.cc +++ b/common/visualizer/lidar_visualizer_utils.cc @@ -1,3 +1,15 @@ #include "lidar_visualizer_utils.h" -namespace common {} // namespace common \ No newline at end of file +namespace common { + +void AddTrajectory(const Trajectory &trajectory, const Color &color, + const std::string &id, PCLVisualizer *const viewer) { + for (size_t i = 0; i < trajectory.size() - 1; ++i) { + Eigen::Vector3f p1 = trajectory.at(i).t_vec().cast(); + Eigen::Vector3f p2 = trajectory.at(i + 1).t_vec().cast(); + AddLine({p1.x(), p1.y(), p1.z()}, {p2.x(), p2.y(), p2.z()}, color, + id + std::to_string(i), viewer); + } +} + +} // namespace common \ No newline at end of file diff --git a/common/visualizer/lidar_visualizer_utils.h b/common/visualizer/lidar_visualizer_utils.h index a379e1c..74d8afd 100644 --- a/common/visualizer/lidar_visualizer_utils.h +++ b/common/visualizer/lidar_visualizer_utils.h @@ -8,6 +8,7 @@ #include #include "common/color/color.h" +#include "common/geometry/trajectory.h" #include "common/pcl/pcl_types.h" namespace common { @@ -49,4 +50,7 @@ void AddSphere(const PT ¢er, double radius, const Color &color, viewer->addSphere(center, radius, color.r, color.g, color.b, id); } +void AddTrajectory(const Trajectory &trajectory, const Color &color, + const std::string &id, PCLVisualizer *const viewer); + } // namespace common \ No newline at end of file diff --git a/ggg.txt b/ggg.txt new file mode 100644 index 0000000..3ecd0d1 --- /dev/null +++ b/ggg.txt @@ -0,0 +1,42 @@ +scan line number 16 +line resolution 0.200000 plane resolution 0.400000 +Mapping 10 Hz +... logging to /home/liufei/.ros/log/8dd723e0-69e0-11eb-931b-4cedfbbfbb36/roslaunch-liufei-fabu-4816.log +Checking log directory for disk usage. This may take awhile. +Press Ctrl-C to interrupt +Done checking log file disk usage. Usage is <1GB. +]2;/home/liufei/catkin_ws/src/A-LOAM/launch/aloam_velodyne_VLP_16.launch +started roslaunch server http://liufei-fabu:38301/ + +SUMMARY +======== + +PARAMETERS + * /mapping_line_resolution: 0.2 + * /mapping_plane_resolution: 0.4 + * /mapping_skip_frame: 1 + * /minimum_range: 0.3 + * /rosdistro: kinetic + * /rosversion: 1.12.16 + * /scan_line: 16 + +NODES + / + alaserMapping (aloam_velodyne/alaserMapping) + alaserOdometry (aloam_velodyne/alaserOdometry) + ascanRegistration (aloam_velodyne/ascanRegistration) + rviz (rviz/rviz) + +ROS_MASTER_URI=http://localhost:11311 +]2;/home/liufei/catkin_ws/src/A-LOAM/launch/aloam_velodyne_VLP_16.launch http://localhost:11311 +process[ascanRegistration-1]: started with pid [4854] +process[alaserOdometry-2]: started with pid [4855] +process[alaserMapping-3]: started with pid [4856] +process[rviz-4]: started with pid [4857] +[rviz-4] killing on exit +[alaserMapping-3] killing on exit +[alaserOdometry-2] killing on exit +[ascanRegistration-1] killing on exit +shutting down processing monitor... +... shutting down processing monitor complete +done diff --git a/oh_my_loam/configs/config.yaml b/oh_my_loam/configs/config.yaml index a0f3bce..f6db393 100644 --- a/oh_my_loam/configs/config.yaml +++ b/oh_my_loam/configs/config.yaml @@ -2,7 +2,7 @@ lidar: VPL16 log_to_file: false log_path: /data/log/oh_my_loam -vis: true +vis: false # configs for extractor extractor_config: @@ -31,7 +31,7 @@ odometer_config: # configs for mapper mapper_config: - vis: false + vis: true verbose: false map_shape: [3, 21, 21] map_step: 50 diff --git a/oh_my_loam/mapper/mapper.cc b/oh_my_loam/mapper/mapper.cc index 85cec18..7ec292e 100644 --- a/oh_my_loam/mapper/mapper.cc +++ b/oh_my_loam/mapper/mapper.cc @@ -26,7 +26,7 @@ bool Mapper::Init() { map_step_ = config_["map_step"].as(); corn_map_.reset(new Map(map_shape_, map_step_)); surf_map_.reset(new Map(map_shape_, map_step_)); - if (is_vis_) visualizer_.reset(MapperVisualizer); + if (is_vis_) visualizer_.reset(new MapperVisualizer); return true; } @@ -64,15 +64,18 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn, TPoint cnt(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(), pose_curr2map.t_vec().z()); // AdjustMap(cnt); - TPointCloudPtr cloud_corn_map = corn_map_->GetSurrPoints(cnt, submap_shape_); - TPointCloudPtr cloud_surf_map = surf_map_->GetSurrPoints(cnt, submap_shape_); - kdtree_corn_.setInputCloud(cloud_corn_map); - kdtree_surf_.setInputCloud(cloud_surf_map); + // TPointCloudPtr cloud_corn_map = corn_map_->GetSurrPoints(cnt, + // submap_shape_); TPointCloudPtr cloud_surf_map = + // surf_map_->GetSurrPoints(cnt, submap_shape_); + kdtree_corn_.setInputCloud(cloud_corn_map_); + kdtree_surf_.setInputCloud(cloud_surf_map_); + std::vector pl_pairs; + std::vector pp_pairs; for (int i = 0; i < config_["icp_iter_num"].as(); ++i) { - std::vector pl_pairs; - MatchCorn(cloud_corn_map, pose_curr2map, &pl_pairs); - std::vector pp_pairs; - MatchSurf(cloud_surf_map, pose_curr2map, &pp_pairs); + std::vector().swap(pl_pairs); + std::vector().swap(pp_pairs); + MatchCorn(cloud_corn, pose_curr2map, &pl_pairs); + MatchSurf(cloud_surf, pose_curr2map, &pp_pairs); AINFO_IF(verbose_) << "Iter " << i << ": matched num: point2line = " << pl_pairs.size() << ", point2plane = " << pp_pairs.size(); @@ -97,10 +100,11 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn, << BLOCK_TIMER_STOP_FMT; } UpdateMap(pose_curr2map, cloud_corn, cloud_surf); - SetState(DONE); // be careful with deadlock std::lock_guard lock(state_mutex_); pose_odom2map_ = pose_curr2map * pose_curr2odom.Inv(); AUSER << "Mapper::Run: " << BLOCK_TIMER_STOP_FMT; + if (is_vis_) Visualize(pl_pairs, pp_pairs, pose_curr2odom, pose_curr2map); + state_ = DONE; // be careful with deadlock } void Mapper::MatchCorn(const TPointCloudConstPtr &cloud_curr, @@ -119,7 +123,7 @@ void Mapper::MatchCorn(const TPointCloudConstPtr &cloud_curr, } if (dists.back() > neighbor_point_dist_sq_th) continue; TPointCloud neighbor_pts; - pcl::copyPointCloud(*cloud_curr, indices, neighbor_pts); + pcl::copyPointCloud(*kdtree_corn_.getInputCloud(), indices, neighbor_pts); double fit_score = 0.0; LineCoeff coeff = common::FitLine3D(neighbor_pts, &fit_score); if (fit_score < config_["min_line_fit_score"].as()) continue; @@ -143,7 +147,7 @@ void Mapper::MatchSurf(const TPointCloudConstPtr &cloud_curr, } if (dists.back() > neighbor_point_dist_sq_th) continue; TPointCloud neighbor_pts; - pcl::copyPointCloud(*cloud_curr, indices, neighbor_pts); + pcl::copyPointCloud(*kdtree_surf_.getInputCloud(), indices, neighbor_pts); double fit_score = 0.0; Eigen::Vector4d coeff = common::FitPlane(neighbor_pts, &fit_score); if (fit_score < config_["min_plane_fit_score"].as()) continue; @@ -189,16 +193,20 @@ void Mapper::AdjustMap(const TPoint ¢er) { void Mapper::UpdateMap(const common::Pose3d &pose_curr2map, const TPointCloudConstPtr &cloud_corn, const TPointCloudConstPtr &cloud_surf) { - auto update_map = [&](const TPointCloudConstPtr &cloud, Map *const map) { + auto update_map = [&](const TPointCloudConstPtr &cloud, + /*Map*const map*/ const TPointCloudPtr &cloud_map) { TPointCloudPtr cloud_trans(new TPointCloud); common::TransformPointCloud(pose_curr2map, *cloud, cloud_trans.get()); - std::vector indices; - map->AddPoints(cloud_trans, &indices); - map->Downsample(indices, config_["downsample_voxel_size"].as()); + *cloud_map += *cloud_trans; + // std::vector indices; + // map->AddPoints(cloud_trans, &indices); + // map->Downsample(indices, config_["downsample_voxel_size"].as()); + common::VoxelDownSample(cloud_map, cloud_map.get(), + config_["downsample_voxel_size"].as()); }; std::lock_guard lock(map_mutex_); - update_map(cloud_corn, corn_map_.get()); - update_map(cloud_surf, surf_map_.get()); + update_map(cloud_corn, cloud_corn_map_); + update_map(cloud_surf, cloud_surf_map_); } TPointCloudPtr Mapper::GetMapCloudCorn() const { @@ -237,9 +245,13 @@ void Mapper::Visualize(const std::vector &pl_pairs, frame->timestamp = timestamp; frame->cloud_corn = kdtree_corn_.getInputCloud()->makeShared(); frame->cloud_surf = kdtree_surf_.getInputCloud()->makeShared(); + AINFO << "Map point num: " << frame->cloud_corn->size() << ", " + << frame->cloud_surf->size(); frame->pl_pairs = pl_pairs; frame->pp_pairs = pp_pairs; - frame->pose_curr2world + frame->pose_curr2odom = pose_curr2odom; + frame->pose_curr2map = pose_curr2map; + visualizer_->Render(frame); } } // namespace oh_my_loam \ No newline at end of file diff --git a/oh_my_loam/mapper/mapper.h b/oh_my_loam/mapper/mapper.h index 4409488..6914116 100644 --- a/oh_my_loam/mapper/mapper.h +++ b/oh_my_loam/mapper/mapper.h @@ -85,6 +85,8 @@ class Mapper { bool verbose_ = false; std::unique_ptr visualizer_{nullptr}; + TPointCloudPtr cloud_corn_map_{new TPointCloud}; + TPointCloudPtr cloud_surf_map_{new TPointCloud}; DISALLOW_COPY_AND_ASSIGN(Mapper) }; diff --git a/oh_my_loam/oh_my_loam.cc b/oh_my_loam/oh_my_loam.cc index 6492c9b..83fcf9e 100644 --- a/oh_my_loam/oh_my_loam.cc +++ b/oh_my_loam/oh_my_loam.cc @@ -50,8 +50,8 @@ void OhMyLoam::Run(double timestamp, common::Pose3d pose_curr2odom; odometer_->Process(timestamp, features, &pose_curr2odom); common::Pose3d pose_curr2map; - const auto &cloud_corn = odometer_->GetCloudCorn(); - const auto &cloud_surf = odometer_->GetCloudSurf(); + const auto &cloud_corn = odometer_->GetCloudCorn()->makeShared(); + const auto &cloud_surf = odometer_->GetCloudSurf()->makeShared(); mapper_->Process(timestamp, cloud_corn, cloud_surf, pose_curr2odom, &pose_curr2map); poses_curr2odom_.push_back(pose_curr2odom); diff --git a/oh_my_loam/solver/cost_function.h b/oh_my_loam/solver/cost_function.h index 2364b6c..e21b815 100644 --- a/oh_my_loam/solver/cost_function.h +++ b/oh_my_loam/solver/cost_function.h @@ -7,10 +7,6 @@ namespace oh_my_loam { -namespace { -const double kEpsilon = 1.0e-6; -} - struct PointLinePair { TPoint pt; struct Line { @@ -149,7 +145,7 @@ bool PointLineCostFunction::operator()(const T *const r_quat, Eigen::Quaternion r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]); Eigen::Matrix t(t_vec[0], t_vec[1], t_vec[2]); - if (time_ <= 1.0 - kEpsilon) { + if (time_ <= 1.0 - 1.0e-6) { r = Eigen::Quaternion::Identity().slerp(T(time_), r); t = T(time_) * t; } @@ -177,7 +173,7 @@ bool PointPlaneCostFunction::operator()(const T *const r_quat, Eigen::Quaternion r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]); Eigen::Matrix t(t_vec[0], t_vec[1], t_vec[2]); - if (time_ <= 1.0 - kEpsilon) { + if (time_ <= 1.0 - 1.0e-6) { r = Eigen::Quaternion::Identity().slerp(T(time_), r); t = T(time_) * t; } @@ -199,7 +195,7 @@ bool PointLineCoeffCostFunction::operator()(const T *const r_quat, Eigen::Quaternion r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]); Eigen::Matrix t(t_vec[0], t_vec[1], t_vec[2]); - if (time_ <= 1.0 - kEpsilon) { + if (time_ <= 1.0 - 1.0e-6) { r = Eigen::Quaternion::Identity().slerp(T(time_), r); t = T(time_) * t; } @@ -221,7 +217,7 @@ bool PointPlaneCoeffCostFunction::operator()(const T *const r_quat, Eigen::Quaternion r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]); Eigen::Matrix t(t_vec[0], t_vec[1], t_vec[2]); - if (time_ <= 1.0 - kEpsilon) { + if (time_ <= 1.0 - 1.0e-6) { r = Eigen::Quaternion::Identity().slerp(T(time_), r); t = T(time_) * t; } diff --git a/oh_my_loam/visualizer/mapper_visualizer.cc b/oh_my_loam/visualizer/mapper_visualizer.cc index a837620..1405290 100644 --- a/oh_my_loam/visualizer/mapper_visualizer.cc +++ b/oh_my_loam/visualizer/mapper_visualizer.cc @@ -13,78 +13,43 @@ void MapperVisualizer::Draw() { DrawPointCloud(frame.cloud_corn, GRAY, "cloud_corn"); DrawPointCloud(frame.cloud_surf, GRAY, "cloud_surf"); - DrawCorn(frame.pose_curr2odom, frame.pl_pairs); - DrawSurf(frame.pose_curr2odom, frame.pp_pairs); + DrawCorn(frame.pose_curr2odom, frame.pose_curr2map, frame.pl_pairs); + DrawSurf(frame.pose_curr2odom, frame.pose_curr2map, frame.pp_pairs); - poses_.push_back(frame.pose_curr2map); + traj_odom_.AddPose(frame.pose_curr2odom); + traj_map_.AddPose(frame.pose_curr2map); DrawTrajectory(); } -void MapperVisualizer::DrawCorn(const Pose3d &pose_curr2odom, - const Pose3d &pose_curr2map, +void MapperVisualizer::DrawCorn(const common::Pose3d &pose_odom, + const common::Pose3d &pose_map, const std::vector &pairs) { TPointCloudPtr src(new TPointCloud); - TPointCloudPtr tgt(new TPointCloud); src->resize(pairs.size()); - tgt->resize(pairs.size() * 2); + const auto &pose = trans_ ? pose_map : pose_odom; for (size_t i = 0; i < pairs.size(); ++i) { const auto &pair = pairs[i]; - src->at(i) = pair.pt; - if (trans_) TransformToStart(pose, pair.pt, &src->points[i]); + TransformToStart(pose, pair.pt, &src->at(i)); } - DrawPointCloud(tgt, YELLOW, "tgt_corn", 4); DrawPointCloud(src, RED, "src_corn", 4); - for (size_t i = 0; i < src->size(); ++i) { - const auto &p = src->at(i), &p1 = tgt->at(2 * i), &p2 = tgt->at(2 * i + 1); - common::AddLine(p, p1, WHITE, "corn_line1_" + std::to_string(i), - viewer_.get()); - common::AddLine(p, p2, WHITE, "corn_line2_" + std::to_string(i), - viewer_.get()); - } } -void MapperVisualizer::DrawSurf(const Pose3d &pose_curr2odom, - const Pose3d &pose_curr2map, +void MapperVisualizer::DrawSurf(const common::Pose3d &pose_odom, + const common::Pose3d &pose_map, const std::vector &pairs) { TPointCloudPtr src(new TPointCloud); - TPointCloudPtr tgt(new TPointCloud); src->resize(pairs.size()); - tgt->resize(pairs.size() * 3); + const auto &pose = trans_ ? pose_map : pose_odom; for (size_t i = 0; i < pairs.size(); ++i) { const auto &pair = pairs[i]; - src->at(i) = pair.pt; - if (trans_) TransformToStart(pose, pair.pt, &src->points[i]); - // tgt->at(3 * i) = pair.plane.pt1; - // tgt->at(3 * i + 1) = pair.plane.pt2; - // tgt->at(3 * i + 2) = pair.plane.pt3; + TransformToStart(pose, pair.pt, &src->at(i)); } - DrawPointCloud(tgt, BLUE, "tgt_surf", 4); DrawPointCloud(src, CYAN, "src_surf", 4); - for (size_t i = 0; i < src->size(); ++i) { - const auto &p = src->at(i), &p1 = tgt->at(3 * i), &p2 = tgt->at(3 * i + 1), - &p3 = tgt->at(3 * i + 2); - AddLine(p, p1, WHITE, "surf_line1_" + std::to_string(i), - viewer_.get()); - AddLine(p, p2, WHITE, "surf_line2_" + std::to_string(i), - viewer_.get()); - AddLine(p, p3, WHITE, "surf_line3_" + std::to_string(i), - viewer_.get()); - } } void MapperVisualizer::DrawTrajectory() { - std::vector poses_n; - poses_n.reserve((poses_.size())); - Pose3d pose_inv = poses_.back().Inv(); - for (const auto &pose : poses_) { - poses_n.emplace_back(pose_inv * pose); - }; - for (size_t i = 0; i < poses_n.size() - 1; ++i) { - Eigen::Vector3f p1 = poses_n[i].t_vec().cast(); - Eigen::Vector3f p2 = poses_n[i + 1].t_vec().cast(); - AddLine({p1.x(), p1.y(), p1.z()}, {p2.x(), p2.y(), p2.z()}, PINK, - "trajectory" + std::to_string(i), viewer_.get()); - } + common::AddTrajectory(traj_odom_, PINK, "traj_odmo", viewer_.get()); + common::AddTrajectory(traj_map_, PURPLE, "traj_map", viewer_.get()); } void MapperVisualizer::KeyboardEventCallback( diff --git a/oh_my_loam/visualizer/mapper_visualizer.h b/oh_my_loam/visualizer/mapper_visualizer.h index 034cf12..db9092f 100644 --- a/oh_my_loam/visualizer/mapper_visualizer.h +++ b/oh_my_loam/visualizer/mapper_visualizer.h @@ -1,13 +1,14 @@ #pragma once +#include "common/geometry/trajectory.h" #include "common/visualizer/lidar_visualizer.h" #include "oh_my_loam/solver/cost_function.h" namespace oh_my_loam { struct MapperVisFrame : public common::LidarVisFrame { - TPointCloudConstPtr cloud_surf; TPointCloudConstPtr cloud_corn; + TPointCloudConstPtr cloud_surf; std::vector pl_pairs; std::vector pp_pairs; common::Pose3d pose_curr2odom; @@ -23,10 +24,10 @@ class MapperVisualizer : public common::LidarVisualizer { private: void Draw() override; - void DrawCorn(const common::Pose3d &pose, + void DrawCorn(const common::Pose3d &pose_odom, const common::Pose3d &pose_map, const std::vector &pairs); - void DrawSurf(const common::Pose3d &pose, + void DrawSurf(const common::Pose3d &pose_odom, const common::Pose3d &pose_map, const std::vector &pairs); void DrawTrajectory(); @@ -36,7 +37,8 @@ class MapperVisualizer : public common::LidarVisualizer { bool trans_ = true; - std::vector poses_; + common::Trajectory traj_odom_; + common::Trajectory traj_map_; }; } // namespace oh_my_loam