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

mapper nice

main
feixyz10 2021-02-08 17:09:52 +08:00 committed by feixyz
parent 083eeac5c4
commit f2993b40b8
12 changed files with 191 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
common/geometry/trajectory.h Normal file
View File

@ -0,0 +1,64 @@
#pragma once
#include <memory>
#include "common/geometry/pose3d.h"
namespace common {
class Trajectory {
public:
typedef std::shared_ptr<Trajectory> Ptr;
typedef std::shared_ptr<const Trajectory> ConstPtr;
Trajectory() = default;
explicit Trajectory(const std::vector<Pose3d> &poses) {
poses_ = poses;
}
size_t size() const {
return poses_.size();
}
bool empty() const {
return poses_.empty();
}
const Pose3d &at(size_t i) const {
return poses_.at(i);
}
void AddPose(const Pose3d &pose) {
poses_.push_back(pose);
}
Trajectory Copy(bool last_as_origin = false) const {
Trajectory traj;
if (empty()) return traj;
if (!last_as_origin) return *this;
const Pose3d &last_inv = poses_.back().Inv();
for (const auto &pose : poses_) {
traj.AddPose(last_inv * pose);
}
return traj;
}
std::vector<Eigen::Vector3d> GetPointSeq(bool last_as_origin = false) const {
std::vector<Eigen::Vector3d> point_seq;
if (empty()) return point_seq;
const Pose3d &last_inv = poses_.back().Inv();
for (const auto &pose : poses_) {
if (last_as_origin) {
point_seq.push_back((last_inv * pose).t_vec());
} else {
point_seq.push_back(pose.t_vec());
}
}
return point_seq;
};
private:
std::vector<Pose3d> poses_;
};
} // namespace common

8
common/pcl/pcl_utils.h
View File

@ -98,7 +98,13 @@ void VoxelDownSample(const typename pcl::PointCloud<PT>::ConstPtr &cloud_in,
pcl::VoxelGrid<PT> filter;
filter.setInputCloud(cloud_in);
filter.setLeafSize(voxel_size, voxel_size, voxel_size);
filter.filter(*cloud_out);
if (cloud_out == cloud_in.get()) {
pcl::PointCloud<PT> cloud_downsampled;
filter.filter(cloud_downsampled);
cloud_out->swap(cloud_downsampled);
} else {
filter.filter(*cloud_out);
}
}
} // namespace common

14
common/visualizer/lidar_visualizer_utils.cc
View File

@ -1,3 +1,15 @@
#include "lidar_visualizer_utils.h"
namespace common {} // namespace common
namespace common {
void AddTrajectory(const Trajectory &trajectory, const Color &color,
const std::string &id, PCLVisualizer *const viewer) {
for (size_t i = 0; i < trajectory.size() - 1; ++i) {
Eigen::Vector3f p1 = trajectory.at(i).t_vec().cast<float>();
Eigen::Vector3f p2 = trajectory.at(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);
}
}
} // namespace common

4
common/visualizer/lidar_visualizer_utils.h
View File

@ -8,6 +8,7 @@
#include <string>
#include "common/color/color.h"
#include "common/geometry/trajectory.h"
#include "common/pcl/pcl_types.h"
namespace common {
@ -49,4 +50,7 @@ void AddSphere(const PT &center, double radius, const Color &color,
viewer->addSphere(center, radius, color.r, color.g, color.b, id);
}
void AddTrajectory(const Trajectory &trajectory, const Color &color,
const std::string &id, PCLVisualizer *const viewer);
} // namespace common

42
ggg.txt Normal file
View File

@ -0,0 +1,42 @@
scan line number 16
line resolution 0.200000 plane resolution 0.400000
Mapping 10 Hz
... logging to /home/liufei/.ros/log/8dd723e0-69e0-11eb-931b-4cedfbbfbb36/roslaunch-liufei-fabu-4816.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
]2;/home/liufei/catkin_ws/src/A-LOAM/launch/aloam_velodyne_VLP_16.launch
started roslaunch server http://liufei-fabu:38301/
SUMMARY
========
PARAMETERS
* /mapping_line_resolution: 0.2
* /mapping_plane_resolution: 0.4
* /mapping_skip_frame: 1
* /minimum_range: 0.3
* /rosdistro: kinetic
* /rosversion: 1.12.16
* /scan_line: 16
NODES
/
alaserMapping (aloam_velodyne/alaserMapping)
alaserOdometry (aloam_velodyne/alaserOdometry)
ascanRegistration (aloam_velodyne/ascanRegistration)
rviz (rviz/rviz)
ROS_MASTER_URI=http://localhost:11311
]2;/home/liufei/catkin_ws/src/A-LOAM/launch/aloam_velodyne_VLP_16.launch http://localhost:11311
process[ascanRegistration-1]: started with pid [4854]
process[alaserOdometry-2]: started with pid [4855]
process[alaserMapping-3]: started with pid [4856]
process[rviz-4]: started with pid [4857]
[rviz-4] killing on exit
[alaserMapping-3] killing on exit
[alaserOdometry-2] killing on exit
[ascanRegistration-1] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

