├── LOAM-multi-thread
    ├── CMakeLists.txt
    ├── include
    │   └── loam_velodyne
    │   │   ├── Angle.h
    │   │   ├── BasicLaserMapping.h
    │   │   ├── BasicLaserOdometry.h
    │   │   ├── BasicScanRegistration.h
    │   │   ├── BasicTransformMaintenance.h
    │   │   ├── CircularBuffer.h
    │   │   ├── LaserMapping.h
    │   │   ├── LaserOdometry.h
    │   │   ├── MultiScanRegistration.h
    │   │   ├── TransformMaintenance.h
    │   │   ├── Twist.h
    │   │   ├── Vector3.h
    │   │   ├── common.h
    │   │   ├── nanoflann.hpp
    │   │   ├── nanoflann_pcl.h
    │   │   └── time_utils.h
    ├── main.cpp
    └── src
    │   └── lib
    │       ├── BasicLaserMapping.cpp
    │       ├── BasicLaserOdometry.cpp
    │       ├── BasicScanRegistration.cpp
    │       ├── BasicTransformMaintenance.cpp
    │       ├── CMakeLists.txt
    │       ├── LaserMapping.cpp
    │       ├── LaserOdometry.cpp
    │       ├── MultiScanRegistration.cpp
    │       ├── TransformMaintenance.cpp
    │       └── math_utils.h
└── README.md


/LOAM-multi-thread/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(loam_velodyne)
 3 | 
 4 | ## Compile as C++14, supported in ROS Kinetic and newer
 5 | # set_property(TARGET invz_player PROPERTY CXX_STANDARD 17)
 6 | add_compile_options(-std=c++11)
 7 | 
 8 | add_definitions( -march=native )
 9 | 
10 | find_package(Eigen3 REQUIRED)
11 | find_package(PCL REQUIRED)
12 | find_package(Pangolin 0.4 REQUIRED)
13 | 
14 | include_directories( 
15 |     "/usr/local/include/eigen3"
16 |     ${Pangolin_INCLUDE_DIRS}
17 | )
18 | 
19 | include_directories(
20 |   include
21 | 	${PCL_INCLUDE_DIRS}
22 |   "./include")
23 | 
24 | add_subdirectory(src/lib)
25 | 
26 | add_executable(run_loam main.cpp)
27 | target_link_libraries(run_loam ${PCL_LIBRARIES} loam ${Pangolin_LIBRARIES})
28 | 
29 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/include/loam_velodyne/Angle.h:
--------------------------------------------------------------------------------
 1 | #ifndef LOAM_ANGLE_H
 2 | #define LOAM_ANGLE_H
 3 | 
 4 | #define _USE_MATH_DEFINES
 5 | #include <cmath>
 6 | #include <math.h>
 7 | 
 8 | 
 9 | namespace loam {
10 | 
11 | 
12 | /** \brief Class for holding an angle.
13 |  *
14 |  * This class provides buffered access to sine and cosine values to the represented angular value.
15 |  */
16 | class Angle {
17 | public:
18 |   Angle()
19 |       : _radian(0.0),
20 |         _cos(1.0),
21 |         _sin(0.0) {}
22 | 
23 |   Angle(float radValue)
24 |       : _radian(radValue),
25 |         _cos(std::cos(radValue)),
26 |         _sin(std::sin(radValue)) {}
27 | 
28 |   Angle(const Angle &other)
29 |       : _radian(other._radian),
30 |         _cos(other._cos),
31 |         _sin(other._sin) {}
32 | 
33 |   void operator=(const Angle &rhs) {
34 |     _radian = (rhs._radian);
35 |     _cos = (rhs._cos);
36 |     _sin = (rhs._sin);
37 |   }
38 | 
39 |   void operator+=(const float &radValue) { *this = (_radian + radValue); }
40 | 
41 |   void operator+=(const Angle &other) { *this = (_radian + other._radian); }
42 | 
43 |   void operator-=(const float &radValue) { *this = (_radian - radValue); }
44 | 
45 |   void operator-=(const Angle &other) { *this = (_radian - other._radian); }
46 | 
47 |   Angle operator-() const {
48 |     Angle out;
49 |     out._radian = -_radian;
50 |     out._cos = _cos;
51 |     out._sin = -(_sin);
52 |     return out;
53 |   }
54 | 
55 |   float rad() const { return _radian; }
56 | 
57 |   float deg() const { return float(_radian * 180 / M_PI); }
58 | 
59 |   float cos() const { return _cos; }
60 | 
61 |   float sin() const { return _sin; }
62 | 
63 | private:
64 |   float _radian;    ///< angle value in radian
65 |   float _cos;       ///< cosine of the angle
66 |   float _sin;       ///< sine of the angle
67 | };
68 | 
69 | } // end namespace loam
70 | 
71 | #endif //LOAM_ANGLE_H
72 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/include/loam_velodyne/BasicLaserMapping.h:
--------------------------------------------------------------------------------
  1 | #pragma once 
  2 | // Copyright 2013, Ji Zhang, Carnegie Mellon University
  3 | // Further contributions copyright (c) 2016, Southwest Research Institute
  4 | // All rights reserved.
  5 | //
  6 | // Redistribution and use in source and binary forms, with or without
  7 | // modification, are permitted provided that the following conditions are met:
  8 | //
  9 | // 1. Redistributions of source code must retain the above copyright notice,
 10 | //    this list of conditions and the following disclaimer.
 11 | // 2. Redistributions in binary form must reproduce the above copyright notice,
 12 | //    this list of conditions and the following disclaimer in the documentation
 13 | //    and/or other materials provided with the distribution.
 14 | // 3. Neither the name of the copyright holder nor the names of its
 15 | //    contributors may be used to endorse or promote products derived from this
 16 | //    software without specific prior written permission.
 17 | //
 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 28 | // POSSIBILITY OF SUCH DAMAGE.
 29 | //
 30 | // This is an implementation of the algorithm described in the following paper:
 31 | //   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
 32 | //     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
 33 | 
 34 | 
 35 | #include "Twist.h"
 36 | #include "CircularBuffer.h"
 37 | #include "time_utils.h"
 38 | 
 39 | #include <pcl/point_cloud.h>
 40 | #include <pcl/point_types.h>
 41 | #include <pcl/filters/voxel_grid.h>
 42 | 
 43 | extern pcl::VoxelGrid<pcl::PointXYZI> _downSizeFilterCorner;
 44 | extern pcl::VoxelGrid<pcl::PointXYZI> _downSizeFilterSurf;
 45 | namespace loam
 46 | {
 47 | 
 48 | /** IMU state data. */
 49 | typedef struct IMUState2
 50 | {
 51 |    /** The time of the measurement leading to this state (in seconds). */
 52 |    Time stamp;
 53 | 
 54 |    /** The current roll angle. */
 55 |    Angle roll;
 56 | 
 57 |    /** The current pitch angle. */
 58 |    Angle pitch;
 59 | 
 60 |    /** \brief Interpolate between two IMU states.
 61 |     *
 62 |     * @param start the first IMU state
 63 |     * @param end the second IMU state
 64 |     * @param ratio the interpolation ratio
 65 |     * @param result the target IMU state for storing the interpolation result
 66 |     */
 67 |    static void interpolate(const IMUState2& start,
 68 |                            const IMUState2& end,
 69 |                            const float& ratio,
 70 |                            IMUState2& result)
 71 |    {
 72 |       float invRatio = 1 - ratio;
 73 | 
 74 |       result.roll = start.roll.rad() * invRatio + end.roll.rad() * ratio;
 75 |       result.pitch = start.pitch.rad() * invRatio + end.pitch.rad() * ratio;
 76 |    };
 77 | } IMUState2;
 78 | 
 79 | class BasicLaserMapping
 80 | {
 81 | public:
 82 |    explicit BasicLaserMapping(const float& scanPeriod = 0.1, const size_t& maxIterations = 10);
 83 | 
 84 |    /** \brief Try to process buffered data. */
 85 |    bool process(/*Time const& laserOdometryTime*/);
 86 |    void updateIMU(IMUState2 const& newState);
 87 |    void updateOdometry(double pitch, double yaw, double roll, double x, double y, double z);
 88 |    void updateOdometry(Twist const& twist);
 89 | 
 90 |    pcl::PointCloud<pcl::PointXYZI>& laserCloud() { return *_laserCloudFullRes; }
 91 |    pcl::PointCloud<pcl::PointXYZI>& laserCloudCornerLast() { return *_laserCloudCornerLast; }
 92 |    pcl::PointCloud<pcl::PointXYZI>& laserCloudSurfLast() { return *_laserCloudSurfLast; }
 93 | 
 94 |    void setScanPeriod(float val) { _scanPeriod = val; }
 95 |    void setMaxIterations(size_t val) { _maxIterations = val; }
 96 |    void setDeltaTAbort(float val) { _deltaTAbort = val; }
 97 |    void setDeltaRAbort(float val) { _deltaRAbort = val; }
 98 | 
 99 |    pcl::VoxelGrid<pcl::PointXYZI>& downSizeFilterCorner() { return _downSizeFilterCorner; }
100 |    pcl::VoxelGrid<pcl::PointXYZI>& downSizeFilterSurf() { return _downSizeFilterSurf; }
101 | 
102 |    long frameCount()    const { return _frameCount; }
103 |    float scanPeriod()    const { return _scanPeriod; }
104 |    size_t maxIterations() const { return _maxIterations; }
105 |    float deltaTAbort()   const { return _deltaTAbort; }
106 |    float deltaRAbort()   const { return _deltaRAbort; }
107 | 
108 |    Twist const& transformAftMapped()   const { return _transformAftMapped; }
109 |    Twist const& transformBefMapped()   const { return _transformBefMapped; }
110 |    pcl::PointCloud<pcl::PointXYZI> const& laserCloudSurroundDS() const { return *_laserCloudSurroundDS; }
111 | 
112 |    bool hasFreshMap() const { return _downsizedMapCreated; }
113 | 
114 | private:
115 |    /** Run an optimization. */
116 |    void optimizeTransformTobeMapped();
117 | 
118 |    void transformAssociateToMap();
119 |    void transformUpdate();
120 |    void pointAssociateToMap(const pcl::PointXYZI& pi, pcl::PointXYZI& po);
121 |    void pointAssociateTobeMapped(const pcl::PointXYZI& pi, pcl::PointXYZI& po);
122 |    void transformFullResToMap();
123 | 
124 |    bool createDownsizedMap();
125 | 
126 |    // private:
127 |    size_t toIndex(int i, int j, int k) const
128 |    { return i + _laserCloudWidth * j + _laserCloudWidth * _laserCloudHeight * k; }
129 | 
130 | public:
131 |    Time _laserOdometryTime;
132 | 
133 |    float _scanPeriod;          ///< time per scan
134 |    const int _stackFrameNum;
135 |    const int _mapFrameNum;
136 |    long _frameCount;
137 |    long _mapFrameCount;
138 | 
139 |    size_t _maxIterations;  ///< maximum number of iterations
140 |    float _deltaTAbort;     ///< optimization abort threshold for deltaT
141 |    float _deltaRAbort;     ///< optimization abort threshold for deltaR
142 | 
143 |    int _laserCloudCenWidth;
144 |    int _laserCloudCenHeight;
145 |    int _laserCloudCenDepth;
146 |    const size_t _laserCloudWidth;
147 |    const size_t _laserCloudHeight;
148 |    const size_t _laserCloudDepth;
149 |    const size_t _laserCloudNum;
150 | 
151 |    pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloudCornerLast;   ///< last corner points cloud
152 |    pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloudSurfLast;     ///< last surface points cloud
153 |    pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloudFullRes;      ///< last full resolution cloud
154 | 
155 |    pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloudCornerStack;
156 |    pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloudSurfStack;
157 |    pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloudCornerStackDS;  ///< down sampled
158 |    pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloudSurfStackDS;    ///< down sampled
159 | 
160 |    pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloudSurround;
161 |    pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloudSurroundDS;     ///< down sampled
162 |    pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloudCornerFromMap;
163 |    pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloudSurfFromMap;
164 | 
165 |    pcl::PointCloud<pcl::PointXYZI> _laserCloudOri;
166 |    pcl::PointCloud<pcl::PointXYZI> _coeffSel;
167 | 
168 |    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> _laserCloudCornerArray;
169 |    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> _laserCloudSurfArray;
170 |    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> _laserCloudCornerDSArray;  ///< down sampled
171 |    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> _laserCloudSurfDSArray;    ///< down sampled
172 | 
173 |    std::vector<size_t> _laserCloudValidInd;
174 |    std::vector<size_t> _laserCloudSurroundInd;
175 | 
176 |    Twist _transformSum, _transformIncre, _transformTobeMapped, _transformBefMapped, _transformAftMapped;
177 | 
178 |    CircularBuffer<IMUState2> _imuHistory;    ///< history of IMU states
179 | 
180 |    // pcl::VoxelGrid<pcl::PointXYZI> _downSizeFilterCorner;   ///< voxel filter for down sizing corner clouds
181 |    // pcl::VoxelGrid<pcl::PointXYZI> _downSizeFilterSurf;     ///< voxel filter for down sizing surface clouds
182 | 
183 |    bool _downsizedMapCreated = false;
184 | };
185 | 
186 | } // end namespace loam
187 | 
188 | 
189 | 
190 | 
191 | 
192 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/include/loam_velodyne/BasicLaserOdometry.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | #include "Twist.h"
  3 | #include "nanoflann_pcl.h"
  4 | #include <pcl/point_cloud.h>
  5 | #include <pcl/point_types.h>
  6 | 
  7 | namespace loam
  8 | {
  9 | 
 10 |   /** \brief Implementation of the LOAM laser odometry component.
 11 |    *
 12 |    */
 13 |   class BasicLaserOdometry
 14 |   {
 15 |   public:
 16 |     explicit BasicLaserOdometry(float scanPeriod = 0.1, size_t maxIterations = 25);
 17 | 
 18 |     /** \brief Try to process buffered data. */
 19 |     void process();
 20 |     void updateIMU(pcl::PointCloud<pcl::PointXYZ> const& imuTrans);
 21 | 
 22 |     pcl::PointCloud<pcl::PointXYZI>::Ptr& cornerPointsSharp()     { return _cornerPointsSharp; }
 23 |     pcl::PointCloud<pcl::PointXYZI>::Ptr& cornerPointsLessSharp() { return _cornerPointsLessSharp; }
 24 |     pcl::PointCloud<pcl::PointXYZI>::Ptr& surfPointsFlat()        { return _surfPointsFlat; }
 25 |     pcl::PointCloud<pcl::PointXYZI>::Ptr& surfPointsLessFlat()    { return _surfPointsLessFlat; }
 26 |     pcl::PointCloud<pcl::PointXYZI>::Ptr& laserCloud() { return _laserCloud; }
 27 | 
 28 |     Twist const& transformSum() { return _transformSum; }
 29 |     Twist const& transform()    { return _transform;    }
 30 |     pcl::PointCloud<pcl::PointXYZI>::Ptr const& lastCornerCloud () { return _lastCornerCloud ; }
 31 |     pcl::PointCloud<pcl::PointXYZI>::Ptr const& lastSurfaceCloud() { return _lastSurfaceCloud; }
 32 | 
 33 |     void setScanPeriod(float val)     { _scanPeriod    = val; }
 34 |     void setMaxIterations(size_t val) { _maxIterations = val; }
 35 |     void setDeltaTAbort(float val)    { _deltaTAbort = val;   }
 36 |     void setDeltaRAbort(float val)    { _deltaRAbort = val;   }
 37 | 
 38 |     long frameCount()    const { return _frameCount;    }
 39 |     float scanPeriod()    const { return _scanPeriod;    }
 40 |     size_t maxIterations() const { return _maxIterations; }
 41 |     float deltaTAbort()   const { return _deltaTAbort;   }
 42 |     float deltaRAbort()   const { return _deltaRAbort;   }
 43 | 
 44 |     /** \brief Transform the given point cloud to the end of the sweep.
 45 |      *
 46 |      * @param cloud the point cloud to transform
 47 |      */
 48 |     size_t transformToEnd(pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud);
 49 | 
 50 |   private:
 51 |     /** \brief Transform the given point to the start of the sweep.
 52 |      *
 53 |      * @param pi the point to transform
 54 |      * @param po the point instance for storing the result
 55 |      */
 56 |     void transformToStart(const pcl::PointXYZI& pi, pcl::PointXYZI& po);
 57 | 
 58 | 
 59 |     void pluginIMURotation(const Angle& bcx, const Angle& bcy, const Angle& bcz,
 60 |                            const Angle& blx, const Angle& bly, const Angle& blz,
 61 |                            const Angle& alx, const Angle& aly, const Angle& alz,
 62 |                            Angle &acx, Angle &acy, Angle &acz);
 63 | 
 64 |     void accumulateRotation(Angle cx, Angle cy, Angle cz,
 65 |                             Angle lx, Angle ly, Angle lz,
 66 |                             Angle &ox, Angle &oy, Angle &oz);
 67 | 
 68 |   public:
 69 |     float _scanPeriod;       ///< time per scan
 70 |     long _frameCount;        ///< number of processed frames
 71 |     size_t _maxIterations;   ///< maximum number of iterations
 72 |     bool _systemInited;      ///< initialization flag
 73 | 
 74 |     float _deltaTAbort;     ///< optimization abort threshold for deltaT
 75 |     float _deltaRAbort;     ///< optimization abort threshold for deltaR
 76 | 
 77 |     pcl::PointCloud<pcl::PointXYZI>::Ptr _lastCornerCloud;    ///< last corner points cloud
 78 |     pcl::PointCloud<pcl::PointXYZI>::Ptr _lastSurfaceCloud;   ///< last surface points cloud
 79 | 
 80 |     pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloudOri;      ///< point selection
 81 |     pcl::PointCloud<pcl::PointXYZI>::Ptr _coeffSel;           ///< point selection coefficients
 82 | 
 83 |     nanoflann::KdTreeFLANN<pcl::PointXYZI> _lastCornerKDTree;   ///< last corner cloud KD-tree
 84 |     nanoflann::KdTreeFLANN<pcl::PointXYZI> _lastSurfaceKDTree;  ///< last surface cloud KD-tree
 85 | 
 86 |     pcl::PointCloud<pcl::PointXYZI>::Ptr _cornerPointsSharp;      ///< sharp corner points cloud
 87 |     pcl::PointCloud<pcl::PointXYZI>::Ptr _cornerPointsLessSharp;  ///< less sharp corner points cloud
 88 |     pcl::PointCloud<pcl::PointXYZI>::Ptr _surfPointsFlat;         ///< flat surface points cloud
 89 |     pcl::PointCloud<pcl::PointXYZI>::Ptr _surfPointsLessFlat;     ///< less flat surface points cloud
 90 |     pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloud;             ///< full resolution cloud
 91 | 
 92 |     std::vector<int> _pointSearchCornerInd1;    ///< first corner point search index buffer
 93 |     std::vector<int> _pointSearchCornerInd2;    ///< second corner point search index buffer
 94 | 
 95 |     std::vector<int> _pointSearchSurfInd1;    ///< first surface point search index buffer
 96 |     std::vector<int> _pointSearchSurfInd2;    ///< second surface point search index buffer
 97 |     std::vector<int> _pointSearchSurfInd3;    ///< third surface point search index buffer
 98 | 
 99 |     Twist _transform;     ///< optimized pose transformation
100 |     Twist _transformSum;  ///< accumulated optimized pose transformation
101 | 
102 |     Angle _imuRollStart, _imuPitchStart, _imuYawStart;
103 |     Angle _imuRollEnd, _imuPitchEnd, _imuYawEnd;
104 | 
105 |     Vector3 _imuShiftFromStart;
106 |     Vector3 _imuVeloFromStart;
107 |   };
108 | 
109 | } // end namespace loam
110 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/include/loam_velodyne/BasicScanRegistration.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include <utility>
  4 | #include <vector>
  5 | #include <iostream>
  6 | #include <pcl/point_cloud.h>
  7 | 
  8 | #include "Angle.h"
  9 | #include "Vector3.h"
 10 | #include "CircularBuffer.h"
 11 | #include "time_utils.h"
 12 | 
 13 | using namespace std;
 14 | namespace loam
 15 | {
 16 | 
 17 |   /** \brief A pair describing the start end end index of a range. */
 18 |   typedef std::pair<size_t, size_t> IndexRange;
 19 | 
 20 | 
 21 | 
 22 |   /** Point label options. */
 23 |   enum PointLabel
 24 |   {
 25 |     CORNER_SHARP = 2,       ///< sharp corner point
 26 |     CORNER_LESS_SHARP = 1,  ///< less sharp corner point
 27 |     SURFACE_LESS_FLAT = 0,  ///< less flat surface point
 28 |     SURFACE_FLAT = -1       ///< flat surface point
 29 |   };
 30 | 
 31 | 
 32 |   /** Scan Registration configuration parameters. */
 33 |   class RegistrationParams
 34 |   {
 35 |   public:
 36 |     RegistrationParams(const float& scanPeriod_ = 0.1,
 37 |       const int& imuHistorySize_ = 200,
 38 |       const int& nFeatureRegions_ = 6,
 39 |       const int& curvatureRegion_ = 5,
 40 |       const int& maxCornerSharp_ = 2,
 41 |       const int& maxSurfaceFlat_ = 4,
 42 |       const float& lessFlatFilterSize_ = 0.2,
 43 |       const float& surfaceCurvatureThreshold_ = 0.1);
 44 | 
 45 |     /** The time per scan. */
 46 |     float scanPeriod;
 47 | 
 48 |     /** The size of the IMU history state buffer. */
 49 |     int imuHistorySize;
 50 | 
 51 |     /** The number of (equally sized) regions used to distribute the feature extraction within a scan. */
 52 |     int nFeatureRegions;
 53 | 
 54 |     /** The number of surrounding points (+/- region around a point) used to calculate a point curvature. */
 55 |     int curvatureRegion;
 56 | 
 57 |     /** The maximum number of sharp corner points per feature region. */
 58 |     int maxCornerSharp;
 59 | 
 60 |     /** The maximum number of less sharp corner points per feature region. */
 61 |     int maxCornerLessSharp;
 62 | 
 63 |     /** The maximum number of flat surface points per feature region. */
 64 |     int maxSurfaceFlat;
 65 | 
 66 |     /** The voxel size used for down sizing the remaining less flat surface points. */
 67 |     float lessFlatFilterSize;
 68 | 
 69 |     /** The curvature threshold below / above a point is considered a flat / corner point. */
 70 |     float surfaceCurvatureThreshold;
 71 |   };
 72 | 
 73 | 
 74 | 
 75 |   /** IMU state data. */
 76 |   typedef struct IMUState
 77 |   {
 78 |     /** The time of the measurement leading to this state (in seconds). */
 79 |     Time stamp;
 80 | 
 81 |     /** The current roll angle. */
 82 |     Angle roll;
 83 | 
 84 |     /** The current pitch angle. */
 85 |     Angle pitch;
 86 | 
 87 |     /** The current yaw angle. */
 88 |     Angle yaw;
 89 | 
 90 |     /** The accumulated global IMU position in 3D space. */
 91 |     Vector3 position;
 92 | 
 93 |     /** The accumulated global IMU velocity in 3D space. */
 94 |     Vector3 velocity;
 95 | 
 96 |     /** The current (local) IMU acceleration in 3D space. */
 97 |     Vector3 acceleration;
 98 | 
 99 |     /** \brief Interpolate between two IMU states.
100 |     *
101 |     * @param start the first IMUState
102 |     * @param end the second IMUState
103 |     * @param ratio the interpolation ratio
104 |     * @param result the target IMUState for storing the interpolation result
105 |     */
106 |     static void interpolate(const IMUState& start,
107 |       const IMUState& end,
108 |       const float& ratio,
109 |       IMUState& result)
110 |     {
111 |       float invRatio = 1 - ratio;
112 | 
113 |       result.roll = start.roll.rad() * invRatio + end.roll.rad() * ratio;
114 |       result.pitch = start.pitch.rad() * invRatio + end.pitch.rad() * ratio;
115 |       if (start.yaw.rad() - end.yaw.rad() > M_PI)
116 |       {
117 |         result.yaw = start.yaw.rad() * invRatio + (end.yaw.rad() + 2 * M_PI) * ratio;
118 |       }
119 |       else if (start.yaw.rad() - end.yaw.rad() < -M_PI)
120 |       {
121 |         result.yaw = start.yaw.rad() * invRatio + (end.yaw.rad() - 2 * M_PI) * ratio;
122 |       }
123 |       else
124 |       {
125 |         result.yaw = start.yaw.rad() * invRatio + end.yaw.rad() * ratio;
126 |       }
127 | 
128 |       result.velocity = start.velocity * invRatio + end.velocity * ratio;
129 |       result.position = start.position * invRatio + end.position * ratio;
130 |     };
131 |   } IMUState;
132 | 
133 | 
134 |   class BasicScanRegistration
135 |   {
136 |   public:
137 |     /** \brief Process a new cloud as a set of scanlines.
138 |     *
139 |     * @param relTime the time relative to the scan time
140 |     */
141 |     void processScanlines(const Time& scanTime, std::vector<pcl::PointCloud<pcl::PointXYZI>> const& laserCloudScans);
142 | 
143 |     void processScanlines(std::vector<pcl::PointCloud<pcl::PointXYZI>> const& laserCloudScans);
144 | 
145 |     bool configure(const RegistrationParams& config = RegistrationParams()); 
146 | 
147 |     /** \brief Update new IMU state. NOTE: MUTATES ARGS! */
148 |     void updateIMUData(Vector3& acc, IMUState& newState);
149 | 
150 |     /** \brief Project a point to the start of the sweep using corresponding IMU data
151 |     *
152 |     * @param point The point to modify
153 |     * @param relTime The time to project by
154 |     */
155 |     void projectPointToStartOfSweep(pcl::PointXYZI& point, float relTime);
156 | 
157 |     pcl::PointCloud<pcl::PointXYZ>& imuTransform() { return _imuTrans; }
158 |     Time& sweepStart() { return _sweepStart; }
159 |     pcl::PointCloud<pcl::PointXYZI>& laserCloud() { return _laserCloud; }
160 |     pcl::PointCloud<pcl::PointXYZI>& cornerPointsSharp() { return _cornerPointsSharp; }
161 |     pcl::PointCloud<pcl::PointXYZI>& cornerPointsLessSharp() { return _cornerPointsLessSharp; }
162 |     pcl::PointCloud<pcl::PointXYZI>& surfacePointsFlat() { return _surfacePointsFlat; }
163 |     pcl::PointCloud<pcl::PointXYZI>& surfacePointsLessFlat() { return _surfacePointsLessFlat; }
164 |     RegistrationParams& config() { return _config; }
165 | 
166 |   private:
167 | 
168 |     /** \brief Check is IMU data is available. */
169 |     inline bool hasIMUData() { return _imuHistory.size() > 0; };
170 | 
171 |     /** \brief Set up the current IMU transformation for the specified relative time.
172 |      *
173 |      * @param relTime the time relative to the scan time
174 |      */
175 |     void setIMUTransformFor(const float& relTime);
176 | 
177 |     /** \brief Project the given point to the start of the sweep, using the current IMU state and position shift.
178 |      *
179 |      * @param point the point to project
180 |      */
181 |     void transformToStartIMU(pcl::PointXYZI& point);
182 | 
183 |     /** \brief Prepare for next scan / sweep.
184 |      *
185 |      * @param scanTime the current scan time
186 |      * @param newSweep indicator if a new sweep has started
187 |      */
188 |     void reset(const Time& scanTime);
189 | 
190 |     void reset();
191 | 
192 |     /** \brief Extract features from current laser cloud.
193 |      *
194 |      * @param beginIdx the index of the first scan to extract features from
195 |      */
196 |     void extractFeatures(const uint16_t& beginIdx = 0);
197 | 
198 |     /** \brief Set up region buffers for the specified point range.
199 |      *
200 |      * @param startIdx the region start index
201 |      * @param endIdx the region end index
202 |      */
203 |     void setRegionBuffersFor(const size_t& startIdx,
204 |       const size_t& endIdx);
205 | 
206 |     /** \brief Set up scan buffers for the specified point range.
207 |      *
208 |      * @param startIdx the scan start index
209 |      * @param endIdx the scan start index
210 |      */
211 |     void setScanBuffersFor(const size_t& startIdx,
212 |       const size_t& endIdx);
213 | 
214 |     /** \brief Mark a point and its neighbors as picked.
215 |      *
216 |      * This method will mark neighboring points within the curvature region as picked,
217 |      * as long as they remain within close distance to each other.
218 |      *
219 |      * @param cloudIdx the index of the picked point in the full resolution cloud
220 |      * @param scanIdx the index of the picked point relative to the current scan
221 |      */
222 |     void markAsPicked(const size_t& cloudIdx,
223 |       const size_t& scanIdx);
224 | 
225 |     /** \brief Try to interpolate the IMU state for the given time.
226 |      *
227 |      * @param relTime the time relative to the scan time
228 |      * @param outputState the output state instance
229 |      */
230 |     void interpolateIMUStateFor(const float& relTime, IMUState& outputState);
231 | 
232 |     void updateIMUTransform();
233 | 
234 |   public:
235 |     RegistrationParams _config;  ///< registration parameter
236 | 
237 |     pcl::PointCloud<pcl::PointXYZI> _laserCloud;   ///< full resolution input cloud
238 |     std::vector<IndexRange> _scanIndices;          ///< start and end indices of the individual scans withing the full resolution cloud
239 | 
240 |     pcl::PointCloud<pcl::PointXYZI> _cornerPointsSharp;      ///< sharp corner points cloud
241 |     pcl::PointCloud<pcl::PointXYZI> _cornerPointsLessSharp;  ///< less sharp corner points cloud
242 |     pcl::PointCloud<pcl::PointXYZI> _surfacePointsFlat;      ///< flat surface points cloud
243 |     pcl::PointCloud<pcl::PointXYZI> _surfacePointsLessFlat;  ///< less flat surface points cloud
244 | 
245 |     Time _sweepStart;            ///< time stamp of beginning of current sweep
246 |     Time _scanTime;              ///< time stamp of most recent scan
247 |     IMUState _imuStart;                     ///< the interpolated IMU state corresponding to the start time of the currently processed laser scan
248 |     IMUState _imuCur;                       ///< the interpolated IMU state corresponding to the time of the currently processed laser scan point
249 |     Vector3 _imuPositionShift;              ///< position shift between accumulated IMU position and interpolated IMU position
250 |     size_t _imuIdx = 0;                         ///< the current index in the IMU history
251 |     CircularBuffer<IMUState> _imuHistory;   ///< history of IMU states for cloud registration
252 | 
253 |     pcl::PointCloud<pcl::PointXYZ> _imuTrans = { 4,1 };  ///< IMU transformation information
254 | 
255 |     std::vector<float> _regionCurvature;      ///< point curvature buffer
256 |     std::vector<PointLabel> _regionLabel;     ///< point label buffer
257 |     std::vector<size_t> _regionSortIndices;   ///< sorted region indices based on point curvature
258 |     std::vector<int> _scanNeighborPicked;     ///< flag if neighboring point was already picked
259 |   };
260 | 
261 | }
262 | 
263 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/include/loam_velodyne/BasicTransformMaintenance.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | // Copyright 2013, Ji Zhang, Carnegie Mellon University
 3 | // Further contributions copyright (c) 2016, Southwest Research Institute
 4 | // All rights reserved.
 5 | //
 6 | // Redistribution and use in source and binary forms, with or without
 7 | // modification, are permitted provided that the following conditions are met:
 8 | //
 9 | // 1. Redistributions of source code must retain the above copyright notice,
