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
#include <iomanip>
#include <memory>
#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) \
classname(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()) {
AFATAL << "Failed to initilize slam system.";
}
ADEBUG << "DEBUG";
AINFO << "INFO";
AWARN << "WARN";
AERROR << "ERROR";
AFATAL << "FATAL";
// ros
ros::init(argc, argv, "oh_my_loam");
@ -49,7 +44,7 @@ void PointCloudHandler(const sensor_msgs::PointCloud2ConstPtr& msg,
OhMyLoam* const slam) {
PointCloud cloud;
pcl::fromROSMsg(*msg, cloud);
AINFO << "Point num = " << cloud.size()
<< ", ts = " << msg->header.stamp.toSec();
ADEBUG << "Point num = " << cloud.size()
<< ", ts = " << LOG_TIMESTAMP(msg->header.stamp.toSec());
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) {
config_ = config;
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);
return true;
}
@ -22,10 +22,9 @@ bool FeaturePointsExtractor::Init(const YAML::Node& config) {
void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
FeaturePoints* const feature) {
PointCloudPtr cloud(new PointCloud);
AINFO << "BEFORE REMOVE, num = " << cloud_in.size();
RemoveNaNPoint<Point>(cloud_in, cloud.get());
RemoveClosedPoints<Point>(*cloud, cloud.get(), kPointMinDist);
AINFO << "AFTER REMOVE, num = " << cloud->size();
ADEBUG << "AFTER REMOVE, point num = " << cloud->size();
if (cloud->size() < kMinPtsNum) {
return;
}
@ -41,7 +40,7 @@ void FeaturePointsExtractor::Extract(const PointCloud& cloud_in,
oss << scan.size() << " ";
AssignPointType(&scan);
}
AWARN << oss.str();
ADEBUG << oss.str();
for (const auto& scan : scans) {
*feature->feature_pts += scan;
for (const auto& pt : scan.points) {

2
src/feature_points_extractor/feature_points_extractor_VLP16.cc
View File

@ -6,7 +6,7 @@ int FeaturePointsExtractorVLP16::GetScanID(const Point& pt) const {
static int i = 0;
double omega = std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0;
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)
<< " z = " << pt.z << " "
<< " d = " << Distance(pt) << std::endl;