Improve tracking performance

CppDependencies/PoissonRecon/Sources/include/PoissonRecon/FEMTree.LevelSet.inl

@@ -529,7 +529,7 @@ namespace LevelSetExtraction
 		void set( const SortedTreeNodes< Dim > &sNodes , int depth , int slice )
 		{
 			std::pair< node_index_type , node_index_type > span( sNodes.begin( depth , slice-1 ) , sNodes.end( depth , slice ) );
-			nodeOffset = (size_t)span.first;
+			nodeOffset = (node_index_type)span.first;
 			resize( (size_t)( span.second - span.first ) );
 			_scratch.resize( size() );
 
@@ -663,7 +663,7 @@ namespace LevelSetExtraction
 		void set( const SortedTreeNodes< Dim > &sNodes , int depth , int slab )
 		{
 			std::pair< node_index_type , node_index_type > span( sNodes.begin( depth , slab ) , sNodes.end( depth , slab ) );
-			nodeOffset = (size_t)span.first;
+			nodeOffset = (node_index_type)span.first;
 			resize( (size_t)( span.second - span.first ) );
 			_scratch.resize( size() );
 

CppDependencies/PoissonRecon/Sources/src/PlyFile.cpp

@@ -1809,7 +1809,7 @@ void get_ascii_item( const std::string &word , int type , int &int_val , unsigne
 
 	case PLY_UINT:
 	case PLY_UINT_32:
-		uint_val = strtol( word.c_str() , (char **)NULL , 10 );
+		uint_val = (unsigned int)strtol( word.c_str() , (char **)NULL , 10 );
 		int_val = (int)uint_val;
 		double_val = (double)uint_val;
 		longlong_val = (long long)uint_val;

CppDependencies/tinygltf/include/tiny_gltf.h

@@ -3065,6 +3065,7 @@ static bool ParseJsonAsValue(Value *ret, const json &o) {
       break;
     case json::value_t::null:
     case json::value_t::discarded:
+    case nlohmann::json_abi_v3_11_3::detail::value_t::binary:
       // default:
       break;
   }

Package.swift

@@ -117,7 +117,12 @@ let package = Package(
             publicHeadersPath: "include",
             cxxSettings: [
                 // Always optimize, even for debug builds, in order to be usable while debugging the rest of an app
-                .unsafeFlags(["-fobjc-arc", "-Os", "-fno-math-errno", "-ffast-math"]),
+                // Keep -fno-finite-math-only: incoming surfel data from the camera uses NaN/inf to
+                // mark unknown values, and -ffast-math would otherwise enable -ffinite-math-only.
+                .unsafeFlags([
+                    "-fobjc-arc", "-Os", "-fno-math-errno", "-ffast-math", "-fno-finite-math-only",
+                    "-Wno-vla-cxx-extension",
+                ]),
                 .headerSearchPath("."),
                 .headerSearchPath("../libigl/include"),
                 .headerSearchPath("StandardCyborgFusion/Algorithm"),

StandardCyborgFusion/Sources/StandardCyborgFusion/Algorithm/ICP.cpp

@@ -10,6 +10,7 @@
 #include <standard_cyborg/util/DataUtils.hpp>
 #include <standard_cyborg/util/DebugHelpers.hpp>
 
+#include "DebugLog.h"
 #include "GeometryHelpers.hpp"
 #include "ThreadPool.hpp"
 #include "ICP.hpp"
@@ -43,24 +44,27 @@ struct _ICPCorrespondence {
         weights(weights)
     {}
 
+    // Weighted RMS: only correspondences whose outlier weight is non-zero contribute.
+    // This is the quantity ICP's convergence test watches.
     float computeRMSCorrespondenceError()
     {
         size_t sourceVerticesLength = sourceVertices.size();
-
+        const Eigen::VectorXf& w = *weights;
+
         double sumSquaredError = 0.0;
         double weightSum = 0;
 
         for (size_t i = 0; i < sourceVerticesLength; ++i) {
+            float weight = w(i);
+            if (weight == 0.0f) { continue; }
             Vector3f correspondenceError = standard_cyborg::toVector3f(sourceVertices[i]) - standard_cyborg::toVector3f(targetVertices[i]);
-            float errorSquared = correspondenceError.squaredNorm(); // squaredNorm is the squared Euclidean distance
-            float weight = 1.0; //(*weights)(i);
+            float errorSquared = correspondenceError.squaredNorm();
             sumSquaredError += errorSquared * weight;
             weightSum += weight;
         }
-
-        double variance = sumSquaredError / weightSum;
-
-        return (float)sqrt(variance);
+
+        if (weightSum == 0.0) { return 0.0f; }
+        return (float)sqrt(sumSquaredError / weightSum);
     }
 };
 
@@ -208,15 +212,31 @@ static _ICPCorrespondence _computeCorrespondence(const std::vector<math::Vec3>&
     }
 #endif
 
-    float avgSquaredError = sumSquaredError / vertexCount;
-    float normalizedVarianceThreshold = config.outlierDeviationsThreshold * config.outlierDeviationsThreshold;
-
+    // Outlier rejection based on the Euclidean distance distribution of correspondences
+    Eigen::VectorXf distances(vertexCount);
+    double sumDistance = 0.0;
+    for (size_t i = 0; i < vertexCount; ++i) {
+        float d = std::sqrt((*squaredErrors)(i));
+        distances(i) = d;
+        sumDistance += d;
+    }
+    double meanDistance = sumDistance / vertexCount;
+
+    double varianceDistance = 0.0;
+    for (size_t i = 0; i < vertexCount; ++i) {
+        double delta = distances(i) - meanDistance;
+        varianceDistance += delta * delta;
+    }
+    float stdDevDistance = (vertexCount > 1) ? (float)std::sqrt(varianceDistance / (vertexCount - 1)) : 0.0f;
+
+    // Small floor so that a nearly-perfect fit doesn't reject its own noise floor
+    const float kMinStdDev = 1e-4f;
+    float effectiveStdDev = std::max(stdDevDistance, kMinStdDev);
+    float rejectDistance = (float)meanDistance + config.outlierDeviationsThreshold * effectiveStdDev;
+
     for (size_t i = 0; i < vertexCount; i++) {
-        float squaredError = (*squaredErrors)(i);
-        float normalizedVariance = squaredError / avgSquaredError;
-        float weight = normalizedVariance > normalizedVarianceThreshold ? 0.0 : 1.0;
-
-        (*weights)(i) = weight; // * sourceWeights[i];
+        float weight = (distances(i) > rejectDistance) ? 0.0f : 1.0f;
+        (*weights)(i) = weight;
     }
 
     return _ICPCorrespondence(sourceVertices, targetVertices, targetNormals, squaredErrors, weights);
@@ -266,12 +286,28 @@ static Eigen::Matrix4f _computePointToPlaneTransform(_ICPCorrespondence& corresp
     FillUpperTriangularMatrixIntoLower6x6(A);
 
     x = A.llt().solve(b);
-
-    Eigen::Matrix4f sourceTransform;
-    sourceTransform.setIdentity();
-    sourceTransform.matrix().topLeftCorner(3, 3) = rotationFromEulerAngles(x.head<3>());
-    sourceTransform.matrix().topRightCorner(3, 1) = x.tail<3>();
-
+
+    // Retract the linearized 6-vector back to SE(3) via Rodrigues (axis-angle exponential), not Euler XYZ.
+    // The Jacobian above (cn.head<3> = p × n) corresponds to the skew-symmetric/axis-angle parameterization
+    // p_new ≈ p + ω × p + t, so retracting with R = exp([ω]×) is the internally consistent choice.
+    Eigen::Vector3f omega = x.head<3>();
+    float theta = omega.norm();
+    Eigen::Matrix3f R;
+    if (theta < 1e-8f) {
+        // Small-angle fallback: R ≈ I + [ω]× (first-order Rodrigues). Avoids the
+        // division by theta without introducing a discontinuity.
+        R = Eigen::Matrix3f::Identity();
+        R(0,1) = -omega.z(); R(0,2) =  omega.y();
+        R(1,0) =  omega.z(); R(1,2) = -omega.x();
+        R(2,0) = -omega.y(); R(2,1) =  omega.x();
+    } else {
+        R = Eigen::AngleAxisf(theta, omega / theta).toRotationMatrix();
+    }
+
+    Eigen::Matrix4f sourceTransform = Eigen::Matrix4f::Identity();
+    sourceTransform.topLeftCorner<3, 3>() = R;
+    sourceTransform.topRightCorner<3, 1>() = x.tail<3>();
+
     return sourceTransform;
 }
 
@@ -366,6 +402,10 @@ ICPResult ICP::run(ICPConfiguration config,
         // This check doesn't enforce overall camera movement limits, but is instead an early bailout
         // for when ICP simply diverges to infinity
         if (sourceTransformAdjustment.col(3).head<3>().squaredNorm() > squaredTranslationLimit) {
+            DEBUG_LOG("[ICP] iteration %d bailed: per-step translation %.3fm > limit %.3fm",
+                      iteration,
+                      sourceTransformAdjustment.col(3).head<3>().norm(),
+                      kTranslationLimit);
             result.succeeded = false;
             break;
         }
@@ -374,12 +414,19 @@ ICPResult ICP::run(ICPConfiguration config,
 
         // Calculate the correspondence error
         float rmsError = correspondence.computeRMSCorrespondenceError();
-
-        relativeError = fabsf(rmsError - previousError) / rmsError;
+
+        // Guard the relative-change convergence test against near-zero error (which
+        // would inject Inf/NaN and exit the loop prematurely).
+        const float kRelativeErrorFloor = 1e-6f;
+        relativeError = fabsf(rmsError - previousError) / std::max(rmsError, kRelativeErrorFloor);
         previousError = rmsError;
         sourceTransform = sourceTransformAdjustment * sourceTransform;
 
         if (sourceTransform.col(3).head<3>().squaredNorm() > squaredTranslationLimit) {
+            DEBUG_LOG("[ICP] iteration %d bailed: cumulative translation %.3fm > limit %.3fm",
+                      iteration,
+                      sourceTransform.col(3).head<3>().norm(),
+                      kTranslationLimit);
             result.succeeded = false;
             break;
         }

StandardCyborgFusion/Sources/StandardCyborgFusion/Algorithm/PBFModel.cpp

@@ -32,6 +32,43 @@ struct CameraVelocity {
     Vector3f velocity;
 };
 
+// Rigid-transform inverse assuming `m` is in SE(3): inv([R|t]) = [R^T | -R^T t].
+// Avoids the general 4×4 inverse which is numerically fragile once `m` drifts even
+// slightly from rigid, and sidesteps the "is this really orthogonal?" question for
+// the accumulated extrinsic matrix.
+static Eigen::Matrix4f _rigidInverse(const Eigen::Matrix4f& m)
+{
+    Eigen::Matrix4f out = Eigen::Matrix4f::Identity();
+    Eigen::Matrix3f Rt = m.topLeftCorner<3, 3>().transpose();
+    out.topLeftCorner<3, 3>() = Rt;
+    out.topRightCorner<3, 1>() = -Rt * m.topRightCorner<3, 1>();
+    return out;
+}
+
+// Scale a small rigid transform toward identity by `damping`. Rotation is slerped
+// from identity via the angle-axis exponential; translation is linearly scaled.
+// Both are valid because the deltas we handle here are small (one frame of motion).
+static Eigen::Matrix4f _dampDelta(const Eigen::Matrix4f& delta, float damping)
+{
+    damping = std::max(0.0f, std::min(1.0f, damping));
+    Eigen::Matrix3f R = delta.topLeftCorner<3, 3>();
+    Eigen::Vector3f t = delta.topRightCorner<3, 1>();
+
+    Eigen::AngleAxisf aa(R);
+    float scaledAngle = aa.angle() * damping;
+    Eigen::Matrix3f Rs;
+    if (fabsf(scaledAngle) < 1e-8f || !aa.axis().allFinite()) {
+        Rs = Eigen::Matrix3f::Identity();
+    } else {
+        Rs = Eigen::AngleAxisf(scaledAngle, aa.axis()).toRotationMatrix();
+    }
+
+    Eigen::Matrix4f out = Eigen::Matrix4f::Identity();
+    out.topLeftCorner<3, 3>() = Rs;
+    out.topRightCorner<3, 1>() = damping * t;
+    return out;
+}
+
 
 PBFModel::PBFModel(std::shared_ptr<SurfelIndexMap> surfelIndexMap, unsigned int randomSeed) :
     _surfelFusion(surfelIndexMap)
@@ -179,6 +216,12 @@ PBFAssimilatedFrameMetadata PBFModel::assimilate(ProcessedFrame& frame,
     // The surfel index map is drawn from the point of view of the incoming frame (inverse world transform)
     // Points in the new frame are un-projected into 3D based on the world transform
 
+    const size_t frameIndex = _assimilatedFrameMetadatas.size();
+    const size_t surfelsAtStart = _surfels.size();
+    DEBUG_LOG("[PBF] --- frame %zu start: surfels=%zu t=%.3f predictedDelta_t=%.4fm",
+              frameIndex, surfelsAtStart, currentTime,
+              _lastAcceptedPoseDelta.topRightCorner<3,1>().norm());
+
     // Get the current most recent metadata (haven't pushed this frame's metadata yet)
     PBFAssimilatedFrameMetadata* previousFrameMeta = _nthMostRecentValidFrameMetadata(0);
 
@@ -194,39 +237,60 @@ PBFAssimilatedFrameMetadata PBFModel::assimilate(ProcessedFrame& frame,
     const size_t height = rawFrame.height;
 
     if (_surfels.size() > 0) {
-        ICPResult icpResult = _runICP(frame, surfelFusionConfiguration, icpConfig, pbfConfig);
+        // Motion prediction: pre-transform the incoming frame by (lastDelta * prevPose) instead of just prevPose,
+        // so ICP starts from where we predict the camera is rather than where it was.
+        // Damping the delta per-frame keeps a single noisy frame's delta from ricocheting into a runaway prediction.
+        Matrix4f predictedExtrinsic = _extrinsicMatrix;
+        if (pbfConfig.enableMotionPrediction) {
+            Matrix4f dampedDelta = _dampDelta(_lastAcceptedPoseDelta, pbfConfig.motionPredictionDamping);
+            predictedExtrinsic = dampedDelta * _extrinsicMatrix;
+        }
+
+        ICPResult icpResult = _runICP(frame, surfelFusionConfiguration, icpConfig, pbfConfig, predictedExtrinsic);
 
-        Matrix4f extrinsicMatrixTmp = toMatrix4f(icpResult.sourceTransform) * _extrinsicMatrix;
+        // The transform ICP returns is the residual on top of `predictedExtrinsic`, so
+        // compose it with `predictedExtrinsic`, NOT `_extrinsicMatrix`.
+        Matrix4f extrinsicMatrixTmp = toMatrix4f(icpResult.sourceTransform) * predictedExtrinsic;
         // Store this whether or not we end up using it since we also store information about whether
         // the frame was assimilated or not
         frameMeta.viewMatrix = extrinsicMatrixTmp;
         frameMeta.icpIterationCount = icpResult.iterationCount;
         frameMeta.correspondenceError = icpResult.rmsCorrespondenceError;
 
         if (!icpResult.succeeded) {
-            DEBUG_LOG("ICP rejected due to bad convergence after %d/%d iterations", icpResult.iterationCount, icpConfig.maxIterations);
+            DEBUG_LOG("[PBF] ICP rejected: bad convergence after %d/%d iterations", icpResult.iterationCount, icpConfig.maxIterations);
             frameMeta.icpUnusedIterationFraction = 0;
         } else {
             frameMeta.icpUnusedIterationFraction = 1.0f - (float)icpResult.iterationCount / (float)icpConfig.maxIterations;
 
             CameraVelocity cv = _cameraVelocity(previousFrameMeta, &frameMeta);
 
             if (cv.angularVelocity.hasNaN() || cv.angularVelocity.norm() > pbfConfig.maxCameraAngularVelocity) {
-                DEBUG_LOG("Rejecting ICP due to bad fit with angular velocity %f", cv.angularVelocity.norm());
+                DEBUG_LOG("[PBF] ICP rejected: angular velocity %.3f rad/s > %.3f",
+                          cv.angularVelocity.norm(), pbfConfig.maxCameraAngularVelocity);
                 frameMeta.icpUnusedIterationFraction = 0;
             }
 
             else if (cv.velocity.hasNaN() || cv.velocity.norm() > pbfConfig.maxCameraVelocity) {
-                DEBUG_LOG("Rejecting ICP due to bad fit with linear velocity %f", cv.velocity.norm());
+                DEBUG_LOG("[PBF] ICP rejected: linear velocity %.3f m/s > %.3f",
+                          cv.velocity.norm(), pbfConfig.maxCameraVelocity);
                 frameMeta.icpUnusedIterationFraction = 0;
             }
         }
 
         if (frameMeta.icpUnusedIterationFraction > 0) {
+            // If the frame was rejected we leave the previous delta in place;
+            // the previous velocity remains the best extrapolation for the next frame.
+            _lastAcceptedPoseDelta = extrinsicMatrixTmp * _rigidInverse(_extrinsicMatrix);
             _extrinsicMatrix = extrinsicMatrixTmp;
+
+            DEBUG_LOG("[PBF] frame %zu accepted: iters=%d/%d rms=%.4fm quality=%.2f",
+                      frameIndex, icpResult.iterationCount, icpConfig.maxIterations,
+                      icpResult.rmsCorrespondenceError,
+                      frameMeta.icpUnusedIterationFraction);
         } else {
-            // It didn't converge in time, so bail out
-            DEBUG_LOG("ICP didn't converge with enough quality (%f) after %d/%d iterations. Ignoring frame.", frameMeta.icpUnusedIterationFraction, icpResult.iterationCount, icpConfig.maxIterations);
+            DEBUG_LOG("[PBF] Dropping frame: quality %.2f after %d/%d iterations",
+                      frameMeta.icpUnusedIterationFraction, icpResult.iterationCount, icpConfig.maxIterations);
             _assimilatedFrameMetadatas.push_back(frameMeta);
             return frameMeta;
         }
@@ -326,6 +390,7 @@ void PBFModel::reset(unsigned int randomSeed)
     DEBUG_LOG("Resetting");
 
     _extrinsicMatrix.setIdentity();
+    _lastAcceptedPoseDelta.setIdentity();
 
     _fastRNG.seed(randomSeed);
 
@@ -338,7 +403,11 @@ void PBFModel::reset(unsigned int randomSeed)
 
 // MARK: - Private
 
-ICPResult PBFModel::_runICP(ProcessedFrame& frame, SurfelFusionConfiguration surfelFusionConfiguration, ICPConfiguration icpConfig, PBFConfiguration pbfConfig)
+ICPResult PBFModel::_runICP(ProcessedFrame& frame,
+                            SurfelFusionConfiguration surfelFusionConfiguration,
+                            ICPConfiguration icpConfig,
+                            PBFConfiguration pbfConfig,
+                            const Eigen::Matrix4f& predictedExtrinsic)
 {
     bool shouldRebuildPointCloud = false;
     if (_assimilatedFrameMetadatas.size() < pbfConfig.kdTreeRebuildInterval / 2 || _ICPTargetCloud == nullptr) {
@@ -367,8 +436,11 @@ ICPResult PBFModel::_runICP(ProcessedFrame& frame, SurfelFusionConfiguration sur
         downsampledVertices.reserve(frame.positions.size());
 
         float downsampledFraction = pbfConfig.icpDownsampleFraction;
-
-        Matrix3f normalTransform = NormalMatrixFromMat4(_extrinsicMatrix);
+
+        // Pre-transform by the predicted pose (constant-velocity extrapolation) so ICP's initial residual
+        // is as small as possible. When motion prediction is disabled in config, `predictedExtrinsic`
+        // is just `_extrinsicMatrix`, which reduces to the original constant-pose seed.
+        Matrix3f normalTransform = NormalMatrixFromMat4(predictedExtrinsic);
 
         // Filter by depth
         size_t pointCount = frame.positions.size();
@@ -387,7 +459,7 @@ ICPResult PBFModel::_runICP(ProcessedFrame& frame, SurfelFusionConfiguration sur
             if (_fastRNG.sample(1000) > frame.weights[i] * 1000.0f) continue;
             if (depth < surfelFusionConfiguration.minDepth || depth > surfelFusionConfiguration.maxDepth) continue;
 
-            downsampledVertices.push_back(standard_cyborg::toVec3(  Vec3TransformMat4( toVector3f(frame.positions[i]), _extrinsicMatrix)  ) );
+            downsampledVertices.push_back(standard_cyborg::toVec3(Vec3TransformMat4(toVector3f(frame.positions[i]), predictedExtrinsic)));
 
             downsampledNormals.push_back(standard_cyborg::toVec3(normalTransform * standard_cyborg::toVector3f(frame.normals[i])));
 

StandardCyborgFusion/Sources/StandardCyborgFusion/Algorithm/PBFModel.hpp

@@ -68,8 +68,15 @@ class PBFModel {
 
     Eigen::Matrix4f _extrinsicMatrix = Eigen::Matrix4f::Identity();
 
+    // Last-accepted pose change for constant-velocity motion prediction
+    Eigen::Matrix4f _lastAcceptedPoseDelta = Eigen::Matrix4f::Identity();
+
     void _cullLowConfidence(bool ignoreLifetime, int minWeight, std::vector<int>* deletedSurfelList = NULL);
-    ICPResult _runICP(ProcessedFrame& frame, SurfelFusionConfiguration surfelFusionConfiguration, ICPConfiguration icpConfig, PBFConfiguration pbfConfig);
+    ICPResult _runICP(ProcessedFrame& frame,
+                      SurfelFusionConfiguration surfelFusionConfiguration,
+                      ICPConfiguration icpConfig,
+                      PBFConfiguration pbfConfig,
+                      const Eigen::Matrix4f& predictedExtrinsic);
 
     PBFAssimilatedFrameMetadata* _nthMostRecentValidFrameMetadata(size_t offset = 0);
     PBFFinalStatistics _calcFinalStatistics();