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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
oh_my_loam/configs/config.yaml
View File

@ -2,7 +2,7 @@
lidar: VPL16 lidar: VPL16
log_to_file: false log_to_file: false
log_path: /data/log/oh_my_loam log_path: /data/log/oh_my_loam
vis: false vis: true
# configs for extractor # configs for extractor
extractor_config: extractor_config:
@ -31,7 +31,7 @@ odometer_config:
# configs for mapper # configs for mapper
mapper_config: mapper_config:
vis: true vis: false
verbose: false verbose: false
map_shape: [3, 21, 21] map_shape: [3, 21, 21]
map_step: 50 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::Map(const std::vector<int> &shape, double step)
: Map(shape, {step, step, step}) {} : Map(shape, {step, step, step}) {}
void Map::Clear() { void Map::clear() {
for (auto &grid : map_) grid.Clear(); for (auto &grid : map_) grid.clear();
} }
TPointCloudPtr &Map::at(int z_idx, int y_idx, int x_idx) { 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) { for (int k = shape_[0] - 1; k >= -n; --k) {
std::swap(this->at(k), this->at(k + n)); 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 { } else {
for (int k = 0; k < shape_[0] - n; ++k) { for (int k = 0; k < shape_[0] - n; ++k) {
std::swap(this->at(k), this->at(k + n)); std::swap(this->at(k), this->at(k + n));
} }
for (int k = shape_[0] - n; k < shape_[0]; ++k) { for (int k = shape_[0] - n; k < shape_[0]; ++k) {
this->at(k).Clear(); this->at(k).clear();
} }
} }
center_[0] -= n; center_[0] -= n;
@ -64,13 +64,13 @@ void Map::ShiftY(int n) {
for (int j = shape_[1] - 1; j >= -n; --j) { for (int j = shape_[1] - 1; j >= -n; --j) {
std::swap(this->at(k, j), this->at(k, j + n)); 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 { } else {
for (int j = 0; j < shape_[1] - n; ++j) { for (int j = 0; j < shape_[1] - n; ++j) {
std::swap(this->at(k, j), this->at(k, j + n)); std::swap(this->at(k, j), this->at(k, j + n));
} }
for (int j = shape_[1] - n; j < shape_[1]; ++j) { 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; return index;
} }
bool Map::IsIndexValid(const Index &index) const { bool Map::CheckIndex(const Index &index) const {
int k = index.k + center_[0], j = index.j + center_[1], int k = index.k, j = index.j, i = index.i;
i = index.i + center_[2];
return k >= 0 && k < shape_[0] && j >= 0 && j < shape_[1] && i >= 0 && return k >= 0 && k < shape_[0] && j >= 0 && j < shape_[1] && i >= 0 &&
i < shape_[2]; i < shape_[2];
} }
TPointCloudPtr Map::GetSurrPoints(const TPoint &point, TPointCloudPtr Map::GetSubmapPoints(
const std::vector<int> &surr_shapes) const { const TPoint &point, const std::vector<int> &submap_shapes) const {
TPointCloudPtr cloud(new TPointCloud); TPointCloudPtr cloud(new TPointCloud);
for (const auto &index : GetSurrIndices(point, surr_shapes)) { for (const auto &index : GetSubmapIndices(point, submap_shapes)) {
*cloud += *this->at(index); *cloud += *this->at(index);
} }
return cloud; return cloud;
@ -135,7 +134,7 @@ void Map::AddPoints(const TPointCloudConstPtr &cloud,
std::set<Index, Index::Comp> index_set; std::set<Index, Index::Comp> index_set;
for (const auto &point : *cloud) { for (const auto &point : *cloud) {
Index index = GetIndex(point); Index index = GetIndex(point);
if (!IsIndexValid(index)) continue; if (!CheckIndex(index)) continue;
this->at(index)->push_back(point); this->at(index)->push_back(point);
if (indices) index_set.insert(index); 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) { void Map::Downsample(const std::vector<Index> &indices, double voxel_size) {
for (const auto &index : indices) { for (const auto &index : indices) {
if (!IsIndexValid(index)) continue; if (!CheckIndex(index)) continue;
TPointCloudPtr cloud_down_sampled(new TPointCloud); common::VoxelDownSample<TPoint>(this->at(index), this->at(index).get(),
common::VoxelDownSample<TPoint>(this->at(index), cloud_down_sampled.get(),
voxel_size); 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); return map_.at(z_idx).at(y_idx);
} }
std::vector<Index> Map::GetSurrIndices( std::vector<Index> Map::GetSubmapIndices(
const TPoint &point, const std::vector<int> &surr_shapes) const { const TPoint &point, const std::vector<int> &submap_shapes) const {
std::vector<Index> indices; std::vector<Index> indices;
Index index = GetIndex(point); 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) { for (int k = -nz; k <= nz; ++k) {
int idx_k = index.k + k; int idx_k = index.k + k;
if (idx_k < 0 || idx_k >= shape_[0]) continue; if (idx_k < 0 || idx_k >= shape_[0]) continue;
@ -221,7 +219,7 @@ const TPointCloudPtr &Row::at(int idx) const {
return row_.at(idx); return row_.at(idx);
} }
void Row::Clear() { void Row::clear() {
for (auto &cloud : row_) cloud->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); for (int i = 0; i < m; ++i) grid_.emplace_back(n);
} }
void Grid::Clear() { void Grid::clear() {
for (auto &row : grid_) row.Clear(); for (auto &row : grid_) row.clear();
} }
Row &Grid::at(int idx) { 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; const TPointCloudPtr &at(const Index &index) const;
void Clear(); void clear();
void ShiftZ(int n); void ShiftZ(int n);
@ -42,16 +42,16 @@ class Map {
void ShiftX(int n); void ShiftX(int n);
const std::vector<int> Shape() const { const std::vector<int> shape() const {
return std::vector<int>(shape_, shape_ + 3); return std::vector<int>(shape_, shape_ + 3);
} }
Index GetIndex(const TPoint &point) const; Index GetIndex(const TPoint &point) const;
bool IsIndexValid(const Index &index) const; bool CheckIndex(const Index &index) const;
TPointCloudPtr GetSurrPoints(const TPoint &point, TPointCloudPtr GetSubmapPoints(const TPoint &point,
const std::vector<int> &surr_shapes) const; const std::vector<int> &submap_shapes) const;
TPointCloudPtr GetAllPoints() const; TPointCloudPtr GetAllPoints() const;
@ -75,8 +75,8 @@ class Map {
const TPointCloudPtr &at(int z_idx, int y_idx, int x_idx) const; const TPointCloudPtr &at(int z_idx, int y_idx, int x_idx) const;
std::vector<Index> GetSurrIndices(const TPoint &point, std::vector<Index> GetSubmapIndices(
const std::vector<int> &surr_shapes) const; const TPoint &point, const std::vector<int> &submap_shapes) const;
std::vector<Index> GetAllIndices() const; std::vector<Index> GetAllIndices() const;
@ -97,7 +97,7 @@ class Row {
const TPointCloudPtr &at(int idx) const; const TPointCloudPtr &at(int idx) const;
void Clear(); void clear();
TPointCloudPtr GetAllPoints() const; TPointCloudPtr GetAllPoints() const;
@ -109,7 +109,7 @@ class Grid {
public: public:
Grid(int m, int n); Grid(int m, int n);
void Clear(); void clear();
Row &at(int idx); 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_); std::lock_guard<std::mutex> lock(state_mutex_);
*pose_curr2map = pose_odom2map_ * pose_curr2odom; *pose_curr2map = pose_odom2map_ * pose_curr2odom;
AINFO << "Pose_curr2map = " << pose_curr2map->ToString();
} }
void Mapper::Run(const TPointCloudConstPtr &cloud_corn, void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
@ -61,14 +62,15 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
BLOCK_TIMER_START; BLOCK_TIMER_START;
SetState(RUNNING); SetState(RUNNING);
common::Pose3d pose_curr2map = pose_odom2map_ * pose_curr2odom; common::Pose3d pose_curr2map = pose_odom2map_ * pose_curr2odom;
TPoint cnt(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(), TPoint t_vec(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(),
pose_curr2map.t_vec().z()); pose_curr2map.t_vec().z());
// AdjustMap(cnt); AdjustMap(t_vec);
// TPointCloudPtr cloud_corn_map = corn_map_->GetSurrPoints(cnt, TPointCloudPtr cloud_corn_map =
// submap_shape_); TPointCloudPtr cloud_surf_map = corn_map_->GetSubmapPoints(t_vec, submap_shape_);
// surf_map_->GetSurrPoints(cnt, submap_shape_); TPointCloudPtr cloud_surf_map =
kdtree_corn_.setInputCloud(cloud_corn_map_); surf_map_->GetSubmapPoints(t_vec, submap_shape_);
kdtree_surf_.setInputCloud(cloud_surf_map_); kdtree_corn_.setInputCloud(cloud_corn_map);
kdtree_surf_.setInputCloud(cloud_surf_map);
std::vector<PointLineCoeffPair> pl_pairs; std::vector<PointLineCoeffPair> pl_pairs;
std::vector<PointPlaneCoeffPair> pp_pairs; std::vector<PointPlaneCoeffPair> pp_pairs;
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) { 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); UpdateMap(pose_curr2map, cloud_corn, cloud_surf);
std::lock_guard<std::mutex> lock(state_mutex_); std::lock_guard<std::mutex> lock(state_mutex_);
pose_odom2map_ = pose_curr2map * pose_curr2odom.Inv(); pose_odom2map_ = pose_curr2map * pose_curr2odom.Inv();
AINFO << "Pose_curr2map = " << pose_curr2map.ToString();
AUSER << "Mapper::Run: " << BLOCK_TIMER_STOP_FMT; AUSER << "Mapper::Run: " << BLOCK_TIMER_STOP_FMT;
if (is_vis_) Visualize(pl_pairs, pp_pairs, pose_curr2odom, pose_curr2map); if (is_vis_) Visualize(pl_pairs, pp_pairs, pose_curr2odom, pose_curr2map);
state_ = DONE; // be careful with deadlock 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_corn,
const TPointCloudConstPtr &cloud_surf) { const TPointCloudConstPtr &cloud_surf) {
auto update_map = [&](const TPointCloudConstPtr &cloud, 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); TPointCloudPtr cloud_trans(new TPointCloud);
common::TransformPointCloud(pose_curr2map, *cloud, cloud_trans.get()); common::TransformPointCloud(pose_curr2map, *cloud, cloud_trans.get());
*cloud_map += *cloud_trans; std::vector<Index> indices;
// std::vector<Index> indices; map->AddPoints(cloud_trans, &indices);
// map->AddPoints(cloud_trans, &indices); map->Downsample(indices, config_["downsample_voxel_size"].as<double>());
// map->Downsample(indices, config_["downsample_voxel_size"].as<double>());
common::VoxelDownSample(cloud_map, cloud_map.get(),
config_["downsample_voxel_size"].as<double>());
}; };
std::lock_guard<std::mutex> lock(map_mutex_); std::lock_guard<std::mutex> lock(map_mutex_);
update_map(cloud_corn, cloud_corn_map_); update_map(cloud_corn, corn_map_.get());
update_map(cloud_surf, cloud_surf_map_); update_map(cloud_surf, surf_map_.get());
} }
TPointCloudPtr Mapper::GetMapCloudCorn() const { TPointCloudPtr Mapper::GetMapCloudCorn() const {
@ -245,8 +245,6 @@ void Mapper::Visualize(const std::vector<PointLineCoeffPair> &pl_pairs,
frame->timestamp = timestamp; frame->timestamp = timestamp;
frame->cloud_corn = kdtree_corn_.getInputCloud()->makeShared(); frame->cloud_corn = kdtree_corn_.getInputCloud()->makeShared();
frame->cloud_surf = kdtree_surf_.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->pl_pairs = pl_pairs;
frame->pp_pairs = pp_pairs; frame->pp_pairs = pp_pairs;
frame->pose_curr2odom = pose_curr2odom; frame->pose_curr2odom = pose_curr2odom;

4
oh_my_loam/mapper/mapper.h
View File

@ -83,10 +83,8 @@ class Mapper {
double map_step_; double map_step_;
bool is_vis_ = false; bool is_vis_ = false;
bool verbose_ = false; bool verbose_ = false;
// visualizer
std::unique_ptr<MapperVisualizer> visualizer_{nullptr}; std::unique_ptr<MapperVisualizer> visualizer_{nullptr};
TPointCloudPtr cloud_corn_map_{new TPointCloud};
TPointCloudPtr cloud_surf_map_{new TPointCloud};
DISALLOW_COPY_AND_ASSIGN(Mapper) 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(); const auto &cloud_surf = odometer_->GetCloudSurf()->makeShared();
mapper_->Process(timestamp, cloud_corn, cloud_surf, pose_curr2odom, mapper_->Process(timestamp, cloud_corn, cloud_surf, pose_curr2odom,
&pose_curr2map); &pose_curr2map);
poses_curr2odom_.push_back(pose_curr2odom);
poses_curr2map_.push_back(pose_curr2map);
if (is_vis_) { 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, void OhMyLoam::Visualize(const common::Pose3d &pose_curr2map,
const common::Pose3d &pose_curr2map, const TPointCloudPtr &cloud_corn,
const TPointCloudConstPtr &cloud_corn, const TPointCloudPtr &cloud_surf, double timestamp) {
const TPointCloudConstPtr &cloud_surf,
double timestamp) {
std::shared_ptr<OhmyloamVisFrame> frame(new OhmyloamVisFrame); std::shared_ptr<OhmyloamVisFrame> frame(new OhmyloamVisFrame);
frame->timestamp = timestamp; frame->timestamp = timestamp;
frame->cloud_map_corn = mapper_->GetMapCloudCorn(); frame->cloud_map_corn = mapper_->GetMapCloudCorn();
frame->cloud_map_surf = mapper_->GetMapCloudSurf(); frame->cloud_map_surf = mapper_->GetMapCloudSurf();
frame->cloud_corn = cloud_corn; frame->cloud_corn = cloud_corn->makeShared();
frame->cloud_surf = cloud_surf; frame->cloud_surf = cloud_surf->makeShared();
frame->pose_odom = pose_curr2odom;
frame->pose_map = pose_curr2map; frame->pose_map = pose_curr2map;
visualizer_->Render(frame); visualizer_->Render(frame);
} }

18
oh_my_loam/oh_my_loam.h
View File

@ -21,10 +21,13 @@ class OhMyLoam {
private: private:
void Reset(); void Reset();
void Visualize(const common::Pose3d &pose_curr2odom, void Visualize(const common::Pose3d &pose_curr2map,
const common::Pose3d &pose_curr2map, const TPointCloudPtr &cloud_corn,
const TPointCloudConstPtr &cloud_corn, const TPointCloudPtr &cloud_surf, double timestamp = 0.0);
const TPointCloudConstPtr &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}; std::unique_ptr<Extractor> extractor_{nullptr};
@ -32,13 +35,6 @@ class OhMyLoam {
std::unique_ptr<Mapper> mapper_{nullptr}; 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_; YAML::Node config_;
bool is_vis_ = false; 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_odom_.AddPose(frame.pose_curr2odom);
traj_map_.AddPose(frame.pose_curr2map); 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, 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); 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( void MapperVisualizer::KeyboardEventCallback(
const pcl::visualization::KeyboardEvent &event) { const pcl::visualization::KeyboardEvent &event) {
if (event.getKeySym() == "p" && event.keyDown()) { 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, void DrawSurf(const common::Pose3d &pose_odom, const common::Pose3d &pose_map,
const std::vector<PointPlaneCoeffPair> &pairs); const std::vector<PointPlaneCoeffPair> &pairs);
void DrawTrajectory();
void KeyboardEventCallback( void KeyboardEventCallback(
const pcl::visualization::KeyboardEvent &event) override; 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); DrawCorn(frame.pose_curr2last, frame.pl_pairs);
DrawSurf(frame.pose_curr2last, frame.pp_pairs); DrawSurf(frame.pose_curr2last, frame.pp_pairs);
poses_.push_back(frame.pose_curr2world); traj_.AddPose(frame.pose_curr2world);
DrawTrajectory(); AddTrajectory(traj_.Copy(true), PINK, "trajectory", viewer_.get());
} }
void OdometerVisualizer::DrawCorn(const Pose3d &pose, 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>(tgt, YELLOW, "tgt_corn", 4);
DrawPointCloud<TPoint>(src, RED, "src_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, 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>(tgt, BLUE, "tgt_surf", 4);
DrawPointCloud<TPoint>(src, CYAN, "src_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( void OdometerVisualizer::KeyboardEventCallback(
@ -107,17 +73,10 @@ void OdometerVisualizer::KeyboardEventCallback(
} else if (event.getKeySym() == "t" && event.keyDown()) { } else if (event.getKeySym() == "t" && event.keyDown()) {
trans_ = !trans_; trans_ = !trans_;
is_updated_ = true; 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()) { } else if (event.getKeySym() == "r" && event.keyDown()) {
viewer_->setCameraPosition(0, 0, 200, 0, 0, 0, 1, 0, 0, 0); viewer_->setCameraPosition(0, 0, 200, 0, 0, 0, 1, 0, 0, 0);
viewer_->setSize(2500, 1500); viewer_->setSize(2500, 1500);
trans_ = true; trans_ = true;
corn_connect_ = surf_connect_ = false;
is_updated_ = true; is_updated_ = true;
} }
} }

7
oh_my_loam/visualizer/odometer_visualizer.h
View File

@ -1,5 +1,6 @@
#pragma once #pragma once
#include "common/geometry/trajectory.h"
#include "common/visualizer/lidar_visualizer.h" #include "common/visualizer/lidar_visualizer.h"
#include "oh_my_loam/solver/cost_function.h" #include "oh_my_loam/solver/cost_function.h"
@ -29,16 +30,12 @@ class OdometerVisualizer : public common::LidarVisualizer {
void DrawSurf(const common::Pose3d &pose, void DrawSurf(const common::Pose3d &pose,
const std::vector<PointPlanePair> &pairs); const std::vector<PointPlanePair> &pairs);
void DrawTrajectory();
void KeyboardEventCallback( void KeyboardEventCallback(
const pcl::visualization::KeyboardEvent &event) override; const pcl::visualization::KeyboardEvent &event) override;
bool trans_ = true; bool trans_ = true;
bool corn_connect_ = false;
bool surf_connect_ = false;
std::vector<common::Pose3d> poses_; common::Trajectory traj_;
}; };
} // namespace oh_my_loam } // 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>(); auto frame = GetCurrentFrame<OhmyloamVisFrame>();
DrawPointCloud<TPoint>(frame.cloud_map_corn, GRAY, "cloud_map_corn"); DrawPointCloud<TPoint>(frame.cloud_map_corn, GRAY, "cloud_map_corn");
DrawPointCloud<TPoint>(frame.cloud_map_surf, GRAY, "cloud_map_surf"); DrawPointCloud<TPoint>(frame.cloud_map_surf, GRAY, "cloud_map_surf");
if (!trans_) { TransformPointCloud(frame.pose_map, *frame.cloud_corn,
DrawPointCloud<TPoint>(frame.cloud_corn, RED, "cloud_corn"); frame.cloud_corn.get());
DrawPointCloud<TPoint>(frame.cloud_surf, CYAN, "cloud_surf"); DrawPointCloud<TPoint>(frame.cloud_corn, "time", "cloud_corn");
} else { TransformPointCloud(frame.pose_map, *frame.cloud_surf,
TPointCloudPtr cloud_corn_trans(new TPointCloud); frame.cloud_surf.get());
TPointCloudPtr cloud_surf_trans(new TPointCloud); DrawPointCloud<TPoint>(frame.cloud_surf, "time", "cloud_surf");
common::TransformPointCloud(frame.pose_map, *frame.cloud_corn, traj_.AddPose(frame.pose_map);
cloud_corn_trans.get()); AddTrajectory(traj_, PINK, "trajectory", viewer_.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;
}
} }
} // namespace oh_my_loam } // namespace oh_my_loam

17
oh_my_loam/visualizer/ohmyloam_visualizer.h
View File

@ -1,6 +1,6 @@
#pragma once #pragma once
#include "common/geometry/pose3d.h" #include "common/geometry/trajectory.h"
#include "common/visualizer/lidar_visualizer.h" #include "common/visualizer/lidar_visualizer.h"
#include "oh_my_loam/base/types.h" #include "oh_my_loam/base/types.h"
@ -9,9 +9,8 @@ namespace oh_my_loam {
struct OhmyloamVisFrame : public common::LidarVisFrame { struct OhmyloamVisFrame : public common::LidarVisFrame {
TPointCloudConstPtr cloud_map_corn; TPointCloudConstPtr cloud_map_corn;
TPointCloudConstPtr cloud_map_surf; TPointCloudConstPtr cloud_map_surf;
TPointCloudConstPtr cloud_corn; // current TPointCloudPtr cloud_corn; // current
TPointCloudConstPtr cloud_surf; // current TPointCloudPtr cloud_surf; // current
common::Pose3d pose_odom;
common::Pose3d pose_map; common::Pose3d pose_map;
}; };
@ -24,15 +23,7 @@ class OhmyloamVisualizer : public common::LidarVisualizer {
private: private:
void Draw() override; void Draw() override;
void DrawTrajectory(const std::vector<common::Pose3d> &poses, common::Trajectory traj_;
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;
}; };
} // namespace oh_my_loam } // namespace oh_my_loam