10 | //    this list of conditions and the following disclaimer.
11 | // 2. Redistributions in binary form must reproduce the above copyright notice,
12 | //    this list of conditions and the following disclaimer in the documentation
13 | //    and/or other materials provided with the distribution.
14 | // 3. Neither the name of the copyright holder nor the names of its
15 | //    contributors may be used to endorse or promote products derived from this
16 | //    software without specific prior written permission.
17 | //
18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
22 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 | // POSSIBILITY OF SUCH DAMAGE.
29 | //
30 | // This is an implementation of the algorithm described in the following paper:
31 | //   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
32 | //     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
33 | 
34 | #include "Twist.h"
35 | 
36 | namespace loam
37 | {
38 | 
39 | /** \brief Implementation of the LOAM transformation maintenance component.
40 |  *
41 |  */
42 | class BasicTransformMaintenance
43 | {
44 | public:
45 |    void updateOdometry(double pitch, double yaw, double roll, double x, double y, double z);
46 |    void updateMappingTransform(Twist const& transformAftMapped, Twist const& transformBefMapped);
47 |    void updateMappingTransform(double pitch, double yaw, double roll,
48 |       double x, double y, double z,
49 |       double twist_rot_x, double twist_rot_y, double twist_rot_z,
50 |       double twist_pos_x, double twist_pos_y, double twist_pos_z);
51 | 
52 |    void transformAssociateToMap();
53 | 
54 |    // result accessor
55 |    float* transformMapped(){ return _transformMapped; }
56 | 
57 | public:
58 |    float _transformSum[6]{};
59 |    float _transformIncre[6]{};
60 |    float _transformMapped[6]{};
61 |    float _transformBefMapped[6]{};
62 |    float _transformAftMapped[6]{};
63 | };
64 | 
65 | } // end namespace loam
66 | 
67 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/include/loam_velodyne/CircularBuffer.h:
--------------------------------------------------------------------------------
  1 | #ifndef LOAM_CIRCULARBUFFER_H
  2 | #define LOAM_CIRCULARBUFFER_H
  3 | 
  4 | #include <cstdlib>
  5 | #include <vector>
  6 | 
  7 | 
  8 | namespace loam {
  9 | 
 10 | 
 11 | 
 12 | /** \brief Simple circular buffer implementation for storing data history.
 13 |  *
 14 |  * @tparam T The buffer element type.
 15 |  */
 16 | template <class T>
 17 | class CircularBuffer {
 18 | public:
 19 |   CircularBuffer(const size_t& capacity = 200)
 20 |         : _capacity(capacity),
 21 |           _size(0),
 22 |           _startIdx(0)
 23 |   {
 24 |     _buffer = new T[capacity];
 25 |   };
 26 | 
 27 |   ~CircularBuffer()
 28 |   {
 29 |     delete[] _buffer;
 30 |     _buffer = NULL;
 31 |   }
 32 | 
 33 |   /** \brief Retrieve the buffer size.
 34 |    *
 35 |    * @return the buffer size
 36 |    */
 37 |   const size_t& size() {
 38 |     return _size;
 39 |   }
 40 | 
 41 |   /** \brief Retrieve the buffer capacity.
 42 |    *
 43 |    * @return the buffer capacity
 44 |    */
 45 |   const size_t& capacity() {
 46 |     return _capacity;
 47 |   }
 48 | 
 49 |   /** \brief Ensure that this buffer has at least the required capacity.
 50 |    *
 51 |    * @param reqCapacity the minimum required capacity
 52 |    */
 53 |   void ensureCapacity(const int& reqCapacity) {
 54 |     if (reqCapacity > 0 && _capacity < reqCapacity) {
 55 |       // create new buffer and copy (valid) entries
 56 |       T* newBuffer = new T[reqCapacity];
 57 |       for (size_t i = 0; i < _size; i++) {
 58 |         newBuffer[i] = (*this)[i];
 59 |       }
 60 | 
 61 |       // switch buffer pointers and delete old buffer
 62 |       T* oldBuffer = _buffer;
 63 |       _buffer = newBuffer;
 64 |       _startIdx = 0;
 65 | 
 66 |       delete[] oldBuffer;
 67 |     }
 68 |   }
 69 | 
 70 |   /** \brief Check if the buffer is empty.
 71 |    *
 72 |    * @return true if the buffer is empty, false otherwise
 73 |    */
 74 |   bool empty() {
 75 |     return _size == 0;
 76 |   }
 77 | 
 78 |   /** \brief Retrieve the i-th element of the buffer.
 79 |    *
 80 |    *
 81 |    * @param i the buffer index
 82 |    * @return the element at the i-th position
 83 |    */
 84 |   const T& operator[](const size_t& i) {
 85 |     return _buffer[(_startIdx + i) % _capacity];
 86 |   }
 87 | 
 88 |   /** \brief Retrieve the first (oldest) element of the buffer.
 89 |    *
 90 |    * @return the first element
 91 |    */
 92 |   const T& first() {
 93 |     return _buffer[_startIdx];
 94 |   }
 95 | 
 96 |   /** \brief Retrieve the last (latest) element of the buffer.
 97 |    *
 98 |    * @return the last element
 99 |    */
100 |   const T& last() {
101 |     size_t idx = _size == 0 ? 0 : (_startIdx + _size - 1) % _capacity;
102 |     return _buffer[idx];
103 |   }
104 | 
105 |   /** \brief Push a new element to the buffer.
106 |    *
107 |    * If the buffer reached its capacity, the oldest element is overwritten.
108 |    *
109 |    * @param element the element to push
110 |    */
111 |   void push(const T& element) {
112 |     if (_size < _capacity) {
113 |       _buffer[_size] = element;
114 |       _size++;
115 |     } else {
116 |       _buffer[_startIdx] = element;
117 |       _startIdx = (_startIdx + 1) % _capacity;
118 |     }
119 |   }
120 | 
121 | private:
122 |   size_t _capacity;   ///< buffer capacity
123 |   size_t _size;       ///< current buffer size
124 |   size_t _startIdx;   ///< current start index
125 |   T* _buffer;         ///< internal element buffer
126 | };
127 | 
128 | } // end namespace loam
129 | 
130 | #endif //LOAM_CIRCULARBUFFER_H
131 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/include/loam_velodyne/LaserMapping.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2013, Ji Zhang, Carnegie Mellon University
  2 | // Further contributions copyright (c) 2016, Southwest Research Institute
  3 | // All rights reserved.
  4 | //
  5 | // Redistribution and use in source and binary forms, with or without
  6 | // modification, are permitted provided that the following conditions are met:
  7 | //
  8 | // 1. Redistributions of source code must retain the above copyright notice,
  9 | //    this list of conditions and the following disclaimer.
 10 | // 2. Redistributions in binary form must reproduce the above copyright notice,
 11 | //    this list of conditions and the following disclaimer in the documentation
 12 | //    and/or other materials provided with the distribution.
 13 | // 3. Neither the name of the copyright holder nor the names of its
 14 | //    contributors may be used to endorse or promote products derived from this
 15 | //    software without specific prior written permission.
 16 | //
 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 | // POSSIBILITY OF SUCH DAMAGE.
 28 | //
 29 | // This is an implementation of the algorithm described in the following paper:
 30 | //   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
 31 | //     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
 32 | 
 33 | #ifndef LOAM_LASERMAPPING_H
 34 | #define LOAM_LASERMAPPING_H
 35 | 
 36 | 
 37 | #include "BasicLaserMapping.h"
 38 | 
 39 | namespace loam
 40 | {
 41 | /** \brief Implementation of the LOAM laser mapping component.
 42 |  *
 43 |  */
 44 | class LaserMapping : public BasicLaserMapping
 45 | {
 46 | public:
 47 |    explicit LaserMapping(const float& scanPeriod = 0.1, const size_t& maxIterations = 10);
 48 | 
 49 |    /** \brief Setup component in active mode.
 50 |     *
 51 |     * @param node the ROS node handle
 52 |     * @param privateNode the private ROS node handle
 53 |     */
 54 |    //virtual bool setup(ros::NodeHandle& node, ros::NodeHandle& privateNode);
 55 | 
 56 |    /** \brief Handler method for a new last corner cloud.
 57 |     *
 58 |     * @param cornerPointsLastMsg the new last corner cloud message
 59 |     */
 60 |    //void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLastMsg);
 61 | 
 62 |    /** \brief Handler method for a new last surface cloud.
 63 |     *
 64 |     * @param surfacePointsLastMsg the new last surface cloud message
 65 |     */
 66 |    //void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& surfacePointsLastMsg);
 67 | 
 68 |    /** \brief Handler method for a new full resolution cloud.
 69 |     *
 70 |     * @param laserCloudFullResMsg the new full resolution cloud message
 71 |     */
 72 |    //void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullResMsg);
 73 | 
 74 |    /** \brief Handler method for a new laser odometry.
 75 |     *
 76 |     * @param laserOdometry the new laser odometry message
 77 |     */
 78 |    //void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry);
 79 | 
 80 |    /** \brief Handler method for IMU messages.
 81 |     *
 82 |     * @param imuIn the new IMU message
 83 |     */
 84 |    //void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn);
 85 | 
 86 |    /** \brief Process incoming messages in a loop until shutdown (used in active mode). */
 87 |    //void spin();
 88 | 
 89 |    /** \brief Try to process buffered data. */
 90 |    void process();
 91 | 
 92 |    void handleOdometry(double rx, double ry, double rz, double x, double y, double z);
 93 | 
 94 |    void readCloudAndOdom(const pcl::PointCloud<pcl::PointXYZI>& fullRes, 
 95 |                              const pcl::PointCloud<pcl::PointXYZI>& corner, 
 96 |                              const pcl::PointCloud<pcl::PointXYZI>& surf,
 97 |                              const Twist& twis);
 98 | protected:
 99 |    /** \brief Reset flags, etc. */
100 |    void reset();
101 | 
102 |    /** \brief Check if all required information for a new processing step is available. */
103 |    bool hasNewData();
104 | 
105 |    /** \brief Publish the current result via the respective topics. */
106 |    //void publishResult();
107 | 
108 | private:
109 |    // ros::Time _timeLaserCloudCornerLast;   ///< time of current last corner cloud
110 |    // ros::Time _timeLaserCloudSurfLast;     ///< time of current last surface cloud
111 |    // ros::Time _timeLaserCloudFullRes;      ///< time of current full resolution cloud
112 |    // ros::Time _timeLaserOdometry;          ///< time of current laser odometry
113 | 
114 |    bool _newLaserCloudCornerLast;  ///< flag if a new last corner cloud has been received
115 |    bool _newLaserCloudSurfLast;    ///< flag if a new last surface cloud has been received
116 |    bool _newLaserCloudFullRes;     ///< flag if a new full resolution cloud has been received
117 |    bool _newLaserOdometry;         ///< flag if a new laser odometry has been received
118 | 
119 | 
120 |    // nav_msgs::Odometry _odomAftMapped;      ///< mapping odometry message
121 |    // tf::StampedTransform _aftMappedTrans;   ///< mapping odometry transformation
122 | 
123 |    // ros::Publisher _pubLaserCloudSurround;    ///< map cloud message publisher
124 |    // ros::Publisher _pubLaserCloudFullRes;     ///< current full resolution cloud message publisher
125 |    // ros::Publisher _pubOdomAftMapped;         ///< mapping odometry publisher
126 |    // tf::TransformBroadcaster _tfBroadcaster;  ///< mapping odometry transform broadcaster
127 | 
128 |    // ros::Subscriber _subLaserCloudCornerLast;   ///< last corner cloud message subscriber
129 |    // ros::Subscriber _subLaserCloudSurfLast;     ///< last surface cloud message subscriber
130 |    // ros::Subscriber _subLaserCloudFullRes;      ///< full resolution cloud message subscriber
131 |    // ros::Subscriber _subLaserOdometry;          ///< laser odometry message subscriber
132 |    // ros::Subscriber _subImu;                    ///< IMU message subscriber
133 | };
134 | 
135 | } // end namespace loam
136 | 
137 | #endif //LOAM_LASERMAPPING_H
138 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/include/loam_velodyne/LaserOdometry.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2013, Ji Zhang, Carnegie Mellon University
  2 | // Further contributions copyright (c) 2016, Southwest Research Institute
  3 | // All rights reserved.
  4 | //
  5 | // Redistribution and use in source and binary forms, with or without
  6 | // modification, are permitted provided that the following conditions are met:
  7 | //
  8 | // 1. Redistributions of source code must retain the above copyright notice,
  9 | //    this list of conditions and the following disclaimer.
 10 | // 2. Redistributions in binary form must reproduce the above copyright notice,
 11 | //    this list of conditions and the following disclaimer in the documentation
 12 | //    and/or other materials provided with the distribution.
 13 | // 3. Neither the name of the copyright holder nor the names of its
 14 | //    contributors may be used to endorse or promote products derived from this
 15 | //    software without specific prior written permission.
 16 | //
 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 | // POSSIBILITY OF SUCH DAMAGE.
 28 | //
 29 | // This is an implementation of the algorithm described in the following paper:
 30 | //   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
 31 | //     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
 32 | 
 33 | #ifndef LOAM_LASERODOMETRY_H
 34 | #define LOAM_LASERODOMETRY_H
 35 | 
 36 | 
 37 | #include "Twist.h"
 38 | #include "nanoflann_pcl.h"
 39 | 
 40 | #include <pcl/point_cloud.h>
 41 | #include <pcl/point_types.h>
 42 | 
 43 | #include "BasicLaserOdometry.h"
 44 | 
 45 | namespace loam
 46 | {
 47 | 
 48 |   /** \brief Implementation of the LOAM laser odometry component.
 49 |    *
 50 |    */
 51 |   class LaserOdometry : public BasicLaserOdometry
 52 |   {
 53 |   public:
 54 |     explicit LaserOdometry(float scanPeriod = 0.1, uint16_t ioRatio = 2, size_t maxIterations = 25);
 55 | 
 56 |     /** \brief Setup component.
 57 |      *
 58 |      * @param node the ROS node handle
 59 |      * @param privateNode the private ROS node handle
 60 |      */
 61 |     // virtual bool setup(ros::NodeHandle& node,
 62 |     //   ros::NodeHandle& privateNode);
 63 | 
 64 |     /** \brief Handler method for a new sharp corner cloud.
 65 |      *
 66 |      * @param cornerPointsSharpMsg the new sharp corner cloud message
 67 |      */
 68 |     //void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharpMsg);
 69 | 
 70 |     /** \brief Handler method for a new less sharp corner cloud.
 71 |      *
 72 |      * @param cornerPointsLessSharpMsg the new less sharp corner cloud message
 73 |      */
 74 |     //void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharpMsg);
 75 | 
 76 |     /** \brief Handler method for a new flat surface cloud.
 77 |      *
 78 |      * @param surfPointsFlatMsg the new flat surface cloud message
 79 |      */
 80 |     //void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlatMsg);
 81 | 
 82 |     /** \brief Handler method for a new less flat surface cloud.
 83 |      *
 84 |      * @param surfPointsLessFlatMsg the new less flat surface cloud message
 85 |      */
 86 |     //void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlatMsg);
 87 | 
 88 |     /** \brief Handler method for a new full resolution cloud.
 89 |      *
 90 |      * @param laserCloudFullResMsg the new full resolution cloud message
 91 |      */
 92 |     //void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullResMsg);
 93 | 
 94 |     /** \brief Handler method for a new IMU transformation information.
 95 |      *
 96 |      * @param laserCloudFullResMsg the new IMU transformation information message
 97 |      */
 98 |     //void imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTransMsg);
 99 |     void readCloud(const pcl::PointCloud<pcl::PointXYZI>& fullRes, const pcl::PointCloud<pcl::PointXYZI>& cornerSharp, 
100 |                     const pcl::PointCloud<pcl::PointXYZI>& lessSharp, const pcl::PointCloud<pcl::PointXYZI>& pointFlat, 
101 |                     const pcl::PointCloud<pcl::PointXYZI>& lessFlat);
102 | 
103 |     /** \brief Process incoming messages in a loop until shutdown (used in active mode). */
104 |     void spin();
105 | 
106 |     /** \brief Try to process buffered data. */
107 |     void process();
108 | 
109 |   protected:
110 |     /** \brief Reset flags, etc. */
111 |     void reset();
112 | 
113 |     /** \brief Check if all required information for a new processing step is available. */
114 |     bool hasNewData();
115 | 
116 |     /** \brief Publish the current result via the respective topics. */
117 |     //void publishResult();
118 | 
119 |   public:
120 |     uint16_t _ioRatio;       ///< ratio of input to output frames
121 | 
122 |     // ros::Time _timeCornerPointsSharp;      ///< time of current sharp corner cloud
123 |     // ros::Time _timeCornerPointsLessSharp;  ///< time of current less sharp corner cloud
124 |     // ros::Time _timeSurfPointsFlat;         ///< time of current flat surface cloud
125 |     // ros::Time _timeSurfPointsLessFlat;     ///< time of current less flat surface cloud
126 |     // ros::Time _timeLaserCloudFullRes;      ///< time of current full resolution cloud
127 |     // ros::Time _timeImuTrans;               ///< time of current IMU transformation information
128 | 
129 |     bool _newCornerPointsSharp;       ///< flag if a new sharp corner cloud has been received
130 |     bool _newCornerPointsLessSharp;   ///< flag if a new less sharp corner cloud has been received
131 |     bool _newSurfPointsFlat;          ///< flag if a new flat surface cloud has been received
132 |     bool _newSurfPointsLessFlat;      ///< flag if a new less flat surface cloud has been received
133 |     bool _newLaserCloudFullRes;       ///< flag if a new full resolution cloud has been received
134 |     bool _newImuTrans;                ///< flag if a new IMU transformation information cloud has been received
135 | 
136 |     // nav_msgs::Odometry _laserOdometryMsg;       ///< laser odometry message
137 |     // tf::StampedTransform _laserOdometryTrans;   ///< laser odometry transformation
138 | 
139 |     // ros::Publisher _pubLaserCloudCornerLast;  ///< last corner cloud message publisher
140 |     // ros::Publisher _pubLaserCloudSurfLast;    ///< last surface cloud message publisher
141 |     // ros::Publisher _pubLaserCloudFullRes;     ///< full resolution cloud message publisher
142 |     // ros::Publisher _pubLaserOdometry;         ///< laser odometry publisher
143 |     // tf::TransformBroadcaster _tfBroadcaster;  ///< laser odometry transform broadcaster
144 | 
145 |     // ros::Subscriber _subCornerPointsSharp;      ///< sharp corner cloud message subscriber
146 |     // ros::Subscriber _subCornerPointsLessSharp;  ///< less sharp corner cloud message subscriber
147 |     // ros::Subscriber _subSurfPointsFlat;         ///< flat surface cloud message subscriber
148 |     // ros::Subscriber _subSurfPointsLessFlat;     ///< less flat surface cloud message subscriber
149 |     // ros::Subscriber _subLaserCloudFullRes;      ///< full resolution cloud message subscriber
150 |     // ros::Subscriber _subImuTrans;               ///< IMU transformation information message subscriber
151 |   };
152 | 
153 | } // end namespace loam
154 | 
155 | #endif //LOAM_LASERODOMETRY_H
156 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/include/loam_velodyne/MultiScanRegistration.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2013, Ji Zhang, Carnegie Mellon University
  2 | // Further contributions copyright (c) 2016, Southwest Research Institute
  3 | // All rights reserved.
  4 | //
  5 | // Redistribution and use in source and binary forms, with or without
  6 | // modification, are permitted provided that the following conditions are met:
  7 | //
  8 | // 1. Redistributions of source code must retain the above copyright notice,
  9 | //    this list of conditions and the following disclaimer.
 10 | // 2. Redistributions in binary form must reproduce the above copyright notice,
 11 | //    this list of conditions and the following disclaimer in the documentation
 12 | //    and/or other materials provided with the distribution.
 13 | // 3. Neither the name of the copyright holder nor the names of its
 14 | //    contributors may be used to endorse or promote products derived from this
 15 | //    software without specific prior written permission.
 16 | //
 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 | // POSSIBILITY OF SUCH DAMAGE.
 28 | //
 29 | // This is an implementation of the algorithm described in the following paper:
 30 | //   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
 31 | //     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
 32 | 
 33 | #ifndef LOAM_MULTISCANREGISTRATION_H
 34 | #define LOAM_MULTISCANREGISTRATION_H
 35 | 
 36 | #include "loam_velodyne/BasicScanRegistration.h"
 37 | 
 38 | namespace loam {
 39 | 
 40 | 
 41 | 
 42 | /** \brief Class realizing a linear mapping from vertical point angle to the corresponding scan ring.
 43 |  *
 44 |  */
 45 | class MultiScanMapper {
 46 | public:
 47 |   /** \brief Construct a new multi scan mapper instance.
 48 |    *
 49 |    * @param lowerBound - the lower vertical bound (degrees)
 50 |    * @param upperBound - the upper vertical bound (degrees)
 51 |    * @param nScanRings - the number of scan rings
 52 |    */
 53 |   MultiScanMapper(const float& lowerBound = -15,
 54 |                   const float& upperBound = 15,
 55 |                   const uint16_t& nScanRings = 16);
 56 | 
 57 |   const float& getLowerBound() { return _lowerBound; }
 58 |   const float& getUpperBound() { return _upperBound; }
 59 |   const uint16_t& getNumberOfScanRings() { return _nScanRings; }
 60 | 
 61 |   /** \brief Set mapping parameters.
 62 |    *
 63 |    * @param lowerBound - the lower vertical bound (degrees)
 64 |    * @param upperBound - the upper vertical bound (degrees)
 65 |    * @param nScanRings - the number of scan rings
 66 |    */
 67 |   void set(const float& lowerBound,
 68 |            const float& upperBound,
 69 |            const uint16_t& nScanRings);
 70 | 
 71 |   /** \brief Map the specified vertical point angle to its ring ID.
 72 |    *
 73 |    * @param angle the vertical point angle (in rad)
 74 |    * @return the ring ID
 75 |    */
 76 |   inline int getRingForAngle(const float& angle);
 77 | 
 78 |   /** Multi scan mapper for Velodyne VLP-16 according to data sheet. */
 79 |   static inline MultiScanMapper Velodyne_VLP_16() { return MultiScanMapper(-15, 15, 16); };
 80 | 
 81 |   /** Multi scan mapper for Velodyne HDL-32 according to data sheet. */
 82 |   static inline MultiScanMapper Velodyne_HDL_32() { return MultiScanMapper(-30.67f, 10.67f, 32); };
 83 | 
 84 |   /** Multi scan mapper for Velodyne HDL-64E according to data sheet. */
 85 |   static inline MultiScanMapper Velodyne_HDL_64E() { return MultiScanMapper(-24.9f, 2, 64); };
 86 | 
 87 | 
 88 | private:
 89 |   float _lowerBound;      ///< the vertical angle of the first scan ring
 90 |   float _upperBound;      ///< the vertical angle of the last scan ring
 91 |   uint16_t _nScanRings;   ///< number of scan rings
 92 |   float _factor;          ///< linear interpolation factor
 93 | };
 94 | 
 95 | 
 96 | 
 97 | /** \brief Class for registering point clouds received from multi-laser lidars.
 98 |  *
 99 |  */
100 | class MultiScanRegistration :  public BasicScanRegistration {
101 | public:
102 |   MultiScanRegistration(const MultiScanMapper& scanMapper = MultiScanMapper());
103 | 
104 | 
105 |   //bool setup(ros::NodeHandle& node, ros::NodeHandle& privateNode);
106 | 
107 |   /** \brief Handler method for input cloud messages.
108 |    *
109 |    * @param laserCloudMsg the new input cloud message to process
110 |    */
111 |   //void handleCloudMessage(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg);
112 |   void handleCloud(const pcl::PointCloud<pcl::PointXYZ>& laserCloudIn);
113 | 
114 | public:
115 |   /** \brief Setup component in active mode.
116 |    *
117 |    * @param node the ROS node handle
118 |    * @param privateNode the private ROS node handle
119 |   //  */
120 |   // bool setupROS(ros::NodeHandle& node, ros::NodeHandle& privateNode, RegistrationParams& config_out) override;
121 | 
122 |   /** \brief Process a new input cloud.
123 |    *
124 |    * @param laserCloudIn the new input cloud to process
125 |    * @param scanTime the scan (message) timestamp
126 |    */
127 |   //void process(const pcl::PointCloud<pcl::PointXYZ>& laserCloudIn, const Time& scanTime);
128 |   void process(const pcl::PointCloud<pcl::PointXYZ>& laserCloudIn/*, const Time& scanTime*/);
129 | 
130 | 
131 | public:
132 |   int _systemDelay = 20;             ///< system startup delay counter
133 |   MultiScanMapper _scanMapper;  ///< mapper for mapping vertical point angles to scan ring IDs
134 |   std::vector<pcl::PointCloud<pcl::PointXYZI> > _laserCloudScans;
135 |   //ros::Subscriber _subLaserCloud;   ///< input cloud message subscriber
136 | 
137 | };
138 | 
139 | } // end namespace loam
140 | 
141 | 
142 | #endif //LOAM_MULTISCANREGISTRATION_H
143 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/include/loam_velodyne/TransformMaintenance.h:
--------------------------------------------------------------------------------
 1 | // Copyright 2013, Ji Zhang, Carnegie Mellon University
 2 | // Further contributions copyright (c) 2016, Southwest Research Institute
 3 | // All rights reserved.
 4 | //
 5 | // Redistribution and use in source and binary forms, with or without
 6 | // modification, are permitted provided that the following conditions are met:
 7 | //
 8 | // 1. Redistributions of source code must retain the above copyright notice,
 9 | //    this list of conditions and the following disclaimer.
