Merge pull request #72 from Cardinal-Space-Mining/functionality

Battle hardened odometry

Author: S1ink

include/modules/impl/lidar_odom_impl.hpp

@@ -321,6 +321,9 @@ void LidarOdometry<PT>::initState()
     this->gicp.setSearchMethodSource(temp, true);
     this->gicp.setSearchMethodTarget(temp, true);
 
+    // this->gicp_s2s.setDebugPrint(true);
+    // this->gicp.setDebugPrint(true);
+
     this->vf_scan.setLeafSize(
         this->param.vf_scan_res_,
         this->param.vf_scan_res_,
@@ -342,14 +345,14 @@ bool LidarOdometry<PT>::setInitial(const util::geom::Pose3f& pose)
         // set known position
         this->state.T.template block<3, 1>(0, 3) =
             this->state.T_s2s.template block<3, 1>(0, 3) =
-                this->state.T_s2s_prev.template block<3, 1>(0, 3) =
+                this->state.T_prev.template block<3, 1>(0, 3) =
                     this->state.translation = pose.vec;
 
         // set known orientation
         this->state.last_rotq = this->state.rotq = pose.quat;
         this->state.T.template block<3, 3>(0, 0) =
             this->state.T_s2s.template block<3, 3>(0, 0) =
-                this->state.T_s2s_prev.template block<3, 3>(0, 0) =
+                this->state.T_prev.template block<3, 3>(0, 0) =
                     this->state.rotq.toRotationMatrix();
 
         return true;
@@ -369,7 +372,8 @@ typename LidarOdometry<PT>::IterationStatus LidarOdometry<PT>::processScan(
     if (stamp <= this->state.prev_frame_stamp)
     {
         std::cout << "[ODOMETRY]: Scan timestamp is older than the previous by "
-                  << (this->state.prev_frame_stamp - stamp) << "s!" << std::endl;
+                  << (this->state.prev_frame_stamp - stamp) << "s!"
+                  << std::endl;
         return {};
     }
 
@@ -414,7 +418,10 @@ typename LidarOdometry<PT>::IterationStatus LidarOdometry<PT>::processScan(
     this->setInputSources();
 
     // Get the next pose via IMU + S2S + S2M
-    this->getNextPose(align_estimate);
+    if(!this->getNextPose(align_estimate))
+    {
+        return {};
+    }
 
     // Transform point cloud
     this->transformCurrentScan();
@@ -508,6 +515,15 @@ bool LidarOdometry<PT>::preprocessPoints(const PointCloudT& scan)
         return false;
     }
 
+    // for(const auto& pt : scan.points)
+    // {
+    //     if(pt.getArray3fMap().isNaN().any())
+    //     {
+    //         std::cout << "[ODOMETRY]: Input cloud had NaN input!" << std::endl;
+    //         return false;
+    //     }
+    // }
+
     // Find new voxel size before applying filter
     if (this->param.adaptive_params_use_)
     {
@@ -710,7 +726,7 @@ void LidarOdometry<PT>::setInputSources()
 }
 
 template<typename PT>
-void LidarOdometry<PT>::getNextPose(const std::optional<Mat4f>& align_estimate)
+bool LidarOdometry<PT>::getNextPose(const std::optional<Mat4f>& align_estimate)
 {
     //
     // FRAME-TO-FRAME PROCEDURE
@@ -732,16 +748,20 @@ void LidarOdometry<PT>::getNextPose(const std::optional<Mat4f>& align_estimate)
     }
 
     // Get the local S2S transform
-    Mat4f T_S2S = this->gicp_s2s.getFinalTransformation();
+    const Mat4f T_s2s_local = this->gicp_s2s.getFinalTransformation();
+
+    if (T_s2s_local.array().isNaN().any())
+    {
+        std::cout << "[ODOMETRY]: S2S transform contained NaN values!"
+                  << std::endl;
+        return false;
+    }
 
     // Get the global S2S transform
-    this->propagateS2S(T_S2S);
+    this->propagateS2S(T_s2s_local);
 
     // reuse covariances from s2s for s2m
-    this->gicp.source_covs_ = this->gicp_s2s.source_covs_;
-
-    // Swap source and target (which also swaps KdTrees internally) for next S2S
-    this->gicp_s2s.swapSourceAndTarget();
+    this->gicp.source_covs_ = this->gicp_s2s.target_covs_;
 
     //
     // FRAME-TO-SUBMAP
@@ -768,24 +788,39 @@ void LidarOdometry<PT>::getNextPose(const std::optional<Mat4f>& align_estimate)
     }
 
     // Get final transformation in global frame
-    this->state.T = this->gicp.getFinalTransformation();
+    const Mat4f T = this->gicp.getFinalTransformation();
+
+    if (T.array().isNaN().any())
+    {
+        std::cout << "[ODOMETRY]: S2M transform contained NaN values!"
+                  << std::endl;
+        return false;
+    }
+
+    this->state.T = std::move(T);
+
+    // Swap source and target (which also swaps KdTrees internally) for next S2S
+    // (don't do this until we know it is safe to do so)
+    this->gicp_s2s.swapSourceAndTarget();
 
     // Update the S2S transform for next propagation
-    this->state.T_s2s_prev = this->state.T;
+    this->state.T_prev = this->state.T;
 
     // Update next global pose
     // Both source and target clouds are in the global frame now, so tranformation is global
     this->propagateS2M();
 
     // Set next target cloud as current source cloud
     *this->target_cloud = *this->current_scan;
+
+    return true;
 }
 
 template<typename PT>
