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

odometer ok

main
feixyz10 2021-01-25 21:00:10 +08:00 committed by feixyz
parent f47143f01b
commit 42eaae9acd
15 changed files with 229 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
common/color/color.h
View File

@ -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)

12
common/pcl/pcl_utils.h
View File

@ -5,15 +5,15 @@
namespace common {
// Distance squred of a point to origin
// Distance squared of a point to origin
template <typename PT>
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 <typename PT>
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 <typename PT>
inline double Distance(const PT &pt) {
return std::sqrt(DistanceSqure(pt));
return std::sqrt(DistanceSquare(pt));
}
// Distance squred of two points
template <typename PT>
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

15
common/visualizer/lidar_visualizer_utils.cc
View File

@ -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 &center, 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
namespace common {} // namespace common

15
common/visualizer/lidar_visualizer_utils.h
View File

@ -37,11 +37,16 @@ void AddPointCloud(const typename pcl::PointCloud<PT>::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 <typename PT>
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 &center, double radius, const Color &color,
const std::string &id, PCLVisualizer *const viewer);
template <typename PT>
void AddSphere(const PT &center, 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

4
main.cc
View File

@ -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);
}

15
oh_my_loam/configs/config.yaml
View File

@ -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:

40
oh_my_loam/extractor/extractor.cc
View File

