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

odometer ok

main
feixyz10 2021-01-25 21:00:10 +08:00 committed by feixyz
parent f47143f01b
commit 42eaae9acd
15 changed files with 229 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
common/color/color.h
View File

@ -12,7 +12,7 @@ struct Color {
#define RED Color(255, 0, 0) #define RED Color(255, 0, 0)
#define GREEN Color(0, 255, 0) #define GREEN Color(0, 255, 0)
#define BLUE Color(0, 0, 255) #define BLUE Color(0, 0, 255)
#define DRAK_GRAY Color(60, 60, 60) #define DRAK_GRAY Color(50, 50, 50)
#define GRAY Color(128, 128, 128) #define GRAY Color(128, 128, 128)
#define CYAN Color(0, 255, 255) #define CYAN Color(0, 255, 255)
#define PURPLE Color(160, 32, 240) #define PURPLE Color(160, 32, 240)

12
common/pcl/pcl_utils.h
View File

@ -5,15 +5,15 @@
namespace common { namespace common {
// Distance squred of a point to origin // Distance squared of a point to origin
template <typename PT> template <typename PT>
inline double DistanceSqure(const PT &pt) { inline double DistanceSquare(const PT &pt) {
return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z; return pt.x * pt.x + pt.y * pt.y + pt.z * pt.z;
} }
// Distance squred of two points // Distance squared of two points
template <typename PT> template <typename PT>
inline double DistanceSqure(const PT &pt1, const PT &pt2) { inline double DistanceSquare(const PT &pt1, const PT &pt2) {
return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y) + return (pt1.x - pt2.x) * (pt1.x - pt2.x) + (pt1.y - pt2.y) * (pt1.y - pt2.y) +
(pt1.z - pt2.z) * (pt1.z - pt2.z); (pt1.z - pt2.z) * (pt1.z - pt2.z);
} }
@ -21,13 +21,13 @@ inline double DistanceSqure(const PT &pt1, const PT &pt2) {
// Distance of a point to origin // Distance of a point to origin
template <typename PT> template <typename PT>
inline double Distance(const PT &pt) { inline double Distance(const PT &pt) {
return std::sqrt(DistanceSqure(pt)); return std::sqrt(DistanceSquare(pt));
} }
// Distance squred of two points // Distance squred of two points
template <typename PT> template <typename PT>
inline double Distance(const PT &pt1, const PT &pt2) { inline double Distance(const PT &pt1, const PT &pt2) {
return std::sqrt(DistanceSqure(pt1, pt2)); return std::sqrt(DistanceSquare(pt1, pt2));
} }
// Check whether is a finite point: neither infinite nor nan // Check whether is a finite point: neither infinite nor nan

15
common/visualizer/lidar_visualizer_utils.cc
View File

@ -1,16 +1,3 @@
#include "lidar_visualizer_utils.h" #include "lidar_visualizer_utils.h"
namespace common { namespace common {} // namespace common
void AddLine(const pcl::PointXYZ &pt1, const pcl::PointXYZ &pt2,
const Color &color, const std::string &id,
PCLVisualizer *const viewer) {
viewer->addLine(pt1, pt2, color.r, color.g, color.b, id);
}
void AddSphere(const pcl::PointXYZ &center, double radius, const Color &color,
const std::string &id, PCLVisualizer *const viewer) {
viewer->addSphere(center, radius, color.r, color.g, color.b, id);
}
} // namespace common

15
common/visualizer/lidar_visualizer_utils.h
View File

@ -37,11 +37,16 @@ void AddPointCloud(const typename pcl::PointCloud<PT>::ConstPtr &cloud,
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id); pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, id);
} }
void AddLine(const pcl::PointXYZ &pt1, const pcl::PointXYZ &pt2, template <typename PT>
const Color &color, const std::string &id, void AddLine(const PT &pt1, const PT &pt2, const Color &color,
PCLVisualizer *const viewer); const std::string &id, PCLVisualizer *const viewer) {
viewer->addLine(pt1, pt2, color.r, color.g, color.b, id);
}
void AddSphere(const pcl::PointXYZ &center, double radius, const Color &color, template <typename PT>
const std::string &id, PCLVisualizer *const viewer); void AddSphere(const PT &center, double radius, const Color &color,
const std::string &id, PCLVisualizer *const viewer) {
viewer->addSphere(center, radius, color.r, color.g, color.b, id);
}
} // namespace common } // namespace common

4
main.cc
View File

@ -48,6 +48,8 @@ void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr &msg,
PointCloudPtr cloud(new PointCloud); PointCloudPtr cloud(new PointCloud);
pcl::fromROSMsg(*msg, *cloud); pcl::fromROSMsg(*msg, *cloud);
double timestamp = msg->header.stamp.toSec(); double timestamp = msg->header.stamp.toSec();
AUSER << "Timestamp: " << FMT_TIMESTAMP(timestamp); static size_t frame_id = 0;
AINFO << "Ohmyloam: frame_id = " << ++frame_id
<< ", timestamp = " << FMT_TIMESTAMP(timestamp);
slam->Run(timestamp, cloud); slam->Run(timestamp, cloud);
} }

