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

add mapper visualizer, not done

main
feixyz10 2021-02-06 23:40:42 +08:00 committed by feixyz
parent f16cf4a092
commit 083eeac5c4
7 changed files with 204 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
oh_my_loam/configs/config.yaml
View File

@ -32,7 +32,7 @@ odometer_config:
# configs for mapper
mapper_config:
vis: false
verbose: true
verbose: false
map_shape: [3, 21, 21]
map_step: 50
submap_shape: [1, 5, 5]

3
oh_my_loam/extractor/extractor.cc
View File

@ -75,7 +75,8 @@ void Extractor::SplitScan(const common::PointCloud &cloud,
half_passed = true;
yaw_diff += kTwoPi;
}
double time = std::min(yaw_diff / kTwoPi, 1 - kEps) + scan_id;
// double time = std::min(yaw_diff / kTwoPi, 1 - kEps) + scan_id;
double time = 0.999999 + scan_id;
scans->at(scan_id).push_back(
{pt.x, pt.y, pt.z, static_cast<float>(time), std::nanf("")});
}

40
oh_my_loam/mapper/mapper.cc
View File

@ -1,10 +1,12 @@
#include "mapper.h"
#include <memory>
#include <mutex>
#include "common/log/log.h"
#include "common/math/fitting.h"
#include "common/pcl/pcl_utils.h"
#include "oh_my_loam/visualizer/mapper_visualizer.h"
namespace oh_my_loam {
@ -24,6 +26,7 @@ bool Mapper::Init() {
map_step_ = config_["map_step"].as<double>();
corn_map_.reset(new Map(map_shape_, map_step_));
surf_map_.reset(new Map(map_shape_, map_step_));
if (is_vis_) visualizer_.reset(MapperVisualizer);
return true;
}
@ -63,15 +66,13 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
// 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);
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
pcl::KdTreeFLANN<TPoint> kdtree_corn;
kdtree_corn.setInputCloud(cloud_corn_map);
pcl::KdTreeFLANN<TPoint> kdtree_surf;
kdtree_surf.setInputCloud(cloud_surf_map);
std::vector<PointLineCoeffPair> pl_pairs;
MatchCorn(kdtree_corn, cloud_corn_map, pose_curr2map, &pl_pairs);
MatchCorn(cloud_corn_map, pose_curr2map, &pl_pairs);
std::vector<PointPlaneCoeffPair> pp_pairs;
MatchSurf(kdtree_surf, cloud_surf_map, pose_curr2map, &pp_pairs);
MatchSurf(cloud_surf_map, pose_curr2map, &pp_pairs);
AINFO_IF(verbose_) << "Iter " << i
<< ": matched num: point2line = " << pl_pairs.size()
<< ", point2plane = " << pp_pairs.size();
@ -102,8 +103,7 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
AUSER << "Mapper::Run: " << BLOCK_TIMER_STOP_FMT;
}
void Mapper::MatchCorn(const pcl::KdTreeFLANN<TPoint> &kdtree,
const TPointCloudConstPtr &cloud_curr,
void Mapper::MatchCorn(const TPointCloudConstPtr &cloud_curr,
const common::Pose3d &pose_curr2map,
std::vector<PointLineCoeffPair> *const pairs) const {
std::vector<int> indices;
@ -113,8 +113,8 @@ void Mapper::MatchCorn(const pcl::KdTreeFLANN<TPoint> &kdtree,
config_["neighbor_point_dist_sq_th"].as<float>();
for (const auto &pt : *cloud_curr) {
TPoint pt_query = common::TransformPoint(pose_curr2map, pt);
if (kdtree.nearestKSearch(pt_query, nearest_neighbor_k, indices, dists) !=
nearest_neighbor_k) {
if (kdtree_corn_.nearestKSearch(pt_query, nearest_neighbor_k, indices,
dists) != nearest_neighbor_k) {
continue;
}
if (dists.back() > neighbor_point_dist_sq_th) continue;
@ -127,8 +127,7 @@ void Mapper::MatchCorn(const pcl::KdTreeFLANN<TPoint> &kdtree,
}
}
void Mapper::MatchSurf(const pcl::KdTreeFLANN<TPoint> &kdtree,
const TPointCloudConstPtr &cloud_curr,
void Mapper::MatchSurf(const TPointCloudConstPtr &cloud_curr,
const common::Pose3d &pose_curr2map,
std::vector<PointPlaneCoeffPair> *const pairs) const {
std::vector<int> indices;
@ -138,8 +137,8 @@ void Mapper::MatchSurf(const pcl::KdTreeFLANN<TPoint> &kdtree,
config_["neighbor_point_dist_sq_th"].as<float>();
for (const auto &pt : *cloud_curr) {
TPoint pt_query = common::TransformPoint(pose_curr2map, pt);
if (kdtree.nearestKSearch(pt_query, nearest_neighbor_k, indices, dists) !=
nearest_neighbor_k) {
if (kdtree_surf_.nearestKSearch(pt_query, nearest_neighbor_k, indices,
dists) != nearest_neighbor_k) {
continue;
}
if (dists.back() > neighbor_point_dist_sq_th) continue;
@ -230,4 +229,17 @@ void Mapper::SetState(State state) {
state_ = state;
}
void Mapper::Visualize(const std::vector<PointLineCoeffPair> &pl_pairs,
const std::vector<PointPlaneCoeffPair> &pp_pairs,
const common::Pose3d &pose_curr2odom,
const common::Pose3d &pose_curr2map, double timestamp) {
std::shared_ptr<MapperVisFrame> frame(new MapperVisFrame);
frame->timestamp = timestamp;
frame->cloud_corn = kdtree_corn_.getInputCloud()->makeShared();
frame->cloud_surf = kdtree_surf_.getInputCloud()->makeShared();
frame->pl_pairs = pl_pairs;
frame->pp_pairs = pp_pairs;
frame->pose_curr2world
}
} // namespace oh_my_loam

17
oh_my_loam/mapper/mapper.h
View File

@ -10,7 +10,9 @@
#include "oh_my_loam/base/types.h"
#include "oh_my_loam/base/utils.h"
#include "oh_my_loam/mapper/map.h"
#include "oh_my_loam/solver/cost_function.h"
#include "oh_my_loam/solver/solver.h"
#include "oh_my_loam/visualizer/mapper_visualizer.h"
namespace oh_my_loam {
@ -40,13 +42,11 @@ class Mapper {
const TPointCloudConstPtr &cloud_surf,
const common::Pose3d &pose_init);
void MatchCorn(const pcl::KdTreeFLANN<TPoint> &kdtree,
const TPointCloudConstPtr &cloud_curr,
void MatchCorn(const TPointCloudConstPtr &cloud_curr,
const common::Pose3d &pose_curr2map,
std::vector<PointLineCoeffPair> *const pairs) const;
void MatchSurf(const pcl::KdTreeFLANN<TPoint> &kdtree,
const TPointCloudConstPtr &cloud_curr,
void MatchSurf(const TPointCloudConstPtr &cloud_curr,
const common::Pose3d &pose_curr2map,
std::vector<PointPlaneCoeffPair> *const pairs) const;
@ -60,10 +60,17 @@ class Mapper {
const TPointCloudConstPtr &cloud_corn = nullptr,
const TPointCloudConstPtr &cloud_surf = nullptr);
void Visualize(const std::vector<PointLineCoeffPair> &pl_pairs,
const std::vector<PointPlaneCoeffPair> &pp_pairs,
const common::Pose3d &pose_curr2odom,
const common::Pose3d &pose_curr2map, double timestamp = 0.0);
// map
mutable std::mutex map_mutex_;
std::unique_ptr<Map> corn_map_;
std::unique_ptr<Map> surf_map_;
pcl::KdTreeFLANN<TPoint> kdtree_corn_;
pcl::KdTreeFLANN<TPoint> kdtree_surf_;
// state
mutable std::mutex state_mutex_;
State state_ = UN_INIT;
@ -77,6 +84,8 @@ class Mapper {
bool is_vis_ = false;
bool verbose_ = false;
std::unique_ptr<MapperVisualizer> visualizer_{nullptr};
DISALLOW_COPY_AND_ASSIGN(Mapper)
};

8
oh_my_loam/odometer/odometer.cc
View File

@ -54,10 +54,10 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
}
PoseSolver solver(pose_curr2last_);
for (const auto &pair : pl_pairs) {
solver.AddPointLinePair(pair, GetTime(pair.pt));
solver.AddPointLinePair(pair, 1.0);
}
for (const auto &pair : pp_pairs) {
solver.AddPointPlanePair(pair, GetTime(pair.pt));
solver.AddPointPlanePair(pair, 1.0);
}
bool is_converge = solver.Solve(config_["solve_iter_num"].as<int>(),
verbose_, &pose_curr2last_);
@ -190,8 +190,8 @@ void Odometer::Visualize(const std::vector<PointLinePair> &pl_pairs,
double timestamp) const {
std::shared_ptr<OdometerVisFrame> frame(new OdometerVisFrame);
frame->timestamp = timestamp;
frame->cloud_corn = kdtree_corn_.getInputCloud();
frame->cloud_surf = kdtree_surf_.getInputCloud();
frame->cloud_corn = kdtree_corn_.getInputCloud()->makeShared();
frame->cloud_surf = kdtree_surf_.getInputCloud()->makeShared();
frame->pl_pairs = pl_pairs;
frame->pp_pairs = pp_pairs;
frame->pose_curr2last = pose_curr2last_;

116
oh_my_loam/visualizer/mapper_visualizer.cc Normal file
View File

@ -0,0 +1,116 @@
#include "mapper_visualizer.h"
#include "common/color/color.h"
namespace oh_my_loam {
namespace {
using namespace common;
} // namespace
void MapperVisualizer::Draw() {
auto frame = GetCurrentFrame<MapperVisFrame>();
DrawPointCloud<TPoint>(frame.cloud_corn, GRAY, "cloud_corn");
DrawPointCloud<TPoint>(frame.cloud_surf, GRAY, "cloud_surf");
DrawCorn(frame.pose_curr2odom, frame.pl_pairs);
DrawSurf(frame.pose_curr2odom, frame.pp_pairs);
poses_.push_back(frame.pose_curr2map);
DrawTrajectory();
}
void MapperVisualizer::DrawCorn(const Pose3d &pose_curr2odom,
const Pose3d &pose_curr2map,
const std::vector<PointLineCoeffPair> &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]);
}
DrawPointCloud<TPoint>(tgt, YELLOW, "tgt_corn", 4);
DrawPointCloud<TPoint>(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<TPoint>(p, p1, WHITE, "corn_line1_" + std::to_string(i),
viewer_.get());
common::AddLine<TPoint>(p, p2, WHITE, "corn_line2_" + std::to_string(i),
viewer_.get());
}
}
void MapperVisualizer::DrawSurf(const Pose3d &pose_curr2odom,
const Pose3d &pose_curr2map,
const std::vector<PointPlaneCoeffPair> &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<TPoint>(tgt, BLUE, "tgt_surf", 4);
DrawPointCloud<TPoint>(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<TPoint>(p, p1, WHITE, "surf_line1_" + std::to_string(i),
viewer_.get());
AddLine<TPoint>(p, p2, WHITE, "surf_line2_" + std::to_string(i),
viewer_.get());
AddLine<TPoint>(p, p3, WHITE, "surf_line3_" + std::to_string(i),
viewer_.get());
}
}
void MapperVisualizer::DrawTrajectory() {
std::vector<Pose3d> 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<float>();
Eigen::Vector3f p2 = poses_n[i + 1].t_vec().cast<float>();
AddLine<Point>({p1.x(), p1.y(), p1.z()}, {p2.x(), p2.y(), p2.z()}, PINK,
"trajectory" + std::to_string(i), viewer_.get());
}
}
void MapperVisualizer::KeyboardEventCallback(
const pcl::visualization::KeyboardEvent &event) {
if (event.getKeySym() == "p" && event.keyDown()) {
std::lock_guard<std::mutex> 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<std::mutex> 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() == "r" && event.keyDown()) {
viewer_->setCameraPosition(0, 0, 200, 0, 0, 0, 1, 0, 0, 0);
viewer_->setSize(2500, 1500);
trans_ = true;
is_updated_ = true;
}
}
} // namespace oh_my_loam

42
oh_my_loam/visualizer/mapper_visualizer.h Normal file
View File

@ -0,0 +1,42 @@
#pragma once
#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;
std::vector<PointLineCoeffPair> pl_pairs;
std::vector<PointPlaneCoeffPair> pp_pairs;
common::Pose3d pose_curr2odom;
common::Pose3d pose_curr2map;
};
class MapperVisualizer : public common::LidarVisualizer {
public:
explicit MapperVisualizer(const std::string &name = "MapperVisualizer",
size_t max_history_size = 10)
: common::LidarVisualizer(name, max_history_size) {}
private:
void Draw() override;
void DrawCorn(const common::Pose3d &pose,
const std::vector<PointLineCoeffPair> &pairs);
void DrawSurf(const common::Pose3d &pose,
const std::vector<PointPlaneCoeffPair> &pairs);
void DrawTrajectory();
void KeyboardEventCallback(
const pcl::visualization::KeyboardEvent &event) override;
bool trans_ = true;
std::vector<common::Pose3d> poses_;
};
} // namespace oh_my_loam