From 79f61e4e7d6426e13492f9845ebb7498897e27dc Mon Sep 17 00:00:00 2001 From: feixyz10 Date: Tue, 2 Feb 2021 21:06:45 +0800 Subject: [PATCH] no_convergence needs to be solved --- common/config/yaml_config.h | 4 +- common/log/log.cc | 55 +++++++++++- common/log/log.h | 90 +++++++------------- common/math/fitting.h | 6 +- main.cc | 3 - oh_my_loam/configs/config.yaml | 16 ++-- oh_my_loam/extractor/extractor.cc | 2 +- oh_my_loam/mapper/map.cc | 8 ++ oh_my_loam/mapper/map.h | 2 + oh_my_loam/mapper/mapper.cc | 79 ++++++++++++----- oh_my_loam/mapper/mapper.h | 21 +++-- oh_my_loam/odometer/odometer.cc | 10 +-- oh_my_loam/oh_my_loam.cc | 26 +++++- oh_my_loam/oh_my_loam.h | 10 ++- oh_my_loam/visualizer/odometer_visualizer.h | 2 +- oh_my_loam/visualizer/ohmyloam_visualizer.cc | 77 ++++++++++++----- oh_my_loam/visualizer/ohmyloam_visualizer.h | 13 ++- 17 files changed, 282 insertions(+), 142 deletions(-) diff --git a/common/config/yaml_config.h b/common/config/yaml_config.h index 1b4c770..2735c71 100644 --- a/common/config/yaml_config.h +++ b/common/config/yaml_config.h @@ -29,7 +29,7 @@ class YAMLConfig { template static const std::vector GetSeq(const YAML::Node &node) { - ACHECK(node.IsSequence()) << "Not sequence."; + ACHECK(node.IsSequence()); std::vector seq; for (auto it = node.begin(); it != node.end(); ++it) { seq.push_back(it->as()); @@ -39,7 +39,7 @@ class YAMLConfig { template static const std::map GetMap(const YAML::Node &node) { - ACHECK(node.IsMap()) << "Not sequence."; + ACHECK(node.IsMap()); std::map map; for (auto it = node.begin(); it != node.end(); ++it) { map.insert({it->first.as(), it->second.as()}); diff --git a/common/log/log.cc b/common/log/log.cc index 73ded48..47c8b8c 100644 --- a/common/log/log.cc +++ b/common/log/log.cc @@ -5,9 +5,8 @@ namespace common { void InitG3Logging(bool log_to_file, const std::string &prefix, const std::string &path) { - static std::unique_ptr worker; - if (worker != nullptr) return; - worker = std::move(g3::LogWorker::createLogWorker()); + static std::unique_ptr worker( + g3::LogWorker::createLogWorker()); worker->addSink(std::make_unique(), &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 \ No newline at end of file +} // 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 diff --git a/common/log/log.h b/common/log/log.h index 013d728..9553f16 100644 --- a/common/log/log.h +++ b/common/log/log.h @@ -8,25 +8,31 @@ #include // 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(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 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 \ No newline at end of file diff --git a/common/math/fitting.h b/common/math/fitting.h index 3e69742..e3017d6 100644 --- a/common/math/fitting.h +++ b/common/math/fitting.h @@ -15,7 +15,7 @@ Eigen::Vector3d FitLine2D(const pcl::PointCloud &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 svd(data_centered, Eigen::ComputeThinV); + Eigen::JacobiSVD 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 FitLine3D(const pcl::PointCloud &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 svd(data_centered, Eigen::ComputeThinV); + Eigen::JacobiSVD svd(data_centered, Eigen::ComputeFullV); Eigen::Vector3f direction = svd.matrixV().col(0); Eigen::Matrix line_coeffs; line_coeffs.topRows(3) = centroid.transpose().cast(); @@ -55,7 +55,7 @@ Eigen::Vector4d FitPlane(const pcl::PointCloud &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 svd(data_centered, Eigen::ComputeThinV); + Eigen::JacobiSVD svd(data_centered, Eigen::ComputeFullV); Eigen::Vector3f normal = svd.matrixV().col(2); float d = -centroid * normal; if (score) { diff --git a/main.cc b/main.cc index a13d01e..a74c4e4 100644 --- a/main.cc +++ b/main.cc @@ -21,17 +21,14 @@ int main(int argc, char *argv[]) { bool log_to_file = YAMLConfig::Instance()->Get("log_to_file"); std::string log_path = YAMLConfig::Instance()->Get("log_path"); std::string lidar = YAMLConfig::Instance()->Get("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; diff --git a/oh_my_loam/configs/config.yaml b/oh_my_loam/configs/config.yaml index c024083..f3322f7 100644 --- a/oh_my_loam/configs/config.yaml +++ b/oh_my_loam/configs/config.yaml @@ -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 diff --git a/oh_my_loam/extractor/extractor.cc b/oh_my_loam/extractor/extractor.cc index bb1e4b2..1bdf52e 100644 --- a/oh_my_loam/extractor/extractor.cc +++ b/oh_my_loam/extractor/extractor.cc @@ -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() && config_["vis"].as(); + is_vis_ = config_["vis"].as(); verbose_ = config_["verbose"].as(); AINFO << "Extraction visualizer: " << (is_vis_ ? "ON" : "OFF"); if (is_vis_) visualizer_.reset(new ExtractorVisualizer); diff --git a/oh_my_loam/mapper/map.cc b/oh_my_loam/mapper/map.cc index 73e6c8d..529f4d9 100644 --- a/oh_my_loam/mapper/map.cc +++ b/oh_my_loam/mapper/map.cc @@ -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 &surr_shapes) const { TPointCloudPtr cloud_all(new TPointCloud); @@ -128,6 +135,7 @@ void Map::AddPoints(const TPointCloudConstPtr &cloud, std::set 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); } diff --git a/oh_my_loam/mapper/map.h b/oh_my_loam/mapper/map.h index a85e0ef..2dd83c4 100644 --- a/oh_my_loam/mapper/map.h +++ b/oh_my_loam/mapper/map.h @@ -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 &surr_shapes) const; diff --git a/oh_my_loam/mapper/mapper.cc b/oh_my_loam/mapper/mapper.cc index 8823ae1..f28f465 100644 --- a/oh_my_loam/mapper/mapper.cc +++ b/oh_my_loam/mapper/mapper.cc @@ -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 kdtree_corn; - kdtree_corn.setInputCloud(cloud_corn_map); - pcl::KdTreeFLANN kdtree_surf; - kdtree_surf.setInputCloud(cloud_surf_map); - std::vector pl_pairs; - std::vector 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(); ++i) { + pcl::KdTreeFLANN kdtree_corn; + kdtree_corn.setInputCloud(cloud_corn_map); + pcl::KdTreeFLANN kdtree_surf; + kdtree_surf.setInputCloud(cloud_surf_map); + std::vector pl_pairs; + MatchCorn(kdtree_corn, cloud_corn_map, pose_curr2map, &pl_pairs); + std::vector 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()) { + 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(), 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 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 &kdtree, @@ -173,18 +190,36 @@ void Mapper::AdjustMap(const TPoint ¢er) { } } +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 indices; + map->AddPoints(cloud_trans, &indices); + map->Downsample(indices, config_["downsample_voxel_size"].as()); + }; + std::lock_guard lock(map_mutex_); + update_map(cloud_corn, corn_map_.get()); + update_map(cloud_surf, surf_map_.get()); +} + TPointCloudPtr Mapper::GetMapCloudCorn() const { + std::lock_guard lock(map_mutex_); return corn_map_->GetAllPoints(); } TPointCloudPtr Mapper::GetMapCloudSurf() const { + std::lock_guard 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 lock(map_mutex_); + *map_points += *corn_map_->GetAllPoints(); + *map_points += *surf_map_->GetAllPoints(); return map_points; } diff --git a/oh_my_loam/mapper/mapper.h b/oh_my_loam/mapper/mapper.h index f0aa277..cc4c569 100644 --- a/oh_my_loam/mapper/mapper.h +++ b/oh_my_loam/mapper/mapper.h @@ -56,6 +56,10 @@ class Mapper { void AdjustMap(const TPoint ¢er); + 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 *const pairs) const; @@ -63,22 +67,21 @@ class Mapper { void MatchSurf(const TPointCloudConstPtr &src, const TCTPointCloudConstPtr &tgt, std::vector *const pairs) const; - - YAML::Node config_; - - std::vector map_shape_, submap_shape_; - double map_step_; + // map + mutable std::mutex map_mutex_; std::unique_ptr corn_map_; std::unique_ptr surf_map_; - + // state mutable std::mutex state_mutex_; State state_ = UN_INIT; common::Pose3d pose_odom2map_; - + // thread to run mapping mutable std::unique_ptr thread_{nullptr}; - + // configs + YAML::Node config_; + std::vector map_shape_, submap_shape_; + double map_step_; bool is_vis_ = false; - bool verbose_ = false; DISALLOW_COPY_AND_ASSIGN(Mapper) diff --git a/oh_my_loam/odometer/odometer.cc b/oh_my_loam/odometer/odometer.cc index e1ec3c7..6ff6a53 100644 --- a/oh_my_loam/odometer/odometer.cc +++ b/oh_my_loam/odometer/odometer.cc @@ -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() && config_["vis"].as(); + is_vis_ = config_["vis"].as(); verbose_ = config_["verbose"].as(); 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 &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 &features, } bool is_converge = solver.Solve(config_["solve_iter_num"].as(), 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); diff --git a/oh_my_loam/oh_my_loam.cc b/oh_my_loam/oh_my_loam.cc index 2f36f1a..6492c9b 100644 --- a/oh_my_loam/oh_my_loam.cc +++ b/oh_my_loam/oh_my_loam.cc @@ -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(); 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 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 { diff --git a/oh_my_loam/oh_my_loam.h b/oh_my_loam/oh_my_loam.h index 6620b09..c274a5a 100644 --- a/oh_my_loam/oh_my_loam.h +++ b/oh_my_loam/oh_my_loam.h @@ -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_{nullptr}; @@ -33,11 +37,13 @@ class OhMyLoam { common::PointCloud *const cloud_out) const; std::vector poses_curr2odom_; - std::vector poses_curr2world_; + std::vector poses_curr2map_; YAML::Node config_; bool is_vis_ = false; + std::unique_ptr visualizer_{nullptr}; + DISALLOW_COPY_AND_ASSIGN(OhMyLoam); }; diff --git a/oh_my_loam/visualizer/odometer_visualizer.h b/oh_my_loam/visualizer/odometer_visualizer.h index 148f7d1..1eba9d3 100644 --- a/oh_my_loam/visualizer/odometer_visualizer.h +++ b/oh_my_loam/visualizer/odometer_visualizer.h @@ -38,7 +38,7 @@ class OdometerVisualizer : public common::LidarVisualizer { bool corn_connect_ = false; bool surf_connect_ = false; - std::deque poses_; + std::vector poses_; }; } // namespace oh_my_loam diff --git a/oh_my_loam/visualizer/ohmyloam_visualizer.cc b/oh_my_loam/visualizer/ohmyloam_visualizer.cc index 12a61d4..b45a9df 100644 --- a/oh_my_loam/visualizer/ohmyloam_visualizer.cc +++ b/oh_my_loam/visualizer/ohmyloam_visualizer.cc @@ -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(); - DrawPointCloud(frame.cloud_map_corn, LIGHT_YELLOW, "cloud_map_corn"); - DrawPointCloud(frame.cloud_map_surf, LIGHT_BLUE, "cloud_map_surf"); - - DrawPointCloud(frame.cloud_corn, RED, "cloud_corn"); - DrawPointCloud(frame.cloud_surf, CYAN, "cloud_surf"); - - DrawTrajectory(frame.poses); + DrawPointCloud(frame.cloud_map_corn, GRAY, "cloud_map_corn"); + DrawPointCloud(frame.cloud_map_surf, GRAY, "cloud_map_surf"); + if (!trans_) { + DrawPointCloud(frame.cloud_corn, RED, "cloud_corn"); + DrawPointCloud(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(cloud_corn_trans, RED, "cloud_corn"); + DrawPointCloud(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 &poses) { - std::vector 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(); - Eigen::Vector3f p2 = poses_n[i + 1].t_vec().cast(); - AddLine({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 &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(); + Eigen::Vector3f p2 = poses[i + 1].t_vec().cast(); + AddLine({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 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 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; } } diff --git a/oh_my_loam/visualizer/ohmyloam_visualizer.h b/oh_my_loam/visualizer/ohmyloam_visualizer.h index 34cc0f3..ff778e8 100644 --- a/oh_my_loam/visualizer/ohmyloam_visualizer.h +++ b/oh_my_loam/visualizer/ohmyloam_visualizer.h @@ -11,7 +11,8 @@ struct OhmyloamVisFrame : public common::LidarVisFrame { TPointCloudConstPtr cloud_map_surf; TPointCloudConstPtr cloud_corn; // current TPointCloudConstPtr cloud_surf; // current - std::vector 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 &poses); + void DrawTrajectory(const std::vector &poses, + const std::string &id, const common::Color &color); + + void KeyboardEventCallback( + const pcl::visualization::KeyboardEvent &event) override; + + std::vector poses_odom_; + std::vector poses_map_; + bool trans_ = true; }; } // namespace oh_my_loam