15
oh_my_loam/configs/config.yaml
View File

@ -9,22 +9,25 @@ extractor_config:
vis: false vis: false
verbose: false verbose: false
min_point_num: 66 min_point_num: 66
sharp_corner_point_num: 2 scan_seg_num: 36
corner_point_num: 10 sharp_corner_point_num: 1
flat_surf_point_num: 4 corner_point_num: 4
surf_point_num: 20 flat_surf_point_num: 1
corner_point_curvature_th: 0.5 corner_point_curvature_th: 0.5
surf_point_curvature_th: 0.5 surf_point_curvature_th: 0.5
neighbor_point_dist_sq_th: 0.1 neighbor_point_dist_sq_th: 0.1
downsample_voxel_size: 0.6
# configs for odometer # configs for odometer
odometer_config: odometer_config:
vis: true vis: true
verbose: true verbose: false
nearby_scan_num: 1
min_correspondence_num: 10
icp_iter_num: 2 icp_iter_num: 2
solve_iter_num: 4 solve_iter_num: 4
corn_match_dist_sq_th: 1.0 corn_match_dist_sq_th: 1.0
surf_match_dist_sq_th: 1.0 surf_match_dist_sq_th: 16.0
# configs for mapper # configs for mapper
mapper_config: mapper_config:

40
oh_my_loam/extractor/extractor.cc
View File

