mirror of
https://github.com/harry7557558/spirula-studio.git
synced 2026-10-02 02:44:54 +08:00
sfm: speed up mapper / BA solver
This commit is contained in:
Harry Chen
@@ -259,6 +259,7 @@ int cmdBa(int argc, char** argv) {
|
||||
if (cmp_step) {
|
||||
opt.solver = SolverSel::CG;
|
||||
opt.cg_fallback = CgFallback::On;
|
||||
opt.cg_model_tol = 0; // --cg-tol alone sets how exact the step is
|
||||
}
|
||||
|
||||
// A directory is a COLMAP sparse model: build the problem the mapper's
|
||||
|
||||
@@ -699,6 +699,11 @@ What the run does with it, in the order it happens:
|
||||
through the calibration exactly as if they shared those images, so a 360
|
||||
capture that reconstructs as one component per direction is merged rather
|
||||
than written as pieces (`map/Merge.h` `poseCorrespondences`).
|
||||
- **Splitting.** The consistency split (`splitInconsistent`) groups images by
|
||||
the verified pairs a model still agrees with, and counts a calibrated rig
|
||||
frame as joining its images too. Back-to-back fisheyes share no matches, so
|
||||
without that a 4000-image dual-fisheye model with every pair agreeing split
|
||||
into its two lenses and spent five minutes merging them back.
|
||||
|
||||
`--final-free-rig` (off) runs one last bundle adjustment with the rig set
|
||||
aside, for a mount that flexed or lenses that did not fire together. With no
|
||||
@@ -804,6 +809,39 @@ adjacent pairs' own two-view geometry, or the video's IMU. `SS_SFM_SEQ_DUMP=1`
|
||||
prints one line per registration attempt (near and whole-pool inliers, and
|
||||
the rival's) to read such a spot from the log.
|
||||
|
||||
### Retriangulation
|
||||
|
||||
Every global refinement round after the first starts by completing tracks and
|
||||
retriangulating the whole model (`completeAndRetriangulate`, COLMAP's
|
||||
CompleteTracks + Retriangulate): each point's correspondences are tested
|
||||
against it, and each registered image's free features against their
|
||||
registered partners. Both were serial whole-model passes, so on a long video
|
||||
they were the stretch between two bundle adjustments where the GPU idles and
|
||||
one core works -- 34 s of a 3000-frame capture's 332 s of mapping, as much as
|
||||
the seed search and registration together.
|
||||
|
||||
Each half now collects its per-item result on every core against the state
|
||||
the pass starts from, and commits in the old serial order. A claim only ever
|
||||
takes a free feature, so an item's collected result can be wrong only if an
|
||||
earlier commit took one of the features it claims; that item is redone
|
||||
serially at its turn. The output is the serial pass's exactly -- checked by
|
||||
running both on the same state, 0 differences over 373 passes and every
|
||||
growth-time triangulation of a 638-image capture. Growth-time triangulation
|
||||
after each registration goes the same way, feature by feature.
|
||||
|
||||
Most of what the pass then does is futile: on a 4000-image dual-fisheye video
|
||||
it tried 166M candidate pairs a pass for 13M free features and made no point
|
||||
from any of them in steady state -- neighbouring frames 1/30 s apart are all
|
||||
far under the 1.5 degree triangulation angle. Two rays can only reach that
|
||||
angle if the angle between them is within both reprojection tolerances of it
|
||||
(each bounded as twice the tolerance over the focal), so a candidate further
|
||||
off is dropped before triangulation, on world-frame rays cached per pass in
|
||||
float (`worldRays`; the fisheye bearing behind each is an iterative inversion,
|
||||
and computing it per candidate was the actual cost). That is 94% of the
|
||||
candidates there; results are identical with the filter on and off on all
|
||||
three stress captures. A pass over the 4000-image model went from 27.3 s to
|
||||
4.9 s, over a 3000-frame DJI walk from 8.1 s to 0.6 s.
|
||||
|
||||
### The finishing passes
|
||||
|
||||
Reconstruction ends with up to two more global bundle adjustments, on models
|
||||
|
||||
+14
-2
@@ -60,8 +60,13 @@ public:
|
||||
factor(pool, nthreads);
|
||||
solveInPlace(g, pool, nthreads);
|
||||
}
|
||||
void solve(double* g, Pool& pool, int nthreads) { solveInPlace(g, pool, nthreads); }
|
||||
|
||||
void factor(Pool& pool, int nthreads) {
|
||||
// With `diag`, a pivot under `rel` of its row's diag entry is replaced by
|
||||
// that entry (cholesky.slang's pivot(), for the coarse matrix).
|
||||
void factor(Pool& pool, int nthreads, const double* diag = nullptr, double rel = 0) {
|
||||
diag_in_ = diag;
|
||||
rel_ = rel;
|
||||
const uint32_t nb = (n_ + kBlock - 1) / kBlock;
|
||||
for (uint32_t k = 0; k < nb; k++) {
|
||||
const uint32_t base = k * kBlock;
|
||||
@@ -120,7 +125,12 @@ private:
|
||||
void factorDiag(uint32_t base, uint32_t m) {
|
||||
for (uint32_t j = 0; j < m; j++) {
|
||||
double* Rj = row(base + j) + base;
|
||||
double d = std::sqrt(std::max(Rj[j], 1e-30));
|
||||
double d;
|
||||
if (!diag_in_)
|
||||
d = std::sqrt(Rj[j] > 1e-30 ? Rj[j] : 1e-30);
|
||||
else
|
||||
d = std::sqrt(Rj[j] > rel_ * diag_in_[base + j] ? Rj[j]
|
||||
: std::max(diag_in_[base + j], 1e-30));
|
||||
Rj[j] = d;
|
||||
const double inv = 1.0 / d;
|
||||
for (uint32_t r = j + 1; r < m; r++) {
|
||||
@@ -254,6 +264,8 @@ private:
|
||||
}
|
||||
|
||||
std::vector<double> a_, panel_, panelT_, diag_;
|
||||
const double* diag_in_ = nullptr;
|
||||
double rel_ = 0;
|
||||
uint32_t n_ = 0;
|
||||
};
|
||||
|
||||
|
||||
@@ -97,6 +97,10 @@ struct SolverOptions {
|
||||
bool over_budget_throws = false;
|
||||
int cg_max_iters = 100; // CG iteration cap per LM step
|
||||
double cg_tol = 0.1; // relative residual tolerance eta
|
||||
// ... and CG also stops once a step improves the quadratic model by under
|
||||
// this fraction of the total so far (Nash-Sofer; 0 = off). It settles for a
|
||||
// residual near sqrt of it, so a caller that wants the exact step turns it off.
|
||||
double cg_model_tol = 0.1;
|
||||
CgFallback cg_fallback = CgFallback::Auto;
|
||||
// The kernels are compiled per (real, loss); `loss` selects the embedded
|
||||
// blob "ba_<real>_<loss>". spv_path overrides it with a module from disk
|
||||
|
||||
@@ -16,6 +16,7 @@
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "core/Env.h"
|
||||
#include "sfm/core/Log.h"
|
||||
|
||||
// Camera model registry; must match the entry points in sfm/shaders/ba/ba.slang.
|
||||
@@ -211,6 +212,70 @@ inline uint64_t pairEntryCount(const BAProblem& P) {
|
||||
|
||||
static const uint64_t kMaxPairEntries = 400ull << 20; // 3.2 GB of entry data
|
||||
|
||||
// Coarse-correction tables (cg.slang). A run is a track's observations in one
|
||||
// cluster of k frames (contiguous: tracks are sorted by image); each pair of a
|
||||
// point's runs u >= v is one entry, (first obs of u, first obs of v).
|
||||
template <class F>
|
||||
inline void forCoarseRunPairs(const BAProblem& P, uint32_t k, F&& fn) {
|
||||
auto cl = [&](uint32_t o) { return P.image_frame[P.obs_image[o]] / k; };
|
||||
std::vector<uint32_t> runs;
|
||||
for (uint32_t p = 0; p < P.num_points; p++) {
|
||||
runs.clear();
|
||||
for (uint32_t o = P.obs_ranges[p]; o < P.obs_ranges[p + 1]; o++)
|
||||
if (o == P.obs_ranges[p] || cl(o) != cl(o - 1)) runs.push_back(o);
|
||||
for (size_t u = 0; u < runs.size(); u++)
|
||||
for (size_t v = 0; v <= u; v++) {
|
||||
const uint64_t cu = cl(runs[u]), cv = cl(runs[v]);
|
||||
fn(cu * (cu + 1) / 2 + cv, runs[u], runs[v]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline uint64_t coarseEntryCount(const BAProblem& P, uint32_t k) {
|
||||
uint64_t n = 0;
|
||||
forCoarseRunPairs(P, k, [&](uint64_t, uint32_t, uint32_t) { n++; });
|
||||
return n;
|
||||
}
|
||||
|
||||
// Entries grouped by cluster pair: `key` holds each pair's range, indexed
|
||||
// cu (cu + 1) / 2 + cv.
|
||||
inline void buildCoarseEntries(const BAProblem& P, uint32_t k, std::vector<uint32_t>& ent,
|
||||
std::vector<uint32_t>& key) {
|
||||
const uint64_t nc = (P.num_frames + k - 1) / k, nkeys = nc * (nc + 1) / 2;
|
||||
key.assign(nkeys + 1, 0);
|
||||
forCoarseRunPairs(P, k, [&](uint64_t kk, uint32_t, uint32_t) { key[kk + 1]++; });
|
||||
for (uint64_t i = 0; i < nkeys; i++) key[i + 1] += key[i];
|
||||
std::vector<uint32_t> fill(key.begin(), key.end() - 1);
|
||||
ent.resize(2 * (size_t)key[nkeys]);
|
||||
forCoarseRunPairs(P, k, [&](uint64_t kk, uint32_t a, uint32_t b) {
|
||||
const uint32_t e = fill[kk]++;
|
||||
ent[2 * (size_t)e] = a;
|
||||
ent[2 * (size_t)e + 1] = b;
|
||||
});
|
||||
}
|
||||
|
||||
// Clusters as small as a coarse matrix of `max_dim` allows (7 dofs each), and
|
||||
// as large as keeps the entries under two an observation (long tracks span many
|
||||
// small clusters). False: fewer than 4 clusters, or SS_SFM_BA_COARSE=0.
|
||||
inline bool planCoarse(const BAProblem& P, uint32_t max_dim, uint32_t& k, uint32_t& dim,
|
||||
uint64_t& entries) {
|
||||
k = dim = 0;
|
||||
entries = 0;
|
||||
const char* e = spirula::env("SFM_BA_COARSE");
|
||||
if (e && std::atoi(e) == 0) return false;
|
||||
const uint32_t nf = P.num_frames;
|
||||
for (uint32_t kk = std::max<uint32_t>(2, (7 * nf + max_dim - 1) / max_dim);
|
||||
(nf + kk - 1) / kk >= 4; kk *= 2) {
|
||||
const uint64_t n = coarseEntryCount(P, kk);
|
||||
if (n > 2 * (uint64_t)P.num_obs) continue;
|
||||
k = kk;
|
||||
dim = 7 * ((nf + kk - 1) / kk);
|
||||
entries = n;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// Group observations by image (CSR) for the CG path's per-camera kernels.
|
||||
inline void buildCamTables(BAProblem& P) {
|
||||
P.cam_obs_ranges.assign(P.num_images + 1, 0);
|
||||
|
||||
+106
-13
@@ -288,16 +288,103 @@ state (rho, alpha, beta, tolerance) lives in a small device buffer, dot
|
||||
products are two-stage reductions, and every kernel no-ops once the
|
||||
convergence flag is set, so a fixed iteration cap is recorded (adapted each
|
||||
LM iteration from the previous count) and the LM loop keeps its single
|
||||
cost readback. Stopping rule: relative residual `--cg-tol` (default 0.1,
|
||||
inexact-Newton style -- LM's accept/reject guards the step quality) with a
|
||||
`--cg-iters` cap (default 100).
|
||||
cost readback.
|
||||
|
||||
Stopping rule: relative residual `--cg-tol` (default 0.1, inexact-Newton
|
||||
style -- LM's accept/reject guards the step quality), or, whichever comes
|
||||
first, the last step lowering the quadratic model `Q = x^T S x / 2 - g^T x` by
|
||||
less than `SolverOptions::cg_model_tol` (default 0.1) of the average step so
|
||||
far -- Nash and Sofer's truncated-Newton test, `i (Q_i - Q_i-1) / Q_i`, which
|
||||
is how Ceres stops its CG under LM. `Q` costs nothing to track: a CG step
|
||||
lowers it by `alpha rho / 2`. The residual test alone keeps iterating after
|
||||
the steps have stopped mattering: on the 4102-image capture below the model
|
||||
test takes 40% fewer iterations (2376 against 3954 over a 50-iteration solve,
|
||||
34 s against 55 s) to the same final cost, 1.879209721e6 to all ten digits
|
||||
printed. It settles for a residual near the square root of its tolerance,
|
||||
though, so a caller that wants the exact step (the tests, `SS_SFM_CMP_STEP`)
|
||||
sets it to 0. `--cg-iters` caps both (default 100).
|
||||
|
||||
### Coarse correction (two-level preconditioner)
|
||||
|
||||
Block Jacobi sees one camera at a time, and the slow modes of a long capture
|
||||
are not local: a stretch of the trajectory bending, twisting or drifting in
|
||||
scale *as a whole*, which every camera block finds nearly free. On a
|
||||
4102-image dataset, CG hit its 100-iteration cap from the
|
||||
fourth LM iteration on, and the truncated steps fell behind the dense
|
||||
solver's: 1.879283e6 after 10 LM iterations against its 1.879271e6. Holding
|
||||
the intrinsics fixed changed nothing, so it is not the dropped pose/intrinsics
|
||||
coupling.
|
||||
|
||||
So the preconditioner has a second level: `M^-1 = M_J^-1 + P A_c^-1 P^T` with
|
||||
`A_c = P^T S P`. `P` gives every cluster of `k` consecutive frames 7 dofs, a
|
||||
similarity motion of the world (rotation `w`, translation `tau`, scale `s`),
|
||||
mapped onto each frame's pose parameters exactly: `d(angle-axis) = -Jr^-1 w`,
|
||||
`dt = s t - R tau` (`tc_basis`, in fp32 -- the basis only has to span the slow
|
||||
modes). A constant pose delta per cluster instead was measured and is clearly
|
||||
worse (66 and 100 CG iterations where the similarity basis takes 35 and 50):
|
||||
it cannot move a cluster rigidly unless every frame in it faces the same way.
|
||||
|
||||
`A_c` comes from the same per-observation Jacobians as everything else: the
|
||||
`B` part per image from `cg_cam_diag`'s Gram blocks (`tc_bpart`), the Schur
|
||||
part per pair of *runs* -- a track's observations inside one cluster, which
|
||||
are contiguous since tracks are sorted by image -- grouped by cluster pair on
|
||||
the host so that one warp sums a coarse block and writes it once
|
||||
(`tc_schur`; issuing its sums as atomics per point was 145 ms a build against
|
||||
80). It is factored by the dense kernels in the packed-`S` buffer the CG path
|
||||
otherwise leaves empty, and the factor is then inverted in place (`tc_dinv`,
|
||||
`tc_inv_row`), so applying it is two triangular matrix-vector products
|
||||
(`tc_lmul`, `tc_ltmul`, ~0.17 ms at 3591 dofs) rather than 2 n / 32 dependent
|
||||
solve dispatches (~4 ms, more than half a CG iteration).
|
||||
|
||||
Size and cost: `k` is the smallest cluster that keeps `A_c` under 4096 dofs
|
||||
and the run-pair table under two entries per observation (long tracks span
|
||||
many small clusters) -- 8 frames on the 4102-image capture, 38 on a
|
||||
22042-image one. The extra VRAM is the table and the frame basis; `A_c`
|
||||
itself (67 MB at most) reuses packed `S`. A build is the size of a dense
|
||||
factor, so it happens only when the last CG solve took more than 12
|
||||
iterations, and is reused for up to three solves unless the damping has moved
|
||||
tenfold -- a stale `A_c` only costs iterations.
|
||||
|
||||
One global BA each (`spirula sfm ba`, defaults, RTX 5070), before and after
|
||||
this correction and the stopping rule above:
|
||||
|
||||
| capture | solver | CG its/solve | solve | final cost |
|
||||
|---|---|---|---|---|
|
||||
| 4102-image (well-conditioned) | cg | 89.8 -> 50.4 | 29.1 s -> 31.6 s (30 -> 49 LM its) | 1.879241e6 -> 1.879210e6 |
|
||||
| 6946-image (dual-fisheye rig) | cg | 81.0 -> 41.8 | 97.6 s -> 51.2 s | 4.659347e6 -> 4.659219e6 |
|
||||
| 22042-image ([campus](https://repo-sam.inria.fr/fungraph/hierarchical-3d-gaussians/datasets/)) | cg | 47.5 -> 73.7 | 120.2 s -> 193.4 s (47 -> 50 LM its) | 1.046347e7 -> 1.044832e7 |
|
||||
| 1068-image (linear trajectory) | dense -> cg | -- -> 8.1 | 56.7 s -> 3.6 s | 1.822009e6 both |
|
||||
|
||||
Where a run got longer it is because it no longer stalls: the old 4102-image
|
||||
solve stopped on tie-zone patience, and the campus one sat at 1.0483e7 from
|
||||
iteration 12 to 20 where the new one passes its final cost at iteration 14
|
||||
(about 55 s) and goes on improving.
|
||||
|
||||
On the 4102-image dataset the coarse-corrected CG tracks the dense solver's cost to
|
||||
seven digits iteration for iteration (1.879220953e6 against 1.879220943e6 at
|
||||
iteration 29), which the plain one never does; it then keeps improving where
|
||||
the plain one stops on its tie-zone patience. `SS_SFM_BA_COARSE=0` turns it
|
||||
off; the host solver has the same correction (`coarseBuild`).
|
||||
|
||||
The global similarity is a gauge freedom, so `A_c` is singular up to the
|
||||
damping. Its diagonal is scaled by `1 + 1e-8`, and a pivot under 1e-6 of its
|
||||
row's original diagonal is replaced by that diagonal (`pivot()` in
|
||||
`cholesky.slang`), which keeps the factor bounded however far rounding has
|
||||
made the matrix indefinite. If a CG solve still stops before its first step,
|
||||
the iteration is redone without `A_c`, and it stays off for the solve when
|
||||
that was the difference.
|
||||
|
||||
The CG path allocates no packed S, no Y, and no pair-entry lists, so VRAM
|
||||
stays linear in observations. Preprocessing also skips the pair-table sort (3.6 s -> 1.2 s
|
||||
there). Solver selection and the dense fallback are VRAM-aware: the budget
|
||||
is `--vram-budget` (default 90% of the device-local heap); `auto` picks
|
||||
dense for small problems, CG when the dense estimate exceeds the budget or
|
||||
`n_dim > 8192`. With `--cg-fallback on` (or `auto`, which enables it only
|
||||
dense for small problems, CG when the dense estimate exceeds the budget,
|
||||
`n_dim > 8192`, or CG's estimated iteration costs under half the dense one's.
|
||||
That last test is about track length: the dense assembly is quadratic in it
|
||||
(`schur_obs` walks the track per observation), CG linear. A 1068-image
|
||||
capture of a robot drifting slowly through one module, 70 observations per
|
||||
track by `sum t^2 / sum t`, spent 3.7 s an iteration in `schur_obs` alone and
|
||||
59 s a solve; on CG it is 6.3 s, to the same final cost. With `--cg-fallback on` (or `auto`, which enables it only
|
||||
when dense+CG together fit in half the budget) the dense machinery is kept
|
||||
allocated; a truncated-CG step is kept if it still lowered the cost, and
|
||||
only a cost-raising capped solve is re-solved densely from the reused
|
||||
@@ -310,6 +397,14 @@ inexact steps on its own, at zero extra memory.
|
||||
step difference: with `--cg-tol 1e-8` the CG step matches the dense step to
|
||||
~1e-8 at fp64.
|
||||
|
||||
Two guards keep a badly scaled problem from breaking CG. A 22042-image model
|
||||
with a point 2e-9 from a camera centre has camera Gram entries of 1e23, and
|
||||
below damping ~1e-9 rounding makes `S` indefinite there: the LM damping is
|
||||
floored at 1e-8, which is still Gauss-Newton to eight digits, and a CG solve
|
||||
that stops before its first step counts as a failed step (damping up) rather
|
||||
than a zero one. The block-Jacobi factor floors a pivot under 1e-10 of its
|
||||
block's largest diagonal and decouples it, where it used to cascade to NaN.
|
||||
|
||||
### Host fallback (`--real cpu`)
|
||||
|
||||
`pickRealForDevice` steps down `double` -> `cpu`, and `cpu` is the same solver
|
||||
@@ -484,14 +579,12 @@ peak (the largest single BA being 6372 images / 12.7 M observations).
|
||||
400M-entry cap it falls back to the atomic per-observation Schur kernel),
|
||||
and packed indexing is 32-bit: `n(n+1)/2 < 2^31` (n ≲ 65k camera DOF).
|
||||
Neither limit applies to the CG path.
|
||||
- CG iteration counts are conditioning-dependent: on well-behaved covis
|
||||
graphs (1936) ~9 iters/solve; on the outlier-heavy 871 ~56, which is why
|
||||
auto keeps problems below `n_dim = 8192` on the dense path. A stronger
|
||||
preconditioner (visibility clustering / power series) is the known next
|
||||
step for ill-conditioned sets -- and the more so now that sharing is
|
||||
allowed, since a shared group costs the preconditioner its pose/intrinsics
|
||||
coupling: the 4194-image capture above converges in 10 iterations/solve, but
|
||||
the 6281-image one sits at the 100 cap for its final refinement pass.
|
||||
- CG iteration counts are conditioning-dependent. The coarse correction
|
||||
takes care of the long-wavelength modes of a long capture, but its clusters
|
||||
are consecutive frames, so it assumes image order follows the trajectory
|
||||
(true of video and of most photo walks); a shuffled collection gets a
|
||||
valid but weaker coarse space. At damping ~1e-7 the 4102-image dataset still
|
||||
needs 80-100 iterations per solve.
|
||||
- fp32 CG inherits the documented fp32 normal-equation stall (block-Jacobi
|
||||
is nearly exact at high damping, so it still descends, but final cost is
|
||||
looser than fp64/df -- same as fp32 dense, slightly amplified).
|
||||
|
||||
+227
-28
@@ -11,6 +11,7 @@
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <limits>
|
||||
#include <map>
|
||||
#include <mutex>
|
||||
#include <set>
|
||||
@@ -189,7 +190,8 @@ public:
|
||||
bModelObs_ = mkUint(P_.model_obs.size());
|
||||
bJcOff_ = mkUint(P_.num_obs);
|
||||
bJp_ = mkReal(6 * (uint64_t)P_.num_obs);
|
||||
bS_ = mkReal(needS ? packed : 1);
|
||||
const uint64_t packedTc = (uint64_t)tcN_ * (tcN_ + 1) / 2;
|
||||
bS_ = mkReal(std::max<uint64_t>(needS ? packed : 1, packedTc + 42 * (uint64_t)P_.num_frames));
|
||||
bG_ = mkReal(P_.n_dim);
|
||||
bApp_ = mkReal(9 * (uint64_t)P_.num_points);
|
||||
bBp_ = mkReal(3 * (uint64_t)P_.num_points);
|
||||
@@ -198,8 +200,8 @@ public:
|
||||
bExtsBak_ = mkReal(P_.exts.size());
|
||||
bIntrBak_ = mkReal(P_.total_intr);
|
||||
bPointsBak_ = mkReal(3 * (uint64_t)P_.num_points);
|
||||
bPairEntries_ = mkUint(std::max<size_t>(P_.pair_entries.size(), 2));
|
||||
bPairChunks_ = mkUint(std::max<size_t>(P_.pair_chunks.size(), 2));
|
||||
bPairEntries_ = mkUint(std::max<size_t>(P_.pair_entries.size() + tcEnt_.size(), 2));
|
||||
bPairChunks_ = mkUint(std::max<size_t>(P_.pair_chunks.size() + tcChk_.size(), 2));
|
||||
// S/g are rebuilt from the per-observation Jacobians on every path, so
|
||||
// a rejected step needs no snapshot of them -- only Bp, which the
|
||||
// point back-substitution overwrites in place.
|
||||
@@ -208,7 +210,7 @@ public:
|
||||
bRes_ = mkReal(2 * (uint64_t)P_.num_obs);
|
||||
bW_ = mkReal(9 * (uint64_t)P_.num_points);
|
||||
bYp_ = mkReal(needS && P_.use_pair_schur ? 6 * (uint64_t)P_.num_obs : 1);
|
||||
bY_ = mkReal(P_.n_dim);
|
||||
bY_ = mkReal(std::max<uint64_t>(P_.n_dim, 34 * (uint64_t)tcN_));
|
||||
// CG path buffers (1-element dummies when unused)
|
||||
bCamRanges_ = mkUint(cgAllocated_ ? P_.num_images + 1 : 1);
|
||||
bCamObs_ = mkUint(cgAllocated_ ? P_.num_obs : 1);
|
||||
@@ -220,7 +222,7 @@ public:
|
||||
bCgV_ = mkReal(cgAllocated_ ? 3 * (uint64_t)P_.num_points : 1);
|
||||
bCgB_ = mkReal(cgAllocated_ ? (uint64_t)bBlk_ * P_.num_images : 1);
|
||||
bCgM_ = mkReal(cgAllocated_ ? (uint64_t)kCamBlk * P_.num_prec_blocks : 1);
|
||||
bCgScal_ = mkReal(8);
|
||||
bCgScal_ = mkReal(16);
|
||||
bCgPart_ = mkReal(cgAllocated_ ? 2 * (uint64_t)npart : 1);
|
||||
bPrecBlocks_ = mkUint(cgAllocated_ ? P_.prec_blocks.size() : 4);
|
||||
|
||||
@@ -278,6 +280,13 @@ public:
|
||||
for (const char* k : {"cg_cam_diag", "cg_gather", "cg_bmul", "cg_scatter"})
|
||||
entries.push_back(std::string(k) + cgSuffix_);
|
||||
}
|
||||
if (tcN_) {
|
||||
const char* tc[] = {"tc_basis", "tc_schur", "tc_reg", "tc_dinv", "tc_inv_row",
|
||||
"tc_inv_copy", "tc_restrict", "tc_lmul", "tc_ltmul",
|
||||
"tc_prolong", "chol_diag", "chol_panel", "chol_update"};
|
||||
entries.insert(entries.end(), std::begin(tc), std::end(tc));
|
||||
entries.push_back(std::string("tc_bpart") + cgSuffix_);
|
||||
}
|
||||
if (!useCG_ || haveFallback_) {
|
||||
const char* ch[] = {"chol_diag", "chol_panel", "chol_update", "tri_fwd", "tri_bwd"};
|
||||
entries.insert(entries.end(), std::begin(ch), std::end(ch));
|
||||
@@ -368,10 +377,17 @@ public:
|
||||
up.push_back({&bMemberInfo_, mi.data(), mi.size() * 4});
|
||||
up.push_back({&bExts_, exts.data(), exts.size()});
|
||||
}
|
||||
if (P_.use_pair_schur) {
|
||||
up.push_back({&bPairEntries_, P_.pair_entries.data(), P_.pair_entries.size() * 4});
|
||||
up.push_back({&bPairChunks_, P_.pair_chunks.data(), P_.pair_chunks.size() * 4});
|
||||
if (P_.use_pair_schur || tcN_) {
|
||||
tcEnt_.insert(tcEnt_.begin(), P_.pair_entries.begin(), P_.pair_entries.end());
|
||||
tcChk_.insert(tcChk_.begin(), P_.pair_chunks.begin(), P_.pair_chunks.end());
|
||||
up.push_back({&bPairEntries_, tcEnt_.data(), tcEnt_.size() * 4});
|
||||
up.push_back({&bPairChunks_, tcChk_.data(), tcChk_.size() * 4});
|
||||
}
|
||||
struct Release {
|
||||
std::vector<uint32_t>& a;
|
||||
std::vector<uint32_t>& b;
|
||||
~Release() { a = {}; b = {}; }
|
||||
} release{tcEnt_, tcChk_};
|
||||
if (cgAllocated_) {
|
||||
up.push_back({&bCamRanges_, P_.cam_obs_ranges.data(), P_.cam_obs_ranges.size() * 4});
|
||||
up.push_back({&bCamObs_, P_.cam_obs.data(), P_.cam_obs.size() * 4});
|
||||
@@ -457,6 +473,20 @@ public:
|
||||
reuse ? " (reuse)" : "");
|
||||
|
||||
LinSolve path = useCG_ ? LinSolve::CG : densePath_;
|
||||
// A few CG iterations do not repay building A_c, and a stale one
|
||||
// only costs iterations: it is rebuilt every third solve, or once
|
||||
// the damping has moved tenfold.
|
||||
tcUse_ = tcN_ && !tcOff_ && lastCg_ > kTcMinIters;
|
||||
const bool tcBuild = tcUse_ && (!tcHave_ || tcAge_ >= 2 ||
|
||||
std::fabs(std::log(damping / tcLambda_)) > std::log(10.0));
|
||||
tcBuild_ = tcBuild;
|
||||
if (tcBuild) {
|
||||
tcHave_ = true;
|
||||
tcAge_ = 0;
|
||||
tcLambda_ = damping;
|
||||
} else if (tcUse_) {
|
||||
tcAge_++;
|
||||
}
|
||||
beginSeg();
|
||||
recordIteration((float)damping, reuse, path);
|
||||
endSeg();
|
||||
@@ -467,8 +497,27 @@ public:
|
||||
bool conv;
|
||||
double cg_iters;
|
||||
readCgStatus(conv, cg_iters);
|
||||
// CG stopped before its first step (r.z or p.Sp not positive):
|
||||
// retried without A_c, then a failed step that raises the damping
|
||||
// -- S went indefinite by rounding below ~1e-9 on Gram blocks of 1e23.
|
||||
if (cg_iters == 0 && !conv && tcUse_) {
|
||||
tcUse_ = tcBuild_ = false;
|
||||
restore_pending_ = true;
|
||||
beginSeg();
|
||||
recordIteration((float)damping, true, path);
|
||||
endSeg();
|
||||
newCost = readCost();
|
||||
readCgStatus(conv, cg_iters);
|
||||
if (cg_iters > 0 || conv) {
|
||||
tcOff_ = true;
|
||||
sfm::slog::diag(sfm::slog::Tag::Map,
|
||||
"[vk] coarse correction broke down; continuing without it");
|
||||
}
|
||||
}
|
||||
if (cg_iters == 0 && !conv) newCost = std::numeric_limits<double>::infinity();
|
||||
stats_.cg_solves++;
|
||||
stats_.cg_iters_total += cg_iters;
|
||||
lastCg_ = conv ? cg_iters : 1e9;
|
||||
if (conv) {
|
||||
consec_fallbacks = 0;
|
||||
// adapt the recorded iteration cap to the observed count
|
||||
@@ -495,6 +544,7 @@ public:
|
||||
recordIteration((float)damping, true, densePath_);
|
||||
endSeg();
|
||||
newCost = readCost();
|
||||
tcHave_ = false; // the dense solve reused u_S
|
||||
stats_.cg_fallbacks++;
|
||||
if (++consec_fallbacks >= 3) {
|
||||
useCG_ = false; // CG is not paying off; stay dense
|
||||
@@ -516,7 +566,7 @@ public:
|
||||
if (++noimprov >= opt_.patience) { cost = newCost; break; }
|
||||
} else {
|
||||
noimprov = 0;
|
||||
damping *= 1.0 / 3.0;
|
||||
damping = std::max(damping / 3.0, kMinDamping);
|
||||
}
|
||||
cost = newCost;
|
||||
stats_.accepted++;
|
||||
@@ -638,6 +688,13 @@ private:
|
||||
const uint32_t tier = maxDof <= 18 ? 18 : 24;
|
||||
bBlk_ = tier * (tier + 1) / 2;
|
||||
wide_ = std::max(maxDof, 6u) / 24.0;
|
||||
double t1 = 0, t2 = 0;
|
||||
for (uint32_t p = 0; p < P_.num_points; p++) {
|
||||
const double t = P_.obs_ranges[p + 1] - P_.obs_ranges[p];
|
||||
t1 += t;
|
||||
t2 += t * t;
|
||||
}
|
||||
if (t1 > 0) meanTrackT_ = t2 / t1;
|
||||
}
|
||||
|
||||
static std::string costEntry(const BAProblem::ModelRange& mr) {
|
||||
@@ -668,8 +725,8 @@ private:
|
||||
const bool pairOk = exclusive && P_.num_obs > 0 && pairEntries <= kMaxPairEntries &&
|
||||
densePairMB <= budget;
|
||||
const double denseMB = pairOk ? densePairMB : denseObsMB;
|
||||
const double cgMB = estimateMB(false, true, false, 0);
|
||||
const double bothMB = estimateMB(true, true, pairOk, pairEntries);
|
||||
double cgMB = estimateMB(false, true, false, 0);
|
||||
double bothMB = estimateMB(true, true, pairOk, pairEntries);
|
||||
|
||||
const uint32_t kDenseMaxDim = 8192;
|
||||
|
||||
@@ -690,12 +747,18 @@ private:
|
||||
}
|
||||
break;
|
||||
case SolverSel::Auto:
|
||||
useCG_ = cgOk && (P_.n_dim > kDenseMaxDim || !denseOk || denseMB > budget);
|
||||
useCG_ = cgOk && (P_.n_dim > kDenseMaxDim || !denseOk || denseMB > budget ||
|
||||
cgWork() < 0.5 * denseWork(pairOk, pairEntries));
|
||||
if (!useCG_ && !denseOk)
|
||||
throw std::runtime_error("reduced system too large for the dense solver");
|
||||
break;
|
||||
}
|
||||
|
||||
if (useCG_ && ::planCoarse(P_, kTcMaxDim, tcK_, tcN_, tcEntries_)) {
|
||||
cgMB = estimateMB(false, true, false, 0);
|
||||
bothMB = estimateMB(true, true, pairOk, pairEntries);
|
||||
}
|
||||
|
||||
haveFallback_ = false;
|
||||
if (useCG_) {
|
||||
bool want = opt_.cg_fallback == CgFallback::On ||
|
||||
@@ -732,8 +795,40 @@ private:
|
||||
buildPrecBlocks(P_, exclusive);
|
||||
}
|
||||
cgMaxit_ = (uint32_t)opt_.cg_max_iters;
|
||||
if (tcN_) {
|
||||
// Chunks of at most 128 entries of one cluster pair, their offsets
|
||||
// past the pair-Schur entries they follow on the device.
|
||||
std::vector<uint32_t> key;
|
||||
buildCoarseEntries(P_, tcK_, tcEnt_, key);
|
||||
const uint32_t base = (uint32_t)(P_.pair_entries.size() / 2);
|
||||
tcChk_.clear();
|
||||
for (size_t kk = 0; kk + 1 < key.size(); kk++)
|
||||
for (uint32_t o = key[kk]; o < key[kk + 1]; o += 128) {
|
||||
tcChk_.push_back(base + o);
|
||||
tcChk_.push_back(std::min(128u, key[kk + 1] - o));
|
||||
}
|
||||
tcChunks_ = (uint32_t)(tcChk_.size() / 2);
|
||||
}
|
||||
}
|
||||
|
||||
static constexpr uint32_t kTcMaxDim = 4096;
|
||||
|
||||
// One LM iteration of each path in the budget's units. The dense assembly
|
||||
// is quadratic in track length: a 1068-image capture whose tracks average
|
||||
// 70 observations spent 3.7 s an iteration there, against 0.4 s on CG.
|
||||
double denseWork(bool pair, uint64_t pairEntries) const {
|
||||
const double n = P_.n_dim;
|
||||
const double schur = pair ? kWPairEntry * wide_ * wide_ * (double)pairEntries
|
||||
: kWSchurObs * wide_ * wide_ * meanTrackT_ / kSchurObsT *
|
||||
P_.num_obs;
|
||||
return schur + kWFlop * 2 * n * n * n / 3 + kWJac * wide_ * P_.num_obs;
|
||||
}
|
||||
double cgWork() const {
|
||||
return kWJac * wide_ * P_.num_obs + kWCamDiag * wide_ * wide_ * P_.num_obs +
|
||||
kCgItersGuess * (kWGather + kWScatter) * wide_ * P_.num_obs;
|
||||
}
|
||||
static constexpr double kCgItersGuess = 40;
|
||||
|
||||
// device-buffer footprint of a path combination, in MB (mirrors init())
|
||||
double estimateMB(bool withDense, bool withCG, bool pairTables, uint64_t pairEntries) const {
|
||||
const double rs = (double)realSize(opt_.real);
|
||||
@@ -753,6 +848,11 @@ private:
|
||||
if (pairTables)
|
||||
b += 8.0 * pairEntries * 1.01 + 6 * no * rs; // pair entries + Y
|
||||
}
|
||||
if (withCG && tcN_) {
|
||||
const double tn = tcN_, tc = tn * (tn + 1) / 2 + 42.0 * P_.num_frames;
|
||||
b += (std::max(withDense ? packed : 0.0, tc) - (withDense ? packed : 0.0)) * rs;
|
||||
b += std::max(0.0, 34.0 * tn - n) * rs + 8.0 * tcEntries_ * 1.01;
|
||||
}
|
||||
if (withCG)
|
||||
b += (4 * n + 3 * np + ni * (double)bBlk_ +
|
||||
(ni + (double)P_.members.size() + (double)P_.groups.size()) * kCamBlk) * rs +
|
||||
@@ -769,7 +869,10 @@ private:
|
||||
static constexpr double kWLaunch = 2000, kWPoint = 0.25, kWVec = 1, kWImage = 5;
|
||||
static constexpr double kWCost = 1.5, kWJac = 5.5, kWDp = 0.6, kWYPrep = 0.3;
|
||||
static constexpr double kWCamDiag = 12.7, kWGather = 0.65, kWScatter = 1.2;
|
||||
static constexpr double kWSchurObs = 40, kWPairEntry = 3, kWFlop = 2.6e-3;
|
||||
static constexpr double kWSchurObs = 40, kWPairEntry = 3, kWFlop = 2.6e-3, kWTcSchur = 6;
|
||||
// schur_obs walks the track per observation: kWSchurObs is at the 5.8
|
||||
// observations of that capture's sum t^2 / sum t.
|
||||
static constexpr double kSchurObsT = 5.8;
|
||||
// Until a submit has been timed, a device is taken to be 64x slower.
|
||||
static constexpr double kPriorRate = 1e9 / 64;
|
||||
|
||||
@@ -929,22 +1032,9 @@ private:
|
||||
void recordCholesky() {
|
||||
const uint32_t n = P_.n_dim, bs = 32;
|
||||
const uint32_t nb = (n + bs - 1) / bs;
|
||||
const double tile = 2.0 * bs * bs * bs * kWFlop;
|
||||
recordFactor(n);
|
||||
Push p;
|
||||
p.u0 = n;
|
||||
p.u1 = 0;
|
||||
room(kWLaunch + tile);
|
||||
ctx_.dispatch(cb_, "chol_diag", 1, p);
|
||||
ctx_.barrier(cb_);
|
||||
for (uint32_t k = 0; k + 1 < nb; k++) {
|
||||
p.u1 = k;
|
||||
uint32_t below = nb - 1 - k;
|
||||
launch("chol_panel", below, 1, tile, p, atBase);
|
||||
ctx_.barrier(cb_);
|
||||
p.u2 = below * (below + 1) / 2;
|
||||
launch("chol_update", p.u2, 1, tile, p, atBase);
|
||||
ctx_.barrier(cb_);
|
||||
}
|
||||
for (uint32_t k = 0; k < nb; k++) {
|
||||
p.u1 = k;
|
||||
p.u2 = (k + 1) * bs < n ? n - (k + 1) * bs : 0;
|
||||
@@ -961,6 +1051,95 @@ private:
|
||||
}
|
||||
}
|
||||
|
||||
// Factor the leading n x n packed triangle of u_S in place; `rel` > 0
|
||||
// replaces a pivot under that fraction of the diagonal tc_reg saved with it.
|
||||
void recordFactor(uint32_t n, float rel = 0) {
|
||||
const uint32_t bs = 32;
|
||||
const uint32_t nb = (n + bs - 1) / bs;
|
||||
const double tile = 2.0 * bs * bs * bs * kWFlop;
|
||||
Push p;
|
||||
p.u0 = n;
|
||||
p.u1 = 0;
|
||||
p.u3 = rel > 0 ? 1 : 0;
|
||||
p.f0 = rel;
|
||||
room(kWLaunch + tile);
|
||||
ctx_.dispatch(cb_, "chol_diag", 1, p);
|
||||
ctx_.barrier(cb_);
|
||||
for (uint32_t k = 0; k + 1 < nb; k++) {
|
||||
p.u1 = k;
|
||||
uint32_t below = nb - 1 - k;
|
||||
launch("chol_panel", below, 1, tile, p, atBase);
|
||||
ctx_.barrier(cb_);
|
||||
p.u2 = below * (below + 1) / 2;
|
||||
launch("chol_update", p.u2, 1, tile, p, atBase);
|
||||
ctx_.barrier(cb_);
|
||||
}
|
||||
}
|
||||
|
||||
// A_c = P^T S P from this iteration's B and W; its Cholesky factor is then
|
||||
// inverted in place, so an application is two matrix-vector products.
|
||||
void recordCoarse() {
|
||||
const uint32_t packedTc = tcN_ * (tcN_ + 1) / 2;
|
||||
Push p;
|
||||
p.u0 = P_.num_frames;
|
||||
p.u2 = tcK_;
|
||||
p.u3 = packedTc;
|
||||
room(2 * kWLaunch + (P_.num_frames + P_.num_images) * kWImage);
|
||||
ctx_.dispatch(cb_, "tc_basis", (P_.num_frames + 63) / 64, p);
|
||||
ctx_.fillZero(cb_, bS_, 0, (VkDeviceSize)packedTc * realSize(opt_.real));
|
||||
ctx_.barrier(cb_);
|
||||
p.u0 = P_.num_images;
|
||||
ctx_.dispatch(cb_, std::string("tc_bpart") + cgSuffix_, (P_.num_images + 63) / 64, p);
|
||||
p.u0 = tcChunks_;
|
||||
p.u1 = P_.num_pair_chunks;
|
||||
launch("tc_schur", tcChunks_, 1,
|
||||
kWTcSchur * (double)tcEntries_ / std::max(1u, tcChunks_), p, atBase);
|
||||
ctx_.barrier(cb_);
|
||||
Push q;
|
||||
q.u0 = tcN_;
|
||||
q.f0 = opt_.real == RealCfg::F32 ? 1e-4f : 1e-8f;
|
||||
room(kWLaunch + tcN_ * kWVec);
|
||||
ctx_.dispatch(cb_, "tc_reg", (tcN_ + 255) / 256, q);
|
||||
ctx_.barrier(cb_);
|
||||
recordFactor(tcN_, opt_.real == RealCfg::F32 ? 1e-3f : 1e-6f);
|
||||
|
||||
const uint32_t bs = 32, nb = (tcN_ + bs - 1) / bs;
|
||||
const double tile = 2.0 * bs * bs * bs * kWFlop;
|
||||
Push d;
|
||||
d.u0 = tcN_;
|
||||
room(kWLaunch + nb * tile);
|
||||
ctx_.dispatch(cb_, "tc_dinv", nb, d);
|
||||
ctx_.barrier(cb_);
|
||||
d.u2 = 2 * tcN_; // scratch rows follow the two vectors in u_y
|
||||
for (uint32_t i = 1; i < nb; i++) {
|
||||
d.u1 = i;
|
||||
room(2 * kWLaunch + i * (i + 1) / 2.0 * tile);
|
||||
ctx_.dispatch(cb_, "tc_inv_row", i, d);
|
||||
ctx_.barrier(cb_);
|
||||
ctx_.dispatch(cb_, "tc_inv_copy", (bs * bs * i + 255) / 256, d);
|
||||
ctx_.barrier(cb_);
|
||||
}
|
||||
}
|
||||
|
||||
// z += P A_c^-1 P^T r, after the block-Jacobi part of the preconditioner.
|
||||
void recordCoarseApply() {
|
||||
Push p;
|
||||
p.u0 = tcN_;
|
||||
p.u1 = P_.num_frames;
|
||||
p.u2 = tcK_;
|
||||
p.u3 = tcN_ * (tcN_ + 1) / 2;
|
||||
room(4 * kWLaunch + (6.0 * P_.num_frames + (double)tcN_ * tcN_) * kWVec);
|
||||
ctx_.dispatch(cb_, "tc_restrict", (tcN_ + 255) / 256, p);
|
||||
ctx_.barrier(cb_);
|
||||
ctx_.dispatch(cb_, "tc_lmul", (tcN_ + 7) / 8, p);
|
||||
ctx_.barrier(cb_);
|
||||
ctx_.dispatch(cb_, "tc_ltmul", (tcN_ + 31) / 32, p);
|
||||
ctx_.barrier(cb_);
|
||||
p.u0 = P_.pose_dim;
|
||||
ctx_.dispatch(cb_, "tc_prolong", (P_.pose_dim + 255) / 256, p);
|
||||
ctx_.barrier(cb_);
|
||||
}
|
||||
|
||||
void recordCost() {
|
||||
ctx_.fillZero(cb_, bCost_);
|
||||
ctx_.barrier(cb_);
|
||||
@@ -1041,7 +1220,7 @@ private:
|
||||
} else if (path == LinSolve::DenseObs) {
|
||||
p.u0 = P_.num_obs;
|
||||
launch(std::string("schur_obs") + schurSuffix_, P_.num_obs, 128,
|
||||
kWSchurObs * wide_ * wide_, p, atBase);
|
||||
kWSchurObs * wide_ * wide_ * meanTrackT_ / kSchurObsT, p, atBase);
|
||||
} else {
|
||||
p.u0 = P_.num_cam_chunks;
|
||||
p.u1 = P_.prec_exclusive ? 1 : 0;
|
||||
@@ -1054,6 +1233,7 @@ private:
|
||||
p.u0 = P_.num_prec_blocks;
|
||||
room(kWLaunch + P_.num_prec_blocks * kWImage);
|
||||
ctx_.dispatch(cb_, "cg_prec_fact", (P_.num_prec_blocks + 255) / 256, p);
|
||||
if (tcBuild_) recordCoarse();
|
||||
}
|
||||
}
|
||||
ctx_.barrier(cb_);
|
||||
@@ -1086,6 +1266,7 @@ private:
|
||||
pc.u1 = 0; // flag was just cleared
|
||||
ctx_.dispatch(cb_, "cg_prec_apply", nib, pc);
|
||||
ctx_.barrier(cb_);
|
||||
if (tcUse_) recordCoarseApply();
|
||||
Push pr;
|
||||
pr.u0 = n;
|
||||
pr.u1 = 1;
|
||||
@@ -1098,6 +1279,7 @@ private:
|
||||
pf.u1 = 0;
|
||||
pf.u2 = npart;
|
||||
pf.f0 = (float)opt_.cg_tol;
|
||||
pf.f1 = (float)opt_.cg_model_tol;
|
||||
ctx_.dispatch(cb_, "cg_fin", 1, pf);
|
||||
ctx_.barrier(cb_);
|
||||
ctx_.dispatch(cb_, "cg_copy", ng, pn);
|
||||
@@ -1138,6 +1320,7 @@ private:
|
||||
ctx_.barrier(cb_);
|
||||
ctx_.dispatch(cb_, "cg_prec_apply", nib, pc);
|
||||
ctx_.barrier(cb_);
|
||||
if (tcUse_) recordCoarseApply();
|
||||
pr.u1 = 1;
|
||||
ctx_.dispatch(cb_, "cg_red2", ng, pr);
|
||||
ctx_.barrier(cb_);
|
||||
@@ -1266,6 +1449,7 @@ private:
|
||||
const char* cgSuffix_ = "_w"; // ... and of the CG ones
|
||||
uint32_t bBlk_ = kCamBlk; // per-image B block stride at that tier
|
||||
double wide_ = 1; // widest camera block over the rig tier's 24
|
||||
double meanTrackT_ = kSchurObsT; // sum t^2 / sum t over the tracks
|
||||
SolverStats stats_;
|
||||
std::unique_ptr<VkContext> owned_; // null when running on a shared context
|
||||
VkContext& ctx_;
|
||||
@@ -1276,6 +1460,21 @@ private:
|
||||
bool cgAllocated_ = false; // CG buffers/tables exist
|
||||
bool haveFallback_ = false;
|
||||
uint32_t cgMaxit_ = 100;
|
||||
// Coarse correction: frames per cluster and the coarse dimension (0: off),
|
||||
// and whether this iteration's solve uses it.
|
||||
uint32_t tcK_ = 0, tcN_ = 0;
|
||||
uint64_t tcEntries_ = 0;
|
||||
uint32_t tcChunks_ = 0;
|
||||
std::vector<uint32_t> tcEnt_, tcChk_; // follow the pair-Schur tables on the device
|
||||
bool tcUse_ = false, tcBuild_ = false, tcHave_ = false, tcOff_ = false;
|
||||
int tcAge_ = 0;
|
||||
double tcLambda_ = 0;
|
||||
double lastCg_ = 0; // iterations the last CG solve took
|
||||
static constexpr double kTcMinIters = 12;
|
||||
// Below ~1e-9 rounding outweighs the damping on a badly scaled camera (Gram
|
||||
// entries of 1e23, from a point at depth 2e-9 in a 22042-image model) and S
|
||||
// goes indefinite; 1e-8 is still Gauss-Newton to eight digits.
|
||||
static constexpr double kMinDamping = 1e-8;
|
||||
|
||||
GpuBuffer bObs_, bObsImage_, bObsPoint_, bImageInfo_, bGroupInfo_, bMemberInfo_;
|
||||
GpuBuffer bPoses_, bExts_, bIntr_, bPoints_, bObsRanges_, bModelObs_, bJcOff_;
|
||||
|
||||
+256
-7
@@ -12,6 +12,7 @@
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <limits>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
@@ -109,12 +110,39 @@ public:
|
||||
damping,
|
||||
reuse ? " (reuse)" : "");
|
||||
const bool cg = useCG_;
|
||||
// as sfm/ba/Solver.h: A_c when CG is slow, rebuilt every third
|
||||
// solve or once the damping has moved tenfold
|
||||
tcUse_ = cg && tcN_ && !tcOff_ && lastCg_ > kTcMinIters;
|
||||
tcBuild_ = tcUse_ && (!tcHave_ || tcAge_ >= 2 ||
|
||||
std::fabs(std::log(damping / tcLambda_)) > std::log(10.0));
|
||||
if (tcBuild_) {
|
||||
tcHave_ = true;
|
||||
tcAge_ = 0;
|
||||
tcLambda_ = damping;
|
||||
} else if (tcUse_) {
|
||||
tcAge_++;
|
||||
}
|
||||
double newCost = iterate(damping, reuse, cg);
|
||||
// as sfm/ba/Solver.h: a CG that stopped before its first step is
|
||||
// retried without the coarse correction, then taken as a failed step
|
||||
if (cg && cgIters_ == 0 && !cgConverged_ && tcUse_) {
|
||||
tcUse_ = tcBuild_ = false;
|
||||
restore();
|
||||
newCost = iterate(damping, true, cg);
|
||||
if (cgIters_ > 0 || cgConverged_) {
|
||||
tcOff_ = true;
|
||||
sfm::slog::diag(sfm::slog::Tag::Map,
|
||||
"[cpu] coarse correction broke down; continuing without it");
|
||||
}
|
||||
}
|
||||
if (cg && cgIters_ == 0 && !cgConverged_)
|
||||
newCost = std::numeric_limits<double>::infinity();
|
||||
stats_.iterations = it + 1;
|
||||
|
||||
if (cg) {
|
||||
stats_.cg_solves++;
|
||||
stats_.cg_iters_total += cgIters_;
|
||||
lastCg_ = cgConverged_ ? cgIters_ : 1e9;
|
||||
if (cgConverged_) {
|
||||
consec_fallbacks = 0;
|
||||
cgMaxit_ = std::min<uint32_t>(
|
||||
@@ -150,7 +178,7 @@ public:
|
||||
if (++noimprov >= opt_.patience) { cost = newCost; break; }
|
||||
} else {
|
||||
noimprov = 0;
|
||||
damping *= 1.0 / 3.0;
|
||||
damping = std::max(damping / 3.0, 1e-8); // kMinDamping in sfm/ba/Solver.h
|
||||
}
|
||||
cost = newCost;
|
||||
stats_.accepted++;
|
||||
@@ -291,6 +319,9 @@ private:
|
||||
b += (double)DenseSpd::elems(n_) * 8 + 2.0 * n * DenseSpd::kBlock * 8;
|
||||
b += (double)nthreads_ * m_ * n * 8;
|
||||
}
|
||||
if (withCG && tcN_)
|
||||
b += ((double)DenseSpd::elems(tcN_) + 2.0 * tcN_ * DenseSpd::kBlock +
|
||||
42.0 * P_.num_frames + tcN_) * 8 + 8.0 * tcEntries_;
|
||||
if (withCG) {
|
||||
const double nblk = exclusive_ ? ni : (double)P_.num_frames + nSharedBlk_;
|
||||
b += (4 * n + 3 * np) * 8 + (double)kCamBlk * (ni + nblk) * 8 +
|
||||
@@ -300,13 +331,34 @@ private:
|
||||
return b / (1024.0 * 1024.0);
|
||||
}
|
||||
|
||||
// One LM iteration of each path in seconds on 8 cores, fitted where dense
|
||||
// assembly's quadratic cost in track length shows: 7.4 s dense against 1.5 s
|
||||
// CG, same cost, on 1068 images averaging 70 a track (GPU's: Solver.h).
|
||||
double denseSeconds() const {
|
||||
double t1 = 0, t2 = 0;
|
||||
for (uint32_t p = 0; p < nPts_; p++) {
|
||||
const double t = P_.obs_ranges[p + 1] - P_.obs_ranges[p];
|
||||
t1 += t;
|
||||
t2 += t * t;
|
||||
}
|
||||
double dof2 = 0;
|
||||
for (uint32_t i = 0; i < nImg_; i++) dof2 = std::max(dof2, (double)dof_[i] * dof_[i]);
|
||||
const double n = n_;
|
||||
return 9e-11 * t2 * dof2 + n * n * n / 3e11;
|
||||
}
|
||||
double cgSeconds() const {
|
||||
double dof = 0;
|
||||
for (uint32_t i = 0; i < nImg_; i++) dof = std::max(dof, (double)dof_[i]);
|
||||
return (7e-9 + 20 * 1.4e-9) * nObs_ * dof;
|
||||
}
|
||||
|
||||
void decidePaths() {
|
||||
const double budget =
|
||||
opt_.vram_budget_mb > 0 ? opt_.vram_budget_mb : defaultBudgetMB();
|
||||
const bool cgOk = nObs_ > 0;
|
||||
const double denseMB = estimateMB(true, false);
|
||||
const double cgMB = estimateMB(false, true);
|
||||
const double bothMB = estimateMB(true, true);
|
||||
double cgMB = estimateMB(false, true);
|
||||
double bothMB = estimateMB(true, true);
|
||||
|
||||
switch (opt_.solver) {
|
||||
case SolverSel::Dense: useCG_ = false; break;
|
||||
@@ -316,9 +368,14 @@ private:
|
||||
"[cpu] warning: no observations, falling back to dense");
|
||||
break;
|
||||
case SolverSel::Auto:
|
||||
useCG_ = cgOk && (n_ > kDenseMaxDim || denseMB > budget);
|
||||
useCG_ = cgOk && (n_ > kDenseMaxDim || denseMB > budget ||
|
||||
cgSeconds() < 0.5 * denseSeconds());
|
||||
break;
|
||||
}
|
||||
if (useCG_ && planCoarse(P_, kTcMaxDim, tcK_, tcN_, tcEntries_)) {
|
||||
cgMB = estimateMB(false, true);
|
||||
bothMB = estimateMB(true, true);
|
||||
}
|
||||
haveFallback_ = false;
|
||||
if (useCG_)
|
||||
haveFallback_ = opt_.cg_fallback == CgFallback::On ||
|
||||
@@ -340,6 +397,21 @@ private:
|
||||
buildCamTables(P_); // the dense path walks the same per-image lists
|
||||
if (useCG_) buildPrecBlocks(P_, exclusive_);
|
||||
cgMaxit_ = (uint32_t)opt_.cg_max_iters;
|
||||
if (tcN_) {
|
||||
buildCoarseEntries(P_, tcK_, tcEnt_, tcKey_);
|
||||
const uint32_t nc = tcN_ / 7;
|
||||
std::vector<uint64_t> w(nc + 1, 0);
|
||||
for (uint32_t c = 0; c < nc; c++)
|
||||
w[c + 1] = w[c] + tcK_ + tcKey_[(uint64_t)(c + 1) * (c + 2) / 2] -
|
||||
tcKey_[(uint64_t)c * (c + 1) / 2];
|
||||
splitByWeight(w, taskCount((int64_t)w[nc], 1 << 12, nthreads_), tcSplit_);
|
||||
frameImg_.assign(P_.num_frames + 1, nImg_);
|
||||
for (uint32_t i = nImg_; i-- > 0;) frameImg_[frame_[i]] = i;
|
||||
tcA_.init(tcN_);
|
||||
tcP_.assign(42 * (size_t)P_.num_frames, 0.0);
|
||||
tcY_.assign(tcN_, 0.0);
|
||||
tcDiag_.assign(tcN_, 0.0);
|
||||
}
|
||||
}
|
||||
|
||||
void allocate() {
|
||||
@@ -410,6 +482,8 @@ private:
|
||||
cgM_.capacity() + cgGIntr_.capacity() + cgSpIntr_.capacity() +
|
||||
cgGrp_.capacity()) *
|
||||
8;
|
||||
b += tcA_.bytes() + (tcP_.capacity() + tcY_.capacity()) * 8 +
|
||||
(tcEnt_.capacity() + tcKey_.capacity()) * 4;
|
||||
b += S_.bytes() + (P_.cam_obs.capacity() + P_.cam_obs_ranges.capacity() +
|
||||
P_.cam_chunks.capacity() + P_.prec_blocks.capacity()) * 4;
|
||||
return (double)b / (1024.0 * 1024.0);
|
||||
@@ -470,6 +544,7 @@ private:
|
||||
if (cg) {
|
||||
cgCamDiag(damping);
|
||||
cgPrecFactor();
|
||||
if (tcBuild_) coarseBuild();
|
||||
prof_.schur += lap();
|
||||
cgIters_ = runPCG(cgMaxit_);
|
||||
} else {
|
||||
@@ -857,10 +932,16 @@ private:
|
||||
const uint32_t dof = P_.prec_blocks[4 * b + 1] + P_.prec_blocks[4 * b + 3];
|
||||
if (!dof) continue;
|
||||
double* L = &cgM_[(size_t)kCamBlk * b];
|
||||
// as cg_prec_fact: a failed pivot is floored and decoupled
|
||||
double f = 1e-30;
|
||||
for (uint32_t j = 0; j < dof; j++)
|
||||
if (L[pidx(j, j)] > f) f = L[pidx(j, j)];
|
||||
f = std::max(1e-10 * f, 1e-30);
|
||||
for (uint32_t j = 0; j < dof; j++) {
|
||||
const double d = std::sqrt(std::max(L[pidx(j, j)], 1e-30));
|
||||
const bool ok = L[pidx(j, j)] > f;
|
||||
const double d = std::sqrt(ok ? L[pidx(j, j)] : f);
|
||||
L[pidx(j, j)] = d;
|
||||
for (uint32_t i = j + 1; i < dof; i++) L[pidx(i, j)] /= d;
|
||||
for (uint32_t i = j + 1; i < dof; i++) L[pidx(i, j)] = ok ? L[pidx(i, j)] / d : 0.0;
|
||||
for (uint32_t c = j + 1; c < dof; c++)
|
||||
for (uint32_t i = c; i < dof; i++)
|
||||
L[pidx(i, c)] -= L[pidx(i, j)] * L[pidx(c, j)];
|
||||
@@ -903,6 +984,154 @@ private:
|
||||
for (uint32_t i = 0; i < dof; i++) z[cols[i]] = y[i];
|
||||
}
|
||||
});
|
||||
if (tcUse_) coarseApply(r, z);
|
||||
}
|
||||
|
||||
// ================
|
||||
// coarse correction (README.md, "Coarse correction")
|
||||
// ================
|
||||
|
||||
// P_f: pose deltas of a similarity motion (w, tau, s) of the world,
|
||||
// d(angle-axis) = -Jr^-1 w and dt = s t - R tau; 6x7 row-major per frame.
|
||||
void coarseBasis() {
|
||||
const uint32_t nf = P_.num_frames;
|
||||
const int nt = taskCount(nf, 1024, nthreads_);
|
||||
pool_->run(nt, nthreads_, [&](int task, int) {
|
||||
int64_t lo, hi;
|
||||
taskRange(nf, nt, task, lo, hi);
|
||||
for (int64_t f = lo; f < hi; f++) {
|
||||
const double* q = &P_.poses[6 * (size_t)f];
|
||||
double* B = &tcP_[42 * (size_t)f];
|
||||
std::fill(B, B + 42, 0.0);
|
||||
const double th2 = q[0] * q[0] + q[1] * q[1] + q[2] * q[2], th = std::sqrt(th2);
|
||||
const double K[9] = {0, -q[2], q[1], q[2], 0, -q[0], -q[1], q[0], 0};
|
||||
double K2[9];
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
K2[3 * i + j] = K[3 * i] * K[j] + K[3 * i + 1] * K[3 + j] + K[3 * i + 2] * K[6 + j];
|
||||
const bool nearZero = th < 1e-3;
|
||||
const double s1 = nearZero ? 1.0 : std::sin(th) / th;
|
||||
const double c1 = nearZero ? 0.5 : (1 - std::cos(th)) / th2;
|
||||
const double c2 =
|
||||
nearZero ? 1.0 / 12 : 1 / th2 - (1 + std::cos(th)) / (2 * th * std::sin(th));
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
B[7 * i + j] = -((i == j) + 0.5 * K[3 * i + j] + c2 * K2[3 * i + j]);
|
||||
B[7 * (3 + i) + 3 + j] = -((i == j) + s1 * K[3 * i + j] + c1 * K2[3 * i + j]);
|
||||
}
|
||||
B[7 * (3 + i) + 6] = q[3 + i];
|
||||
}
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
// G = sum over the run starting at observation o of P_f^T Jc_pose^T Jp.
|
||||
void coarseRunG(uint32_t o, uint32_t end, double* G) const {
|
||||
std::fill(G, G + 21, 0.0);
|
||||
const uint32_t c = frame_[P_.obs_image[o]] / tcK_;
|
||||
for (; o < end && frame_[P_.obs_image[o]] / tcK_ == c; o++) {
|
||||
const uint32_t img = P_.obs_image[o], dof = dof_[img];
|
||||
const double* Pf = &tcP_[42 * (size_t)frame_[img]];
|
||||
const double* Jc = &Jc_[P_.jc_off[o]];
|
||||
const double* Jp = &Jp_[6 * (size_t)o];
|
||||
for (int i = 0; i < 7; i++) {
|
||||
double q0 = 0, q1 = 0;
|
||||
for (int a = 0; a < 6; a++) {
|
||||
q0 += Pf[7 * a + i] * Jc[a];
|
||||
q1 += Pf[7 * a + i] * Jc[dof + a];
|
||||
}
|
||||
for (int j = 0; j < 3; j++) G[3 * i + j] += q0 * Jp[j] + q1 * Jp[3 + j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// A_c = P^T S P, factored. A task owns a range of block rows, so every
|
||||
// write is its own: the B part from its frames' images, the Schur part
|
||||
// from the run pairs keyed to its rows.
|
||||
void coarseBuild() {
|
||||
coarseBasis();
|
||||
tcA_.zero(*pool_, nthreads_);
|
||||
const int nt = (int)tcSplit_.size() - 1;
|
||||
pool_->run(nt, nthreads_, [&](int task, int) {
|
||||
for (uint32_t c = tcSplit_[task]; c < tcSplit_[task + 1]; c++) {
|
||||
const uint32_t f1 = std::min(P_.num_frames, (c + 1) * tcK_);
|
||||
for (uint32_t img = frameImg_[c * tcK_]; img < frameImg_[f1]; img++) {
|
||||
const double* Bb = &cgB_[(size_t)kCamBlk * img];
|
||||
const double* Pf = &tcP_[42 * (size_t)frame_[img]];
|
||||
double BP[42];
|
||||
for (int r = 0; r < 6; r++)
|
||||
for (int j = 0; j < 7; j++) {
|
||||
double v = 0;
|
||||
for (int l = 0; l < 6; l++)
|
||||
v += Bb[r >= l ? pidx(r, l) : pidx(l, r)] * Pf[7 * l + j];
|
||||
BP[7 * r + j] = v;
|
||||
}
|
||||
for (uint32_t i = 0; i < 7; i++) {
|
||||
double* row = tcA_.row(7 * c + i) + 7 * c;
|
||||
for (uint32_t j = 0; j <= i; j++) {
|
||||
double v = 0;
|
||||
for (int l = 0; l < 6; l++) v += Pf[7 * l + i] * BP[7 * l + j];
|
||||
if (std::isfinite(v)) row[j] += v;
|
||||
}
|
||||
}
|
||||
}
|
||||
double G[21], GW[21];
|
||||
for (uint32_t cv = 0; cv <= c; cv++) {
|
||||
const uint64_t key = (uint64_t)c * (c + 1) / 2 + cv;
|
||||
for (uint32_t e = tcKey_[key]; e < tcKey_[key + 1]; e++) {
|
||||
const uint32_t ou = tcEnt_[2 * (size_t)e], ov = tcEnt_[2 * (size_t)e + 1];
|
||||
const uint32_t p = P_.obs_point[ou], end = P_.obs_ranges[p + 1];
|
||||
const double* W = &W_[9 * (size_t)p];
|
||||
coarseRunG(ou, end, G);
|
||||
for (int i = 0; i < 7; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
GW[3 * i + j] = G[3 * i] * W[j] + G[3 * i + 1] * W[3 + j] +
|
||||
G[3 * i + 2] * W[6 + j];
|
||||
if (ov != ou) coarseRunG(ov, end, G);
|
||||
for (uint32_t i = 0; i < 7; i++) {
|
||||
double* row = tcA_.row(7 * c + i) + 7 * cv;
|
||||
for (uint32_t j = 0; j < 7 && 7 * cv + j <= 7 * c + i; j++) {
|
||||
const double v = GW[3 * i] * G[3 * j] + GW[3 * i + 1] * G[3 * j + 1] +
|
||||
GW[3 * i + 2] * G[3 * j + 2];
|
||||
if (std::isfinite(v)) row[j] -= v;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// the global similarity is a gauge freedom: A_c is singular up
|
||||
// to the damping
|
||||
for (uint32_t i = 0; i < 7; i++) {
|
||||
double& d = tcA_.row(7 * c + i)[7 * c + i];
|
||||
tcDiag_[7 * c + i] = d;
|
||||
d *= 1 + 1e-8;
|
||||
}
|
||||
}
|
||||
});
|
||||
tcA_.factor(*pool_, nthreads_, tcDiag_.data(), 1e-6);
|
||||
}
|
||||
|
||||
void coarseApply(const std::vector<double>& r, std::vector<double>& z) {
|
||||
const uint32_t nf = P_.num_frames;
|
||||
std::fill(tcY_.begin(), tcY_.end(), 0.0);
|
||||
for (uint32_t f = 0; f < nf; f++) {
|
||||
const double* Pf = &tcP_[42 * (size_t)f];
|
||||
double* y = &tcY_[7 * (f / tcK_)];
|
||||
for (int j = 0; j < 7; j++) {
|
||||
double v = 0;
|
||||
for (int a = 0; a < 6; a++) v += Pf[7 * a + j] * r[6 * (size_t)f + a];
|
||||
y[j] += v;
|
||||
}
|
||||
}
|
||||
tcA_.solve(tcY_.data(), *pool_, nthreads_);
|
||||
for (uint32_t f = 0; f < nf; f++) {
|
||||
const double* Pf = &tcP_[42 * (size_t)f];
|
||||
const double* y = &tcY_[7 * (f / tcK_)];
|
||||
for (int a = 0; a < 6; a++) {
|
||||
double v = 0;
|
||||
for (int j = 0; j < 7; j++) v += Pf[7 * a + j] * y[j];
|
||||
z[6 * (size_t)f + a] += v;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void cgGather() {
|
||||
@@ -1016,6 +1245,8 @@ private:
|
||||
bool conv = !(std::isfinite(rho) && rho > 0.0);
|
||||
uint32_t iters = 0;
|
||||
cgP_ = cgZ_;
|
||||
double Q = 0; // the quadratic model at g, for cg_fin's Nash-Sofer test
|
||||
bool qstop = false;
|
||||
while (!conv && iters < maxit) {
|
||||
cgGather();
|
||||
cgMatvec();
|
||||
@@ -1025,6 +1256,8 @@ private:
|
||||
break;
|
||||
}
|
||||
const double alpha = rho / pAp;
|
||||
const double dq = -0.5 * rho * alpha;
|
||||
Q += dq;
|
||||
const int nt = taskCount(n_, 1 << 14, nthreads_);
|
||||
pool_->run(nt, nthreads_, [&](int t, int) {
|
||||
int64_t lo, hi;
|
||||
@@ -1044,13 +1277,17 @@ private:
|
||||
conv = true;
|
||||
break;
|
||||
}
|
||||
if (opt_.cg_model_tol > 0 && Q < 0 && iters * dq / Q < opt_.cg_model_tol) {
|
||||
conv = qstop = true;
|
||||
break;
|
||||
}
|
||||
pool_->run(nt, nthreads_, [&](int t, int) {
|
||||
int64_t lo, hi;
|
||||
taskRange(n_, nt, t, lo, hi);
|
||||
for (int64_t i = lo; i < hi; i++) cgP_[i] = cgZ_[i] + beta * cgP_[i];
|
||||
});
|
||||
}
|
||||
cgConverged_ = conv && rr <= tol2;
|
||||
cgConverged_ = conv && (rr <= tol2 || qstop);
|
||||
return iters;
|
||||
}
|
||||
|
||||
@@ -1139,6 +1376,18 @@ private:
|
||||
std::vector<double> poses0_, exts0_, intr0_, points0_;
|
||||
std::vector<double> sbuf_, sgbuf_, part_;
|
||||
std::vector<double> cgR_, cgZ_, cgP_, cgSp_, cgV_, cgB_, cgM_, cgGIntr_, cgSpIntr_, cgGrp_;
|
||||
// Coarse correction: frames per cluster, dimension (0: off), the run-pair
|
||||
// entries by cluster pair, block rows per task, and each frame's first image.
|
||||
uint32_t tcK_ = 0, tcN_ = 0;
|
||||
uint64_t tcEntries_ = 0;
|
||||
std::vector<uint32_t> tcEnt_, tcKey_, tcSplit_, frameImg_;
|
||||
std::vector<double> tcP_, tcY_, tcDiag_;
|
||||
DenseSpd tcA_;
|
||||
bool tcUse_ = false, tcBuild_ = false, tcHave_ = false, tcOff_ = false;
|
||||
int tcAge_ = 0;
|
||||
double tcLambda_ = 0, lastCg_ = 0;
|
||||
static constexpr uint32_t kTcMaxDim = 4096;
|
||||
static constexpr double kTcMinIters = 12;
|
||||
std::vector<uint32_t> asmSplit_, cgSplit_;
|
||||
DenseSpd S_;
|
||||
struct { double jac = 0, prep = 0, schur = 0, lin = 0, back = 0, cost = 0; } prof_;
|
||||
|
||||
@@ -89,11 +89,10 @@ inline Mat3 inverse3(const Mat3& A, bool* ok = nullptr) {
|
||||
|
||||
// Cyclic Jacobi eigen-decomposition of a symmetric n x n matrix (row-major in
|
||||
// `A`, destroyed). Eigenvalues -> w[n], eigenvectors -> columns of V[n*n].
|
||||
// Not sorted.
|
||||
inline void jacobiEigenSymmetric(std::vector<double>& A, int n, std::vector<double>& w,
|
||||
std::vector<double>& V) {
|
||||
w.assign(n, 0);
|
||||
V.assign((size_t)n * n, 0);
|
||||
// Not sorted. This form allocates nothing, for callers in a hot loop.
|
||||
inline void jacobiEigenSymmetric(double* A, int n, double* w, double* V) {
|
||||
for (int i = 0; i < n; i++) w[i] = 0;
|
||||
for (size_t i = 0; i < (size_t)n * n; i++) V[i] = 0;
|
||||
for (int i = 0; i < n; i++) V[(size_t)i * n + i] = 1.0;
|
||||
|
||||
// Convergence is measured against the matrix's own scale. The old test was
|
||||
@@ -149,6 +148,13 @@ inline void jacobiEigenSymmetric(std::vector<double>& A, int n, std::vector<doub
|
||||
for (int i = 0; i < n; i++) w[i] = A[(size_t)i * n + i];
|
||||
}
|
||||
|
||||
inline void jacobiEigenSymmetric(std::vector<double>& A, int n, std::vector<double>& w,
|
||||
std::vector<double>& V) {
|
||||
w.assign(n, 0);
|
||||
V.assign((size_t)n * n, 0);
|
||||
jacobiEigenSymmetric(A.data(), n, w.data(), V.data());
|
||||
}
|
||||
|
||||
// Exact null space of an m x 9 matrix with m < 9, by Householder QR of A^T.
|
||||
//
|
||||
// A minimal RANSAC sample gives a constraint matrix that is rank deficient *by
|
||||
|
||||
@@ -44,14 +44,14 @@ inline Vec3 triangulateDLT(const Mat34& P1, const Mat34& P2, const Vec3& b1, con
|
||||
double A[4][4];
|
||||
dltRows(A[0], A[1], b1, P1);
|
||||
dltRows(A[2], A[3], b2, P2);
|
||||
std::vector<double> AtA(16, 0.0);
|
||||
// On the stack: retriangulation calls this millions of times a pass.
|
||||
double AtA[16], w[4], V[16];
|
||||
for (int i = 0; i < 4; i++)
|
||||
for (int j = 0; j < 4; j++) {
|
||||
double s = 0;
|
||||
for (int r = 0; r < 4; r++) s += A[r][i] * A[r][j];
|
||||
AtA[i * 4 + j] = s;
|
||||
}
|
||||
std::vector<double> w, V;
|
||||
jacobiEigenSymmetric(AtA, 4, w, V);
|
||||
int mi = 0;
|
||||
for (int i = 1; i < 4; i++)
|
||||
|
||||
@@ -493,9 +493,13 @@ inline double runGlobalBA(Reconstruction& rec, const BundleOptions& bopt) {
|
||||
if (MapProf::enabled())
|
||||
slog::diag(slog::Tag::Map,
|
||||
"[prof] BA #%ld: %u img %u pt %u obs | build %.3f init %.3f solve %.3f "
|
||||
"write %.3f s | %d LM iters",
|
||||
"write %.3f s | %d LM iters, %s%s",
|
||||
(long)g_map_prof.n_ba, P.num_images, P.num_points, P.num_obs, t_build, t_init,
|
||||
t_solve, t_write, stats.iterations);
|
||||
t_solve, t_write, stats.iterations, stats.solver,
|
||||
stats.cg_solves ? (" " + std::to_string((int)std::lround(
|
||||
stats.cg_iters_total / stats.cg_solves)) +
|
||||
" its/solve").c_str()
|
||||
: "");
|
||||
return stats.final_cost;
|
||||
}
|
||||
|
||||
|
||||
+294
-86
@@ -1244,6 +1244,21 @@ public:
|
||||
size_t ra = find(a->second), rb = find(b->second);
|
||||
if (ra != rb) parent[ra] = rb;
|
||||
}
|
||||
// A calibrated rig holds a frame's images at one relative pose, which ties
|
||||
// them as surely as an agreeing pair: back-to-back fisheyes share no
|
||||
// matches, and a 4000-image capture split into its lenses and merged back.
|
||||
if (rigs_) {
|
||||
std::map<std::pair<uint32_t, uint32_t>, size_t> frame_of;
|
||||
for (size_t i = 0; i < ids.size(); i++) {
|
||||
const RigSlot sl = rigs_->slot(ids[i]);
|
||||
if (!sl.valid() || m.rig_detached.count(ids[i]) || sl.rig >= m.rigs.size() ||
|
||||
!m.rigs[sl.rig].usable(sl.member))
|
||||
continue;
|
||||
auto it = frame_of.emplace(std::make_pair(sl.rig, sl.frame), i).first;
|
||||
size_t ra = find(i), rb = find(it->second);
|
||||
if (ra != rb) parent[ra] = rb;
|
||||
}
|
||||
}
|
||||
|
||||
std::sort(st.fractions.begin(), st.fractions.end());
|
||||
std::map<size_t, std::vector<uint32_t>> groups;
|
||||
@@ -1763,6 +1778,7 @@ private:
|
||||
missing);
|
||||
}
|
||||
|
||||
|
||||
// Sort, report and return. Split out only because run() has two exits.
|
||||
std::vector<Reconstruction> finishRun(std::vector<Reconstruction>& models,
|
||||
std::chrono::steady_clock::time_point prof_start) {
|
||||
@@ -2222,11 +2238,19 @@ private:
|
||||
|
||||
// reprojErr() against the index. Same arithmetic, same cheirality rules.
|
||||
double reprojErrAt(const ModelIndex& mi, uint32_t img, uint32_t f, const Vec3& X) const {
|
||||
const Camera& cam = *mi.cam[img];
|
||||
return reprojErrAt(mi, img, f, X, cam.wideFov() ? cam.bearing(kp(img, f)) : Vec3{});
|
||||
}
|
||||
// ... given the feature's bearing, which only a wide lens's cheirality reads.
|
||||
// A fisheye bearing is an iterative inversion, and retriangulating an
|
||||
// every-frame video asked for the same ones millions of times a pass.
|
||||
double reprojErrAt(const ModelIndex& mi, uint32_t img, uint32_t f, const Vec3& X,
|
||||
const Vec3& bearing) const {
|
||||
const Pose& p = mi.img[img]->pose;
|
||||
const Camera& cam = *mi.cam[img];
|
||||
Vec3 pc = mul(p.R, X) + p.t;
|
||||
if (cam.wideFov()) {
|
||||
if (pc.dot(cam.bearing(kp(img, f))) <= 0) return 1e30;
|
||||
if (pc.dot(bearing) <= 0) return 1e30;
|
||||
} else if (pc.z < 1e-8) {
|
||||
return 1e30;
|
||||
}
|
||||
@@ -2238,11 +2262,16 @@ private:
|
||||
// triangulatePair() against the index.
|
||||
bool triangulatePairAt(const ModelIndex& mi, uint32_t a, uint32_t fa, uint32_t b, uint32_t fb,
|
||||
Vec3& X, double err_scale = 1.0) const {
|
||||
return triangulatePairAt(mi, a, fa, mi.cam[a]->bearing(kp(a, fa)), b, fb,
|
||||
mi.cam[b]->bearing(kp(b, fb)), X, err_scale);
|
||||
}
|
||||
bool triangulatePairAt(const ModelIndex& mi, uint32_t a, uint32_t fa, const Vec3& ba,
|
||||
uint32_t b, uint32_t fb, const Vec3& bb, Vec3& X,
|
||||
double err_scale = 1.0) const {
|
||||
const Pose& pa_ = mi.img[a]->pose;
|
||||
const Pose& pb_ = mi.img[b]->pose;
|
||||
const Camera& ca = *mi.cam[a];
|
||||
const Camera& cb = *mi.cam[b];
|
||||
Vec3 ba = ca.bearing(kp(a, fa)), bb = cb.bearing(kp(b, fb));
|
||||
Mat34 Pa{pa_.R[0], pa_.R[1], pa_.R[2], pa_.t.x, pa_.R[3], pa_.R[4], pa_.R[5], pa_.t.y,
|
||||
pa_.R[6], pa_.R[7], pa_.R[8], pa_.t.z};
|
||||
Mat34 Pb{pb_.R[0], pb_.R[1], pb_.R[2], pb_.t.x, pb_.R[3], pb_.R[4], pb_.R[5], pb_.t.y,
|
||||
@@ -2253,9 +2282,9 @@ private:
|
||||
if (!inFront(ca, pa, ba, 1e-6) || !inFront(cb, pb, bb, 1e-6)) return false;
|
||||
double ang = triangulationAngle(X, cameraCenter(pa_), cameraCenter(pb_));
|
||||
if (ang * 180.0 / M_PI < opt_.min_tri_angle_deg) return false;
|
||||
if (reprojErrAt(mi, a, fa, X) > err_scale * (opt_.max_reproj_error * mi.pixel_scale[a]))
|
||||
if (reprojErrAt(mi, a, fa, X, ba) > err_scale * (opt_.max_reproj_error * mi.pixel_scale[a]))
|
||||
return false;
|
||||
if (reprojErrAt(mi, b, fb, X) > err_scale * (opt_.max_reproj_error * mi.pixel_scale[b]))
|
||||
if (reprojErrAt(mi, b, fb, X, bb) > err_scale * (opt_.max_reproj_error * mi.pixel_scale[b]))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
@@ -2977,12 +3006,16 @@ private:
|
||||
return false;
|
||||
}
|
||||
|
||||
// Candidates are tried on a reset model (resetModel precedes initialize) and
|
||||
// touch only their two images: sweeping every image cost a 3000-frame video,
|
||||
// which rejects every neighbour pair, 80 MB of stores per candidate.
|
||||
void rollbackInit(uint32_t a, uint32_t b) {
|
||||
rec_.points3D.clear();
|
||||
rec_.next_point3D_id = 1;
|
||||
for (auto& kv : rec_.images) {
|
||||
kv.second.registered = false;
|
||||
std::fill(kv.second.point3D_ids.begin(), kv.second.point3D_ids.end(), kInvalidPoint3D);
|
||||
for (uint32_t i : {a, b}) {
|
||||
Image& im = rec_.images[i];
|
||||
im.registered = false;
|
||||
std::fill(im.point3D_ids.begin(), im.point3D_ids.end(), kInvalidPoint3D);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -3969,66 +4002,179 @@ private:
|
||||
triangulateForImageAt(mi, img, err_scale);
|
||||
}
|
||||
|
||||
using WorldRays = std::vector<std::vector<float>>; // by image id, 3 per feature
|
||||
|
||||
struct NewTrack {
|
||||
Vec3 X;
|
||||
uint32_t f = 0; // the feature of the image it was made for
|
||||
std::vector<TrackElement> track;
|
||||
};
|
||||
|
||||
// The index is a whole-model scan, so a caller that runs this over every
|
||||
// registered image (completeAndRetriangulate) builds it once.
|
||||
void triangulateForImageAt(const ModelIndex& mi, uint32_t img, double err_scale) {
|
||||
Image& me = *mi.img[img];
|
||||
std::vector<Correspondence> obs; // reused across features
|
||||
std::vector<TrackElement> track;
|
||||
for (uint32_t f = 0; f < feats_[img].count(); f++) {
|
||||
if (me.point3D_ids[f] != kInvalidPoint3D) continue;
|
||||
// candidate observations: registered, feature not yet on a 3D point
|
||||
obs.clear();
|
||||
for (const Correspondence& c : graph_.at(img, f))
|
||||
if (mi.img[c.image_id]->registered &&
|
||||
mi.img[c.image_id]->point3D_ids[c.feature_idx] == kInvalidPoint3D)
|
||||
obs.push_back(c);
|
||||
if (obs.empty()) continue;
|
||||
|
||||
// Triangulate with the correspondence of maximum parallax.
|
||||
Vec3 bestX;
|
||||
double bestAng = -1;
|
||||
const Correspondence* bestC = nullptr;
|
||||
for (const Correspondence& c : obs) {
|
||||
Vec3 X;
|
||||
if (!triangulatePairAt(mi, img, f, c.image_id, c.feature_idx, X, err_scale))
|
||||
continue;
|
||||
double ang = triangulationAngle(X, cameraCenter(me.pose),
|
||||
cameraCenter(mi.img[c.image_id]->pose));
|
||||
if (ang > bestAng) { bestAng = ang; bestX = X; bestC = &c; }
|
||||
}
|
||||
if (!bestC) continue;
|
||||
|
||||
// Build the track: this obs + every candidate that reprojects well.
|
||||
track.clear();
|
||||
track.push_back({img, f});
|
||||
for (const Correspondence& c : obs) {
|
||||
if (mi.img[c.image_id]->point3D_ids[c.feature_idx] != kInvalidPoint3D) continue;
|
||||
if (reprojErrAt(mi, c.image_id, c.feature_idx, bestX) <=
|
||||
err_scale * (opt_.max_reproj_error * mi.pixel_scale[c.image_id]))
|
||||
track.push_back({c.image_id, c.feature_idx});
|
||||
}
|
||||
if (track.size() < 2) continue;
|
||||
// Guard against two features from the same image on one track.
|
||||
std::sort(track.begin(), track.end(),
|
||||
[](const TrackElement& a, const TrackElement& b) {
|
||||
return a.image_id < b.image_id;
|
||||
});
|
||||
track.erase(std::unique(track.begin(), track.end(),
|
||||
[](const TrackElement& a, const TrackElement& b) {
|
||||
return a.image_id == b.image_id;
|
||||
}),
|
||||
track.end());
|
||||
if (track.size() < 2) continue;
|
||||
rec_.addPoint3D(bestX, track);
|
||||
// Every track element is a registered image's feature joining a
|
||||
// point. (Also reached from completeAndRetriangulate, where the
|
||||
// counts drift against its direct track edits -- harmless, no
|
||||
// ranking happens before the post-refine rebuildScores().)
|
||||
for (const TrackElement& e : track) attachObservation(e.image_id, e.point2D_idx);
|
||||
}
|
||||
const uint32_t n = feats_[img].count();
|
||||
const size_t kBlock = 256;
|
||||
std::vector<std::vector<NewTrack>> made((n + kBlock - 1) / kBlock);
|
||||
parallelFor(n, kBlock, [&](size_t lo, size_t hi, std::vector<uint8_t>&) {
|
||||
std::vector<Correspondence> obs;
|
||||
NewTrack t;
|
||||
for (size_t f = lo; f < hi; f++)
|
||||
if (featureTrack(mi, img, (uint32_t)f, err_scale, obs, t))
|
||||
made[lo / kBlock].push_back(t);
|
||||
});
|
||||
commitCollected(mi, img, made, err_scale);
|
||||
}
|
||||
|
||||
// Commit tracks featureTrack collected against an earlier state, in order;
|
||||
// one whose elements a commit since has taken is made again from now.
|
||||
void commitCollected(const ModelIndex& mi, uint32_t img,
|
||||
const std::vector<std::vector<NewTrack>>& made, double err_scale) {
|
||||
std::vector<Correspondence> obs;
|
||||
NewTrack again;
|
||||
for (const std::vector<NewTrack>& block : made)
|
||||
for (const NewTrack& t : block) {
|
||||
bool clash = false;
|
||||
for (const TrackElement& e : t.track)
|
||||
clash = clash || mi.img[e.image_id]->point3D_ids[e.point2D_idx] != kInvalidPoint3D;
|
||||
if (!clash) commitNewTrack(t);
|
||||
else if (featureTrack(mi, img, t.f, err_scale, obs, again)) commitNewTrack(again);
|
||||
}
|
||||
}
|
||||
|
||||
// The point triangulateForImageAt makes for feature f of img, if any. Read
|
||||
// only: a later claim changes the answer only by taking one of `out`'s
|
||||
// elements (the best pair's is among them), so a snapshot's answer checks.
|
||||
bool featureTrack(const ModelIndex& mi, uint32_t img, uint32_t f, double err_scale,
|
||||
std::vector<Correspondence>& obs, NewTrack& out,
|
||||
const WorldRays* rays = nullptr) const {
|
||||
const Image& me = *mi.img[img];
|
||||
if (me.point3D_ids[f] != kInvalidPoint3D) return false;
|
||||
// candidate observations: registered, feature not yet on a 3D point
|
||||
obs.clear();
|
||||
for (const Correspondence& c : graph_.at(img, f))
|
||||
if (mi.img[c.image_id]->registered &&
|
||||
mi.img[c.image_id]->point3D_ids[c.feature_idx] == kInvalidPoint3D)
|
||||
obs.push_back(c);
|
||||
if (obs.empty()) return false;
|
||||
const Vec3 bf = mi.cam[img]->bearing(kp(img, f));
|
||||
std::vector<Vec3>& bo = obs_bearing_scratch();
|
||||
bo.assign(obs.size(), Vec3{0, 0, 0});
|
||||
auto bearingOf = [&](size_t k) -> const Vec3& {
|
||||
if (bo[k].x == 0 && bo[k].y == 0 && bo[k].z == 0)
|
||||
bo[k] = mi.cam[obs[k].image_id]->bearing(kp(obs[k].image_id, obs[k].feature_idx));
|
||||
return bo[k];
|
||||
};
|
||||
|
||||
// Triangulate with the correspondence of maximum parallax. Rays closer
|
||||
// than the minimum angle less both reprojection tolerances cannot:
|
||||
// 94% of a dual-fisheye video's candidates, frames 1/30 s apart.
|
||||
Vec3 ra = mul(transpose(me.pose.R), bf);
|
||||
ra = ra * (1.0 / ra.norm());
|
||||
const double tol_a = rayTolerance(mi, img, err_scale);
|
||||
Vec3 bestX;
|
||||
double bestAng = -1;
|
||||
const Correspondence* bestC = nullptr;
|
||||
for (size_t k = 0; k < obs.size(); k++) {
|
||||
const Correspondence& c = obs[k];
|
||||
const double lim = opt_.min_tri_angle_deg * M_PI / 180.0 - tol_a -
|
||||
rayTolerance(mi, c.image_id, err_scale);
|
||||
if (lim > 0) {
|
||||
Vec3 rb;
|
||||
if (rays) {
|
||||
const float* r = &(*rays)[c.image_id][3 * (size_t)c.feature_idx];
|
||||
rb = {r[0], r[1], r[2]};
|
||||
} else {
|
||||
rb = mul(transpose(mi.img[c.image_id]->pose.R), bearingOf(k));
|
||||
rb = rb * (1.0 / rb.norm());
|
||||
}
|
||||
if (ra.dot(rb) > std::cos(lim)) continue;
|
||||
}
|
||||
Vec3 X;
|
||||
if (!triangulatePairAt(mi, img, f, bf, c.image_id, c.feature_idx, bearingOf(k), X,
|
||||
err_scale))
|
||||
continue;
|
||||
double ang = triangulationAngle(X, cameraCenter(me.pose),
|
||||
cameraCenter(mi.img[c.image_id]->pose));
|
||||
if (ang > bestAng) { bestAng = ang; bestX = X; bestC = &c; }
|
||||
}
|
||||
if (!bestC) return false;
|
||||
|
||||
// Build the track: this obs + every candidate that reprojects well.
|
||||
std::vector<TrackElement>& track = out.track;
|
||||
track.clear();
|
||||
track.push_back({img, f});
|
||||
for (size_t k = 0; k < obs.size(); k++) {
|
||||
const Correspondence& c = obs[k];
|
||||
if (reprojErrAt(mi, c.image_id, c.feature_idx, bestX, bearingOf(k)) <=
|
||||
err_scale * (opt_.max_reproj_error * mi.pixel_scale[c.image_id]))
|
||||
track.push_back({c.image_id, c.feature_idx});
|
||||
}
|
||||
if (track.size() < 2) return false;
|
||||
// Guard against two features from the same image on one track.
|
||||
std::sort(track.begin(), track.end(),
|
||||
[](const TrackElement& a, const TrackElement& b) {
|
||||
return a.image_id < b.image_id;
|
||||
});
|
||||
track.erase(std::unique(track.begin(), track.end(),
|
||||
[](const TrackElement& a, const TrackElement& b) {
|
||||
return a.image_id == b.image_id;
|
||||
}),
|
||||
track.end());
|
||||
if (track.size() < 2) return false;
|
||||
out.X = bestX;
|
||||
out.f = f;
|
||||
return true;
|
||||
}
|
||||
|
||||
// Registered images' feature rays, world frame, unit, float: a fisheye bearing
|
||||
// is an iterative inversion and the filter wanted 166M a pass. Float error is
|
||||
// far inside rayTolerance's slack; what passes is decided on exact bearings.
|
||||
WorldRays worldRays(const ModelIndex& mi) {
|
||||
WorldRays out(db_.images.size());
|
||||
std::vector<uint32_t> imgs;
|
||||
for (uint32_t i = 0; i < db_.images.size(); i++)
|
||||
if (mi.img[i] && mi.img[i]->registered) imgs.push_back(i);
|
||||
parallelFor(imgs.size(), 4, [&](size_t lo, size_t hi, std::vector<uint8_t>&) {
|
||||
for (size_t j = lo; j < hi; j++) {
|
||||
const uint32_t i = imgs[j];
|
||||
const Mat3 Rt = transpose(mi.img[i]->pose.R);
|
||||
const uint32_t n = feats_[i].count();
|
||||
std::vector<float>& r = out[i];
|
||||
r.resize(3 * (size_t)n);
|
||||
for (uint32_t f = 0; f < n; f++) {
|
||||
Vec3 w = mul(Rt, mi.cam[i]->bearing(kp(i, f)));
|
||||
w = w * (1.0 / w.norm());
|
||||
r[3 * (size_t)f] = (float)w.x;
|
||||
r[3 * (size_t)f + 1] = (float)w.y;
|
||||
r[3 * (size_t)f + 2] = (float)w.z;
|
||||
}
|
||||
}
|
||||
});
|
||||
return out;
|
||||
}
|
||||
|
||||
// The angle a reprojection tolerance can move a ray by: twice the tolerance
|
||||
// over the focal, for a lens whose rim compresses the pixel scale.
|
||||
double rayTolerance(const ModelIndex& mi, uint32_t img, double err_scale) const {
|
||||
const Camera& c = *mi.cam[img];
|
||||
return 2.0 * err_scale * opt_.max_reproj_error * mi.pixel_scale[img] / std::min(c.fx, c.fy);
|
||||
}
|
||||
|
||||
// featureTrack's candidate bearings; each worker thread sizes and uses its own.
|
||||
static std::vector<Vec3>& obs_bearing_scratch() {
|
||||
static thread_local std::vector<Vec3> v;
|
||||
return v;
|
||||
}
|
||||
|
||||
void commitNewTrack(const NewTrack& t) {
|
||||
rec_.addPoint3D(t.X, t.track);
|
||||
// Each element joins a point. Completion's direct track edits leave these
|
||||
// counts behind -- harmless: nothing ranks before rebuildScores().
|
||||
for (const TrackElement& e : t.track) attachObservation(e.image_id, e.point2D_idx);
|
||||
}
|
||||
|
||||
|
||||
// ---- global refinement: BA + filtering + image de-registration ----
|
||||
//
|
||||
// COLMAP's IterativeGlobalRefinement in miniature: bundle-adjust, filter,
|
||||
@@ -4284,39 +4430,101 @@ private:
|
||||
void completeAndRetriangulate() {
|
||||
const double err = opt_.retri_scale * opt_.max_reproj_error; // x pixel_scale below
|
||||
ModelIndex mi = indexModel();
|
||||
// "Is this image already on the track" as a dense flag rather than a
|
||||
// std::set rebuilt (and heap-allocated) per point: there are hundreds
|
||||
// of thousands of points and the sets were short-lived and tiny.
|
||||
// Cleared by unsetting only what was set, so it stays O(track).
|
||||
std::vector<std::pair<uint64_t, Point3D*>> pts;
|
||||
pts.reserve(rec_.points3D.size());
|
||||
for (auto& kv : rec_.points3D) pts.emplace_back(kv.first, &kv.second);
|
||||
std::vector<uint32_t> imgs;
|
||||
for (const auto& kv : rec_.images)
|
||||
if (kv.second.registered) imgs.push_back(kv.first);
|
||||
|
||||
// Collected in parallel against the pass's starting state, committed in
|
||||
// the serial order; an item whose claims an earlier commit took is redone
|
||||
// then, so the result is the serial pass's (src/sfm/README.md, "Retriangulation").
|
||||
std::vector<std::vector<TrackElement>> added(pts.size());
|
||||
parallelFor(pts.size(), 256, [&](size_t lo, size_t hi, std::vector<uint8_t>& on_track) {
|
||||
for (size_t i = lo; i < hi; i++) collectCompletion(mi, *pts[i].second, err, on_track,
|
||||
added[i]);
|
||||
});
|
||||
std::vector<uint8_t> on_track(db_.images.size(), 0);
|
||||
for (auto& kv : rec_.points3D) {
|
||||
Point3D& pt = kv.second;
|
||||
for (const TrackElement& e : pt.track) on_track[e.image_id] = 1;
|
||||
for (size_t ti = 0; ti < pt.track.size(); ti++) {
|
||||
const TrackElement e = pt.track[ti]; // copy: track grows below
|
||||
for (const Correspondence& c : graph_.at(e.image_id, e.point2D_idx)) {
|
||||
Image& oi = *mi.img[c.image_id];
|
||||
if (!oi.registered || on_track[c.image_id] ||
|
||||
oi.point3D_ids[c.feature_idx] != kInvalidPoint3D)
|
||||
continue;
|
||||
if (reprojErrAt(mi, c.image_id, c.feature_idx, pt.xyz) <=
|
||||
err * mi.pixel_scale[c.image_id]) {
|
||||
pt.track.push_back({c.image_id, c.feature_idx});
|
||||
on_track[c.image_id] = 1;
|
||||
oi.point3D_ids[c.feature_idx] = kv.first;
|
||||
}
|
||||
}
|
||||
std::vector<TrackElement> redo;
|
||||
for (size_t i = 0; i < pts.size(); i++) {
|
||||
Point3D& pt = *pts[i].second;
|
||||
bool clash = false;
|
||||
for (const TrackElement& e : added[i])
|
||||
clash = clash || mi.img[e.image_id]->point3D_ids[e.point2D_idx] != kInvalidPoint3D;
|
||||
if (clash) collectCompletion(mi, pt, err, on_track, redo);
|
||||
for (const TrackElement& e : clash ? redo : added[i]) {
|
||||
pt.track.push_back(e);
|
||||
mi.img[e.image_id]->point3D_ids[e.point2D_idx] = pts[i].first;
|
||||
}
|
||||
for (const TrackElement& e : pt.track) on_track[e.image_id] = 0;
|
||||
}
|
||||
for (auto& kv : rec_.images)
|
||||
if (kv.second.registered) triangulateForImageAt(mi, kv.first, opt_.retri_scale);
|
||||
|
||||
std::vector<std::vector<std::vector<NewTrack>>> made(imgs.size());
|
||||
{
|
||||
const WorldRays rays = worldRays(mi);
|
||||
parallelFor(imgs.size(), 4, [&](size_t lo, size_t hi, std::vector<uint8_t>&) {
|
||||
std::vector<Correspondence> obs;
|
||||
NewTrack t;
|
||||
for (size_t i = lo; i < hi; i++) {
|
||||
made[i].resize(1);
|
||||
for (uint32_t f = 0; f < feats_[imgs[i]].count(); f++)
|
||||
if (featureTrack(mi, imgs[i], f, opt_.retri_scale, obs, t, &rays))
|
||||
made[i][0].push_back(t);
|
||||
}
|
||||
});
|
||||
}
|
||||
for (size_t i = 0; i < imgs.size(); i++)
|
||||
commitCollected(mi, imgs[i], made[i], opt_.retri_scale);
|
||||
if (opt_.merge_tracks) {
|
||||
ProfTimer pt(g_map_prof.merge);
|
||||
g_map_prof.n_merged += mergeTracks(mi, opt_.retri_scale);
|
||||
}
|
||||
}
|
||||
|
||||
// The observations track completion adds to `pt`: every correspondence of
|
||||
// an element, transitively, that is free, on an image the track lacks, and
|
||||
// reprojects within `err`. `on_track` is all zero on entry and on return.
|
||||
void collectCompletion(const ModelIndex& mi, const Point3D& pt, double err,
|
||||
std::vector<uint8_t>& on_track, std::vector<TrackElement>& add) const {
|
||||
add.clear();
|
||||
for (const TrackElement& e : pt.track) on_track[e.image_id] = 1;
|
||||
const size_t n0 = pt.track.size();
|
||||
for (size_t ti = 0; ti < n0 + add.size(); ti++) {
|
||||
const TrackElement e = ti < n0 ? pt.track[ti] : add[ti - n0];
|
||||
for (const Correspondence& c : graph_.at(e.image_id, e.point2D_idx)) {
|
||||
const Image& oi = *mi.img[c.image_id];
|
||||
if (!oi.registered || on_track[c.image_id] ||
|
||||
oi.point3D_ids[c.feature_idx] != kInvalidPoint3D)
|
||||
continue;
|
||||
if (reprojErrAt(mi, c.image_id, c.feature_idx, pt.xyz) <=
|
||||
err * mi.pixel_scale[c.image_id]) {
|
||||
add.push_back({c.image_id, c.feature_idx});
|
||||
on_track[c.image_id] = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
for (const TrackElement& e : pt.track) on_track[e.image_id] = 0;
|
||||
for (const TrackElement& e : add) on_track[e.image_id] = 0;
|
||||
}
|
||||
|
||||
// fn(lo, hi, scratch) over [0, n) in blocks, on the mapper's threads; each
|
||||
// worker gets a zeroed flag buffer the size of the image table.
|
||||
template <class F>
|
||||
void parallelFor(size_t n, size_t block, F&& fn) {
|
||||
const unsigned hc = std::thread::hardware_concurrency();
|
||||
int nt = opt_.threads > 0 ? opt_.threads : (hc > 0 ? (int)hc : 1);
|
||||
nt = std::max(1, std::min<int>(nt, (int)((n + block - 1) / block)));
|
||||
std::atomic<size_t> next{0};
|
||||
auto worker = [&] {
|
||||
std::vector<uint8_t> scratch(db_.images.size(), 0);
|
||||
for (size_t b; (b = next.fetch_add(block)) < n;) fn(b, std::min(b + block, n), scratch);
|
||||
};
|
||||
if (nt == 1) return worker();
|
||||
std::vector<std::thread> pool;
|
||||
for (int t = 0; t < nt; t++) pool.emplace_back(worker);
|
||||
for (std::thread& t : pool) t.join();
|
||||
}
|
||||
|
||||
size_t countObservations() const {
|
||||
size_t n = 0;
|
||||
for (const auto& kv : rec_.points3D) n += kv.second.track.size();
|
||||
|
||||
+304
-7
@@ -34,6 +34,8 @@ static const uint kCgRR = 4; // ||r||^2
|
||||
static const uint kCgTol2 = 5; // eta^2 ||r0||^2
|
||||
static const uint kCgConv = 6; // > 0.5: loop kernels become no-ops
|
||||
static const uint kCgIters = 7; // completed CG iterations
|
||||
static const uint kCgQ = 8; // quadratic model 1/2 x^T S x - g^T x at x
|
||||
static const uint kCgDQ = 9; // ... and its change over the last iteration
|
||||
|
||||
// Packed block strides (lower triangles): preconditioner blocks are at most
|
||||
// kPrecDof wide, per-image B blocks as wide as the tier.
|
||||
@@ -174,8 +176,9 @@ void cgCamDiagImpl<let MD : int>(uint wgx, uint gi) {
|
||||
atomicAccum(u_g_a, cols[gi], -gacc);
|
||||
}
|
||||
|
||||
// Factor each block in place (packed lower Cholesky, guarded like chol_diag).
|
||||
// One thread per block; blocks are at most kPrecDof wide. u0 = num blocks
|
||||
// Factor each block in place (packed lower Cholesky). A block can be all
|
||||
// cancellation noise (Gram entries of 2e23 on a 22042-image capture): a pivot
|
||||
// under 1e-10 of its largest diagonal is floored and decoupled. u0 = blocks
|
||||
[shader("compute")] [numthreads(256, 1, 1)]
|
||||
void cg_prec_fact(uint3 t : SV_DispatchThreadID) {
|
||||
uint b = t.x;
|
||||
@@ -186,10 +189,15 @@ void cg_prec_fact(uint3 t : SV_DispatchThreadID) {
|
||||
uint cb = kCamBlk * b;
|
||||
Real L[kCamBlk];
|
||||
for (uint i = 0; i < dof * (dof + 1) / 2; i++) L[i] = u_cg_M[cb + i];
|
||||
Real f = Real(1e-30);
|
||||
for (uint j = 0; j < dof; j++)
|
||||
if (L[pidx(j, j)] > f) f = L[pidx(j, j)];
|
||||
f = max(Real(1e-10) * f, Real(1e-30));
|
||||
for (uint j = 0; j < dof; j++) {
|
||||
Real d = sqrt(max(L[pidx(j, j)], Real(1e-30)));
|
||||
bool ok = L[pidx(j, j)] > f;
|
||||
Real d = sqrt(ok ? L[pidx(j, j)] : f);
|
||||
L[pidx(j, j)] = d;
|
||||
for (uint i = j + 1; i < dof; i++) L[pidx(i, j)] = L[pidx(i, j)] / d;
|
||||
for (uint i = j + 1; i < dof; i++) L[pidx(i, j)] = ok ? L[pidx(i, j)] / d : Real(0.0);
|
||||
for (uint c = j + 1; c < dof; c++)
|
||||
for (uint i = c; i < dof; i++)
|
||||
L[pidx(i, c)] = L[pidx(i, c)] - L[pidx(i, j)] * L[pidx(c, j)];
|
||||
@@ -394,9 +402,9 @@ void cg_red2(uint3 t : SV_DispatchThreadID, uint3 wg : SV_GroupID, uint gi : SV_
|
||||
}
|
||||
}
|
||||
|
||||
// Final partial reduction + scalar updates; one workgroup.
|
||||
// u0 = partial count, u1 = mode (0: init rho/tol, 1: alpha, 2: beta/converge),
|
||||
// u2 = partial stride, f0 = eta (mode 0: relative residual tolerance)
|
||||
// Final partial reduction + scalar updates; one workgroup. u0 = partials,
|
||||
// u1 = mode (0: init rho/tol, 1: alpha, 2: beta/converge), u2 = partial stride,
|
||||
// f0 = relative residual tolerance (mode 0), f1 = Nash-Sofer tolerance (0: off)
|
||||
[shader("compute")] [numthreads(256, 1, 1)]
|
||||
void cg_fin(uint gi : SV_GroupIndex) {
|
||||
if (u1 != 0 && cgDone()) return;
|
||||
@@ -423,6 +431,7 @@ void cg_fin(uint gi : SV_GroupIndex) {
|
||||
u_cg_scal[kCgRR] = B;
|
||||
u_cg_scal[kCgTol2] = Real(f0) * Real(f0) * B;
|
||||
u_cg_scal[kCgIters] = Real(0.0);
|
||||
u_cg_scal[kCgQ] = Real(0.0);
|
||||
// zero RHS (or a broken preconditioner) => keep the zero step
|
||||
u_cg_scal[kCgConv] = (isfinite(A) && A > Real(0.0)) ? Real(0.0) : Real(1.0);
|
||||
} else if (u1 == 1) { // A = p.Sp
|
||||
@@ -430,6 +439,10 @@ void cg_fin(uint gi : SV_GroupIndex) {
|
||||
bool ok = isfinite(A) && A > Real(0.0);
|
||||
u_cg_scal[kCgPAp] = A;
|
||||
u_cg_scal[kCgAlpha] = ok ? rho / A : Real(0.0);
|
||||
// the step along p lowers the model by alpha rho / 2
|
||||
Real dq = ok ? Real(-0.5) * rho * rho / A : Real(0.0);
|
||||
u_cg_scal[kCgDQ] = dq;
|
||||
u_cg_scal[kCgQ] = u_cg_scal[kCgQ] + dq;
|
||||
if (!ok) u_cg_scal[kCgConv] = Real(1.0); // lost positive-definiteness
|
||||
} else { // A = new r.z, B = new ||r||^2
|
||||
Real rho = u_cg_scal[kCgRho];
|
||||
@@ -439,6 +452,15 @@ void cg_fin(uint gi : SV_GroupIndex) {
|
||||
u_cg_scal[kCgIters] = u_cg_scal[kCgIters] + Real(1.0);
|
||||
if (!(B > u_cg_scal[kCgTol2]) || !isfinite(A) || !(A > Real(0.0)))
|
||||
u_cg_scal[kCgConv] = Real(1.0);
|
||||
// Nash-Sofer, as Ceres stops LM's CG: i (Q_i - Q_i-1) / Q_i under f1.
|
||||
// 40% fewer iterations than the residual test alone on a 4102-image
|
||||
// capture, same final cost. Marked as the residual test being met.
|
||||
Real q = u_cg_scal[kCgQ];
|
||||
if (f1 > 0.0f && q < Real(0.0) &&
|
||||
u_cg_scal[kCgIters] * u_cg_scal[kCgDQ] / q < Real(f1)) {
|
||||
u_cg_scal[kCgConv] = Real(1.0);
|
||||
u_cg_scal[kCgTol2] = B;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -461,3 +483,278 @@ void cg_updp(uint3 t : SV_DispatchThreadID) {
|
||||
if (i >= u0) return;
|
||||
u_cg_p[i] = u_cg_z[i] + u_cg_scal[kCgBeta] * u_cg_p[i];
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------
|
||||
// coarse correction: M^-1 = block Jacobi + P A_c^-1 P^T
|
||||
// ---------------------------------------------------------------------
|
||||
|
||||
// P: a similarity motion (7 dofs) per cluster of u2 frames, as pose deltas
|
||||
// (README.md, "Coarse correction"). The CG path leaves u_S and u_y free: A_c
|
||||
// goes in u_S with P (6x7 per frame) from u3; tables follow the pair-Schur ones.
|
||||
|
||||
static const uint kTcDof = 7;
|
||||
|
||||
uint tcCluster(uint o) { return u_image_info[u_obs_image[o]].x / u2; }
|
||||
|
||||
// d(angle-axis) = -Jr^-1 w and dt = s t - R tau for a world similarity
|
||||
// (w, tau, s), in fp32: the basis only has to span the slow modes, not be
|
||||
// exact. u0 = num_frames
|
||||
[shader("compute")] [numthreads(64, 1, 1)]
|
||||
void tc_basis(uint3 t : SV_DispatchThreadID) {
|
||||
uint f = t.x;
|
||||
if (f >= u0) return;
|
||||
float a[3], tr[3];
|
||||
for (int i = 0; i < 3; i++) {
|
||||
a[i] = rToFloat(u_poses[6 * f + i]);
|
||||
tr[i] = rToFloat(u_poses[6 * f + 3 + i]);
|
||||
}
|
||||
float th2 = a[0] * a[0] + a[1] * a[1] + a[2] * a[2], th = sqrt(th2);
|
||||
float K[9] = {0, -a[2], a[1], a[2], 0, -a[0], -a[1], a[0], 0};
|
||||
float K2[9];
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 3; j++)
|
||||
K2[3 * i + j] = K[3 * i] * K[j] + K[3 * i + 1] * K[3 + j] + K[3 * i + 2] * K[6 + j];
|
||||
bool nearZero = th < 1e-3f;
|
||||
float s1 = nearZero ? 1.0f : sin(th) / th;
|
||||
float c1 = nearZero ? 0.5f : (1.0f - cos(th)) / th2;
|
||||
float c2 = nearZero ? 1.0f / 12.0f : 1.0f / th2 - (1.0f + cos(th)) / (2.0f * th * sin(th));
|
||||
uint b = u3 + 42 * f;
|
||||
for (int i = 0; i < 3; i++)
|
||||
for (int j = 0; j < 7; j++) {
|
||||
float rot = j < 3 ? -(float(i == j) + 0.5f * K[3 * i + j] + c2 * K2[3 * i + j]) : 0.0f;
|
||||
float R = j >= 3 && j < 6 ? float(i == j - 3) + s1 * K[3 * i + j - 3] + c1 * K2[3 * i + j - 3]
|
||||
: 0.0f;
|
||||
u_S[b + 7 * i + j] = Real(rot);
|
||||
u_S[b + 7 * (3 + i) + j] = Real(j == 6 ? tr[i] : -R);
|
||||
}
|
||||
}
|
||||
|
||||
// A_c += P_f^T B_img P_f over the pose rows of each image's damped Gram block.
|
||||
// u0 = num_images, u2 = frames per cluster, u3 = basis offset
|
||||
void tcBpartImpl<let MD : int>(uint img) {
|
||||
if (img >= u0) return;
|
||||
uint f = u_image_info[img].x, c = f / u2;
|
||||
uint cb = bblk<MD>() * img, pb = u3 + 42 * f;
|
||||
Real BP[42];
|
||||
for (uint r = 0; r < 6; r++)
|
||||
for (uint j = 0; j < 7; j++) {
|
||||
Real s = Real(0.0);
|
||||
for (uint l = 0; l < 6; l++)
|
||||
s = s + u_cg_B[cb + (r >= l ? pidx(r, l) : pidx(l, r))] * u_S[pb + 7 * l + j];
|
||||
BP[7 * r + j] = s;
|
||||
}
|
||||
for (uint i = 0; i < 7; i++)
|
||||
for (uint j = 0; j <= i; j++) {
|
||||
Real s = Real(0.0);
|
||||
for (uint l = 0; l < 6; l++) s = s + u_S[pb + 7 * l + i] * BP[7 * l + j];
|
||||
if (isfinite(s)) atomicAccum(u_S_a, pidx(kTcDof * c + i, kTcDof * c + j), s);
|
||||
}
|
||||
}
|
||||
|
||||
// G = sum over the run starting at observation o of P_f^T Jc_pose^T Jp (7x3).
|
||||
void tcRunG(uint o, uint t1, out Real G[21]) {
|
||||
for (int i = 0; i < 21; i++) G[i] = Real(0.0);
|
||||
uint c = tcCluster(o);
|
||||
for (; o < t1 && tcCluster(o) == c; o++) {
|
||||
uint pb = u3 + 42 * u_image_info[u_obs_image[o]].x, jb = u_jc_off[o];
|
||||
for (uint i = 0; i < 7; i++) {
|
||||
Real q0 = Real(0.0), q1 = Real(0.0);
|
||||
for (uint a = 0; a < 6; a++) {
|
||||
q0 = q0 + u_S[pb + 7 * a + i] * u_Jc[jb + 2 * a];
|
||||
q1 = q1 + u_S[pb + 7 * a + i] * u_Jc[jb + 2 * a + 1];
|
||||
}
|
||||
for (uint j = 0; j < 3; j++)
|
||||
G[3 * i + j] = G[3 * i + j] + q0 * u_Jp[6 * o + 2 * j] + q1 * u_Jp[6 * o + 2 * j + 1];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// A_c[cu, cv] -= sum G_u W G_v^T, one warp per chunk of one cluster pair's
|
||||
// entries: lanes stage their 7x7 products in shared and the warp folds them
|
||||
// into 49 sums, written once. u0 = chunks, u1 = first chunk, u2/u3 as above
|
||||
[shader("compute")] [numthreads(kPairWg, 1, 1)]
|
||||
void tc_schur(uint3 wg : SV_GroupID, uint gi : SV_GroupIndex) {
|
||||
uint wgx = wg.x + u4;
|
||||
if (wgx >= u0) return;
|
||||
uint2 ck = u_pair_chunks[u1 + wgx];
|
||||
uint2 e0 = u_pair_entries[ck.x];
|
||||
uint cu = tcCluster(e0.x), cv = tcCluster(e0.y);
|
||||
Real acc0 = Real(0.0), acc1 = Real(0.0);
|
||||
for (uint b0 = 0; b0 < ck.y; b0 += kPairWg) {
|
||||
GroupMemoryBarrierWithGroupSync();
|
||||
if (b0 + gi < ck.y) {
|
||||
uint2 e = u_pair_entries[ck.x + b0 + gi];
|
||||
uint p = u_obs_point[e.x], t1 = u_obs_ranges[p + 1];
|
||||
Real G[21], GW[21];
|
||||
tcRunG(e.x, t1, G);
|
||||
for (uint j = 0; j < 3; j++) {
|
||||
Real w0 = u_W[9 * p + j], w1 = u_W[9 * p + 3 + j], w2 = u_W[9 * p + 6 + j];
|
||||
for (uint i = 0; i < 7; i++)
|
||||
GW[3 * i + j] = G[3 * i] * w0 + G[3 * i + 1] * w1 + G[3 * i + 2] * w2;
|
||||
}
|
||||
if (e.y != e.x) tcRunG(e.y, t1, G);
|
||||
for (uint i = 0; i < 7; i++)
|
||||
for (uint j = 0; j < 7; j++) {
|
||||
Real v = GW[3 * i] * G[3 * j] + GW[3 * i + 1] * G[3 * j + 1]
|
||||
+ GW[3 * i + 2] * G[3 * j + 2];
|
||||
g_chol[49 * gi + 7 * i + j] = isfinite(v) ? v : Real(0.0);
|
||||
}
|
||||
} else {
|
||||
for (uint i = 0; i < 49; i++) g_chol[49 * gi + i] = Real(0.0);
|
||||
}
|
||||
GroupMemoryBarrierWithGroupSync();
|
||||
uint nl = min(kPairWg, ck.y - b0);
|
||||
for (uint l = 0; l < nl; l++) {
|
||||
acc0 = acc0 + g_chol[49 * l + gi];
|
||||
if (gi + kPairWg < 49) acc1 = acc1 + g_chol[49 * l + gi + kPairWg];
|
||||
}
|
||||
}
|
||||
for (uint k = 0; k < 2; k++) {
|
||||
uint el = gi + kPairWg * k;
|
||||
if (el >= 49) continue;
|
||||
uint R = kTcDof * cu + el / 7, C = kTcDof * cv + el % 7;
|
||||
if (R >= C) atomicAccum(u_S_a, pidx(R, C), -(k == 0 ? acc0 : acc1));
|
||||
}
|
||||
}
|
||||
|
||||
// A_c diagonal *= 1 + f0, and saved in u_y for the factor's pivot floor: the
|
||||
// global similarity is a gauge freedom, so A_c is singular up to the damping.
|
||||
// u0 = coarse dim
|
||||
[shader("compute")] [numthreads(256, 1, 1)]
|
||||
void tc_reg(uint3 t : SV_DispatchThreadID) {
|
||||
uint i = t.x;
|
||||
if (i >= u0) return;
|
||||
Real d = u_S[pidx(i, i)];
|
||||
u_y[i] = d;
|
||||
u_S[pidx(i, i)] = d * (Real(1.0) + Real(f0));
|
||||
}
|
||||
|
||||
// ---- L -> L^-1 in place, so that each application is two matrix-vector
|
||||
// products rather than 2 n / 32 dependent triangular-solve dispatches ----
|
||||
|
||||
// Invert every 32x32 diagonal block, one thread per column. u0 = n
|
||||
[shader("compute")] [numthreads(kBs, 1, 1)]
|
||||
void tc_dinv(uint3 wg : SV_GroupID, uint gi : SV_GroupIndex) {
|
||||
uint base = wg.x * kBs, m = min(kBs, u0 - base), c = gi;
|
||||
for (uint r = c; r < m; r++)
|
||||
g_chol[r * kBs + c] = u_S[pidx(base + r, base + c)];
|
||||
GroupMemoryBarrierWithGroupSync();
|
||||
// column c of the inverse, into the upper half of the pool
|
||||
if (c < m) {
|
||||
g_chol[kSh2 + c * kPitch + c] = Real(1.0) / g_chol[c * kBs + c];
|
||||
for (uint r = c + 1; r < m; r++) {
|
||||
Real s = Real(0.0);
|
||||
for (uint k = c; k < r; k++) s = s + g_chol[r * kBs + k] * g_chol[kSh2 + k * kPitch + c];
|
||||
g_chol[kSh2 + r * kPitch + c] = -s / g_chol[r * kBs + r];
|
||||
}
|
||||
}
|
||||
GroupMemoryBarrierWithGroupSync();
|
||||
for (uint r = c; r < m; r++)
|
||||
u_S[pidx(base + r, base + c)] = g_chol[kSh2 + r * kPitch + c];
|
||||
}
|
||||
|
||||
// Block row u1 of the inverse, X_ij = -X_ii sum_{j<=k<i} L_ik X_kj, into
|
||||
// scratch rows u_y[u2 + r * n + col] while the row's L blocks are still read.
|
||||
// One 32x32 workgroup per block j < i. u0 = n
|
||||
[shader("compute")] [numthreads(kBs, kBs, 1)]
|
||||
void tc_inv_row(uint3 wg : SV_GroupID, uint3 lt : SV_GroupThreadID) {
|
||||
uint n = u0, i = u1, j = wg.x;
|
||||
uint r = lt.y, c = lt.x;
|
||||
uint bi = i * kBs, bj = j * kBs;
|
||||
Real acc = Real(0.0);
|
||||
for (uint k = j; k < i; k++) {
|
||||
uint bk = k * kBs;
|
||||
GroupMemoryBarrierWithGroupSync();
|
||||
g_chol[r * kBs + c] = bi + r < n ? u_S[pidx(bi + r, bk + c)] : Real(0.0);
|
||||
g_chol[kSh2 + r * kPitch + c] = bk + r >= bj + c ? u_S[pidx(bk + r, bj + c)] : Real(0.0);
|
||||
GroupMemoryBarrierWithGroupSync();
|
||||
for (uint m = 0; m < kBs; m++)
|
||||
acc = acc + g_chol[r * kBs + m] * g_chol[kSh2 + m * kPitch + c];
|
||||
}
|
||||
GroupMemoryBarrierWithGroupSync();
|
||||
g_chol[kSh2 + r * kPitch + c] = acc;
|
||||
g_chol[r * kBs + c] = bi + r < n && c <= r ? u_S[pidx(bi + r, bi + c)] : Real(0.0);
|
||||
GroupMemoryBarrierWithGroupSync();
|
||||
Real x = Real(0.0);
|
||||
for (uint m = 0; m <= r; m++) x = x + g_chol[r * kBs + m] * g_chol[kSh2 + m * kPitch + c];
|
||||
if (bi + r < n) u_y[u2 + r * n + bj + c] = -x;
|
||||
}
|
||||
|
||||
// Scratch row -> block row u1. u0 = n, one thread per element.
|
||||
[shader("compute")] [numthreads(256, 1, 1)]
|
||||
void tc_inv_copy(uint3 t : SV_DispatchThreadID) {
|
||||
uint n = u0, bi = u1 * kBs;
|
||||
uint r = t.x / bi, col = t.x - r * bi;
|
||||
if (r >= kBs || bi + r >= n) return;
|
||||
u_S[pidx(bi + r, col)] = u_y[u2 + r * n + col];
|
||||
}
|
||||
|
||||
// ---- application ----
|
||||
|
||||
// v = P^T r into u_y[0, u0), one thread per coarse dof. u1 = num_frames
|
||||
[shader("compute")] [numthreads(256, 1, 1)]
|
||||
void tc_restrict(uint3 t : SV_DispatchThreadID) {
|
||||
if (cgDone()) return;
|
||||
uint q = t.x;
|
||||
if (q >= u0) return;
|
||||
uint c = q / kTcDof, j = q - kTcDof * c;
|
||||
uint fend = min(u1, (c + 1) * u2);
|
||||
Real s = Real(0.0);
|
||||
for (uint f = c * u2; f < fend; f++)
|
||||
for (uint a = 0; a < 6; a++) s = s + u_S[u3 + 42 * f + 7 * a + j] * u_cg_r[6 * f + a];
|
||||
u_y[q] = s;
|
||||
}
|
||||
|
||||
// w = X v into u_y[u0, 2 u0), one warp per row. u0 = coarse dim
|
||||
[shader("compute")] [numthreads(256, 1, 1)]
|
||||
void tc_lmul(uint3 t : SV_DispatchThreadID, uint gi : SV_GroupIndex) {
|
||||
if (cgDone()) return;
|
||||
uint row = t.x / 32, lane = gi % 32, w = gi / 32;
|
||||
Real s = Real(0.0);
|
||||
if (row < u0)
|
||||
for (uint j = lane; j <= row; j += 32) s = s + u_S[pidx(row, j)] * u_y[j];
|
||||
g_sh[gi] = s;
|
||||
GroupMemoryBarrierWithGroupSync();
|
||||
for (uint h = 16; h > 0; h >>= 1) {
|
||||
if (lane < h) g_sh[gi] = g_sh[gi] + g_sh[gi + h];
|
||||
GroupMemoryBarrierWithGroupSync();
|
||||
}
|
||||
if (lane == 0 && row < u0) u_y[u0 + row] = g_sh[32 * w];
|
||||
}
|
||||
|
||||
// z = X^T w into u_y[0, u0): 32 columns per workgroup, so each packed row
|
||||
// segment is one coalesced read, and 8 row lanes summed in shared.
|
||||
[shader("compute")] [numthreads(256, 1, 1)]
|
||||
void tc_ltmul(uint3 wg : SV_GroupID, uint gi : SV_GroupIndex) {
|
||||
if (cgDone()) return;
|
||||
uint cc = gi % 32, rl = gi / 32;
|
||||
uint col = wg.x * 32 + cc;
|
||||
Real s = Real(0.0);
|
||||
if (col < u0)
|
||||
for (uint r = wg.x * 32 + rl; r < u0; r += 8)
|
||||
if (r >= col) s = s + u_S[pidx(r, col)] * u_y[u0 + r];
|
||||
g_sh[gi] = s;
|
||||
GroupMemoryBarrierWithGroupSync();
|
||||
if (rl == 0 && col < u0) {
|
||||
Real z = Real(0.0);
|
||||
for (uint k = 0; k < 8; k++) z = z + g_sh[32 * k + cc];
|
||||
u_y[col] = z;
|
||||
}
|
||||
}
|
||||
|
||||
// z += P y over the pose columns. u0 = pose_dim
|
||||
[shader("compute")] [numthreads(256, 1, 1)]
|
||||
void tc_prolong(uint3 t : SV_DispatchThreadID) {
|
||||
if (cgDone()) return;
|
||||
uint i = t.x;
|
||||
if (i >= u0) return;
|
||||
uint f = i / 6, a = i - 6 * f, c = f / u2;
|
||||
Real s = Real(0.0);
|
||||
for (uint j = 0; j < 7; j++) s = s + u_S[u3 + 42 * f + 7 * a + j] * u_y[kTcDof * c + j];
|
||||
u_cg_z[i] = u_cg_z[i] + s;
|
||||
}
|
||||
|
||||
[shader("compute")] [numthreads(64, 1, 1)]
|
||||
void tc_bpart_w(uint3 t : SV_DispatchThreadID) { tcBpartImpl<kDofWide>(t.x); }
|
||||
[shader("compute")] [numthreads(64, 1, 1)]
|
||||
void tc_bpart_x(uint3 t : SV_DispatchThreadID) { tcBpartImpl<kDofRig>(t.x); }
|
||||
|
||||
@@ -12,6 +12,15 @@
|
||||
// Included at the end of ba.slang; shares its bindings and push constants.
|
||||
|
||||
static const uint kBs = 32;
|
||||
|
||||
// Pivot from trailing diagonal x (NaN included): at least 1e-30; with u3 set, one
|
||||
// under f0 of the row's original diagonal (in u_y) becomes that diagonal, which
|
||||
// bounds the factor however indefinite rounding made it (cg.slang's coarse one).
|
||||
Real pivot(Real x, uint row) {
|
||||
if (u3 == 0) return sqrt(x > Real(1e-30) ? x : Real(1e-30));
|
||||
Real d = u_y[row];
|
||||
return sqrt(x > Real(f0) * d ? x : max(d, Real(1e-30)));
|
||||
}
|
||||
static const uint kPitch = 33; // padded shared row pitch (bank-conflict-free)
|
||||
static const uint kShV = kSh2 + 32 * 33; // RHS vector slot in the shared pool
|
||||
|
||||
@@ -30,7 +39,7 @@ void chol_diag(uint3 t : SV_DispatchThreadID) {
|
||||
for (uint j = 0; j < m; j++) {
|
||||
GroupMemoryBarrierWithGroupSync();
|
||||
if (r == j)
|
||||
g_chol[j * kPitch + j] = sqrt(max(g_chol[j * kPitch + j], Real(1e-30)));
|
||||
g_chol[j * kPitch + j] = pivot(g_chol[j * kPitch + j], base + j);
|
||||
GroupMemoryBarrierWithGroupSync();
|
||||
if (r > j && r < m)
|
||||
g_chol[r * kPitch + j] = g_chol[r * kPitch + j] / g_chol[j * kPitch + j];
|
||||
@@ -128,7 +137,7 @@ void chol_update(uint3 wg : SV_GroupID, uint3 lt : SV_GroupThreadID) {
|
||||
for (uint j = 0; j < m2; j++) {
|
||||
GroupMemoryBarrierWithGroupSync();
|
||||
if (r == j && c == j)
|
||||
g_chol[j * kPitch + j] = sqrt(max(g_chol[j * kPitch + j], Real(1e-30)));
|
||||
g_chol[j * kPitch + j] = pivot(g_chol[j * kPitch + j], bi + j);
|
||||
GroupMemoryBarrierWithGroupSync();
|
||||
if (c == j && r > j && r < m2)
|
||||
g_chol[r * kPitch + j] = g_chol[r * kPitch + j] / g_chol[j * kPitch + j];
|
||||
|
||||
@@ -433,6 +433,7 @@ void testAgainstReference(uint32_t model, uint32_t groups, const char* loss, boo
|
||||
opt.verbose = false;
|
||||
opt.solver = cg ? SolverSel::CG : SolverSel::Dense;
|
||||
opt.cg_tol = 1e-12;
|
||||
opt.cg_model_tol = 0;
|
||||
opt.cg_max_iters = 4000;
|
||||
opt.cg_fallback = CgFallback::Off;
|
||||
|
||||
@@ -512,6 +513,7 @@ void testFullSolve(uint32_t model, uint32_t groups, uint32_t rig = 0) {
|
||||
opt.verbose = false;
|
||||
opt.solver = k ? SolverSel::CG : SolverSel::Dense;
|
||||
opt.cg_tol = 1e-10;
|
||||
opt.cg_model_tol = 0;
|
||||
opt.cg_max_iters = 2000;
|
||||
opt.cg_fallback = CgFallback::Off;
|
||||
bacpu::Solver s(P, opt);
|
||||
|
||||
@@ -52,6 +52,7 @@ SolverOptions baseOptions(RealCfg real, int device, bool cg) {
|
||||
o.verbose = false;
|
||||
o.solver = cg ? SolverSel::CG : SolverSel::Dense;
|
||||
o.cg_tol = 1e-10;
|
||||
o.cg_model_tol = 0;
|
||||
o.cg_max_iters = 3000;
|
||||
o.cg_fallback = CgFallback::Off;
|
||||
return o;
|
||||
|
||||
Reference in New Issue
Block a user