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

writing mapper ...

main
feixyz10 2021-01-30 01:19:14 +08:00 committed by feixyz
parent d0fc4f2322
commit 682d8426c1
9 changed files with 174 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
common/geometry/pose3d.h
View File

@ -26,6 +26,13 @@ class Pose3d {
t_vec_.setZero();
}
const Eigen::Matrix4d TransMat() const {
Eigen::Matrix4d trans_mat = Eigen::Matrix4d::Identity();
trans_mat.topLeftCorner<3, 3>() = r_quat_.toRotationMatrix();
trans_mat.topRightCorner<3, 1>() = t_vec_;
return trans_mat;
}
Pose3d Inv() const {
Eigen::Quaterniond r_inv = r_quat_.inverse();
Eigen::Vector3d t_inv = r_inv * t_vec_;

30
common/pcl/pcl_utils.h
View File

@ -1,5 +1,10 @@
#pragma once
#include <pcl/common/eigen.h>
#include <pcl/common/transforms.h>
#include <pcl/point_cloud.h>
#include "common/geometry/pose3d.h"
#include "common/log/log.h"
#include "pcl_types.h"
@ -36,6 +41,31 @@ inline double IsFinite(const PT &pt) {
return std::isfinite(pt.x) && std::isfinite(pt.y) && std::isfinite(pt.z);
}
template <typename PT>
inline void TransformPoint(const Pose3d &pose, const PT &pt_in,
PT *const pt_out) {
*pt_out = pt_in;
Eigen::Vector3d p = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z);
pt_out->x = p.x();
pt_out->y = p.y();
pt_out->z = p.z();
}
template <typename PT>
inline PT TransformPoint(const Pose3d &pose, const PT &pt_in) {
PT pt_out;
TransformPoint<PT>(pose, pt_in, &pt_out);
return pt_out;
}
template <typename PT>
inline void TransformPointCloud(const Pose3d &pose,
const pcl::PointCloud<PT> &cloud_in,
pcl::PointCloud<PT> *const cloud_out) {
ACHECK(cloud_out);
pcl::transformPointCloud(cloud_in, *cloud_out, pose.TransMat().cast<float>());
}
// Remove point if the condition evaluated to true on it
template <typename PT>
void RemovePoints(const pcl::PointCloud<PT> &cloud_in,

8
oh_my_loam/base/helper.cc
View File

@ -1,11 +1,13 @@
#include "helper.h"
#include "oh_my_loam/base/helper.h"
#include "common/pcl/pcl_utils.h"
namespace oh_my_loam {
void TransformToStart(const Pose3d &pose, const TPoint &pt_in,
TPoint *const pt_out) {
Pose3d pose_interp = Pose3d().Interpolate(pose, GetTime(pt_in));
TransformPoint<TPoint>(pose_interp, pt_in, pt_out);
common::TransformPoint<TPoint>(pose_interp, pt_in, pt_out);
}
TPoint TransformToStart(const Pose3d &pose, const TPoint &pt_in) {
@ -17,7 +19,7 @@ TPoint TransformToStart(const Pose3d &pose, const TPoint &pt_in) {
void TransformToEnd(const Pose3d &pose, const TPoint &pt_in,
TPoint *const pt_out) {
TransformToStart(pose, pt_in, pt_out);
TransformPoint<TPoint>(pose.Inv(), *pt_out, pt_out);
common::TransformPoint<TPoint>(pose.Inv(), *pt_out, pt_out);
}
TPoint TransformToEnd(const Pose3d &pose, const TPoint &pt_in) {

16
oh_my_loam/base/helper.h
View File

@ -15,22 +15,6 @@ inline float GetTime(const TPoint &pt) {
return pt.time - GetScanId(pt);
}
template <typename PT>
void TransformPoint(const Pose3d &pose, const PT &pt_in, PT *const pt_out) {
*pt_out = pt_in;
Eigen::Vector3d p = pose * Eigen::Vector3d(pt_in.x, pt_in.y, pt_in.z);
pt_out->x = p.x();
pt_out->y = p.y();
pt_out->z = p.z();
}
template <typename PT>
PT TransformPoint(const Pose3d &pose, const PT &pt_in) {
PT pt_out;
TransformPoint<PT>(pose, pt_in, &pt_out);
return pt_out;
}
/**
* @brief Transform a lidar point to the start of the scan
*

1
oh_my_loam/configs/config.yaml
View File

@ -35,3 +35,4 @@ mapper_config:
verbose: false
map_shape: [11, 21, 21]
map_step_: 50 # meter
submap_shape: 5

9
oh_my_loam/mapper/map.cc
View File

@ -122,11 +122,16 @@ TPointCloudPtr Map::GetAllPoints() const {
return cloud_all;
}
void Map::AddPoints(const TPointCloudConstPtr &cloud, IndexSet *const indices) {
void Map::AddPoints(const TPointCloudConstPtr &cloud,
std::vector<Index> *const indices) {
std::set<Index, Index::Comp> index_set;
for (const auto &point : *cloud) {
Index index = GetIndex(point);
this->at(index)->push_back(point);
if (indices) indices->insert(index);
if (indices) index_set.insert(index);
}
if (indices) {
for (const auto &index : index_set) indices->push_back(index);
}
}

8
oh_my_loam/mapper/map.h
View File

@ -22,8 +22,6 @@ struct Index {
};
};
using IndexSet = std::set<Index, Index::Comp>;
class Map {
public:
Map(const std::vector<int> &shape, const std::vector<double> &step);
@ -42,6 +40,10 @@ class Map {
void ShiftX(int n);
const std::vector<int> Shape() const {
return std::vector<int>(shape_, shape_ + 3);
}
Index GetIndex(const TPoint &point) const;
TPointCloudPtr GetSurrPoints(const TPoint &point, int n) const;
@ -49,7 +51,7 @@ class Map {
TPointCloudPtr GetAllPoints() const;
void AddPoints(const TPointCloudConstPtr &cloud,
IndexSet *const indices = nullptr);
std::vector<Index> *const indices = nullptr);
void Downsample(double voxel_size);

90
oh_my_loam/mapper/mapper.cc
View File

@ -2,6 +2,13 @@
#include <mutex>
#include "common/log/log.h"
#include "common/pcl/pcl_utils.h"
#include "oh_my_loam/base/helper.h"
#include "oh_my_loam/base/types.h"
#include "oh_my_loam/mapper/map.h"
#include "oh_my_loam/solver/solver.h"
namespace oh_my_loam {
namespace {
@ -15,8 +22,8 @@ bool Mapper::Init() {
verbose_ = config_["vis"].as<bool>();
AINFO << "Mapping visualizer: " << (is_vis_ ? "ON" : "OFF");
std::vector<int> shape = YAMLConfig::GetSeq<int>(config_["map_shape"]);
cloud_corn_map_.reset(new Map(shape, config_["map_step"].as<double>()));
cloud_surf_map_.reset(new Map(shape, config_["map_step"].as<double>()));
corn_map_.reset(new Map(shape, config_["map_step"].as<double>()));
surf_map_.reset(new Map(shape, config_["map_step"].as<double>()));
return true;
}
@ -24,24 +31,83 @@ void Mapper::Reset() {}
void Mapper::Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf,
common::Pose3d *const pose_out) {
const common::Pose3d &pose_curr2odom,
common::Pose3d *const pose_curr2map) {
if (GetState() == UN_INIT) {
cloud_corn_map_->AddPoints(cloud_corn);
cloud_surf_map_->AddPoints(cloud_surf);
pose_out->SetIdentity();
corn_map_->AddPoints(cloud_corn);
surf_map_->AddPoints(cloud_surf);
pose_curr2map->SetIdentity();
pose_odom2map_.SetIdentity();
SetState(DONE);
return;
}
if (GetState() == DONE) {
} else { // RUNNING
thread_.reset(new std::thread(&Mapper::Run, this, cloud_corn, cloud_surf,
pose_curr2odom));
thread_->detach();
}
std::lock_guard<std::mutex> lock(state_mutex_);
*pose_curr2map = pose_odom2map_ * pose_curr2odom;
}
void Mapper::Run(double timestamp, const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf) {
TimePose pose;
pose.timestamp = timestamp;
std::lock_guard<std::mutex> lock(mutex_);
void Mapper::Run(const TPointCloudConstPtr &cloud_corn_in,
const TPointCloudConstPtr &cloud_surf_in,
const Pose3d &pose_curr2odom) {
SetState(RUNNING);
Pose3d pose_curr2map = pose_odom2map_ * pose_curr2odom;
TPoint cnt(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(),
pose_curr2map.t_vec().z());
AdjustMap(corn_map_->GetIndex(cnt));
int submap_shape = config_["submap_shape"].as<int>();
TPointCloudPtr cloud_corn(new TPointCloud);
TPointCloudPtr cloud_surf(new TPointCloud);
TransformPointCloud(pose_curr2map, *cloud_corn_in, cloud_corn.get());
TransformPointCloud(pose_curr2map, *cloud_surf_in, cloud_surf.get());
TPointCloudPtr cloud_corn_map =
corn_map_->GetSurrPoints(cnt, submap_shape / 2);
TPointCloudPtr cloud_surf_map =
corn_map_->GetSurrPoints(cnt, submap_shape / 2);
// MatchCorn();
// MatchSurf();
PoseSolver solver(pose_curr2map);
if (!solver.Solve(5, false, &pose_curr2map)) {
AWARN << "Solve: no_convergence";
}
std::lock_guard<std::mutex> lock(state_mutex_);
pose_odom2map_ = pose_curr2map * pose_curr2odom.Inv();
SetState(DONE);
}
void Mapper::AdjustMap(const Index &index) {
std::vector<int> map_shape = corn_map_->Shape();
int min_idx = config_["submap_shape"].as<int>() / 2 + 1;
int max_idx_z = map_shape[0] - min_idx - 1;
if (index.k < min_idx) {
corn_map_->ShiftZ(index.k - min_idx);
surf_map_->ShiftZ(index.k - min_idx);
}
if (index.k > max_idx_z) {
corn_map_->ShiftZ(index.k - max_idx_z);
surf_map_->ShiftZ(index.k - max_idx_z);
}
int max_idx_y = map_shape[1] - min_idx - 1;
if (index.j < min_idx) {
corn_map_->ShiftY(index.j - min_idx);
surf_map_->ShiftY(index.j - min_idx);
}
if (index.j > max_idx_y) {
corn_map_->ShiftY(index.j - max_idx_y);
surf_map_->ShiftY(index.j - max_idx_y);
}
int max_idx_x = map_shape[2] - min_idx - 1;
if (index.i < min_idx) {
corn_map_->ShiftX(index.i - min_idx);
surf_map_->ShiftX(index.i - min_idx);
}
if (index.i > max_idx_x) {
corn_map_->ShiftX(index.i - max_idx_x);
surf_map_->ShiftX(index.i - max_idx_x);
}
}
void Mapper::Visualize() {}

60
oh_my_loam/mapper/mapper.h
View File

@ -3,9 +3,12 @@
#include <sys/stat.h>
#include <mutex>
#include <shared_mutex>
#include <vector>
#include "common/geometry/pose3d.h"
#include "oh_my_loam/base/helper.h"
#include "oh_my_loam/base/types.h"
#include "oh_my_loam/mapper/map.h"
namespace oh_my_loam {
@ -18,48 +21,67 @@ class Mapper {
void Process(double timestamp, const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf,
common::Pose3d *const pose_out);
const common::Pose3d &pose_curr2odom,
common::Pose3d *const pose_curr2map);
TPointCloudPtr GetCornMapPoints() const {
std::shared_lock<std::shared_mutex> lock(corn_map_mutex_);
return corn_map_->GetAllPoints();
}
TPointCloudPtr GetSurfMapPoints() const {
std::shared_lock<std::shared_mutex> lock(surf_map_mutex_);
return surf_map_->GetAllPoints();
}
TPointCloudPtr GetMapPoints() const {
TPointCloudPtr map_points(new TPointCloud);
*map_points += *GetCornMapPoints();
*map_points += *GetSurfMapPoints();
return map_points;
}
void Reset();
private:
enum State { DONE, RUNNING, UN_INIT };
void Run(double timestamp, const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf);
void Run(const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf, const Pose3d &pose_init);
void TransformUpdate();
void MatchCorn(const TPointCloudConstPtr &src, const TPointCloudConstPtr &tgt,
std::vector<PointLinePair> *const pair) const;
void MapUpdate();
void MatchSurf(const TPointCloudConstPtr &src, const TPointCloudConstPtr &tgt,
std::vector<PointPlanePair> *const pair) const;
State GetState() {
std::lock_guard<std::mutex> lock(mutex_);
State GetState() const {
std::lock_guard<std::mutex> lock(state_mutex_);
return state_;
}
void SetState(State state) {
std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard<std::mutex> lock(state_mutex_);
state_ = state;
}
void AdjustMap(const Index &index);
void Visualize();
YAML::Node config_;
struct TimePose {
double timestamp;
common::Pose3d pose;
};
std::unique_ptr<Map> cloud_corn_map_;
std::unique_ptr<Map> cloud_surf_map_;
std::mutex mutex_;
std::vector<TimePose> poses_;
std::unique_ptr<std::thread> thread_{nullptr};
mutable std::shared_mutex corn_map_mutex_;
mutable std::shared_mutex surf_map_mutex_;
std::unique_ptr<Map> corn_map_;
std::unique_ptr<Map> surf_map_;
mutable std::mutex state_mutex_;
Pose3d pose_odom2map_;
State state_ = UN_INIT;
mutable std::unique_ptr<std::thread> thread_{nullptr};
bool is_vis_ = false;
bool verbose_ = false;