@ -7,7 +7,6 @@
namespace oh_my_loam { namespace oh_my_loam {
namespace { namespace {
const int kScanSegNum = 6;
const double kTwoPi = 2 * M_PI; const double kTwoPi = 2 * M_PI;
const double kEps = 1e-6; const double kEps = 1e-6;
} // namespace } // namespace
@ -58,18 +57,23 @@ void Extractor::Process(double timestamp,
void Extractor::SplitScan(const common::PointCloud &cloud, void Extractor::SplitScan(const common::PointCloud &cloud,
std::vector<TCTPointCloud> *const scans) const { std::vector<TCTPointCloud> *const scans) const {
scans->resize(num_scans_); scans->resize(num_scans_);
double yaw_start = -std::atan2(cloud.points[0].y, cloud.points[0].x); auto it = std::min_element(cloud.begin(), cloud.begin() + num_scans(),
[](const auto &p1, const auto &p2) {
return -std::atan2(p1.y, p1.x) <
-std::atan2(p2.y, p2.x);
});
double yaw_start = -std::atan2(it->y, it->x);
bool half_passed = false; bool half_passed = false;
for (const auto &pt : cloud) { for (const auto &pt : cloud) {
int scan_id = GetScanID(pt); int scan_id = GetScanID(pt);
if (scan_id >= num_scans_ || scan_id < 0) continue; if (scan_id >= num_scans_ || scan_id < 0) continue;
double yaw = -std::atan2(pt.y, pt.x); double yaw = -std::atan2(pt.y, pt.x);
double yaw_diff = common::NormalizeAngle(yaw - yaw_start); double yaw_diff = common::NormalizeAngle(yaw - yaw_start);
if (yaw_diff > 0) { if (yaw_diff >= -kEps) {
if (half_passed) yaw_start += kTwoPi; if (half_passed) yaw_diff += kTwoPi;
} else { } else {
half_passed = true; half_passed = true;
yaw_start += kTwoPi; yaw_diff += kTwoPi;
} }
double time = std::min(yaw_diff / kTwoPi, 1 - kEps) + scan_id; double time = std::min(yaw_diff / kTwoPi, 1 - kEps) + scan_id;
scans->at(scan_id).push_back( scans->at(scan_id).push_back(
@ -78,7 +82,8 @@ void Extractor::SplitScan(const common::PointCloud &cloud,
} }
void Extractor::ComputeCurvature(TCTPointCloud *const scan) const { void Extractor::ComputeCurvature(TCTPointCloud *const scan) const {
if (scan->size() <= 10 + kScanSegNum) return; size_t seg_num = config_["scan_seg_num"].as<int>();
if (scan->size() <= 10 + seg_num) return;
auto &pts = scan->points; auto &pts = scan->points;
for (size_t i = 5; i < pts.size() - 5; ++i) { for (size_t i = 5; i < pts.size() - 5; ++i) {
float dx = pts[i - 5].x + pts[i - 4].x + pts[i - 3].x + pts[i - 2].x + float dx = pts[i - 5].x + pts[i - 4].x + pts[i - 3].x + pts[i - 2].x +
@ -99,19 +104,21 @@ void Extractor::ComputeCurvature(TCTPointCloud *const scan) const {
void Extractor::AssignType(TCTPointCloud *const scan) const { void Extractor::AssignType(TCTPointCloud *const scan) const {
int pt_num = scan->size(); int pt_num = scan->size();
if (pt_num < kScanSegNum) return; int seg_num = config_["scan_seg_num"].as<int>();
int seg_pt_num = (pt_num - 1) / kScanSegNum + 1; if (pt_num < seg_num) return;
int seg_pt_num = (pt_num - 1) / seg_num + 1;
AERROR << pt_num << " " << seg_num << " " << seg_pt_num;
std::vector<bool> picked(pt_num, false); std::vector<bool> picked(pt_num, false);
std::vector<int> indices = common::Range(pt_num); std::vector<int> indices = common::Range(pt_num);
int sharp_corner_point_num = config_["sharp_corner_point_num"].as<int>(); int sharp_corner_point_num = config_["sharp_corner_point_num"].as<int>();
int corner_point_num = config_["corner_point_num"].as<int>(); int corner_point_num = config_["corner_point_num"].as<int>();
int flat_surf_point_num = config_["flat_surf_point_num"].as<int>(); int flat_surf_point_num = config_["flat_surf_point_num"].as<int>();
int surf_point_num = config_["surf_point_num"].as<int>();
float corner_point_curvature_th = float corner_point_curvature_th =
config_["corner_point_curvature_th"].as<float>(); config_["corner_point_curvature_th"].as<float>();
float surf_point_curvature_th = float surf_point_curvature_th =
config_["surf_point_curvature_th"].as<float>(); config_["surf_point_curvature_th"].as<float>();
for (int seg = 0; seg < kScanSegNum; ++seg) { for (int seg = 0; seg < seg_num; ++seg) {
int b = seg * seg_pt_num; int b = seg * seg_pt_num;
int e = std::min((seg + 1) * seg_pt_num, pt_num); int e = std::min((seg + 1) * seg_pt_num, pt_num);
// sort by curvature for each segment: large -> small // sort by curvature for each segment: large -> small
@ -144,8 +151,6 @@ void Extractor::AssignType(TCTPointCloud *const scan) const {
++surf_point_picked_num; ++surf_point_picked_num;
if (surf_point_picked_num <= flat_surf_point_num) { if (surf_point_picked_num <= flat_surf_point_num) {
scan->at(ix).type = PointType::FLAT_SURF; scan->at(ix).type = PointType::FLAT_SURF;
} else if (surf_point_picked_num <= surf_point_num) {
scan->at(ix).type = PointType::SURF;
} else { } else {
break; break;
} }
@ -174,10 +179,15 @@ void Extractor::GenerateFeature(const TCTPointCloud &scan,
feature->cloud_corner->push_back(point); feature->cloud_corner->push_back(point);
break; break;
default: default:
// feature->cloud_surf->push_back(point); feature->cloud_surf->push_back(point);
break; break;
} }
} }
TPointCloudPtr dowm_sampled(new TPointCloud);
common::VoxelDownSample<TPoint>(
feature->cloud_surf, dowm_sampled.get(),
config_["downsample_voxel_size"].as<double>());
feature->cloud_surf = dowm_sampled;
} }
void Extractor::Visualize(const common::PointCloudConstPtr &cloud, void Extractor::Visualize(const common::PointCloudConstPtr &cloud,
@ -193,12 +203,12 @@ void Extractor::Visualize(const common::PointCloudConstPtr &cloud,
void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix, void Extractor::UpdateNeighborsPicked(const TCTPointCloud &scan, size_t ix,
std::vector<bool> *const picked) const { std::vector<bool> *const picked) const {
auto dist_sq = [&](size_t i, size_t j) -> double { auto dist_sq = [&](size_t i, size_t j) -> double {
return common::DistanceSqure<TCTPoint>(scan[i], scan[j]); return common::DistanceSquare<TCTPoint>(scan[i], scan[j]);
}; };
double neighbor_point_dist_sq_th = double neighbor_point_dist_sq_th =
config_["neighbor_point_dist_sq_th"].as<float>(); config_["neighbor_point_dist_sq_th"].as<float>();
for (size_t i = 1; i <= 5; ++i) { for (size_t i = 1; i <= 5; ++i) {
if (ix < i) break; if (ix - i < 0) break;
if (picked->at(ix - i)) continue; if (picked->at(ix - i)) continue;
if (dist_sq(ix - i, ix - i + 1) > neighbor_point_dist_sq_th) break; if (dist_sq(ix - i, ix - i + 1) > neighbor_point_dist_sq_th) break;
picked->at(ix - i) = true; picked->at(ix - i) = true;

55
oh_my_loam/odometer/odometer.cc
View File

@ -10,11 +10,6 @@
namespace oh_my_loam { namespace oh_my_loam {
namespace {
int kNearScanN = 2;
size_t kMinMatchNum = 10;
} // namespace
bool Odometer::Init() { bool Odometer::Init() {
const auto &config = common::YAMLConfig::Instance()->config(); const auto &config = common::YAMLConfig::Instance()->config();
config_ = config["odometer_config"]; config_ = config["odometer_config"];
@ -33,17 +28,19 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
pose_curr2last_.SetIdentity(); pose_curr2last_.SetIdentity();
pose_curr2world_.SetIdentity(); pose_curr2world_.SetIdentity();
pose_out->SetIdentity(); pose_out->SetIdentity();
AWARN << "Odometer initialized...."; AINFO << "Odometer initialized....";
return; return;
} }
BLOCK_TIMER_START; BLOCK_TIMER_START;
AINFO << "Pose before: " << pose_curr2world_.ToString(); const auto &cloud_corn = kdtree_corn_.getInputCloud();
std::vector<PointLinePair> pl_pairs; const auto &cloud_surf = kdtree_surf_.getInputCloud();
AINFO_IF(verbose_) << "Points to be matched: " << cloud_corn->size() << " + "
<< cloud_surf->size();
std::vector<PointPlanePair> pp_pairs; std::vector<PointPlanePair> pp_pairs;
AINFO_IF(verbose_) << "Points to be matched: " std::vector<PointLinePair> pl_pairs;
<< kdtree_corn_.getInputCloud()->size() << " + "
<< kdtree_surf_.getInputCloud()->size();
for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) { for (int i = 0; i < config_["icp_iter_num"].as<int>(); ++i) {
std::vector<PointLinePair>().swap(pl_pairs);
std::vector<PointPlanePair>().swap(pp_pairs);
for (const auto &feature : features) { for (const auto &feature : features) {
MatchCorn(*feature.cloud_sharp_corner, &pl_pairs); MatchCorn(*feature.cloud_sharp_corner, &pl_pairs);
MatchSurf(*feature.cloud_flat_surf, &pp_pairs); MatchSurf(*feature.cloud_flat_surf, &pp_pairs);
@ -51,7 +48,8 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
AINFO_IF(verbose_) << "Iter " << i AINFO_IF(verbose_) << "Iter " << i
<< ": matched num: point2line = " << pl_pairs.size() << ": matched num: point2line = " << pl_pairs.size()
<< ", point2plane = " << pp_pairs.size(); << ", point2plane = " << pp_pairs.size();
if (pl_pairs.size() + pp_pairs.size() < kMinMatchNum) { if (pl_pairs.size() + pp_pairs.size() <
config_["min_correspondence_num"].as<size_t>()) {
AWARN << "Too less correspondence: " << pl_pairs.size() << " + " AWARN << "Too less correspondence: " << pl_pairs.size() << " + "
<< pp_pairs.size(); << pp_pairs.size();
continue; continue;
@ -63,17 +61,19 @@ void Odometer::Process(double timestamp, const std::vector<Feature> &features,
for (const auto &pair : pp_pairs) { for (const auto &pair : pp_pairs) {
solver.AddPointPlanePair(pair, GetTime(pair.pt)); solver.AddPointPlanePair(pair, GetTime(pair.pt));
} }
solver.Solve(config_["solve_iter_num"].as<int>(), verbose_, bool is_converge = solver.Solve(config_["solve_iter_num"].as<int>(),
&pose_curr2last_); verbose_, &pose_curr2last_);
AINFO << "Odometer::ICP: iter_" << i << ": " << BLOCK_TIMER_STOP_FMT; AWARN_IF(!is_converge) << "Solve: no_convergence";
AINFO_IF(verbose_) << "Odometer::ICP: iter_" << i << ": "
<< BLOCK_TIMER_STOP_FMT;
} }
pose_curr2world_ = pose_curr2world_ * pose_curr2last_; pose_curr2world_ = pose_curr2world_ * pose_curr2last_;
*pose_out = pose_curr2world_; *pose_out = pose_curr2world_;
AINFO << "Pose increase: " << pose_curr2last_.ToString(); AINFO_IF(verbose_) << "Pose increase: " << pose_curr2last_.ToString();
AINFO << "Pose after: " << pose_curr2world_.ToString(); AINFO_IF(verbose_) << "Pose after: " << pose_curr2world_.ToString();
UpdatePre(features); UpdatePre(features);
AINFO << "Odometer::Porcess: " << BLOCK_TIMER_STOP_FMT; AINFO << "Odometer::Porcess: " << BLOCK_TIMER_STOP_FMT;
if (is_vis_) Visualize(features, pl_pairs, pp_pairs); if (is_vis_) Visualize(cloud_corn, cloud_surf, pl_pairs, pp_pairs);
} }
void Odometer::MatchCorn(const TPointCloud &src, void Odometer::MatchCorn(const TPointCloud &src,
@ -90,12 +90,13 @@ void Odometer::MatchCorn(const TPointCloud &src,
TPoint pt1 = kdtree_corn_.getInputCloud()->at(indices[0]); TPoint pt1 = kdtree_corn_.getInputCloud()->at(indices[0]);
int pt1_scan_id = GetScanId(pt1); int pt1_scan_id = GetScanId(pt1);
int nearby_scan_num = config_["nearby_scan_num"].as<int>();
TPoint pt2; TPoint pt2;
bool pt2_fount = false; bool pt2_fount = false;
float min_pt2_dist_sq = dist_sq_thresh; float min_pt2_dist_sq = dist_sq_thresh;
int i_begin = std::max<int>(0, pt1_scan_id - kNearScanN); int i_begin = std::max<int>(0, pt1_scan_id - nearby_scan_num);
int i_end = int i_end = std::min<int>(kdtrees_scan_corn_.size(),
std::min<int>(kdtrees_scan_corn_.size(), pt1_scan_id + kNearScanN + 1); pt1_scan_id + nearby_scan_num + 1);
for (int i = i_begin; i < i_end; ++i) { for (int i = i_begin; i < i_end; ++i) {
if (i == pt1_scan_id) continue; if (i == pt1_scan_id) continue;
const auto &kdtree = kdtrees_scan_corn_[i]; const auto &kdtree = kdtrees_scan_corn_[i];
@ -128,12 +129,13 @@ void Odometer::MatchSurf(const TPointCloud &src,
TPoint pt1 = kdtree_surf_.getInputCloud()->at(indices[0]); TPoint pt1 = kdtree_surf_.getInputCloud()->at(indices[0]);
int pt1_scan_id = GetScanId(pt1); int pt1_scan_id = GetScanId(pt1);
int nearby_scan_num = config_["nearby_scan_num"].as<int>();
TPoint pt2; TPoint pt2;
bool pt2_fount = false; bool pt2_fount = false;
float min_pt2_dist_sq = dist_sq_thresh; float min_pt2_dist_sq = dist_sq_thresh;
int i_begin = std::max<int>(0, pt1_scan_id - kNearScanN); int i_begin = std::max<int>(0, pt1_scan_id - nearby_scan_num);
int i_end = int i_end = std::min<int>(kdtrees_scan_surf_.size(),
std::min<int>(kdtrees_scan_surf_.size(), pt1_scan_id + kNearScanN + 1); pt1_scan_id + nearby_scan_num + 1);
for (int i = i_begin; i < i_end; ++i) { for (int i = i_begin; i < i_end; ++i) {
if (i == pt1_scan_id) continue; if (i == pt1_scan_id) continue;
const auto &kdtree = kdtrees_scan_surf_[i]; const auto &kdtree = kdtrees_scan_surf_[i];
@ -185,12 +187,15 @@ void Odometer::UpdatePre(const std::vector<Feature> &features) {
kdtree_surf_.setInputCloud(surf_pre); kdtree_surf_.setInputCloud(surf_pre);
} }
void Odometer::Visualize(const std::vector<Feature> &features, void Odometer::Visualize(const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf,
const std::vector<PointLinePair> &pl_pairs, const std::vector<PointLinePair> &pl_pairs,
const std::vector<PointPlanePair> &pp_pairs, const std::vector<PointPlanePair> &pp_pairs,
double timestamp) const { double timestamp) const {
std::shared_ptr<OdometerVisFrame> frame(new OdometerVisFrame); std::shared_ptr<OdometerVisFrame> frame(new OdometerVisFrame);
frame->timestamp = timestamp; frame->timestamp = timestamp;
frame->cloud_corn = cloud_corn;
frame->cloud_surf = cloud_surf;
frame->pl_pairs = pl_pairs; frame->pl_pairs = pl_pairs;
frame->pp_pairs = pp_pairs; frame->pp_pairs = pp_pairs;
frame->pose_curr2last = pose_curr2last_; frame->pose_curr2last = pose_curr2last_;

3
oh_my_loam/odometer/odometer.h
View File

@ -27,7 +27,8 @@ class Odometer {
void MatchSurf(const TPointCloud &src, void MatchSurf(const TPointCloud &src,
std::vector<PointPlanePair> *const pairs) const; std::vector<PointPlanePair> *const pairs) const;
void Visualize(const std::vector<Feature> &features, void Visualize(const TPointCloudConstPtr &cloud_corn,
const TPointCloudConstPtr &cloud_surf,
const std::vector<PointLinePair> &pl_pairs, const std::vector<PointLinePair> &pl_pairs,
const std::vector<PointPlanePair> &pp_pairs, const std::vector<PointPlanePair> &pp_pairs,
double timestamp = 0.0) const; double timestamp = 0.0) const;

19
oh_my_loam/oh_my_loam.cc
View File

@ -7,11 +7,10 @@ namespace oh_my_loam {
namespace { namespace {
const double kPointMinDist = 0.1; const double kPointMinDist = 0.1;
using namespace common;
} // namespace } // namespace
bool OhMyLoam::Init() { bool OhMyLoam::Init() {
YAML::Node config = YAMLConfig::Instance()->config(); YAML::Node config = common::YAMLConfig::Instance()->config();
extractor_.reset(new ExtractorVLP16); extractor_.reset(new ExtractorVLP16);
if (!extractor_->Init()) { if (!extractor_->Init()) {
AERROR << "Failed to initialize extractor"; AERROR << "Failed to initialize extractor";
@ -30,8 +29,9 @@ bool OhMyLoam::Init() {
return true; return true;
} }
void OhMyLoam::Run(double timestamp, const PointCloudConstPtr &cloud_in) { void OhMyLoam::Run(double timestamp,
PointCloudPtr cloud(new PointCloud); const common::PointCloudConstPtr &cloud_in) {
common::PointCloudPtr cloud(new common::PointCloud);
RemoveOutliers(*cloud_in, cloud.get()); RemoveOutliers(*cloud_in, cloud.get());
std::vector<Feature> features; std::vector<Feature> features;
extractor_->Process(timestamp, cloud, &features); extractor_->Process(timestamp, cloud, &features);
@ -40,11 +40,12 @@ void OhMyLoam::Run(double timestamp, const PointCloudConstPtr &cloud_in) {
poses_.emplace_back(pose); poses_.emplace_back(pose);
} }
void OhMyLoam::RemoveOutliers(const PointCloud &cloud_in, void OhMyLoam::RemoveOutliers(const common::PointCloud &cloud_in,
PointCloud *const cloud_out) const { common::PointCloud *const cloud_out) const {
RemovePoints<Point>(cloud_in, cloud_out, [&](const Point &pt) { common::RemovePoints<common::Point>(cloud_in, cloud_out, [&](const auto &pt) {
return !IsFinite<Point>(pt) || return !common::IsFinite<common::Point>(pt) ||
DistanceSqure<Point>(pt) < kPointMinDist * kPointMinDist; common::DistanceSquare<common::Point>(pt) <
kPointMinDist * kPointMinDist;
}); });
} }

21
oh_my_loam/solver/cost_function.h
View File

@ -59,10 +59,22 @@ bool PointLineCostFunction::operator()(const T *const r_quat,
Eigen::Matrix<T, 3, 1> p(T(pt.x), T(pt.y), T(pt.z)); Eigen::Matrix<T, 3, 1> p(T(pt.x), T(pt.y), T(pt.z));
Eigen::Matrix<T, 3, 1> p1(T(pt1.x), T(pt1.y), T(pt1.z)); Eigen::Matrix<T, 3, 1> p1(T(pt1.x), T(pt1.y), T(pt1.z));
Eigen::Matrix<T, 3, 1> p2(T(pt2.x), T(pt2.y), T(pt2.z)); Eigen::Matrix<T, 3, 1> p2(T(pt2.x), T(pt2.y), T(pt2.z));
// if constexpr (!std::is_arithmetic<T>::value) {
// AERROR << p.x().a << ", " << p.y().a << ", " << p.z().a;
// AERROR << p1.x().a << ", " << p1.y().a << ", " << p1.z().a;
// AERROR << p2.x().a << ", " << p2.y().a << ", " << p2.z().a;
// }
Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]); Eigen::Quaternion<T> r(r_quat[3], r_quat[0], r_quat[1], r_quat[2]);
Eigen::Quaternion<T> r_interp = Eigen::Quaternion<T> r_interp =
Eigen::Quaternion<T>::Identity().slerp(T(time_), r); Eigen::Quaternion<T>::Identity().slerp(T(time_), r);
// if constexpr (!std::is_arithmetic<T>::value) {
// AERROR << time_;
// AERROR << r.w().a << " " << r.x().a << ", " << r.y().a << ", " <<
// r.z().a; AERROR << r_interp.w().a << " " << r_interp.x().a << ", " <<
// r_interp.y().a
// << ", " << r_interp.z().a;
// }
Eigen::Matrix<T, 3, 1> t(T(time_) * t_vec[0], T(time_) * t_vec[1], Eigen::Matrix<T, 3, 1> t(T(time_) * t_vec[0], T(time_) * t_vec[1],
T(time_) * t_vec[2]); T(time_) * t_vec[2]);
Eigen::Matrix<T, 3, 1> p0 = r_interp * p + t; Eigen::Matrix<T, 3, 1> p0 = r_interp * p + t;
@ -73,9 +85,9 @@ bool PointLineCostFunction::operator()(const T *const r_quat,
residual[0] = area[0] / base_length; residual[0] = area[0] / base_length;
residual[1] = area[1] / base_length; residual[1] = area[1] / base_length;
residual[2] = area[2] / base_length; residual[2] = area[2] / base_length;
if constexpr (!std::is_arithmetic<T>::value) { // if constexpr (!std::is_arithmetic<T>::value) {
AERROR << p.transpose() << ", "; // AERROR << base_length.a;
} // }
return true; return true;
} }
@ -99,9 +111,6 @@ bool PointPlaneCostFunction::operator()(const T *const r_quat,
Eigen::Matrix<T, 3, 1> normal = (p2 - p1).cross(p3 - p1).normalized(); Eigen::Matrix<T, 3, 1> normal = (p2 - p1).cross(p3 - p1).normalized();
residual[0] = (p0 - p1).dot(normal); residual[0] = (p0 - p1).dot(normal);
if constexpr (!std::is_arithmetic<T>::value) {
AERROR << "ppres" << residual[0].a;
}
return true; return true;
} }

2
oh_my_loam/solver/solver.cc
View File

@ -30,8 +30,6 @@ bool PoseSolver::Solve(int max_iter_num, bool verbose,
ceres::Solver::Summary summary; ceres::Solver::Summary summary;
ceres::Solve(options, &problem_, &summary); ceres::Solve(options, &problem_, &summary);
AINFO_IF(verbose) << summary.BriefReport(); AINFO_IF(verbose) << summary.BriefReport();
AWARN_IF(summary.termination_type != ceres::CONVERGENCE)
<< "Solve: no convergence";
if (pose) *pose = common::Pose3d(r_quat_, t_vec_); if (pose) *pose = common::Pose3d(r_quat_, t_vec_);
return summary.termination_type == ceres::CONVERGENCE; return summary.termination_type == ceres::CONVERGENCE;
} }

2
oh_my_loam/visualizer/extractor_visualizer.cc
View File

@ -7,7 +7,7 @@ using namespace common;
void ExtractorVisualizer::Draw() { void ExtractorVisualizer::Draw() {
auto frame = GetCurrentFrame<ExtractorVisFrame>(); auto frame = GetCurrentFrame<ExtractorVisFrame>();
DrawPointCloud<Point>(frame.cloud, GRAY, "cloud_raw"); DrawPointCloud<Point>(frame.cloud, DRAK_GRAY, "cloud_raw");
for (size_t i = 0; i < frame.features.size(); ++i) { for (size_t i = 0; i < frame.features.size(); ++i) {
const auto &feature = frame.features[i]; const auto &feature = frame.features[i];
std::string id_suffix = std::to_string(i); std::string id_suffix = std::to_string(i);

135
oh_my_loam/visualizer/odometer_visualizer.cc
View File

@ -10,49 +10,116 @@ using namespace common;
void OdometerVisualizer::Draw() { void OdometerVisualizer::Draw() {
auto frame = GetCurrentFrame<OdometerVisFrame>(); auto frame = GetCurrentFrame<OdometerVisFrame>();
TPointCloudPtr src_corn(new TPointCloud); DrawPointCloud<TPoint>(frame.cloud_corn, GRAY, "cloud_corn");
TPointCloudPtr tgt_corn(new TPointCloud); DrawPointCloud<TPoint>(frame.cloud_surf, GRAY, "cloud_surf");
src_corn->resize(frame.pl_pairs.size());
tgt_corn->resize(frame.pl_pairs.size() * 2); DrawCorn(frame.pose_curr2last, frame.pl_pairs);
for (size_t i = 0; i < frame.pl_pairs.size(); ++i) { DrawSurf(frame.pose_curr2last, frame.pp_pairs);
const auto &pair = frame.pl_pairs[i];
TransformToStart(frame.pose_curr2last, pair.pt, &src_corn->points[i]); poses_.push_back(frame.pose_curr2world);
tgt_corn->points[2 * i] = pair.line.pt1; DrawTrajectory();
tgt_corn->points[2 * i + 1] = pair.line.pt2; }
void OdometerVisualizer::DrawCorn(const Pose3d &pose,
const std::vector<PointLinePair> &pairs) {
TPointCloudPtr src(new TPointCloud);
TPointCloudPtr tgt(new TPointCloud);
src->resize(pairs.size());
tgt->resize(pairs.size() * 2);
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(2 * i) = pair.line.pt1;
tgt->at(2 * i + 1) = pair.line.pt2;
} }
TPointCloudPtr src_surf(new TPointCloud); DrawPointCloud<TPoint>(tgt, YELLOW, "tgt_corn", 4);
TPointCloudPtr tgt_surf(new TPointCloud); DrawPointCloud<TPoint>(src, RED, "src_corn", 4);
src_surf->resize(frame.pp_pairs.size()); if (!corn_connect_) return;
tgt_surf->resize(frame.pp_pairs.size() * 3); for (size_t i = 0; i < src->size(); ++i) {
for (size_t i = 0; i < frame.pp_pairs.size(); ++i) { const auto &p = src->at(i), &p1 = tgt->at(2 * i), &p2 = tgt->at(2 * i + 1);
const auto &pair = frame.pp_pairs[i]; common::AddLine<TPoint>(p, p1, WHITE, "corn_line1_" + std::to_string(i),
TransformToStart(frame.pose_curr2last, pair.pt, &src_surf->points[i]); viewer_.get());
tgt_surf->points[3 * i] = pair.plane.pt1; common::AddLine<TPoint>(p, p2, WHITE, "corn_line2_" + std::to_string(i),
tgt_surf->points[3 * i + 1] = pair.plane.pt2; viewer_.get());
tgt_surf->points[3 * i + 2] = pair.plane.pt3;
} }
DrawPointCloud<TPoint>(tgt_corn, YELLOW, "tgt_corn", 4); }
DrawPointCloud<TPoint>(src_corn, RED, "src_corn", 4);
DrawPointCloud<TPoint>(tgt_surf, BLUE, "tgt_surf", 4); void OdometerVisualizer::DrawSurf(const Pose3d &pose,
DrawPointCloud<TPoint>(src_surf, CYAN, "src_surf", 4); const std::vector<PointPlanePair> &pairs) {
TPointCloudPtr src(new TPointCloud);
TPointCloudPtr tgt(new TPointCloud);
src->resize(pairs.size());
tgt->resize(pairs.size() * 3);
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;
}
DrawPointCloud<TPoint>(tgt, BLUE, "tgt_surf", 4);
DrawPointCloud<TPoint>(src, CYAN, "src_surf", 4);
if (!surf_connect_) return;
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 OdometerVisualizer::DrawTrajectory() {
std::vector<Pose3d> poses_n; std::vector<Pose3d> poses_n;
poses_n.reserve((poses_.size())); poses_n.reserve((poses_.size()));
Pose3d pose_inv = frame.pose_curr2world.Inv(); Pose3d pose_inv = poses_.back().Inv();
for (const auto &pose : poses_) { for (const auto &pose : poses_) {
poses_n.emplace_back(pose_inv * pose); poses_n.emplace_back(pose_inv * pose);
}; };
for (size_t i = 0; i < poses_n.size(); ++i) { for (size_t i = 0; i < poses_n.size() - 1; ++i) {
Eigen::Vector3f p1 = poses_n[i].t_vec().cast<float>(); Eigen::Vector3f p1 = poses_n[i].t_vec().cast<float>();
if (i < poses_n.size() - 1) { Eigen::Vector3f p2 = poses_n[i + 1].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,
AddLine(Point(p1.x(), p1.y(), p1.z()), Point(p2.x(), p2.y(), p2.z()), "trajectory" + std::to_string(i), viewer_.get());
WHITE, "line" + std::to_string(i), viewer_.get()); }
} else { }
AddLine(Point(p1.x(), p1.y(), p1.z()), Point(0, 0, 0), WHITE,
"line" + std::to_string(i), viewer_.get()); void OdometerVisualizer::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() == "c" && event.keyDown()) {
corn_connect_ = !corn_connect_;
is_updated_ = true;
} else if (event.getKeySym() == "s" && event.keyDown()) {
surf_connect_ = !surf_connect_;
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;
corn_connect_ = surf_connect_ = false;
is_updated_ = true;
} }
poses_.push_back(frame.pose_curr2world);
} }
} // namespace oh_my_loam } // namespace oh_my_loam

17
oh_my_loam/visualizer/odometer_visualizer.h
View File

@ -6,8 +6,8 @@
namespace oh_my_loam { namespace oh_my_loam {
struct OdometerVisFrame : public common::LidarVisFrame { struct OdometerVisFrame : public common::LidarVisFrame {
TPointCloudPtr cloud_surf; TPointCloudConstPtr cloud_surf;
TPointCloudPtr cloud_corner; TPointCloudConstPtr cloud_corn;
std::vector<PointLinePair> pl_pairs; std::vector<PointLinePair> pl_pairs;
std::vector<PointPlanePair> pp_pairs; std::vector<PointPlanePair> pp_pairs;
common::Pose3d pose_curr2last; common::Pose3d pose_curr2last;
@ -23,6 +23,19 @@ class OdometerVisualizer : public common::LidarVisualizer {
private: private:
void Draw() override; void Draw() override;
void DrawCorn(const Pose3d &pose, const std::vector<PointLinePair> &pairs);
void DrawSurf(const Pose3d &pose, const std::vector<PointPlanePair> &pairs);
void DrawTrajectory();
void KeyboardEventCallback(
const pcl::visualization::KeyboardEvent &event) override;
bool trans_ = true;
bool corn_connect_ = false;
bool surf_connect_ = false;
std::deque<common::Pose3d> poses_; std::deque<common::Pose3d> poses_;
}; };