From aa9965d0f5b208561b0ef788c4efa6d96e30b3c1 Mon Sep 17 00:00:00 2001 From: Stefan Glaser Date: Sun, 3 Dec 2017 12:50:44 +0100 Subject: [PATCH] Code and project cleanup. * Moved all class header files into include directory. * Moved Vector3, Angle and Twist class definitions from math_utils.h into own loam_types.h in include directory. * Added publish cloud message function to common.h to simplify publishing of cloud messages. * Made all point calculations / transformations in math_utils.h generic with respect to the pcl point type. --- .../loam_velodyne}/CircularBuffer.h | 0 .../loam_velodyne}/LaserMapping.h | 3 +- .../loam_velodyne}/LaserOdometry.h | 3 +- .../loam_velodyne}/ScanRegistration.h | 11 +- .../loam_velodyne}/TransformMaintenance.h | 0 .../loam_velodyne}/VelodyneScanRegistration.h | 4 +- include/loam_velodyne/common.h | 33 +++- include/loam_velodyne/loam_types.h | 140 +++++++++++++++ src/laser_mapping_node.cpp | 2 +- src/laser_odometry_node.cpp | 2 +- src/lib/CMakeLists.txt | 6 - src/lib/LaserMapping.cpp | 23 +-- src/lib/LaserOdometry.cpp | 27 +-- src/lib/ScanRegistration.cpp | 59 ++----- src/lib/TransformMaintenance.cpp | 8 +- src/lib/VelodyneScanRegistration.cpp | 7 +- src/lib/math_utils.h | 162 +++--------------- src/transform_maintenance_node.cpp | 2 +- src/velodyne_scan_registration_node.cpp | 2 +- 19 files changed, 250 insertions(+), 244 deletions(-) rename {src/lib => include/loam_velodyne}/CircularBuffer.h (100%) rename {src/lib => include/loam_velodyne}/LaserMapping.h (99%) rename {src/lib => include/loam_velodyne}/LaserOdometry.h (99%) rename {src/lib => include/loam_velodyne}/ScanRegistration.h (97%) rename {src/lib => include/loam_velodyne}/TransformMaintenance.h (100%) rename {src/lib => include/loam_velodyne}/VelodyneScanRegistration.h (97%) create mode 100644 include/loam_velodyne/loam_types.h diff --git a/src/lib/CircularBuffer.h b/include/loam_velodyne/CircularBuffer.h similarity index 100% rename from src/lib/CircularBuffer.h rename to include/loam_velodyne/CircularBuffer.h diff --git a/src/lib/LaserMapping.h b/include/loam_velodyne/LaserMapping.h similarity index 99% rename from src/lib/LaserMapping.h rename to include/loam_velodyne/LaserMapping.h index e4c404e..d1e8af5 100644 --- a/src/lib/LaserMapping.h +++ b/include/loam_velodyne/LaserMapping.h @@ -33,7 +33,8 @@ #ifndef LOAM_LASERMAPPING_H #define LOAM_LASERMAPPING_H -#include "math_utils.h" + +#include "loam_types.h" #include "CircularBuffer.h" #include diff --git a/src/lib/LaserOdometry.h b/include/loam_velodyne/LaserOdometry.h similarity index 99% rename from src/lib/LaserOdometry.h rename to include/loam_velodyne/LaserOdometry.h index c589525..e77fa15 100644 --- a/src/lib/LaserOdometry.h +++ b/include/loam_velodyne/LaserOdometry.h @@ -33,7 +33,8 @@ #ifndef LOAM_LASERODOMETRY_H #define LOAM_LASERODOMETRY_H -#include "math_utils.h" + +#include "loam_types.h" #include "loam_velodyne/nanoflann_pcl.h" #include diff --git a/src/lib/ScanRegistration.h b/include/loam_velodyne/ScanRegistration.h similarity index 97% rename from src/lib/ScanRegistration.h rename to include/loam_velodyne/ScanRegistration.h index b75ffd2..cb194f9 100644 --- a/src/lib/ScanRegistration.h +++ b/include/loam_velodyne/ScanRegistration.h @@ -34,17 +34,14 @@ #define LOAM_SCANREGISTRATION_H +#include "loam_types.h" #include "CircularBuffer.h" -#include "math_utils.h" - -#include #include #include -#include #include -#include #include +#include namespace loam { @@ -353,10 +350,6 @@ class ScanRegistration { */ void setIMUTrans(const double& sweepDuration); - /** \brief Generate a point cloud message for the specified cloud. */ - void generateROSMsg(sensor_msgs::PointCloud2& msg, - const pcl::PointCloud::Ptr& cloud); - /** \brief Publish the current result via the respective topics. */ void publishResult(); diff --git a/src/lib/TransformMaintenance.h b/include/loam_velodyne/TransformMaintenance.h similarity index 100% rename from src/lib/TransformMaintenance.h rename to include/loam_velodyne/TransformMaintenance.h diff --git a/src/lib/VelodyneScanRegistration.h b/include/loam_velodyne/VelodyneScanRegistration.h similarity index 97% rename from src/lib/VelodyneScanRegistration.h rename to include/loam_velodyne/VelodyneScanRegistration.h index f0852e2..cbe474e 100644 --- a/src/lib/VelodyneScanRegistration.h +++ b/include/loam_velodyne/VelodyneScanRegistration.h @@ -34,7 +34,9 @@ #define LOAM_VELODYNESCANREGISTRATION_H -#include "ScanRegistration.h" +#include "loam_velodyne/ScanRegistration.h" + +#include namespace loam { diff --git a/include/loam_velodyne/common.h b/include/loam_velodyne/common.h index d47348c..e4bfa50 100644 --- a/include/loam_velodyne/common.h +++ b/include/loam_velodyne/common.h @@ -30,6 +30,37 @@ // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. +#ifndef LOAM_COMMON_H +#define LOAM_COMMON_H + +#include +#include +#include #include -typedef pcl::PointXYZI PointType; + +namespace loam { + +/** \brief Construct a new point cloud message from the specified information and publish it via the given publisher. + * + * @tparam PointT the point type + * @param publisher the publisher instance + * @param cloud the cloud to publish + * @param stamp the time stamp of the cloud message + * @param frameID the message frame ID + */ +template +inline void publishCloudMsg(ros::Publisher& publisher, + const pcl::PointCloud& cloud, + const ros::Time& stamp, + std::string frameID) { + sensor_msgs::PointCloud2 msg; + pcl::toROSMsg(cloud, msg); + msg.header.stamp = stamp; + msg.header.frame_id = frameID; + publisher.publish(msg); +} + +} // end namespace loam + +#endif // LOAM_COMMON_H diff --git a/include/loam_velodyne/loam_types.h b/include/loam_velodyne/loam_types.h new file mode 100644 index 0000000..1afee6c --- /dev/null +++ b/include/loam_velodyne/loam_types.h @@ -0,0 +1,140 @@ +#ifndef LOAM_LOAM_TYPES_H +#define LOAM_LOAM_TYPES_H + + +#include + + +namespace loam { + +/** \brief Vector4f decorator for convenient handling. + * + */ +class Vector3 : public Eigen::Vector4f { +public: + Vector3(float x, float y, float z) + : Eigen::Vector4f(x, y, z, 0) {} + + Vector3(void) + : Eigen::Vector4f(0, 0, 0, 0) {} + + template + Vector3(const Eigen::MatrixBase &other) + : Eigen::Vector4f(other) {} + + Vector3(const pcl::PointXYZI &p) + : Eigen::Vector4f(p.x, p.y, p.z, 0) {} + + template + Vector3 &operator=(const Eigen::MatrixBase &rhs) { + this->Eigen::Vector4f::operator=(rhs); + return *this; + } + + Vector3 &operator=(const pcl::PointXYZ &rhs) { + x() = rhs.x; + y() = rhs.y; + z() = rhs.z; + return *this; + } + + Vector3 &operator=(const pcl::PointXYZI &rhs) { + x() = rhs.x; + y() = rhs.y; + z() = rhs.z; + return *this; + } + + float x() const { return (*this)(0); } + + float y() const { return (*this)(1); } + + float z() const { return (*this)(2); } + + float &x() { return (*this)(0); } + + float &y() { return (*this)(1); } + + float &z() { return (*this)(2); } + + // easy conversion + operator pcl::PointXYZI() { + pcl::PointXYZI dst; + dst.x = x(); + dst.y = y(); + dst.z = z(); + dst.intensity = 0; + return dst; + } +}; + + +/** \brief Class for holding an angle. + * + * This class provides buffered access to sine and cosine values to the represented angular value. + */ +class Angle { +public: + Angle() + : _radian(0.0), + _cos(1.0), + _sin(0.0) {} + + Angle(float radValue) + : _radian(radValue), + _cos(std::cos(radValue)), + _sin(std::sin(radValue)) {} + + Angle(const Angle &other) + : _radian(other._radian), + _cos(other._cos), + _sin(other._sin) {} + + void operator=(const Angle &rhs) { + _radian = (rhs._radian); + _cos = (rhs._cos); + _sin = (rhs._sin); + } + + void operator+=(const float &radValue) { *this = (_radian + radValue); } + + void operator+=(const Angle &other) { *this = (_radian + other._radian); } + + void operator-=(const float &radValue) { *this = (_radian - radValue); } + + void operator-=(const Angle &other) { *this = (_radian - other._radian); } + + Angle operator-() const { + Angle out; + out._radian = -_radian; + out._cos = _cos; + out._sin = -(_sin); + return out; + } + + float rad() const { return _radian; } + + float deg() const { return _radian * 180 / M_PI; } + + float cos() const { return _cos; } + + float sin() const { return _sin; } + +private: + float _radian; ///< angle value in radian + float _cos; ///< cosine of the angle + float _sin; ///< sine of the angle +}; + + +/** \brief Twist composed by three angles and a three-dimensional position. */ +struct Twist { + Angle rot_x; + Angle rot_y; + Angle rot_z; + Vector3 pos; +}; + +} // end namespace loam + +#endif //LOAM_LOAM_TYPES_H diff --git a/src/laser_mapping_node.cpp b/src/laser_mapping_node.cpp index 2fcc3f3..5a34dcc 100644 --- a/src/laser_mapping_node.cpp +++ b/src/laser_mapping_node.cpp @@ -1,5 +1,5 @@ #include -#include "lib/LaserMapping.h" +#include "loam_velodyne/LaserMapping.h" /** Main node entry point. */ diff --git a/src/laser_odometry_node.cpp b/src/laser_odometry_node.cpp index b1187bd..e73ff25 100644 --- a/src/laser_odometry_node.cpp +++ b/src/laser_odometry_node.cpp @@ -1,5 +1,5 @@ #include -#include "lib/LaserOdometry.h" +#include "loam_velodyne/LaserOdometry.h" /** Main node entry point. */ diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index a82faa8..da2f148 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -1,14 +1,8 @@ add_library(loam math_utils.h - CircularBuffer.h - ScanRegistration.h ScanRegistration.cpp - VelodyneScanRegistration.h VelodyneScanRegistration.cpp - LaserOdometry.h LaserOdometry.cpp - LaserMapping.h LaserMapping.cpp - TransformMaintenance.h TransformMaintenance.cpp) target_link_libraries(loam ${catkin_LIBRARIES} ${PCL_LIBRARIES}) diff --git a/src/lib/LaserMapping.cpp b/src/lib/LaserMapping.cpp index 3d4d75d..940f528 100644 --- a/src/lib/LaserMapping.cpp +++ b/src/lib/LaserMapping.cpp @@ -30,10 +30,11 @@ // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. -#include "LaserMapping.h" +#include "loam_velodyne/LaserMapping.h" +#include "loam_velodyne/common.h" #include "loam_velodyne/nanoflann_pcl.h" +#include "math_utils.h" -#include #include #include @@ -1036,6 +1037,8 @@ void LaserMapping::optimizeTransformTobeMapped() void LaserMapping::publishResult() { + ros::Time odometryTime = ros::Time().fromSec(_timeLaserOdometry); + // publish new map cloud according to the input output ratio _mapFrameCount++; if (_mapFrameCount >= _mapFrameNum) { @@ -1056,11 +1059,7 @@ void LaserMapping::publishResult() _downSizeFilterCorner.filter(*_laserCloudSurroundDS); // publish new map cloud - sensor_msgs::PointCloud2 laserCloudSurround3; - pcl::toROSMsg(*_laserCloudSurroundDS, laserCloudSurround3); - laserCloudSurround3.header.stamp = ros::Time().fromSec(_timeLaserOdometry); - laserCloudSurround3.header.frame_id = "/camera_init"; - _pubLaserCloudSurround.publish(laserCloudSurround3); + publishCloudMsg(_pubLaserCloudSurround, *_laserCloudSurroundDS, odometryTime, "/camera_init"); } @@ -1071,11 +1070,7 @@ void LaserMapping::publishResult() } // publish transformed full resolution input cloud - sensor_msgs::PointCloud2 laserCloudFullRes3; - pcl::toROSMsg(*_laserCloudFullRes, laserCloudFullRes3); - laserCloudFullRes3.header.stamp = ros::Time().fromSec(_timeLaserOdometry); - laserCloudFullRes3.header.frame_id = "/camera_init"; - _pubLaserCloudFullRes.publish(laserCloudFullRes3); + publishCloudMsg(_pubLaserCloudFullRes, *_laserCloudFullRes, odometryTime, "/camera_init"); // publish odometry after mapped transformations @@ -1084,7 +1079,7 @@ void LaserMapping::publishResult() -_transformAftMapped.rot_x.rad(), -_transformAftMapped.rot_y.rad()); - _odomAftMapped.header.stamp = ros::Time().fromSec(_timeLaserOdometry); + _odomAftMapped.header.stamp = odometryTime; _odomAftMapped.pose.pose.orientation.x = -geoQuat.y; _odomAftMapped.pose.pose.orientation.y = -geoQuat.z; _odomAftMapped.pose.pose.orientation.z = geoQuat.x; @@ -1100,7 +1095,7 @@ void LaserMapping::publishResult() _odomAftMapped.twist.twist.linear.z = _transformBefMapped.pos.z(); _pubOdomAftMapped.publish(_odomAftMapped); - _aftMappedTrans.stamp_ = ros::Time().fromSec(_timeLaserOdometry); + _aftMappedTrans.stamp_ = odometryTime; _aftMappedTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); _aftMappedTrans.setOrigin(tf::Vector3(_transformAftMapped.pos.x(), _transformAftMapped.pos.y(), diff --git a/src/lib/LaserOdometry.cpp b/src/lib/LaserOdometry.cpp index 2455b15..172882a 100644 --- a/src/lib/LaserOdometry.cpp +++ b/src/lib/LaserOdometry.cpp @@ -30,9 +30,10 @@ // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. -#include "LaserOdometry.h" +#include "loam_velodyne/LaserOdometry.h" +#include "loam_velodyne/common.h" +#include "math_utils.h" -#include #include #include #include @@ -846,25 +847,11 @@ void LaserOdometry::publishResult() } ros::Time sweepTime = ros::Time().fromSec(_timeSurfPointsLessFlat); + transformToEnd(_laserCloud); // transform full resolution cloud to sweep end before sending it - sensor_msgs::PointCloud2 laserCloudCornerLast2; - pcl::toROSMsg(*_lastCornerCloud, laserCloudCornerLast2); - laserCloudCornerLast2.header.stamp = sweepTime; - laserCloudCornerLast2.header.frame_id = "/camera"; - _pubLaserCloudCornerLast.publish(laserCloudCornerLast2); - - sensor_msgs::PointCloud2 laserCloudSurfLast2; - pcl::toROSMsg(*_lastSurfaceCloud, laserCloudSurfLast2); - laserCloudSurfLast2.header.stamp = sweepTime; - laserCloudSurfLast2.header.frame_id = "/camera"; - _pubLaserCloudSurfLast.publish(laserCloudSurfLast2); - - sensor_msgs::PointCloud2 laserCloudFullRes3; - transformToEnd(_laserCloud); // transform full resolution cloud before sending it - pcl::toROSMsg(*_laserCloud, laserCloudFullRes3); - laserCloudFullRes3.header.stamp = sweepTime; - laserCloudFullRes3.header.frame_id = "/camera"; - _pubLaserCloudFullRes.publish(laserCloudFullRes3); + publishCloudMsg(_pubLaserCloudCornerLast, *_lastCornerCloud, sweepTime, "/camera"); + publishCloudMsg(_pubLaserCloudSurfLast, *_lastSurfaceCloud, sweepTime, "/camera"); + publishCloudMsg(_pubLaserCloudFullRes, *_laserCloud, sweepTime, "/camera"); } } // end namespace loam diff --git a/src/lib/ScanRegistration.cpp b/src/lib/ScanRegistration.cpp index c70d4ad..166742e 100644 --- a/src/lib/ScanRegistration.cpp +++ b/src/lib/ScanRegistration.cpp @@ -30,7 +30,9 @@ // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. -#include "ScanRegistration.h" +#include "loam_velodyne/ScanRegistration.h" +#include "loam_velodyne/common.h" +#include "math_utils.h" #include #include @@ -408,13 +410,13 @@ void ScanRegistration::setScanBuffersFor(const size_t& startIdx, // mark unreliable points as picked for (size_t i = startIdx + _config.curvatureRegion; i < endIdx - _config.curvatureRegion; i++) { - const PointType& prevPoint = (_laserCloud->points[i - 1]); - const PointType& point = (_laserCloud->points[i]); - const PointType& nextPoint = (_laserCloud->points[i + 1]); + const pcl::PointXYZI& previousPoint = (_laserCloud->points[i - 1]); + const pcl::PointXYZI& point = (_laserCloud->points[i]); + const pcl::PointXYZI& nextPoint = (_laserCloud->points[i + 1]); - float diff = calcSquaredDiff(nextPoint, point); + float diffNext = calcSquaredDiff(nextPoint, point); - if (diff > 0.1) { + if (diffNext > 0.1) { float depth1 = calcPointDistance(point); float depth2 = calcPointDistance(nextPoint); @@ -441,10 +443,10 @@ void ScanRegistration::setScanBuffersFor(const size_t& startIdx, } } - float diff2 = calcSquaredDiff(point, prevPoint); + float diffPrevious = calcSquaredDiff(point, previousPoint); float dis = calcSquaredPointDistance(point); - if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) { + if (diffNext > 0.0002 * dis && diffPrevious > 0.0002 * dis) { _scanNeighborPicked[i - startIdx] = 1; } } @@ -452,46 +454,17 @@ void ScanRegistration::setScanBuffersFor(const size_t& startIdx, -void ScanRegistration::generateROSMsg(sensor_msgs::PointCloud2& msg, - const pcl::PointCloud::Ptr& cloud) -{ - pcl::toROSMsg(*cloud, msg); - msg.header.stamp = _sweepStamp; - msg.header.frame_id = "/camera"; -} - - - void ScanRegistration::publishResult() { // publish full resolution and feature point clouds - sensor_msgs::PointCloud2 laserCloudOutMsg; - generateROSMsg(laserCloudOutMsg, _laserCloud); - _pubLaserCloud.publish(laserCloudOutMsg); - - sensor_msgs::PointCloud2 cornerPointsSharpMsg; - generateROSMsg(cornerPointsSharpMsg, _cornerPointsSharp); - _pubCornerPointsSharp.publish(cornerPointsSharpMsg); - - sensor_msgs::PointCloud2 cornerPointsLessSharpMsg; - generateROSMsg(cornerPointsLessSharpMsg, _cornerPointsLessSharp); - _pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg); - - sensor_msgs::PointCloud2 surfPointsFlat; - generateROSMsg(surfPointsFlat, _surfacePointsFlat); - _pubSurfPointsFlat.publish(surfPointsFlat); - - sensor_msgs::PointCloud2 surfPointsLessFlat; - generateROSMsg(surfPointsLessFlat, _surfacePointsLessFlat); - _pubSurfPointsLessFlat.publish(surfPointsLessFlat); - + publishCloudMsg(_pubLaserCloud, *_laserCloud, _sweepStamp, "/camera"); + publishCloudMsg(_pubCornerPointsSharp, *_cornerPointsSharp, _sweepStamp, "/camera"); + publishCloudMsg(_pubCornerPointsLessSharp, *_cornerPointsLessSharp, _sweepStamp, "/camera"); + publishCloudMsg(_pubSurfPointsFlat, *_surfacePointsFlat, _sweepStamp, "/camera"); + publishCloudMsg(_pubSurfPointsLessFlat, *_surfacePointsLessFlat, _sweepStamp, "/camera"); // publish corresponding IMU transformation information - sensor_msgs::PointCloud2 imuTransMsg; - pcl::toROSMsg(*_imuTrans, imuTransMsg); - imuTransMsg.header.stamp = _sweepStamp; - imuTransMsg.header.frame_id = "/camera"; - _pubImuTrans.publish(imuTransMsg); + publishCloudMsg(_pubImuTrans, *_imuTrans, _sweepStamp, "/camera"); } } // end namespace loam diff --git a/src/lib/TransformMaintenance.cpp b/src/lib/TransformMaintenance.cpp index 277ad5f..12c004d 100644 --- a/src/lib/TransformMaintenance.cpp +++ b/src/lib/TransformMaintenance.cpp @@ -30,11 +30,17 @@ // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. -#include "TransformMaintenance.h" +#include "loam_velodyne/TransformMaintenance.h" namespace loam { +using std::sin; +using std::cos; +using std::asin; +using std::atan2; + + TransformMaintenance::TransformMaintenance() { // initialize odometry and odometry tf messages diff --git a/src/lib/VelodyneScanRegistration.cpp b/src/lib/VelodyneScanRegistration.cpp index 467cd6a..84c457d 100644 --- a/src/lib/VelodyneScanRegistration.cpp +++ b/src/lib/VelodyneScanRegistration.cpp @@ -30,7 +30,10 @@ // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. -#include "VelodyneScanRegistration.h" +#include "loam_velodyne/VelodyneScanRegistration.h" +#include "math_utils.h" + +#include namespace loam { @@ -120,7 +123,7 @@ void VelodyneScanRegistration::process(const pcl::PointCloud& las } // calculate vertical point angle and scan ID - float angle = std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z)) * 180 / float(M_PI); + float angle = rad2deg(std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z))); int roundedAngle = int(angle + (angle < 0.0 ? -0.5 : 0.5)); int scanID = roundedAngle > 0 ? roundedAngle : roundedAngle + (_nScans - 1); if (scanID > (_nScans - 1) || scanID < 0 ){ diff --git a/src/lib/math_utils.h b/src/lib/math_utils.h index 28e26d8..9e3651b 100644 --- a/src/lib/math_utils.h +++ b/src/lib/math_utils.h @@ -1,142 +1,13 @@ #ifndef LOAM_MATH_UTILS_H #define LOAM_MATH_UTILS_H -#include -#include - - -namespace loam { - -/** \brief Vector4f decorator for convenient handling. - * - */ -class Vector3 : public Eigen::Vector4f -{ -public: - Vector3(float x, float y, float z) - : Eigen::Vector4f(x, y, z, 0) {} - - Vector3(void) - : Eigen::Vector4f(0, 0, 0, 0) {} - - template - Vector3(const Eigen::MatrixBase& other) - : Eigen::Vector4f(other) {} - - Vector3(const pcl::PointXYZI& p) - : Eigen::Vector4f(p.x, p.y, p.z, 0) {} - - template - Vector3& operator=(const Eigen::MatrixBase & rhs) { - this->Eigen::Vector4f::operator=(rhs); - return *this; - } - - Vector3& operator=(const pcl::PointXYZ& rhs) { - x() = rhs.x; - y() = rhs.y; - z() = rhs.z; - return *this; - } - - Vector3& operator=(const pcl::PointXYZI& rhs) { - x() = rhs.x; - y() = rhs.y; - z() = rhs.z; - return *this; - } - - float x() const { return (*this)(0); } - float y() const { return (*this)(1); } - float z() const { return (*this)(2); } - - float& x() { return (*this)(0); } - float& y() { return (*this)(1); } - float& z() { return (*this)(2); } - - // easy conversion - operator pcl::PointXYZI() - { - pcl::PointXYZI dst; - dst.x = x(); - dst.y = y(); - dst.z = z(); - dst.intensity = 0; - return dst; - } -}; - - - -/** \brief Class for holding an angle. - * - * This class provides buffered access to sine and cosine values to the represented angular value. - */ -class Angle{ -public: - Angle() - : _radian(0.0), - _cos(1.0), - _sin(0.0) {} - - Angle(float radValue) - : _radian(radValue), - _cos(std::cos(radValue)), - _sin(std::sin(radValue)) {} - - Angle(const Angle& other) - : _radian(other._radian), - _cos(other._cos), - _sin(other._sin) {} - - void operator =(const Angle& rhs){ - _radian = (rhs._radian); - _cos = (rhs._cos); - _sin = (rhs._sin); - } - void operator +=( const float& radValue) { *this = ( _radian + radValue) ; } +#include "loam_velodyne/loam_types.h" - void operator +=( const Angle& other) { *this = ( _radian + other._radian ); } - - void operator -=( const float& radValue) { *this = ( _radian - radValue ); } - - void operator -=( const Angle& other) { *this = ( _radian - other._radian ); } - - Angle operator-() const - { - Angle out; - out._radian = -_radian; - out._cos = _cos; - out._sin = -(_sin); - return out; - } - - float rad() const { return _radian; } - - float deg() const { return _radian * 180 / M_PI; } - - float cos() const { return _cos; } - - float sin() const { return _sin; } - -private: - float _radian; ///< angle value in radian - float _cos; ///< cosine of the angle - float _sin; ///< sine of the angle -}; - - - -/** \brief Twist composed by three angles and a three-dimensional position. */ -struct Twist { - Angle rot_x; - Angle rot_y; - Angle rot_z; - Vector3 pos; -}; +#include +namespace loam { /** \brief Convert the given radian angle to degrees. * @@ -193,7 +64,8 @@ inline float deg2rad(float degrees) * @param b The second point. * @return The squared difference between point a and b. */ -inline float calcSquaredDiff(const pcl::PointXYZI& a, const pcl::PointXYZI& b) +template +inline float calcSquaredDiff(const PointT& a, const PointT& b) { float diffX = a.x - b.x; float diffY = a.y - b.y; @@ -211,7 +83,8 @@ inline float calcSquaredDiff(const pcl::PointXYZI& a, const pcl::PointXYZI& b) * @param wb The weighting factor for the SECOND point. * @return The squared difference between point a and b. */ -inline float calcSquaredDiff(const pcl::PointXYZI& a, const pcl::PointXYZI& b, const float& wb) +template +inline float calcSquaredDiff(const PointT& a, const PointT& b, const float& wb) { float diffX = a.x - b.x * wb; float diffY = a.y - b.y * wb; @@ -226,7 +99,8 @@ inline float calcSquaredDiff(const pcl::PointXYZI& a, const pcl::PointXYZI& b, c * @param p The point. * @return The distance to the point. */ -inline float calcPointDistance(const pcl::PointXYZI& p) +template +inline float calcPointDistance(const PointT& p) { return std::sqrt(p.x * p.x + p.y * p.y + p.z * p.z); } @@ -238,7 +112,8 @@ inline float calcPointDistance(const pcl::PointXYZI& p) * @param p The point. * @return The squared distance to the point. */ -inline float calcSquaredPointDistance(const pcl::PointXYZI& p) +template +inline float calcSquaredPointDistance(const PointT& p) { return p.x * p.x + p.y * p.y + p.z * p.z; } @@ -262,7 +137,8 @@ inline void rotX(Vector3& v, const Angle& ang) * @param p the point to rotate * @param ang the rotation angle */ -inline void rotX(pcl::PointXYZI& p, const Angle& ang) +template +inline void rotX(PointT& p, const Angle& ang) { float y = p.y; p.y = ang.cos() * y - ang.sin() * p.z; @@ -288,7 +164,8 @@ inline void rotY(Vector3& v, const Angle& ang) * @param p the point to rotate * @param ang the rotation angle */ -inline void rotY(pcl::PointXYZI& p, const Angle& ang) +template +inline void rotY(PointT& p, const Angle& ang) { float x = p.x; p.x = ang.cos() * x + ang.sin() * p.z; @@ -314,7 +191,8 @@ inline void rotZ(Vector3& v, const Angle& ang) * @param p the point to rotate * @param ang the rotation angle */ -inline void rotZ(pcl::PointXYZI& p, const Angle& ang) +template +inline void rotZ(PointT& p, const Angle& ang) { float x = p.x; p.x = ang.cos() * x - ang.sin() * p.y; @@ -347,7 +225,8 @@ inline void rotateZXY(Vector3& v, * @param angX the rotation angle around the x-axis * @param angY the rotation angle around the y-axis */ -inline void rotateZXY(pcl::PointXYZI& p, +template +inline void rotateZXY(PointT& p, const Angle& angZ, const Angle& angX, const Angle& angY) @@ -383,7 +262,8 @@ inline void rotateYXZ(Vector3& v, * @param angX the rotation angle around the x-axis * @param angZ the rotation angle around the z-axis */ -inline void rotateYXZ(pcl::PointXYZI& p, +template +inline void rotateYXZ(PointT& p, const Angle& angY, const Angle& angX, const Angle& angZ) diff --git a/src/transform_maintenance_node.cpp b/src/transform_maintenance_node.cpp index 87ab611..2142d71 100644 --- a/src/transform_maintenance_node.cpp +++ b/src/transform_maintenance_node.cpp @@ -1,5 +1,5 @@ #include -#include "lib/TransformMaintenance.h" +#include "loam_velodyne/TransformMaintenance.h" /** Main node entry point. */ diff --git a/src/velodyne_scan_registration_node.cpp b/src/velodyne_scan_registration_node.cpp index 3615ae5..678804c 100644 --- a/src/velodyne_scan_registration_node.cpp +++ b/src/velodyne_scan_registration_node.cpp @@ -1,5 +1,5 @@ #include -#include "lib/VelodyneScanRegistration.h" +#include "loam_velodyne/VelodyneScanRegistration.h" /** Main node entry point. */