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

mapper compile ok

main
feixyz10 2021-02-02 00:26:49 +08:00 committed by feixyz
parent ed9a75b7d6
commit 1b0919d1b7
11 changed files with 50 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
common/math/fitting.h
View File

@ -48,7 +48,7 @@ Eigen::Matrix<double, 6, 1> FitLine3D(const pcl::PointCloud<PT> &cloud,
* b^2 + c^2 = 1)
*/
template <typename PT>
Eigen::Vector3d FitPlane(const pcl::PointCloud<PT> &cloud,
Eigen::Vector4d FitPlane(const pcl::PointCloud<PT> &cloud,
double *const score = nullptr) {
Eigen::MatrixX3f data(cloud.size(), 3);
size_t i = 0;
@ -56,7 +56,7 @@ Eigen::Vector3d FitPlane(const pcl::PointCloud<PT> &cloud,
Eigen::RowVector3f centroid = data.colwise().mean();
Eigen::MatrixX3f data_centered = data.rowwise() - centroid;
Eigen::JacobiSVD<Eigen::MatrixX3f> svd(data_centered, Eigen::ComputeThinV);
Eigen::Vector2f normal = svd.matrixV().col(2);
Eigen::Vector3f normal = svd.matrixV().col(2);
float d = -centroid * normal;
if (score) {
*score = svd.singularValues()[1] * svd.singularValues()[1] /

13
oh_my_loam/base/utils.cc
View File

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

2
oh_my_loam/mapper/map.h
View File

@ -13,6 +13,8 @@ class Grid;
struct Index {
int k, j, i;
Index() = default;
Index(int k, int j, int i) : k(k), j(j), i(i) {}
struct Comp {
bool operator()(const Index &idx1, const Index &idx2) const {

12
oh_my_loam/mapper/mapper.cc
View File

@ -173,18 +173,18 @@ void Mapper::AdjustMap(const TPoint &center) {
}
}
TPointCloudPtr Mapper::GetCornMapPoints() const {
TPointCloudPtr Mapper::GetMapCloudCorn() const {
return corn_map_->GetAllPoints();
}
TPointCloudPtr Mapper::GetSurfMapPoints() const {
TPointCloudPtr Mapper::GetMapCloudSurf() const {
return surf_map_->GetAllPoints();
}
TPointCloudPtr Mapper::GetMapPoints() const {
TPointCloudPtr Mapper::GetMapCloud() const {
TPointCloudPtr map_points(new TPointCloud);
*map_points += *GetCornMapPoints();
*map_points += *GetSurfMapPoints();
*map_points += *GetMapCloudCorn();
*map_points += *GetMapCloudSurf();
return map_points;
}
@ -198,6 +198,4 @@ void Mapper::SetState(State state) {
state_ = state;
}
void Mapper::Visualize() {}
} // namespace oh_my_loam

10
oh_my_loam/mapper/mapper.h
View File

@ -25,11 +25,11 @@ class Mapper {
const common::Pose3d &pose_curr2odom,
common::Pose3d *const pose_curr2map);
TPointCloudPtr GetCornMapPoints() const;
TPointCloudPtr GetMapCloudCorn() const;
TPointCloudPtr GetSurfMapPoints() const;
TPointCloudPtr GetMapCloudSurf() const;
TPointCloudPtr GetMapPoints() const;
TPointCloudPtr GetMapCloud() const;
void Reset();
@ -64,8 +64,6 @@ class Mapper {
const TCTPointCloudConstPtr &tgt,
std::vector<PointLinePair> *const pairs) const;
void Visualize();
YAML::Node config_;
std::vector<int> map_shape_, submap_shape_;
@ -74,8 +72,8 @@ class Mapper {
std::unique_ptr<Map> surf_map_;
mutable std::mutex state_mutex_;
common::Pose3d pose_odom2map_;
State state_ = UN_INIT;
common::Pose3d pose_odom2map_;
mutable std::unique_ptr<std::thread> thread_{nullptr};

2
oh_my_loam/odometer/odometer.cc
View File

@ -23,7 +23,7 @@ void Odometer::Reset() {
}
void Odometer::Process(double timestamp, const std::vector<Feature> &features,
Pose3d *const pose_out) {
common::Pose3d *const pose_out) {
if (!is_initialized_) {
UpdatePre(features);
is_initialized_ = true;

4
oh_my_loam/odometer/odometer.h
View File

@ -18,11 +18,11 @@ class Odometer {
void Process(double timestamp, const std::vector<Feature> &features,
common::Pose3d *const pose_out);
TPointCloudConstPtr cloud_corn() const {
TPointCloudConstPtr GetCloudCorn() const {
return kdtree_corn_.getInputCloud();
}
TPointCloudConstPtr cloud_surf() const {
TPointCloudConstPtr GetCloudSurf() const {
return kdtree_surf_.getInputCloud();
}

43
oh_my_loam/oh_my_loam.cc
View File

@ -2,8 +2,8 @@
#include <vector>
#include "common/common.h"
#include "common/pcl/pcl_utils.h"
#include "oh_my_loam/base/helper.h"
#include "oh_my_loam/extractor/extractor_VLP16.h"
namespace oh_my_loam {
@ -33,6 +33,7 @@ bool OhMyLoam::Init() {
}
void OhMyLoam::Reset() {
AWARN << "OhMySlam RESET";
extractor_->Reset();
odometer_->Reset();
mapper_->Reset();
@ -44,37 +45,27 @@ void OhMyLoam::Run(double timestamp,
RemoveOutliers(*cloud_in, cloud.get());
std::vector<Feature> features;
extractor_->Process(timestamp, cloud, &features);
FusionOdometryMapping();
auto pose_odom =
poses_curr2world_.empty() ? TimePose() : poses_curr2world_.back();
odometer_->Process(timestamp, features, &pose_odom.pose);
poses_curr2world_.push_back(pose_odom);
const auto &cloud_corn = odometer_->cloud_corn();
const auto &cloud_surf = odometer_->cloud_surf();
if (!pose_mapping_updated_) return;
mapping_thread_.reset(new std::thread(&OhMyLoam::MappingProcess, this,
timestamp, cloud_corn, cloud_surf));
if (mapping_thread_->joinable()) mapping_thread_->detach();
}
void OhMyLoam::FusionOdometryMapping() {
std::lock_guard<std::mutex> lock(mutex_);
TimePose pose_m = pose_mapping_;
pose_mapping_updated_ = false;
for (;;) {
}
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();
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);
}
void OhMyLoam::Visualize(double timestamp) {}
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;
});
common::RemovePoints<common::Point>(
cloud_in, cloud_out, [&](const common::Point &pt) {
return !common::IsFinite(pt) ||
common::DistanceSquare(pt) < kPointMinDist * kPointMinDist;
});
}
} // namespace oh_my_loam

6
oh_my_loam/oh_my_loam.h
View File

@ -20,8 +20,6 @@ class OhMyLoam {
private:
void Reset();
void FusionOdometryMapping();
void Visualize(double timestamp = 0.0);
std::unique_ptr<Extractor> extractor_{nullptr};
@ -34,11 +32,13 @@ class OhMyLoam {
void RemoveOutliers(const common::PointCloud &cloud_in,
common::PointCloud *const cloud_out) const;
std::vector<common::Pose3d> poses_curr2odom_;
std::vector<common::Pose3d> poses_curr2world_;
YAML::Node config_;
bool is_vis_ = false;
DISALLOW_COPY_AND_ASSIGN(OhMyLoam)
DISALLOW_COPY_AND_ASSIGN(OhMyLoam);
};
} // namespace oh_my_loam

3
oh_my_loam/solver/cost_function.h
View File

@ -174,7 +174,8 @@ bool PointPlaneCoeffCostFunction::operator()(const T *const r_quat,
}
Eigen::Matrix<T, 3, 1> p0 = r * p + t;
residual[0] = coeff.topRows(3).transpose() * p + coeff[3];
residual[0] = coeff.topRows(3).transpose() * p0;
residual[0] += coeff(3);
return true;
}

8
oh_my_loam/visualizer/odometer_visualizer.h
View File

@ -1,7 +1,7 @@
#pragma once
#include "common/visualizer/lidar_visualizer.h"
#include "oh_my_loam/base/helper.h"
#include "oh_my_loam/solver/cost_function.h"
namespace oh_my_loam {
@ -23,9 +23,11 @@ class OdometerVisualizer : public common::LidarVisualizer {
private:
void Draw() override;
void DrawCorn(const Pose3d &pose, const std::vector<PointLinePair> &pairs);
void DrawCorn(const common::Pose3d &pose,
const std::vector<PointLinePair> &pairs);
void DrawSurf(const Pose3d &pose, const std::vector<PointPlanePair> &pairs);
void DrawSurf(const common::Pose3d &pose,
const std::vector<PointPlanePair> &pairs);
void DrawTrajectory();