├── README.md
├── LICENSE
└── lidar_undistortion.hpp


/README.md:
--------------------------------------------------------------------------------
 1 | lidar_undisrtoriton
 2 | ====
 3 | 
 4 | A header-only program of 3d lidar undistortion using 9-axis imu.
 5 | separated from [LeGO-LOAM](https://github.com/RobustFieldAutonomyLab/LeGO-LOAM).  
 6 | The assumption is that the IMU center and the LIDAR center are aligned.
 7 | ## how to use
 8 | Initialization
 9 | ```cpp
10 | #include <lidar_undistortion.hpp>
11 | 
12 | LidarUndistortion lidar_undistortion_;
13 | double scan_period = ...; //The rotation period of lidar
14 | lidar_undistortion_.setScanPeriod(scan_period);
15 | ```
16 | when imu is received
17 | ```cpp
18 | Eigen::Vector3f angular_velo{...};
19 | Eigen::Vector3f acc{...};
20 | Eigen::Quaternionf quat{...};
21 | double imu_time = ...;
22 | lidar_undistortion_.getImu(angular_velo, acc, quat, imu_time);
23 | ```
24 | when LIDAR scan is received
25 | ```cpp
26 | double scan_time = ...;
27 | // output:cloud_ptr
28 | lidar_undistortion_.adjustDistortion(cloud_ptr, scan_time);
29 | ```
30 | 
31 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | BSD 3-Clause License
 2 | 
 3 | Copyright (c) 2020, Ryohei Sasaki
 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, this
10 |    list of conditions and the following disclaimer.
11 | 
12 | 2. Redistributions in binary form must reproduce the above copyright notice,
13 |    this list of conditions and the following disclaimer in the documentation
14 |    and/or other materials provided with the distribution.
15 | 
16 | 3. Neither the name of the copyright holder nor the names of its
17 |    contributors may be used to endorse or promote products derived from
18 |    this software without specific prior written permission.
19 | 
20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 | 


--------------------------------------------------------------------------------
/lidar_undistortion.hpp:
--------------------------------------------------------------------------------
  1 | #ifndef LIDAR_UNDISTORTION_HPP_
  2 | #define LIDAR_UNDISTORTION_HPP_
  3 | 
  4 | #include <pcl/point_types.h>
  5 | #include <pcl/io/pcd_io.h>
  6 | #include <pcl/common/common.h>
  7 | #include <pcl/common/eigen.h>
  8 | #include <Eigen/Core>
  9 | #include <Eigen/Geometry>
 10 | #include <iostream>
 11 | 
 12 | class LidarUndistortion
 13 | {
 14 | public:
 15 |   LidarUndistortion(){}
 16 | 
 17 |   // Ref:LeGO-LOAM(BSD-3 LICENSE)
 18 |   // https://github.com/RobustFieldAutonomyLab/LeGO-LOAM/blob/master/LeGO-LOAM/src/featureAssociation.cpp#L431-L459
 19 |   void getImu(Eigen::Vector3f angular_velo, Eigen::Vector3f acc, const Eigen::Quaternionf quat, const double imu_time /*[sec]*/)
 20 |   {
 21 |     float roll, pitch, yaw;
 22 |     Eigen::Affine3f affine(quat);
 23 |     pcl::getEulerAngles(affine, roll, pitch, yaw);
 24 |     
 25 |     imu_ptr_last_ = (imu_ptr_last_ + 1) % imu_que_length_;
 26 | 
 27 |     if ((imu_ptr_last_ + 1) % imu_que_length_ == imu_ptr_front_) {
 28 |         imu_ptr_front_ = (imu_ptr_front_ + 1) % imu_que_length_;
 29 |     }
 30 | 
 31 |     imu_time_[imu_ptr_last_] = imu_time;
 32 |     imu_roll_[imu_ptr_last_] = roll;
 33 |     imu_pitch_[imu_ptr_last_] = pitch;
 34 |     imu_yaw_[imu_ptr_last_] = yaw;
 35 |     imu_acc_x_[imu_ptr_last_] = acc.x();
 36 |     imu_acc_y_[imu_ptr_last_] = acc.y();
 37 |     imu_acc_z_[imu_ptr_last_] = acc.z();
 38 |     imu_angular_velo_x_[imu_ptr_last_] = angular_velo.x();
 39 |     imu_angular_velo_y_[imu_ptr_last_] = angular_velo.y();
 40 |     imu_angular_velo_z_[imu_ptr_last_] = angular_velo.z();
 41 | 
 42 |     Eigen::Matrix3f rot = quat.toRotationMatrix();
 43 |     acc = rot * acc;
 44 |     angular_velo = rot * angular_velo;
 45 | 
 46 |     int imu_ptr_back = (imu_ptr_last_ - 1 + imu_que_length_) % imu_que_length_;
 47 |     double time_diff = imu_time_[imu_ptr_last_] - imu_time_[imu_ptr_back];
 48 |     if (time_diff < scan_period_) {
 49 |         imu_shift_x_[imu_ptr_last_] =
 50 |         imu_shift_x_[imu_ptr_back] +imu_velo_x_[imu_ptr_back] * time_diff + acc(0) * time_diff * time_diff * 0.5;
 51 |         imu_shift_y_[imu_ptr_last_] =
 52 |         imu_shift_y_[imu_ptr_back] + imu_velo_y_[imu_ptr_back] * time_diff + acc(1) * time_diff * time_diff * 0.5;
 53 |         imu_shift_z_[imu_ptr_last_] =
 54 |         imu_shift_z_[imu_ptr_back] + imu_velo_z_[imu_ptr_back] * time_diff + acc(2) * time_diff * time_diff * 0.5;
 55 | 
 56 |         imu_velo_x_[imu_ptr_last_] = imu_velo_x_[imu_ptr_back] + acc(0) * time_diff;
 57 |         imu_velo_y_[imu_ptr_last_] = imu_velo_y_[imu_ptr_back] + acc(1) * time_diff;
 58 |         imu_velo_z_[imu_ptr_last_] = imu_velo_z_[imu_ptr_back] + acc(2) * time_diff;
 59 | 
 60 |         imu_angular_rot_x_[imu_ptr_last_] = imu_angular_rot_x_[imu_ptr_back] + angular_velo(0) * time_diff;
 61 |         imu_angular_rot_y_[imu_ptr_last_] = imu_angular_rot_y_[imu_ptr_back] + angular_velo(1) * time_diff;
 62 |         imu_angular_rot_z_[imu_ptr_last_] = imu_angular_rot_z_[imu_ptr_back] + angular_velo(2) * time_diff;
 63 |     }
 64 |   }
 65 | 
 66 |   // Ref:LeGO-LOAM(BSD-3 LICENSE)
 67 |   // https://github.com/RobustFieldAutonomyLab/LeGO-LOAM/blob/master/LeGO-LOAM/src/featureAssociation.cpp#L491-L619
 68 |   void adjustDistortion(pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, const double scan_time /*[sec]*/)
 69 |   {
 70 |     bool half_passed = false;
 71 |     int cloud_size = cloud->points.size();
 72 | 
 73 |     float start_ori = -std::atan2(cloud->points[0].y, cloud->points[0].x);
 74 |     float end_ori = -std::atan2(cloud->points[cloud_size - 1].y, cloud->points[cloud_size - 1].x);
 75 |     if (end_ori - start_ori > 3 * M_PI) {
 76 |         end_ori -= 2 * M_PI;
 77 |     }
 78 |     else if (end_ori - start_ori < M_PI) {
 79 |         end_ori += 2 * M_PI;
 80 |     }
 81 |     float ori_diff = end_ori - start_ori;
 82 | 
 83 |     Eigen::Vector3f rpy_start, shift_start, velo_start, rpy_cur, shift_cur, velo_cur;
 84 |     Eigen::Vector3f shift_from_start;
 85 |     Eigen::Matrix3f r_s_i, r_c;
 86 |     Eigen::Vector3f adjusted_p;
 87 |     float ori_h;
 88 |     for (int i = 0; i < cloud_size; ++i) {
 89 |         pcl::PointXYZI &p = cloud->points[i];
 90 |         ori_h = -std::atan2(p.y, p.x);
 91 |         if (!half_passed) {
 92 |             if (ori_h < start_ori - M_PI * 0.5) {
 93 |                 ori_h += 2 * M_PI;
 94 |             } else if (ori_h > start_ori + M_PI * 1.5) {
 95 |                 ori_h -= 2 * M_PI;
 96 |             }
 97 | 
 98 |             if (ori_h - start_ori > M_PI) {
 99 |             half_passed = true;
100 |             }
101 |         } else {
102 |             ori_h += 2 * M_PI;
103 |             if (ori_h < end_ori - 1.5 * M_PI) {
104 |             ori_h += 2 * M_PI;
105 |             }
106 |         else if (ori_h > end_ori + 0.5 * M_PI) {
107 |             ori_h -= 2 * M_PI;
108 |             }
109 |         }
110 | 
111 |         float rel_time = (ori_h - start_ori) / ori_diff * scan_period_;
112 | 
113 |         if (imu_ptr_last_ > 0) {
114 |             imu_ptr_front_ = imu_ptr_last_iter_;
115 |             while (imu_ptr_front_ != imu_ptr_last_) {
116 |             if (scan_time + rel_time > imu_time_[imu_ptr_front_]) {
117 |                 break;
118 |             }
119 |             imu_ptr_front_ = (imu_ptr_front_ + 1) % imu_que_length_;
120 |             }
121 | 
122 |             if (scan_time + rel_time > imu_time_[imu_ptr_front_]) {
123 |                 rpy_cur(0) = imu_roll_[imu_ptr_front_];
124 |                 rpy_cur(1) = imu_pitch_[imu_ptr_front_];
125 |                 rpy_cur(2) = imu_yaw_[imu_ptr_front_];
126 |                 shift_cur(0) = imu_shift_x_[imu_ptr_front_];
127 |                 shift_cur(1) = imu_shift_y_[imu_ptr_front_];
128 |                 shift_cur(2) = imu_shift_z_[imu_ptr_front_];
129 |                 velo_cur(0) = imu_velo_x_[imu_ptr_front_];
130 |                 velo_cur(1) = imu_velo_y_[imu_ptr_front_];
131 |                 velo_cur(2) = imu_velo_z_[imu_ptr_front_];
132 |             } else {
133 |                 int imu_ptr_back = (imu_ptr_front_ - 1 + imu_que_length_) % imu_que_length_;
134 |                 float ratio_front = (scan_time + rel_time - imu_time_[imu_ptr_back]) /
135 |                     (imu_time_[imu_ptr_front_] - imu_time_[imu_ptr_back]);
136 |                 float ratio_back = 1.0 - ratio_front;
137 |                 rpy_cur(0) = imu_roll_[imu_ptr_front_] * ratio_front + imu_roll_[imu_ptr_back] * ratio_back;
138 |                 rpy_cur(1) = imu_pitch_[imu_ptr_front_] * ratio_front + imu_pitch_[imu_ptr_back] * ratio_back;
139 |                 rpy_cur(2) = imu_yaw_[imu_ptr_front_] * ratio_front + imu_yaw_[imu_ptr_back] * ratio_back;
140 |                 shift_cur(0) = imu_shift_x_[imu_ptr_front_] * ratio_front + imu_shift_x_[imu_ptr_back] * ratio_back;
141 |                 shift_cur(1) = imu_shift_y_[imu_ptr_front_] * ratio_front + imu_shift_y_[imu_ptr_back] * ratio_back;
142 |                 shift_cur(2) = imu_shift_z_[imu_ptr_front_] * ratio_front + imu_shift_z_[imu_ptr_back] * ratio_back;
143 |                 velo_cur(0) = imu_velo_x_[imu_ptr_front_] * ratio_front + imu_velo_x_[imu_ptr_back] * ratio_back;
144 |                 velo_cur(1) = imu_velo_y_[imu_ptr_front_] * ratio_front + imu_velo_y_[imu_ptr_back] * ratio_back;
145 |                 velo_cur(2) = imu_velo_z_[imu_ptr_front_] * ratio_front + imu_velo_z_[imu_ptr_back] * ratio_back;
146 |             }
147 | 
148 |             r_c = (
149 |             Eigen::AngleAxisf(rpy_cur(2), Eigen::Vector3f::UnitZ()) *
150 |             Eigen::AngleAxisf(rpy_cur(1), Eigen::Vector3f::UnitY()) *
151 |             Eigen::AngleAxisf(rpy_cur(0), Eigen::Vector3f::UnitX())
152 |             ).toRotationMatrix();
153 | 
154 |             if (i == 0) {
155 |                 rpy_start = rpy_cur;
156 |                 shift_start = shift_cur;
157 |                 velo_start = velo_cur;
158 |                 r_s_i = r_c.inverse();
159 |             } else {
160 |                 shift_from_start = shift_cur - shift_start - velo_start * rel_time;
161 |                 adjusted_p = r_s_i * (r_c * Eigen::Vector3f(p.x, p.y, p.z) + shift_from_start);
162 |                 p.x = adjusted_p.x();
163 |                 p.y = adjusted_p.y();
164 |                 p.z = adjusted_p.z();
165 |             }
166 |         }
167 |         imu_ptr_last_iter_ = imu_ptr_front_;
168 |     }
169 |   }
170 | 
171 | 
172 |   void setScanPeriod(const double scan_period  /*[sec]*/)
173 |   {
174 |     scan_period_ = scan_period;
175 |   }
176 | 
177 | private:
178 |   double scan_period_{0.1};
179 |   static const int imu_que_length_{200};
180 |   int imu_ptr_front_{0}, imu_ptr_last_{-1}, imu_ptr_last_iter_{0};
181 | 
182 |   std::array<double, imu_que_length_> imu_time_;
183 |   std::array<float, imu_que_length_> imu_roll_;
184 |   std::array<float, imu_que_length_> imu_pitch_;
185 |   std::array<float, imu_que_length_> imu_yaw_;
186 | 
187 |   std::array<float, imu_que_length_> imu_acc_x_;
188 |   std::array<float, imu_que_length_> imu_acc_y_;
189 |   std::array<float, imu_que_length_> imu_acc_z_;
190 |   std::array<float, imu_que_length_> imu_velo_x_;
191 |   std::array<float, imu_que_length_> imu_velo_y_;
192 |   std::array<float, imu_que_length_> imu_velo_z_;
193 |   std::array<float, imu_que_length_> imu_shift_x_;
194 |   std::array<float, imu_que_length_> imu_shift_y_;
195 |   std::array<float, imu_que_length_> imu_shift_z_;
196 | 
197 |   std::array<float, imu_que_length_> imu_angular_velo_x_;
198 |   std::array<float, imu_que_length_> imu_angular_velo_y_;
199 |   std::array<float, imu_que_length_> imu_angular_velo_z_;
200 |   std::array<float, imu_que_length_> imu_angular_rot_x_;
201 |   std::array<float, imu_que_length_> imu_angular_rot_y_;
202 |   std::array<float, imu_que_length_> imu_angular_rot_z_;
203 | };
204 | 
205 | #endif  // LIDAR_UNDISTORTION_HPP_


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