4
oh_my_loam/configs/config.yaml
View File

@ -2,7 +2,7 @@
lidar: VPL16
log_to_file: false
log_path: /data/log/oh_my_loam
vis: true
vis: false
# configs for extractor
extractor_config:
@ -31,7 +31,7 @@ odometer_config:
# configs for mapper
mapper_config:
vis: false
vis: true
verbose: false
map_shape: [3, 21, 21]
map_step: 50

50
oh_my_loam/mapper/mapper.cc
View File

@ -26,7 +26,7 @@ bool Mapper::Init() {
map_step_ = config_["map_step"].as<double>();
corn_map_.reset(new Map(map_shape_, map_step_));
surf_map_.reset(new Map(map_shape_, map_step_));
if (is_vis_) visualizer_.reset(MapperVisualizer);
if (is_vis_) visualizer_.reset(new MapperVisualizer);
return true;
}
@ -64,15 +64,18 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
TPoint cnt(pose_curr2map.t_vec().x(), pose_curr2map.t_vec().y(),
pose_curr2map.t_vec().z());
// AdjustMap(cnt);
TPointCloudPtr cloud_corn_map = corn_map_->GetSurrPoints(cnt, submap_shape_);
TPointCloudPtr cloud_surf_map = surf_map_->GetSurrPoints(cnt, submap_shape_);
kdtree_corn_.setInputCloud(cloud_corn_map);
kdtree_surf_.setInputCloud(cloud_surf_map);
// TPointCloudPtr cloud_corn_map = corn_map_->GetSurrPoints(cnt,
// submap_shape_); TPointCloudPtr cloud_surf_map =
// surf_map_->GetSurrPoints(cnt, submap_shape_);
kdtree_corn_.setInputCloud(cloud_corn_map_);
kdtree_surf_.setInputCloud(cloud_surf_map_);
std::vector<PointLineCoeffPair> pl_pairs;
std::vector<PointPlaneCoeffPair> pp_pairs;
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
std::vector<PointLineCoeffPair> pl_pairs;
MatchCorn(cloud_corn_map, pose_curr2map, &pl_pairs);
std::vector<PointPlaneCoeffPair> pp_pairs;
MatchSurf(cloud_surf_map, pose_curr2map, &pp_pairs);
std::vector<PointLineCoeffPair>().swap(pl_pairs);
std::vector<PointPlaneCoeffPair>().swap(pp_pairs);
MatchCorn(cloud_corn, pose_curr2map, &pl_pairs);
MatchSurf(cloud_surf, pose_curr2map, &pp_pairs);
AINFO_IF(verbose_) << "Iter " << i
<< ": matched num: point2line = " << pl_pairs.size()
<< ", point2plane = " << pp_pairs.size();
@ -97,10 +100,11 @@ void Mapper::Run(const TPointCloudConstPtr &cloud_corn,
<< 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();
AUSER << "Mapper::Run: " << BLOCK_TIMER_STOP_FMT;
if (is_vis_) Visualize(pl_pairs, pp_pairs, pose_curr2odom, pose_curr2map);
state_ = DONE; // be careful with deadlock
}
void Mapper::MatchCorn(const TPointCloudConstPtr &cloud_curr,
@ -119,7 +123,7 @@ void Mapper::MatchCorn(const TPointCloudConstPtr &cloud_curr,
}
if (dists.back() > neighbor_point_dist_sq_th) continue;
TPointCloud neighbor_pts;
pcl::copyPointCloud(*cloud_curr, indices, neighbor_pts);
pcl::copyPointCloud(*kdtree_corn_.getInputCloud(), indices, neighbor_pts);
double fit_score = 0.0;
LineCoeff coeff = common::FitLine3D(neighbor_pts, &fit_score);
if (fit_score < config_["min_line_fit_score"].as<double>()) continue;
@ -143,7 +147,7 @@ void Mapper::MatchSurf(const TPointCloudConstPtr &cloud_curr,
}
if (dists.back() > neighbor_point_dist_sq_th) continue;
TPointCloud neighbor_pts;
pcl::copyPointCloud(*cloud_curr, indices, neighbor_pts);
pcl::copyPointCloud(*kdtree_surf_.getInputCloud(), indices, neighbor_pts);
double fit_score = 0.0;
Eigen::Vector4d coeff = common::FitPlane(neighbor_pts, &fit_score);
if (fit_score < config_["min_plane_fit_score"].as<double>()) continue;
@ -189,16 +193,20 @@ 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) {
auto update_map = [&](const TPointCloudConstPtr &cloud,
/*Map*const map*/ const TPointCloudPtr &cloud_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>());
*cloud_map += *cloud_trans;
// std::vector<Index> indices;
// map->AddPoints(cloud_trans, &indices);
// map->Downsample(indices, config_["downsample_voxel_size"].as<double>());
common::VoxelDownSample(cloud_map, cloud_map.get(),
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());
update_map(cloud_corn, cloud_corn_map_);
update_map(cloud_surf, cloud_surf_map_);
}
TPointCloudPtr Mapper::GetMapCloudCorn() const {
@ -237,9 +245,13 @@ void Mapper::Visualize(const std::vector<PointLineCoeffPair> &pl_pairs,
frame->timestamp = timestamp;
frame->cloud_corn = kdtree_corn_.getInputCloud()->makeShared();
frame->cloud_surf = kdtree_surf_.getInputCloud()->makeShared();
AINFO << "Map point num: " << frame->cloud_corn->size() << ", "
<< frame->cloud_surf->size();
frame->pl_pairs = pl_pairs;
frame->pp_pairs = pp_pairs;
frame->pose_curr2world
frame->pose_curr2odom = pose_curr2odom;
frame->pose_curr2map = pose_curr2map;
visualizer_->Render(frame);
}
} // namespace oh_my_loam

