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

it works!!!

main
feixyz10 2021-02-08 20:16:45 +08:00 committed by feixyz
parent f2993b40b8
commit e371c2499f
13 changed files with 81 additions and 201 deletions

4
oh_my_loam/configs/config.yaml
View File

@ -2,7 +2,7 @@
lidar: VPL16
log_to_file: false
log_path: /data/log/oh_my_loam
vis: false
vis: true
# configs for extractor
extractor_config:
@ -31,7 +31,7 @@ odometer_config:
# configs for mapper
mapper_config:
vis: true
vis: false
verbose: false
map_shape: [3, 21, 21]
map_step: 50

44
oh_my_loam/mapper/map.cc
View File

@ -19,8 +19,8 @@ Map::Map(const std::vector<int> &shape, const std::vector<double> &step) {
Map::Map(const std::vector<int> &shape, double step)
: Map(shape, {step, step, step}) {}
void Map::Clear() {
for (auto &grid : map_) grid.Clear();
void Map::clear() {
for (auto &grid : map_) grid.clear();
}
TPointCloudPtr &Map::at(int z_idx, int y_idx, int x_idx) {
@ -45,13 +45,13 @@ void Map::ShiftZ(int n) {
for (int k = shape_[0] - 1; k >= -n; --k) {
std::swap(this->at(k), this->at(k + n));
}
for (int k = 0; k < -n; ++k) this->at(k).Clear();
for (int k = 0; k < -n; ++k) this->at(k).clear();
} else {
for (int k = 0; k < shape_[0] - n; ++k) {
std::swap(this->at(k), this->at(k + n));
}
for (int k = shape_[0] - n; k < shape_[0]; ++k) {
this->at(k).Clear();
this->at(k).clear();
}
}
center_[0] -= n;
@ -64,13 +64,13 @@ void Map::ShiftY(int n) {
for (int j = shape_[1] - 1; j >= -n; --j) {
std::swap(this->at(k, j), this->at(k, j + n));
}
for (int j = 0; j < -n; ++j) this->at(k, j).Clear();
for (int j = 0; j < -n; ++j) this->at(k, j).clear();
} else {
for (int j = 0; j < shape_[1] - n; ++j) {
std::swap(this->at(k, j), this->at(k, j + n));
}
for (int j = shape_[1] - n; j < shape_[1]; ++j) {
this->at(k, j).Clear();
this->at(k, j).clear();
}
}
}
@ -108,17 +108,16 @@ Index Map::GetIndex(const TPoint &point) const {
return index;
}
bool Map::IsIndexValid(const Index &index) const {
int k = index.k + center_[0], j = index.j + center_[1],
i = index.i + center_[2];
bool Map::CheckIndex(const Index &index) const {
int k = index.k, j = index.j, i = index.i;
return k >= 0 && k < shape_[0] && j >= 0 && j < shape_[1] && i >= 0 &&
i < shape_[2];
}
TPointCloudPtr Map::GetSurrPoints(const TPoint &point,
const std::vector<int> &surr_shapes) const {
TPointCloudPtr Map::GetSubmapPoints(
const TPoint &point, const std::vector<int> &submap_shapes) const {
TPointCloudPtr cloud(new TPointCloud);
for (const auto &index : GetSurrIndices(point, surr_shapes)) {
for (const auto &index : GetSubmapIndices(point, submap_shapes)) {
*cloud += *this->at(index);
}
return cloud;
@ -135,7 +134,7 @@ void Map::AddPoints(const TPointCloudConstPtr &cloud,
std::set<Index, Index::Comp> index_set;
for (const auto &point : *cloud) {
Index index = GetIndex(point);
if (!IsIndexValid(index)) continue;
if (!CheckIndex(index)) continue;
this->at(index)->push_back(point);
if (indices) index_set.insert(index);
}
@ -151,11 +150,9 @@ void Map::Downsample(double voxel_size) {
void Map::Downsample(const std::vector<Index> &indices, double voxel_size) {
for (const auto &index : indices) {
if (!IsIndexValid(index)) continue;
TPointCloudPtr cloud_down_sampled(new TPointCloud);
common::VoxelDownSample<TPoint>(this->at(index), cloud_down_sampled.get(),
if (!CheckIndex(index)) continue;
common::VoxelDownSample<TPoint>(this->at(index), this->at(index).get(),
voxel_size);
this->at(index) = cloud_down_sampled;
}
}
@ -175,11 +172,12 @@ const Row &Map::at(int z_idx, int y_idx) const {
return map_.at(z_idx).at(y_idx);
}
std::vector<Index> Map::GetSurrIndices(
const TPoint &point, const std::vector<int> &surr_shapes) const {
std::vector<Index> Map::GetSubmapIndices(
const TPoint &point, const std::vector<int> &submap_shapes) const {
std::vector<Index> indices;
Index index = GetIndex(point);
int nz = surr_shapes[0] / 2, ny = surr_shapes[1] / 2, nx = surr_shapes[2] / 2;
int nz = submap_shapes[0] / 2, ny = submap_shapes[1] / 2,
nx = submap_shapes[2] / 2;
for (int k = -nz; k <= nz; ++k) {
int idx_k = index.k + k;
if (idx_k < 0 || idx_k >= shape_[0]) continue;
@ -221,7 +219,7 @@ const TPointCloudPtr &Row::at(int idx) const {
return row_.at(idx);
}
void Row::Clear() {
void Row::clear() {
for (auto &cloud : row_) cloud->clear();
}
@ -235,8 +233,8 @@ Grid::Grid(int m, int n) {
for (int i = 0; i < m; ++i) grid_.emplace_back(n);
}
void Grid::Clear() {
for (auto &row : grid_) row.Clear();
void Grid::clear() {
for (auto &row : grid_) row.clear();
}
Row &Grid::at(int idx) {

18
oh_my_loam/mapper/map.h
View File

@ -34,7 +34,7 @@ class Map {
const TPointCloudPtr &at(const Index &index) const;
void Clear();
void clear();
void ShiftZ(int n);
@ -42,16 +42,16 @@ class Map {
void ShiftX(int n);
const std::vector<int> Shape() const {
const std::vector<int> shape() const {
return std::vector<int>(shape_, shape_ + 3);
}
Index GetIndex(const TPoint &point) const;
bool IsIndexValid(const Index &index) const;
bool CheckIndex(const Index &index) const;
TPointCloudPtr GetSurrPoints(const TPoint &point,
const std::vector<int> &surr_shapes) const;
TPointCloudPtr GetSubmapPoints(const TPoint &point,
const std::vector<int> &submap_shapes) const;
TPointCloudPtr GetAllPoints() const;
@ -75,8 +75,8 @@ class Map {
const TPointCloudPtr &at(int z_idx, int y_idx, int x_idx) const;
std::vector<Index> GetSurrIndices(const TPoint &point,
const std::vector<int> &surr_shapes) const;
std::vector<Index> GetSubmapIndices(
const TPoint &point, const std::vector<int> &submap_shapes) const;
std::vector<Index> GetAllIndices() const;
@ -97,7 +97,7 @@ class Row {
const TPointCloudPtr &at(int idx) const;
void Clear();
void clear();
TPointCloudPtr GetAllPoints() const;
@ -109,7 +109,7 @@ class Grid {
public:
Grid(int m, int n);
void Clear();
void clear();
Row &at(int idx);

36
oh_my_loam/mapper/mapper.cc
View File

@ -53,6 +53,7 @@ void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
}
std::lock_guard<std::mutex> lock(state_mutex_);
*pose_curr2map = pose_odom2map_ * pose_curr2odom;
AINFO << "Pose_curr2map = " << pose_curr2map->ToString();
}
void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
@ -61,14 +62,15 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
BLOCK_TIMER_START;
SetState(RUNNING);
common::Pose3d pose_curr2map = pose_odom2map_ * pose_curr2odom;
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_);
TPoint t_vec(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(),
pose_curr2map.t_vec().z());
AdjustMap(t_vec);
TPointCloudPtr cloud_corn_map =
corn_map_->GetSubmapPoints(t_vec, submap_shape_);
TPointCloudPtr cloud_surf_map =
surf_map_->GetSubmapPoints(t_vec, submap_shape_);
kdtree_corn_.setInputCloud(cloud_corn_map);
kdtree_surf_.setInputCloud(cloud_surf_map);
std::vector<PointLineCoeffPair> pl_pairs;
std::vector<PointPlaneCoeffPair> pp_pairs;
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
@ -102,6 +104,7 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
UpdateMap(pose_curr2map, cloud_corn, cloud_surf);
std::lock_guard<std::mutex> lock(state_mutex_);
pose_odom2map_ = pose_curr2map * pose_curr2odom.Inv();
AINFO << "Pose_curr2map = " << pose_curr2map.ToString();
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
@ -194,19 +197,16 @@ 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*/ const TPointCloudPtr &cloud_map) {
Map *const map /* const TPointCloudPtr &cloud_map*/) {
TPointCloudPtr cloud_trans(new TPointCloud);
common::TransformPointCloud(pose_curr2map, *cloud, cloud_trans.get());
*cloud_map += *cloud_trans;
// std::vector<Index> indices;
// map->AddPoints(cloud_trans, &indices);
// map->Downsample(indices, config_["downsample_voxel_size"].as<double>());
common::VoxelDownSample(cloud_map, cloud_map.get(),
config_["downsample_voxel_size"].as<double>());
std::vector<Index> indices;
map->AddPoints(cloud_trans, &indices);
map->Downsample(indices, config_["downsample_voxel_size"].as<double>());
};
std::lock_guard<std::mutex> lock(map_mutex_);
update_map(cloud_corn, cloud_corn_map_);
update_map(cloud_surf, cloud_surf_map_);
update_map(cloud_corn, corn_map_.get());
update_map(cloud_surf, surf_map_.get());
}
TPointCloudPtr Mapper::GetMapCloudCorn() const {
@ -245,8 +245,6 @@ void Mapper::Visualize(const std::vector<PointLineCoeffPair> &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_curr2odom = pose_curr2odom;

4
oh_my_loam/mapper/mapper.h
View File

@ -83,10 +83,8 @@ class Mapper {
double map_step_;
bool is_vis_ = false;
bool verbose_ = false;
// visualizer
std::unique_ptr<MapperVisualizer> visualizer_{nullptr};
TPointCloudPtr cloud_corn_map_{new TPointCloud};
TPointCloudPtr cloud_surf_map_{new TPointCloud};
DISALLOW_COPY_AND_ASSIGN(Mapper)
};

17
oh_my_loam/oh_my_loam.cc
View File

@ -54,25 +54,20 @@ void OhMyLoam::Run(double timestamp,
const auto &cloud_surf = odometer_->GetCloudSurf()->makeShared();
mapper_->Process(timestamp, cloud_corn, cloud_surf, pose_curr2odom,
&pose_curr2map);
poses_curr2odom_.push_back(pose_curr2odom);
poses_curr2map_.push_back(pose_curr2map);
if (is_vis_) {
Visualize(pose_curr2odom, pose_curr2map, cloud_corn, cloud_surf, timestamp);
Visualize(pose_curr2map, cloud_corn, cloud_surf, timestamp);
}
}
void OhMyLoam::Visualize(const common::Pose3d &pose_curr2odom,
const common::Pose3d &pose_curr2map,
const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf,
double timestamp) {
void OhMyLoam::Visualize(const common::Pose3d &pose_curr2map,
const TPointCloudPtr &cloud_corn,
const TPointCloudPtr &cloud_surf, double timestamp) {
std::shared_ptr<OhmyloamVisFrame> frame(new OhmyloamVisFrame);
frame->timestamp = timestamp;
frame->cloud_map_corn = mapper_->GetMapCloudCorn();
frame->cloud_map_surf = mapper_->GetMapCloudSurf();
frame->cloud_corn = cloud_corn;
frame->cloud_surf = cloud_surf;
frame->pose_odom = pose_curr2odom;
frame->cloud_corn = cloud_corn->makeShared();
frame->cloud_surf = cloud_surf->makeShared();
frame->pose_map = pose_curr2map;
visualizer_->Render(frame);
}

18
oh_my_loam/oh_my_loam.h
View File

@ -21,10 +21,13 @@ class OhMyLoam {
private:
void Reset();
void Visualize(const common::Pose3d &pose_curr2odom,
const common::Pose3d &pose_curr2map,
const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf, double timestamp = 0.0);
void Visualize(const common::Pose3d &pose_curr2map,
const TPointCloudPtr &cloud_corn,
const TPointCloudPtr &cloud_surf, double timestamp = 0.0);
// remove outliers: nan or very close points
void RemoveOutliers(const common::PointCloud &cloud_in,
common::PointCloud *const cloud_out) const;
std::unique_ptr<Extractor> extractor_{nullptr};
@ -32,13 +35,6 @@ class OhMyLoam {
std::unique_ptr<Mapper> mapper_{nullptr};
// remove outliers: nan or very close points
void RemoveOutliers(const common::PointCloud &cloud_in,
common::PointCloud *const cloud_out) const;
std::vector<common::Pose3d> poses_curr2odom_;
std::vector<common::Pose3d> poses_curr2map_;
YAML::Node config_;
bool is_vis_ = false;

8
oh_my_loam/visualizer/mapper_visualizer.cc
View File

@ -18,7 +18,8 @@ void MapperVisualizer::Draw() {
traj_odom_.AddPose(frame.pose_curr2odom);
traj_map_.AddPose(frame.pose_curr2map);
DrawTrajectory();
AddTrajectory(traj_odom_, PINK, "traj_odmo", viewer_.get());
AddTrajectory(traj_map_, PURPLE, "traj_map", viewer_.get());
}
void MapperVisualizer::DrawCorn(const common::Pose3d &pose_odom,
@ -47,11 +48,6 @@ void MapperVisualizer::DrawSurf(const common::Pose3d &pose_odom,
DrawPointCloud<TPoint>(src, CYAN, "src_surf", 4);
}
void MapperVisualizer::DrawTrajectory() {
common::AddTrajectory(traj_odom_, PINK, "traj_odmo", viewer_.get());
common::AddTrajectory(traj_map_, PURPLE, "traj_map", viewer_.get());
}
void MapperVisualizer::KeyboardEventCallback(
const pcl::visualization::KeyboardEvent &event) {
if (event.getKeySym() == "p" && event.keyDown()) {

2
oh_my_loam/visualizer/mapper_visualizer.h
View File

@ -30,8 +30,6 @@ class MapperVisualizer : public common::LidarVisualizer {
void DrawSurf(const common::Pose3d &pose_odom, const common::Pose3d &pose_map,
const std::vector<PointPlaneCoeffPair> &pairs);
void DrawTrajectory();
void KeyboardEventCallback(
const pcl::visualization::KeyboardEvent &event) override;

45
oh_my_loam/visualizer/odometer_visualizer.cc
View File

@ -16,8 +16,8 @@ void OdometerVisualizer::Draw() {
DrawCorn(frame.pose_curr2last, frame.pl_pairs);
DrawSurf(frame.pose_curr2last, frame.pp_pairs);
poses_.push_back(frame.pose_curr2world);
DrawTrajectory();
traj_.AddPose(frame.pose_curr2world);
AddTrajectory(traj_.Copy(true), PINK, "trajectory", viewer_.get());
}
void OdometerVisualizer::DrawCorn(const Pose3d &pose,
@ -35,14 +35,6 @@ void OdometerVisualizer::DrawCorn(const Pose3d &pose,
}
DrawPointCloud<TPoint>(tgt, YELLOW, "tgt_corn", 4);
DrawPointCloud<TPoint>(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<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 OdometerVisualizer::DrawSurf(const Pose3d &pose,
@ -61,32 +53,6 @@ void OdometerVisualizer::DrawSurf(const Pose3d &pose,
}
DrawPointCloud<TPoint>(tgt, BLUE, "tgt_surf", 4);
DrawPointCloud<TPoint>(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<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 OdometerVisualizer::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 OdometerVisualizer::KeyboardEventCallback(
@ -107,17 +73,10 @@ void OdometerVisualizer::KeyboardEventCallback(
} 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;
}
}

7
oh_my_loam/visualizer/odometer_visualizer.h
View File

@ -1,5 +1,6 @@
#pragma once
#include "common/geometry/trajectory.h"
#include "common/visualizer/lidar_visualizer.h"
#include "oh_my_loam/solver/cost_function.h"
@ -29,16 +30,12 @@ class OdometerVisualizer : public common::LidarVisualizer {
void DrawSurf(const common::Pose3d &pose,
const std::vector<PointPlanePair> &pairs);
void DrawTrajectory();
void KeyboardEventCallback(
const pcl::visualization::KeyboardEvent &event) override;
bool trans_ = true;
bool corn_connect_ = false;
bool surf_connect_ = false;
std::vector<common::Pose3d> poses_;
common::Trajectory traj_;
};
} // namespace oh_my_loam

62
oh_my_loam/visualizer/ohmyloam_visualizer.cc
View File

@ -15,60 +15,14 @@ void OhmyloamVisualizer::Draw() {
auto frame = GetCurrentFrame<OhmyloamVisFrame>();
DrawPointCloud<TPoint>(frame.cloud_map_corn, GRAY, "cloud_map_corn");
DrawPointCloud<TPoint>(frame.cloud_map_surf, GRAY, "cloud_map_surf");
if (!trans_) {
DrawPointCloud<TPoint>(frame.cloud_corn, RED, "cloud_corn");
DrawPointCloud<TPoint>(frame.cloud_surf, CYAN, "cloud_surf");
} else {
TPointCloudPtr cloud_corn_trans(new TPointCloud);
TPointCloudPtr cloud_surf_trans(new TPointCloud);
common::TransformPointCloud(frame.pose_map, *frame.cloud_corn,
cloud_corn_trans.get());
common::TransformPointCloud(frame.pose_map, *frame.cloud_surf,
cloud_surf_trans.get());
DrawPointCloud<TPoint>(cloud_corn_trans, RED, "cloud_corn");
DrawPointCloud<TPoint>(cloud_surf_trans, CYAN, "cloud_surf");
}
// poses_odom_.push_back(frame.pose_odom);
// DrawTrajectory(poses_odom_, "odom_traj", PINK);
poses_map_.push_back(frame.pose_map);
DrawTrajectory(poses_map_, "map_traj", WHITE);
}
void OhmyloamVisualizer::DrawTrajectory(
const std::vector<common::Pose3d> &poses, const std::string &id,
const common::Color &color) {
for (size_t i = 0; i < poses.size() - 1; ++i) {
Eigen::Vector3f p1 = poses[i].t_vec().cast<float>();
Eigen::Vector3f p2 = poses[i + 1].t_vec().cast<float>();
AddLine<Point>({p1.x(), p1.y(), p1.z()}, {p2.x(), p2.y(), p2.z()}, color,
id + std::to_string(i), viewer_.get());
}
}
void OhmyloamVisualizer::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;
}
TransformPointCloud(frame.pose_map, *frame.cloud_corn,
frame.cloud_corn.get());
DrawPointCloud<TPoint>(frame.cloud_corn, "time", "cloud_corn");
TransformPointCloud(frame.pose_map, *frame.cloud_surf,
frame.cloud_surf.get());
DrawPointCloud<TPoint>(frame.cloud_surf, "time", "cloud_surf");
traj_.AddPose(frame.pose_map);
AddTrajectory(traj_, PINK, "trajectory", viewer_.get());
}
} // namespace oh_my_loam

17
oh_my_loam/visualizer/ohmyloam_visualizer.h
View File

@ -1,6 +1,6 @@
#pragma once
#include "common/geometry/pose3d.h"
#include "common/geometry/trajectory.h"
#include "common/visualizer/lidar_visualizer.h"
#include "oh_my_loam/base/types.h"
@ -9,9 +9,8 @@ namespace oh_my_loam {
struct OhmyloamVisFrame : public common::LidarVisFrame {
TPointCloudConstPtr cloud_map_corn;
TPointCloudConstPtr cloud_map_surf;
TPointCloudConstPtr cloud_corn; // current
TPointCloudConstPtr cloud_surf; // current
common::Pose3d pose_odom;
TPointCloudPtr cloud_corn; // current
TPointCloudPtr cloud_surf; // current
common::Pose3d pose_map;
};
@ -24,15 +23,7 @@ class OhmyloamVisualizer : public common::LidarVisualizer {
private:
void Draw() override;
void DrawTrajectory(const std::vector<common::Pose3d> &poses,
const std::string &id, const common::Color &color);
void KeyboardEventCallback(
const pcl::visualization::KeyboardEvent &event) override;
std::vector<common::Pose3d> poses_odom_;
std::vector<common::Pose3d> poses_map_;
bool trans_ = true;
common::Trajectory traj_;
};
} // namespace oh_my_loam