@ -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<TCTPointCloud> *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<int>();
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<int>();
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<bool> picked(pt_num, false);
std::vector<int> indices = common::Range(pt_num);
int sharp_corner_point_num = config_["sharp_corner_point_num"].as<int>();
int corner_point_num = config_["corner_point_num"].as<int>();
int flat_surf_point_num = config_["flat_surf_point_num"].as<int>();
int surf_point_num = config_["surf_point_num"].as<int>();
float corner_point_curvature_th =
config_["corner_point_curvature_th"].as<float>();
float surf_point_curvature_th =
config_["surf_point_curvature_th"].as<float>();
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<TPoint>(
feature->cloud_surf, dowm_sampled.get(),
config_["downsample_voxel_size"].as<double>());
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<bool> *const picked) const {
auto dist_sq = [&](size_t i, size_t j) -> double {
return common::DistanceSqure<TCTPoint>(scan[i], scan[j]);
return common::DistanceSquare<TCTPoint>(scan[i], scan[j]);
};
double neighbor_point_dist_sq_th =
config_["neighbor_point_dist_sq_th"].as<float>();
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;

55
oh_my_loam/odometer/odometer.cc
View File

@ -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<Feature> &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<PointLinePair> 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<PointPlanePair> pp_pairs;
AINFO_IF(verbose_) << "Points to be matched: "
<< kdtree_corn_.getInputCloud()->size() << " + "
<< kdtree_surf_.getInputCloud()->size();
std::vector<PointLinePair> pl_pairs;
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
std::vector<PointLinePair>().swap(pl_pairs);
std::vector<PointPlanePair>().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<Feature> &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<size_t>()) {
AWARN << "Too less correspondence: " << pl_pairs.size() << " + "
<< pp_pairs.size();
continue;
@ -63,17 +61,19 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
for (const auto &pair : pp_pairs) {
solver.AddPointPlanePair(pair, GetTime(pair.pt));
}
solver.Solve(config_["solve_iter_num"].as<int>(), verbose_,
&pose_curr2last_);
AINFO << "Odometer::ICP: iter_" << i << ": " << BLOCK_TIMER_STOP_FMT;
bool is_converge = solver.Solve(config_["solve_iter_num"].as<int>(),
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<int>();
TPoint pt2;
bool pt2_fount = false;
float min_pt2_dist_sq = dist_sq_thresh;
int i_begin = std::max<int>(0, pt1_scan_id - kNearScanN);
int i_end =
std::min<int>(kdtrees_scan_corn_.size(), pt1_scan_id + kNearScanN + 1);
int i_begin = std::max<int>(0, pt1_scan_id - nearby_scan_num);
int i_end = std::min<int>(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<int>();
TPoint pt2;
bool pt2_fount = false;
float min_pt2_dist_sq = dist_sq_thresh;
int i_begin = std::max<int>(0, pt1_scan_id - kNearScanN);
int i_end =
std::min<int>(kdtrees_scan_surf_.size(), pt1_scan_id + kNearScanN + 1);
int i_begin = std::max<int>(0, pt1_scan_id - nearby_scan_num);
int i_end = std::min<int>(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<Feature> &features) {
kdtree_surf_.setInputCloud(surf_pre);
}
void Odometer::Visualize(const std::vector<Feature> &features,
void Odometer::Visualize(const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf,
const std::vector<PointLinePair> &pl_pairs,
const std::vector<PointPlanePair> &pp_pairs,
double timestamp) const {
std::shared_ptr<OdometerVisFrame> 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_;

3
oh_my_loam/odometer/odometer.h
View File

@ -27,7 +27,8 @@ class Odometer {
void MatchSurf(const TPointCloud &src,
std::vector<PointPlanePair> *const pairs) const;
void Visualize(const std::vector<Feature> &features,
void Visualize(const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf,
const std::vector<PointLinePair> &pl_pairs,
const std::vector<PointPlanePair> &pp_pairs,
double timestamp = 0.0) const;

19
oh_my_loam/oh_my_loam.cc
View File

@ -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<Feature> 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<Point>(cloud_in, cloud_out, [&](const Point &pt) {
return !IsFinite<Point>(pt) ||
DistanceSqure<Point>(pt) < kPointMinDist * kPointMinDist;
void OhMyLoam::RemoveOutliers(const common::PointCloud &cloud_in,
common::PointCloud *const cloud_out) const {
common::RemovePoints<common::Point>(cloud_in, cloud_out, [&](const auto &pt) {
return !common::IsFinite<common::Point>(pt) ||
common::DistanceSquare<common::Point>(pt) <
kPointMinDist * kPointMinDist;
});
}

21
oh_my_loam/solver/cost_function.h
View File

@ -59,10 +59,22 @@ bool PointLineCostFunction::operator()(const T *const r_quat,
Eigen::Matrix<T, 3, 1> p(T(pt.x), T(pt.y), T(pt.z));
Eigen::Matrix<T, 3, 1> p1(T(pt1.x), T(pt1.y), T(pt1.z));
Eigen::Matrix<T, 3, 1> p2(T(pt2.x), T(pt2.y), T(pt2.z));
// if constexpr (!std::is_arithmetic<T>::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<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
Eigen::Quaternion<T> r_interp =
Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
// if constexpr (!std::is_arithmetic<T>::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, 3, 1> t(T(time_) * t_vec[0], T(time_) * t_vec[1],
T(time_) * t_vec[2]);
Eigen::Matrix<T, 3, 1> 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<T>::value) {
AERROR << p.transpose() << ", ";
}
// if constexpr (!std::is_arithmetic<T>::value) {
// AERROR << base_length.a;
// }
return true;
}
@ -99,9 +111,6 @@ bool PointPlaneCostFunction::operator()(const T *const r_quat,
Eigen::Matrix<T, 3, 1> normal = (p2 - p1).cross(p3 - p1).normalized();
residual[0] = (p0 - p1).dot(normal);
if constexpr (!std::is_arithmetic<T>::value) {
AERROR << "ppres" << residual[0].a;
}
return true;
}

2
oh_my_loam/solver/solver.cc
View File

@ -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;
}

2
oh_my_loam/visualizer/extractor_visualizer.cc
View File

@ -7,7 +7,7 @@ using namespace common;
void ExtractorVisualizer::Draw() {
auto frame = GetCurrentFrame<ExtractorVisFrame>();
DrawPointCloud<Point>(frame.cloud, GRAY, "cloud_raw");
DrawPointCloud<Point>(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);

135
oh_my_loam/visualizer/odometer_visualizer.cc
View File

@ -10,49 +10,116 @@ using namespace common;
void OdometerVisualizer::Draw() {
auto frame = GetCurrentFrame<OdometerVisFrame>();
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<TPoint>(frame.cloud_corn, GRAY, "cloud_corn");
DrawPointCloud<TPoint>(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<PointLinePair> &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<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());
}
DrawPointCloud<TPoint>(tgt_corn, YELLOW, "tgt_corn", 4);
DrawPointCloud<TPoint>(src_corn, RED, "src_corn", 4);
DrawPointCloud<TPoint>(tgt_surf, BLUE, "tgt_surf", 4);
DrawPointCloud<TPoint>(src_surf, CYAN, "src_surf", 4);
}
void OdometerVisualizer::DrawSurf(const Pose3d &pose,
const std::vector<PointPlanePair> &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);
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 = 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<float>();
if (i < poses_n.size() - 1) {
Eigen::Vector3f p2 = poses_n[i + 1].t_vec().cast<float>();
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<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(
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() == "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

17
oh_my_loam/visualizer/odometer_visualizer.h
View File

@ -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<PointLinePair> pl_pairs;
std::vector<PointPlanePair> 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<PointLinePair> &pairs);
void DrawSurf(const 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::deque<common::Pose3d> poses_;
};