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

odometry visualizer: ok

main
feixyz10 2020-11-03 15:10:16 +08:00 committed by feixyz
parent 2a052c462e
commit 33a74e5f65
8 changed files with 99 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -57,9 +57,9 @@ target_link_libraries(main
common
oh_my_loam
extractor
visualizer
odometry
solver
${CERES_LIBRARIES}
helper
visualizer
)

4
common/log.h
View File

@ -6,8 +6,8 @@
#include <g3log/logworker.hpp>
#include <iostream>
// heere we define G3LOG instead of use LOG directly, since LOG macro in g3log
// is conflict with LOG macro in glog. Same for G3LOG_IF and G3CHECK
// here we define G3LOG instead of use LOG directly, since LOG macro in g3log
// conflicts LOG macro in glog. Same for G3LOG_IF and G3CHECK
#define G3LOG(level) \
if (!g3::logLevel(level)) { \
} else \

4
configs/config.yaml
View File

@ -7,7 +7,7 @@ vis: true
# configs for feature points extractor
extractor_config:
min_point_num: 66
vis: true
vis: false
sharp_corner_point_num: 2
corner_point_num: 20
flat_surf_point_num: 4
@ -20,4 +20,4 @@ extractor_config:
odometry_config:
icp_iter_num : 2
match_dist_sq_thresh: 1
vis: false
vis: true

23
src/odometry/odometry.cc
View File