-void LidarOdometry<PT>::propagateS2S(const Mat4f& T)
+void LidarOdometry<PT>::propagateS2S(const Mat4f& T_rel)
 {
-    this->state.T_s2s = this->state.T_s2s_prev * T;
-    this->state.T_s2s_prev = this->state.T_s2s;
+    this->state.T_s2s = this->state.T_prev * T_rel;
+    // this->state.T_prev = this->state.T_s2s;
 }
 
 template<typename PT>

include/modules/imu_integrator.hpp

@@ -84,6 +84,7 @@ class ImuIntegrator
 public:
     void addSample(const ImuMsg& imu);
     void trimSamples(TimeFloatT trim_ts);
+    void setAutoTrimWindow(TimeFloatT window_s = 0.f);
     bool recalibrate(TimeFloatT dt, bool force = false);
 
     Vec3 estimateGravity(
@@ -121,6 +122,7 @@ class ImuIntegrator
     std::atomic<bool> use_orientation;
 
     const double calib_time;
+    double trim_window{600.0};
 };
 
 
@@ -156,10 +158,13 @@ void ImuIntegrator<F>::addSample(const ImuMsg& imu)
             stamp,
             q);
 
-        // 10 min max, orientation is likely still valid but at this point we probably have worse problems
-        util::tsq::trimToStamp(
-            this->orient_buffer,
-            (util::tsq::newestStamp(this->orient_buffer) - 600.));
+        if (this->trim_window > 0)
+        {
+            util::tsq::trimToStamp(
+                this->orient_buffer,
+                (util::tsq::newestStamp(this->orient_buffer) -
+                 this->trim_window));
+        }
     }
 
     {
@@ -179,10 +184,12 @@ void ImuIntegrator<F>::addSample(const ImuMsg& imu)
             this->recalibrateRange(0, this->raw_buffer.size());
         }
 
-        // 5 min max, integration is definitely deviated after this for most imus
-        util::tsq::trimToStamp(
-            this->raw_buffer,
-            (util::tsq::newestStamp(this->raw_buffer) - 300.));
+        if (this->trim_window > 0)
+        {
+            util::tsq::trimToStamp(
+                this->raw_buffer,
+                (util::tsq::newestStamp(this->raw_buffer) - this->trim_window));
+        }
     }
 }
 
@@ -195,6 +202,14 @@ void ImuIntegrator<F>::trimSamples(TimeFloatT trim_ts)
     util::tsq::trimToStamp(this->raw_buffer, trim_ts);
 }
 
