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.

src/lib/LaserMapping.h → 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 <ros/ros.h>

src/lib/LaserOdometry.h → 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 <ros/node_handle.h>

src/lib/ScanRegistration.h → 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 <loam_velodyne/common.h>
 
 #include <stdint.h>
 #include <vector>
-#include <sensor_msgs/PointCloud2.h>
 #include <ros/node_handle.h>
-#include <pcl_conversions/pcl_conversions.h>
 #include <sensor_msgs/Imu.h>
+#include <pcl/point_cloud.h>
 
 
 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<pcl::PointXYZI>::Ptr& cloud);
-
   /** \brief Publish the current result via the respective topics. */
   void publishResult();
 

src/lib/VelodyneScanRegistration.h → include/loam_velodyne/VelodyneScanRegistration.h

@@ -34,7 +34,9 @@
 #define LOAM_VELODYNESCANREGISTRATION_H
 
 
-#include "ScanRegistration.h"
+#include "loam_velodyne/ScanRegistration.h"
+
+#include <sensor_msgs/PointCloud2.h>
 
 
 namespace loam {

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 <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_conversions/pcl_conversions.h>
 #include <pcl/point_types.h>
 
-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 <typename PointT>
+inline void publishCloudMsg(ros::Publisher& publisher,
+                            const pcl::PointCloud<PointT>& 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

include/loam_velodyne/loam_types.h

@@ -0,0 +1,140 @@
+#ifndef LOAM_LOAM_TYPES_H
+#define LOAM_LOAM_TYPES_H
+
+
+#include <pcl/point_types.h>
+
+
+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<typename OtherDerived>
+  Vector3(const Eigen::MatrixBase <OtherDerived> &other)
+      : Eigen::Vector4f(other) {}
+
+  Vector3(const pcl::PointXYZI &p)
+      : Eigen::Vector4f(p.x, p.y, p.z, 0) {}
+
+  template<typename OtherDerived>
+  Vector3 &operator=(const Eigen::MatrixBase <OtherDerived> &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

src/laser_mapping_node.cpp

@@ -1,5 +1,5 @@
 #include <ros/ros.h>
-#include "lib/LaserMapping.h"
+#include "loam_velodyne/LaserMapping.h"
 
 
 /** Main node entry point. */

src/laser_odometry_node.cpp

@@ -1,5 +1,5 @@
 #include <ros/ros.h>
-#include "lib/LaserOdometry.h"
+#include "loam_velodyne/LaserOdometry.h"
 
 
 /** Main node entry point. */

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})

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 <pcl_conversions/pcl_conversions.h>
 #include <Eigen/Eigenvalues>
 #include <Eigen/QR>
 
@@ -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(),

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 <pcl_conversions/pcl_conversions.h>
 #include <pcl/filters/filter.h>
 #include <Eigen/Eigenvalues>
 #include <Eigen/QR>
@@ -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