@ -13,12 +13,15 @@ bool Odometry::Init(const YAML::Node& config) {
config_ = config;
kdtree_surf_pts_.reset(new pcl::KdTreeFLANN<TPoint>);
kdtree_corn_pts_.reset(new pcl::KdTreeFLANN<TPoint>);
is_vis_ = Config::Instance()->Get<bool>("vis") && config_["vis"].as<bool>();
AINFO << "Odometry visualizer: " << (is_vis_ ? "ON" : "OFF");
if (is_vis_) visualizer_.reset(new OdometryVisualizer);
return true;
}
void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
if (!is_initialized) {
is_initialized = true;
if (!is_initialized_) {
is_initialized_ = true;
UpdatePre(feature);
*pose = pose_curr2world_;
AWARN << "Odometry initialized....";
@ -27,10 +30,10 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
TicToc timer_whole;
double match_dist_sq_thresh = config_["match_dist_sq_thresh"].as<double>();
AINFO << "Pose before: " << pose_curr2world_.ToString();
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
TicToc timer;
std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs;
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
TicToc timer;
MatchCornPoints(*feature.sharp_corner_pts, *corn_pts_pre_, &pl_pairs,
match_dist_sq_thresh);
double time_pl_match = timer.toc();
@ -73,6 +76,7 @@ void Odometry::Process(const FeaturePoints& feature, Pose3D* const pose) {
AWARN << "Pose after: " << pose_curr2world_.ToString();
UpdatePre(feature);
AUSER << "Time elapsed (ms): whole odometry = " << timer_whole.toc();
if (is_vis_) Visualize(feature, pl_pairs, pp_pairs);
}
void Odometry::MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
@ -192,4 +196,15 @@ void Odometry::UpdatePre(const FeaturePoints& feature) {
kdtree_corn_pts_->setInputCloud(corn_pts_pre_);
}
void Odometry::Visualize(const FeaturePoints& feature,
const std::vector<PointLinePair>& pl_pairs,
const std::vector<PointPlanePair>& pp_pairs) const {
std::shared_ptr<OdometryVisFrame> frame(new OdometryVisFrame);
frame->pl_pairs = pl_pairs;
frame->pp_pairs = pp_pairs;
frame->pose_curr2last = pose_curr2last_;
frame->pose_curr2world = pose_curr2world_;
visualizer_->Render(frame);
}
} // namespace oh_my_loam

13
src/odometry/odometry.h
View File

@ -1,6 +1,7 @@
#pragma once
#include <extractor/feature_points.h>
#include "extractor/feature_points.h"
#include "visualizer/odometry_visualizer.h"
#include "common.h"
#include "helper/helper.h"
@ -18,6 +19,7 @@ class Odometry {
protected:
void UpdatePre(const FeaturePoints& feature);
void MatchCornPoints(const TPointCloud& src, const TPointCloud& tgt,
std::vector<PointLinePair>* const pairs,
double dist_sq_thresh) const;
@ -26,6 +28,10 @@ class Odometry {
std::vector<PointPlanePair>* const pairs,
double dist_sq_thresh) const;
void Visualize(const FeaturePoints& feature,
const std::vector<PointLinePair>& pl_pairs,
const std::vector<PointPlanePair>& pp_pairs) const;
Pose3D pose_curr2world_;
Pose3D pose_curr2last_;
@ -35,9 +41,12 @@ class Odometry {
pcl::KdTreeFLANN<TPoint>::Ptr kdtree_surf_pts_{nullptr};
pcl::KdTreeFLANN<TPoint>::Ptr kdtree_corn_pts_{nullptr};
bool is_initialized = false;
bool is_initialized_ = false;
bool is_vis_ = false;
YAML::Node config_;
std::unique_ptr<OdometryVisualizer> visualizer_{nullptr};
DISALLOW_COPY_AND_ASSIGN(Odometry)
};

72
src/visualizer/odometry_visualizer.cc
View File

@ -1,38 +1,66 @@
#include "Odometry_visualizer.h"
#include "odometry_visualizer.h"
namespace oh_my_loam {
void OdometryVisualizer::Draw() {
auto frame = GetCurrentFrame<OdometryVisFrame>();
{ // add raw point cloud
std::string id = "raw point cloud";
DrawPointCloud(*frame.cloud, WHITE, id, viewer_.get());
TPointCloud src_corn_pts, tgt_corn_pts;
src_corn_pts.resize(frame.pl_pairs.size());
tgt_corn_pts.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_pts[i]);
tgt_corn_pts[2 * i] = pair.line.pt1;
tgt_corn_pts[2 * i + 1] = pair.line.pt2;
}
TPointCloud src_surf_pts, tgt_surf_pts;
src_surf_pts.resize(frame.pp_pairs.size());
tgt_surf_pts.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_pts[i]);
tgt_surf_pts[3 * i] = pair.plane.pt1;
tgt_surf_pts[3 * i + 1] = pair.plane.pt2;
tgt_surf_pts[3 * i + 2] = pair.plane.pt3;
}
{ // add src_corn_pts
std::string id = "src_corn_pts";
DrawPointCloud(src_corn_pts, CYAN, id, viewer_.get(), 7);
rendered_cloud_ids_.push_back(id);
}
{ // add less_flat_surf_pts
std::string id = "less_flat_surf_pts";
DrawPointCloud(*frame.feature_pts.less_flat_surf_pts, CYAN, id,
viewer_.get(), 5);
{ // add tgt_corn_pts
std::string id = "tgt_corn_pts";
DrawPointCloud(tgt_corn_pts, BLUE, id, viewer_.get(), 4);
rendered_cloud_ids_.push_back(id);
}
{ // add flat_surf_pts
std::string id = "flat_surf_pts";
DrawPointCloud(*frame.feature_pts.flat_surf_pts, BLUE, id, viewer_.get(),
7);
{ // add src_surf_pts
std::string id = "src_surf_pts";
DrawPointCloud(src_surf_pts, PURPLE, id, viewer_.get(), 7);
rendered_cloud_ids_.push_back(id);
}
{ // add less_sharp_corner_pts
std::string id = "less_sharp_corner_pts";
DrawPointCloud(*frame.feature_pts.less_sharp_corner_pts, PURPLE, id,
viewer_.get(), 5);
rendered_cloud_ids_.push_back(id);
}
{ // add sharp_corner_pts
std::string id = "sharp_corner_pts";
DrawPointCloud(*frame.feature_pts.sharp_corner_pts, RED, id, viewer_.get(),
7);
{ // add tgt_surf_pts
std::string id = "tgt_surf_pts";
DrawPointCloud(tgt_surf_pts, RED, id, viewer_.get(), 4);
rendered_cloud_ids_.push_back(id);
}
std::vector<Pose3D> poses_n;
poses_n.reserve((poses_.size()));
Pose3D pose_inv = frame.pose_curr2world.Inv();
for (const auto& pose : poses_) {
poses_n.emplace_back(pose_inv * pose);
};
for (size_t i = 0; i < poses_n.size(); ++i) {
Eigen::Vector3f p1 = poses_n[i].p().cast<float>();
if (i < poses_n.size() - 1) {
Eigen::Vector3f p2 = poses_n[i + 1].p().cast<float>();
DrawLine(Point(p1.x(), p1.y(), p1.z()), Point(p2.x(), p2.y(), p2.z()),
WHITE, "line" + std::to_string(i), viewer_.get());
} else {
DrawLine(Point(p1.x(), p1.y(), p1.z()), Point(0, 0, 0), WHITE,
"line" + std::to_string(i), viewer_.get());
}
}
poses_.push_back(frame.pose_curr2world);
}
} // namespace oh_my_loam

10
src/visualizer/odometry_visualizer.h
View File

@ -1,12 +1,17 @@
#pragma once
#include "base_visualizer.h"
#include "extractor/feature_points.h"
#include "helper/helper.h"
namespace oh_my_loam {
struct OdometryVisFrame : public VisFrame {
FeaturePoints feature_pts;
TPointCloudPtr surf_pts;
TPointCloudPtr corn_pts;
std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs;
Pose3D pose_curr2last;
Pose3D pose_curr2world;
};
class OdometryVisualizer : public Visualizer {
@ -17,6 +22,7 @@ class OdometryVisualizer : public Visualizer {
private:
void Draw() override;
std::deque<Pose3D> poses_;
};
} // namespace oh_my_loam

6
src/visualizer/utils.h
View File

@ -28,4 +28,10 @@ void DrawPointCloud(const pcl::PointCloud<PointT>& cloud,
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pt_size, id);
}
template <typename PointT>
void DrawLine(const PointT& pt1, const PointT& pt2, const Color& color,
const std::string& id, PCLVisualizer* const viewer) {
viewer->addLine(pt1, pt2, color.r, color.g, color.b, id);
}
} // namespace oh_my_loam