10 | // 2. Redistributions in binary form must reproduce the above copyright notice,
11 | //    this list of conditions and the following disclaimer in the documentation
12 | //    and/or other materials provided with the distribution.
13 | // 3. Neither the name of the copyright holder nor the names of its
14 | //    contributors may be used to endorse or promote products derived from this
15 | //    software without specific prior written permission.
16 | //
17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 | // POSSIBILITY OF SUCH DAMAGE.
28 | //
29 | // This is an implementation of the algorithm described in the following paper:
30 | //   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
31 | //     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
32 | 
33 | #ifndef LOAM_TRANSFORMMAINTENANCE_H
34 | #define LOAM_TRANSFORMMAINTENANCE_H
35 | 
36 | #include "loam_velodyne/BasicTransformMaintenance.h"
37 | 
38 | namespace loam {
39 | 
40 | /** \brief Implementation of the LOAM transformation maintenance component.
41 |  *
42 |  */
43 | class TransformMaintenance: public BasicTransformMaintenance {
44 | public:
45 |   //TransformMaintenance();
46 | 
47 |   /** \brief Setup component.
48 |    *
49 |    * @param node the ROS node handle
50 |    * @param privateNode the private ROS node handle
51 |    */
52 |   //virtual bool setup(ros::NodeHandle& node, ros::NodeHandle& privateNode);
53 | 
54 |   /** \brief Handler method for laser odometry messages.
55 |    *
56 |    * @param laserOdometry the new laser odometry
57 |    */
58 |   //void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry);
59 | 
60 |   /** \brief Handler method for mapping odometry messages.
61 |    *
62 |    * @param odomAftMapped the new mapping odometry
63 |    */
64 |   //void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped);
65 |   void handleLaserOdom(const Twist& odom);
66 |   void handleOdomAftMapped(const Twist& aftMap, const Twist& befMap);
67 | 
68 | }; 
69 | } // end namespace loam
70 | 
71 | 
72 | #endif //LOAM_TRANSFORMMAINTENANCE_H
73 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/include/loam_velodyne/Twist.h:
--------------------------------------------------------------------------------
 1 | #ifndef LOAM_TWIST_H
 2 | #define LOAM_TWIST_H
 3 | 
 4 | 
 5 | #include "Angle.h"
 6 | #include "Vector3.h"
 7 | 
 8 | 
 9 | namespace loam {
10 | 
11 | 
12 | /** \brief Twist composed by three angles and a three-dimensional position.
13 |  *
14 |  */
15 | class Twist {
16 | public:
17 |   Twist()
18 |         : rot_x(),
19 |           rot_y(),
20 |           rot_z(),
21 |           pos() {};
22 | 
23 |   Angle rot_x;
24 |   Angle rot_y;
25 |   Angle rot_z;
26 |   Vector3 pos;
27 | };
28 | 
29 | } // end namespace loam
30 | 
31 | #endif //LOAM_TWIST_H
32 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/include/loam_velodyne/Vector3.h:
--------------------------------------------------------------------------------
 1 | #ifndef LOAM_VECTOR3_H
 2 | #define LOAM_VECTOR3_H
 3 | 
 4 | 
 5 | #include <pcl/point_types.h>
 6 | 
 7 | 
 8 | namespace loam {
 9 | 
10 | /** \brief Vector4f decorator for convenient handling.
11 |  *
12 |  */
13 | class Vector3 : public Eigen::Vector4f {
14 | public:
15 |   Vector3(float x, float y, float z)
16 |       : Eigen::Vector4f(x, y, z, 0) {}
17 | 
18 |   Vector3(void)
19 |       : Eigen::Vector4f(0, 0, 0, 0) {}
20 | 
21 |   template<typename OtherDerived>
22 |   Vector3(const Eigen::MatrixBase <OtherDerived> &other)
23 |       : Eigen::Vector4f(other) {}
24 | 
25 |   Vector3(const pcl::PointXYZI &p)
26 |       : Eigen::Vector4f(p.x, p.y, p.z, 0) {}
27 | 
28 |   template<typename OtherDerived>
29 |   Vector3 &operator=(const Eigen::MatrixBase <OtherDerived> &rhs) {
30 |     this->Eigen::Vector4f::operator=(rhs);
31 |     return *this;
32 |   }
33 | 
34 |   Vector3 &operator=(const pcl::PointXYZ &rhs) {
35 |     x() = rhs.x;
36 |     y() = rhs.y;
37 |     z() = rhs.z;
38 |     return *this;
39 |   }
40 | 
41 |   Vector3 &operator=(const pcl::PointXYZI &rhs) {
42 |     x() = rhs.x;
43 |     y() = rhs.y;
44 |     z() = rhs.z;
45 |     return *this;
46 |   }
47 | 
48 |   float x() const { return (*this)(0); }
49 | 
50 |   float y() const { return (*this)(1); }
51 | 
52 |   float z() const { return (*this)(2); }
53 | 
54 |   float &x() { return (*this)(0); }
55 | 
56 |   float &y() { return (*this)(1); }
57 | 
58 |   float &z() { return (*this)(2); }
59 | 
60 |   // easy conversion
61 |   operator pcl::PointXYZI() {
62 |     pcl::PointXYZI dst;
63 |     dst.x = x();
64 |     dst.y = y();
65 |     dst.z = z();
66 |     dst.intensity = 0;
67 |     return dst;
68 |   }
69 | };
70 | 
71 | } // end namespace loam
72 | 
73 | #endif //LOAM_VECTOR3_H
74 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/include/loam_velodyne/common.h:
--------------------------------------------------------------------------------
 1 | // Copyright 2013, Ji Zhang, Carnegie Mellon University
 2 | // Further contributions copyright (c) 2016, Southwest Research Institute
 3 | // All rights reserved.
 4 | //
 5 | // Redistribution and use in source and binary forms, with or without
 6 | // modification, are permitted provided that the following conditions are met:
 7 | //
 8 | // 1. Redistributions of source code must retain the above copyright notice,
 9 | //    this list of conditions and the following disclaimer.
10 | // 2. Redistributions in binary form must reproduce the above copyright notice,
11 | //    this list of conditions and the following disclaimer in the documentation
12 | //    and/or other materials provided with the distribution.
13 | // 3. Neither the name of the copyright holder nor the names of its
14 | //    contributors may be used to endorse or promote products derived from this
15 | //    software without specific prior written permission.
16 | //
17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 | // POSSIBILITY OF SUCH DAMAGE.
28 | //
29 | // This is an implementation of the algorithm described in the following paper:
30 | //   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
31 | //     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
32 | 
33 | #ifndef LOAM_COMMON_H
34 | #define LOAM_COMMON_H
35 | 
36 | #include <ros/ros.h>
37 | #include <sensor_msgs/PointCloud2.h>
38 | #include <pcl_conversions/pcl_conversions.h>
39 | #include <pcl/point_types.h>
40 | #include "time_utils.h"
41 | 
42 | namespace loam {
43 | 
44 | /** \brief Construct a new point cloud message from the specified information and publish it via the given publisher.
45 |  *
46 |  * @tparam PointT the point type
47 |  * @param publisher the publisher instance
48 |  * @param cloud the cloud to publish
49 |  * @param stamp the time stamp of the cloud message
50 |  * @param frameID the message frame ID
51 |  */
52 | template <typename PointT>
53 | inline void publishCloudMsg(ros::Publisher& publisher,
54 |                             const pcl::PointCloud<PointT>& cloud,
55 |                             const ros::Time& stamp,
56 |                             std::string frameID) {
57 |   sensor_msgs::PointCloud2 msg;
58 |   pcl::toROSMsg(cloud, msg);
59 |   msg.header.stamp = stamp;
60 |   msg.header.frame_id = frameID;
61 |   publisher.publish(msg);
62 | }
63 | 
64 | 
65 | // ROS time adapters
66 | inline Time fromROSTime(ros::Time const& rosTime)
67 | {
68 |   auto epoch = std::chrono::system_clock::time_point();
69 |   auto since_epoch = std::chrono::seconds(rosTime.sec) + std::chrono::nanoseconds(rosTime.nsec);
70 |   return epoch + since_epoch;
71 | }
72 | 
73 | inline ros::Time toROSTime(Time const& time_point)
74 | {
75 |   return ros::Time().fromNSec(std::chrono::duration_cast<std::chrono::nanoseconds>(time_point.time_since_epoch()).count());
76 | }
77 | 
78 | } // end namespace loam
79 | 
80 | #endif // LOAM_COMMON_H
81 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/include/loam_velodyne/nanoflann_pcl.h:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Software License Agreement (BSD License)
  3 |  *
  4 |  *  Point Cloud Library (PCL) - www.pointclouds.org
  5 |  *  Copyright (c) 2010-2012, Willow Garage, Inc.
  6 |  *  Copyright (c) 2012-, Open Perception, Inc.
  7 |  *
  8 |  *  All rights reserved.
  9 |  *
 10 |  *  Redistribution and use in source and binary forms, with or without
 11 |  *  modification, are permitted provided that the following conditions
 12 |  *  are met:
 13 |  *
 14 |  *   * Redistributions of source code must retain the above copyright
 15 |  *     notice, this list of conditions and the following disclaimer.
 16 |  *   * Redistributions in binary form must reproduce the above
 17 |  *     copyright notice, this list of conditions and the following
 18 |  *     disclaimer in the documentation and/or other materials provided
 19 |  *     with the distribution.
 20 |  *   * Neither the name of the copyright holder(s) nor the names of its
 21 |  *     contributors may be used to endorse or promote products derived
 22 |  *     from this software without specific prior written permission.
 23 |  *
 24 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 25 |  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 26 |  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 27 |  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 28 |  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 29 |  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 30 |  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 31 |  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 32 |  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 33 |  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 34 |  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 35 |  *  POSSIBILITY OF SUCH DAMAGE.
 36 |  *
 37 |  * $Id: kdtree_flann.h 36261 2011-02-26 01:34:42Z mariusm $
 38 |  *
 39 |  */
 40 | 
 41 | #ifndef NANO_KDTREE_KDTREE_FLANN_H_
 42 | #define NANO_KDTREE_KDTREE_FLANN_H_
 43 | 
 44 | #include <boost/shared_ptr.hpp>
 45 | #include <pcl/point_cloud.h>
 46 | #include <pcl/point_types.h>
 47 | #include "nanoflann.hpp"
 48 | 
 49 | namespace nanoflann
 50 | {
 51 | 
 52 | // Adapter class to give to nanoflann the same "look and fell" of pcl::KdTreeFLANN.
 53 | // limited to squared distance between 3D points
 54 | template <typename PointT>
 55 | class KdTreeFLANN
 56 | {
 57 | public:
 58 | 
 59 |     typedef boost::shared_ptr<KdTreeFLANN<PointT> > Ptr;
 60 |     typedef boost::shared_ptr<const KdTreeFLANN<PointT> > ConstPtr;
 61 | 
 62 |     typedef typename pcl::PointCloud<PointT> PointCloud;
 63 |     typedef typename pcl::PointCloud<PointT>::Ptr PointCloudPtr;
 64 |     typedef typename pcl::PointCloud<PointT>::ConstPtr PointCloudConstPtr;
 65 | 
 66 |     typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
 67 |     typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
 68 | 
 69 |     KdTreeFLANN (bool sorted = true);
 70 | 
 71 |     KdTreeFLANN (const KdTreeFLANN<PointT> &k);
 72 | 
 73 |     void  setEpsilon (float eps);
 74 | 
 75 |     void  setSortedResults (bool sorted);
 76 | 
 77 |     inline Ptr makeShared () { return Ptr (new KdTreeFLANN<PointT> (*this)); }
 78 | 
 79 |     void setInputCloud (const PointCloudPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ());
 80 | 
 81 |     int  nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices,
 82 |                          std::vector<float> &k_sqr_distances) const;
 83 | 
 84 |     int radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices,
 85 |                       std::vector<float> &k_sqr_distances) const;
 86 | 
 87 | private:
 88 | 
 89 |     nanoflann::SearchParams _params;
 90 | 
 91 |     struct PointCloud_Adaptor
 92 |     {
 93 |       inline size_t kdtree_get_point_count() const;
 94 |       inline float kdtree_get_pt(const size_t idx, int dim) const;
 95 |       template <class BBOX> bool kdtree_get_bbox(BBOX&) const { return false; }
 96 |       PointCloudConstPtr pcl;
 97 |       IndicesConstPtr indices;
 98 |     };
 99 | 
