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

fix bug in scanid computing

main
feixyz10 2020-11-20 20:07:41 +08:00 committed by feixyz
parent c7cc12fab2
commit 7584eee1f9
3 changed files with 6 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -1,3 +1,4 @@
/.vscode /.vscode
/.log /.log
*.pcd
/build /build

6
main.cc
View File

@ -1,3 +1,4 @@
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
@ -44,6 +45,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 << "Timestamp = " << LOG_TIMESTAMP(msg->header.stamp.toSec()); double timestamp = msg->header.stamp.toSec();
slam->Run(cloud, 0.0); AINFO << "Timestamp = " << LOG_TIMESTAMP(timestamp);
slam->Run(cloud, timestamp);
} }

10
src/extractor/extractor_VLP16.cc
View File

@ -3,15 +3,7 @@
namespace oh_my_loam { namespace oh_my_loam {
int ExtractorVLP16::GetScanID(const Point& pt) const { int ExtractorVLP16::GetScanID(const Point& pt) const {
double omega = std::atan2(pt.z, Distance(pt)) * 180 * M_1_PI + 15.0; double omega = std::atan2(pt.z, std::hypot(pt.x, pt.y)) * 180 * M_1_PI + 15.0;
// static int i = 0;
// if (i++ < 10) {
// 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;
// }
return static_cast<int>(std::round(omega / 2.0) + 1.e-5); return static_cast<int>(std::round(omega / 2.0) + 1.e-5);
}; };