+template<typename F>
+void ImuIntegrator<F>::setAutoTrimWindow(TimeFloatT window_s)
+{
+    std::unique_lock imu_lock{this->mtx};
+
+    this->trim_window = window_s;
+}
+
 template<typename F>
 bool ImuIntegrator<F>::recalibrate(TimeFloatT dt, bool force)
 {
@@ -265,6 +280,8 @@ typename ImuIntegrator<F>::Quat ImuIntegrator<F>::getDelta(
     TimeFloatT start,
     TimeFloatT end) const
 {
+    using namespace util::geom::cvt::ops;
+
     Quat q = Quat::Identity();
     std::unique_lock imu_lock{this->mtx};
 
@@ -282,72 +299,89 @@ typename ImuIntegrator<F>::Quat ImuIntegrator<F>::getDelta(
             newest--;
         }
 
-        if (newest != oldest)
+        if (newest < oldest)
         {
             const auto& a = this->orient_buffer[oldest];
             const auto& b = this->orient_buffer[oldest - 1];
             const auto& c = this->orient_buffer[newest + 1];
             const auto& d = this->orient_buffer[newest];
 
-            Quat prev = a.second.slerp(
+            const Quat prev = a.second.slerp(
                 (start - a.first) / (b.first - a.first),
                 b.second);
-            Quat curr =
+            const Quat curr =
                 c.second.slerp((end - c.first) / (d.first - c.first), d.second);
 
             q = prev.inverse() * curr;
         }
     }
     else
     {
-        assert(!"IMU angular velocity integration is not implemented!");
-#if 0  // TODO
-        const size_t
-            init_idx = util::tsq::binarySearchIdx(this->imu_buffer, start),
-            end_idx = util::tsq::binarySearchIdx(this->imu_buffer, end);
-
-        // Relative IMU integration of gyro and accelerometer
-        double curr_imu_stamp = 0.;
-        double prev_imu_stamp = 0.;
-        double dt;
-
-        for(size_t i = init_idx; i >= end_idx; i--)
+        size_t oldest = util::tsq::binarySearchIdx(this->raw_buffer, start);
+        size_t newest = util::tsq::binarySearchIdx(this->raw_buffer, end);
+
+        if (oldest == this->raw_buffer.size() && oldest > 0)
+        {
+            oldest--;
+        }
+        if (newest > 0)
         {
-            const auto& imu_sample = this->raw_buffer[i];
+            newest--;
+        }
 
-            if(prev_imu_stamp == 0.)
+        if (newest < oldest)
+        {
+            const auto& a = this->raw_buffer[oldest];
+            const auto& b = this->raw_buffer[oldest - 1];
+            const auto& c = this->raw_buffer[newest + 1];
+            const auto& d = this->raw_buffer[newest];
+
+            const Vec3 head =
+                a.second.ang_vel + (b.second.ang_vel - a.second.ang_vel) *
+                                       (start - a.first) / (b.first - a.first);
+            const Vec3 tail =
+                c.second.ang_vel + (d.second.ang_vel - c.second.ang_vel) *
+                                       (end - c.first) / (d.first - c.first);
+
+            for (size_t i = oldest; i > newest; i--)
             {
-                prev_imu_stamp = imu_sample.first;
-                continue;
+                const Vec3 *from, *to;
+                double from_t, to_t;
+                if (i == oldest)
+                {
+                    from = &head;
+                    from_t = start;
+                }
+                else
+                {
+                    from = &(this->raw_buffer[i].second.ang_vel);
+                    from_t = this->raw_buffer[i].first;
+                }
+
+                if (i - 1 == newest)
+                {
+                    to = &tail;
+                    to_t = end;
+                }
+                else
+                {
+                    to = &(this->raw_buffer[i - 1].second.ang_vel);
+                    to_t = this->raw_buffer[i - 1].first;
+                }
+
+                const Vec3 avg_w = (*from + *to) * 0.5f;
+                const FloatT dt = static_cast<FloatT>(to_t - from_t);
+                const Vec3 theta = avg_w * dt;
+                const FloatT angle = theta.norm();
+
+                if (angle > 1e-8)
+                {
+                    const Vec3 axis = theta / angle;
+                    q = (q * Quat{Eigen::AngleAxis<FloatT>(angle, axis)})
+                            .normalized();
+                }
             }
-
-            // Calculate difference in imu measurement times IN SECONDS
-            curr_imu_stamp = imu_sample.first;
-            dt = curr_imu_stamp - prev_imu_stamp;
-            prev_imu_stamp = curr_imu_stamp;
-
-            // Relative gyro propagation quaternion dynamics
-            Quatd qq = q;
-            q.w() -= 0.5 * dt *
-                (qq.x() * imu_sample.second.ang_vel.x()
-                    + qq.y() * imu_sample.second.ang_vel.y()
-                    + qq.z() * imu_sample.second.ang_vel.z() );
-            q.x() += 0.5 * dt *
-                (qq.w() * imu_sample.second.ang_vel.x()
-                    - qq.z() * imu_sample.second.ang_vel.y()
-                    + qq.y() * imu_sample.second.ang_vel.z() );
-            q.y() += 0.5 * dt *
-                (qq.z() * imu_sample.second.ang_vel.x()
-                    + qq.w() * imu_sample.second.ang_vel.y()
-                    - qq.x() * imu_sample.second.ang_vel.z() );
-            q.z() += 0.5 * dt *
-                (qq.x() * imu_sample.second.ang_vel.y()
-                    - qq.y() * imu_sample.second.ang_vel.x()
-                    + qq.w() * imu_sample.second.ang_vel.z() );
         }
-
-        q.normalize();
-#endif
     }
 
     return q;

include/modules/lidar_odom.hpp

@@ -151,7 +151,7 @@ class LidarOdometry
     void initializeInputTarget();
     void setInputSources();
 
-    void getNextPose(const std::optional<Mat4f>& align_estimate = std::nullopt);
+    bool getNextPose(const std::optional<Mat4f>& align_estimate = std::nullopt);
 
     void propagateS2S(const Mat4f& T);
     void propagateS2M();
@@ -211,7 +211,7 @@ class LidarOdometry
 
         Mat4f T{Mat4f::Identity()};
         Mat4f T_s2s{Mat4f::Identity()};
-        Mat4f T_s2s_prev{Mat4f::Identity()};
+        Mat4f T_prev{Mat4f::Identity()};
 
         std::mutex mtx;
     } state;