diff --git a/common/color/color.h b/common/color/color.h index 3835ceb..b5aac89 100644 --- a/common/color/color.h +++ b/common/color/color.h @@ -12,7 +12,7 @@ struct Color { #define RED Color(255, 0, 0) #define GREEN Color(0, 255, 0) #define BLUE Color(0, 0, 255) -#define DRAK_GRAY Color(60, 60, 60) +#define DRAK_GRAY Color(50, 50, 50) #define GRAY Color(128, 128, 128) #define CYAN Color(0, 255, 255) #define PURPLE Color(160, 32, 240) diff --git a/common/pcl/pcl_utils.h b/common/pcl/pcl_utils.h index 10f5895..bf9c437 100644 --- a/common/pcl/pcl_utils.h +++ b/common/pcl/pcl_utils.h @@ -5,15 +5,15 @@ namespace common { -// Distance squred of a point to origin +// Distance squared of a point to origin template -inline double DistanceSqure(const PT &pt) { +inline double DistanceSquare(const PT &pt) { return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z; } -// Distance squred of two points +// Distance squared of two points template -inline double DistanceSqure(const PT &pt1, const PT &pt2) { +inline double DistanceSquare(const PT &pt1, const PT &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); } @@ -21,13 +21,13 @@ inline double DistanceSqure(const PT &pt1, const PT &pt2) { // Distance of a point to origin template inline double Distance(const PT &pt) { - return std::sqrt(DistanceSqure(pt)); + return std::sqrt(DistanceSquare(pt)); } // Distance squred of two points template inline double Distance(const PT &pt1, const PT &pt2) { - return std::sqrt(DistanceSqure(pt1, pt2)); + return std::sqrt(DistanceSquare(pt1, pt2)); } // Check whether is a finite point: neither infinite nor nan diff --git a/common/visualizer/lidar_visualizer_utils.cc b/common/visualizer/lidar_visualizer_utils.cc index fcefcc6..6272475 100644 --- a/common/visualizer/lidar_visualizer_utils.cc +++ b/common/visualizer/lidar_visualizer_utils.cc @@ -1,16 +1,3 @@ #include "lidar_visualizer_utils.h" -namespace common { - -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 ¢er, double radius, const Color &color, - const std::string &id, PCLVisualizer *const viewer) { - viewer->addSphere(center, radius, color.r, color.g, color.b, id); -} - -} // namespace common \ No newline at end of file +namespace common {} // 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 4175b2f..a379e1c 100644 --- a/common/visualizer/lidar_visualizer_utils.h +++ b/common/visualizer/lidar_visualizer_utils.h @@ -37,11 +37,16 @@ void AddPointCloud(const typename pcl::PointCloud::ConstPtr &cloud, 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); +template +void AddLine(const PT &pt1, const PT &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 ¢er, double radius, const Color &color, - const std::string &id, PCLVisualizer *const viewer); +template +void AddSphere(const PT ¢er, double radius, const Color &color, + const std::string &id, PCLVisualizer *const viewer) { + viewer->addSphere(center, radius, color.r, color.g, color.b, id); +} } // namespace common \ No newline at end of file diff --git a/main.cc b/main.cc index 079e1ba..a13d01e 100644 --- a/main.cc +++ b/main.cc @@ -48,6 +48,8 @@ void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr &msg, PointCloudPtr cloud(new PointCloud); pcl::fromROSMsg(*msg, *cloud); double timestamp = msg->header.stamp.toSec(); - AUSER << "Timestamp: " << FMT_TIMESTAMP(timestamp); + static size_t frame_id = 0; + AINFO << "Ohmyloam: frame_id = " << ++frame_id + << ", timestamp = " << FMT_TIMESTAMP(timestamp); slam->Run(timestamp, cloud); } \ No newline at end of file diff --git a/oh_my_loam/configs/config.yaml b/oh_my_loam/configs/config.yaml index 6aa52fd..57f0f6d 100644 --- a/oh_my_loam/configs/config.yaml +++ b/oh_my_loam/configs/config.yaml @@ -9,22 +9,25 @@ extractor_config: vis: false verbose: false min_point_num: 66 - sharp_corner_point_num: 2 - corner_point_num: 10 - flat_surf_point_num: 4 - surf_point_num: 20 + scan_seg_num: 36 + sharp_corner_point_num: 1 + corner_point_num: 4 + flat_surf_point_num: 1 corner_point_curvature_th: 0.5 surf_point_curvature_th: 0.5 neighbor_point_dist_sq_th: 0.1 + downsample_voxel_size: 0.6 # configs for odometer odometer_config: vis: true - verbose: true + verbose: false + nearby_scan_num: 1 + min_correspondence_num: 10 icp_iter_num: 2 solve_iter_num: 4 corn_match_dist_sq_th: 1.0 - surf_match_dist_sq_th: 1.0 + surf_match_dist_sq_th: 16.0 # configs for mapper mapper_config: diff --git a/oh_my_loam/extractor/extractor.cc b/oh_my_loam/extractor/extractor.cc index ece1095..4649548 100644 --- a/oh_my_loam/extractor/extractor.cc +++ b/oh_my_loam/extractor/extractor.cc @@ -7,7 +7,6 @@ namespace oh_my_loam { namespace { -const int kScanSegNum = 6; const double kTwoPi = 2 * M_PI; const double kEps = 1e-6; } // namespace @@ -58,18 +57,23 @@ void Extractor::Process(double timestamp, void Extractor::SplitScan(const common::PointCloud &cloud, std::vector *const scans) const { scans->resize(num_scans_); - double yaw_start = -std::atan2(cloud.points[0].y, cloud.points[0].x); + auto it = std::min_element(cloud.begin(), cloud.begin() + num_scans(), + [](const auto &p1, const auto &p2) { + return -std::atan2(p1.y, p1.x) < + -std::atan2(p2.y, p2.x); + }); + double yaw_start = -std::atan2(it->y, it->x); bool half_passed = false; for (const auto &pt : cloud) { int scan_id = GetScanID(pt); if (scan_id >= num_scans_ || scan_id < 0) continue; double yaw = -std::atan2(pt.y, pt.x); double yaw_diff = common::NormalizeAngle(yaw - yaw_start); - if (yaw_diff > 0) { - if (half_passed) yaw_start += kTwoPi; + if (yaw_diff >= -kEps) { + if (half_passed) yaw_diff += kTwoPi; } else { half_passed = true; - yaw_start += kTwoPi; + yaw_diff += kTwoPi; } double time = std::min(yaw_diff / kTwoPi, 1 - kEps) + scan_id; scans->at(scan_id).push_back( @@ -78,7 +82,8 @@ void Extractor::SplitScan(const common::PointCloud &cloud, } void Extractor::ComputeCurvature(TCTPointCloud *const scan) const { - if (scan->size() <= 10 + kScanSegNum) return; + size_t seg_num = config_["scan_seg_num"].as(); + if (scan->size() <= 10 + seg_num) return; 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 + @@ -99,19 +104,21 @@ void Extractor::ComputeCurvature(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; + int seg_num = config_["scan_seg_num"].as(); + if (pt_num < seg_num) return; + int seg_pt_num = (pt_num - 1) / seg_num + 1; + AERROR << pt_num << " " << seg_num << " " << seg_pt_num; + std::vector picked(pt_num, false); std::vector indices = common::Range(pt_num); int sharp_corner_point_num = config_["sharp_corner_point_num"].as(); int corner_point_num = config_["corner_point_num"].as(); int flat_surf_point_num = config_["flat_surf_point_num"].as(); - int surf_point_num = config_["surf_point_num"].as(); float corner_point_curvature_th = config_["corner_point_curvature_th"].as(); float surf_point_curvature_th = config_["surf_point_curvature_th"].as(); - for (int seg = 0; seg < kScanSegNum; ++seg) { + for (int seg = 0; seg < seg_num; ++seg) { int b = seg * seg_pt_num; int e = std::min((seg + 1) * seg_pt_num, pt_num); // sort by curvature for each segment: large -> small @@ -144,8 +151,6 @@ void Extractor::AssignType(TCTPointCloud *const scan) const { ++surf_point_picked_num; if (surf_point_picked_num <= flat_surf_point_num) { scan->at(ix).type = PointType::FLAT_SURF; - } else if (surf_point_picked_num <= surf_point_num) { - scan->at(ix).type = PointType::SURF; } else { break; } @@ -174,10 +179,15 @@ void Extractor::GenerateFeature(const TCTPointCloud &scan, feature->cloud_corner->push_back(point); break; default: - // feature->cloud_surf->push_back(point); + feature->cloud_surf->push_back(point); break; } } + TPointCloudPtr dowm_sampled(new TPointCloud); + common::VoxelDownSample( + feature->cloud_surf, dowm_sampled.get(), + config_["downsample_voxel_size"].as()); + feature->cloud_surf = dowm_sampled; } void Extractor::Visualize(const common::PointCloudConstPtr &cloud, @@ -193,12 +203,12 @@ void Extractor::Visualize(const common::PointCloudConstPtr &cloud, void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix, std::vector *const picked) const { auto dist_sq = [&](size_t i, size_t j) -> double { - return common::DistanceSqure(scan[i], scan[j]); + return common::DistanceSquare(scan[i], scan[j]); }; double neighbor_point_dist_sq_th = config_["neighbor_point_dist_sq_th"].as(); for (size_t i = 1; i <= 5; ++i) { - if (ix < i) break; + if (ix - i < 0) break; if (picked->at(ix - i)) continue; if (dist_sq(ix - i, ix - i + 1) > neighbor_point_dist_sq_th) break; picked->at(ix - i) = true; diff --git a/oh_my_loam/odometer/odometer.cc b/oh_my_loam/odometer/odometer.cc index 058b60e..afd2f81 100644 --- a/oh_my_loam/odometer/odometer.cc +++ b/oh_my_loam/odometer/odometer.cc @@ -10,11 +10,6 @@ namespace oh_my_loam { -namespace { -int kNearScanN = 2; -size_t kMinMatchNum = 10; -} // namespace - bool Odometer::Init() { const auto &config = common::YAMLConfig::Instance()->config(); config_ = config["odometer_config"]; @@ -33,17 +28,19 @@ void Odometer::Process(double timestamp, const std::vector &features, pose_curr2last_.SetIdentity(); pose_curr2world_.SetIdentity(); pose_out->SetIdentity(); - AWARN << "Odometer initialized...."; + AINFO << "Odometer initialized...."; return; } BLOCK_TIMER_START; - AINFO << "Pose before: " << pose_curr2world_.ToString(); - std::vector pl_pairs; + const auto &cloud_corn = kdtree_corn_.getInputCloud(); + const auto &cloud_surf = kdtree_surf_.getInputCloud(); + AINFO_IF(verbose_) << "Points to be matched: " << cloud_corn->size() << " + " + << cloud_surf->size(); std::vector pp_pairs; - AINFO_IF(verbose_) << "Points to be matched: " - << kdtree_corn_.getInputCloud()->size() << " + " - << kdtree_surf_.getInputCloud()->size(); + std::vector pl_pairs; for (int i = 0; i < config_["icp_iter_num"].as(); ++i) { + std::vector().swap(pl_pairs); + std::vector().swap(pp_pairs); for (const auto &feature : features) { MatchCorn(*feature.cloud_sharp_corner, &pl_pairs); MatchSurf(*feature.cloud_flat_surf, &pp_pairs); @@ -51,7 +48,8 @@ void Odometer::Process(double timestamp, const std::vector &features, AINFO_IF(verbose_) << "Iter " << i << ": matched num: point2line = " << pl_pairs.size() << ", point2plane = " << pp_pairs.size(); - if (pl_pairs.size() + pp_pairs.size() < kMinMatchNum) { + if (pl_pairs.size() + pp_pairs.size() < + config_["min_correspondence_num"].as()) { AWARN << "Too less correspondence: " << pl_pairs.size() << " + " << pp_pairs.size(); continue; @@ -63,17 +61,19 @@ void Odometer::Process(double timestamp, const std::vector &features, for (const auto &pair : pp_pairs) { solver.AddPointPlanePair(pair, GetTime(pair.pt)); } - solver.Solve(config_["solve_iter_num"].as(), verbose_, - &pose_curr2last_); - AINFO << "Odometer::ICP: iter_" << i << ": " << BLOCK_TIMER_STOP_FMT; + bool is_converge = solver.Solve(config_["solve_iter_num"].as(), + verbose_, &pose_curr2last_); + AWARN_IF(!is_converge) << "Solve: no_convergence"; + AINFO_IF(verbose_) << "Odometer::ICP: iter_" << i << ": " + << BLOCK_TIMER_STOP_FMT; } pose_curr2world_ = pose_curr2world_ * pose_curr2last_; *pose_out = pose_curr2world_; - AINFO << "Pose increase: " << pose_curr2last_.ToString(); - AINFO << "Pose after: " << pose_curr2world_.ToString(); + AINFO_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString(); + AINFO_IF(verbose_) << "Pose after: " << pose_curr2world_.ToString(); UpdatePre(features); AINFO << "Odometer::Porcess: " << BLOCK_TIMER_STOP_FMT; - if (is_vis_) Visualize(features, pl_pairs, pp_pairs); + if (is_vis_) Visualize(cloud_corn, cloud_surf, pl_pairs, pp_pairs); } void Odometer::MatchCorn(const TPointCloud &src, @@ -90,12 +90,13 @@ void Odometer::MatchCorn(const TPointCloud &src, TPoint pt1 = kdtree_corn_.getInputCloud()->at(indices[0]); int pt1_scan_id = GetScanId(pt1); + int nearby_scan_num = config_["nearby_scan_num"].as(); TPoint pt2; bool pt2_fount = false; float min_pt2_dist_sq = dist_sq_thresh; - int i_begin = std::max(0, pt1_scan_id - kNearScanN); - int i_end = - std::min(kdtrees_scan_corn_.size(), pt1_scan_id + kNearScanN + 1); + int i_begin = std::max(0, pt1_scan_id - nearby_scan_num); + int i_end = std::min(kdtrees_scan_corn_.size(), + pt1_scan_id + nearby_scan_num + 1); for (int i = i_begin; i < i_end; ++i) { if (i == pt1_scan_id) continue; const auto &kdtree = kdtrees_scan_corn_[i]; @@ -128,12 +129,13 @@ void Odometer::MatchSurf(const TPointCloud &src, TPoint pt1 = kdtree_surf_.getInputCloud()->at(indices[0]); int pt1_scan_id = GetScanId(pt1); + int nearby_scan_num = config_["nearby_scan_num"].as(); TPoint pt2; bool pt2_fount = false; float min_pt2_dist_sq = dist_sq_thresh; - int i_begin = std::max(0, pt1_scan_id - kNearScanN); - int i_end = - std::min(kdtrees_scan_surf_.size(), pt1_scan_id + kNearScanN + 1); + int i_begin = std::max(0, pt1_scan_id - nearby_scan_num); + int i_end = std::min(kdtrees_scan_surf_.size(), + pt1_scan_id + nearby_scan_num + 1); for (int i = i_begin; i < i_end; ++i) { if (i == pt1_scan_id) continue; const auto &kdtree = kdtrees_scan_surf_[i]; @@ -185,12 +187,15 @@ void Odometer::UpdatePre(const std::vector &features) { kdtree_surf_.setInputCloud(surf_pre); } -void Odometer::Visualize(const std::vector &features, +void Odometer::Visualize(const TPointCloudConstPtr &cloud_corn, + const TPointCloudConstPtr &cloud_surf, const std::vector &pl_pairs, const std::vector &pp_pairs, double timestamp) const { std::shared_ptr frame(new OdometerVisFrame); frame->timestamp = timestamp; + frame->cloud_corn = cloud_corn; + frame->cloud_surf = cloud_surf; frame->pl_pairs = pl_pairs; frame->pp_pairs = pp_pairs; frame->pose_curr2last = pose_curr2last_; diff --git a/oh_my_loam/odometer/odometer.h b/oh_my_loam/odometer/odometer.h index b1592bc..cd9ae3d 100644 --- a/oh_my_loam/odometer/odometer.h +++ b/oh_my_loam/odometer/odometer.h @@ -27,7 +27,8 @@ class Odometer { void MatchSurf(const TPointCloud &src, std::vector *const pairs) const; - void Visualize(const std::vector &features, + void Visualize(const TPointCloudConstPtr &cloud_corn, + const TPointCloudConstPtr &cloud_surf, const std::vector &pl_pairs, const std::vector &pp_pairs, double timestamp = 0.0) const; diff --git a/oh_my_loam/oh_my_loam.cc b/oh_my_loam/oh_my_loam.cc index dc1f32e..eca1a34 100644 --- a/oh_my_loam/oh_my_loam.cc +++ b/oh_my_loam/oh_my_loam.cc @@ -7,11 +7,10 @@ namespace oh_my_loam { namespace { const double kPointMinDist = 0.1; -using namespace common; } // namespace bool OhMyLoam::Init() { - YAML::Node config = YAMLConfig::Instance()->config(); + YAML::Node config = common::YAMLConfig::Instance()->config(); extractor_.reset(new ExtractorVLP16); if (!extractor_->Init()) { AERROR << "Failed to initialize extractor"; @@ -30,8 +29,9 @@ bool OhMyLoam::Init() { return true; } -void OhMyLoam::Run(double timestamp, const PointCloudConstPtr &cloud_in) { - PointCloudPtr cloud(new PointCloud); +void OhMyLoam::Run(double timestamp, + const common::PointCloudConstPtr &cloud_in) { + common::PointCloudPtr cloud(new common::PointCloud); RemoveOutliers(*cloud_in, cloud.get()); std::vector features; extractor_->Process(timestamp, cloud, &features); @@ -40,11 +40,12 @@ 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(cloud_in, cloud_out, [&](const Point &pt) { - return !IsFinite(pt) || - DistanceSqure(pt) < kPointMinDist * kPointMinDist; +void OhMyLoam::RemoveOutliers(const common::PointCloud &cloud_in, + common::PointCloud *const cloud_out) const { + common::RemovePoints(cloud_in, cloud_out, [&](const auto &pt) { + return !common::IsFinite(pt) || + common::DistanceSquare(pt) < + kPointMinDist * kPointMinDist; }); } diff --git a/oh_my_loam/solver/cost_function.h b/oh_my_loam/solver/cost_function.h index 9052556..8e371fa 100644 --- a/oh_my_loam/solver/cost_function.h +++ b/oh_my_loam/solver/cost_function.h @@ -59,10 +59,22 @@ bool PointLineCostFunction::operator()(const T *const r_quat, Eigen::Matrix p(T(pt.x), T(pt.y), T(pt.z)); Eigen::Matrix p1(T(pt1.x), T(pt1.y), T(pt1.z)); Eigen::Matrix p2(T(pt2.x), T(pt2.y), T(pt2.z)); + // if constexpr (!std::is_arithmetic::value) { + // AERROR << p.x().a << ", " << p.y().a << ", " << p.z().a; + // AERROR << p1.x().a << ", " << p1.y().a << ", " << p1.z().a; + // AERROR << p2.x().a << ", " << p2.y().a << ", " << p2.z().a; + // } Eigen::Quaternion r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]); Eigen::Quaternion r_interp = Eigen::Quaternion::Identity().slerp(T(time_), r); + // if constexpr (!std::is_arithmetic::value) { + // AERROR << time_; + // AERROR << r.w().a << " " << r.x().a << ", " << r.y().a << ", " << + // r.z().a; AERROR << r_interp.w().a << " " << r_interp.x().a << ", " << + // r_interp.y().a + // << ", " << r_interp.z().a; + // } Eigen::Matrix t(T(time_) * t_vec[0], T(time_) * t_vec[1], T(time_) * t_vec[2]); Eigen::Matrix p0 = r_interp * p + t; @@ -73,9 +85,9 @@ bool PointLineCostFunction::operator()(const T *const r_quat, residual[0] = area[0] / base_length; residual[1] = area[1] / base_length; residual[2] = area[2] / base_length; - if constexpr (!std::is_arithmetic::value) { - AERROR << p.transpose() << ", "; - } + // if constexpr (!std::is_arithmetic::value) { + // AERROR << base_length.a; + // } return true; } @@ -99,9 +111,6 @@ bool PointPlaneCostFunction::operator()(const T *const r_quat, Eigen::Matrix normal = (p2 - p1).cross(p3 - p1).normalized(); residual[0] = (p0 - p1).dot(normal); - if constexpr (!std::is_arithmetic::value) { - AERROR << "ppres" << residual[0].a; - } return true; } diff --git a/oh_my_loam/solver/solver.cc b/oh_my_loam/solver/solver.cc index 2b5c3f9..96334a6 100644 --- a/oh_my_loam/solver/solver.cc +++ b/oh_my_loam/solver/solver.cc @@ -30,8 +30,6 @@ bool PoseSolver::Solve(int max_iter_num, bool verbose, ceres::Solver::Summary summary; ceres::Solve(options, &problem_, &summary); AINFO_IF(verbose) << summary.BriefReport(); - AWARN_IF(summary.termination_type != ceres::CONVERGENCE) - << "Solve: no convergence"; if (pose) *pose = common::Pose3d(r_quat_, t_vec_); return summary.termination_type == ceres::CONVERGENCE; } diff --git a/oh_my_loam/visualizer/extractor_visualizer.cc b/oh_my_loam/visualizer/extractor_visualizer.cc index 9823b01..c41d40c 100644 --- a/oh_my_loam/visualizer/extractor_visualizer.cc +++ b/oh_my_loam/visualizer/extractor_visualizer.cc @@ -7,7 +7,7 @@ using namespace common; void ExtractorVisualizer::Draw() { auto frame = GetCurrentFrame(); - DrawPointCloud(frame.cloud, GRAY, "cloud_raw"); + DrawPointCloud(frame.cloud, DRAK_GRAY, "cloud_raw"); for (size_t i = 0; i < frame.features.size(); ++i) { const auto &feature = frame.features[i]; std::string id_suffix = std::to_string(i); diff --git a/oh_my_loam/visualizer/odometer_visualizer.cc b/oh_my_loam/visualizer/odometer_visualizer.cc index 7253fb5..d9e629b 100644 --- a/oh_my_loam/visualizer/odometer_visualizer.cc +++ b/oh_my_loam/visualizer/odometer_visualizer.cc @@ -10,49 +10,116 @@ using namespace common; void OdometerVisualizer::Draw() { auto frame = GetCurrentFrame(); - TPointCloudPtr src_corn(new TPointCloud); - TPointCloudPtr tgt_corn(new TPointCloud); - src_corn->resize(frame.pl_pairs.size()); - tgt_corn->resize(frame.pl_pairs.size() * 2); - for (size_t i = 0; i < frame.pl_pairs.size(); ++i) { - const auto &pair = frame.pl_pairs[i]; - TransformToStart(frame.pose_curr2last, pair.pt, &src_corn->points[i]); - tgt_corn->points[2 * i] = pair.line.pt1; - tgt_corn->points[2 * i + 1] = pair.line.pt2; + DrawPointCloud(frame.cloud_corn, GRAY, "cloud_corn"); + DrawPointCloud(frame.cloud_surf, GRAY, "cloud_surf"); + + DrawCorn(frame.pose_curr2last, frame.pl_pairs); + DrawSurf(frame.pose_curr2last, frame.pp_pairs); + + poses_.push_back(frame.pose_curr2world); + DrawTrajectory(); +} + +void OdometerVisualizer::DrawCorn(const Pose3d &pose, + const std::vector &pairs) { + TPointCloudPtr src(new TPointCloud); + TPointCloudPtr tgt(new TPointCloud); + src->resize(pairs.size()); + tgt->resize(pairs.size() * 2); + 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(2 * i) = pair.line.pt1; + tgt->at(2 * i + 1) = pair.line.pt2; } - TPointCloudPtr src_surf(new TPointCloud); - TPointCloudPtr tgt_surf(new TPointCloud); - src_surf->resize(frame.pp_pairs.size()); - tgt_surf->resize(frame.pp_pairs.size() * 3); - for (size_t i = 0; i < frame.pp_pairs.size(); ++i) { - const auto &pair = frame.pp_pairs[i]; - TransformToStart(frame.pose_curr2last, pair.pt, &src_surf->points[i]); - tgt_surf->points[3 * i] = pair.plane.pt1; - tgt_surf->points[3 * i + 1] = pair.plane.pt2; - tgt_surf->points[3 * i + 2] = pair.plane.pt3; + DrawPointCloud(tgt, YELLOW, "tgt_corn", 4); + DrawPointCloud(src, RED, "src_corn", 4); + if (!corn_connect_) return; + 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()); } - DrawPointCloud(tgt_corn, YELLOW, "tgt_corn", 4); - DrawPointCloud(src_corn, RED, "src_corn", 4); - DrawPointCloud(tgt_surf, BLUE, "tgt_surf", 4); - DrawPointCloud(src_surf, CYAN, "src_surf", 4); +} + +void OdometerVisualizer::DrawSurf(const Pose3d &pose, + const std::vector &pairs) { + TPointCloudPtr src(new TPointCloud); + TPointCloudPtr tgt(new TPointCloud); + src->resize(pairs.size()); + tgt->resize(pairs.size() * 3); + 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; + } + DrawPointCloud(tgt, BLUE, "tgt_surf", 4); + DrawPointCloud(src, CYAN, "src_surf", 4); + if (!surf_connect_) return; + 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 OdometerVisualizer::DrawTrajectory() { std::vector poses_n; poses_n.reserve((poses_.size())); - Pose3d pose_inv = frame.pose_curr2world.Inv(); + 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(); ++i) { + for (size_t i = 0; i < poses_n.size() - 1; ++i) { Eigen::Vector3f p1 = poses_n[i].t_vec().cast(); - if (i < poses_n.size() - 1) { - Eigen::Vector3f p2 = poses_n[i + 1].t_vec().cast(); - AddLine(Point(p1.x(), p1.y(), p1.z()), Point(p2.x(), p2.y(), p2.z()), - WHITE, "line" + std::to_string(i), viewer_.get()); - } else { - AddLine(Point(p1.x(), p1.y(), p1.z()), Point(0, 0, 0), WHITE, - "line" + std::to_string(i), viewer_.get()); - } + 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()); + } +} + +void OdometerVisualizer::KeyboardEventCallback( + const pcl::visualization::KeyboardEvent &event) { + if (event.getKeySym() == "p" && event.keyDown()) { + std::lock_guard lock(mutex_); + ++curr_frame_iter_; + if (curr_frame_iter_ == history_frames_.end()) { + --curr_frame_iter_; + } + is_updated_ = true; + } else if (event.getKeySym() == "n" && event.keyDown()) { + std::lock_guard lock(mutex_); + if (curr_frame_iter_ != history_frames_.begin()) { + --curr_frame_iter_; + } + is_updated_ = true; + } else if (event.getKeySym() == "t" && event.keyDown()) { + trans_ = !trans_; + is_updated_ = true; + } else if (event.getKeySym() == "c" && event.keyDown()) { + corn_connect_ = !corn_connect_; + is_updated_ = true; + } else if (event.getKeySym() == "s" && event.keyDown()) { + surf_connect_ = !surf_connect_; + is_updated_ = true; + } else if (event.getKeySym() == "r" && event.keyDown()) { + viewer_->setCameraPosition(0, 0, 200, 0, 0, 0, 1, 0, 0, 0); + viewer_->setSize(2500, 1500); + trans_ = true; + corn_connect_ = surf_connect_ = false; + is_updated_ = true; } - poses_.push_back(frame.pose_curr2world); } } // namespace oh_my_loam diff --git a/oh_my_loam/visualizer/odometer_visualizer.h b/oh_my_loam/visualizer/odometer_visualizer.h index e1ec406..3b87b02 100644 --- a/oh_my_loam/visualizer/odometer_visualizer.h +++ b/oh_my_loam/visualizer/odometer_visualizer.h @@ -6,8 +6,8 @@ namespace oh_my_loam { struct OdometerVisFrame : public common::LidarVisFrame { - TPointCloudPtr cloud_surf; - TPointCloudPtr cloud_corner; + TPointCloudConstPtr cloud_surf; + TPointCloudConstPtr cloud_corn; std::vector pl_pairs; std::vector pp_pairs; common::Pose3d pose_curr2last; @@ -23,6 +23,19 @@ class OdometerVisualizer : public common::LidarVisualizer { private: void Draw() override; + void DrawCorn(const Pose3d &pose, const std::vector &pairs); + + void DrawSurf(const Pose3d &pose, const std::vector &pairs); + + void DrawTrajectory(); + + void KeyboardEventCallback( + const pcl::visualization::KeyboardEvent &event) override; + + bool trans_ = true; + bool corn_connect_ = false; + bool surf_connect_ = false; + std::deque poses_; };