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

minor change

main
feixyz10 2020-10-16 20:49:26 +08:00 committed by feixyz
parent 5830e46fed
commit 93b707c82f
4 changed files with 15 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
common/macros.h
View File

@ -1,8 +1,14 @@
#pragma once #pragma once
#include <iomanip>
#include <memory> #include <memory>
#include <mutex> #include <mutex>
// #define LOG_TIMESTAMP(timestamp, precision)
// std::fixed << std::precision(precision) << timestamp;
#define LOG_TIMESTAMP(timestamp) \
std::fixed << std::setprecision(3) << timestamp;
#define DISALLOW_COPY_AND_ASSIGN(classname) \ #define DISALLOW_COPY_AND_ASSIGN(classname) \
classname(const classname &) = delete; \ classname(const classname &) = delete; \
classname &operator=(const classname &) = delete; classname &operator=(const classname &) = delete;

9
main.cc
View File

@ -28,11 +28,6 @@ int main(int argc, char* argv[]) {
if (!slam.Init()) { if (!slam.Init()) {
AFATAL << "Failed to initilize slam system."; AFATAL << "Failed to initilize slam system.";
} }
ADEBUG << "DEBUG";
AINFO << "INFO";
AWARN << "WARN";
AERROR << "ERROR";
AFATAL << "FATAL";
// ros // ros
ros::init(argc, argv, "oh_my_loam"); ros::init(argc, argv, "oh_my_loam");
@ -49,7 +44,7 @@ void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
OhMyLoam* const slam) { OhMyLoam* const slam) {
PointCloud cloud; PointCloud cloud;
pcl::fromROSMsg(*msg, cloud); pcl::fromROSMsg(*msg, cloud);
AINFO << "Point num = " << cloud.size() ADEBUG << "Point num = " << cloud.size()
<< ", ts = " << msg->header.stamp.toSec(); << ", ts = " << LOG_TIMESTAMP(msg->header.stamp.toSec());
slam->Run(cloud, 0.0); slam->Run(cloud, 0.0);
} }

7
src/feature_points_extractor/base_feature_points_extractor.cc
View File

@ -14,7 +14,7 @@ const int kMinPtsNum = 100;
bool FeaturePointsExtractor::Init(const YAML::Node& config) { bool FeaturePointsExtractor::Init(const YAML::Node& config) {
config_ = config; config_ = config;
is_vis_ = Config::Instance()->Get<bool>("vis") && config_["vis"].as<bool>(); is_vis_ = Config::Instance()->Get<bool>("vis") && config_["vis"].as<bool>();
AINFO << "Feature points visualization: " << (is_vis_ ? "ON" : "OFF"); AINFO << "Feature points extraction visualizer: " << (is_vis_ ? "ON" : "OFF");
if (is_vis_) visualizer_.reset(new FeaturePointsVisualizer); if (is_vis_) visualizer_.reset(new FeaturePointsVisualizer);
return true; return true;
} }
@ -22,10 +22,9 @@ bool FeaturePointsExtractor::Init(const YAML::Node& config) {
void FeaturePointsExtractor::Extract(const PointCloud& cloud_in, void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
FeaturePoints* const feature) { FeaturePoints* const feature) {
PointCloudPtr cloud(new PointCloud); PointCloudPtr cloud(new PointCloud);
AINFO << "BEFORE REMOVE, num = " << cloud_in.size();
RemoveNaNPoint<Point>(cloud_in, cloud.get()); RemoveNaNPoint<Point>(cloud_in, cloud.get());
RemoveClosedPoints<Point>(*cloud, cloud.get(), kPointMinDist); RemoveClosedPoints<Point>(*cloud, cloud.get(), kPointMinDist);
AINFO << "AFTER REMOVE, num = " << cloud->size(); ADEBUG << "AFTER REMOVE, point num = " << cloud->size();
if (cloud->size() < kMinPtsNum) { if (cloud->size() < kMinPtsNum) {
return; return;
} }
@ -41,7 +40,7 @@ void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
oss << scan.size() << " "; oss << scan.size() << " ";
AssignPointType(&scan); AssignPointType(&scan);
} }
AWARN << oss.str(); ADEBUG << oss.str();
for (const auto& scan : scans) { for (const auto& scan : scans) {
*feature->feature_pts += scan; *feature->feature_pts += scan;
for (const auto& pt : scan.points) { for (const auto& pt : scan.points) {

8
src/feature_points_extractor/feature_points_extractor_VLP16.cc
View File

@ -6,10 +6,10 @@ int FeaturePointsExtractorVLP16::GetScanID(const Point& pt) const {
static int i = 0; static int i = 0;
double omega = std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0; double omega = std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0;
if (i++ < 10) { if (i++ < 10) {
AWARN << "OMEGA: " << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0 ADEBUG << "OMEGA: " << std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0
<< " id = " << static_cast<int>(std::round(omega / 2.0) + 0.01) << " id = " << static_cast<int>(std::round(omega / 2.0) + 0.01)
<< " z = " << pt.z << " " << " z = " << pt.z << " "
<< " d = " << Distance(pt) << std::endl; << " d = " << Distance(pt) << std::endl;
} }
return static_cast<int>(std::round(omega / 2.0) + 1.e-5); return static_cast<int>(std::round(omega / 2.0) + 1.e-5);
}; };