100 |     typedef nanoflann::KDTreeSingleIndexAdaptor<
101 |       nanoflann::SO3_Adaptor<float, PointCloud_Adaptor > ,
102 |       PointCloud_Adaptor, 3, int> KDTreeFlann_PCL_SO3;
103 | 
104 |     PointCloud_Adaptor _adaptor;
105 | 
106 |     KDTreeFlann_PCL_SO3 _kdtree;
107 | 
108 | };
109 | 
110 | //---------- Definitions ---------------------
111 | 
112 | template<typename PointT> inline
113 | KdTreeFLANN<PointT>::KdTreeFLANN(bool sorted):
114 |     _kdtree(3,_adaptor)
115 | {
116 |     _params.sorted = sorted;
117 | }
118 | 
119 | template<typename PointT> inline
120 | void KdTreeFLANN<PointT>::setEpsilon(float eps)
121 | {
122 |     _params.eps = eps;
123 | }
124 | 
125 | template<typename PointT> inline
126 | void KdTreeFLANN<PointT>::setSortedResults(bool sorted)
127 | {
128 |     _params.sorted = sorted;
129 | }
130 | 
131 | template<typename PointT> inline
132 | void KdTreeFLANN<PointT>::setInputCloud(const KdTreeFLANN::PointCloudPtr &cloud,
133 |                                         const IndicesConstPtr &indices)
134 | {
135 |     _adaptor.pcl = cloud;
136 |     _adaptor.indices = indices;
137 |     _kdtree.buildIndex();
138 | }
139 | 
140 | template<typename PointT> inline
141 | int KdTreeFLANN<PointT>::nearestKSearch(const PointT &point, int num_closest,
142 |                                 std::vector<int> &k_indices,
143 |                                 std::vector<float> &k_sqr_distances) const
144 | {
145 |     k_indices.resize(num_closest);
146 |     k_sqr_distances.resize(num_closest);
147 | 
148 |     nanoflann::KNNResultSet<float,int> resultSet(num_closest);
149 |     resultSet.init( k_indices.data(), k_sqr_distances.data());
150 |     _kdtree.findNeighbors(resultSet, point.data, nanoflann::SearchParams() );
151 |     return resultSet.size();
152 | }
153 | 
154 | template<typename PointT> inline
155 | int KdTreeFLANN<PointT>::radiusSearch(const PointT &point, double radius,
156 |                               std::vector<int> &k_indices,
157 |                               std::vector<float> &k_sqr_distances) const
158 | {
159 |     static std::vector<std::pair<int, float> > indices_dist;
160 |     indices_dist.reserve( 128 );
161 | 
162 |     RadiusResultSet<float, int> resultSet(radius, indices_dist);
163 |     const size_t nFound = _kdtree.findNeighbors(resultSet, point.data, _params);
164 | 
165 |     if (_params.sorted)
166 |         std::sort(indices_dist.begin(), indices_dist.end(), IndexDist_Sorter() );
167 | 
168 |     k_indices.resize(nFound);
169 |     k_sqr_distances.resize(nFound);
170 |     for(int i=0; i<nFound; i++ ){
171 |         k_indices[i]       = indices_dist[i].first;
172 |         k_sqr_distances[i] = indices_dist[i].second;
173 |     }
174 |     return nFound;
175 | }
176 | 
177 | template<typename PointT> inline
178 | size_t KdTreeFLANN<PointT>::PointCloud_Adaptor::kdtree_get_point_count() const {
179 |     if( indices ) return indices->size();
180 |     if( pcl)  return pcl->points.size();
181 |     return 0;
182 | }
183 | 
184 | template<typename PointT> inline
185 | float KdTreeFLANN<PointT>::PointCloud_Adaptor::kdtree_get_pt(const size_t idx, int dim) const{
186 |     const PointT& p = ( indices ) ? pcl->points[(*indices)[idx]] : pcl->points[idx];
187 |     if (dim==0) return p.x;
188 |     else if (dim==1) return p.y;
189 |     else if (dim==2) return p.z;
190 |     else return 0.0;
191 | }
192 | 
193 | }
194 | 
195 | 
196 | #endif
197 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/include/loam_velodyne/time_utils.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <chrono>
 4 | 
 5 | namespace loam
 6 | { 
 7 |   /** \brief A standard non-ROS alternative to ros::Time.*/
 8 |   using Time = std::chrono::system_clock::time_point;
 9 | 
10 |   // helper function
11 |   inline double toSec(Time::duration duration)
12 |   {
13 |     return std::chrono::duration<double>(duration).count();
14 |   };
15 | }
16 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/main.cpp:
--------------------------------------------------------------------------------
  1 | #include <boost/thread/thread.hpp>
  2 | #include <pcl/common/common_headers.h>
  3 | #include <pcl/features/normal_3d.h>
  4 | #include <pcl/visualization/pcl_visualizer.h>
  5 | #include <pcl/console/parse.h>
  6 | #include <pcl/io/pcd_io.h>
  7 | #include <pcl/point_types.h>
  8 | 
  9 | #include <pangolin/pangolin.h>
 10 | 
 11 | #include <fstream>
 12 | #include <string>
 13 | #include <sstream>
 14 | #include <iostream>
 15 | #include <queue>
 16 | #include <condition_variable>
 17 | #include <chrono>
 18 | #include <thread>
 19 | #include <mutex>
 20 | 
 21 | //#include "loam_velodyne/BasicScanRegistration.h"
 22 | #include "loam_velodyne/MultiScanRegistration.h"
 23 | #include "loam_velodyne/LaserOdometry.h"
 24 | #include "loam_velodyne/LaserMapping.h"
 25 | #include "loam_velodyne/TransformMaintenance.h"
 26 | 
 27 | #include <Eigen/Core>
 28 | #include <Eigen/Dense>
 29 | 
 30 | using namespace std;
 31 | using namespace Eigen;
 32 | int nFRAMES = 849;
 33 | std::mutex m_buf;
 34 | std::mutex m_odom2map,m_transMain;
 35 | queue<pcl::PointCloud<pcl::PointXYZI>> fullRes_buf;
 36 | queue<pcl::PointCloud<pcl::PointXYZI>> pointsSharp_buf;
 37 | queue<pcl::PointCloud<pcl::PointXYZI>> lessSharp_buf;
 38 | queue<pcl::PointCloud<pcl::PointXYZI>> pointsFlat_buf;
 39 | queue<pcl::PointCloud<pcl::PointXYZI>> lessFlat_buf;
 40 | 
 41 | queue<pcl::PointCloud<pcl::PointXYZI>> fullRes_odom2map;
 42 | queue<pcl::PointCloud<pcl::PointXYZI>> corner_odom2map;
 43 | queue<pcl::PointCloud<pcl::PointXYZI>> surf_odom2map;
 44 | queue<loam::Twist> trans_odom2map;
 45 | 
 46 | loam::MultiScanRegistration multiScan;
 47 | loam::LaserOdometry laserOdom;
 48 | loam::LaserMapping laserMap;
 49 | loam::TransformMaintenance transMain;
 50 | //double time_stamp = 0.5;
 51 | 
 52 | int MaxQueueLength = 3;
 53 | 
 54 | std::condition_variable con;
 55 | 
 56 | vector<loam::Twist> Trajectory;
 57 | vector<Eigen::Vector3d> Trajectory2;
 58 | 
 59 | pcl::PointCloud<pcl::PointXYZI>::Ptr pMap;
 60 | pcl::PointCloud<pcl::PointXYZI> laserReg;
 61 | int getRingForAngle(float angle)
 62 | {
 63 |     float _factor = (16.0 - 1.0) / 30.0;
 64 |     return int(((angle * 180.0 / M_PI) + 15) * _factor + 0.5);
 65 | }
 66 | 
 67 | boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis (pcl::PointCloud<pcl::PointXYZI>::ConstPtr cloud)
 68 | {
 69 |    // --------------------------------------------
 70 |    // -----Open 3D viewer and add point cloud-----
 71 |    // --------------------------------------------
 72 |    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
 73 |    viewer->setBackgroundColor (0, 0, 0);
 74 |    viewer->addPointCloud<pcl::PointXYZI> (cloud, "sample cloud");
 75 |    viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
 76 |    viewer->addCoordinateSystem (1.0);
 77 |    viewer->initCameraParameters ();
 78 |    return (viewer);
 79 | }
 80 | 
 81 | 
 82 | boost::shared_ptr<pcl::visualization::PCLVisualizer> displayFeatures(pcl::PointCloud<pcl::PointXYZI>::ConstPtr original, pcl::PointCloud<pcl::PointXYZI>::ConstPtr c1, pcl::PointCloud<pcl::PointXYZI>::ConstPtr c2)
 83 | {
 84 |     //     // --------------------------------------------
 85 | //     // -----Open 3D viewer and add point cloud-----
 86 | //     // --------------------------------------------
 87 |     boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
 88 |     viewer->setBackgroundColor (0, 0, 0);
 89 |     pcl::PointCloud<pcl::PointXYZRGB> orig;
 90 |     pcl::PointCloud<pcl::PointXYZRGB> corner;
 91 |     pcl::PointCloud<pcl::PointXYZRGB> flat;
 92 |     for(int i = 0; i < original->points.size(); i++)
 93 |     {
 94 |         pcl::PointXYZRGB point;
 95 |         point.x = original->points[i].x;
 96 |         point.y = original->points[i].y;
 97 |         point.z = original->points[i].z;
 98 | 
 99 |         uint32_t rgb = (static_cast<uint32_t>(100) << 16 |
100 |                         static_cast<uint32_t>(100) << 8 | static_cast<uint32_t>(100));
101 |         point.rgb = rgb;
102 |         orig.points.push_back(point);
103 |     }
104 |     pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color(orig.makeShared());
105 |     viewer->addPointCloud<pcl::PointXYZRGB> (orig.makeShared(), color, "sample cloud");
106 |     viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
107 | 
108 |     for(int i = 0; i < c1->points.size(); i++)
109 |     {
110 |         pcl::PointXYZRGB point;
111 |         point.x = c1->points[i].x;
112 |         point.y = c1->points[i].y;
113 |         point.z = c1->points[i].z;
114 | 
115 |         uint32_t rgb = (static_cast<uint32_t>(255) << 16 |
116 |                         static_cast<uint32_t>(0) << 8 | static_cast<uint32_t>(0));
117 |         point.rgb = rgb;
118 |         corner.points.push_back(point);
119 |     }
120 |     pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color1(corner.makeShared());
121 |     viewer->addPointCloud<pcl::PointXYZRGB> (corner.makeShared(), color1, "corner");
122 |     viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "corner");
123 | 
124 |     for(int i = 0; i < c2->points.size(); i++)
125 |     {
126 |         pcl::PointXYZRGB point;
127 |         point.x = c2->points[i].x;
128 |         point.y = c2->points[i].y;
129 |         point.z = c2->points[i].z;
130 | 
131 |         uint32_t rgb = (static_cast<uint32_t>(125) << 16 |
132 |                         static_cast<uint32_t>(125) << 8 | static_cast<uint32_t>(0));
133 |         point.rgb = rgb;
134 |         flat.points.push_back(point);
135 |     }
136 |     pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color2(flat.makeShared());
137 |     viewer->addPointCloud<pcl::PointXYZRGB> (flat.makeShared(), color2, "flat");
138 |     viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "flat");
139 | 
140 |     viewer->addCoordinateSystem (1.0);
141 |     viewer->initCameraParameters ();
142 |     return (viewer);
143 | }
144 | vector<string> loadData()
145 | {
146 |     std::vector<string> v;
147 |     for(int i = 0; i < nFRAMES; i++)
148 |     {
149 |         stringstream sstr;
150 |         sstr<<"/Users/walker/Downloads/nsh_indoor_outdoor_pcd/"<<i<<".pcd";
151 |         v.push_back(sstr.str());
152 |     }
153 |     return v;
154 | }
155 | void Draw() 
156 | {
157 |     float fx = 277.34;
158 |     float fy = 291.402;
159 |     float cx = 312.234;
160 |     float cy = 239.777;
161 |     // create pangolin window and plot the trajectory
162 |     pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
163 |     glEnable(GL_DEPTH_TEST);
164 |     glEnable(GL_BLEND);
165 |     glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
166 | 
167 |     pangolin::OpenGlRenderState s_cam(
168 |             pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
169 |             pangolin::ModelViewLookAt(0, 10, -10, 0, 0, 0, 0.0, 1.0, 0.0)
170 |     );
171 | 
172 |     pangolin::View &d_cam = pangolin::CreateDisplay()
173 |             .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
174 |             .SetHandler(new pangolin::Handler3D(s_cam));
175 | 
176 |    
177 |     while (pangolin::ShouldQuit() == false) {
178 |         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
179 | 
180 |         d_cam.Activate(s_cam);
181 |         glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
182 | 
183 |         // draw poses
184 |         float sz = 0.1;
185 |         int width = 640, height = 480;
186 |         
187 |         glLineWidth(2);
188 |         if(Trajectory2.size() > 1)
189 |         {
190 |             for(int i = 0;i < Trajectory2.size() - 1;i++)
191 |             {
192 |                 glColor3f(0, 1, 0);
193 |                 glBegin(GL_LINES);
194 |                 auto p1 = Trajectory2[i], p2 = Trajectory2[i + 1];
195 |                 glVertex3d(p1[0], p1[1], p1[2]);
196 |                 glVertex3d(p2[0], p2[1], p2[2]);
197 |                 glEnd();
198 |             }
199 |         }
200 | 
201 | 
202 |         if(pMap->points.size() > 2)
203 |         {
204 |             glPointSize(1);
205 |             glBegin(GL_POINTS);
206 |             for (size_t i = 0; i < pMap->points.size(); i++) 
207 |             {
208 |                 glColor3f(0.6, 0.6, 0.6);
209 |                 glVertex3d(pMap->points[i].x, 
210 |                     pMap->points[i].y, 
211 |                     pMap->points[i].z);
212 |             }
213 |             glEnd();
214 |         }
215 |         
216 |         if(laserReg.points.size() > 2)
217 |         {
218 |             glPointSize(1);
219 |             glBegin(GL_POINTS);
220 |             for (size_t i = 0; i < laserReg.points.size(); i++) 
221 |             {
222 |                 glColor3f(1, 0, 0);
223 |                 glVertex3d(laserReg.points[i].x, 
224 |                     laserReg.points[i].y, 
225 |                     laserReg.points[i].z);
226 |             }
227 |             glEnd();
228 |         }
229 | 
230 |         pangolin::FinishFrame();
231 |         
232 |         usleep(5000);   // sleep 5 ms
233 |     }
234 | }
235 | 
236 | int main (int argc, char** argv)
237 | {
238 | 	//loam::BasicScanRegistration res;
239 | 	//res.nScans = 16;
240 |     
241 |     vector<string> files = loadData();
242 | 
243 |     // for(auto& f:files)
244 |     //     cout<<f<<endl;
245 |     std::thread registration([&](){
246 |         for(int i = 0; i < nFRAMES; i++)
247 |         {
248 |             m_buf.lock();
249 | 
250 |             //cout<<i<<endl;
251 |             pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
252 | 
253 |             if (pcl::io::loadPCDFile<pcl::PointXYZ> (files[i], *cloud) == -1) //* load the file
254 |             {
255 |                 PCL_ERROR ("Couldn't read file \n");
256 |                 return (-1);
257 |             }
258 | 
259 |             //std::chrono::steady_clock::time_point  now = std::chrono::steady_clock::now();
260 |             multiScan.handleCloud(*cloud);
261 |             fullRes_buf.emplace(std::move(multiScan._laserCloud));
262 |             pointsSharp_buf.emplace(std::move(multiScan._cornerPointsSharp));
263 |             lessSharp_buf.emplace(std::move(multiScan._cornerPointsLessSharp));
264 |             pointsFlat_buf.emplace(std::move(multiScan._surfacePointsFlat));
265 |             lessFlat_buf.emplace(std::move(multiScan._surfacePointsLessFlat));
266 | 
267 |             con.notify_one();
268 |             //cout<<"multiscan._lasercloud: "<<multiScan._laserCloud.points.size()<<endl;
269 |             // auto t2 = std::chrono::steady_clock::now();
270 |             // std::chrono::duration<double> time_span = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - now);
271 |             // std::cout << "Feature extration time: " << time_span.count() << " seconds."<<endl;
272 |             m_buf.unlock();
273 |         }
274 |     });
275 |     
276 | 
277 |     std::thread odometry([&](){
278 |         while(true)
279 |         {
280 | 
281 |             std::unique_lock<std::mutex> lk(m_buf);
282 |             con.wait(lk);
283 |             //std::chrono::steady_clock::time_point  now = std::chrono::steady_clock::now();
284 |             if((!fullRes_buf.empty()) && 
285 |                (!pointsSharp_buf.empty()) && 
286 |                (!lessSharp_buf.empty()) && 
287 |                (!pointsFlat_buf.empty()) && 
288 |                (!lessFlat_buf.empty()))
289 |             {
290 |                 pcl::PointCloud<pcl::PointXYZI> _fullRes = std::move(fullRes_buf.front());
291 |                 fullRes_buf.pop();
292 |                 pcl::PointCloud<pcl::PointXYZI> _cornerCloud = std::move(pointsSharp_buf.front());
293 |                 pointsSharp_buf.pop();
294 |                 pcl::PointCloud<pcl::PointXYZI> _lessSharp = std::move(lessSharp_buf.front());
295 |                 lessSharp_buf.pop();
296 |                 pcl::PointCloud<pcl::PointXYZI> _cloudFlat = std::move(pointsFlat_buf.front());
297 |                 pointsFlat_buf.pop();
298 |                 pcl::PointCloud<pcl::PointXYZI> _lessFlat = std::move(lessFlat_buf.front());
299 |                 lessFlat_buf.pop();
300 |                 laserOdom.readCloud(_fullRes, _cornerCloud, _lessSharp, _cloudFlat, _lessFlat);
301 | 
302 |             }
303 |             else
304 |                 continue;
305 |             lk.unlock();
306 |             std::chrono::steady_clock::time_point  now = std::chrono::steady_clock::now();
307 |             laserOdom.process();
308 |             m_transMain.lock();
309 |             transMain.handleLaserOdom(laserOdom.transformSum());
310 |             Trajectory2.push_back(Eigen::Vector3d(transMain.transformMapped()[3],
311 |                                                 transMain.transformMapped()[4],
312 |                                                 transMain.transformMapped()[5]));
313 |             m_transMain.unlock();
314 |             m_odom2map.lock();
315 |             if(laserOdom._ioRatio < 2 || laserOdom.frameCount() % laserOdom._ioRatio == 1 )
316 |             {
317 |                 while(fullRes_odom2map.size() > MaxQueueLength)
318 |                 {
319 |                     fullRes_odom2map.pop();
320 |                     corner_odom2map.pop();
321 |                     surf_odom2map.pop();
322 |                     trans_odom2map.pop();
323 |                 }
324 |                 fullRes_odom2map.emplace(std::move(*(laserOdom.laserCloud())));
325 |                 corner_odom2map.emplace(std::move(*(laserOdom.lastCornerCloud())));
326 |                 surf_odom2map.emplace(std::move(*(laserOdom.lastSurfaceCloud())));
327 |                 trans_odom2map.push(laserOdom._transformSum);
328 |             }
329 |             m_odom2map.unlock();
330 | 
331 |             auto t2 = std::chrono::steady_clock::now();
332 |             std::chrono::duration<double> time_span = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - now);
333 |             std::cout << "odometry process time: " << time_span.count() << " seconds."<<endl;
334 |             //cout<<laserOdom._transformSum.pos.transpose()<<endl;
335 |         }
336 |     });
337 |    
338 |     pMap = laserMap._laserCloudSurroundDS;
339 |     
340 |     std::thread mapping([&](){
341 |         while(true)
342 |         {
343 |             if((!fullRes_odom2map.empty()) && 
344 |                (!corner_odom2map.empty()) && 
345 |                (!surf_odom2map.empty()) &&
346 |                (!trans_odom2map.empty()))
347 |             {
348 |                 m_odom2map.lock();
349 |                 pcl::PointCloud<pcl::PointXYZI> _fullRes = std::move(fullRes_odom2map.front());
350 |                 fullRes_odom2map.pop();
351 |                 pcl::PointCloud<pcl::PointXYZI> _cornerCloud = std::move(corner_odom2map.front());
352 |                 corner_odom2map.pop();
353 |                 pcl::PointCloud<pcl::PointXYZI> _surfCloud = std::move(surf_odom2map.front());
354 |                 surf_odom2map.pop();
355 |                 loam::Twist trans = trans_odom2map.front();
356 |                 trans_odom2map.pop();
357 |                 m_odom2map.unlock();
358 |                 laserMap.readCloudAndOdom(_fullRes, _cornerCloud, _surfCloud, trans);
359 |             }
360 |             else
361 |                 continue;
362 |             std::chrono::steady_clock::time_point  now = std::chrono::steady_clock::now();
363 |             laserMap.process();
364 |             laserReg = std::move(*(laserMap._laserCloudFullRes));
365 |             m_transMain.lock();
366 |             transMain.handleOdomAftMapped(laserMap.transformAftMapped(), laserMap.transformBefMapped());
367 |             m_transMain.unlock();
368 |             auto t2 = std::chrono::steady_clock::now();
369 |             //Trajectory.push_back(laserMap.transformAftMapped());
370 |             std::chrono::duration<double> time_span = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - now);
371 |             std::cout << "mapping process time: " << time_span.count() << " seconds."<<endl;
372 |         }
373 |    });
374 | 
375 |     Draw();
376 |     registration.join();
377 |     odometry.join();
378 |     mapping.join();
379 |     return (0);
380 | }
381 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/src/lib/BasicLaserOdometry.cpp:
--------------------------------------------------------------------------------
  1 | #include "loam_velodyne/BasicLaserOdometry.h"
  2 | 
  3 | #include "math_utils.h"
  4 | #include <pcl/filters/filter.h>
  5 | #include <Eigen/Eigenvalues>
  6 | #include <Eigen/QR>
  7 | 
  8 | namespace loam
  9 | {
 10 | 
 11 | using std::sin;
 12 | using std::cos;
 13 | using std::asin;
 14 | using std::atan2;
 15 | using std::sqrt;
 16 | using std::fabs;
 17 | using std::pow;
 18 | 
 19 | 
 20 | BasicLaserOdometry::BasicLaserOdometry(float scanPeriod, size_t maxIterations) :
 21 |    _scanPeriod(scanPeriod),
 22 |    _systemInited(false),
 23 |    _frameCount(0),
 24 |    _maxIterations(maxIterations),
 25 |    _deltaTAbort(0.1),
 26 |    _deltaRAbort(0.1),
 27 |    _cornerPointsSharp(new pcl::PointCloud<pcl::PointXYZI>()),
 28 |    _cornerPointsLessSharp(new pcl::PointCloud<pcl::PointXYZI>()),
 29 |    _surfPointsFlat(new pcl::PointCloud<pcl::PointXYZI>()),
 30 |    _surfPointsLessFlat(new pcl::PointCloud<pcl::PointXYZI>()),
 31 |    _laserCloud(new pcl::PointCloud<pcl::PointXYZI>()),
 32 |    _lastCornerCloud(new pcl::PointCloud<pcl::PointXYZI>()),
 33 |    _lastSurfaceCloud(new pcl::PointCloud<pcl::PointXYZI>()),
 34 |    _laserCloudOri(new pcl::PointCloud<pcl::PointXYZI>()),
 35 |    _coeffSel(new pcl::PointCloud<pcl::PointXYZI>())
 36 | {}
 37 | 
 38 | 
 39 | 
 40 | void BasicLaserOdometry::transformToStart(const pcl::PointXYZI& pi, pcl::PointXYZI& po)
 41 | {
 42 |    float s = (1.f / _scanPeriod) * (pi.intensity - int(pi.intensity));
 43 | 
 44 |    po.x = pi.x - s * _transform.pos.x();
 45 |    po.y = pi.y - s * _transform.pos.y();
 46 |    po.z = pi.z - s * _transform.pos.z();
 47 |    po.intensity = pi.intensity;
 48 | 
 49 |    Angle rx = -s * _transform.rot_x.rad();
 50 |    Angle ry = -s * _transform.rot_y.rad();
 51 |    Angle rz = -s * _transform.rot_z.rad();
 52 |    rotateZXY(po, rz, rx, ry);
 53 | }
 54 | 
 55 | 
 56 | 
 57 | size_t BasicLaserOdometry::transformToEnd(pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud)
 58 | {
 59 |    size_t cloudSize = cloud->points.size();
 60 | 
 61 |    for (size_t i = 0; i < cloudSize; i++)
 62 |    {
 63 |       pcl::PointXYZI& point = cloud->points[i];
 64 | 
 65 |       float s = (1.f / _scanPeriod) * (point.intensity - int(point.intensity));
 66 | 
 67 |       point.x -= s * _transform.pos.x();
 68 |       point.y -= s * _transform.pos.y();
 69 |       point.z -= s * _transform.pos.z();
 70 |       point.intensity = int(point.intensity);
 71 | 
 72 |       Angle rx = -s * _transform.rot_x.rad();
 73 |       Angle ry = -s * _transform.rot_y.rad();
 74 |       Angle rz = -s * _transform.rot_z.rad();
 75 |       rotateZXY(point, rz, rx, ry);
 76 |       rotateYXZ(point, _transform.rot_y, _transform.rot_x, _transform.rot_z);
 77 | 
 78 |       point.x += _transform.pos.x() - _imuShiftFromStart.x();
 79 |       point.y += _transform.pos.y() - _imuShiftFromStart.y();
 80 |       point.z += _transform.pos.z() - _imuShiftFromStart.z();
 81 | 
 82 |       rotateZXY(point, _imuRollStart, _imuPitchStart, _imuYawStart);
 83 |       rotateYXZ(point, -_imuYawEnd, -_imuPitchEnd, -_imuRollEnd);
 84 |    }
 85 | 
 86 |    return cloudSize;
 87 | }
 88 | 
 89 | 
 90 | 
 91 | void BasicLaserOdometry::pluginIMURotation(const Angle& bcx, const Angle& bcy, const Angle& bcz,
 92 |                                            const Angle& blx, const Angle& bly, const Angle& blz,
 93 |                                            const Angle& alx, const Angle& aly, const Angle& alz,
 94 |                                            Angle &acx, Angle &acy, Angle &acz)
 95 | {
 96 |    float sbcx = bcx.sin();
 97 |    float cbcx = bcx.cos();
 98 |    float sbcy = bcy.sin();
 99 |    float cbcy = bcy.cos();
100 |    float sbcz = bcz.sin();
101 |    float cbcz = bcz.cos();
102 | 
103 |    float sblx = blx.sin();
104 |    float cblx = blx.cos();
105 |    float sbly = bly.sin();
106 |    float cbly = bly.cos();
107 |    float sblz = blz.sin();
108 |    float cblz = blz.cos();
109 | 
110 |    float salx = alx.sin();
111 |    float calx = alx.cos();
112 |    float saly = aly.sin();
113 |    float caly = aly.cos();
114 |    float salz = alz.sin();
115 |    float calz = alz.cos();
116 | 
117 |    float srx = -sbcx * (salx*sblx + calx * caly*cblx*cbly + calx * cblx*saly*sbly)
118 |       - cbcx * cbcz*(calx*saly*(cbly*sblz - cblz * sblx*sbly)
119 |                      - calx * caly*(sbly*sblz + cbly * cblz*sblx) + cblx * cblz*salx)
120 |       - cbcx * sbcz*(calx*caly*(cblz*sbly - cbly * sblx*sblz)
121 |                      - calx * saly*(cbly*cblz + sblx * sbly*sblz) + cblx * salx*sblz);
122 |    acx = -asin(srx);
123 | 
124 |    float srycrx = (cbcy*sbcz - cbcz * sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz * sblx*sbly)
125 |                                                   - calx * caly*(sbly*sblz + cbly * cblz*sblx) + cblx * cblz*salx)
126 |       - (cbcy*cbcz + sbcx * sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly * sblx*sblz)
127 |                                         - calx * saly*(cbly*cblz + sblx * sbly*sblz) + cblx * salx*sblz)
128 |       + cbcx * sbcy*(salx*sblx + calx * caly*cblx*cbly + calx * cblx*saly*sbly);
129 |    float crycrx = (cbcz*sbcy - cbcy * sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly * sblx*sblz)
130 |                                                   - calx * saly*(cbly*cblz + sblx * sbly*sblz) + cblx * salx*sblz)
131 |       - (sbcy*sbcz + cbcy * cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz * sblx*sbly)
132 |                                         - calx * caly*(sbly*sblz + cbly * cblz*sblx) + cblx * cblz*salx)
133 |       + cbcx * cbcy*(salx*sblx + calx * caly*cblx*cbly + calx * cblx*saly*sbly);
134 |    acy = atan2(srycrx / acx.cos(), crycrx / acx.cos());
135 | 
136 |    float srzcrx = sbcx * (cblx*cbly*(calz*saly - caly * salx*salz) - cblx * sbly*(caly*calz + salx * saly*salz) + calx * salz*sblx)
137 |       - cbcx * cbcz*((caly*calz + salx * saly*salz)*(cbly*sblz - cblz * sblx*sbly)
138 |                      + (calz*saly - caly * salx*salz)*(sbly*sblz + cbly * cblz*sblx)
139 |                      - calx * cblx*cblz*salz)
140 |       + cbcx * sbcz*((caly*calz + salx * saly*salz)*(cbly*cblz + sblx * sbly*sblz)
141 |                      + (calz*saly - caly * salx*salz)*(cblz*sbly - cbly * sblx*sblz)
142 |                      + calx * cblx*salz*sblz);
143 |    float crzcrx = sbcx * (cblx*sbly*(caly*salz - calz * salx*saly) - cblx * cbly*(saly*salz + caly * calz*salx) + calx * calz*sblx)
144 |       + cbcx * cbcz*((saly*salz + caly * calz*salx)*(sbly*sblz + cbly * cblz*sblx)
145 |                      + (caly*salz - calz * salx*saly)*(cbly*sblz - cblz * sblx*sbly)
146 |                      + calx * calz*cblx*cblz)
147 |       - cbcx * sbcz*((saly*salz + caly * calz*salx)*(cblz*sbly - cbly * sblx*sblz)
148 |                      + (caly*salz - calz * salx*saly)*(cbly*cblz + sblx * sbly*sblz)
149 |                      - calx * calz*cblx*sblz);
150 |    acz = atan2(srzcrx / acx.cos(), crzcrx / acx.cos());
151 | }
152 | 
153 | 
154 | 
155 | void BasicLaserOdometry::accumulateRotation(Angle cx, Angle cy, Angle cz,
156 |                                             Angle lx, Angle ly, Angle lz,
157 |                                             Angle &ox, Angle &oy, Angle &oz)
158 | {
159 |    float srx = lx.cos()*cx.cos()*ly.sin()*cz.sin()
160 |       - cx.cos()*cz.cos()*lx.sin()
161 |       - lx.cos()*ly.cos()*cx.sin();
162 |    ox = -asin(srx);
163 | 
164 |    float srycrx = lx.sin()*(cy.cos()*cz.sin() - cz.cos()*cx.sin()*cy.sin())
165 |       + lx.cos()*ly.sin()*(cy.cos()*cz.cos() + cx.sin()*cy.sin()*cz.sin())
166 |       + lx.cos()*ly.cos()*cx.cos()*cy.sin();
167 |    float crycrx = lx.cos()*ly.cos()*cx.cos()*cy.cos()
168 |       - lx.cos()*ly.sin()*(cz.cos()*cy.sin() - cy.cos()*cx.sin()*cz.sin())
169 |       - lx.sin()*(cy.sin()*cz.sin() + cy.cos()*cz.cos()*cx.sin());
170 |    oy = atan2(srycrx / ox.cos(), crycrx / ox.cos());
171 | 
172 |    float srzcrx = cx.sin()*(lz.cos()*ly.sin() - ly.cos()*lx.sin()*lz.sin())
173 |       + cx.cos()*cz.sin()*(ly.cos()*lz.cos() + lx.sin()*ly.sin()*lz.sin())
174 |       + lx.cos()*cx.cos()*cz.cos()*lz.sin();
175 |    float crzcrx = lx.cos()*lz.cos()*cx.cos()*cz.cos()
176 |       - cx.cos()*cz.sin()*(ly.cos()*lz.sin() - lz.cos()*lx.sin()*ly.sin())
177 |       - cx.sin()*(ly.sin()*lz.sin() + ly.cos()*lz.cos()*lx.sin());
178 |    oz = atan2(srzcrx / ox.cos(), crzcrx / ox.cos());
179 | }
180 | 
181 | void BasicLaserOdometry::updateIMU(pcl::PointCloud<pcl::PointXYZ> const& imuTrans)
182 | {
183 |    assert(4 == imuTrans.size());
184 |    _imuPitchStart = imuTrans.points[0].x;
185 |    _imuYawStart = imuTrans.points[0].y;
186 |    _imuRollStart = imuTrans.points[0].z;
187 | 
188 |    _imuPitchEnd = imuTrans.points[1].x;
189 |    _imuYawEnd = imuTrans.points[1].y;
190 |    _imuRollEnd = imuTrans.points[1].z;
191 | 
192 |    _imuShiftFromStart = imuTrans.points[2];
193 |    _imuVeloFromStart = imuTrans.points[3];
194 | }
195 | 
196 | void BasicLaserOdometry::process()
197 | {
198 |    if (!_systemInited)
199 |    {
200 |       _cornerPointsLessSharp.swap(_lastCornerCloud);
201 |       _surfPointsLessFlat.swap(_lastSurfaceCloud);
202 | 
203 |       _lastCornerKDTree.setInputCloud(_lastCornerCloud);
204 |       _lastSurfaceKDTree.setInputCloud(_lastSurfaceCloud);
205 | 
206 |       _transformSum.rot_x += _imuPitchStart;
207 |       _transformSum.rot_z += _imuRollStart;
208 | 
209 |       _systemInited = true;
210 |       return;
211 |    }
212 | 
213 |    pcl::PointXYZI coeff;
214 |    bool isDegenerate = false;
215 |    Eigen::Matrix<float, 6, 6> matP;
216 | 
217 |    _frameCount++;
218 |    _transform.pos -= _imuVeloFromStart * _scanPeriod;
219 | 
220 | 
221 |    size_t lastCornerCloudSize = _lastCornerCloud->points.size();
222 |    size_t lastSurfaceCloudSize = _lastSurfaceCloud->points.size();
223 | 
224 |    if (lastCornerCloudSize > 10 && lastSurfaceCloudSize > 100)
225 |    {
226 |       std::vector<int> pointSearchInd(1);
227 |       std::vector<float> pointSearchSqDis(1);
228 |       std::vector<int> indices;
229 | 
230 |       pcl::removeNaNFromPointCloud(*_cornerPointsSharp, *_cornerPointsSharp, indices);
231 |       size_t cornerPointsSharpNum = _cornerPointsSharp->points.size();
232 |       size_t surfPointsFlatNum = _surfPointsFlat->points.size();
233 | 
234 |       _pointSearchCornerInd1.resize(cornerPointsSharpNum); ///vector<int>  first corner point search index buffer
235 |       _pointSearchCornerInd2.resize(cornerPointsSharpNum);
236 |       _pointSearchSurfInd1.resize(surfPointsFlatNum);
237 |       _pointSearchSurfInd2.resize(surfPointsFlatNum);
238 |       _pointSearchSurfInd3.resize(surfPointsFlatNum);
239 | 
240 |       for (size_t iterCount = 0; iterCount < _maxIterations; iterCount++)
241 |       {
242 |          pcl::PointXYZI pointSel, pointProj, tripod1, tripod2, tripod3;
243 |          _laserCloudOri->clear();
244 |          _coeffSel->clear();
245 | 
246 |          for (int i = 0; i < cornerPointsSharpNum; i++)
247 |          {
248 |             transformToStart(_cornerPointsSharp->points[i], pointSel);//转换到开始扫描时刻
249 | 
250 |             if (iterCount % 5 == 0)
251 |             {
252 |                pcl::removeNaNFromPointCloud(*_lastCornerCloud, *_lastCornerCloud, indices);//去除极端数值点
253 |                _lastCornerKDTree.nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);//找到_lastCornerCloud中离pointSel最近的点
254 |                //在_lastCornerCloud中找到离pointSel第二近的点
255 |                int closestPointInd = -1, minPointInd2 = -1;
256 |                if (pointSearchSqDis[0] < 25)
257 |                {
258 |                   closestPointInd = pointSearchInd[0];
259 |                   int closestPointScan = int(_lastCornerCloud->points[closestPointInd].intensity);
260 | 
261 |                   float pointSqDis, minPointSqDis2 = 25;
262 |                   for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++)
263 |                   {
264 |                      if (int(_lastCornerCloud->points[j].intensity) > closestPointScan + 2.5)
265 |                      {
266 |                         break;
267 |                      }
268 | 
269 |                      pointSqDis = calcSquaredDiff(_lastCornerCloud->points[j], pointSel);
270 | 
271 |                      if (int(_lastCornerCloud->points[j].intensity) > closestPointScan)
272 |                      {
273 |                         if (pointSqDis < minPointSqDis2)
274 |                         {
275 |                            minPointSqDis2 = pointSqDis;
276 |                            minPointInd2 = j;
277 |                         }
278 |                      }
279 |                   }
280 |                   for (int j = closestPointInd - 1; j >= 0; j--)
281 |                   {
282 |                      if (int(_lastCornerCloud->points[j].intensity) < closestPointScan - 2.5)
283 |                      {
284 |                         break;
285 |                      }
286 | 
287 |                      pointSqDis = calcSquaredDiff(_lastCornerCloud->points[j], pointSel);
288 | 
289 |                      if (int(_lastCornerCloud->points[j].intensity) < closestPointScan)
290 |                      {
291 |                         if (pointSqDis < minPointSqDis2)
292 |                         {
293 |                            minPointSqDis2 = pointSqDis;
294 |                            minPointInd2 = j;
295 |                         }
296 |                      }
297 |                   }
298 |                }
299 | 
300 |                _pointSearchCornerInd1[i] = closestPointInd;
301 |                _pointSearchCornerInd2[i] = minPointInd2;
302 |             }//if(iterCount % 5 == 0)
303 | 
304 |             if (_pointSearchCornerInd2[i] >= 0)
305 |             {
306 |                tripod1 = _lastCornerCloud->points[_pointSearchCornerInd1[i]];
307 |                tripod2 = _lastCornerCloud->points[_pointSearchCornerInd2[i]];
308 | 
309 |                float x0 = pointSel.x;
310 |                float y0 = pointSel.y;
311 |                float z0 = pointSel.z;
312 |                float x1 = tripod1.x;
313 |                float y1 = tripod1.y;
314 |                float z1 = tripod1.z;
315 |                float x2 = tripod2.x;
316 |                float y2 = tripod2.y;
317 |                float z2 = tripod2.z;
318 | 
319 |                float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
320 |                                  * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
321 |                                  + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
322 |                                  * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
323 |                                  + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))
324 |                                  * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
325 | 
326 |                float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
327 | 
328 |                float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
329 |                            + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;
330 | 
331 |                float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
332 |                             - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
333 | 
334 |                float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
335 |                             + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
336 | 
337 |                float ld2 = a012 / l12; // Eq. (2)
338 | 
339 |                // TODO: Why writing to a variable that's never read?
340 |                pointProj = pointSel;
341 |                pointProj.x -= la * ld2;
342 |                pointProj.y -= lb * ld2;
343 |                pointProj.z -= lc * ld2;
344 | 
345 |                float s = 1;
346 |                if (iterCount >= 5)
347 |                {
348 |                   s = 1 - 1.8f * fabs(ld2);
349 |                }
350 |                //应该是权重
351 |                coeff.x = s * la;
352 |                coeff.y = s * lb;
353 |                coeff.z = s * lc;
354 |                coeff.intensity = s * ld2;
355 | 
356 |                if (s > 0.1 && ld2 != 0)
357 |                {
358 |                   _laserCloudOri->push_back(_cornerPointsSharp->points[i]);
359 |                   _coeffSel->push_back(coeff);
360 |                }
361 |             }
362 |          }
363 | 
364 |          for (int i = 0; i < surfPointsFlatNum; i++)
365 |          {
366 |             transformToStart(_surfPointsFlat->points[i], pointSel);
367 | 
368 |             if (iterCount % 5 == 0)
369 |             {
370 |                _lastSurfaceKDTree.nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
371 |                int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
372 |                if (pointSearchSqDis[0] < 25)
373 |                {
374 |                   closestPointInd = pointSearchInd[0];
375 |                   int closestPointScan = int(_lastSurfaceCloud->points[closestPointInd].intensity);
376 | 
377 |                   float pointSqDis, minPointSqDis2 = 25, minPointSqDis3 = 25;
378 |                   for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++)
379 |                   {
380 |                      if (int(_lastSurfaceCloud->points[j].intensity) > closestPointScan + 2.5)
381 |                      {
382 |                         break;
383 |                      }
384 | 
385 |                      pointSqDis = calcSquaredDiff(_lastSurfaceCloud->points[j], pointSel);
386 | 
387 |                      if (int(_lastSurfaceCloud->points[j].intensity) <= closestPointScan)
388 |                      {
389 |                         if (pointSqDis < minPointSqDis2)
390 |                         {
391 |                            minPointSqDis2 = pointSqDis;
392 |                            minPointInd2 = j;
393 |                         }
394 |                      }
395 |                      else
396 |                      {
397 |                         if (pointSqDis < minPointSqDis3)
398 |                         {
399 |                            minPointSqDis3 = pointSqDis;
400 |                            minPointInd3 = j;
401 |                         }
402 |                      }
403 |                   }
404 |                   for (int j = closestPointInd - 1; j >= 0; j--)
405 |                   {
406 |                      if (int(_lastSurfaceCloud->points[j].intensity) < closestPointScan - 2.5)
407 |                      {
408 |                         break;
409 |                      }
410 | 
411 |                      pointSqDis = calcSquaredDiff(_lastSurfaceCloud->points[j], pointSel);
412 | 
413 |                      if (int(_lastSurfaceCloud->points[j].intensity) >= closestPointScan)
414 |                      {
415 |                         if (pointSqDis < minPointSqDis2)
416 |                         {
417 |                            minPointSqDis2 = pointSqDis;
418 |                            minPointInd2 = j;
419 |                         }
420 |                      }
421 |                      else
422 |                      {
423 |                         if (pointSqDis < minPointSqDis3)
424 |                         {
425 |                            minPointSqDis3 = pointSqDis;
426 |                            minPointInd3 = j;
427 |                         }
428 |                      }
429 |                   }
430 |                }
431 | 
432 |                _pointSearchSurfInd1[i] = closestPointInd;
433 |                _pointSearchSurfInd2[i] = minPointInd2;
434 |                _pointSearchSurfInd3[i] = minPointInd3;
435 |             }
436 | 
437 |             if (_pointSearchSurfInd2[i] >= 0 && _pointSearchSurfInd3[i] >= 0)
438 |             {
439 |                tripod1 = _lastSurfaceCloud->points[_pointSearchSurfInd1[i]];
440 |                tripod2 = _lastSurfaceCloud->points[_pointSearchSurfInd2[i]];
441 |                tripod3 = _lastSurfaceCloud->points[_pointSearchSurfInd3[i]];
442 | 
443 |                float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z)
444 |                   - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);
445 |                float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x)
446 |                   - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);
447 |                float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y)
448 |                   - (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y);
449 |                float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z);
450 | 
451 |                float ps = sqrt(pa * pa + pb * pb + pc * pc);
452 |                pa /= ps;
453 |                pb /= ps;
454 |                pc /= ps;
455 |                pd /= ps;
456 | 
457 |                float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; //Eq. (3)??
458 | 
459 |                // TODO: Why writing to a variable that's never read? Maybe it should be used afterwards?
460 |                pointProj = pointSel;
461 |                pointProj.x -= pa * pd2;
462 |                pointProj.y -= pb * pd2;
463 |                pointProj.z -= pc * pd2;
464 | 
465 |                float s = 1;
466 |                if (iterCount >= 5)
467 |                {
468 |                   s = 1 - 1.8f * fabs(pd2) / sqrt(calcPointDistance(pointSel));
469 |                }
470 | 
471 |                coeff.x = s * pa;
472 |                coeff.y = s * pb;
473 |                coeff.z = s * pc;
474 |                coeff.intensity = s * pd2;
475 | 
476 |                if (s > 0.1 && pd2 != 0)
477 |                {
478 |                   _laserCloudOri->push_back(_surfPointsFlat->points[i]);
479 |                   _coeffSel->push_back(coeff);
480 |                }
481 |             }
482 |          }
483 | 
484 |          int pointSelNum = _laserCloudOri->points.size();
485 |          if (pointSelNum < 10)
486 |          {
487 |             continue;
488 |          }
489 | 
490 |          Eigen::Matrix<float, Eigen::Dynamic, 6> matA(pointSelNum, 6);
491 |          Eigen::Matrix<float, 6, Eigen::Dynamic> matAt(6, pointSelNum);
492 |          Eigen::Matrix<float, 6, 6> matAtA;
493 |          Eigen::VectorXf matB(pointSelNum);
494 |          Eigen::Matrix<float, 6, 1> matAtB;
495 |          Eigen::Matrix<float, 6, 1> matX;
496 | 
497 |          for (int i = 0; i < pointSelNum; i++)
498 |          {
499 |             const pcl::PointXYZI& pointOri = _laserCloudOri->points[i];
500 |             coeff = _coeffSel->points[i];
501 | 
502 |             float s = 1;
503 | 
504 |             float srx = sin(s * _transform.rot_x.rad());
505 |             float crx = cos(s * _transform.rot_x.rad());
506 |             float sry = sin(s * _transform.rot_y.rad());
507 |             float cry = cos(s * _transform.rot_y.rad());
508 |             float srz = sin(s * _transform.rot_z.rad());
509 |             float crz = cos(s * _transform.rot_z.rad());
510 |             float tx = s * _transform.pos.x();
511 |             float ty = s * _transform.pos.y();
512 |             float tz = s * _transform.pos.z();
513 | 
514 |             float arx = (-s * crx*sry*srz*pointOri.x + s * crx*crz*sry*pointOri.y + s * srx*sry*pointOri.z
515 |                          + s * tx*crx*sry*srz - s * ty*crx*crz*sry - s * tz*srx*sry) * coeff.x
516 |                + (s*srx*srz*pointOri.x - s * crz*srx*pointOri.y + s * crx*pointOri.z
517 |                   + s * ty*crz*srx - s * tz*crx - s * tx*srx*srz) * coeff.y
518 |                + (s*crx*cry*srz*pointOri.x - s * crx*cry*crz*pointOri.y - s * cry*srx*pointOri.z
519 |                   + s * tz*cry*srx + s * ty*crx*cry*crz - s * tx*crx*cry*srz) * coeff.z;
520 | 
521 |             float ary = ((-s * crz*sry - s * cry*srx*srz)*pointOri.x
522 |                          + (s*cry*crz*srx - s * sry*srz)*pointOri.y - s * crx*cry*pointOri.z
523 |                          + tx * (s*crz*sry + s * cry*srx*srz) + ty * (s*sry*srz - s * cry*crz*srx)
524 |                          + s * tz*crx*cry) * coeff.x
525 |                + ((s*cry*crz - s * srx*sry*srz)*pointOri.x
526 |                   + (s*cry*srz + s * crz*srx*sry)*pointOri.y - s * crx*sry*pointOri.z
527 |                   + s * tz*crx*sry - ty * (s*cry*srz + s * crz*srx*sry)
528 |                   - tx * (s*cry*crz - s * srx*sry*srz)) * coeff.z;
529 | 
530 |             float arz = ((-s * cry*srz - s * crz*srx*sry)*pointOri.x + (s*cry*crz - s * srx*sry*srz)*pointOri.y
531 |                          + tx * (s*cry*srz + s * crz*srx*sry) - ty * (s*cry*crz - s * srx*sry*srz)) * coeff.x
532 |                + (-s * crx*crz*pointOri.x - s * crx*srz*pointOri.y
533 |                   + s * ty*crx*srz + s * tx*crx*crz) * coeff.y
534 |                + ((s*cry*crz*srx - s * sry*srz)*pointOri.x + (s*crz*sry + s * cry*srx*srz)*pointOri.y
535 |                   + tx * (s*sry*srz - s * cry*crz*srx) - ty * (s*crz*sry + s * cry*srx*srz)) * coeff.z;
536 | 
537 |             float atx = -s * (cry*crz - srx * sry*srz) * coeff.x + s * crx*srz * coeff.y
538 |                - s * (crz*sry + cry * srx*srz) * coeff.z;
539 | 
540 |             float aty = -s * (cry*srz + crz * srx*sry) * coeff.x - s * crx*crz * coeff.y
541 |                - s * (sry*srz - cry * crz*srx) * coeff.z;
542 | 
543 |             float atz = s * crx*sry * coeff.x - s * srx * coeff.y - s * crx*cry * coeff.z;
544 | 
545 |             float d2 = coeff.intensity;
546 | 
547 |             matA(i, 0) = arx;
548 |             matA(i, 1) = ary;
549 |             matA(i, 2) = arz;
550 |             matA(i, 3) = atx;
551 |             matA(i, 4) = aty;
552 |             matA(i, 5) = atz;
553 |             matB(i, 0) = -0.05 * d2;
554 |          }
555 |          matAt = matA.transpose();
556 |          matAtA = matAt * matA;
557 |          matAtB = matAt * matB;
558 | 
559 |          matX = matAtA.colPivHouseholderQr().solve(matAtB);
560 | 
561 |          if (iterCount == 0)
562 |          {
563 |             Eigen::Matrix<float, 1, 6> matE;
564 |             Eigen::Matrix<float, 6, 6> matV;
565 |             Eigen::Matrix<float, 6, 6> matV2;
566 | 
567 |             Eigen::SelfAdjointEigenSolver< Eigen::Matrix<float, 6, 6> > esolver(matAtA);
568 |             matE = esolver.eigenvalues().real();
569 |             matV = esolver.eigenvectors().real();
570 | 
571 |             matV2 = matV;
572 | 
573 |             isDegenerate = false;
574 |             float eignThre[6] = { 10, 10, 10, 10, 10, 10 };
575 |             for (int i = 0; i < 6; i++)
576 |             {
577 |                if (matE(0, i) < eignThre[i])
578 |                {
579 |                   for (int j = 0; j < 6; j++)
580 |                   {
581 |                      matV2(i, j) = 0;
582 |                   }
583 |                   isDegenerate = true;
584 |                }
585 |                else
586 |                {
587 |                   break;
588 |                }
589 |             }
590 |             matP = matV.inverse() * matV2;
591 |          }
592 | 
593 |          if (isDegenerate)
594 |          {
595 |             Eigen::Matrix<float, 6, 1> matX2(matX);
596 |             matX = matP * matX2;
597 |          }
598 | 
599 |          _transform.rot_x = _transform.rot_x.rad() + matX(0, 0);
600 |          _transform.rot_y = _transform.rot_y.rad() + matX(1, 0);
601 |          _transform.rot_z = _transform.rot_z.rad() + matX(2, 0);
602 |          _transform.pos.x() += matX(3, 0);
603 |          _transform.pos.y() += matX(4, 0);
604 |          _transform.pos.z() += matX(5, 0);
605 | 
606 |          if (!pcl_isfinite(_transform.rot_x.rad())) _transform.rot_x = Angle();
607 |          if (!pcl_isfinite(_transform.rot_y.rad())) _transform.rot_y = Angle();
608 |          if (!pcl_isfinite(_transform.rot_z.rad())) _transform.rot_z = Angle();
609 | 
610 |          if (!pcl_isfinite(_transform.pos.x())) _transform.pos.x() = 0.0;
611 |          if (!pcl_isfinite(_transform.pos.y())) _transform.pos.y() = 0.0;
612 |          if (!pcl_isfinite(_transform.pos.z())) _transform.pos.z() = 0.0;
613 | 
614 |          float deltaR = sqrt(pow(rad2deg(matX(0, 0)), 2) +
615 |                              pow(rad2deg(matX(1, 0)), 2) +
616 |                              pow(rad2deg(matX(2, 0)), 2));
617 |          float deltaT = sqrt(pow(matX(3, 0) * 100, 2) +
618 |                              pow(matX(4, 0) * 100, 2) +
619 |                              pow(matX(5, 0) * 100, 2));
620 | 
621 |          if (deltaR < _deltaRAbort && deltaT < _deltaTAbort)
622 |             break;
623 |       }
624 |    }
625 | 
626 |    Angle rx, ry, rz;
627 |    accumulateRotation(_transformSum.rot_x,
628 |                       _transformSum.rot_y,
629 |                       _transformSum.rot_z,
630 |                       -_transform.rot_x,
631 |                       -_transform.rot_y.rad() * 1.05,
632 |                       -_transform.rot_z,
633 |                       rx, ry, rz);
634 | 
635 |    Vector3 v(_transform.pos.x() - _imuShiftFromStart.x(),
636 |              _transform.pos.y() - _imuShiftFromStart.y(),
637 |              _transform.pos.z() * 1.05 - _imuShiftFromStart.z());
638 |    rotateZXY(v, rz, rx, ry);
639 |    Vector3 trans = _transformSum.pos - v;
640 | 
641 |    pluginIMURotation(rx, ry, rz,
642 |                      _imuPitchStart, _imuYawStart, _imuRollStart,
643 |                      _imuPitchEnd, _imuYawEnd, _imuRollEnd,
644 |                      rx, ry, rz);
645 | 
646 |    _transformSum.rot_x = rx;
647 |    _transformSum.rot_y = ry;
648 |    _transformSum.rot_z = rz;
649 |    _transformSum.pos = trans;
650 | 
651 |    transformToEnd(_cornerPointsLessSharp);
652 |    transformToEnd(_surfPointsLessFlat);
653 | 
654 |    _cornerPointsLessSharp.swap(_lastCornerCloud);
655 |    _surfPointsLessFlat.swap(_lastSurfaceCloud);
656 | 
657 |    lastCornerCloudSize = _lastCornerCloud->points.size();
658 |    lastSurfaceCloudSize = _lastSurfaceCloud->points.size();
659 | 
660 |    if (lastCornerCloudSize > 10 && lastSurfaceCloudSize > 100)
661 |    {
662 |       _lastCornerKDTree.setInputCloud(_lastCornerCloud);
663 |       _lastSurfaceKDTree.setInputCloud(_lastSurfaceCloud);
664 |    }
665 | 
666 | }
667 | 
668 | 
669 | 
670 | } // end namespace loam
671 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/src/lib/BasicScanRegistration.cpp:
--------------------------------------------------------------------------------
  1 | #include <pcl/filters/voxel_grid.h>
  2 | 
  3 | #include "loam_velodyne/BasicScanRegistration.h"
  4 | #include "math_utils.h"
  5 | 
  6 | pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;
  7 | pcl::PointCloud<pcl::PointXYZI> surfPointsLessFlatScanDS;
  8 | 
  9 | namespace loam
 10 | {
 11 | 
 12 | RegistrationParams::RegistrationParams(const float& scanPeriod_,
 13 |                                        const int& imuHistorySize_,
 14 |                                        const int& nFeatureRegions_,
 15 |                                        const int& curvatureRegion_,
 16 |                                        const int& maxCornerSharp_,
 17 |                                        const int& maxSurfaceFlat_,
 18 |                                        const float& lessFlatFilterSize_,
 19 |                                        const float& surfaceCurvatureThreshold_)
 20 |     : scanPeriod(scanPeriod_),
 21 |       imuHistorySize(imuHistorySize_),
 22 |       nFeatureRegions(nFeatureRegions_),
 23 |       curvatureRegion(curvatureRegion_),
 24 |       maxCornerSharp(maxCornerSharp_),
 25 |       maxCornerLessSharp(10 * maxCornerSharp_),
 26 |       maxSurfaceFlat(maxSurfaceFlat_),
 27 |       lessFlatFilterSize(lessFlatFilterSize_),
 28 |       surfaceCurvatureThreshold(surfaceCurvatureThreshold_)
 29 | {};
 30 | 
 31 | void BasicScanRegistration::processScanlines(const Time& scanTime, std::vector<pcl::PointCloud<pcl::PointXYZI>> const& laserCloudScans)
 32 | {
 33 |   // reset internal buffers and set IMU start state based on current scan time
 34 |   reset(scanTime);  
 35 | 
 36 |   // construct sorted full resolution cloud
 37 |   size_t cloudSize = 0;
 38 |   for (int i = 0; i < laserCloudScans.size(); i++) 
 39 |   {
 40 |     _laserCloud += laserCloudScans[i];
 41 |     IndexRange range(cloudSize, 0);
 42 |     cloudSize += laserCloudScans[i].size();
 43 |     range.second = cloudSize > 0 ? cloudSize - 1 : 0;
 44 |     _scanIndices.push_back(range);//每根线的index range
 45 |   }
 46 | 
 47 |   extractFeatures();
 48 |   updateIMUTransform();
 49 | }
 50 | 
 51 | void BasicScanRegistration::processScanlines(std::vector<pcl::PointCloud<pcl::PointXYZI>> const& laserCloudScans)
 52 | {
 53 |   // reset internal buffers and set IMU start state based on current scan time
 54 |   reset();  
 55 | 
 56 |   // construct sorted full resolution cloud
 57 |   size_t cloudSize = 0;
 58 |   for (int i = 0; i < laserCloudScans.size(); i++) 
 59 |   {
 60 |     _laserCloud += laserCloudScans[i];
 61 |     //cout<<laserCloudScans[8].points.size()<<endl;
 62 |     IndexRange range(cloudSize, 0);
 63 |     cloudSize += laserCloudScans[i].size();
 64 |     range.second = cloudSize > 0 ? cloudSize - 1 : 0;
 65 |     _scanIndices.push_back(range);//每根线的index range
 66 |     //cout<<range.first<<" "<<range.second<<endl;
 67 |   }
 68 | 
 69 |   extractFeatures();
 70 |   //updateIMUTransform();
 71 | }
 72 | 
 73 | bool BasicScanRegistration::configure(const RegistrationParams& config)
 74 | {
 75 |   _config = config;
 76 |   _imuHistory.ensureCapacity(_config.imuHistorySize);
 77 |   return true;
 78 | }
 79 | 
 80 | void BasicScanRegistration::reset(const Time& scanTime)
 81 | {
 82 |   _scanTime = scanTime;
 83 | 
 84 |   // re-initialize IMU start index and state
 85 |   _imuIdx = 0;
 86 |   if (hasIMUData()) {
 87 |     interpolateIMUStateFor(0, _imuStart);
 88 |   }
 89 | 
 90 |   // clear internal cloud buffers at the beginning of a sweep
 91 |   if (true/*newSweep*/) {
 92 |     _sweepStart = scanTime;
 93 | 
 94 |     // clear cloud buffers
 95 |     _laserCloud.clear();
 96 |     _cornerPointsSharp.clear();
 97 |     _cornerPointsLessSharp.clear();
 98 |     _surfacePointsFlat.clear();
 99 |     _surfacePointsLessFlat.clear();
100 | 
101 |     // clear scan indices vector
102 |     _scanIndices.clear();
103 |   }
104 | }
105 | void BasicScanRegistration::reset()
106 | {
107 |   //_scanTime = scanTime;
108 | 
109 |   // re-initialize IMU start index and state
110 |   _imuIdx = 0;
111 |   if (hasIMUData()) {
112 |     interpolateIMUStateFor(0, _imuStart);
113 |   }
114 | 
115 |   // clear internal cloud buffers at the beginning of a sweep
116 |   if (true/*newSweep*/) {
117 |     _laserCloud.clear();
118 |     _cornerPointsSharp.clear();
119 |     _cornerPointsLessSharp.clear();
120 |     _surfacePointsFlat.clear();
121 |     _surfacePointsLessFlat.clear();
122 | 
123 |     // clear scan indices vector
124 |     _scanIndices.clear();
125 |   }
126 | }
127 | 
128 | void BasicScanRegistration::updateIMUData(Vector3& acc, IMUState& newState)
129 | {
130 |   if (_imuHistory.size() > 0) {
131 |     // accumulate IMU position and velocity over time
132 |     rotateZXY(acc, newState.roll, newState.pitch, newState.yaw);
133 | 
134 |     const IMUState& prevState = _imuHistory.last();
135 |     float timeDiff = toSec(newState.stamp - prevState.stamp);
136 |     newState.position = prevState.position
137 |                         + (prevState.velocity * timeDiff)
138 |                         + (0.5 * acc * timeDiff * timeDiff);
139 |     newState.velocity = prevState.velocity
140 |                         + acc * timeDiff;
141 |   }
142 | 
143 |   _imuHistory.push(newState);
144 | }
145 | 
146 | 
147 | void BasicScanRegistration::projectPointToStartOfSweep(pcl::PointXYZI& point, float relTime)
148 | {
149 |   // project point to the start of the sweep using corresponding IMU data
150 |   if (hasIMUData())
151 |   {
152 |     setIMUTransformFor(relTime);
153 |     transformToStartIMU(point);
154 |   }
155 | }
156 | 
157 | 
158 | void BasicScanRegistration::setIMUTransformFor(const float& relTime)
159 | {
160 |   interpolateIMUStateFor(relTime, _imuCur);
161 | 
162 |   float relSweepTime = toSec(_scanTime - _sweepStart) + relTime;
163 |   _imuPositionShift = _imuCur.position - _imuStart.position - _imuStart.velocity * relSweepTime;
164 | }
165 | 
166 | 
167 | 
168 | void BasicScanRegistration::transformToStartIMU(pcl::PointXYZI& point)
169 | {
170 |   // rotate point to global IMU system
171 |   rotateZXY(point, _imuCur.roll, _imuCur.pitch, _imuCur.yaw);
172 | 
173 |   // add global IMU position shift
174 |   point.x += _imuPositionShift.x();
175 |   point.y += _imuPositionShift.y();
176 |   point.z += _imuPositionShift.z();
177 | 
178 |   // rotate point back to local IMU system relative to the start IMU state
179 |   rotateYXZ(point, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);
180 | }
181 | 
182 | 
183 | 
184 | void BasicScanRegistration::interpolateIMUStateFor(const float &relTime, IMUState &outputState)
185 | {
186 |   double timeDiff = toSec(_scanTime - _imuHistory[_imuIdx].stamp) + relTime;
187 |   while (_imuIdx < _imuHistory.size() - 1 && timeDiff > 0) {
188 |     _imuIdx++;
189 |     timeDiff = toSec(_scanTime - _imuHistory[_imuIdx].stamp) + relTime;
190 |   }
191 | 
192 |   if (_imuIdx == 0 || timeDiff > 0) {
193 |     outputState = _imuHistory[_imuIdx];
194 |   } else {
195 |     float ratio = -timeDiff / toSec(_imuHistory[_imuIdx].stamp - _imuHistory[_imuIdx - 1].stamp);
196 |     IMUState::interpolate(_imuHistory[_imuIdx], _imuHistory[_imuIdx - 1], ratio, outputState);
197 |   }
198 | }
199 | 
200 | 
201 | void BasicScanRegistration::extractFeatures(const uint16_t& beginIdx)
202 | {
203 |   // extract features from individual scans
204 |   size_t nScans = _scanIndices.size();//nScans = 16
205 |   //cout<<"nScans: "<<nScans<<endl;
206 |   pcl::PointCloud<pcl::PointXYZI> splfs;
207 |   for (size_t i = beginIdx; i < nScans; i++) 
208 |   {
209 |     //cout<<"extractFeatures. "<<i<<endl;
210 |     pcl::PointCloud<pcl::PointXYZI>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<pcl::PointXYZI>);
211 |     size_t scanStartIdx = _scanIndices[i].first;
212 |     size_t scanEndIdx = _scanIndices[i].second;
213 | 
214 |     // skip empty scans
215 |     if (scanEndIdx <= scanStartIdx + 2 * _config.curvatureRegion) 
216 |     {
217 |       continue;
218 |     }
219 | 
220 |     // Quick&Dirty fix for relative point time calculation without IMU data
221 |     /*float scanSize = scanEndIdx - scanStartIdx + 1;
222 |     for (int j = scanStartIdx; j <= scanEndIdx; j++) {
223 |       _laserCloud[j].intensity = i + _scanPeriod * (j - scanStartIdx) / scanSize;
224 |     }*/
225 | 
226 |     // reset scan buffers
227 |     setScanBuffersFor(scanStartIdx, scanEndIdx);//给_scanNeighborPicked赋值
228 | 
229 |     // extract features from equally sized scan regions
230 |     for (int j = 0; j < _config.nFeatureRegions; j++) //对于第i条线提取特征
231 |     {
232 |       //_config.curvatureRegion = 5,nFeatureRegions = 6
233 |       size_t sp = ((scanStartIdx + _config.curvatureRegion) * (_config.nFeatureRegions - j)
234 |                    + (scanEndIdx - _config.curvatureRegion) * j) / _config.nFeatureRegions;
235 |       size_t ep = ((scanStartIdx + _config.curvatureRegion) * (_config.nFeatureRegions - 1 - j)
236 |                    + (scanEndIdx - _config.curvatureRegion) * (j + 1)) / _config.nFeatureRegions - 1;
237 | 
238 |       // skip empty regions
239 |       if (ep <= sp) 
240 |       {
241 |         continue;
242 |       }
243 | 
244 |       size_t regionSize = ep - sp + 1;
245 | 
246 |       // reset region buffers
247 |       setRegionBuffersFor(sp, ep);//计算sp~ep之间的点的曲率并排序
248 | 
249 | 
250 |       // extract corner features
251 |       int largestPickedNum = 0;
252 |       for (size_t k = regionSize; k > 0 && largestPickedNum < _config.maxCornerLessSharp;) //maxCornerLessSharp = 10*maxCornerSharp = 20
253 |       {
254 |         size_t idx = _regionSortIndices[--k];
255 |         size_t scanIdx = idx - scanStartIdx;
256 |         size_t regionIdx = idx - sp;
257 | 
258 |         if (_scanNeighborPicked[scanIdx] == 0 && _regionCurvature[regionIdx] > _config.surfaceCurvatureThreshold) 
259 |         {
260 | 
261 |           largestPickedNum++;
262 |           if (largestPickedNum <= _config.maxCornerSharp) 
263 |           {
264 |             _regionLabel[regionIdx] = CORNER_SHARP;
265 |             _cornerPointsSharp.push_back(_laserCloud[idx]);
266 |           } 
267 |           else 
268 |           {
269 |             _regionLabel[regionIdx] = CORNER_LESS_SHARP;
270 |           }
271 |           _cornerPointsLessSharp.push_back(_laserCloud[idx]);
272 | 
273 |           markAsPicked(idx, scanIdx);
274 |         }
275 |       }
276 | 
277 |       // extract flat surface features
278 |       int smallestPickedNum = 0;
279 |       for (int k = 0; k < regionSize && smallestPickedNum < _config.maxSurfaceFlat; k++) 
280 |       {
281 |         size_t idx = _regionSortIndices[k];
282 |         size_t scanIdx = idx - scanStartIdx;
283 |         size_t regionIdx = idx - sp;
284 | 
285 |         if (_scanNeighborPicked[scanIdx] == 0 && _regionCurvature[regionIdx] < _config.surfaceCurvatureThreshold) 
286 |         {
287 | 
288 |           smallestPickedNum++;
289 |           _regionLabel[regionIdx] = SURFACE_FLAT;
290 |           _surfacePointsFlat.push_back(_laserCloud[idx]);
291 | 
292 |           markAsPicked(idx, scanIdx);
293 |         }
294 |       }
295 | 
296 |       // // extract less flat surface features
297 |       for (int k = 0; k < regionSize; k++) 
298 |       {
299 |         if (_regionLabel[k] <= SURFACE_LESS_FLAT) 
300 |         {
301 |           surfPointsLessFlatScan->push_back(_laserCloud[sp + k]);
302 |         }
303 |       }
304 |     }
305 |       
306 |       downSizeFilter.setInputCloud(surfPointsLessFlatScan);
307 |       downSizeFilter.setLeafSize(0.2, 0.2, 0.2);//0.2, 0.2, 0.2
308 |       
309 |       downSizeFilter.filter(surfPointsLessFlatScanDS);
310 | 
311 |       _surfacePointsLessFlat += surfPointsLessFlatScanDS;
312 |   }
313 | }
314 | 
315 | 
316 | 
317 | void BasicScanRegistration::updateIMUTransform()
318 | {
319 |   _imuTrans[0].x = _imuStart.pitch.rad();
320 |   _imuTrans[0].y = _imuStart.yaw.rad();
321 |   _imuTrans[0].z = _imuStart.roll.rad();
322 | 
323 |   _imuTrans[1].x = _imuCur.pitch.rad();
324 |   _imuTrans[1].y = _imuCur.yaw.rad();
325 |   _imuTrans[1].z = _imuCur.roll.rad();
326 | 
327 |   Vector3 imuShiftFromStart = _imuPositionShift;
328 |   rotateYXZ(imuShiftFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);
329 | 
330 |   _imuTrans[2].x = imuShiftFromStart.x();
331 |   _imuTrans[2].y = imuShiftFromStart.y();
332 |   _imuTrans[2].z = imuShiftFromStart.z();
333 | 
334 |   Vector3 imuVelocityFromStart = _imuCur.velocity - _imuStart.velocity;
335 |   rotateYXZ(imuVelocityFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);
336 | 
337 |   _imuTrans[3].x = imuVelocityFromStart.x();
338 |   _imuTrans[3].y = imuVelocityFromStart.y();
339 |   _imuTrans[3].z = imuVelocityFromStart.z();
340 | }
341 | 
342 | 
343 | void BasicScanRegistration::setRegionBuffersFor(const size_t& startIdx, const size_t& endIdx)
344 | {
345 |   // resize buffers
346 |   size_t regionSize = endIdx - startIdx + 1;
347 |   _regionCurvature.resize(regionSize);
348 |   _regionSortIndices.resize(regionSize);
349 |   _regionLabel.assign(regionSize, SURFACE_LESS_FLAT);
350 | 
351 |   // calculate point curvatures and reset sort indices
352 |   float pointWeight = -2 * _config.curvatureRegion;
353 | 
354 |   for (size_t i = startIdx, regionIdx = 0; i <= endIdx; i++, regionIdx++) 
355 |   {
356 |     float diffX = pointWeight * _laserCloud[i].x;
357 |     float diffY = pointWeight * _laserCloud[i].y;
358 |     float diffZ = pointWeight * _laserCloud[i].z;
359 | 
360 |     for (int j = 1; j <= _config.curvatureRegion; j++) 
361 |     {
362 |       diffX += _laserCloud[i + j].x + _laserCloud[i - j].x;
363 |       diffY += _laserCloud[i + j].y + _laserCloud[i - j].y;
364 |       diffZ += _laserCloud[i + j].z + _laserCloud[i - j].z;
365 |     }
366 | 
367 |     _regionCurvature[regionIdx] = diffX * diffX + diffY * diffY + diffZ * diffZ;
368 |     _regionSortIndices[regionIdx] = i;
369 |   }
370 | 
371 |   // sort point curvatures
372 |   for (size_t i = 1; i < regionSize; i++) 
373 |   {
374 |     for (size_t j = i; j >= 1; j--) 
375 |     {
376 |       if (_regionCurvature[_regionSortIndices[j] - startIdx] < _regionCurvature[_regionSortIndices[j - 1] - startIdx]) 
377 |       {
378 |         std::swap(_regionSortIndices[j], _regionSortIndices[j - 1]);
379 |       }
380 |     }
381 |   }
382 | }
383 | 
384 | 
385 | void BasicScanRegistration::setScanBuffersFor(const size_t& startIdx, const size_t& endIdx)
386 | {
387 |   // resize buffers
388 |   size_t scanSize = endIdx - startIdx + 1;
389 |   _scanNeighborPicked.assign(scanSize, 0);//长度为scanSize的int数组，初值为0
390 | 
391 |   // mark unreliable points as picked
392 |   for (size_t i = startIdx + _config.curvatureRegion; i < endIdx - _config.curvatureRegion; i++) 
393 |   {
394 |     const pcl::PointXYZI& previousPoint = (_laserCloud[i - 1]);
395 |     const pcl::PointXYZI& point = (_laserCloud[i]);
396 |     const pcl::PointXYZI& nextPoint = (_laserCloud[i + 1]);
397 | 
398 |     float diffNext = calcSquaredDiff(nextPoint, point);//计算delta_x^2+delta_y^2+delta_z^2
399 | 
400 |     if (diffNext > 0.1) 
401 |     {
402 |       //计算2点离雷达的距离
403 |       float depth1 = calcPointDistance(point);
404 |       float depth2 = calcPointDistance(nextPoint);
405 | 
406 |       if (depth1 > depth2) 
407 |       {
408 |         float weighted_distance = std::sqrt(calcSquaredDiff(nextPoint, point, depth2 / depth1)) / depth2;
409 | 
410 |         if (weighted_distance < 0.1) 
411 |         {
412 |           std::fill_n(&_scanNeighborPicked[i - startIdx - _config.curvatureRegion], _config.curvatureRegion + 1, 1);
413 | 
414 |           continue;
415 |         }
416 |       } 
417 |       else 
418 |       {
419 |         float weighted_distance = std::sqrt(calcSquaredDiff(point, nextPoint, depth1 / depth2)) / depth1;
420 | 
421 |         if (weighted_distance < 0.1) {
422 |           std::fill_n(&_scanNeighborPicked[i - startIdx + 1], _config.curvatureRegion + 1, 1);
423 |         }
424 |       }
425 |     }
426 | 
427 |     float diffPrevious = calcSquaredDiff(point, previousPoint);
428 |     float dis = calcSquaredPointDistance(point);
429 | 
430 |     if (diffNext > 0.0002 * dis && diffPrevious > 0.0002 * dis)
431 |     {
432 |       _scanNeighborPicked[i - startIdx] = 1;
433 |     }
434 |   }
435 | }
436 | 
437 | 
438 | 
439 | void BasicScanRegistration::markAsPicked(const size_t& cloudIdx, const size_t& scanIdx)
440 | {
441 |   _scanNeighborPicked[scanIdx] = 1;
442 | 
443 |   for (int i = 1; i <= _config.curvatureRegion; i++) {
444 |     if (calcSquaredDiff(_laserCloud[cloudIdx + i], _laserCloud[cloudIdx + i - 1]) > 0.05) {
445 |       break;
446 |     }
447 | 
448 |     _scanNeighborPicked[scanIdx + i] = 1;
449 |   }
450 | 
451 |   for (int i = 1; i <= _config.curvatureRegion; i++) {
452 |     if (calcSquaredDiff(_laserCloud[cloudIdx - i], _laserCloud[cloudIdx - i + 1]) > 0.05) {
453 |       break;
454 |     }
455 | 
456 |     _scanNeighborPicked[scanIdx - i] = 1;
457 |   }
458 | }
459 | 
460 | 
461 | }
462 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/src/lib/BasicTransformMaintenance.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright 2013, Ji Zhang, Carnegie Mellon University
  2 | // Further contributions copyright (c) 2016, Southwest Research Institute
  3 | // All rights reserved.
  4 | //
  5 | // Redistribution and use in source and binary forms, with or without
  6 | // modification, are permitted provided that the following conditions are met:
  7 | //
  8 | // 1. Redistributions of source code must retain the above copyright notice,
  9 | //    this list of conditions and the following disclaimer.
 10 | // 2. Redistributions in binary form must reproduce the above copyright notice,
 11 | //    this list of conditions and the following disclaimer in the documentation
 12 | //    and/or other materials provided with the distribution.
 13 | // 3. Neither the name of the copyright holder nor the names of its
 14 | //    contributors may be used to endorse or promote products derived from this
 15 | //    software without specific prior written permission.
 16 | //
 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 | // POSSIBILITY OF SUCH DAMAGE.
 28 | //
 29 | // This is an implementation of the algorithm described in the following paper:
 30 | //   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
 31 | //     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
 32 | 
 33 | #include <cmath>
 34 | #include "loam_velodyne/BasicTransformMaintenance.h"
 35 | 
 36 | 
 37 | namespace loam
 38 | {
 39 | 
 40 | using std::sin;
 41 | using std::cos;
 42 | using std::asin;
 43 | using std::atan2;
 44 | 
 45 | 
 46 | void BasicTransformMaintenance::updateOdometry(double pitch, double yaw, double roll, double x, double y, double z)
 47 | {
 48 |    _transformSum[0] = pitch;
 49 |    _transformSum[1] = yaw;
 50 |    _transformSum[2] = roll;
 51 |    _transformSum[3] = x;
 52 |    _transformSum[4] = y;
 53 |    _transformSum[5] = z;
 54 | }
 55 | 
 56 | void BasicTransformMaintenance::updateMappingTransform(double pitch, double yaw, double roll, double x, double y, double z, double twist_rot_x, double twist_rot_y, double twist_rot_z, double twist_pos_x, double twist_pos_y, double twist_pos_z)
 57 | {
 58 |    _transformAftMapped[0] = pitch;
 59 |    _transformAftMapped[1] = yaw;
 60 |    _transformAftMapped[2] = roll;
 61 | 
 62 |    _transformAftMapped[3] = x;
 63 |    _transformAftMapped[4] = y;
 64 |    _transformAftMapped[5] = z;
 65 | 
 66 |    _transformBefMapped[0] = twist_rot_x;
 67 |    _transformBefMapped[1] = twist_rot_y;
 68 |    _transformBefMapped[2] = twist_rot_z;
 69 | 
 70 |    _transformBefMapped[3] = twist_pos_x;
 71 |    _transformBefMapped[4] = twist_pos_y;
 72 |    _transformBefMapped[5] = twist_pos_z;
 73 | }
 74 | 
 75 | void BasicTransformMaintenance::updateMappingTransform(Twist const& transformAftMapped, Twist const& transformBefMapped)
 76 | {
 77 |    updateMappingTransform( transformAftMapped.rot_x.rad(), transformAftMapped.rot_y.rad(), transformAftMapped.rot_z.rad(), 
 78 |                            transformAftMapped.pos.x(), transformAftMapped.pos.y(), transformAftMapped.pos.z(),
 79 |                            transformBefMapped.rot_x.rad(), transformBefMapped.rot_y.rad(), transformBefMapped.rot_z.rad(),
 80 |                            transformBefMapped.pos.x(), transformBefMapped.pos.y(), transformBefMapped.pos.z());
 81 | }
 82 | 
 83 | void BasicTransformMaintenance::transformAssociateToMap()
 84 | {
 85 |    float x1 = cos(_transformSum[1]) * (_transformBefMapped[3] - _transformSum[3])
 86 |       - sin(_transformSum[1]) * (_transformBefMapped[5] - _transformSum[5]);
 87 |    float y1 = _transformBefMapped[4] - _transformSum[4];
 88 |    float z1 = sin(_transformSum[1]) * (_transformBefMapped[3] - _transformSum[3])
 89 |       + cos(_transformSum[1]) * (_transformBefMapped[5] - _transformSum[5]);
 90 | 
 91 |    float x2 = x1;
 92 |    float y2 = cos(_transformSum[0]) * y1 + sin(_transformSum[0]) * z1;
 93 |    float z2 = -sin(_transformSum[0]) * y1 + cos(_transformSum[0]) * z1;
 94 | 
 95 |    _transformIncre[3] = cos(_transformSum[2]) * x2 + sin(_transformSum[2]) * y2;
 96 |    _transformIncre[4] = -sin(_transformSum[2]) * x2 + cos(_transformSum[2]) * y2;
 97 |    _transformIncre[5] = z2;
 98 | 
 99 |    float sbcx = sin(_transformSum[0]);
100 |    float cbcx = cos(_transformSum[0]);
101 |    float sbcy = sin(_transformSum[1]);
102 |    float cbcy = cos(_transformSum[1]);
103 |    float sbcz = sin(_transformSum[2]);
104 |    float cbcz = cos(_transformSum[2]);
105 | 
106 |    float sblx = sin(_transformBefMapped[0]);
107 |    float cblx = cos(_transformBefMapped[0]);
108 |    float sbly = sin(_transformBefMapped[1]);
109 |    float cbly = cos(_transformBefMapped[1]);
110 |    float sblz = sin(_transformBefMapped[2]);
111 |    float cblz = cos(_transformBefMapped[2]);
112 | 
113 |    float salx = sin(_transformAftMapped[0]);
114 |    float calx = cos(_transformAftMapped[0]);
115 |    float saly = sin(_transformAftMapped[1]);
116 |    float caly = cos(_transformAftMapped[1]);
117 |    float salz = sin(_transformAftMapped[2]);
118 |    float calz = cos(_transformAftMapped[2]);
119 | 
120 |    float srx = -sbcx * (salx*sblx + calx * cblx*salz*sblz + calx * calz*cblx*cblz)
121 |       - cbcx * sbcy*(calx*calz*(cbly*sblz - cblz * sblx*sbly)
122 |          - calx * salz*(cbly*cblz + sblx * sbly*sblz)
123 |          + cblx * salx*sbly)
124 |       - cbcx * cbcy*(calx*salz*(cblz*sbly - cbly * sblx*sblz)
125 |          - calx * calz*(sbly*sblz + cbly * cblz*sblx)
126 |          + cblx * cbly*salx);
127 |    _transformMapped[0] = -asin(srx);
128 | 
129 |    float srycrx = sbcx * (cblx*cblz*(caly*salz - calz * salx*saly)
130 |       - cblx * sblz*(caly*calz + salx * saly*salz) + calx * saly*sblx)
131 |       - cbcx * cbcy*((caly*calz + salx * saly*salz)*(cblz*sbly - cbly * sblx*sblz)
132 |          + (caly*salz - calz * salx*saly)*(sbly*sblz + cbly * cblz*sblx)
133 |          - calx * cblx*cbly*saly)
134 |       + cbcx * sbcy*((caly*calz + salx * saly*salz)*(cbly*cblz + sblx * sbly*sblz)
135 |          + (caly*salz - calz * salx*saly)*(cbly*sblz - cblz * sblx*sbly)
136 |          + calx * cblx*saly*sbly);
137 |    float crycrx = sbcx * (cblx*sblz*(calz*saly - caly * salx*salz)
138 |       - cblx * cblz*(saly*salz + caly * calz*salx) + calx * caly*sblx)
139 |       + cbcx * cbcy*((saly*salz + caly * calz*salx)*(sbly*sblz + cbly * cblz*sblx)
140 |          + (calz*saly - caly * salx*salz)*(cblz*sbly - cbly * sblx*sblz)
141 |          + calx * caly*cblx*cbly)
142 |       - cbcx * sbcy*((saly*salz + caly * calz*salx)*(cbly*sblz - cblz * sblx*sbly)
143 |          + (calz*saly - caly * salx*salz)*(cbly*cblz + sblx * sbly*sblz)
144 |          - calx * caly*cblx*sbly);
145 |    _transformMapped[1] = atan2(srycrx / cos(_transformMapped[0]),
146 |       crycrx / cos(_transformMapped[0]));
147 | 
148 |    float srzcrx = (cbcz*sbcy - cbcy * sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly * sblx*sblz)
149 |       - calx * calz*(sbly*sblz + cbly * cblz*sblx)
150 |       + cblx * cbly*salx)
151 |       - (cbcy*cbcz + sbcx * sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz * sblx*sbly)
152 |          - calx * salz*(cbly*cblz + sblx * sbly*sblz)
153 |          + cblx * salx*sbly)
154 |       + cbcx * sbcz*(salx*sblx + calx * cblx*salz*sblz + calx * calz*cblx*cblz);
155 |    float crzcrx = (cbcy*sbcz - cbcz * sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz * sblx*sbly)
156 |       - calx * salz*(cbly*cblz + sblx * sbly*sblz)
157 |       + cblx * salx*sbly)
158 |       - (sbcy*sbcz + cbcy * cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly * sblx*sblz)
159 |          - calx * calz*(sbly*sblz + cbly * cblz*sblx)
160 |          + cblx * cbly*salx)
161 |       + cbcx * cbcz*(salx*sblx + calx * cblx*salz*sblz + calx * calz*cblx*cblz);
162 |    _transformMapped[2] = atan2(srzcrx / cos(_transformMapped[0]),
163 |       crzcrx / cos(_transformMapped[0]));
164 | 
165 |    x1 = cos(_transformMapped[2]) * _transformIncre[3] - sin(_transformMapped[2]) * _transformIncre[4];
166 |    y1 = sin(_transformMapped[2]) * _transformIncre[3] + cos(_transformMapped[2]) * _transformIncre[4];
167 |    z1 = _transformIncre[5];
168 | 
169 |    x2 = x1;
170 |    y2 = cos(_transformMapped[0]) * y1 - sin(_transformMapped[0]) * z1;
171 |    z2 = sin(_transformMapped[0]) * y1 + cos(_transformMapped[0]) * z1;
172 | 
173 |    _transformMapped[3] = _transformAftMapped[3]
174 |       - (cos(_transformMapped[1]) * x2 + sin(_transformMapped[1]) * z2);
175 |    _transformMapped[4] = _transformAftMapped[4] - y2;
176 |    _transformMapped[5] = _transformAftMapped[5]
177 |       - (-sin(_transformMapped[1]) * x2 + cos(_transformMapped[1]) * z2);
178 | }
179 | 
180 | 
181 | } // end namespace loam
182 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/src/lib/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | include_directories(../../include)
 2 | add_library(loam SHARED
 3 |             math_utils.h
 4 |             BasicScanRegistration.cpp
 5 |             MultiScanRegistration.cpp
 6 |             BasicLaserOdometry.cpp
 7 |             LaserOdometry.cpp
 8 |             BasicLaserMapping.cpp
 9 |             LaserMapping.cpp
10 |             BasicTransformMaintenance.cpp
11 |             TransformMaintenance.cpp)
12 | target_link_libraries(loam ${PCL_LIBRARIES})
13 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/src/lib/LaserMapping.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright 2013, Ji Zhang, Carnegie Mellon University
  2 | // Further contributions copyright (c) 2016, Southwest Research Institute
  3 | // All rights reserved.
  4 | //
  5 | // Redistribution and use in source and binary forms, with or without
  6 | // modification, are permitted provided that the following conditions are met:
  7 | //
  8 | // 1. Redistributions of source code must retain the above copyright notice,
  9 | //    this list of conditions and the following disclaimer.
 10 | // 2. Redistributions in binary form must reproduce the above copyright notice,
 11 | //    this list of conditions and the following disclaimer in the documentation
 12 | //    and/or other materials provided with the distribution.
 13 | // 3. Neither the name of the copyright holder nor the names of its
 14 | //    contributors may be used to endorse or promote products derived from this
 15 | //    software without specific prior written permission.
 16 | //
 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 | // POSSIBILITY OF SUCH DAMAGE.
 28 | //
 29 | // This is an implementation of the algorithm described in the following paper:
 30 | //   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
 31 | //     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
 32 | 
 33 | #include "loam_velodyne/LaserMapping.h"
 34 | #include "math_utils.h"
 35 | namespace loam
 36 | {
 37 | 
 38 | LaserMapping::LaserMapping(const float& scanPeriod, const size_t& maxIterations)
 39 | {
 40 |    // initialize mapping odometry and odometry tf messages
 41 |    // _odomAftMapped.header.frame_id = "/camera_init";
 42 |    // _odomAftMapped.child_frame_id = "/aft_mapped";
 43 | 
 44 |    // _aftMappedTrans.frame_id_ = "/camera_init";
 45 |    // _aftMappedTrans.child_frame_id_ = "/aft_mapped";
 46 | }
 47 | 
 48 | 
 49 | // bool LaserMapping::setup(ros::NodeHandle& node, ros::NodeHandle& privateNode)
 50 | // {
 51 | //    // fetch laser mapping params
 52 | //    float fParam;
 53 | //    int iParam;
 54 | 
 55 | //    if (privateNode.getParam("scanPeriod", fParam))
 56 | //    {
 57 | //       if (fParam <= 0)
 58 | //       {
 59 | //          ROS_ERROR("Invalid scanPeriod parameter: %f (expected > 0)", fParam);
 60 | //          return false;
 61 | //       }
 62 | //       else
 63 | //       {
 64 | //          setScanPeriod(fParam);
 65 | //          ROS_INFO("Set scanPeriod: %g", fParam);
 66 | //       }
 67 | //    }
 68 | 
 69 | //    if (privateNode.getParam("maxIterations", iParam))
 70 | //    {
 71 | //       if (iParam < 1)
 72 | //       {
 73 | //          ROS_ERROR("Invalid maxIterations parameter: %d (expected > 0)", iParam);
 74 | //          return false;
 75 | //       }
 76 | //       else
 77 | //       {
 78 | //          setMaxIterations(iParam);
 79 | //          ROS_INFO("Set maxIterations: %d", iParam);
 80 | //       }
 81 | //    }
 82 | 
 83 | //    if (privateNode.getParam("deltaTAbort", fParam))
 84 | //    {
 85 | //       if (fParam <= 0)
 86 | //       {
 87 | //          ROS_ERROR("Invalid deltaTAbort parameter: %f (expected > 0)", fParam);
 88 | //          return false;
 89 | //       }
 90 | //       else
 91 | //       {
 92 | //          setDeltaTAbort(fParam);
 93 | //          ROS_INFO("Set deltaTAbort: %g", fParam);
 94 | //       }
 95 | //    }
 96 | 
 97 | //    if (privateNode.getParam("deltaRAbort", fParam))
 98 | //    {
 99 | //       if (fParam <= 0)
100 | //       {
101 | //          ROS_ERROR("Invalid deltaRAbort parameter: %f (expected > 0)", fParam);
102 | //          return false;
103 | //       }
104 | //       else
105 | //       {
106 | //          setDeltaRAbort(fParam);
107 | //          ROS_INFO("Set deltaRAbort: %g", fParam);
108 | //       }
109 | //    }
110 | 
111 | //    if (privateNode.getParam("cornerFilterSize", fParam))
112 | //    {
113 | //       if (fParam < 0.001)
114 | //       {
115 | //          ROS_ERROR("Invalid cornerFilterSize parameter: %f (expected >= 0.001)", fParam);
116 | //          return false;
117 | //       }
118 | //       else
119 | //       {
120 | //          downSizeFilterCorner().setLeafSize(fParam, fParam, fParam);
121 | //          ROS_INFO("Set corner down size filter leaf size: %g", fParam);
122 | //       }
123 | //    }
124 | 
125 | //    if (privateNode.getParam("surfaceFilterSize", fParam))
126 | //    {
127 | //       if (fParam < 0.001)
128 | //       {
129 | //          ROS_ERROR("Invalid surfaceFilterSize parameter: %f (expected >= 0.001)", fParam);
130 | //          return false;
131 | //       }
132 | //       else
133 | //       {
134 | //          downSizeFilterSurf().setLeafSize(fParam, fParam, fParam);
135 | //          ROS_INFO("Set surface down size filter leaf size: %g", fParam);
136 | //       }
137 | //    }
138 | 
139 | //    if (privateNode.getParam("mapFilterSize", fParam))
140 | //    {
141 | //       if (fParam < 0.001)
142 | //       {
143 | //          ROS_ERROR("Invalid mapFilterSize parameter: %f (expected >= 0.001)", fParam);
144 | //          return false;
145 | //       }
146 | //       else
147 | //       {
148 | //          downSizeFilterMap().setLeafSize(fParam, fParam, fParam);
149 | //          ROS_INFO("Set map down size filter leaf size: %g", fParam);
150 | //       }
151 | //    }
152 | 
153 | //    // advertise laser mapping topics
154 | //    _pubLaserCloudSurround = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surround", 1);
155 | //    _pubLaserCloudFullRes  = node.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_registered", 2);
156 | //    _pubOdomAftMapped      = node.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 5);
157 | 
158 | //    // subscribe to laser odometry topics
159 | //    _subLaserCloudCornerLast = node.subscribe<sensor_msgs::PointCloud2>
160 | //       ("/laser_cloud_corner_last", 2, &LaserMapping::laserCloudCornerLastHandler, this);
161 | 
162 | //    _subLaserCloudSurfLast = node.subscribe<sensor_msgs::PointCloud2>
163 | //       ("/laser_cloud_surf_last", 2, &LaserMapping::laserCloudSurfLastHandler, this);
164 | 
165 | //    _subLaserOdometry = node.subscribe<nav_msgs::Odometry>
166 | //       ("/laser_odom_to_init", 5, &LaserMapping::laserOdometryHandler, this);
167 | 
168 | //    _subLaserCloudFullRes = node.subscribe<sensor_msgs::PointCloud2>
169 | //       ("/velodyne_cloud_3", 2, &LaserMapping::laserCloudFullResHandler, this);
170 | 
171 | //    // subscribe to IMU topic
172 | //    _subImu = node.subscribe<sensor_msgs::Imu>("/imu/data", 50, &LaserMapping::imuHandler, this);
173 | 
174 | //    return true;
175 | // }
176 | 
177 | 
178 | 
179 | // void LaserMapping::laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLastMsg)
180 | // {
181 | //    _timeLaserCloudCornerLast = cornerPointsLastMsg->header.stamp;
182 | //    laserCloudCornerLast().clear();
183 | //    pcl::fromROSMsg(*cornerPointsLastMsg, laserCloudCornerLast());
184 | //    _newLaserCloudCornerLast = true;
185 | // }
186 | 
187 | // void LaserMapping::laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& surfacePointsLastMsg)
188 | // {
189 | //    _timeLaserCloudSurfLast = surfacePointsLastMsg->header.stamp;
190 | //    laserCloudSurfLast().clear();
191 | //    pcl::fromROSMsg(*surfacePointsLastMsg, laserCloudSurfLast());
192 | //    _newLaserCloudSurfLast = true;
193 | // }
194 | 
195 | // void LaserMapping::laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullResMsg)
196 | // {
197 | //    _timeLaserCloudFullRes = laserCloudFullResMsg->header.stamp;
198 | //    laserCloud().clear();
199 | //    pcl::fromROSMsg(*laserCloudFullResMsg, laserCloud());
200 | //    _newLaserCloudFullRes = true;
201 | // }
202 | 
203 | // void LaserMapping::laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
204 | // {
205 | //    _timeLaserOdometry = laserOdometry->header.stamp;
206 | 
207 | //    double roll, pitch, yaw;
208 | //    geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
209 | //    tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
210 | 
211 | //    updateOdometry(-pitch, -yaw, roll,
212 | //                   laserOdometry->pose.pose.position.x,
213 | //                   laserOdometry->pose.pose.position.y,
214 | //                   laserOdometry->pose.pose.position.z);
215 | 
216 | //    _newLaserOdometry = true;
217 | // }
218 | 
219 | 
220 | // void LaserMapping::handleOdometry(double rx, double ry, double rz, double x, double y, double z)
221 | // {
222 | //    //_timeLaserOdometry = laserOdometry->header.stamp;
223 | 
224 | //    updateOdometry(rx, ry, rz, x, y, z);
225 | //    _newLaserOdometry = true;
226 | // }
227 | 
228 | void LaserMapping::readCloudAndOdom(const pcl::PointCloud<pcl::PointXYZI>& fullRes, 
229 |                                     const pcl::PointCloud<pcl::PointXYZI>& corner, 
230 |                                     const pcl::PointCloud<pcl::PointXYZI>& surf, 
231 |                                     const Twist& twist)
232 | {
233 |    laserCloud().clear();
234 |    laserCloud() = fullRes;
235 |     _newLaserCloudFullRes = true;
236 | 
237 |    laserCloudCornerLast().clear();
238 |    laserCloudCornerLast() = corner;
239 |    _newLaserCloudCornerLast = true;
240 | 
241 |    laserCloudSurfLast().clear();
242 |    laserCloudSurfLast() = surf;
243 |    _newLaserCloudSurfLast = true;
244 | 
245 |    updateOdometry(twist);
246 |    _newLaserOdometry = true;
247 | 
248 | }
249 | // void LaserMapping::imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
250 | // {
251 | //    double roll, pitch, yaw;
252 | //    tf::Quaternion orientation;
253 | //    tf::quaternionMsgToTF(imuIn->orientation, orientation);
254 | //    tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
255 | //    updateIMU({ fromROSTime(imuIn->header.stamp) , roll, pitch });
256 | // }
257 | 
258 | // void LaserMapping::spin()
259 | // {
260 | //    ros::Rate rate(100);
261 | //    bool status = ros::ok();
262 | 
263 | //    while (status)
264 | //    {
265 | //       ros::spinOnce();
266 | 
267 | //       // try processing buffered data
268 | //       process();
269 | 
270 | //       status = ros::ok();
271 | //       rate.sleep();
272 | //    }
273 | // }
274 | 
275 | void LaserMapping::reset()
276 | {
277 |    _newLaserCloudCornerLast = false;
278 |    _newLaserCloudSurfLast = false;
279 |    _newLaserCloudFullRes = false;
280 |    _newLaserOdometry = false;
281 | }
282 | 
283 | bool LaserMapping::hasNewData()
284 | {
285 |    return _newLaserCloudCornerLast && _newLaserCloudSurfLast &&
286 |       _newLaserCloudFullRes && _newLaserOdometry; //&&
287 |       // fabs((_timeLaserCloudCornerLast - _timeLaserOdometry).toSec()) < 0.005 &&
288 |       // fabs((_timeLaserCloudSurfLast - _timeLaserOdometry).toSec()) < 0.005 &&
289 |       // fabs((_timeLaserCloudFullRes - _timeLaserOdometry).toSec()) < 0.005;
290 | }
291 | 
292 | void LaserMapping::process()
293 | {
294 |    if (!hasNewData())// waiting for new data to arrive...
295 |       return;
296 | 
297 |    reset();// reset flags, etc.
298 | 
299 |    if (!BasicLaserMapping::process(/*fromROSTime(_timeLaserOdometry)*/))
300 |       return;
301 | 
302 |    //publishResult();
303 | }
304 | 
305 | // void LaserMapping::publishResult()
306 | // {
307 | //    // publish new map cloud according to the input output ratio
308 | //    if (hasFreshMap()) // publish new map cloud
309 | //       publishCloudMsg(_pubLaserCloudSurround, laserCloudSurroundDS(), _timeLaserOdometry, "/camera_init");
310 | 
311 | //    // publish transformed full resolution input cloud
312 | //    publishCloudMsg(_pubLaserCloudFullRes, laserCloud(), _timeLaserOdometry, "/camera_init");
313 | 
314 | //    // publish odometry after mapped transformations
315 | //    geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw
316 | //    (transformAftMapped().rot_z.rad(), -transformAftMapped().rot_x.rad(), -transformAftMapped().rot_y.rad());
317 | 
318 | //    _odomAftMapped.header.stamp = _timeLaserOdometry;
319 | //    _odomAftMapped.pose.pose.orientation.x = -geoQuat.y;
320 | //    _odomAftMapped.pose.pose.orientation.y = -geoQuat.z;
321 | //    _odomAftMapped.pose.pose.orientation.z = geoQuat.x;
322 | //    _odomAftMapped.pose.pose.orientation.w = geoQuat.w;
323 | //    _odomAftMapped.pose.pose.position.x = transformAftMapped().pos.x();
324 | //    _odomAftMapped.pose.pose.position.y = transformAftMapped().pos.y();
325 | //    _odomAftMapped.pose.pose.position.z = transformAftMapped().pos.z();
326 | //    _odomAftMapped.twist.twist.angular.x = transformBefMapped().rot_x.rad();
327 | //    _odomAftMapped.twist.twist.angular.y = transformBefMapped().rot_y.rad();
328 | //    _odomAftMapped.twist.twist.angular.z = transformBefMapped().rot_z.rad();
329 | //    _odomAftMapped.twist.twist.linear.x = transformBefMapped().pos.x();
330 | //    _odomAftMapped.twist.twist.linear.y = transformBefMapped().pos.y();
331 | //    _odomAftMapped.twist.twist.linear.z = transformBefMapped().pos.z();
332 | //    _pubOdomAftMapped.publish(_odomAftMapped);
333 | 
334 | //    _aftMappedTrans.stamp_ = _timeLaserOdometry;
335 | //    _aftMappedTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
336 | //    _aftMappedTrans.setOrigin(tf::Vector3(transformAftMapped().pos.x(),
337 | //                                          transformAftMapped().pos.y(),
338 | //                                          transformAftMapped().pos.z()));
339 | //    _tfBroadcaster.sendTransform(_aftMappedTrans);
340 | // }
341 | 
342 | } // end namespace loam
343 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/src/lib/LaserOdometry.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright 2013, Ji Zhang, Carnegie Mellon University
  2 | // Further contributions copyright (c) 2016, Southwest Research Institute
  3 | // All rights reserved.
  4 | //
  5 | // Redistribution and use in source and binary forms, with or without
  6 | // modification, are permitted provided that the following conditions are met:
  7 | //
  8 | // 1. Redistributions of source code must retain the above copyright notice,
  9 | //    this list of conditions and the following disclaimer.
 10 | // 2. Redistributions in binary form must reproduce the above copyright notice,
 11 | //    this list of conditions and the following disclaimer in the documentation
 12 | //    and/or other materials provided with the distribution.
 13 | // 3. Neither the name of the copyright holder nor the names of its
 14 | //    contributors may be used to endorse or promote products derived from this
 15 | //    software without specific prior written permission.
 16 | //
 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 | // POSSIBILITY OF SUCH DAMAGE.
 28 | //
 29 | // This is an implementation of the algorithm described in the following paper:
 30 | //   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
 31 | //     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
 32 | 
 33 | #include <pcl/filters/filter.h>
 34 | 
 35 | #include "loam_velodyne/LaserOdometry.h"
 36 | //#include "loam_velodyne/common.h"
 37 | #include "math_utils.h"
 38 | 
 39 | namespace loam
 40 | {
 41 | 
 42 |   using std::sin;
 43 |   using std::cos;
 44 |   using std::asin;
 45 |   using std::atan2;
 46 |   using std::sqrt;
 47 |   using std::fabs;
 48 |   using std::pow;
 49 | 
 50 | 
 51 |   LaserOdometry::LaserOdometry(float scanPeriod, uint16_t ioRatio, size_t maxIterations):
 52 |     BasicLaserOdometry(scanPeriod, maxIterations),
 53 |     _ioRatio(ioRatio)
 54 |   {
 55 |     // initialize odometry and odometry tf messages
 56 |     // _laserOdometryMsg.header.frame_id = "/camera_init";
 57 |     // _laserOdometryMsg.child_frame_id  = "/laser_odom";
 58 | 
 59 |     // _laserOdometryTrans.frame_id_       = "/camera_init";
 60 |     // _laserOdometryTrans.child_frame_id_ = "/laser_odom";
 61 |   }
 62 | 
 63 | 
 64 |   // bool LaserOdometry::setup(ros::NodeHandle &node, ros::NodeHandle &privateNode)
 65 |   // {
 66 |   //   // fetch laser odometry params
 67 |   //   float fParam;
 68 |   //   int iParam;
 69 | 
 70 |   //   if (privateNode.getParam("scanPeriod", fParam))
 71 |   //   {
 72 |   //     if (fParam <= 0)
 73 |   //     {
 74 |   //       ROS_ERROR("Invalid scanPeriod parameter: %f (expected > 0)", fParam);
 75 |   //       return false;
 76 |   //     }
 77 |   //     else
 78 |   //     {
 79 |   //       setScanPeriod(fParam);
 80 |   //       ROS_INFO("Set scanPeriod: %g", fParam);
 81 |   //     }
 82 |   //   }
 83 | 
 84 |   //   if (privateNode.getParam("ioRatio", iParam))
 85 |   //   {
 86 |   //     if (iParam < 1)
 87 |   //     {
 88 |   //       ROS_ERROR("Invalid ioRatio parameter: %d (expected > 0)", iParam);
 89 |   //       return false;
 90 |   //     }
 91 |   //     else
 92 |   //     {
 93 |   //       _ioRatio = iParam;
 94 |   //       ROS_INFO("Set ioRatio: %d", iParam);
 95 |   //     }
 96 |   //   }
 97 | 
 98 |   //   if (privateNode.getParam("maxIterations", iParam))
 99 |   //   {
100 |   //     if (iParam < 1)
101 |   //     {
102 |   //       ROS_ERROR("Invalid maxIterations parameter: %d (expected > 0)", iParam);
103 |   //       return false;
104 |   //     }
105 |   //     else
106 |   //     {
107 |   //       setMaxIterations(iParam);
108 |   //       ROS_INFO("Set maxIterations: %d", iParam);
109 |   //     }
110 |   //   }
111 | 
112 |   //   if (privateNode.getParam("deltaTAbort", fParam))
113 |   //   {
114 |   //     if (fParam <= 0)
115 |   //     {
116 |   //       ROS_ERROR("Invalid deltaTAbort parameter: %f (expected > 0)", fParam);
117 |   //       return false;
118 |   //     }
119 |   //     else
120 |   //     {
121 |   //       setDeltaTAbort(fParam);
122 |   //       ROS_INFO("Set deltaTAbort: %g", fParam);
123 |   //     }
124 |   //   }
125 | 
126 |   //   if (privateNode.getParam("deltaRAbort", fParam))
127 |   //   {
128 |   //     if (fParam <= 0)
129 |   //     {
130 |   //       ROS_ERROR("Invalid deltaRAbort parameter: %f (expected > 0)", fParam);
131 |   //       return false;
132 |   //     }
133 |   //     else
134 |   //     {
135 |   //       setDeltaRAbort(fParam);
136 |   //       ROS_INFO("Set deltaRAbort: %g", fParam);
137 |   //     }
138 |   //   }
139 | 
140 |   //   // advertise laser odometry topics
141 |   //   _pubLaserCloudCornerLast = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);
142 |   //   _pubLaserCloudSurfLast = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);
143 |   //   _pubLaserCloudFullRes = node.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 2);
144 |   //   _pubLaserOdometry = node.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 5);
145 | 
146 |   //   // subscribe to scan registration topics
147 |   //   _subCornerPointsSharp = node.subscribe<sensor_msgs::PointCloud2>
148 |   //     ("/laser_cloud_sharp", 2, &LaserOdometry::laserCloudSharpHandler, this);
149 | 
150 |   //   _subCornerPointsLessSharp = node.subscribe<sensor_msgs::PointCloud2>
151 |   //     ("/laser_cloud_less_sharp", 2, &LaserOdometry::laserCloudLessSharpHandler, this);
152 | 
153 |   //   _subSurfPointsFlat = node.subscribe<sensor_msgs::PointCloud2>
154 |   //     ("/laser_cloud_flat", 2, &LaserOdometry::laserCloudFlatHandler, this);
155 | 
156 |   //   _subSurfPointsLessFlat = node.subscribe<sensor_msgs::PointCloud2>
157 |   //     ("/laser_cloud_less_flat", 2, &LaserOdometry::laserCloudLessFlatHandler, this);
158 | 
159 |   //   _subLaserCloudFullRes = node.subscribe<sensor_msgs::PointCloud2>
160 |   //     ("/velodyne_cloud_2", 2, &LaserOdometry::laserCloudFullResHandler, this);
161 | 
162 |   //   _subImuTrans = node.subscribe<sensor_msgs::PointCloud2>
163 |   //     ("/imu_trans", 5, &LaserOdometry::imuTransHandler, this);
164 | 
165 |   //   return true;
166 |   // }
167 | 
168 |   void LaserOdometry::reset()
169 |   {
170 |     _newCornerPointsSharp = false;
171 |     _newCornerPointsLessSharp = false;
172 |     _newSurfPointsFlat = false;
173 |     _newSurfPointsLessFlat = false;
174 |     _newLaserCloudFullRes = false;
175 |     _newImuTrans = false;
176 |   }
177 | 
178 |   // void LaserOdometry::laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsSharpMsg)
179 |   // {
180 |   //   _timeCornerPointsSharp = cornerPointsSharpMsg->header.stamp;
181 | 
182 |   //   cornerPointsSharp()->clear();
183 |   //   pcl::fromROSMsg(*cornerPointsSharpMsg, *cornerPointsSharp());
184 |   //   std::vector<int> indices;
185 |   //   pcl::removeNaNFromPointCloud(*cornerPointsSharp(), *cornerPointsSharp(), indices);
186 |   //   _newCornerPointsSharp = true;
187 |   // }
188 | 
189 | 
190 | 
191 |   // void LaserOdometry::laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr& cornerPointsLessSharpMsg)
192 |   // {
193 |   //   _timeCornerPointsLessSharp = cornerPointsLessSharpMsg->header.stamp;
194 | 
195 |   //   cornerPointsLessSharp()->clear();
196 |   //   pcl::fromROSMsg(*cornerPointsLessSharpMsg, *cornerPointsLessSharp());
197 |   //   std::vector<int> indices;
198 |   //   pcl::removeNaNFromPointCloud(*cornerPointsLessSharp(), *cornerPointsLessSharp(), indices);
199 |   //   _newCornerPointsLessSharp = true;
200 |   // }
201 | 
202 | 
203 | 
204 |   // void LaserOdometry::laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlatMsg)
205 |   // {
206 |   //   _timeSurfPointsFlat = surfPointsFlatMsg->header.stamp;
207 | 
208 |   //   surfPointsFlat()->clear();
209 |   //   pcl::fromROSMsg(*surfPointsFlatMsg, *surfPointsFlat());
210 |   //   std::vector<int> indices;
211 |   //   pcl::removeNaNFromPointCloud(*surfPointsFlat(), *surfPointsFlat(), indices);
212 |   //   _newSurfPointsFlat = true;
213 |   // }
214 | 
215 | 
216 | 
217 |   // void LaserOdometry::laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsLessFlatMsg)
218 |   // {
219 |   //   _timeSurfPointsLessFlat = surfPointsLessFlatMsg->header.stamp;
220 | 
221 |   //   surfPointsLessFlat()->clear();
222 |   //   pcl::fromROSMsg(*surfPointsLessFlatMsg, *surfPointsLessFlat());
223 |   //   std::vector<int> indices;
224 |   //   pcl::removeNaNFromPointCloud(*surfPointsLessFlat(), *surfPointsLessFlat(), indices);
225 |   //   _newSurfPointsLessFlat = true;
226 |   // }
227 | 
228 | 
229 | 
230 |   // void LaserOdometry::laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullResMsg)
231 |   // {
232 |   //   _timeLaserCloudFullRes = laserCloudFullResMsg->header.stamp;
233 | 
234 |   //   laserCloud()->clear();
235 |   //   pcl::fromROSMsg(*laserCloudFullResMsg, *laserCloud());
236 |   //   std::vector<int> indices;
237 |   //   pcl::removeNaNFromPointCloud(*laserCloud(), *laserCloud(), indices);
238 |   //   _newLaserCloudFullRes = true;
239 |   // }
240 | 
241 | 
242 | 
243 |   // void LaserOdometry::imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTransMsg)
244 |   // {
245 |   //   _timeImuTrans = imuTransMsg->header.stamp;
246 | 
247 |   //   pcl::PointCloud<pcl::PointXYZ> imuTrans;
248 |   //   pcl::fromROSMsg(*imuTransMsg, imuTrans);
249 |   //   updateIMU(imuTrans);
250 |   //   _newImuTrans = true;
251 |   // }
252 | 
253 | 
254 |   // void LaserOdometry::spin()
255 |   // {
256 |   //   ros::Rate rate(100);
257 |   //   bool status = ros::ok();
258 | 
259 |   //   // loop until shutdown
260 |   //   while (status)
261 |   //   {
262 |   //     ros::spinOnce();
263 | 
264 |   //     // try processing new data
265 |   //     process();
266 | 
267 |   //     status = ros::ok();
268 |   //     rate.sleep();
269 |   //   }
270 |   // }
271 | 
272 | 
273 |   bool LaserOdometry::hasNewData()
274 |   {
275 |     return _newCornerPointsSharp && _newCornerPointsLessSharp && _newSurfPointsFlat &&
276 |       _newSurfPointsLessFlat && _newLaserCloudFullRes; //&& _newImuTrans &&
277 |       // fabs((_timeCornerPointsSharp - _timeSurfPointsLessFlat).toSec()) < 0.005 &&
278 |       // fabs((_timeCornerPointsLessSharp - _timeSurfPointsLessFlat).toSec()) < 0.005 &&
279 |       // fabs((_timeSurfPointsFlat - _timeSurfPointsLessFlat).toSec()) < 0.005 &&
280 |       // fabs((_timeLaserCloudFullRes - _timeSurfPointsLessFlat).toSec()) < 0.005 &&
281 |       // fabs((_timeImuTrans - _timeSurfPointsLessFlat).toSec()) < 0.005;
282 |   }
283 | 
284 | 
285 | 
286 |   void LaserOdometry::process()
287 |   {
288 |     if (!hasNewData())
289 |       return;// waiting for new data to arrive...
290 | 
291 |     reset();// reset flags, etc.
292 |     BasicLaserOdometry::process();
293 |     //publishResult();
294 |   }
295 | 
296 | 
297 |   // void LaserOdometry::publishResult()
298 |   // {
299 |   //   // publish odometry transformations
300 |   //   geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformSum().rot_z.rad(),
301 |   //                                                                              -transformSum().rot_x.rad(),
302 |   //                                                                              -transformSum().rot_y.rad());
303 | 
304 |   //   _laserOdometryMsg.header.stamp            = _timeSurfPointsLessFlat;
305 |   //   _laserOdometryMsg.pose.pose.orientation.x = -geoQuat.y;
306 |   //   _laserOdometryMsg.pose.pose.orientation.y = -geoQuat.z;
307 |   //   _laserOdometryMsg.pose.pose.orientation.z = geoQuat.x;
308 |   //   _laserOdometryMsg.pose.pose.orientation.w = geoQuat.w;
309 |   //   _laserOdometryMsg.pose.pose.position.x    = transformSum().pos.x();
310 |   //   _laserOdometryMsg.pose.pose.position.y    = transformSum().pos.y();
311 |   //   _laserOdometryMsg.pose.pose.position.z    = transformSum().pos.z();
312 |   //   _pubLaserOdometry.publish(_laserOdometryMsg);
313 | 
314 |   //   _laserOdometryTrans.stamp_ = _timeSurfPointsLessFlat;
315 |   //   _laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
316 |   //   _laserOdometryTrans.setOrigin(tf::Vector3(transformSum().pos.x(), transformSum().pos.y(), transformSum().pos.z()));
317 |   //   _tfBroadcaster.sendTransform(_laserOdometryTrans);
318 | 
319 |   //   // publish cloud results according to the input output ratio
320 |   //   if (_ioRatio < 2 || frameCount() % _ioRatio == 1)
321 |   //   {
322 |   //     ros::Time sweepTime = _timeSurfPointsLessFlat;
323 |   //     publishCloudMsg(_pubLaserCloudCornerLast, *lastCornerCloud(), sweepTime, "/camera");
324 |   //     publishCloudMsg(_pubLaserCloudSurfLast, *lastSurfaceCloud(), sweepTime, "/camera");
325 | 
326 |   //     transformToEnd(laserCloud());  // transform full resolution cloud to sweep end before sending it
327 |   //     publishCloudMsg(_pubLaserCloudFullRes, *laserCloud(), sweepTime, "/camera");
328 |   //   }
329 |   // }
330 | 
331 |   void LaserOdometry::readCloud(const pcl::PointCloud<pcl::PointXYZI>& fullRes, 
332 |                                   const pcl::PointCloud<pcl::PointXYZI>& cornerSharp, 
333 |                                   const pcl::PointCloud<pcl::PointXYZI>& lessSharp, 
334 |                                   const pcl::PointCloud<pcl::PointXYZI>& pointFlat, 
335 |                                   const pcl::PointCloud<pcl::PointXYZI>& lessFlat)
336 |   {
337 |     laserCloud()->clear();
338 |     std::vector<int> indices;
339 |     pcl::removeNaNFromPointCloud(fullRes, *laserCloud(), indices);
340 |     _newLaserCloudFullRes = true;
341 | 
342 |     cornerPointsSharp()->clear();
343 |     //std::vector<int> indices;
344 |     pcl::removeNaNFromPointCloud(cornerSharp, *surfPointsLessFlat(), indices);
345 |      _newCornerPointsSharp = true;
346 | 
347 |     cornerPointsLessSharp()->clear();
348 |     //std::vector<int> indices;
349 |     pcl::removeNaNFromPointCloud(lessSharp, *cornerPointsLessSharp(), indices);
350 |     _newCornerPointsLessSharp = true;
351 | 
352 |     surfPointsFlat()->clear();
353 |     //std::vector<int> indices;
354 |     pcl::removeNaNFromPointCloud(pointFlat, *surfPointsFlat(), indices);
355 |     _newSurfPointsFlat = true;
356 | 
357 |     surfPointsLessFlat()->clear();
358 |     //std::vector<int> indices;
359 |     pcl::removeNaNFromPointCloud(lessFlat, *surfPointsLessFlat(), indices);
360 |     _newSurfPointsLessFlat = true;
361 | 
362 |   }
363 | 
364 | 
365 | } // end namespace loam
366 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/src/lib/MultiScanRegistration.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright 2013, Ji Zhang, Carnegie Mellon University
  2 | // Further contributions copyright (c) 2016, Southwest Research Institute
  3 | // All rights reserved.
  4 | //
  5 | // Redistribution and use in source and binary forms, with or without
  6 | // modification, are permitted provided that the following conditions are met:
  7 | //
  8 | // 1. Redistributions of source code must retain the above copyright notice,
  9 | //    this list of conditions and the following disclaimer.
 10 | // 2. Redistributions in binary form must reproduce the above copyright notice,
 11 | //    this list of conditions and the following disclaimer in the documentation
 12 | //    and/or other materials provided with the distribution.
 13 | // 3. Neither the name of the copyright holder nor the names of its
 14 | //    contributors may be used to endorse or promote products derived from this
 15 | //    software without specific prior written permission.
 16 | //
 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 | // POSSIBILITY OF SUCH DAMAGE.
 28 | //
 29 | // This is an implementation of the algorithm described in the following paper:
 30 | //   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
 31 | //     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
 32 | 
 33 | #include "loam_velodyne/MultiScanRegistration.h"
 34 | #include "math_utils.h"
 35 | 
 36 | //#include <pcl_conversions/pcl_conversions.h>
 37 | 
 38 | 
 39 | namespace loam {
 40 | 
 41 | MultiScanMapper::MultiScanMapper(const float& lowerBound,
 42 |                                  const float& upperBound,
 43 |                                  const uint16_t& nScanRings)
 44 |     : _lowerBound(lowerBound),
 45 |       _upperBound(upperBound),
 46 |       _nScanRings(nScanRings),
 47 |       _factor((nScanRings - 1) / (upperBound - lowerBound))
 48 | {
 49 | 
 50 | }
 51 | 
 52 | void MultiScanMapper::set(const float &lowerBound,
 53 |                           const float &upperBound,
 54 |                           const uint16_t &nScanRings)
 55 | {
 56 |   _lowerBound = lowerBound;
 57 |   _upperBound = upperBound;
 58 |   _nScanRings = nScanRings;
 59 |   _factor = (nScanRings - 1) / (upperBound - lowerBound);
 60 | }
 61 | 
 62 | 
 63 | 
 64 | int MultiScanMapper::getRingForAngle(const float& angle) {
 65 |   return int(((angle * 180 / M_PI) - _lowerBound) * _factor + 0.5);
 66 | }
 67 | 
 68 | 
 69 | 
 70 | 
 71 | 
 72 | 
 73 | MultiScanRegistration::MultiScanRegistration(const MultiScanMapper& scanMapper)
 74 |     : _scanMapper(scanMapper)
 75 | {};
 76 | 
 77 | 
 78 | 
 79 | // bool MultiScanRegistration::setup(ros::NodeHandle& node, ros::NodeHandle& privateNode)
 80 | // {
 81 | //   RegistrationParams config;
 82 | //   if (!setupROS(node, privateNode, config))
 83 | //     return false;
 84 | 
 85 | //   configure(config);
 86 | //   return true;
 87 | // }
 88 | 
 89 | // bool MultiScanRegistration::setupROS(ros::NodeHandle& node, ros::NodeHandle& privateNode, RegistrationParams& config_out)
 90 | // {
 91 | //   if (!ScanRegistration::setupROS(node, privateNode, config_out))
 92 | //     return false;
 93 | 
 94 | //   // fetch scan mapping params
 95 | //   std::string lidarName;
 96 | 
 97 | //   if (privateNode.getParam("lidar", lidarName)) 
 98 | //   {
 99 | //     if (lidarName == "VLP-16") 
100 | //     {
101 | //       _scanMapper = MultiScanMapper::Velodyne_VLP_16();
102 | //     } 
103 | //     else if (lidarName == "HDL-32") 
104 | //     {
105 | //       _scanMapper = MultiScanMapper::Velodyne_HDL_32();
106 | //     } 
107 | //     else if (lidarName == "HDL-64E") 
108 | //     {
109 | //       _scanMapper = MultiScanMapper::Velodyne_HDL_64E();
110 | //     } 
111 | //     else 
112 | //     {
113 | //       ROS_ERROR("Invalid lidar parameter: %s (only \"VLP-16\", \"HDL-32\" and \"HDL-64E\" are supported)", lidarName.c_str());
114 | //       return false;
115 | //     }
116 | 
117 | //     ROS_INFO("Set  %s  scan mapper.", lidarName.c_str());
118 | //     if (!privateNode.hasParam("scanPeriod")) 
119 | //     {
120 | //       config_out.scanPeriod = 0.1;
121 | //       ROS_INFO("Set scanPeriod: %f", config_out.scanPeriod);
122 | //     }
123 | //   } 
124 | //   else 
125 | //   {
126 | //     float vAngleMin, vAngleMax;
127 | //     int nScanRings;
128 | 
129 | //     if (privateNode.getParam("minVerticalAngle", vAngleMin) &&
130 | //         privateNode.getParam("maxVerticalAngle", vAngleMax) &&
131 | //         privateNode.getParam("nScanRings", nScanRings)) 
132 | //     {
133 | //       if (vAngleMin >= vAngleMax) 
134 | //       {
135 | //         ROS_ERROR("Invalid vertical range (min >= max)");
136 | //         return false;
137 | //       } 
138 | //       else if (nScanRings < 2) 
139 | //       {
140 | //         ROS_ERROR("Invalid number of scan rings (n < 2)");
141 | //         return false;
142 | //       }
143 | 
144 | //       _scanMapper.set(vAngleMin, vAngleMax, nScanRings);
145 | //       ROS_INFO("Set linear scan mapper from %g to %g degrees with %d scan rings.", vAngleMin, vAngleMax, nScanRings);
146 | //     }
147 | //   }
148 | 
149 | //   // subscribe to input cloud topic
150 | //   _subLaserCloud = node.subscribe<sensor_msgs::PointCloud2>
151 | //       ("/multi_scan_points", 2, &MultiScanRegistration::handleCloudMessage, this);
152 | 
153 | //   return true;
154 | // }
155 | 
156 | 
157 | 
158 | void MultiScanRegistration::handleCloud(const pcl::PointCloud<pcl::PointXYZ> &laserCloudIn)
159 | {
160 |   // if (_systemDelay > 0) 
161 |   // {
162 |   //   --_systemDelay;
163 |   //   return;
164 |   // }
165 | 
166 |   // fetch new input cloud
167 | 
168 |   process(laserCloudIn/*, fromROSTime(laserCloudMsg->header.stamp)*/);
169 | }
170 | 
171 | 
172 | 
173 | void MultiScanRegistration::process(const pcl::PointCloud<pcl::PointXYZ> &laserCloudIn/*, const Time& scanTime*/)//scanTime:发送时候的时间
174 | {
175 |   size_t cloudSize = laserCloudIn.size();
176 | 
177 |   // determine scan start and end orientations
178 |   float startOri = -std::atan2(laserCloudIn[0].y, laserCloudIn[0].x);
179 |   float endOri = -std::atan2(laserCloudIn[cloudSize - 1].y,
180 |                              laserCloudIn[cloudSize - 1].x) + 2 * float(M_PI);
181 |   if (endOri - startOri > 3 * M_PI) {
182 |     endOri -= 2 * M_PI;
183 |   } else if (endOri - startOri < M_PI) {
184 |     endOri += 2 * M_PI;
185 |   }
186 | 
187 |   bool halfPassed = false;
188 |   pcl::PointXYZI point;
189 |   _laserCloudScans.resize(_scanMapper.getNumberOfScanRings());//_scanMapper.getNumberOfScanRings() = 16
190 |   // clear all scanline points
191 |   std::for_each(_laserCloudScans.begin(), _laserCloudScans.end(), [](pcl::PointCloud<pcl::PointXYZI>& v) {v.clear(); }); 
192 |   //cout<<_laserCloudScans[5].points.size()<<endl;
193 |   // extract valid points from input cloud
194 |   for (int i = 0; i < cloudSize; i++) 
195 |   {
196 |     point.x = laserCloudIn[i].y;
197 |     point.y = laserCloudIn[i].z;
198 |     point.z = laserCloudIn[i].x;
199 | 
200 |     // skip NaN and INF valued points
201 |     if (!pcl_isfinite(point.x) ||
202 |         !pcl_isfinite(point.y) ||
203 |         !pcl_isfinite(point.z)) {
204 |       continue;
205 |     }
206 | 
207 |     // skip zero valued points
208 |     if (point.x * point.x + point.y * point.y + point.z * point.z < 0.0001) {
209 |       continue;
210 |     }
211 | 
212 |     // calculate vertical point angle and scan ID
213 |     float angle = std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z));
214 |     int scanID = _scanMapper.getRingForAngle(angle);
215 |     if (scanID >= _scanMapper.getNumberOfScanRings() || scanID < 0 )//_scanMapper.getNumberOfScanRings() = 16
216 |     {
217 |       continue;
218 |     }
219 | 
220 |     // calculate horizontal point angle
221 |     float ori = -std::atan2(point.x, point.z);
222 |     if (!halfPassed) 
223 |     {
224 |       if (ori < startOri - M_PI / 2) 
225 |       {
226 |         ori += 2 * M_PI;
227 |       } 
228 |       else if (ori > startOri + M_PI * 3 / 2) 
229 |       {
230 |         ori -= 2 * M_PI;
231 |       }
232 | 
233 |       if (ori - startOri > M_PI) 
234 |       {
235 |         halfPassed = true;
236 |       }
237 |     } 
238 |     else 
239 |     {
240 |       ori += 2 * M_PI;
241 | 
242 |       if (ori < endOri - M_PI * 3 / 2) 
243 |       {
244 |         ori += 2 * M_PI;
245 |       } 
246 |       else if (ori > endOri + M_PI / 2) 
247 |       {
248 |         ori -= 2 * M_PI;
249 |       }
250 |     }
251 | 
252 |     // calculate relative scan time based on point orientation
253 |     float relTime = config().scanPeriod * (ori - startOri) / (endOri - startOri);
254 |     point.intensity = scanID + relTime;
255 | 
256 |     projectPointToStartOfSweep(point, relTime);//没IMU，point不变
257 | 
258 |     _laserCloudScans[scanID].push_back(point);
259 |   }
260 | 
261 |   // processScanlines(scanTime, _laserCloudScans);
262 |   processScanlines(_laserCloudScans);
263 |   //publishResult();
264 | }
265 | 
266 | } // end namespace loam
267 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/src/lib/TransformMaintenance.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright 2013, Ji Zhang, Carnegie Mellon University
  2 | // Further contributions copyright (c) 2016, Southwest Research Institute
  3 | // All rights reserved.
  4 | //
  5 | // Redistribution and use in source and binary forms, with or without
  6 | // modification, are permitted provided that the following conditions are met:
  7 | //
  8 | // 1. Redistributions of source code must retain the above copyright notice,
  9 | //    this list of conditions and the following disclaimer.
 10 | // 2. Redistributions in binary form must reproduce the above copyright notice,
 11 | //    this list of conditions and the following disclaimer in the documentation
 12 | //    and/or other materials provided with the distribution.
 13 | // 3. Neither the name of the copyright holder nor the names of its
 14 | //    contributors may be used to endorse or promote products derived from this
 15 | //    software without specific prior written permission.
 16 | //
 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 | // POSSIBILITY OF SUCH DAMAGE.
 28 | //
 29 | // This is an implementation of the algorithm described in the following paper:
 30 | //   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
 31 | //     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
 32 | 
 33 | #include "loam_velodyne/TransformMaintenance.h"
 34 | 
 35 | namespace loam
 36 | {
 37 | 
 38 | // TransformMaintenance::TransformMaintenance()
 39 | // {
 40 | //    // initialize odometry and odometry tf messages
 41 | //    _laserOdometry2.header.frame_id = "/camera_init";
 42 | //    _laserOdometry2.child_frame_id = "/camera";
 43 | 
 44 | //    _laserOdometryTrans2.frame_id_ = "/camera_init";
 45 | //    _laserOdometryTrans2.child_frame_id_ = "/camera";
 46 | // }
 47 | 
 48 | 
 49 | // bool TransformMaintenance::setup(ros::NodeHandle &node, ros::NodeHandle &privateNode)
 50 | // {
 51 | //    // advertise integrated laser odometry topic
 52 | //    _pubLaserOdometry2 = node.advertise<nav_msgs::Odometry>("/integrated_to_init", 5);
 53 | 
 54 | //    // subscribe to laser odometry and mapping odometry topics
 55 | //    _subLaserOdometry = node.subscribe<nav_msgs::Odometry>
 56 | //       ("/laser_odom_to_init", 5, &TransformMaintenance::laserOdometryHandler, this);
 57 | 
 58 | //    _subOdomAftMapped = node.subscribe<nav_msgs::Odometry>
 59 | //       ("/aft_mapped_to_init", 5, &TransformMaintenance::odomAftMappedHandler, this);
 60 | 
 61 | //    return true;
 62 | // }
 63 | 
 64 | 
 65 | 
 66 | // void TransformMaintenance::laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
 67 | // {
 68 | //    double roll, pitch, yaw;
 69 | //    geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
 70 | //    tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
 71 | 
 72 | //    updateOdometry(-pitch, -yaw, roll,
 73 | //       laserOdometry->pose.pose.position.x,
 74 | //       laserOdometry->pose.pose.position.y,
 75 | //       laserOdometry->pose.pose.position.z);
 76 | 
 77 | //    transformAssociateToMap();
 78 | 
 79 | //    geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformMapped()[2], -transformMapped()[0], -transformMapped()[1]);
 80 | 
 81 |    // _laserOdometry2.header.stamp = laserOdometry->header.stamp;
 82 |    // _laserOdometry2.pose.pose.orientation.x = -geoQuat.y;
 83 |    // _laserOdometry2.pose.pose.orientation.y = -geoQuat.z;
 84 |    // _laserOdometry2.pose.pose.orientation.z = geoQuat.x;
 85 |    // _laserOdometry2.pose.pose.orientation.w = geoQuat.w;
 86 |    // _laserOdometry2.pose.pose.position.x = transformMapped()[3];
 87 |    // _laserOdometry2.pose.pose.position.y = transformMapped()[4];
 88 |    // _laserOdometry2.pose.pose.position.z = transformMapped()[5];
 89 |    // _pubLaserOdometry2.publish(_laserOdometry2);
 90 | 
 91 |    // _laserOdometryTrans2.stamp_ = laserOdometry->header.stamp;
 92 |    // _laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
 93 |    // _laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped()[3], transformMapped()[4], transformMapped()[5]));
 94 |    // _tfBroadcaster2.sendTransform(_laserOdometryTrans2);
 95 | //}
 96 | 
 97 | void TransformMaintenance::handleLaserOdom(const Twist& odom)
 98 | {
 99 |    updateOdometry(odom.rot_x.rad(), 
100 |                   odom.rot_y.rad(), 
101 |                   odom.rot_z.rad(),
102 |                   odom.pos.x(),
103 |                   odom.pos.y(),
104 |                   odom.pos.z());
105 | 
106 |    transformAssociateToMap();
107 | }
108 | 
109 | // void TransformMaintenance::odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped)
110 | // {
111 | //    double roll, pitch, yaw;
112 | //    geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation;
113 | //    tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
114 | 
115 | //    updateMappingTransform(-pitch, -yaw, roll,
116 | //       odomAftMapped->pose.pose.position.x,
117 | //       odomAftMapped->pose.pose.position.y,
118 | //       odomAftMapped->pose.pose.position.z,
119 | 
120 | //       odomAftMapped->twist.twist.angular.x,
121 | //       odomAftMapped->twist.twist.angular.y,
122 | //       odomAftMapped->twist.twist.angular.z,
123 | 
124 | //       odomAftMapped->twist.twist.linear.x,
125 | //       odomAftMapped->twist.twist.linear.y,
126 | //       odomAftMapped->twist.twist.linear.z);
127 | // }
128 | 
129 | void TransformMaintenance::handleOdomAftMapped(const Twist& aftMap, const Twist& befMap)
130 | {
131 | 
132 |    updateMappingTransform(aftMap.rot_x.rad(), 
133 |                         aftMap.rot_y.rad(), 
134 |                         aftMap.rot_z.rad(),
135 |                         aftMap.pos.x(),
136 |                         aftMap.pos.y(),
137 |                         aftMap.pos.z(),
138 |                         befMap.rot_x.rad(),
139 |                         befMap.rot_y.rad(),
140 |                         befMap.rot_z.rad(),
141 |                         befMap.pos.x(),
142 |                         befMap.pos.y(),
143 |                         befMap.pos.z());
144 | }
145 | 
146 | } // end namespace loam
147 | 