2
oh_my_loam/mapper/mapper.h
View File

@ -85,6 +85,8 @@ class Mapper {
bool verbose_ = false;
std::unique_ptr<MapperVisualizer> visualizer_{nullptr};
TPointCloudPtr cloud_corn_map_{new TPointCloud};
TPointCloudPtr cloud_surf_map_{new TPointCloud};
DISALLOW_COPY_AND_ASSIGN(Mapper)
};

4
oh_my_loam/oh_my_loam.cc
View File

@ -50,8 +50,8 @@ void OhMyLoam::Run(double timestamp,
common::Pose3d pose_curr2odom;
odometer_->Process(timestamp, features, &pose_curr2odom);
common::Pose3d pose_curr2map;
const auto &cloud_corn = odometer_->GetCloudCorn();
const auto &cloud_surf = odometer_->GetCloudSurf();
const auto &cloud_corn = odometer_->GetCloudCorn()->makeShared();
const auto &cloud_surf = odometer_->GetCloudSurf()->makeShared();
mapper_->Process(timestamp, cloud_corn, cloud_surf, pose_curr2odom,
&pose_curr2map);
poses_curr2odom_.push_back(pose_curr2odom);

12
oh_my_loam/solver/cost_function.h
View File

@ -7,10 +7,6 @@
namespace oh_my_loam {
namespace {
const double kEpsilon = 1.0e-6;
}
struct PointLinePair {
TPoint pt;
struct Line {
@ -149,7 +145,7 @@ bool PointLineCostFunction::operator()(const T *const r_quat,
Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
if (time_ <= 1.0 - kEpsilon) {
if (time_ <= 1.0 - 1.0e-6) {
r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
t = T(time_) * t;
}
@ -177,7 +173,7 @@ bool PointPlaneCostFunction::operator()(const T *const r_quat,
Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
if (time_ <= 1.0 - kEpsilon) {
if (time_ <= 1.0 - 1.0e-6) {
r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
t = T(time_) * t;
}
@ -199,7 +195,7 @@ bool PointLineCoeffCostFunction::operator()(const T *const r_quat,
Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
if (time_ <= 1.0 - kEpsilon) {
if (time_ <= 1.0 - 1.0e-6) {
r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
t = T(time_) * t;
}
@ -221,7 +217,7 @@ bool PointPlaneCoeffCostFunction::operator()(const T *const r_quat,
Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
Eigen::Matrix<T, 3, 1> t(t_vec[0], t_vec[1], t_vec[2]);
if (time_ <= 1.0 - kEpsilon) {
if (time_ <= 1.0 - 1.0e-6) {
r = Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
t = T(time_) * t;
}

63
oh_my_loam/visualizer/mapper_visualizer.cc
View File

@ -13,78 +13,43 @@ void MapperVisualizer::Draw() {
DrawPointCloud<TPoint>(frame.cloud_corn, GRAY, "cloud_corn");
DrawPointCloud<TPoint>(frame.cloud_surf, GRAY, "cloud_surf");
DrawCorn(frame.pose_curr2odom, frame.pl_pairs);
DrawSurf(frame.pose_curr2odom, frame.pp_pairs);
DrawCorn(frame.pose_curr2odom, frame.pose_curr2map, frame.pl_pairs);
DrawSurf(frame.pose_curr2odom, frame.pose_curr2map, frame.pp_pairs);
poses_.push_back(frame.pose_curr2map);
traj_odom_.AddPose(frame.pose_curr2odom);
traj_map_.AddPose(frame.pose_curr2map);
DrawTrajectory();
}
void MapperVisualizer::DrawCorn(const Pose3d &pose_curr2odom,
const Pose3d &pose_curr2map,
void MapperVisualizer::DrawCorn(const common::Pose3d &pose_odom,
const common::Pose3d &pose_map,
const std::vector<PointLineCoeffPair> &pairs) {
TPointCloudPtr src(new TPointCloud);
TPointCloudPtr tgt(new TPointCloud);
src->resize(pairs.size());
tgt->resize(pairs.size() * 2);
const auto &pose = trans_ ? pose_map : pose_odom;
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]);
TransformToStart(pose, pair.pt, &src->at(i));
}
DrawPointCloud<TPoint>(tgt, YELLOW, "tgt_corn", 4);
DrawPointCloud<TPoint>(src, RED, "src_corn", 4);
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());
}
}
void MapperVisualizer::DrawSurf(const Pose3d &pose_curr2odom,
const Pose3d &pose_curr2map,
void MapperVisualizer::DrawSurf(const common::Pose3d &pose_odom,
const common::Pose3d &pose_map,
const std::vector<PointPlaneCoeffPair> &pairs) {
TPointCloudPtr src(new TPointCloud);
TPointCloudPtr tgt(new TPointCloud);
src->resize(pairs.size());
tgt->resize(pairs.size() * 3);
const auto &pose = trans_ ? pose_map : pose_odom;
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;
TransformToStart(pose, pair.pt, &src->at(i));
}
DrawPointCloud<TPoint>(tgt, BLUE, "tgt_surf", 4);
DrawPointCloud<TPoint>(src, CYAN, "src_surf", 4);
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 MapperVisualizer::DrawTrajectory() {
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());
}
common::AddTrajectory(traj_odom_, PINK, "traj_odmo", viewer_.get());
common::AddTrajectory(traj_map_, PURPLE, "traj_map", viewer_.get());
}
void MapperVisualizer::KeyboardEventCallback(

10
oh_my_loam/visualizer/mapper_visualizer.h
View File

@ -1,13 +1,14 @@
#pragma once
#include "common/geometry/trajectory.h"
#include "common/visualizer/lidar_visualizer.h"
#include "oh_my_loam/solver/cost_function.h"
namespace oh_my_loam {
struct MapperVisFrame : public common::LidarVisFrame {
TPointCloudConstPtr cloud_surf;
TPointCloudConstPtr cloud_corn;
TPointCloudConstPtr cloud_surf;
std::vector<PointLineCoeffPair> pl_pairs;
std::vector<PointPlaneCoeffPair> pp_pairs;
common::Pose3d pose_curr2odom;
@ -23,10 +24,10 @@ class MapperVisualizer : public common::LidarVisualizer {
private:
void Draw() override;
void DrawCorn(const common::Pose3d &pose,
void DrawCorn(const common::Pose3d &pose_odom, const common::Pose3d &pose_map,
const std::vector<PointLineCoeffPair> &pairs);
void DrawSurf(const common::Pose3d &pose,
void DrawSurf(const common::Pose3d &pose_odom, const common::Pose3d &pose_map,
const std::vector<PointPlaneCoeffPair> &pairs);
void DrawTrajectory();
@ -36,7 +37,8 @@ class MapperVisualizer : public common::LidarVisualizer {
bool trans_ = true;
std::vector<common::Pose3d> poses_;
common::Trajectory traj_odom_;
common::Trajectory traj_map_;
};
} // namespace oh_my_loam