From 2424aa6d7284cb9e40057bb92d22857967baaa65 Mon Sep 17 00:00:00 2001 From: Sibo Wang-Chen Date: Mon, 27 Jul 2026 13:57:50 +0200 Subject: [PATCH] reuse preallocated buffer for \delta x solution in Cholesky solver --- benchmark/plot/results/comparison-2d.svg | 528 ++++++++------- benchmark/plot/results/comparison.svg | 782 +++++++++++------------ benchmark/plot/results/scaling.svg | 64 +- src/solver.rs | 16 +- 4 files changed, 672 insertions(+), 718 deletions(-) diff --git a/benchmark/plot/results/comparison-2d.svg b/benchmark/plot/results/comparison-2d.svg index ae14701..c225da5 100644 --- a/benchmark/plot/results/comparison-2d.svg +++ b/benchmark/plot/results/comparison-2d.svg @@ -6,7 +6,7 @@ - 2026-07-27T01:01:04.649962 + 2026-07-27T13:46:40.210975 image/svg+xml @@ -41,12 +41,12 @@ z - - + @@ -56,110 +56,100 @@ L 0 3.5 - + - 0.01 + 0.01 - + - 0.02 + 0.02 - + - 0.03 + 0.03 - + - 0.04 + 0.04 - + - 0.05 + 0.05 - + - 0.06 + 0.06 - + - 0.07 + 0.07 - - - - - - - - 0.08 - - - + Latency (ms) - + - - + - + 3D - + - + - + XYView @@ -180,24 +170,24 @@ L 339.01875 73.334767 L 339.01875 60.609353 L 47.41875 60.609353 z -" clip-path="url(#p1ec19649fb)" style="fill: #4051b5"/> +" clip-path="url(#p823f54e09b)" style="fill: #4051b5"/> +" clip-path="url(#p823f54e09b)" style="fill: #2fb170"/> - + Mean single-frame latency (with early stopping) - - 66.8 μs + + 61.7 μs - - 58.6 μs + + 55.5 μs @@ -210,108 +200,108 @@ z " style="fill: #ffffff"/> + + + + + + + + 0.00 + + - + - 0.00 + 0.02 - + - 0.02 + 0.04 - + - 0.04 + 0.06 - + - 0.06 + 0.08 - + - 0.08 + 0.10 - + - 0.10 + 0.12 - + - 0.12 - - - - - - - - - - 0.14 + 0.14 - + Latency (ms) - + - + - + 3D - + - + - + XYView @@ -332,24 +322,24 @@ L 339.01875 176.410617 L 339.01875 163.685203 L 47.41875 163.685203 z -" clip-path="url(#p6bb2df0dac)" style="fill: #4051b5"/> +" clip-path="url(#p9020e59196)" style="fill: #4051b5"/> +" clip-path="url(#p9020e59196)" style="fill: #2fb170"/> - + Single-frame latency (fixed 10 iterations) - - 133 μs + + 122 μs - - 115 μs + + 111 μs @@ -362,118 +352,118 @@ z " style="fill: #ffffff"/> + + + + + + + + 0.0 + + - + - 0.0 + 2.5 - + - 2.5 + 5.0 - + - 5.0 + 7.5 - + - 7.5 + 10.0 - + - 10.0 + 12.5 - + - 12.5 + 15.0 - + - 15.0 + 17.5 - + - 17.5 + 20.0 - - - - - - - - 20.0 - - - + Throughput (kFPS) - + - + - + 3D - + - + - + XYView @@ -490,11 +480,11 @@ L 397.33875 295.393233 +" clip-path="url(#pb5033cfc21)" style="fill: #4051b5"/> +" clip-path="url(#pb5033cfc21)" style="fill: #2fb170"/> - + Throughput (sequential frames, single thread) - - 10.9 kFPS + + 12.0 kFPS - - 18.2 kFPS + + 18.5 kFPS @@ -524,98 +514,98 @@ z " style="fill: #ffffff"/> + + + + + + + + 0 + + - + - 0 + 20 - + - 20 + 40 - + - 40 + 60 - + - 60 + 80 - + - 80 + 100 - + - 100 + 120 - - - - - - - - 120 - - - + Throughput (kFPS) - + - + - + 3D - + - + - + XYView @@ -632,11 +622,11 @@ L 397.33875 398.469083 +" clip-path="url(#p41d96375ae)" style="fill: #4051b5"/> +" clip-path="url(#p41d96375ae)" style="fill: #2fb170"/> - + Throughput (sequential frames, 8 threads) - - 73.5 kFPS + + 79.8 kFPS - - 101 kFPS + + 107 kFPS @@ -666,148 +656,148 @@ z " style="fill: #ffffff"/> + + + + + + + + 0.000 + + - + - 0.000 + 0.025 - + - 0.025 + 0.050 - + - 0.050 + 0.075 - + - 0.075 + 0.100 - + - 0.100 + 0.125 - + - 0.125 + 0.150 - + - 0.150 + 0.175 - + - 0.175 - - - - - - - - - 0.200 - + 3D distance from solved pose to target (model units) - + - + - + 0 - + - + - + 25 - + - + - + 50 - + - + - + 75 - + density - - - + + - - - + + @@ -2437,7 +2427,7 @@ L 47.41875 469.731398 L 397.33875 565.172 " style="fill: none; stroke: #000000; stroke-width: 0.8; stroke-linejoin: miter; stroke-linecap: square"/> - + +" clip-path="url(#pd1a67dee49)" style="fill: none; stroke: #4051b5; stroke-width: 2; stroke-linecap: square"/> - + +" clip-path="url(#pd1a67dee49)" style="fill: none; stroke: #2fb170; stroke-width: 2; stroke-linecap: square"/> - + Fit-residual distribution (per-frame average, n=22 real mocap frames) - + - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + - + - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + @@ -2622,7 +2612,7 @@ L 127.093047 15.657324 z " style="fill: #4051b5"/> - + 3D @@ -2633,25 +2623,25 @@ L 194.163828 15.657324 z " style="fill: #2fb170"/> - + XYView - + - + - + - + - + diff --git a/benchmark/plot/results/comparison.svg b/benchmark/plot/results/comparison.svg index 565591a..597615a 100644 --- a/benchmark/plot/results/comparison.svg +++ b/benchmark/plot/results/comparison.svg @@ -6,7 +6,7 @@ - 2026-07-27T01:01:02.723375 + 2026-07-27T13:46:38.320407 image/svg+xml @@ -43,18 +43,18 @@ L 520.9925 135.780962 L 520.9925 56.78 L 100.5125 56.78 z -" clip-path="url(#p367144b49b)" style="fill: #ffec3d; opacity: 0.3; stroke: #ffec3d; stroke-linejoin: miter"/> +" clip-path="url(#pe574cce5ea)" style="fill: #ffec3d; opacity: 0.3; stroke: #ffec3d; stroke-linejoin: miter"/> - - + @@ -64,71 +64,71 @@ L 0 3.5 - + - 1 + 1 - + - 2 + 2 - + - 3 + 3 - + - 4 + 4 - + - 5 + 5 - + - 6 + 6 - + - 7 + 7 @@ -139,12 +139,12 @@ L 0 3.5 - - + @@ -154,7 +154,7 @@ L -3.5 0 - + @@ -164,7 +164,7 @@ L -3.5 0 - + @@ -174,7 +174,7 @@ L -3.5 0 - + @@ -184,7 +184,7 @@ L -3.5 0 - + @@ -194,7 +194,7 @@ L -3.5 0 - + @@ -204,7 +204,7 @@ L -3.5 0 - + @@ -214,7 +214,7 @@ L -3.5 0 - + @@ -234,67 +234,67 @@ L 520.9925 267.449231 +" clip-path="url(#pe574cce5ea)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#pe574cce5ea)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#pe574cce5ea)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#pe574cce5ea)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#pe574cce5ea)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#pe574cce5ea)" style="fill: #4051b5"/> +" clip-path="url(#pe574cce5ea)" style="fill: #4051b5"/> +" clip-path="url(#pe574cce5ea)" style="fill: #4051b5"/> +" clip-path="url(#pe574cce5ea)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#pe574cce5ea)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#pe574cce5ea)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#pe574cce5ea)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#pe574cce5ea)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#pe574cce5ea)" style="fill: #2fb170"/> +" clip-path="url(#pe574cce5ea)" style="fill: #2fb170"/> +" clip-path="url(#pe574cce5ea)" style="fill: #2fb170"/> Mean single-frame latency (with early stopping) - 6.20 ms + 6.20 ms - 1.14 ms + 1.15 ms - 714 μs + 709 μs - 322 μs + 319 μs - 312 μs + 311 μs - 63.6 μs + 61.8 μs - 61.1 μs + 61.0 μs - 66.8 μs + 61.7 μs - 6.55 ms + 6.56 ms - 867 μs + 867 μs - 381 μs + 379 μs - 235 μs + 236 μs - 234 μs + 233 μs - 45.0 μs + 42.2 μs - 42.2 μs + 42.6 μs - 44.5 μs + 43.2 μs @@ -427,13 +427,13 @@ L 520.9925 430.717885 L 520.9925 351.716923 L 100.5125 351.716923 z -" clip-path="url(#pc3aa72ed61)" style="fill: #ffec3d; opacity: 0.3; stroke: #ffec3d; stroke-linejoin: miter"/> +" clip-path="url(#p2933785fe4)" style="fill: #ffec3d; opacity: 0.3; stroke: #ffec3d; stroke-linejoin: miter"/> - + @@ -443,51 +443,51 @@ z - + - 2 + 2 - + - 4 + 4 - + - 6 + 6 - + - 8 + 8 - + - 10 + 10 @@ -498,7 +498,7 @@ z - + @@ -508,7 +508,7 @@ z - + @@ -518,7 +518,7 @@ z - + @@ -528,7 +528,7 @@ z - + @@ -538,7 +538,7 @@ z - + @@ -548,7 +548,7 @@ z - + @@ -558,7 +558,7 @@ z - + @@ -568,7 +568,7 @@ z - + @@ -592,178 +592,178 @@ L 466.147283 549.219327 L 466.147283 538.685865 L 100.5125 538.685865 z -" clip-path="url(#pc3aa72ed61)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#p2933785fe4)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#p2933785fe4)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#p2933785fe4)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#p2933785fe4)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#p2933785fe4)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#p2933785fe4)" style="fill: #4051b5"/> +" clip-path="url(#p2933785fe4)" style="fill: #4051b5"/> +" clip-path="url(#p2933785fe4)" style="fill: #4051b5"/> +" clip-path="url(#p2933785fe4)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#p2933785fe4)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#p2933785fe4)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#p2933785fe4)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#p2933785fe4)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#p2933785fe4)" style="fill: #2fb170"/> +" clip-path="url(#p2933785fe4)" style="fill: #2fb170"/> +" clip-path="url(#p2933785fe4)" style="fill: #2fb170"/> Single-frame latency (fixed 10 iterations) - 10.4 ms + 10.3 ms - 2.20 ms + 2.20 ms - 1.42 ms + 1.41 ms - 614 μs + 616 μs - 601 μs + 601 μs - 126 μs + 122 μs - 121 μs + 122 μs - 133 μs + 122 μs - 6.57 ms + 6.54 ms - 2.06 ms + 2.08 ms - 951 μs + 947 μs - 393 μs + 392 μs - 391 μs + 390 μs - 114 μs + 104 μs - 105 μs + 105 μs - 111 μs + 108 μs @@ -781,13 +781,13 @@ L 520.9925 725.654808 L 520.9925 646.653846 L 100.5125 646.653846 z -" clip-path="url(#p113513ce56)" style="fill: #ffec3d; opacity: 0.3; stroke: #ffec3d; stroke-linejoin: miter"/> +" clip-path="url(#p9ac235d6b8)" style="fill: #ffec3d; opacity: 0.3; stroke: #ffec3d; stroke-linejoin: miter"/> - + @@ -797,61 +797,61 @@ z - + - 2 + 2 - + - 4 + 4 - + - 6 + 6 - + - 8 + 8 - + - 10 + 10 - + - 12 + 12 @@ -862,7 +862,7 @@ z - + @@ -872,7 +872,7 @@ z - + @@ -882,7 +882,7 @@ z - + @@ -892,7 +892,7 @@ z - + @@ -902,7 +902,7 @@ z - + @@ -912,7 +912,7 @@ z - + @@ -922,7 +922,7 @@ z - + @@ -932,7 +932,7 @@ z - + @@ -952,182 +952,182 @@ L 520.9925 857.323077 +" clip-path="url(#p9ac235d6b8)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#p9ac235d6b8)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#p9ac235d6b8)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#p9ac235d6b8)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#p9ac235d6b8)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#p9ac235d6b8)" style="fill: #4051b5"/> +" clip-path="url(#p9ac235d6b8)" style="fill: #4051b5"/> +" clip-path="url(#p9ac235d6b8)" style="fill: #4051b5"/> +" clip-path="url(#p9ac235d6b8)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#p9ac235d6b8)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#p9ac235d6b8)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#p9ac235d6b8)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#p9ac235d6b8)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#p9ac235d6b8)" style="fill: #2fb170"/> +" clip-path="url(#p9ac235d6b8)" style="fill: #2fb170"/> +" clip-path="url(#p9ac235d6b8)" style="fill: #2fb170"/> Throughput (sequential frames, single thread) - 124 FPS + 125 FPS - 643 FPS + 643 FPS - 1.03 kFPS + 1.04 kFPS - 2.28 kFPS + 2.28 kFPS - 2.34 kFPS + 2.37 kFPS - 11.7 kFPS + 12.0 kFPS - 11.9 kFPS + 11.8 kFPS - 10.9 kFPS + 12.0 kFPS - 15.2 FPS + 15.3 FPS - 464 FPS + 461 FPS - 1.05 kFPS + 1.06 kFPS - 2.53 kFPS + 2.53 kFPS - 2.53 kFPS + 2.53 kFPS - 9.14 kFPS + 9.56 kFPS - 9.15 kFPS + 9.49 kFPS - 7.95 kFPS + 9.31 kFPS @@ -1145,13 +1145,13 @@ L 520.9925 1020.591731 L 520.9925 941.590769 L 100.5125 941.590769 z -" clip-path="url(#p43a69c3c0e)" style="fill: #ffec3d; opacity: 0.3; stroke: #ffec3d; stroke-linejoin: miter"/> +" clip-path="url(#p8c6974cdd4)" style="fill: #ffec3d; opacity: 0.3; stroke: #ffec3d; stroke-linejoin: miter"/> - + @@ -1161,165 +1161,125 @@ z - + - 10 + 20 - + - 20 + 40 - + - 30 + 60 - + - 40 + 80 - - - - - - - - 50 - - - - - - - - - - 60 - - - - - - - - - - 70 - - - - - - - - - - 80 - - - + Throughput (kFPS) - + - + - + KDL - + - + - + Pinocchio (Python) - + - + - + Pinocchio (C++) - + - + - + RBDL (Python) - + - + - + RBDL (C++) - + - + - + QuickIK (Python) - + - + - + QuickIK (C++) - + - + - + QuickIK (Rust) @@ -1336,182 +1296,182 @@ L 520.9925 1152.26 +" clip-path="url(#p8c6974cdd4)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#p8c6974cdd4)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#p8c6974cdd4)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#p8c6974cdd4)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#p8c6974cdd4)" style="fill: #4051b5; opacity: 0.5"/> +" clip-path="url(#p8c6974cdd4)" style="fill: #4051b5"/> +" clip-path="url(#p8c6974cdd4)" style="fill: #4051b5"/> +" clip-path="url(#p8c6974cdd4)" style="fill: #4051b5"/> +" clip-path="url(#p8c6974cdd4)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#p8c6974cdd4)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#p8c6974cdd4)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#p8c6974cdd4)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#p8c6974cdd4)" style="fill: #2fb170; opacity: 0.5"/> +" clip-path="url(#p8c6974cdd4)" style="fill: #2fb170"/> +" clip-path="url(#p8c6974cdd4)" style="fill: #2fb170"/> +" clip-path="url(#p8c6974cdd4)" style="fill: #2fb170"/> - + Throughput (sequential frames, 8 threads) + + 896 FPS + + + 4.50 kFPS + + + 7.50 kFPS + + + 15.9 kFPS + - 886 FPS + 17.0 kFPS - 4.57 kFPS + 77.0 kFPS - 7.46 kFPS + 75.9 kFPS - 14.9 kFPS + 79.8 kFPS - 17.1 kFPS + 109 FPS - 76.1 kFPS + 3.28 kFPS - 77.4 kFPS + 7.72 kFPS - 73.5 kFPS + 16.3 kFPS - 109 FPS + 18.9 kFPS - 3.12 kFPS + 62.2 kFPS - 7.63 kFPS + 63.0 kFPS - 15.5 kFPS - - - 18.8 kFPS - - - 59.4 kFPS - - - 62.8 kFPS - - - 59.0 kFPS + 60.9 kFPS @@ -1523,7 +1483,7 @@ L 84.144141 15.657324 z " style="fill: #4051b5"/> - + NeuroMechFly (42 DOFs) @@ -1534,22 +1494,22 @@ L 263.555859 15.657324 z " style="fill: #2fb170"/> - + G1 humanoid (29 DOFs) - + - + - + - + diff --git a/benchmark/plot/results/scaling.svg b/benchmark/plot/results/scaling.svg index 83962b8..70e7b8b 100644 --- a/benchmark/plot/results/scaling.svg +++ b/benchmark/plot/results/scaling.svg @@ -6,7 +6,7 @@ - 2026-07-27T01:01:03.415472 + 2026-07-27T13:46:38.974354 image/svg+xml @@ -41,12 +41,12 @@ z - - + @@ -56,7 +56,7 @@ L 0 3.5 - + @@ -66,7 +66,7 @@ L 0 3.5 - + @@ -76,7 +76,7 @@ L 0 3.5 - + @@ -86,7 +86,7 @@ L 0 3.5 - + @@ -101,12 +101,12 @@ L 0 3.5 - - + @@ -116,7 +116,7 @@ L -3.5 0 - + @@ -126,7 +126,7 @@ L -3.5 0 - + @@ -136,7 +136,7 @@ L -3.5 0 - + @@ -146,7 +146,7 @@ L -3.5 0 - + @@ -163,17 +163,17 @@ L 147.225385 168.113266 L 226.916298 123.711049 L 306.607211 79.308832 L 386.298124 34.906615 -" clip-path="url(#p2a7bf0b2c6)" style="fill: none; stroke-dasharray: 5.55,2.4; stroke-dashoffset: 0; stroke: #808080; stroke-width: 1.5"/> +" clip-path="url(#p20b142b52a)" style="fill: none; stroke-dasharray: 5.55,2.4; stroke-dashoffset: 0; stroke: #808080; stroke-width: 1.5"/> +L 147.225385 173.470943 +L 226.916298 132.924871 +L 306.607211 90.074366 +L 386.298124 82.827118 +" clip-path="url(#p20b142b52a)" style="fill: none; stroke: #4051b5; stroke-width: 1.5; stroke-linecap: square"/> - - - - - - - + + + + + + @@ -207,16 +207,16 @@ L 402.236307 235.363672 1.00x - 1.81x + 1.84x - 3.46x + 3.46x - 6.52x + 6.76x - 7.36x + 7.57x Speedup v.s. workers @@ -237,7 +237,7 @@ L 70.596289 60.832813 L 80.596289 60.832813 " style="fill: none; stroke: #4051b5; stroke-width: 1.5; stroke-linecap: square"/> - + @@ -247,7 +247,7 @@ L 80.596289 60.832813 - + diff --git a/src/solver.rs b/src/solver.rs index 5190fcc..39f534c 100644 --- a/src/solver.rs +++ b/src/solver.rs @@ -79,6 +79,9 @@ pub struct Solver { joint_weight_scalers: Vec, jtj: DMatrix, jtr: DVector, + /// Gauss-Newton update step, reused across iterations and solved into in + /// place by `Cholesky::solve_mut` to avoid a per-iteration allocation. + delta: DVector, /// Per-keypoint Jacobian buffer in compact form: shape is 3 x state_dim, /// but nonzero columns are moved to the left, and the rest is ignored. jacobian_buffer: DMatrix, @@ -114,6 +117,7 @@ impl Solver { joint_weight_scalers, jtj: DMatrix::zeros(state_dim, state_dim), jtr: DVector::zeros(state_dim), + delta: DVector::zeros(state_dim), jacobian_buffer: DMatrix::zeros(3, state_dim), jacobian_2d_buffer: DMatrix::zeros(2, state_dim), config, @@ -203,11 +207,11 @@ impl Solver { // contains garbage value after decomposition. But this is fine // because jtj is zeroed at the start of each solver iteration. let jtj_owned = std::mem::replace(&mut self.jtj, DMatrix::zeros(0, 0)); - let delta = match nalgebra::linalg::Cholesky::new(jtj_owned) { + match nalgebra::linalg::Cholesky::new(jtj_owned) { Some(chol) => { - let delta = chol.solve(&self.jtr); + self.delta.copy_from(&self.jtr); + chol.solve_mut(&mut self.delta); self.jtj = chol.unpack(); - delta } None => { // Not positive-definite (numerically unstable): no @@ -215,12 +219,12 @@ impl Solver { // is observed (even if some are observed, the root might // be underconstrained and matches the targets exactly). self.jtj = DMatrix::zeros(state_dim, state_dim); - DVector::zeros(state_dim) + self.delta.as_mut_slice().fill(0.0); } }; - state.apply_delta(&delta); + state.apply_delta(&self.delta); - if self.has_converged(&delta) { + if self.has_converged(&self.delta) { break; } }