--------------------------------------------------------------------------------
/LOAM-multi-thread/src/lib/math_utils.h:
--------------------------------------------------------------------------------
  1 | #ifndef LOAM_MATH_UTILS_H
  2 | #define LOAM_MATH_UTILS_H
  3 | 
  4 | 
  5 | #include "loam_velodyne/Angle.h"
  6 | #include "loam_velodyne/Vector3.h"
  7 | 
  8 | #include <cmath>
  9 | 
 10 | // Quaterniond RPY2Quaternion(Vector3d rpy)
 11 | // {
 12 | //   double alpha = rpy(0);
 13 | //   double beta = rpy(1);
 14 | //   double gamma = rpy(2);
 15 | 
 16 | //   Quaterniond q;
 17 | //   q.w() = cos(alpha/2)*cos(beta/2)*cos(gamma/2) + sin(alpha/2)*sin(beta/2)*sin(gamma/2);
 18 | //   q.x() = sin(alpha/2)*cos(beta/2)*cos(gamma/2) - cos(alpha/2)*sin(beta/2)*sin(gamma/2);
 19 | //   q.y() = cos(alpha/2)*sin(beta/2)*cos(gamma/2) + sin(alpha/2)*cos(beta/2)*sin(gamma/2);
 20 | //   q.z() = cos(alpha/2)*cos(beta/2)*sin(gamma/2) - sin(alpha/2)*sin(beta/2)*cos(gamma/2);
 21 | 
 22 | //   return q;
 23 | // }
 24 | 
 25 | // void Quaternion2Euler(const Quaterniond& q, double& roll, double& pitch, double& yaw)
 26 | // {
 27 | //   // roll (x-axis rotation)
 28 | //   double sinr_cosp = +2.0 * (q.w() * q.x() + q.y() * q.z());
 29 | //   double cosr_cosp = +1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
 30 | //   roll = atan2(sinr_cosp, cosr_cosp);
 31 | 
 32 | //   // pitch (y-axis rotation)
 33 | //   double sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());
 34 | //   if (fabs(sinp) >= 1)
 35 | //     pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
 36 | //   else
 37 | //     pitch = asin(sinp);
 38 | 
 39 | //   // yaw (z-axis rotation)
 40 | //   double siny_cosp = +2.0 * (q.w() * q.z() + q.x() * q.y());
 41 | //   double cosy_cosp = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());  
 42 | //   yaw = atan2(siny_cosp, cosy_cosp);
 43 | // }
 44 | 
 45 | namespace loam {
 46 | 
 47 | /** \brief Convert the given radian angle to degrees.
 48 |  *
 49 |  * @param radians The radian angle to convert.
 50 |  * @return The angle in degrees.
 51 |  */
 52 | inline double rad2deg(double radians)
 53 | {
 54 |   return radians * 180.0 / M_PI;
 55 | }
 56 | 
 57 | 
 58 | 
 59 | /** \brief Convert the given radian angle to degrees.
 60 |  *
 61 |  * @param radians The radian angle to convert.
 62 |  * @return The angle in degrees.
 63 |  */
 64 | inline float rad2deg(float radians)
 65 | {
 66 |   return (float) (radians * 180.0 / M_PI);
 67 | }
 68 | 
 69 | 
 70 | 
 71 | /** \brief Convert the given degree angle to radian.
 72 |  *
 73 |  * @param degrees The degree angle to convert.
 74 |  * @return The radian angle.
 75 |  */
 76 | inline double deg2rad(double degrees)
 77 | {
 78 |   return degrees * M_PI / 180.0;
 79 | }
 80 | 
 81 | 
 82 | 
 83 | /** \brief Convert the given degree angle to radian.
 84 |  *
 85 |  * @param degrees The degree angle to convert.
 86 |  * @return The radian angle.
 87 |  */
 88 | inline float deg2rad(float degrees)
 89 | {
 90 |   return (float) (degrees * M_PI / 180.0);
 91 | }
 92 | 
 93 | 
 94 | 
 95 | 
 96 | /** \brief Calculate the squared difference of the given two points.
 97 |  *
 98 |  * @param a The first point.
 99 |  * @param b The second point.
100 |  * @return The squared difference between point a and b.
101 |  */
102 | template <typename PointT>
103 | inline float calcSquaredDiff(const PointT& a, const PointT& b)
104 | {
105 |   float diffX = a.x - b.x;
106 |   float diffY = a.y - b.y;
107 |   float diffZ = a.z - b.z;
108 | 
109 |   return diffX * diffX + diffY * diffY + diffZ * diffZ;
110 | }
111 | 
112 | 
113 | 
114 | /** \brief Calculate the squared difference of the given two points.
115 |  *
116 |  * @param a The first point.
117 |  * @param b The second point.
118 |  * @param wb The weighting factor for the SECOND point.
119 |  * @return The squared difference between point a and b.
120 |  */
121 | template <typename PointT>
122 | inline float calcSquaredDiff(const PointT& a, const PointT& b, const float& wb)
123 | {
124 |   float diffX = a.x - b.x * wb;
125 |   float diffY = a.y - b.y * wb;
126 |   float diffZ = a.z - b.z * wb;
127 | 
128 |   return diffX * diffX + diffY * diffY + diffZ * diffZ;
129 | }
130 | 
131 | 
132 | /** \brief Calculate the absolute distance of the point to the origin.
133 |  *
134 |  * @param p The point.
135 |  * @return The distance to the point.
136 |  */
137 | template <typename PointT>
138 | inline float calcPointDistance(const PointT& p)
139 | {
140 |   return std::sqrt(p.x * p.x + p.y * p.y + p.z * p.z);
141 | }
142 | 
143 | 
144 | 
145 | /** \brief Calculate the squared distance of the point to the origin.
146 |  *
147 |  * @param p The point.
148 |  * @return The squared distance to the point.
149 |  */
150 | template <typename PointT>
151 | inline float calcSquaredPointDistance(const PointT& p)
152 | {
153 |   return p.x * p.x + p.y * p.y + p.z * p.z;
154 | }
155 | 
156 | 
157 | 
158 | /** \brief Rotate the given vector by the specified angle around the x-axis.
159 |  *
160 |  * @param v the vector to rotate
161 |  * @param ang the rotation angle
162 |  */
163 | inline void rotX(Vector3& v, const Angle& ang)
164 | {
165 |   float y = v.y();
166 |   v.y() = ang.cos() * y - ang.sin() * v.z();
167 |   v.z() = ang.sin() * y + ang.cos() * v.z();
168 | }
169 | 
170 | /** \brief Rotate the given point by the specified angle around the x-axis.
171 |  *
172 |  * @param p the point to rotate
173 |  * @param ang the rotation angle
174 |  */
175 | template <typename PointT>
176 | inline void rotX(PointT& p, const Angle& ang)
177 | {
178 |   float y = p.y;
179 |   p.y = ang.cos() * y - ang.sin() * p.z;
180 |   p.z = ang.sin() * y + ang.cos() * p.z;
181 | }
182 | 
183 | 
184 | 
185 | /** \brief Rotate the given vector by the specified angle around the y-axis.
186 |  *
187 |  * @param v the vector to rotate
188 |  * @param ang the rotation angle
189 |  */
190 | inline void rotY(Vector3& v, const Angle& ang)
191 | {
192 |   float x = v.x();
193 |   v.x() = ang.cos() * x + ang.sin() * v.z();
194 |   v.z() = ang.cos() * v.z() - ang.sin() * x;
195 | }
196 | 
197 | /** \brief Rotate the given point by the specified angle around the y-axis.
198 |  *
199 |  * @param p the point to rotate
200 |  * @param ang the rotation angle
201 |  */
202 | template <typename PointT>
203 | inline void rotY(PointT& p, const Angle& ang)
204 | {
205 |   float x = p.x;
206 |   p.x = ang.cos() * x + ang.sin() * p.z;
207 |   p.z = ang.cos() * p.z - ang.sin() * x;
208 | }
209 | 
210 | 
211 | 
212 | /** \brief Rotate the given vector by the specified angle around the z-axis.
213 |  *
214 |  * @param v the vector to rotate
215 |  * @param ang the rotation angle
216 |  */
217 | inline void rotZ(Vector3& v, const Angle& ang)
218 | {
219 |   float x = v.x();
220 |   v.x() = ang.cos() * x - ang.sin() * v.y();
221 |   v.y() = ang.sin() * x + ang.cos() * v.y();
222 | }
223 | 
224 | /** \brief Rotate the given point by the specified angle around the z-axis.
225 |  *
226 |  * @param p the point to rotate
227 |  * @param ang the rotation angle
228 |  */
229 | template <typename PointT>
230 | inline void rotZ(PointT& p, const Angle& ang)
231 | {
232 |   float x = p.x;
233 |   p.x = ang.cos() * x - ang.sin() * p.y;
234 |   p.y = ang.sin() * x + ang.cos() * p.y;
235 | }
236 | 
237 | 
238 | 
239 | /** \brief Rotate the given vector by the specified angles around the z-, x- respectively y-axis.
240 |  *
241 |  * @param v the vector to rotate
242 |  * @param angZ the rotation angle around the z-axis
243 |  * @param angX the rotation angle around the x-axis
244 |  * @param angY the rotation angle around the y-axis
245 |  */
246 | inline void rotateZXY(Vector3& v,
247 |                       const Angle& angZ,
248 |                       const Angle& angX,
249 |                       const Angle& angY)
250 | {
251 |   rotZ(v, angZ);
252 |   rotX(v, angX);
253 |   rotY(v, angY);
254 | }
255 | 
256 | /** \brief Rotate the given point by the specified angles around the z-, x- respectively y-axis.
257 |  *
258 |  * @param p the point to rotate
259 |  * @param angZ the rotation angle around the z-axis
260 |  * @param angX the rotation angle around the x-axis
261 |  * @param angY the rotation angle around the y-axis
262 |  */
263 | template <typename PointT>
264 | inline void rotateZXY(PointT& p,
265 |                       const Angle& angZ,
266 |                       const Angle& angX,
267 |                       const Angle& angY)
268 | {
269 |   rotZ(p, angZ);
270 |   rotX(p, angX);
271 |   rotY(p, angY);
272 | }
273 | 
274 | 
275 | 
276 | /** \brief Rotate the given vector by the specified angles around the y-, x- respectively z-axis.
277 |  *
278 |  * @param v the vector to rotate
279 |  * @param angY the rotation angle around the y-axis
280 |  * @param angX the rotation angle around the x-axis
281 |  * @param angZ the rotation angle around the z-axis
282 |  */
283 | inline void rotateYXZ(Vector3& v,
284 |                       const Angle& angY,
285 |                       const Angle& angX,
286 |                       const Angle& angZ)
287 | {
288 |   rotY(v, angY);
289 |   rotX(v, angX);
290 |   rotZ(v, angZ);
291 | }
292 | 
293 | /** \brief Rotate the given point by the specified angles around the y-, x- respectively z-axis.
294 |  *
295 |  * @param p the point to rotate
296 |  * @param angY the rotation angle around the y-axis
297 |  * @param angX the rotation angle around the x-axis
298 |  * @param angZ the rotation angle around the z-axis
299 |  */
300 | template <typename PointT>
301 | inline void rotateYXZ(PointT& p,
302 |                       const Angle& angY,
303 |                       const Angle& angX,
304 |                       const Angle& angZ)
305 | {
306 |   rotY(p, angY);
307 |   rotX(p, angX);
308 |   rotZ(p, angZ);
309 | }
310 | 
311 | } // end namespace loam
312 | 
313 | 
314 | #endif // LOAM_MATH_UTILS_H
315 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # LOAM-multi-thread
 2 | Adjusted the original LOAM to a multi-threaded version which doesn't require ROS, so we can run the algorithm under multilple 
 3 | platforms which are not convenient with ROS like MacOS and Windows.
 4 | 
 5 | ## Prerequisites
 6 | **PCL**  
 7 | **Pangolin**   
 8 | **Eigen**  
 9 | <center>
10 | <img src="https://img-blog.csdnimg.cn/20181229151156872.jpg" width="55%">
11 | 


--------------------------------------------------------------------------------