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

no_convergence needs to be solved

main
feixyz10 2021-02-02 21:06:45 +08:00 committed by feixyz
parent 1b0919d1b7
commit 79f61e4e7d
17 changed files with 282 additions and 142 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
common/config/yaml_config.h
View File

@ -29,7 +29,7 @@ class YAMLConfig {
template <typename T>
static const std::vector<T> GetSeq(const YAML::Node &node) {
ACHECK(node.IsSequence()) << "Not sequence.";
ACHECK(node.IsSequence());
std::vector<T> seq;
for (auto it = node.begin(); it != node.end(); ++it) {
seq.push_back(it->as<T>());
@ -39,7 +39,7 @@ class YAMLConfig {
template <typename TK, typename TV>
static const std::map<TK, TV> GetMap(const YAML::Node &node) {
ACHECK(node.IsMap()) << "Not sequence.";
ACHECK(node.IsMap());
std::map<TK, TV> map;
for (auto it = node.begin(); it != node.end(); ++it) {
map.insert({it->first.as<TK>(), it->second.as<TV>()});

55
common/log/log.cc
View File

@ -5,9 +5,8 @@ namespace common {
void InitG3Logging(bool log_to_file, const std::string &prefix,
const std::string &path) {
static std::unique_ptr<g3::LogWorker> worker;
if (worker != nullptr) return;
worker = std::move(g3::LogWorker::createLogWorker());
static std::unique_ptr<g3::LogWorker> worker(
g3::LogWorker::createLogWorker());
worker->addSink(std::make_unique<g3::CustomSink>(),
&g3::CustomSink::StdLogMessage);
if (log_to_file) {
@ -26,4 +25,52 @@ void InitG3Logging(bool log_to_file, const std::string &prefix,
g3::initializeLogging(worker.get());
}
} // namespace common
} // namespace common
namespace g3 {
CustomSink::~CustomSink() {
std::ostringstream oss;
oss << "\ng3log " << (log_to_file_ ? "FileSink" : "StdSink")
<< " shutdown at: ";
auto now = std::chrono::system_clock::now();
oss << g3::localtime_formatted(now, "%Y%m%d %H:%M:%S.%f3");
if (log_to_file_) {
(*ofs_) << oss.str() << std::endl;
} else {
std::clog << oss.str() << std::endl;
}
};
std::string CustomSink::FormatedMessage(const g3::LogMessage &msg) const {
std::ostringstream oss;
oss << "[" << msg.level()[0] << msg.timestamp("%Y%m%d %H:%M:%S.%f3") << " "
<< msg.file() << ":" << msg.line() << "] " << msg.message();
return oss.str();
}
std::string CustomSink::ColorFormatedMessage(const g3::LogMessage &msg) const {
std::ostringstream oss;
oss << GetColorCode(msg._level) << FormatedMessage(msg) << "\033[m";
return oss.str();
}
std::string CustomSink::GetColorCode(const LEVELS &level) const {
if (level.value == WARNING.value) {
return "\033[33m"; // yellow
}
if (level.value == DEBUG.value) {
return "\033[32m"; // green
}
if (level.value == ERROR.value) {
return "\033[31m"; // red
}
if (level.value == USER.value) {
return "\033[1m\033[34m"; // bold blue
}
if (g3::internal::wasFatal(level)) {
return "\033[1m\033[31m"; // red
}
return "\033[97m"; // white
}
} // namespace g3

90
common/log/log.h
View File

@ -8,25 +8,31 @@
#include <iostream>
// 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 \
// conflicts LOG macro in glog.
#define G3LOG(level) \
if (g3::logLevel(level)) INTERNAL_LOG_MESSAGE(level).stream()
#define VARIABLE_WITH_LINE_NUM(var_name, line) var_name##line
#define COUNT_VAR_WITH_LINE_NUM VARIABLE_WITH_LINE_NUM(_count_var_, __LINE__)
#define G3LOG_EVERY(level, n) \
static size_t COUNT_VAR_WITH_LINE_NUM = 0; \
if ((COUNT_VAR_WITH_LINE_NUM)++ % static_cast<size_t>(n) == 0 && \
g3::logLevel(level)) \
INTERNAL_LOG_MESSAGE(level).stream();
#define G3LOG_IF(level, boolean_expression) \
if ((boolean_expression) && g3::logLevel(level)) \
INTERNAL_LOG_MESSAGE(level).stream()
#define G3LOG_IF(level, boolean_expression) \
if (false == (boolean_expression) || !g3::logLevel(level)) { \
} else \
INTERNAL_LOG_MESSAGE(level).stream()
#define G3CHECK(boolean_expression) \
if (true == (boolean_expression)) { \
} else \
#define G3CHECK(boolean_expression) \
if (!(boolean_expression)) \
INTERNAL_CONTRACT_MESSAGE(#boolean_expression).stream()
const LEVELS ERROR{WARNING.value + 100, "ERROR"};
const LEVELS USER(ERROR.value + 100, "USER");
// LOG
#define ADEBUG G3LOG(DEBUG)
#define AINFO G3LOG(INFO)
#define AWARN G3LOG(WARNING)
@ -43,6 +49,13 @@ const LEVELS USER(ERROR.value + 100, "USER");
#define AFATAL_IF(cond) G3LOG_IF(FATAL, cond)
#define ACHECK(cond) G3CHECK(cond)
// LOG_EVERY
#define ADEBUG_EVERY(n) G3LOG_EVERY(DEBUG, n)
#define AINFO_EVERY(n) G3LOG_EVERY(INFO, n)
#define AWARN_EVERY(n) G3LOG_EVERY(WARNING, n)
#define AERROR_EVERY(n) G3LOG_EVERY(ERROR, n)
#define AUSER_EVERY(n) G3LOG_EVERY(USER, n)
namespace common {
void InitG3Logging(bool log_to_file = false, const std::string &prefix = "",
const std::string &path = "./");
@ -58,26 +71,15 @@ class CustomSink {
ofs_.reset(new std::ofstream(log_file_name));
}
~CustomSink() {
std::ostringstream oss;
oss << "\ng3log " << (log_to_file_ ? "FileSink" : "StdSink")
<< " shutdown at: ";
auto now = std::chrono::system_clock::now();
oss << g3::localtime_formatted(now, "%Y%m%d %H:%M:%S.%f3");
if (log_to_file_) {
(*ofs_) << oss.str() << std::endl;
} else {
std::clog << oss.str() << std::endl;
}
};
~CustomSink();
void StdLogMessage(g3::LogMessageMover logEntry) {
std::clog << ColorFormatedMessage(logEntry.get()) << std::endl;
void StdLogMessage(g3::LogMessageMover log_entry) {
std::clog << ColorFormatedMessage(log_entry.get()) << std::endl;
}
void FileLogMessage(g3::LogMessageMover logEntry) {
void FileLogMessage(g3::LogMessageMover log_entry) {
if (log_to_file_) {
(*ofs_) << FormatedMessage(logEntry.get()) << std::endl;
(*ofs_) << FormatedMessage(log_entry.get()) << std::endl;
}
}
@ -86,37 +88,11 @@ class CustomSink {
bool log_to_file_{false};
std::unique_ptr<std::ofstream> ofs_{nullptr};
std::string FormatedMessage(const g3::LogMessage &msg) const {
std::ostringstream oss;
oss << "[" << msg.level()[0] << msg.timestamp("%Y%m%d %H:%M:%S.%f3") << " "
<< msg.file() << ":" << msg.line() << "] " << msg.message();
return oss.str();
}
std::string FormatedMessage(const g3::LogMessage &msg) const;
std::string ColorFormatedMessage(const g3::LogMessage &msg) const {
std::ostringstream oss;
oss << GetColorCode(msg._level) << FormatedMessage(msg) << "\033[m";
return oss.str();
}
std::string ColorFormatedMessage(const g3::LogMessage &msg) const;
std::string GetColorCode(const LEVELS &level) const {
if (level.value == WARNING.value) {
return "\033[33m"; // yellow
}
if (level.value == DEBUG.value) {
return "\033[32m"; // green
}
if (level.value == ERROR.value) {
return "\033[31m"; // red
}
if (level.value == USER.value) {
return "\033[1m\033[34m"; // bold blue
}
if (g3::internal::wasFatal(level)) {
return "\033[1m\033[31m"; // red
}
return "\033[97m"; // white
}
std::string GetColorCode(const LEVELS &level) const;
};
} // namespace g3

6
common/math/fitting.h
View File

@ -15,7 +15,7 @@ Eigen::Vector3d FitLine2D(const pcl::PointCloud<PT> &cloud,
for (const auto &p : cloud) data.row(i++) << p.x, p.y;
Eigen::RowVector2f centroid = data.colwise().mean();
Eigen::MatrixX2f data_centered = data.rowwise() - centroid;
Eigen::JacobiSVD<Eigen::MatrixX2f> svd(data_centered, Eigen::ComputeThinV);
Eigen::JacobiSVD<Eigen::MatrixX2f> svd(data_centered, Eigen::ComputeFullV);
Eigen::Vector2f normal = svd.matrixV().col(1);
float c = -centroid * normal;
if (score) *score = svd.singularValues()[0] / svd.singularValues()[1];
@ -34,7 +34,7 @@ Eigen::Matrix<double, 6, 1> FitLine3D(const pcl::PointCloud<PT> &cloud,
for (const auto &p : cloud) data.row(i++) << p.x, p.y, p.z;
Eigen::RowVector3f centroid = data.colwise().mean();
Eigen::MatrixX3f data_centered = data.rowwise() - centroid;
Eigen::JacobiSVD<Eigen::MatrixX3f> svd(data_centered, Eigen::ComputeThinV);
Eigen::JacobiSVD<Eigen::MatrixX3f> svd(data_centered, Eigen::ComputeFullV);
Eigen::Vector3f direction = svd.matrixV().col(0);
Eigen::Matrix<double, 6, 1> line_coeffs;
line_coeffs.topRows(3) = centroid.transpose().cast<double>();
@ -55,7 +55,7 @@ Eigen::Vector4d FitPlane(const pcl::PointCloud<PT> &cloud,
for (const auto &p : cloud) data.row(i++) << p.x, p.y, p.z;
Eigen::RowVector3f centroid = data.colwise().mean();
Eigen::MatrixX3f data_centered = data.rowwise() - centroid;
Eigen::JacobiSVD<Eigen::MatrixX3f> svd(data_centered, Eigen::ComputeThinV);
Eigen::JacobiSVD<Eigen::MatrixX3f> svd(data_centered, Eigen::ComputeFullV);
Eigen::Vector3f normal = svd.matrixV().col(2);
float d = -centroid * normal;
if (score) {

3
main.cc
View File

@ -21,17 +21,14 @@ int main(int argc, char *argv[]) {
bool log_to_file = YAMLConfig::Instance()->Get<bool>("log_to_file");
std::string log_path = YAMLConfig::Instance()->Get<std::string>("log_path");
std::string lidar = YAMLConfig::Instance()->Get<std::string>("lidar");
// logging
InitG3Logging(log_to_file, "oh_my_loam_" + lidar, log_path);
AUSER << "LOAM start..., lidar = " << lidar;
// SLAM system
OhMyLoam slam;
if (!slam.Init()) {
AFATAL << "Failed to initilize slam system.";
}
// ros
ros::init(argc, argv, "oh_my_loam");
ros::NodeHandle nh;

16
oh_my_loam/configs/config.yaml
View File

@ -2,11 +2,11 @@
lidar: VPL16
log_to_file: false
log_path: /data/log/oh_my_loam
vis: true
vis: false
# configs for extractor
extractor_config:
vis: false
vis: true
verbose: false
min_point_num: 66
scan_seg_num: 12
@ -20,11 +20,11 @@ extractor_config:
# configs for odometer
odometer_config:
vis: true
verbose: false
vis: false
verbose: true
nearby_scan_num: 1
min_correspondence_num: 10
icp_iter_num: 2
icp_iter_num: 4
solve_iter_num: 4
corn_match_dist_sq_th: 16.0
surf_match_dist_sq_th: 16.0
@ -34,9 +34,13 @@ mapper_config:
vis: false
verbose: false
map_shape: [3, 21, 21]
map_step: 50 # meter
map_step: 50
submap_shape: [1, 5, 5]
icp_iter_num: 2
solve_iter_num: 4
min_correspondence_num: 10
nearest_neighbor_k: 5
neighbor_point_dist_sq_th: 1.0
min_line_fit_score: 2
min_plane_fit_score: 2
downsample_voxel_size: 0.6

2
oh_my_loam/extractor/extractor.cc
View File

@ -14,7 +14,7 @@ const double kEps = 1e-6;
bool Extractor::Init() {
const auto &config = common::YAMLConfig::Instance()->config();
config_ = config["extractor_config"];
is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
is_vis_ = config_["vis"].as<bool>();
verbose_ = config_["verbose"].as<bool>();
AINFO << "Extraction visualizer: " << (is_vis_ ? "ON" : "OFF");
if (is_vis_) visualizer_.reset(new ExtractorVisualizer);

8
oh_my_loam/mapper/map.cc
View File

@ -108,6 +108,13 @@ 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];
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 cloud_all(new TPointCloud);
@ -128,6 +135,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;
this->at(index)->push_back(point);
if (indices) index_set.insert(index);
}

2
oh_my_loam/mapper/map.h
View File

@ -48,6 +48,8 @@ class Map {
Index GetIndex(const TPoint &point) const;
bool IsIndexValid(const Index &index) const;
TPointCloudPtr GetSurrPoints(const TPoint &point,
const std::vector<int> &surr_shapes) const;

79
oh_my_loam/mapper/mapper.cc
View File

@ -36,11 +36,11 @@ void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
const common::Pose3d &pose_curr2odom,
common::Pose3d *const pose_curr2map) {
if (GetState() == UN_INIT) {
corn_map_->AddPoints(cloud_corn);
surf_map_->AddPoints(cloud_surf);
pose_curr2map->SetIdentity();
pose_odom2map_.SetIdentity();
UpdateMap(*pose_curr2map, cloud_corn, cloud_surf);
SetState(DONE);
AINFO << "Mapper initialized...";
return;
}
if (GetState() == DONE) {
@ -55,6 +55,7 @@ void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf,
const common::Pose3d &pose_curr2odom) {
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(),
@ -62,27 +63,43 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
AdjustMap(cnt);
TPointCloudPtr cloud_corn_map = corn_map_->GetSurrPoints(cnt, submap_shape_);
TPointCloudPtr cloud_surf_map = corn_map_->GetSurrPoints(cnt, submap_shape_);
pcl::KdTreeFLANN<TPoint> kdtree_corn;
kdtree_corn.setInputCloud(cloud_corn_map);
pcl::KdTreeFLANN<TPoint> kdtree_surf;
kdtree_surf.setInputCloud(cloud_surf_map);
std::vector<PointLinePair> pl_pairs;
std::vector<PointPlaneCoeffPair> pp_pairs;
MatchCorn(kdtree_corn, cloud_corn_map, pose_curr2map, &pl_pairs);
MatchSurf(kdtree_surf, cloud_surf_map, pose_curr2map, &pp_pairs);
PoseSolver solver(pose_curr2map);
for (const auto &pair : pl_pairs) {
solver.AddPointLinePair(pair, 0.0);
}
for (const auto &pair : pp_pairs) {
solver.AddPointPlaneCoeffPair(pair, 0.0);
}
if (!solver.Solve(5, false, &pose_curr2map)) {
AWARN << "Solve: no_convergence";
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<PointLinePair> pl_pairs;
MatchCorn(kdtree_corn, cloud_corn_map, pose_curr2map, &pl_pairs);
std::vector<PointPlaneCoeffPair> pp_pairs;
MatchSurf(kdtree_surf, cloud_surf_map, pose_curr2map, &pp_pairs);
AINFO_IF(verbose_) << "Iter " << i
<< ": matched num: point2line = " << pl_pairs.size()
<< ", point2plane = " << pp_pairs.size();
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;
}
PoseSolver solver(pose_curr2map);
for (const auto &pair : pl_pairs) {
solver.AddPointLinePair(pair, 0.0);
}
for (const auto &pair : pp_pairs) {
solver.AddPointPlaneCoeffPair(pair, 0.0);
}
if (!solver.Solve(config_["solve_iter_num"].as<int>(), verbose_,
&pose_curr2map)) {
AWARN << "Mapping solve: no_convergence";
}
AINFO_IF(verbose_) << "Odometer::ICP: iter_" << i << ": "
<< BLOCK_TIMER_STOP_FMT;
}
UpdateMap(pose_curr2map, cloud_corn, cloud_surf);
SetState(DONE); // be careful with deadlock
std::lock_guard<std::mutex> lock(state_mutex_);
pose_odom2map_ = pose_curr2map * pose_curr2odom.Inv();
SetState(DONE);
AUSER << "Mapper::Run: " << BLOCK_TIMER_STOP_FMT;
}
void Mapper::MatchCorn(const pcl::KdTreeFLANN<TPoint> &kdtree,
@ -173,18 +190,36 @@ void Mapper::AdjustMap(const TPoint &center) {
}
}
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) {
TPointCloudPtr cloud_trans(new TPointCloud);
common::TransformPointCloud(pose_curr2map, *cloud, cloud_trans.get());
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, corn_map_.get());
update_map(cloud_surf, surf_map_.get());
}
TPointCloudPtr Mapper::GetMapCloudCorn() const {
std::lock_guard<std::mutex> lock(map_mutex_);
return corn_map_->GetAllPoints();
}
TPointCloudPtr Mapper::GetMapCloudSurf() const {
std::lock_guard<std::mutex> lock(map_mutex_);
return surf_map_->GetAllPoints();
}
TPointCloudPtr Mapper::GetMapCloud() const {
TPointCloudPtr map_points(new TPointCloud);
*map_points += *GetMapCloudCorn();
*map_points += *GetMapCloudSurf();
std::lock_guard<std::mutex> lock(map_mutex_);
*map_points += *corn_map_->GetAllPoints();
*map_points += *surf_map_->GetAllPoints();
return map_points;
}

21
oh_my_loam/mapper/mapper.h
View File

@ -56,6 +56,10 @@ class Mapper {
void AdjustMap(const TPoint &center);
void UpdateMap(const common::Pose3d &pose_curr2map,
const TPointCloudConstPtr &cloud_corn = nullptr,
const TPointCloudConstPtr &cloud_surf = nullptr);
void MatchCorn(const TPointCloudConstPtr &src,
const TCTPointCloudConstPtr &tgt,
std::vector<PointLinePair> *const pairs) const;
@ -63,22 +67,21 @@ class Mapper {
void MatchSurf(const TPointCloudConstPtr &src,
const TCTPointCloudConstPtr &tgt,
std::vector<PointLinePair> *const pairs) const;
YAML::Node config_;
std::vector<int> map_shape_, submap_shape_;
double map_step_;
// map
mutable std::mutex map_mutex_;
std::unique_ptr<Map> corn_map_;
std::unique_ptr<Map> surf_map_;
// state
mutable std::mutex state_mutex_;
State state_ = UN_INIT;
common::Pose3d pose_odom2map_;
// thread to run mapping
mutable std::unique_ptr<std::thread> thread_{nullptr};
// configs
YAML::Node config_;
std::vector<int> map_shape_, submap_shape_;
double map_step_;
bool is_vis_ = false;
bool verbose_ = false;
DISALLOW_COPY_AND_ASSIGN(Mapper)

10
oh_my_loam/odometer/odometer.cc
View File

@ -11,7 +11,7 @@ namespace oh_my_loam {
bool Odometer::Init() {
const auto &config = common::YAMLConfig::Instance()->config();
config_ = config["odometer_config"];
is_vis_ = config["vis"].as<bool>() && config_["vis"].as<bool>();
is_vis_ = config_["vis"].as<bool>();
verbose_ = config_["verbose"].as<bool>();
AINFO << "Odometry visualizer: " << (is_vis_ ? "ON" : "OFF");
if (is_vis_) visualizer_.reset(new OdometerVisualizer);
@ -30,7 +30,7 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
pose_curr2last_.SetIdentity();
pose_curr2world_.SetIdentity();
pose_out->SetIdentity();
AINFO << "Odometer initialized....";
AINFO << "Odometer initialized...";
return;
}
BLOCK_TIMER_START;
@ -61,12 +61,12 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
}
bool is_converge = solver.Solve(config_["solve_iter_num"].as<int>(),
verbose_, &pose_curr2last_);
AWARN_IF(!is_converge) << "Solve: no_convergence";
AWARN_IF(!is_converge) << "Odometry solve: no_convergence";
AINFO_IF(verbose_) << "Odometer::ICP: iter_" << i << ": "
<< BLOCK_TIMER_STOP_FMT;
}
*pose_out = *pose_out * pose_curr2last_;
pose_curr2world_ = *pose_out;
pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
*pose_out = pose_curr2world_;
AINFO_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString();
// mush called before calling UpdatePre
if (is_vis_) Visualize(pl_pairs, pp_pairs);

26
oh_my_loam/oh_my_loam.cc
View File

@ -9,11 +9,12 @@
namespace oh_my_loam {
namespace {
const double kPointMinDist = 0.1;
const double kPointMinDist = 0.5;
} // namespace
bool OhMyLoam::Init() {
config_ = common::YAMLConfig::Instance()->config();
is_vis_ = config_["vis"].as<bool>();
extractor_.reset(new ExtractorVLP16);
if (!extractor_->Init()) {
AERROR << "Failed to initialize extractor";
@ -29,6 +30,7 @@ bool OhMyLoam::Init() {
AERROR << "Failed to initialize mapper";
return false;
}
if (is_vis_) visualizer_.reset(new OhmyloamVisualizer);
return true;
}
@ -53,11 +55,27 @@ void OhMyLoam::Run(double timestamp,
mapper_->Process(timestamp, cloud_corn, cloud_surf, pose_curr2odom,
&pose_curr2map);
poses_curr2odom_.push_back(pose_curr2odom);
poses_curr2world_.push_back(pose_curr2map);
if (is_vis_) Visualize(timestamp);
poses_curr2map_.push_back(pose_curr2map);
if (is_vis_) {
Visualize(pose_curr2odom, pose_curr2map, cloud_corn, cloud_surf, timestamp);
}
}
void OhMyLoam::Visualize(double timestamp) {}
void OhMyLoam::Visualize(const common::Pose3d &pose_curr2odom,
const common::Pose3d &pose_curr2map,
const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &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->pose_map = pose_curr2map;
visualizer_->Render(frame);
}
void OhMyLoam::RemoveOutliers(const common::PointCloud &cloud_in,
common::PointCloud *const cloud_out) const {

10
oh_my_loam/oh_my_loam.h
View File

@ -6,6 +6,7 @@
#include "oh_my_loam/extractor/extractor.h"
#include "oh_my_loam/mapper/mapper.h"
#include "oh_my_loam/odometer/odometer.h"
#include "oh_my_loam/visualizer/ohmyloam_visualizer.h"
namespace oh_my_loam {
@ -20,7 +21,10 @@ class OhMyLoam {
private:
void Reset();
void Visualize(double timestamp = 0.0);
void Visualize(const common::Pose3d &pose_curr2odom,
const common::Pose3d &pose_curr2map,
const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf, double timestamp = 0.0);
std::unique_ptr<Extractor> extractor_{nullptr};
@ -33,11 +37,13 @@ class OhMyLoam {
common::PointCloud *const cloud_out) const;
std::vector<common::Pose3d> poses_curr2odom_;
std::vector<common::Pose3d> poses_curr2world_;
std::vector<common::Pose3d> poses_curr2map_;
YAML::Node config_;
bool is_vis_ = false;
std::unique_ptr<OhmyloamVisualizer> visualizer_{nullptr};
DISALLOW_COPY_AND_ASSIGN(OhMyLoam);
};

2
oh_my_loam/visualizer/odometer_visualizer.h
View File

@ -38,7 +38,7 @@ class OdometerVisualizer : public common::LidarVisualizer {
bool corn_connect_ = false;
bool surf_connect_ = false;
std::deque<common::Pose3d> poses_;
std::vector<common::Pose3d> poses_;
};
} // namespace oh_my_loam

77
oh_my_loam/visualizer/ohmyloam_visualizer.cc
View File

@ -1,38 +1,73 @@
#include "ohmyloam_visualizer.h"
#include "common/color/color.h"
#include "common/pcl/pcl_utils.h"
namespace oh_my_loam {
namespace {
using namespace common;
#define LIGHT_BLUE common::Color(0, 0, 80)
#define LIGHT_YELLOW common::Color(80, 80, 0)
#define LIGHT_BLUE common::Color(0, 0, 150)
#define LIGHT_YELLOW common::Color(150, 150, 0)
} // namespace
void OhmyloamVisualizer::Draw() {
auto frame = GetCurrentFrame<OhmyloamVisFrame>();
DrawPointCloud<TPoint>(frame.cloud_map_corn, LIGHT_YELLOW, "cloud_map_corn");
DrawPointCloud<TPoint>(frame.cloud_map_surf, LIGHT_BLUE, "cloud_map_surf");
DrawPointCloud<TPoint>(frame.cloud_corn, RED, "cloud_corn");
DrawPointCloud<TPoint>(frame.cloud_surf, CYAN, "cloud_surf");
DrawTrajectory(frame.poses);
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(std::vector<common::Pose3d> &poses) {
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 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;
}
}

13
oh_my_loam/visualizer/ohmyloam_visualizer.h
View File

@ -11,7 +11,8 @@ struct OhmyloamVisFrame : public common::LidarVisFrame {
TPointCloudConstPtr cloud_map_surf;
TPointCloudConstPtr cloud_corn; // current
TPointCloudConstPtr cloud_surf; // current
std::vector<common::Pose3d> poses;
common::Pose3d pose_odom;
common::Pose3d pose_map;
};
class OhmyloamVisualizer : public common::LidarVisualizer {
@ -23,7 +24,15 @@ class OhmyloamVisualizer : public common::LidarVisualizer {
private:
void Draw() override;
void DrawTrajectory(std::vector<common::Pose3d> &poses);
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;
};
} // namespace oh_my_loam