trending/spirula-studio
1
0
Fork 0
mirror of https://github.com/harry7557558/spirula-studio.git synced 2026-10-02 02:44:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

provide BA solver CPU fallback for devices without native FP64 support

Browse Source
This commit is contained in:
Harry Chen
2026-08-09 22:11:23 -04:00
parent 4f18ef0a41
commit bcc3571777
18 changed files with 2823 additions and 214 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/app/cli/sfm_ba.cpp
+5 -11
View File
@@ -57,7 +57,7 @@ void printBaHelp(FILE* out) {
// language allows.
struct Row { const char* flag; const char* def; const spirula::i18n::Msg* help; };
static const Row kRows[] = {
{"--real {float|double|df}", "double", &H::ba_opt_real},
{"--real {float|double|df|cpu}", "double", &H::ba_opt_real},
{"--loss {trivial|huber|cauchy}", "trivial", &H::ba_opt_loss},
{"--loss-param X", "1", &H::ba_opt_loss_param},
{"--model {snavely|snavely_f}", "snavely", &H::ba_opt_model},
@@ -103,10 +103,8 @@ int cmdBa(int argc, char** argv) {
std::string a = argv[i];
auto next = [&]() { return std::string(argv[++i]); };
if (a == "--help" || a == "-h") { printBaHelp(stdout); return 0; }
else if (a == "--real") {
std::string r = next();
opt.real = r == "float" ? RealCfg::F32 : r == "df" ? RealCfg::DF64 : RealCfg::F64;
} else if (a == "--loss") { loss = next(); loss_given = true; }
else if (a == "--real") opt.real = realCfgFromName(next());
else if (a == "--loss") { loss = next(); loss_given = true; }
else if (a == "--spv-path") opt.spv_path = next();
else if (a == "--loss-param") { opt.loss_param = std::stof(next()); loss_given = true; }
else if (a == "-o" || a == "--output") out_dir = next();
@@ -202,15 +200,11 @@ int cmdBa(int argc, char** argv) {
if (spirula::env("SFM_DUMP_SG")) {
solver.debugAssemble(atof(env_or("SFM_DUMP_SG_LAMBDA", "0.01")));
std::vector<uint8_t> raw(solver.bufS().size);
std::vector<double> v;
solver.ctx().download(solver.bufS(), raw.data(), solver.bufS().size);
unpackReals(v, raw.data(), (uint64_t)P.n_dim * (P.n_dim + 1) / 2, solver.real());
std::string base = spirula::env("SFM_DUMP_SG");
std::vector<double> v = solver.debugPackedS();
std::ofstream fs(base + "_S.bin", std::ios::binary);
fs.write((const char*)v.data(), v.size() * 8);
solver.ctx().download(solver.bufG(), raw.data(), solver.bufG().size);
unpackReals(v, raw.data(), P.n_dim, solver.real());
v = solver.debugG();
std::ofstream fg(base + "_g.bin", std::ios::binary);
fg.write((const char*)v.data(), v.size() * 8);
return 0;
src/app/cli/sfm_main.cpp
-16
View File
@@ -1822,22 +1822,6 @@ static int cmdAuto(int argc, char** argv) {
if (fs::is_directory(sibling)) cfg.mask_dir = sibling.string();
}
// The mapper's bundle adjustment needs one of three scalar configurations,
// each with its own device requirement (see pickRealForDevice). A device
// that supports none of them -- Intel's UHD 750 has neither fp64 nor int64
// nor a float32 atomic add -- can extract and match perfectly well and then
// fail at the first solve, which is a poor way to spend a minute of someone's
// time. Say so before any of the work starts.
{
const VkDeviceCaps caps = VkContext::probeCaps(cfg.device);
if (!realSupportedByDevice(RealCfg::F64, caps) &&
!realSupportedByDevice(RealCfg::DF64, caps) &&
!realSupportedByDevice(RealCfg::F32, caps)) {
L::fail(Tag::Run, M::run_device_cannot_solve);
return 1;
}
}
fs::path ws(workspace);
fs::create_directories(ws);
const fs::path featdir = ws / "features";
src/i18n/catalog/Sfm.h
-38
View File
@@ -224,44 +224,6 @@ SS_MSG(run_nested_images,
RU("В {0} нет ничего для восстановления, кроме папки images/, поэтому каталог изображений — {1}."),
TR("{0} içinde images/ klasöründen başka yeniden oluşturulacak bir şey yok, bu yüzden görüntü dizini {1}."));
SS_MSG(run_device_cannot_solve,
EN("The selected device can run none of the bundle-adjustment scalar types: "
"it has no fp64, no buffer int64 atomics and no buffer float32 atomic add. "
"Pick another device with --device (`spirula sam devices` lists them)."),
JA("選択したデバイスはバンドル調整のどのスカラー型も実行できません。fp64も、バッファint64アトミックも、"
"バッファfloat32アトミック加算もありません。--device で別のデバイスを選んでください"
"（一覧は `spirula sam devices`）。"),
ZH_HANS("所选设备无法运行任何一种光束法平差标量类型: 它没有 fp64，没有缓冲区 int64 原子操作，"
"也没有缓冲区 float32 原子加。请用 --device 选择别的设备（`spirula sam devices` 可列出）。"),
ZH_HANT("所選裝置無法執行任何一種光束法平差純量型別: 它沒有 fp64，沒有緩衝區 int64 原子操作，"
"也沒有緩衝區 float32 原子加。請用 --device 選擇別的裝置（`spirula sam devices` 可列出）。"),
KO("선택한 장치는 번들 조정의 스칼라 형식을 하나도 실행할 수 없습니다. fp64도, 버퍼 int64 원자 연산도, "
"버퍼 float32 원자 덧셈도 없습니다. --device 로 다른 장치를 고르세요(`spirula sam devices` 로 목록 확인)."),
DE("Das gewählte Gerät beherrscht keinen der Skalartypen der Bündelblockausgleichung: "
"kein fp64, keine int64-Atomics auf Puffern, kein atomares float32-Addieren auf Puffern. "
"Wählen Sie mit --device ein anderes Gerät (`spirula sam devices` listet sie auf)."),
FR("L'appareil choisi ne peut exécuter aucun des types scalaires de l'ajustement de faisceaux : "
"ni fp64, ni atomiques int64 sur tampon, ni addition atomique float32 sur tampon. "
"Choisissez un autre appareil avec --device (`spirula sam devices` les liste)."),
ES("El equipo elegido no puede ejecutar ninguno de los tipos escalares del ajuste de haces: "
"no tiene fp64, ni atómicas int64 en búfer, ni suma atómica float32 en búfer. "
"Elija otro con --device (`spirula sam devices` los enumera)."),
PT("O aparelho escolhido não executa nenhum dos tipos escalares do ajustamento de feixes: "
"não tem fp64, nem atômicas int64 em buffer, nem soma atômica float32 em buffer. "
"Escolha outro com --device (`spirula sam devices` os lista)."),
IT("L'unità scelta non può eseguire nessuno dei tipi scalari del bundle adjustment: "
"niente fp64, niente atomiche int64 su buffer, niente somma atomica float32 su buffer. "
"Ne scelga un'altra con --device (`spirula sam devices` le elenca)."),
NL("Het gekozen apparaat kan geen van de scalaire typen van de bundelaanpassing draaien: "
"geen fp64, geen int64-atomics op buffers, geen atomaire float32-optelling op buffers. "
"Kies met --device een ander apparaat (`spirula sam devices` toont ze)."),
RU("Выбранное устройство не поддерживает ни один скалярный тип уравнивания связок: "
"нет fp64, нет атомарных int64 в буферах, нет атомарного сложения float32 в буферах. "
"Выберите другое устройство через --device (список: `spirula sam devices`)."),
TR("Seçilen aygıt demet düzeltmesinin skaler türlerinden hiçbirini çalıştıramıyor: "
"fp64 yok, tampon int64 atomikleri yok, tampon float32 atomik toplaması yok. "
"--device ile başka bir aygıt seçin (`spirula sam devices` listeler)."));
SS_MSG(device_using,
EN("Using {0}"),
JA("{0} を使用します"),
src/sfm/README.md
+21 -11
View File
@@ -122,22 +122,30 @@ into an unattributable `VK_ERROR_FEATURE_NOT_PRESENT`:
| `double` | fp64 arithmetic + fp64 and fp32 buffer atomic add | NVIDIA |
| `df` | `shaderInt64` + int64 buffer atomics | AMD (both ICDs), Intel Xe/RPL-S, llvmpipe |
| `float` | fp32 buffer atomic add | NVIDIA, AMD |
| `cpu` | nothing: it runs on the host (`sfm/ba/SolverCpu.h`) | anything |
`BundleSolver::init` steps down to the most accurate configuration the device
can run and says so once; ask the solver what it settled on with
`solver.real()` rather than reading `SolverOptions::real` back, because packing
`double` into buffers a `df` kernel reads is silent garbage, not an error. Note
that no AMD part here has an fp64 buffer atomic add, so `double` falls back to
`df` on all of them — accurate to ~48 bits, which is enough for every real
capture (0.01 px of reprojection against fp64) but not for the two deliberately
ill-conditioned convergence checks in `sfm_map_test`, which report rather than
assert when fp64 is missing.
`BundleSolver::init` steps down and says so once; ask the solver what it
settled on with `solver.real()` rather than reading `SolverOptions::real` back,
because packing `double` into buffers a `df` kernel reads is silent garbage,
not an error. The chain is **`double` -> `cpu`**: neither fp32-based
configuration is in it, because neither is a better default than a host solver
that is fp64 throughout — `float` stalls the normal equations above ~1e-7
relative accuracy (above), and `df` buys its ~48-bit accuracy with CAS-loop
atomics and emulated transcendentals. Both remain available on request, and
`df` is the one to ask for on a big GPU without fp64 atomics: it is accurate
enough for every real capture (0.01 px of reprojection against fp64), though
not for the two deliberately ill-conditioned convergence checks in
`sfm_map_test`, which report rather than assert when fp64 is missing. No AMD
part here has an fp64 buffer atomic add, so all of them take the host path
unless `--ba-real df` says otherwise.
Two devices deserve naming:
- **Intel UHD 750 (Gen12, RPL-S desktop)** has *none* of the three: no fp64, no
int64 atomics, no fp32 atomic add. It extracts and matches perfectly well and
then cannot solve, so `sfm auto` refuses up front rather than a minute in.
reaches the solver with nothing to run there, which is what the host solver
exists for; a reconstruction on such a device costs roughly twice the
bundle-adjustment time of an fp64 GPU and nothing else changes.
It also lacks `VK_KHR_shader_integer_dot_product`, which is what the second
build of the matcher (`match_nodot`, same integer result without DP4A) is
for; `SS_SFM_NO_DOT4=1` forces that path on a device that has DP4A.
@@ -169,7 +177,8 @@ geometry/ Essential, Fundamental, Homography, P3P, AbsolutePose,
Triangulation, TwoView, LinAlg
optim/ Ransac LO-RANSAC with MSAC scoring
ba/ Problem (model registry + problem layout), Solver (LM, dense
Cholesky / implicit-Schur PCG), README.md
Cholesky / implicit-Schur PCG), SolverCpu (the same two on the
host, for devices that run neither fp64 nor df), README.md
map/ Mapper, Bundle, CorrespondenceGraph, Merge, Profile,
ModelOps (the passes over a *set* of models: merge validator,
audit, split, fold cut, prune -- shared, owned by neither)
@@ -222,6 +231,7 @@ spirula sfm match feats/ -o matches.bin
spirula sfm map matches.bin feats/ -o sparse/ --images IMAGES/
spirula sfm merge sparse/ -o merged/
spirula sfm ba problem.txt --real df # solver benchmark on a BAL problem
spirula sfm ba sparse/0 --real cpu # ... the same solve, on the host
```
`spirula sfm --help` lists the commands, `spirula sfm <command> --help` (or
src/sfm/SfmConfig.h
+2 -2
View File
@@ -310,9 +310,9 @@ struct SfmConfig {
F(mapper.seed_homography, "seed-homography", CMD_AUTO | CMD_MAP, Tier::Advanced, "mapper", 0, \
0, "", seed_homography) \
F(mapper.ba_real, "ba-real", CMD_AUTO | CMD_MAP, Tier::Advanced, "mapper", 0, 0, \
"float|double|df", ba_real) \
"float|double|df|cpu", ba_real) \
F(mapper.ba_real_coarse, "ba-real-coarse", CMD_AUTO | CMD_MAP, Tier::Advanced, "mapper", 0, 0, \
"float|double|df", ba_real_coarse) \
"float|double|df|cpu", ba_real_coarse) \
F(mapper.ba_solver, "ba-solver", CMD_AUTO | CMD_MAP, Tier::Advanced, "mapper", 0, 0, \
"auto|dense|cg", ba_solver) \
F(mapper.retri_scale, "retri-scale", CMD_AUTO | CMD_MAP, Tier::Advanced, "mapper", 0, 10, "", \
src/sfm/ba/CpuCamera.h
+451
View File
@@ -0,0 +1,451 @@
// Host mirror of the bundle-adjustment device math -- the camera models of
// sfm/shaders/common/camera.slang and the losses of loss.slang -- written once
// over a scalar template, so the residual evaluates in `double` and the
// Jacobian in a dual number. Forward mode rather than the device's reverse
// mode because the widths are small (3 + intrinsics) and it needs no generated
// code; the derivative is the same up to rounding.
#pragma once
#include <cmath>
#include <cstdint>
#include <stdexcept>
#include <string>
namespace bacpu {
using std::atan2;
using std::cos;
using std::exp;
using std::log;
using std::sin;
using std::sqrt;
// ================
// Forward-mode dual number
// ================
template <int N>
struct Jet {
double a;
double d[N];
Jet() = default;
Jet(double x) : a(x) {
for (int i = 0; i < N; i++) d[i] = 0.0;
}
static Jet var(double x, int k) {
Jet j(x);
j.d[k] = 1.0;
return j;
}
};
template <int N> inline Jet<N> operator+(const Jet<N>& x, const Jet<N>& y) {
Jet<N> r;
r.a = x.a + y.a;
for (int i = 0; i < N; i++) r.d[i] = x.d[i] + y.d[i];
return r;
}
template <int N> inline Jet<N> operator-(const Jet<N>& x, const Jet<N>& y) {
Jet<N> r;
r.a = x.a - y.a;
for (int i = 0; i < N; i++) r.d[i] = x.d[i] - y.d[i];
return r;
}
template <int N> inline Jet<N> operator-(const Jet<N>& x) {
Jet<N> r;
r.a = -x.a;
for (int i = 0; i < N; i++) r.d[i] = -x.d[i];
return r;
}
template <int N> inline Jet<N> operator*(const Jet<N>& x, const Jet<N>& y) {
Jet<N> r;
r.a = x.a * y.a;
for (int i = 0; i < N; i++) r.d[i] = x.d[i] * y.a + x.a * y.d[i];
return r;
}
template <int N> inline Jet<N> operator/(const Jet<N>& x, const Jet<N>& y) {
Jet<N> r;
const double inv = 1.0 / y.a;
r.a = x.a * inv;
for (int i = 0; i < N; i++) r.d[i] = (x.d[i] - r.a * y.d[i]) * inv;
return r;
}
template <int N> inline Jet<N> operator*(const Jet<N>& x, double s) {
Jet<N> r;
r.a = x.a * s;
for (int i = 0; i < N; i++) r.d[i] = x.d[i] * s;
return r;
}
template <int N> inline Jet<N> operator*(double s, const Jet<N>& x) { return x * s; }
template <int N> inline Jet<N> operator+(const Jet<N>& x, double s) {
Jet<N> r = x;
r.a += s;
return r;
}
template <int N> inline Jet<N> operator+(double s, const Jet<N>& x) { return x + s; }
template <int N> inline Jet<N> operator-(const Jet<N>& x, double s) { return x + (-s); }
template <int N> inline Jet<N> operator-(double s, const Jet<N>& x) { return (-x) + s; }
template <int N> inline Jet<N> operator/(const Jet<N>& x, double s) { return x * (1.0 / s); }
template <int N> inline Jet<N> sqrt(const Jet<N>& x) {
Jet<N> r;
r.a = std::sqrt(x.a);
const double k = 0.5 / r.a;
for (int i = 0; i < N; i++) r.d[i] = x.d[i] * k;
return r;
}
template <int N> inline Jet<N> exp(const Jet<N>& x) {
Jet<N> r;
r.a = std::exp(x.a);
for (int i = 0; i < N; i++) r.d[i] = x.d[i] * r.a;
return r;
}
template <int N> inline Jet<N> log(const Jet<N>& x) {
Jet<N> r;
r.a = std::log(x.a);
const double k = 1.0 / x.a;
for (int i = 0; i < N; i++) r.d[i] = x.d[i] * k;
return r;
}
template <int N> inline Jet<N> sin(const Jet<N>& x) {
Jet<N> r;
r.a = std::sin(x.a);
const double k = std::cos(x.a);
for (int i = 0; i < N; i++) r.d[i] = x.d[i] * k;
return r;
}
template <int N> inline Jet<N> cos(const Jet<N>& x) {
Jet<N> r;
r.a = std::cos(x.a);
const double k = -std::sin(x.a);
for (int i = 0; i < N; i++) r.d[i] = x.d[i] * k;
return r;
}
// Matches rAtan2's custom derivative in common/real.slang, which is the exact
// one rather than a differentiation of its Newton loop.
template <int N> inline Jet<N> atan2(const Jet<N>& y, const Jet<N>& x) {
Jet<N> r;
r.a = std::atan2(y.a, x.a);
const double inv = 1.0 / (y.a * y.a + x.a * x.a);
const double ky = x.a * inv, kx = -y.a * inv;
for (int i = 0; i < N; i++) r.d[i] = y.d[i] * ky + x.d[i] * kx;
return r;
}
// ================
// Camera models -- parameter order is packIntrinsics' (sfm/core/Camera.h)
// ================
struct SnavelyModel {
static constexpr int kNumIntr = 3;
template <class T> static void project(const T* c, const T p[3], T out[2]) {
const T& f_log = c[0];
const T& k1 = c[1];
const T& k2 = c[2];
T xp = -p[0] / p[2];
T yp = -p[1] / p[2];
T r2 = xp * xp + yp * yp;
T distortion = T(1.0) + r2 * (k1 + r2 * k2);
T f = exp(f_log) * distortion;
out[0] = f * xp;
out[1] = f * yp;
}
};
struct SnavelyFModel {
static constexpr int kNumIntr = 3;
template <class T> static void project(const T* c, const T p[3], T out[2]) {
const T& f = c[0];
const T& k1 = c[1];
const T& k2 = c[2];
T xp = -p[0] / p[2];
T yp = -p[1] / p[2];
T r2 = xp * xp + yp * yp;
T fd = f * (T(1.0) + r2 * (k1 + r2 * k2));
out[0] = fd * xp;
out[1] = fd * yp;
}
};
struct PinholeRadialModel {
static constexpr int kNumIntr = 5;
template <class T> static void project(const T* c, const T p[3], T out[2]) {
const T& f = c[0];
const T& k1 = c[1];
const T& k2 = c[2];
const T& cx = c[3];
const T& cy = c[4];
T xp = p[0] / p[2];
T yp = p[1] / p[2];
T r2 = xp * xp + yp * yp;
T fd = f * (T(1.0) + r2 * (k1 + r2 * k2));
out[0] = fd * xp + cx;
out[1] = fd * yp + cy;
}
};
struct SimplePinholeModel {
static constexpr int kNumIntr = 3;
template <class T> static void project(const T* c, const T p[3], T out[2]) {
out[0] = c[0] * (p[0] / p[2]) + c[1];
out[1] = c[0] * (p[1] / p[2]) + c[2];
}
};
struct PinholeModel {
static constexpr int kNumIntr = 4;
template <class T> static void project(const T* c, const T p[3], T out[2]) {
out[0] = c[0] * (p[0] / p[2]) + c[2];
out[1] = c[1] * (p[1] / p[2]) + c[3];
}
};
struct OpenCVModel {
static constexpr int kNumIntr = 8;
template <class T> static void project(const T* c, const T p[3], T out[2]) {
const T& fx = c[0];
const T& fy = c[1];
const T& k1 = c[2];
const T& k2 = c[3];
const T& p1 = c[4];
const T& p2 = c[5];
const T& cx = c[6];
const T& cy = c[7];
T xp = p[0] / p[2];
T yp = p[1] / p[2];
T r2 = xp * xp + yp * yp;
T radial = T(1.0) + r2 * (k1 + r2 * k2);
T dx = xp * radial + 2.0 * p1 * xp * yp + p2 * (r2 + 2.0 * xp * xp);
T dy = yp * radial + p1 * (r2 + 2.0 * yp * yp) + 2.0 * p2 * xp * yp;
out[0] = fx * dx + cx;
out[1] = fy * dy + cy;
}
};
struct FisheyeModel {
static constexpr int kNumIntr = 8;
template <class T> static void project(const T* c, const T p[3], T out[2]) {
const T& fx = c[0];
const T& fy = c[1];
const T& k1 = c[2];
const T& k2 = c[3];
const T& k3 = c[4];
const T& k4 = c[5];
const T& cx = c[6];
const T& cy = c[7];
T r = sqrt(p[0] * p[0] + p[1] * p[1] + T(1e-24));
T theta = atan2(r, p[2]);
T t2 = theta * theta;
T thd = theta * (T(1.0) + t2 * (k1 + t2 * (k2 + t2 * (k3 + t2 * k4))));
T scale = thd / r;
out[0] = fx * scale * p[0] + cx;
out[1] = fy * scale * p[1] + cy;
}
};
struct FullOpenCVModel {
static constexpr int kNumIntr = 9;
template <class T> static void project(const T* c, const T p[3], T out[2]) {
const T& fx = c[0];
const T& fy = c[1];
const T& k1 = c[2];
const T& k2 = c[3];
const T& p1 = c[4];
const T& p2 = c[5];
const T& k3 = c[6];
const T& cx = c[7];
const T& cy = c[8];
T xp = p[0] / p[2];
T yp = p[1] / p[2];
T r2 = xp * xp + yp * yp;
T radial = T(1.0) + r2 * (k1 + r2 * (k2 + r2 * k3));
T dx = xp * radial + 2.0 * p1 * xp * yp + p2 * (r2 + 2.0 * xp * xp);
T dy = yp * radial + p1 * (r2 + 2.0 * yp * yp) + 2.0 * p2 * xp * yp;
out[0] = fx * dx + cx;
out[1] = fy * dy + cy;
}
};
struct ThinPrismFisheyeModel {
static constexpr int kNumIntr = 12;
template <class T> static void project(const T* c, const T p[3], T out[2]) {
const T& fx = c[0];
const T& fy = c[1];
const T& k1 = c[2];
const T& k2 = c[3];
const T& p1 = c[4];
const T& p2 = c[5];
const T& k3 = c[6];
const T& k4 = c[7];
const T& sx1 = c[8];
const T& sy1 = c[9];
const T& cx = c[10];
const T& cy = c[11];
T r = sqrt(p[0] * p[0] + p[1] * p[1] + T(1e-24));
T theta = atan2(r, p[2]);
T s = theta / r;
T uf = s * p[0];
T vf = s * p[1];
T rf2 = theta * theta;
T radial = rf2 * (k1 + rf2 * (k2 + rf2 * (k3 + rf2 * k4)));
T du = uf * radial + 2.0 * p1 * uf * vf + p2 * (rf2 + 2.0 * uf * uf) + sx1 * rf2;
T dv = vf * radial + p1 * (rf2 + 2.0 * vf * vf) + 2.0 * p2 * uf * vf + sy1 * rf2;
out[0] = fx * (uf + du) + cx;
out[1] = fy * (vf + dv) + cy;
}
};
struct EquirectModel {
static constexpr int kNumIntr = 2;
template <class T> static void project(const T* c, const T p[3], T out[2]) {
const T& w = c[0];
const T& h = c[1];
T horiz = sqrt(p[0] * p[0] + p[2] * p[2] + T(1e-24));
T theta = atan2(p[0], p[2]);
T phi = atan2(-p[1], horiz);
out[0] = (theta * 0.15915494309189535 + 0.5) * w;
out[1] = (T(0.5) - phi * 0.3183098861837907) * h;
}
};
// Dispatch on the kModels index in sfm/ba/Problem.h.
template <class Fn> inline void withModel(uint32_t model, Fn&& fn) {
switch (model) {
case 0: fn(SnavelyModel{}); return;
case 1: fn(SnavelyFModel{}); return;
case 2: fn(PinholeRadialModel{}); return;
case 3: fn(OpenCVModel{}); return;
case 4: fn(SimplePinholeModel{}); return;
case 5: fn(PinholeModel{}); return;
case 6: fn(FisheyeModel{}); return;
case 7: fn(FullOpenCVModel{}); return;
case 8: fn(ThinPrismFisheyeModel{}); return;
case 9: fn(EquirectModel{}); return;
}
throw std::runtime_error("camera model index outside the registry");
}
// ================
// Robust losses (s is the squared residual norm)
// ================
struct TrivialLoss {
static double cost(double s, double) { return s; }
static double weight(double, double) { return 1.0; }
};
struct HuberLoss {
static double cost(double s, double p) {
double d2 = p * p;
return s <= d2 ? s : 2.0 * p * std::sqrt(s) - d2;
}
static double weight(double s, double p) {
double d2 = p * p;
return s <= d2 ? 1.0 : p / std::sqrt(s);
}
};
struct CauchyLoss {
static double cost(double s, double p) {
double c2 = p * p;
return c2 * std::log(1.0 + s / c2);
}
static double weight(double s, double p) {
double c2 = p * p;
return 1.0 / (1.0 + s / c2);
}
};
template <class Fn> inline void withLoss(const std::string& loss, Fn&& fn) {
if (loss == "huber") fn(HuberLoss{});
else if (loss == "cauchy") fn(CauchyLoss{});
else fn(TrivialLoss{});
}
// ================
// Residual and Jacobian
// ================
// out = R(aa) p, matching camera.slang down to its 1e-15 guard on the norm.
template <class T> inline void angleAxisRotate(const T axis[3], const T p[3], T out[3]) {
T theta = sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]);
T inv = T(1.0) / (theta + T(1e-15));
T a[3] = {axis[0] * inv, axis[1] * inv, axis[2] * inv};
T st = sin(theta), ct = cos(theta);
T dp = a[0] * p[0] + a[1] * p[1] + a[2] * p[2];
T cr[3] = {a[1] * p[2] - a[2] * p[1], a[2] * p[0] - a[0] * p[2], a[0] * p[1] - a[1] * p[0]};
T w = dp * (T(1.0) - ct);
for (int i = 0; i < 3; i++) out[i] = p[i] * ct + cr[i] * st + a[i] * w;
}
template <class M>
inline void residual(const double pose[6], const double* intr, const double X[3],
const double obs[2], double r[2]) {
double p[3], px[2];
angleAxisRotate<double>(pose, X, p);
for (int i = 0; i < 3; i++) p[i] += pose[3 + i];
M::template project<double>(intr, p, px);
r[0] = px[0] - obs[0];
r[1] = px[1] - obs[1];
}
// ct I + st [a]_x + (1-ct) a a^T: what angleAxisRotate applies, and its
// derivative in the point.
inline void angleAxisMatrix(const double axis[3], double R[9]) {
const double th = std::sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]);
const double inv = 1.0 / (th + 1e-15);
const double a0 = axis[0] * inv, a1 = axis[1] * inv, a2 = axis[2] * inv;
const double st = std::sin(th), ct = std::cos(th), w = 1.0 - ct;
R[0] = ct + w * a0 * a0;
R[1] = w * a0 * a1 - st * a2;
R[2] = w * a0 * a2 + st * a1;
R[3] = w * a1 * a0 + st * a2;
R[4] = ct + w * a1 * a1;
R[5] = w * a1 * a2 - st * a0;
R[6] = w * a2 * a0 - st * a1;
R[7] = w * a2 * a1 + st * a0;
R[8] = ct + w * a2 * a2;
}
// Jc is [row][6 pose | kNumIntr intrinsics], Jp is [row][3]. The angle-axis
// block takes a dual pass of its own and the point block comes from the
// rotation matrix: the axis norm has no derivative at a zero rotation (every
// seed pair's first image), and one pass over both would spread that 0/0 out
// of the axis columns the assembly's isfinite guards drop it in.
template <class M>
inline void jacobian(const double pose[6], const double* intr, const double X[3],
const double obs[2], double r[2], double* Jc, double* Jp) {
constexpr int NI = M::kNumIntr;
constexpr int NP = 3 + NI;
constexpr int DOF = 6 + NI;
Jet<3> aa[3], Xj[3], pj[3];
for (int i = 0; i < 3; i++) aa[i] = Jet<3>::var(pose[i], i);
for (int i = 0; i < 3; i++) Xj[i] = Jet<3>(X[i]);
angleAxisRotate<Jet<3>>(aa, Xj, pj);
double R[9];
angleAxisMatrix(pose, R);
Jet<NP> pc[3], ic[NI], px[2];
for (int i = 0; i < 3; i++) pc[i] = Jet<NP>::var(pj[i].a + pose[3 + i], i);
for (int i = 0; i < NI; i++) ic[i] = Jet<NP>::var(intr[i], 3 + i);
M::template project<Jet<NP>>(ic, pc, px);
for (int row = 0; row < 2; row++) {
r[row] = px[row].a - obs[row];
double* jc = Jc + row * DOF;
double* jp = Jp + row * 3;
for (int j = 0; j < 3; j++) {
double da = 0, dx = 0;
for (int k = 0; k < 3; k++) {
da += px[row].d[k] * pj[k].d[j];
dx += px[row].d[k] * R[3 * k + j];
}
jc[j] = da;
jc[3 + j] = px[row].d[j];
jp[j] = dx;
}
for (int i = 0; i < NI; i++) jc[6 + i] = px[row].d[3 + i];
}
}
} // namespace bacpu
src/sfm/ba/CpuDense.h
+260
View File
@@ -0,0 +1,260 @@
// Dense SPD solver for the reduced camera system on the host, in place on the
// same packed lower triangle the GPU path uses (row r starts at r(r+1)/2).
//
// That layout has a per-row stride, so the trailing update copies the current
// block column out to a contiguous panel once per step -- 4n^2 bytes over the
// factorization against n^3/3 flops -- and both operands of every tile update
// are then unit-stride.
#pragma once
#include <algorithm>
#include <cmath>
#include <cstdint>
#include <cstring>
#include <vector>
#include "sfm/ba/CpuParallel.h"
namespace bacpu {
class DenseSpd {
public:
static constexpr uint32_t kBlock = 48;
void init(uint32_t n) {
n_ = n;
a_.assign(elems(n), 0.0);
panel_.resize((size_t)n * kBlock);
panelT_.resize((size_t)n * kBlock);
diag_.resize((size_t)kBlock * kBlock);
}
void release() {
a_ = std::vector<double>();
panel_ = std::vector<double>();
panelT_ = std::vector<double>();
diag_ = std::vector<double>();
}
static uint64_t elems(uint32_t n) { return (uint64_t)n * (n + 1) / 2; }
static uint64_t rowOff(uint32_t r) { return (uint64_t)r * (r + 1) / 2; }
bool allocated() const { return !a_.empty(); }
size_t bytes() const {
return (a_.capacity() + panel_.capacity() + panelT_.capacity() + diag_.capacity()) * 8;
}
double* row(uint32_t r) { return a_.data() + rowOff(r); }
const double* row(uint32_t r) const { return a_.data() + rowOff(r); }
const std::vector<double>& data() const { return a_; }
void zero(Pool& pool, int nthreads) {
const uint64_t n = a_.size();
const int nt = taskCount((int64_t)n, 1 << 22, nthreads);
pool.run(nt, nthreads, [&](int t, int) {
int64_t lo, hi;
taskRange((int64_t)n, nt, t, lo, hi);
if (hi > lo) std::memset(a_.data() + lo, 0, (size_t)(hi - lo) * sizeof(double));
});
}
// Factor in place, then solve L L^T x = g leaving x in g.
void factorSolve(double* g, Pool& pool, int nthreads) {
factor(pool, nthreads);
solveInPlace(g, pool, nthreads);
}
void factor(Pool& pool, int nthreads) {
const uint32_t nb = (n_ + kBlock - 1) / kBlock;
for (uint32_t k = 0; k < nb; k++) {
const uint32_t base = k * kBlock;
const uint32_t m = std::min(kBlock, n_ - base);
factorDiag(base, m);
const uint32_t rest = n_ - base - m;
if (!rest) break;
for (uint32_t r = 0; r < m; r++)
std::memcpy(&diag_[(size_t)r * m], row(base + r) + base, (r + 1) * sizeof(double));
const uint32_t rbase = base + m;
{
const int nt = taskCount(rest, 64, nthreads);
pool.run(nt, nthreads, [&](int t, int) {
int64_t lo, hi;
taskRange(rest, nt, t, lo, hi);
for (int64_t i = lo; i < hi; i++) trsmRow(row((uint32_t)(rbase + i)) + base, m);
});
}
const uint32_t tb = (rest + kBlock - 1) / kBlock;
{
const int nt = taskCount(rest, 64, nthreads);
pool.run(nt, nthreads, [&](int t, int) {
int64_t lo, hi;
taskRange(rest, nt, t, lo, hi);
for (int64_t i = lo; i < hi; i++)
std::memcpy(&panel_[(size_t)i * m], row((uint32_t)(rbase + i)) + base,
m * sizeof(double));
});
pool.run((int)tb, nthreads, [&](int j, int) {
const uint32_t c0 = (uint32_t)j * kBlock;
const uint32_t nc = std::min(kBlock, rest - c0);
double* bt = &panelT_[(size_t)j * kBlock * kBlock];
for (uint32_t mm = 0; mm < m; mm++)
for (uint32_t c = 0; c < nc; c++)
bt[(size_t)mm * kBlock + c] = panel_[(size_t)(c0 + c) * m + mm];
});
}
const int ntiles = (int)(tb * (tb + 1) / 2);
pool.run(ntiles, nthreads, [&](int t, int) {
uint32_t i = (uint32_t)((std::sqrt(8.0 * t + 1.0) - 1.0) * 0.5);
while ((uint64_t)(i + 1) * (i + 2) / 2 <= (uint64_t)t) i++;
while ((uint64_t)i * (i + 1) / 2 > (uint64_t)t) i--;
const uint32_t j = (uint32_t)(t - (int)(i * (i + 1) / 2));
updateTile(rbase, i, j, m, rest);
});
}
}
private:
// Right-looking factorization of one diagonal block, guarded exactly as
// chol_diag in sfm/shaders/ba/cholesky.slang.
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));
Rj[j] = d;
const double inv = 1.0 / d;
for (uint32_t r = j + 1; r < m; r++) {
double* Rr = row(base + r) + base;
double f = Rr[j] * inv;
Rr[j] = f;
for (uint32_t c = j + 1; c <= r; c++) Rr[c] -= f * row(base + c)[base + j];
}
}
}
// One row of L_ik = A_ik L_kk^-T.
void trsmRow(double* a, uint32_t m) {
for (uint32_t j = 0; j < m; j++) {
const double* Lj = &diag_[(size_t)j * m];
double v = a[j];
for (uint32_t mm = 0; mm < j; mm++) v -= a[mm] * Lj[mm];
a[j] = v / Lj[j];
}
}
// A_ij -= L_ik L_jk^T for one tile of the trailing submatrix.
void updateTile(uint32_t rbase, uint32_t i, uint32_t j, uint32_t m, uint32_t rest) {
const uint32_t r0 = i * kBlock, c0 = j * kBlock;
const uint32_t mr = std::min(kBlock, rest - r0), nc = std::min(kBlock, rest - c0);
const double* A = &panel_[(size_t)r0 * m];
const double* Bt = &panelT_[(size_t)j * kBlock * kBlock];
const uint32_t gi = rbase + r0, gj = rbase + c0;
if (i == j) { // symmetric: lower triangle of the tile only
for (uint32_t r = 0; r < mr; r++) {
double* C = row(gi + r) + gj;
const double* Ar = A + (size_t)r * m;
for (uint32_t c = 0; c <= r; c++) {
const double* Ac = A + (size_t)c * m;
double acc = 0;
for (uint32_t mm = 0; mm < m; mm++) acc += Ar[mm] * Ac[mm];
C[c] -= acc;
}
}
return;
}
// 2x8 accumulators fill the baseline build's 16 SSE2 registers exactly,
// and measured best of the shapes tried: 132 GFLOP/s at n=6102 on 32
// threads, against 108 for 4x4 and 90 for 6x4.
constexpr uint32_t MR = 2, NR = 8;
uint32_t r = 0;
for (; r + MR <= mr; r += MR) {
const double* A0 = A + (size_t)r * m;
uint32_t c = 0;
for (; c + NR <= nc; c += NR) {
double acc[MR][NR] = {};
for (uint32_t mm = 0; mm < m; mm++) {
const double* b = Bt + (size_t)mm * kBlock + c;
for (uint32_t p = 0; p < MR; p++) {
const double a = A0[p * m + mm];
for (uint32_t q = 0; q < NR; q++) acc[p][q] += a * b[q];
}
}
for (uint32_t p = 0; p < MR; p++) {
double* C = row(gi + r + p) + gj;
for (uint32_t q = 0; q < NR; q++) C[c + q] -= acc[p][q];
}
}
for (; c < nc; c++) {
const double* Bc = &panel_[(size_t)(c0 + c) * m];
for (uint32_t p = 0; p < MR; p++) {
double a = 0;
for (uint32_t mm = 0; mm < m; mm++) a += A0[p * m + mm] * Bc[mm];
row(gi + r + p)[gj + c] -= a;
}
}
}
for (; r < mr; r++) {
const double* Ar = A + (size_t)r * m;
double* C = row(gi + r) + gj;
for (uint32_t c = 0; c < nc; c++) {
const double* Bc = &panel_[(size_t)(c0 + c) * m];
double acc = 0;
for (uint32_t mm = 0; mm < m; mm++) acc += Ar[mm] * Bc[mm];
C[c] -= acc;
}
}
}
void solveInPlace(double* g, Pool& pool, int nthreads) {
const uint32_t nb = (n_ + kBlock - 1) / kBlock;
for (uint32_t k = 0; k < nb; k++) {
const uint32_t base = k * kBlock, m = std::min(kBlock, n_ - base);
for (uint32_t r = 0; r < m; r++) {
const double* L = row(base + r) + base;
double v = g[base + r];
for (uint32_t j = 0; j < r; j++) v -= L[j] * g[base + j];
g[base + r] = v / L[r];
}
const uint32_t rest = n_ - base - m;
if (!rest) continue;
const int nt = taskCount(rest, 256, nthreads);
pool.run(nt, nthreads, [&](int t, int) {
int64_t lo, hi;
taskRange(rest, nt, t, lo, hi);
for (int64_t i = lo; i < hi; i++) {
const double* L = row((uint32_t)(base + m + i)) + base;
double acc = 0;
for (uint32_t j = 0; j < m; j++) acc += L[j] * g[base + j];
g[base + m + i] -= acc;
}
});
}
for (int kk = (int)nb - 1; kk >= 0; kk--) {
const uint32_t base = (uint32_t)kk * kBlock, m = std::min(kBlock, n_ - base);
for (int r = (int)m - 1; r >= 0; r--) {
double v = g[base + r];
for (uint32_t j = (uint32_t)r + 1; j < m; j++)
v -= row(base + j)[base + r] * g[base + j];
g[base + r] = v / row(base + r)[base + r];
}
if (!base) continue;
const int nt = taskCount(base, 256, nthreads);
pool.run(nt, nthreads, [&](int t, int) {
int64_t lo, hi;
taskRange(base, nt, t, lo, hi);
for (uint32_t j = 0; j < m; j++) {
const double* L = row(base + j);
const double x = g[base + j];
for (int64_t i = lo; i < hi; i++) g[i] -= L[i] * x;
}
});
}
}
std::vector<double> a_, panel_, panelT_, diag_;
uint32_t n_ = 0;
};
} // namespace bacpu
src/sfm/ba/CpuParallel.h
+145
View File
@@ -0,0 +1,145 @@
// Worker pool for the CPU bundle adjustment: one for the process, not one per
// solver. The bottom-up phase runs a mapper (and its solves) per atom worker,
// and a pool inside each would oversubscribe the machine by that factor. So
// parallel regions serialize against each other, which is what keeps them at
// full width, and small solves stay inline instead of entering the pool.
#pragma once
#include <atomic>
#include <condition_variable>
#include <cstdint>
#include <cstdlib>
#include <mutex>
#include <thread>
#include <utility>
#include <vector>
#include "core/Env.h"
namespace bacpu {
class Pool {
public:
// Machine-wide (SS_SFM_BA_THREADS overrides); a solve that wants fewer
// threads caps its task count instead. Sizing this from the first caller
// would leave every later solve as narrow as an atom worker's threads=1.
static Pool& get() {
static Pool p;
return p;
}
int size() const { return (int)workers_.size() + 1; }
// fn(task, tid) for task in [0, ntasks), handed out dynamically over at
// most `maxWorkers` threads. Anything reduced afterwards must be keyed on
// `task` rather than `tid`, or the result depends on the scheduling.
template <class F>
void run(int ntasks, int maxWorkers, F&& fn) {
if (ntasks <= 0) return;
if (workers_.empty() || ntasks == 1 || maxWorkers <= 1) {
for (int t = 0; t < ntasks; t++) fn(t, 0);
return;
}
Job<F> job{std::forward<F>(fn)};
dispatch(ntasks, maxWorkers, job);
}
template <class F>
void run(int ntasks, F&& fn) {
run(ntasks, size(), std::forward<F>(fn));
}
private:
struct AnyJob {
virtual void call(int task, int tid) const = 0;
virtual ~AnyJob() = default;
};
template <class F>
struct Job : AnyJob {
F f;
explicit Job(F&& fn) : f(std::forward<F>(fn)) {}
void call(int task, int tid) const override { f(task, tid); }
};
Pool() {
int threads = 0;
if (const char* e = spirula::env("SFM_BA_THREADS")) threads = atoi(e);
unsigned hc = std::thread::hardware_concurrency();
int n = threads > 0 ? threads : (int)(hc ? hc : 1u);
workers_.reserve((size_t)n - 1);
for (int i = 1; i < n; i++) workers_.emplace_back([this, i] { loop(i); });
}
~Pool() {
{
std::lock_guard<std::mutex> lk(mu_);
stop_ = true;
}
cv_.notify_all();
for (std::thread& t : workers_) t.join();
}
void dispatch(int ntasks, int maxWorkers, const AnyJob& job) {
std::lock_guard<std::mutex> region(region_); // one parallel region at a time
{
std::lock_guard<std::mutex> lk(mu_);
job_ = &job;
ntasks_ = ntasks;
limit_ = maxWorkers;
next_.store(0, std::memory_order_relaxed);
done_ = 0;
gen_++;
}
cv_.notify_all();
drain(0);
std::unique_lock<std::mutex> lk(mu_);
cv_done_.wait(lk, [&] { return done_ == (int)workers_.size(); });
job_ = nullptr;
}
void drain(int tid) {
if (tid >= limit_) return;
for (int t = next_.fetch_add(1, std::memory_order_relaxed); t < ntasks_;
t = next_.fetch_add(1, std::memory_order_relaxed))
job_->call(t, tid);
}
void loop(int tid) {
uint64_t seen = 0;
for (;;) {
std::unique_lock<std::mutex> lk(mu_);
cv_.wait(lk, [&] { return stop_ || gen_ != seen; });
if (stop_) return;
seen = gen_;
lk.unlock();
drain(tid);
lk.lock();
if (++done_ == (int)workers_.size()) cv_done_.notify_one();
}
}
std::vector<std::thread> workers_;
std::mutex region_, mu_;
std::condition_variable cv_, cv_done_;
const AnyJob* job_ = nullptr;
std::atomic<int> next_{0};
int ntasks_ = 0, done_ = 0, limit_ = 0;
uint64_t gen_ = 0;
bool stop_ = false;
};
// Split [0, n) into `ntasks` contiguous pieces; piece `t` is [lo, hi).
inline void taskRange(int64_t n, int ntasks, int t, int64_t& lo, int64_t& hi) {
int64_t q = n / ntasks, r = n % ntasks;
lo = q * t + (t < r ? t : r);
hi = lo + q + (t < r ? 1 : 0);
}
// `n` items at `grain` apiece, capped by the pool. One task runs inline, which
// is what keeps a forty-image solve off the pool entirely.
inline int taskCount(int64_t n, int64_t grain, int nthreads) {
if (n <= grain || nthreads <= 1) return 1;
int64_t k = (n + grain - 1) / grain;
return (int)(k < nthreads ? k : nthreads);
}
} // namespace bacpu
src/sfm/ba/Options.h
+116
View File
@@ -0,0 +1,116 @@
// Solver-facing configuration shared by the GPU driver (sfm/ba/Solver.h) and
// the host fallback (sfm/ba/SolverCpu.h).
#pragma once
#include <cstring>
#include <stdexcept>
#include <string>
#include <vector>
// The arithmetic the solver runs in. `CPU` is double precision on the host, for
// devices that can run none of the kernels (see realSupportedByDevice).
enum class RealCfg { F32, F64, DF64, CPU };
inline RealCfg realCfgFromName(const std::string& s) {
return s == "float" ? RealCfg::F32
: s == "df" ? RealCfg::DF64
: s == "cpu" ? RealCfg::CPU
: RealCfg::F64;
}
inline const char* realCfgName(RealCfg c) {
switch (c) {
case RealCfg::F32: return "float";
case RealCfg::F64: return "double";
case RealCfg::CPU: return "cpu";
default: return "df";
}
}
inline size_t realSize(RealCfg c) { return c == RealCfg::F32 ? 4 : 8; }
inline void packReals(std::vector<uint8_t>& out, const double* v, size_t n, RealCfg cfg) {
out.resize(n * realSize(cfg));
if (cfg == RealCfg::F32) {
float* p = (float*)out.data();
for (size_t i = 0; i < n; i++) p[i] = (float)v[i];
} else if (cfg == RealCfg::DF64) {
float* p = (float*)out.data();
for (size_t i = 0; i < n; i++) {
float hi = (float)v[i];
p[2 * i] = hi;
p[2 * i + 1] = (float)(v[i] - hi);
}
} else {
memcpy(out.data(), v, n * 8);
}
}
inline void unpackReals(std::vector<double>& out, const uint8_t* v, size_t n, RealCfg cfg) {
out.resize(n);
if (cfg == RealCfg::F32) {
const float* p = (const float*)v;
for (size_t i = 0; i < n; i++) out[i] = p[i];
} else if (cfg == RealCfg::DF64) {
const float* p = (const float*)v;
for (size_t i = 0; i < n; i++) out[i] = (double)p[2 * i] + (double)p[2 * i + 1];
} else {
memcpy(out.data(), v, n * 8);
}
}
enum class SolverSel { Auto, Dense, CG };
enum class CgFallback { Auto, On, Off };
// Raised, before anything is allocated, when the chosen path does not fit the
// memory budget and the caller asked to be told rather than to find out from
// the driver. There is nothing below CG to fall back to -- its footprint is the
// problem data plus a few vectors -- so the only answer is a smaller problem,
// and only the caller knows how to make one (Mapper::jointRefine splits its
// models into batches).
struct BAOverBudget : std::runtime_error {
BAOverBudget(double need, double budget)
: std::runtime_error("bundle adjustment needs more device memory than the budget allows"),
need_mb(need), budget_mb(budget) {}
double need_mb, budget_mb;
};
struct SolverOptions {
RealCfg real = RealCfg::F64;
float loss_param = 1.0f; // Huber delta / Cauchy c (unused by trivial loss)
int max_iters = 50;
double init_damping = 1e-2;
double rtol = 1e-6;
int patience = 10;
SolverSel solver = SolverSel::Auto;
double vram_budget_mb = 0; // 0 = 90% of the device-local heap (host: half the RAM)
// Throw BAOverBudget instead of warning and trying anyway. For a caller
// that can split the problem; the default keeps the old behaviour, since a
// caller that cannot split is better served by an attempt than by a refusal.
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
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
// (a hand-compiled shader, for iteration without relinking).
std::string loss = "trivial";
std::string spv_path;
int device = -1;
// Host worker threads for the CPU path; 0 = every core. Caps the tasks one
// solve splits into, not the shared pool's width (bacpu::Pool).
int threads = 0;
bool validate = false;
bool verbose = true;
bool profile = false;
};
struct SolverStats {
double initial_cost = 0, final_cost = 0;
int iterations = 0, accepted = 0;
double solve_seconds = 0;
double vram_mb = 0; // host RAM on the CPU path
const char* solver = "dense";
double cg_iters_total = 0; // CG iterations summed over LM solves
int cg_solves = 0;
int cg_fallbacks = 0; // LM iterations re-solved densely
};
src/sfm/ba/README.md
+71 -3
View File
@@ -6,7 +6,9 @@ Cholesky, or -- for large camera counts -- by a matrix-free implicit-Schur PCG
that never forms the reduced matrix, keeping VRAM linear in the observation
count. Selection is automatic by problem size and VRAM budget (`--solver` to
force). All GPU code is [Slang](https://shader-slang.org/) compiled to SPIR-V;
Jacobians come from Slang's autodiff.
Jacobians come from Slang's autodiff. The same solver also exists on the host,
for devices that can run none of the scalar configurations -- see "Host
fallback".
This is the last stage of the SfM pipeline and the one it was built around; the
mapper drives it through `sfm/map/Bundle.h`. For the pipeline as a whole see
@@ -28,7 +30,12 @@ sfm/shaders/
sfm/ba/
Problem.h model registry, camera groups, column layout, per-model obs lists,
BAL problem loading
Options.h scalar config, solver selection, options and stats
Solver.h LM driver (records one command buffer per iteration)
SolverCpu.h the same solver on the host -- see "Host fallback" below
CpuCamera.h host mirror of the camera and loss models, forward-mode duals
CpuDense.h packed blocked Cholesky for the host path
CpuParallel.h the process-wide worker pool the host path runs on
src/app/cli/sfm_ba.cpp the `spirula sfm ba` subcommand: BAL problems + PLY dump
```
@@ -44,7 +51,9 @@ bash build_develop.bash -DSS_BACKEND=vulkan -DSS_BUILD_CLI=ON
./build/spirula sfm ba /path/to/bal/problem-16-22106-pre.txt --real double
./build/spirula sfm ba problem.txt --real df --loss huber --loss-param 1.0 --ply out
./build/spirula sfm ba /path/to/sparse/0 -o refined/ # a COLMAP model
./build/spirula sfm ba /path/to/sparse/0 --real cpu # ... on the host
./build/sfm_cholesky_test 500 --real df # dense solver unit test
./build/sfm_ba_cpu_test # host solver vs a written-out reference
```
Given a *directory* rather than a BAL file, `ba` reads a COLMAP sparse model and
@@ -57,7 +66,7 @@ The shader variant matrix can be trimmed for faster iteration:
`-DSS_SFM_REALS=df -DSS_SFM_LOSSES=trivial`. slangc is taken from PATH
or downloaded into the build tree (`cmake/SsSlang.cmake`).
Options: `--real float|double|df`, `--loss trivial|huber|cauchy`,
Options: `--real float|double|df|cpu`, `--loss trivial|huber|cauchy`,
`--loss-param X`, `--model snavely|snavely_f`, `--shared-intrinsics`,
`--max-iters N`, `--damping X`, `--rtol X`, `--patience N`, `--ply prefix`,
`--solver auto|dense|cg`, `--vram-budget MB`, `--cg-iters N`, `--cg-tol X`,
@@ -151,7 +160,8 @@ line in `sfm/ba/Problem.h`. Different intrinsic counts need no other changes.
### Scalar configs
Everything is written against `Real`, selected per-module with a `-D` flag:
Everything on the device is written against `Real`, selected per-module with a
`-D` flag (`cpu` is not one of them -- it is the host solver, below):
- `float` — fp32 + native f32 buffer atomic add (`VK_EXT_shader_atomic_float`)
- `double` — fp64 + native f64 atomic add. GLSL.std.450 does not define
@@ -268,6 +278,64 @@ 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.
### Host fallback (`--real cpu`)
`pickRealForDevice` steps down `double` -> `cpu`, and `cpu` is the same solver
in fp64 on the host: same parameterization, same LM loop, same two linear
solvers, same automatic selection between them, same `BAOverBudget` contract.
Final costs match the fp64 GPU path to seven digits on real captures, and the
assembled `S` and `g` match an independently written unreduced normal-equation
reference to ~1e-15 (`sfm_ba_cpu_test`).
Neither fp32-based configuration is in that chain. `float` stalls the normal
equations above ~1e-7 relative accuracy, which is looser than the tolerance the
mapper's finishing passes ask for, and `df` pays for its ~48 bits with CAS-loop
atomics and emulated transcendentals. Both are still there to ask for --
`--ba-real df` is the right answer on a large GPU with no fp64 atomics -- but a
host solver that is fp64 throughout is the better default for a device that
cannot run `double`.
Three things differ from the GPU path, all of them consequences of running on
a CPU rather than choices:
- **No pair tables.** The Schur assembly is one task per *image*: for each of
its observations it walks that point's track prefix, so an element of `S`
belongs to the image pair that produced it and the pose rows are written with
no atomics and no aggregation buffer. Columns owned by an intrinsics group
that several images refine are the exception -- every element touching one
lands in an intrinsics *row* after symmetrization -- and those go to a
per-task `(shared columns) x n_dim` buffer that is summed afterwards. That is
~800 kB a task on a real capture, and zero when each image owns its
intrinsics. The GPU's pair-entry tables, which cost 3.6 s and hundreds of MB
to build, are never built.
- **Jacobians by forward-mode duals** (`CpuCamera.h`), against Slang's reverse
mode on the device. The widths are small (3 + intrinsics) and it needs no
generated code. The angle-axis block gets a dual pass of its own and the point
block comes from the rotation matrix, because the axis norm has no derivative
at a zero rotation -- which is every seed pair's first image -- and one pass
over both would spread that 0/0 across the point and translation blocks
instead of leaving it where reverse mode does.
- **A process-wide worker pool** (`CpuParallel.h`), not one per solver: the
bottom-up phase runs a mapper per atom worker and a pool inside each would
oversubscribe the machine by that factor. Solves too small to be worth
splitting -- which is every atom -- run inline on the calling thread instead.
`--threads` caps the tasks one solve splits into; `SS_SFM_BA_THREADS`
overrides the pool's own width.
The dense factorization is `CpuDense.h`: the same packed lower triangle, a
blocked right-looking Cholesky (block 48) whose trailing update copies the
current block column out to a contiguous panel once per step, so both operands
of every tile update are unit-stride. 132 GFLOP/s at `n_dim = 6102` on an
i9-14900HX (32 threads), 17.6 single-threaded, which is ~88% of the SSE2
baseline's peak; `-march=native` measured 1.27x on top and is deliberately not
taken, since nothing else in the tree is built for it.
Measured against the fp64 GPU path on an RTX 5070 Laptop, same machine, one
global BA: a 152-image capture 1.85 s against 0.81 s, a 1015-image one
(`n_dim = 6102`) 3.6 s against 2.4 s, both at identical final cost. Peak
memory is slightly *below* the GPU path's, which uploads a copy of the problem
tables. `SS_SFM_MAP_PROF=1` prints the per-phase breakdown.
### LM loop
Multiplicative damping `diag *= (1+λ)` on both the camera and point blocks;
src/sfm/ba/Solver.h
+49 -115
View File
@@ -12,6 +12,10 @@
// or forced with SolverOptions::solver. Optionally the dense machinery is
// kept allocated as a fallback: if CG fails to converge, the iteration is
// re-solved densely (assembly reuse, like a rejected step).
//
// A device that can run none of the scalar configurations gets `RealCfg::CPU`,
// and every entry point below delegates to bacpu::Solver -- the same LM loop
// and the same two linear solvers, on the host.
#pragma once
#include <chrono>
@@ -25,31 +29,19 @@
#include <memory>
#include <vector>
#include "sfm/ba/Options.h"
#include "sfm/ba/Problem.h"
#include "sfm/ba/SolverCpu.h"
#include "sfm/vk/EmbeddedSpirv.h"
#include "sfm/vk/VkContext.h"
#include "core/Env.h"
enum class RealCfg { F32, F64, DF64 };
inline RealCfg realCfgFromName(const std::string& s) {
return s == "float" ? RealCfg::F32 : s == "df" ? RealCfg::DF64 : RealCfg::F64;
}
inline const char* realCfgName(RealCfg c) {
switch (c) {
case RealCfg::F32: return "float";
case RealCfg::F64: return "double";
default: return "df";
}
}
inline size_t realSize(RealCfg c) { return c == RealCfg::F32 ? 4 : 8; }
// Can this device run the kernels compiled for `c`?
// double - fp64 arithmetic, and an fp64 atomic add for the reductions
// float - an fp32 atomic add
// df - neither; the emulated double-float pair reduces through int64
// atomics, which is why it is the fallback and not a last resort
// atomics
// cpu - nothing at all; it runs on the host (sfm/ba/SolverCpu.h)
inline bool realSupportedByDevice(RealCfg c, const VkDeviceCaps& caps) {
switch (c) {
case RealCfg::F64:
@@ -58,19 +50,20 @@ inline bool realSupportedByDevice(RealCfg c, const VkDeviceCaps& caps) {
return caps.float32AtomicAdd;
case RealCfg::DF64:
return caps.int64Atomics;
case RealCfg::CPU:
return true;
}
return false;
}
// The closest thing to `want` the device can actually run, most accurate
// first. Returns `want` unchanged when nothing fits, so device creation can
// report the missing feature by name rather than this silently picking a
// configuration that fails the same way.
// The closest thing to `want` the device can run: fp64, else the host. Neither
// fp32-based configuration is in the chain -- `float` stalls the normal
// equations above ~1e-7 relative accuracy and `df` buys its ~48 bits with
// CAS-loop atomics; both stay available on request (../README.md).
inline RealCfg pickRealForDevice(RealCfg want, const VkDeviceCaps& caps) {
if (realSupportedByDevice(want, caps)) return want;
for (RealCfg c : {RealCfg::F64, RealCfg::DF64, RealCfg::F32})
if (realSupportedByDevice(c, caps)) return c;
return want;
if (realSupportedByDevice(RealCfg::F64, caps)) return RealCfg::F64;
return RealCfg::CPU;
}
// Probing means an instance + an enumeration, and the mapper's scoped solves
@@ -86,90 +79,6 @@ inline const VkDeviceCaps& cachedDeviceCaps(int deviceIndex) {
return it->second;
}
inline void packReals(std::vector<uint8_t>& out, const double* v, size_t n, RealCfg cfg) {
out.resize(n * realSize(cfg));
if (cfg == RealCfg::F32) {
float* p = (float*)out.data();
for (size_t i = 0; i < n; i++) p[i] = (float)v[i];
} else if (cfg == RealCfg::F64) {
memcpy(out.data(), v, n * 8);
} else {
float* p = (float*)out.data();
for (size_t i = 0; i < n; i++) {
float hi = (float)v[i];
p[2 * i] = hi;
p[2 * i + 1] = (float)(v[i] - hi);
}
}
}
inline void unpackReals(std::vector<double>& out, const uint8_t* v, size_t n, RealCfg cfg) {
out.resize(n);
if (cfg == RealCfg::F32) {
const float* p = (const float*)v;
for (size_t i = 0; i < n; i++) out[i] = p[i];
} else if (cfg == RealCfg::F64) {
memcpy(out.data(), v, n * 8);
} else {
const float* p = (const float*)v;
for (size_t i = 0; i < n; i++) out[i] = (double)p[2 * i] + (double)p[2 * i + 1];
}
}
enum class SolverSel { Auto, Dense, CG };
enum class CgFallback { Auto, On, Off };
// Raised, before anything is allocated, when the chosen path does not fit the
// VRAM budget and the caller asked to be told rather than to find out from the
// driver. There is nothing below CG to fall back to -- its footprint is the
// problem data plus a few vectors -- so the only answer is a smaller problem,
// and only the caller knows how to make one (Mapper::jointRefine splits its
// models into batches).
struct BAOverBudget : std::runtime_error {
BAOverBudget(double need, double budget)
: std::runtime_error("bundle adjustment needs more device memory than the budget allows"),
need_mb(need), budget_mb(budget) {}
double need_mb, budget_mb;
};
struct SolverOptions {
RealCfg real = RealCfg::F64;
float loss_param = 1.0f; // Huber delta / Cauchy c (unused by trivial loss)
int max_iters = 50;
double init_damping = 1e-2;
double rtol = 1e-6;
int patience = 10;
SolverSel solver = SolverSel::Auto;
double vram_budget_mb = 0; // 0 = 90% of the device-local heap
// Throw BAOverBudget instead of warning and trying anyway. For a caller
// that can split the problem; the default keeps the old behaviour, since a
// caller that cannot split is better served by an attempt than by a refusal.
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
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
// (a hand-compiled shader, for iteration without relinking).
std::string loss = "trivial";
std::string spv_path;
int device = -1;
bool validate = false;
bool verbose = true;
bool profile = false;
};
struct SolverStats {
double initial_cost = 0, final_cost = 0;
int iterations = 0, accepted = 0;
double solve_seconds = 0;
double vram_mb = 0;
const char* solver = "dense";
double cg_iters_total = 0; // CG iterations summed over LM solves
int cg_solves = 0;
int cg_fallbacks = 0; // LM iterations re-solved densely
};
class BundleSolver {
enum class LinSolve { DenseObs, DensePair, CG };
static constexpr uint32_t kCamBlk = kMaxCamDof * (kMaxCamDof + 1) / 2; // matches cg.slang
@@ -200,13 +109,11 @@ public:
prof_t0 = t1;
return dt;
};
// Not every device runs every scalar type, and the gap does not follow
// "bigger GPU, more features": Intel's Xe iGPU has int64 buffer atomics
// but no fp64 and no fp32 atomic add, so `df` is the only configuration
// it can run. Fall back rather than fail -- an unsupported request used
// to surface as VK_ERROR_FEATURE_NOT_PRESENT from vkCreateDevice, which
// killed the run at its first bundle adjustment.
{
// Fall back rather than fail: an unsupported request used to surface as
// VK_ERROR_FEATURE_NOT_PRESENT from vkCreateDevice, which killed the
// run at its first bundle adjustment. No AMD part has an fp64 buffer
// atomic add, so this is the ordinary path, not a corner.
if (opt_.real != RealCfg::CPU) {
const VkDeviceCaps& caps = ctx_.initialized()
? ctx_.caps()
: cachedDeviceCaps(opt_.device);
@@ -230,6 +137,11 @@ public:
opt_.real = real;
}
}
if (opt_.real == RealCfg::CPU) {
cpu_.reset(new bacpu::Solver(P_, opt_));
cpu_->init();
return;
}
VkContextOptions vopt;
vopt.needFloat64 = opt_.real == RealCfg::F64;
vopt.needFloatAtomics = opt_.real != RealCfg::DF64;
@@ -446,7 +358,10 @@ public:
P_.n_dim, stats_.solver, stats_.vram_mb);
}
// The host solver works on the problem's own parameter vectors, so upload
// and download are the identity there.
void uploadParams() {
if (cpu_) return;
std::vector<uint8_t> poses, intr, points;
packReals(poses, P_.poses.data(), P_.poses.size(), opt_.real);
packReals(intr, P_.intr.data(), P_.intr.size(), opt_.real);
@@ -460,6 +375,7 @@ public:
}
void downloadParams() {
if (cpu_) return;
std::vector<uint8_t> tmp(std::max({bPoses_.size, bIntr_.size, bPoints_.size}));
ctx_.download(bPoses_, tmp.data(), P_.poses.size() * realSize(opt_.real));
unpackReals(P_.poses, tmp.data(), P_.poses.size(), opt_.real);
@@ -470,6 +386,7 @@ public:
}
double computeCost() {
if (cpu_) return cpu_->computeCost();
VkCommandBuffer cb = ctx_.begin();
recordCost(cb);
ctx_.barrier(cb);
@@ -479,6 +396,7 @@ public:
}
void solve() {
if (cpu_) return cpu_->solve();
auto t0 = std::chrono::high_resolution_clock::now();
double damping = opt_.init_damping;
double cost = computeCost();
@@ -580,26 +498,41 @@ public:
ctx_.printProfile();
}
const SolverStats& stats() const { return stats_; }
const SolverStats& stats() const { return cpu_ ? cpu_->stats() : stats_; }
// The scalar type actually in use, which init() may have stepped down from
// what SolverOptions asked for (see pickRealForDevice). Anything that packs
// or unpacks solver buffers from outside has to ask -- packing `double`
// into buffers a `df` kernel reads gives silent garbage, not an error.
RealCfg real() const { return opt_.real; }
// GPU-path handles; the host solver has no context and no device buffers.
VkContext& ctx() { return ctx_; }
GpuBuffer& bufS() { return bS_; }
GpuBuffer& bufG() { return bG_; }
// debug: run one full assembly (no factor/solve) so S and g can be dumped
void debugAssemble(float damping) {
if (cpu_) return cpu_->assembleOnly(damping);
VkCommandBuffer cb = ctx_.begin();
recordAssembly(cb, damping, false, densePath_);
ctx_.submit(cb);
}
// debug: the assembled S (packed lower triangle) and g, from whichever path
// ran, as doubles
std::vector<double> debugPackedS() {
if (cpu_) return cpu_->packedS();
std::vector<uint8_t> raw(bS_.size);
ctx_.download(bS_, raw.data(), bS_.size);
std::vector<double> v;
unpackReals(v, raw.data(), (uint64_t)P_.n_dim * (P_.n_dim + 1) / 2, opt_.real);
return v;
}
std::vector<double> debugG() { return cpu_ ? cpu_->gradient() : downloadG(); }
// debug: solve the same assembly with both paths and report the step
// difference (requires the cg+fallback configuration)
double debugCompareStep(float damping) {
if (cpu_) return cpu_->compareStep(damping);
if (!useCG_ || !haveFallback_)
throw std::runtime_error("step comparison needs --solver cg + fallback on");
VkCommandBuffer cb = ctx_.begin();
@@ -1043,6 +976,7 @@ private:
BAProblem& P_;
SolverOptions opt_;
std::unique_ptr<bacpu::Solver> cpu_; // non-null when running on the host
const char* schurSuffix_ = "_c"; // "_c"/"_w" dof tier for the Schur kernels
SolverStats stats_;
std::unique_ptr<VkContext> owned_; // null when running on a shared context
src/sfm/ba/SolverCpu.h
+1064
View File
File diff suppressed because it is too large Load Diff
src/sfm/core/HostMemory.h
+27
View File
@@ -0,0 +1,27 @@
#pragma once
// How much physical memory the machine has, or 0 when the platform will not
// say. Callers that need a working number must supply their own fallback.
#include <cstddef>
#if defined(_WIN32)
#include <windows.h>
#else
#include <unistd.h>
#endif
namespace sfm {
inline size_t physicalRamBytes() {
#if defined(_WIN32)
MEMORYSTATUSEX st{};
st.dwLength = sizeof(st);
if (GlobalMemoryStatusEx(&st)) return (size_t)st.ullTotalPhys;
#elif defined(_SC_PHYS_PAGES) && defined(_SC_PAGESIZE)
long pages = sysconf(_SC_PHYS_PAGES), page = sysconf(_SC_PAGESIZE);
if (pages > 0 && page > 0) return (size_t)pages * (size_t)page;
#endif
return 0;
}
} // namespace sfm
src/sfm/core/ImageLoader.h
+2 -15
View File
@@ -45,14 +45,9 @@
#include <utility>
#include <vector>
#include "sfm/core/HostMemory.h"
#include "sfm/core/Image.h"
#if defined(_WIN32)
#include <windows.h>
#else
#include <unistd.h>
#endif
namespace sfm {
struct ImageLoadOptions {
@@ -84,15 +79,7 @@ struct ImageLoadOptions {
// very files being decoded. Falls back to the historical 1 GiB when the
// platform will not say how much memory it has.
inline size_t defaultDecodeBudget() {
size_t total = 0;
#if defined(_WIN32)
MEMORYSTATUSEX st{};
st.dwLength = sizeof(st);
if (GlobalMemoryStatusEx(&st)) total = (size_t)st.ullTotalPhys;
#elif defined(_SC_PHYS_PAGES) && defined(_SC_PAGESIZE)
long pages = sysconf(_SC_PHYS_PAGES), page = sysconf(_SC_PAGESIZE);
if (pages > 0 && page > 0) total = (size_t)pages * (size_t)page;
#endif
const size_t total = physicalRamBytes();
if (total == 0) return 1ull << 30;
return std::min<size_t>(std::max<size_t>(total / 4, 1ull << 30), 8ull << 30);
}
src/sfm/map/Bundle.h
+3
View File
@@ -80,6 +80,8 @@ struct BundleOptions {
// outlive one solve. The caller owns it and must keep (real, loss) fixed
// across calls on the same context. Null = scoped context per call.
VkContext* shared_ctx = nullptr;
// Host worker threads, for the `cpu` scalar; 0 = hardware_concurrency.
int threads = 0;
};
// The problem built from a reconstruction, plus what writing the solution back
@@ -242,6 +244,7 @@ inline SolverOptions bundleSolverOptions(const BundleOptions& bopt) {
if (bopt.solver == "dense") sopt.solver = SolverSel::Dense;
else if (bopt.solver == "cg") sopt.solver = SolverSel::CG;
sopt.over_budget_throws = bopt.over_budget_throws;
sopt.threads = bopt.threads;
return sopt;
}
src/sfm/map/Mapper.h
+4 -1
View File
@@ -320,7 +320,8 @@ struct MapperOptions {
std::string ba_real_coarse = "double";
int device = -1;
// Host worker threads for the passes that fan out over points
// (filterPoints). 0 = hardware_concurrency.
// (filterPoints) and for a bundle adjustment that runs on the host.
// 0 = hardware_concurrency.
int threads = 0;
bool verbose = true;
};
@@ -1235,6 +1236,7 @@ public:
BundleOptions bo;
bo.real = baReal(coarse);
bo.device = opt_.device;
bo.threads = opt_.threads;
bo.verbose = false;
bo.loss = opt_.ba_loss;
bo.loss_param = (float)(opt_.ba_loss_param * medianPixelScale());
@@ -3172,6 +3174,7 @@ private:
BundleOptions bo;
bo.real = realCfgFromName(opt_.ba_real);
bo.device = opt_.device;
bo.threads = opt_.threads;
bo.verbose = false;
bo.loss = opt_.ba_loss;
bo.loss_param = (float)(opt_.ba_loss_param * medianPixelScale());
src/sfm/tests/sfm_ba_cpu_test.cpp
+593
View File
@@ -0,0 +1,593 @@
// The host bundle adjustment (sfm/ba/SolverCpu.h) against independent
// references, on synthetic problems: the dense SPD solver against a textbook
// Cholesky, the analytic Jacobians against central differences, and the whole
// Schur assembly plus parameter update against the unreduced normal equations
// written out in full (dense U/V/W blocks, no packing and no task splitting --
// nothing the solver's own code path shares).
//
// sfm_ba_cpu_test [--quick]
//
// Prints PASS/FAIL per case and returns 0/1. Needs no GPU. See docs/testing.md.
#include <cmath>
#include <cstdio>
#include <cstring>
#include <random>
#include <string>
#include <vector>
#include "sfm/ba/CpuCamera.h"
#include "sfm/ba/CpuDense.h"
#include "sfm/ba/Problem.h"
#include "sfm/ba/SolverCpu.h"
#include "sfm/core/Pose.h"
#include "sfm/tests/TestMain.h"
namespace {
int g_fail = 0;
void report(const char* name, double err, double tol) {
const bool ok = err < tol && std::isfinite(err);
printf("%-34s err %.3e (tol %.0e) %s\n", name, err, tol, ok ? "PASS" : "FAIL");
if (!ok) g_fail++;
}
// ---------------------------------------------------------------------------
// dense SPD factor + solve
// ---------------------------------------------------------------------------
void testChol(uint32_t n) {
std::mt19937 rng(1234);
std::normal_distribution<double> gauss;
std::vector<double> A((size_t)n * n, 0.0), b(n);
{
std::vector<double> M((size_t)n * n);
for (double& v : M) v = gauss(rng);
for (uint32_t i = 0; i < n; i++)
for (uint32_t j = 0; j <= i; j++) {
double s = 0;
for (uint32_t k = 0; k < n; k++) s += M[(size_t)i * n + k] * M[(size_t)j * n + k];
A[(size_t)i * n + j] = A[(size_t)j * n + i] = s + (i == j ? (double)n : 0.0);
}
for (double& v : b) v = gauss(rng);
}
bacpu::DenseSpd S;
S.init(n);
for (uint32_t i = 0; i < n; i++)
for (uint32_t j = 0; j <= i; j++) S.row(i)[j] = A[(size_t)i * n + j];
std::vector<double> x = b;
S.factorSolve(x.data(), bacpu::Pool::get(), bacpu::Pool::get().size());
std::vector<double> L = A; // textbook Cholesky, unblocked
for (uint32_t j = 0; j < n; j++) {
for (uint32_t k = 0; k < j; k++)
for (uint32_t i = j; i < n; i++)
L[(size_t)i * n + j] -= L[(size_t)i * n + k] * L[(size_t)j * n + k];
double d = std::sqrt(L[(size_t)j * n + j]);
for (uint32_t i = j; i < n; i++) L[(size_t)i * n + j] /= d;
}
std::vector<double> y = b;
for (uint32_t i = 0; i < n; i++) {
for (uint32_t j = 0; j < i; j++) y[i] -= L[(size_t)i * n + j] * y[j];
y[i] /= L[(size_t)i * n + i];
}
for (int i = (int)n - 1; i >= 0; i--) {
for (uint32_t j = i + 1; j < n; j++) y[i] -= L[(size_t)j * n + i] * y[j];
y[i] /= L[(size_t)i * n + i];
}
double e = 0, s = 0;
for (uint32_t i = 0; i < n; i++) {
e = std::max(e, std::fabs(x[i] - y[i]));
s = std::max(s, std::fabs(y[i]));
}
char name[64];
snprintf(name, sizeof name, "chol n=%u", n);
report(name, e / s, 1e-10);
}
// ---------------------------------------------------------------------------
// analytic Jacobian vs central differences
// ---------------------------------------------------------------------------
template <class M>
void testJacobianModel(const char* name, const double* intr0, std::mt19937& rng) {
std::normal_distribution<double> gauss;
std::uniform_real_distribution<double> unit(-1.0, 1.0);
double worst = 0;
for (int trial = 0; trial < 20; trial++) {
double pose[6], X[3], obs[2] = {0.3, -0.7};
for (int i = 0; i < 3; i++) pose[i] = 0.3 * unit(rng);
for (int i = 0; i < 3; i++) pose[3 + i] = 0.5 * unit(rng);
for (int i = 0; i < 3; i++) X[i] = unit(rng);
X[2] = 2.0 + unit(rng); // in front of a +z camera
double intr[M::kNumIntr];
for (int i = 0; i < M::kNumIntr; i++) intr[i] = intr0[i] * (1.0 + 0.01 * unit(rng));
double r[2], Jc[2 * (6 + M::kNumIntr)], Jp[6];
bacpu::jacobian<M>(pose, intr, X, obs, r, Jc, Jp);
// central differences over pose, intrinsics and the point
const int DOF = 6 + M::kNumIntr;
for (int k = 0; k < DOF + 3; k++) {
double* p = k < 6 ? &pose[k] : k < DOF ? &intr[k - 6] : &X[k - DOF];
const double h = 1e-6 * std::max(1.0, std::fabs(*p));
const double keep = *p;
double rp[2], rm[2];
*p = keep + h;
bacpu::residual<M>(pose, intr, X, obs, rp);
*p = keep - h;
bacpu::residual<M>(pose, intr, X, obs, rm);
*p = keep;
for (int row = 0; row < 2; row++) {
const double num = (rp[row] - rm[row]) / (2 * h);
const double ana = k < DOF ? Jc[row * DOF + k] : Jp[row * 3 + (k - DOF)];
worst = std::max(worst, std::fabs(num - ana) /
std::max(1.0, std::fabs(num) + std::fabs(ana)));
}
}
}
report(name, worst, 1e-6);
}
// At a zero angle-axis -- the identity every seed pair starts from -- the norm
// has no derivative. That belongs to the axis columns alone.
template <class M>
void testZeroRotation(const char* name, const double* intr) {
double pose[6] = {0, 0, 0, 0.1, -0.2, 0.0};
double X[3] = {0.3, -0.4, 3.0}, obs[2] = {5.0, -2.0};
double r[2], Jc[2 * (6 + M::kNumIntr)], Jp[6];
bacpu::jacobian<M>(pose, intr, X, obs, r, Jc, Jp);
const int DOF = 6 + M::kNumIntr;
bool ok = std::isfinite(r[0]) && std::isfinite(r[1]);
for (int i = 0; i < 6; i++) ok = ok && std::isfinite(Jp[i]);
for (int row = 0; row < 2; row++)
for (int k = 3; k < DOF; k++) ok = ok && std::isfinite(Jc[row * DOF + k]);
printf("%-34s %s\n", name, ok ? " PASS" : " FAIL");
if (!ok) g_fail++;
}
// ---------------------------------------------------------------------------
// synthetic problems
// ---------------------------------------------------------------------------
// Camera-frame +z is forward for every model but Snavely's, which projects
// along -z; the generator flips the look-at frame for those two.
bool forwardIsMinusZ(uint32_t model) { return model == 0 || model == 1; }
const double* defaultIntr(uint32_t model, int& n) {
static const double snavely[3] = {std::log(600.0), -1e-8, 1e-12};
static const double snavely_f[3] = {600.0, -1e-8, 1e-12};
static const double radial[5] = {600.0, -0.02, 0.003, 320.0, 240.0};
static const double opencv[8] = {600.0, 605.0, -0.02, 0.003, 1e-4, -2e-4, 320.0, 240.0};
static const double simple[3] = {600.0, 320.0, 240.0};
static const double pinhole[4] = {600.0, 605.0, 320.0, 240.0};
static const double fisheye[8] = {300.0, 305.0, 0.01, -0.002, 3e-4, -1e-5, 320.0, 240.0};
static const double full[9] = {600.0, 605.0, -0.02, 0.003, 1e-4, -2e-4, 1e-4, 320.0, 240.0};
static const double prism[12] = {300.0, 305.0, 0.01, -0.002, 3e-4, -1e-5,
2e-4, -1e-6, 1e-4, -2e-4, 320.0, 240.0};
static const double equirect[2] = {640.0, 480.0};
switch (model) {
case 0: n = 3; return snavely;
case 1: n = 3; return snavely_f;
case 2: n = 5; return radial;
case 3: n = 8; return opencv;
case 4: n = 3; return simple;
case 5: n = 4; return pinhole;
case 6: n = 8; return fisheye;
case 7: n = 9; return full;
case 8: n = 12; return prism;
default: n = 2; return equirect;
}
}
void project(uint32_t model, const double* intr, const double p[3], double out[2]) {
bacpu::withModel(model, [&](auto M) { decltype(M)::template project<double>(intr, p, out); });
}
// nImg cameras on a sphere looking at the origin, nPt points inside it; every
// observation that projects sanely is kept. `groups` is 1 (one shared camera)
// or nImg (one per image, the exclusive case).
BAProblem makeProblem(uint32_t model, uint32_t nImg, uint32_t nPt, uint32_t groups,
double noise, uint32_t seed, int nfree = -1) {
std::mt19937 rng(seed);
std::normal_distribution<double> gauss;
std::uniform_real_distribution<double> unit(-1.0, 1.0);
int ni = 0;
const double* base = defaultIntr(model, ni);
const bool minusZ = forwardIsMinusZ(model);
BAProblem P;
P.num_images = nImg;
P.poses.resize(6 * (size_t)nImg);
std::vector<double> centers(3 * (size_t)nImg), rot(9 * (size_t)nImg);
for (uint32_t i = 0; i < nImg; i++) {
const double th = 2.0 * M_PI * i / nImg + 0.03 * unit(rng);
const double ph = 0.4 * unit(rng);
const sfm::Vec3 c{4.0 * std::cos(th) * std::cos(ph), 4.0 * std::sin(ph),
4.0 * std::sin(th) * std::cos(ph)};
const sfm::Vec3 f = (sfm::Vec3{0, 0, 0} - c).normalized();
const sfm::Vec3 rt = sfm::Vec3{0, 1, 0}.cross(f).normalized();
const sfm::Vec3 u2 = f.cross(rt);
// -z forward for Snavely; flipping two rows keeps det = +1
const double s = minusZ ? -1.0 : 1.0;
const sfm::Mat3 R{rt.x, rt.y, rt.z, s * u2.x, s * u2.y,
s * u2.z, s * f.x, s * f.y, s * f.z};
const sfm::Vec3 aa = sfm::rotationToAngleAxis(R);
P.poses[6 * (size_t)i + 0] = aa.x;
P.poses[6 * (size_t)i + 1] = aa.y;
P.poses[6 * (size_t)i + 2] = aa.z;
for (int r = 0; r < 3; r++) {
P.poses[6 * (size_t)i + 3 + r] =
-(R[3 * r] * c.x + R[3 * r + 1] * c.y + R[3 * r + 2] * c.z);
for (int k = 0; k < 3; k++) rot[9 * (size_t)i + 3 * r + k] = R[3 * r + k];
}
centers[3 * (size_t)i + 0] = c.x;
centers[3 * (size_t)i + 1] = c.y;
centers[3 * (size_t)i + 2] = c.z;
}
P.groups.resize(groups);
P.image_group.resize(nImg);
P.intr.resize((size_t)ni * groups);
for (uint32_t g = 0; g < groups; g++) {
for (int j = 0; j < ni; j++) P.intr[(size_t)ni * g + j] = base[j] * (1.0 + 0.005 * unit(rng));
// EQUIRECTANGULAR's two parameters are the image size and never refine
const uint32_t nf = model == 9 ? 0u : (uint32_t)(nfree >= 0 ? std::min(nfree, ni) : ni);
P.groups[g] = {(uint32_t)(ni * g), 0, nf, model};
}
for (uint32_t i = 0; i < nImg; i++) P.image_group[i] = groups == 1 ? 0 : i;
P.num_points = nPt;
P.points.resize(3 * (size_t)nPt);
for (uint32_t p = 0; p < nPt; p++)
for (int k = 0; k < 3; k++) P.points[3 * (size_t)p + k] = 1.2 * unit(rng);
P.obs_ranges.assign(nPt + 1, 0);
for (uint32_t p = 0; p < nPt; p++) {
for (uint32_t i = 0; i < nImg; i++) {
double d[3] = {P.points[3 * (size_t)p] - centers[3 * (size_t)i],
P.points[3 * (size_t)p + 1] - centers[3 * (size_t)i + 1],
P.points[3 * (size_t)p + 2] - centers[3 * (size_t)i + 2]};
double pc[3];
for (int r = 0; r < 3; r++)
pc[r] = rot[9 * (size_t)i + 3 * r] * d[0] + rot[9 * (size_t)i + 3 * r + 1] * d[1] +
rot[9 * (size_t)i + 3 * r + 2] * d[2];
if (minusZ) pc[2] = -std::fabs(pc[2]);
double px[2];
project(model, &P.intr[(size_t)ni * (groups == 1 ? 0 : i)], pc, px);
if (!std::isfinite(px[0]) || !std::isfinite(px[1])) continue;
if (std::fabs(px[0]) > 4000 || std::fabs(px[1]) > 4000) continue;
P.obs_image.push_back(i);
P.obs_point.push_back(p);
P.obs_xy.push_back(px[0] + noise * gauss(rng));
P.obs_xy.push_back(px[1] + noise * gauss(rng));
}
P.obs_ranges[p + 1] = (uint32_t)P.obs_image.size();
}
P.num_obs = (uint32_t)P.obs_image.size();
P.pose_dim = 6 * nImg;
P.total_intr = (uint32_t)P.intr.size();
P.free_intr = 0;
for (BAProblem::Group& g : P.groups) {
g.intr_col = P.pose_dim + P.free_intr;
P.free_intr += g.n_intr;
}
P.n_dim = P.pose_dim + P.free_intr;
finalizeTables(P);
// perturb, so the solver has something to do
for (uint32_t i = 0; i < 6 * nImg; i++) P.poses[i] += 0.004 * gauss(rng);
for (uint32_t p = 0; p < 3 * nPt; p++) P.points[p] += 0.01 * gauss(rng);
return P;
}
// ---------------------------------------------------------------------------
// the unreduced normal equations, written out in full
// ---------------------------------------------------------------------------
struct Reference {
std::vector<double> S, g; // n x n (full), n
std::vector<double> dU, dP; // camera step, 3 per point
};
Reference referenceSolve(const BAProblem& P, double lambda, double lossParam,
const std::string& loss) {
const uint32_t n = P.n_dim;
Reference R;
R.S.assign((size_t)n * n, 0.0);
R.g.assign(n, 0.0);
std::vector<double> V(9 * (size_t)P.num_points, 0.0), bp(3 * (size_t)P.num_points, 0.0);
std::vector<double> Wp((size_t)P.num_points * n * 3, 0.0);
bacpu::withLoss(loss, [&](auto L) {
for (uint32_t o = 0; o < P.num_obs; o++) {
const uint32_t img = P.obs_image[o], pt = P.obs_point[o];
const BAProblem::Group& gr = P.groups[P.image_group[img]];
const uint32_t dof = 6 + gr.n_intr;
double Jc[2 * kMaxCamDof] = {}, Jp[6], r[2];
uint32_t cols[kMaxCamDof];
for (uint32_t i = 0; i < 6; i++) cols[i] = 6 * img + i;
for (uint32_t i = 0; i < gr.n_intr; i++) cols[6 + i] = gr.intr_col + i;
bacpu::withModel(gr.model, [&](auto M) {
using MT = decltype(M);
constexpr int DOF = 6 + MT::kNumIntr;
double jc[2 * DOF];
bacpu::jacobian<MT>(&P.poses[6 * (size_t)img], &P.intr[gr.intr_offset],
&P.points[3 * (size_t)pt], &P.obs_xy[2 * (size_t)o], r, jc, Jp);
const double sw = std::sqrt(decltype(L)::weight(r[0] * r[0] + r[1] * r[1],
lossParam));
for (int row = 0; row < 2; row++)
for (uint32_t a = 0; a < dof; a++) Jc[row * kMaxCamDof + a] = jc[row * DOF + a] * sw;
for (int i = 0; i < 6; i++) Jp[i] *= sw;
r[0] *= sw;
r[1] *= sw;
});
for (uint32_t a = 0; a < dof; a++) {
for (uint32_t b = 0; b < dof; b++)
R.S[(size_t)cols[a] * n + cols[b]] +=
Jc[a] * Jc[b] + Jc[kMaxCamDof + a] * Jc[kMaxCamDof + b];
R.g[cols[a]] += Jc[a] * r[0] + Jc[kMaxCamDof + a] * r[1];
for (int j = 0; j < 3; j++)
Wp[((size_t)pt * n + cols[a]) * 3 + j] +=
Jc[a] * Jp[j] + Jc[kMaxCamDof + a] * Jp[3 + j];
}
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++)
V[9 * (size_t)pt + 3 * i + j] += Jp[i] * Jp[j] + Jp[3 + i] * Jp[3 + j];
bp[3 * (size_t)pt + i] += Jp[i] * r[0] + Jp[3 + i] * r[1];
}
}
});
for (uint32_t i = 0; i < n; i++) R.S[(size_t)i * n + i] *= 1.0 + lambda;
R.dP.assign(3 * (size_t)P.num_points, 0.0);
std::vector<double> Vi(9 * (size_t)P.num_points);
for (uint32_t p = 0; p < P.num_points; p++) {
double m[9];
memcpy(m, &V[9 * (size_t)p], sizeof m);
m[0] *= 1.0 + lambda;
m[4] *= 1.0 + lambda;
m[8] *= 1.0 + lambda;
const double c00 = m[4] * m[8] - m[5] * m[7], c01 = m[5] * m[6] - m[3] * m[8],
c02 = m[3] * m[7] - m[4] * m[6];
const double inv = 1.0 / (m[0] * c00 + m[1] * c01 + m[2] * c02);
double* w = &Vi[9 * (size_t)p];
w[0] = c00 * inv;
w[1] = (m[2] * m[7] - m[1] * m[8]) * inv;
w[2] = (m[1] * m[5] - m[2] * m[4]) * inv;
w[3] = c01 * inv;
w[4] = (m[0] * m[8] - m[2] * m[6]) * inv;
w[5] = (m[2] * m[3] - m[0] * m[5]) * inv;
w[6] = c02 * inv;
w[7] = (m[1] * m[6] - m[0] * m[7]) * inv;
w[8] = (m[0] * m[4] - m[1] * m[3]) * inv;
// S -= W V^-1 W^T, g -= W V^-1 bp
std::vector<double> T((size_t)n * 3, 0.0);
for (uint32_t a = 0; a < n; a++)
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
T[(size_t)a * 3 + i] += Wp[((size_t)p * n + a) * 3 + j] * w[3 * j + i];
for (uint32_t a = 0; a < n; a++) {
for (uint32_t b = 0; b < n; b++) {
double s = 0;
for (int j = 0; j < 3; j++)
s += T[(size_t)a * 3 + j] * Wp[((size_t)p * n + b) * 3 + j];
R.S[(size_t)a * n + b] -= s;
}
double s = 0;
for (int j = 0; j < 3; j++) s += T[(size_t)a * 3 + j] * bp[3 * (size_t)p + j];
R.g[a] -= s;
}
}
// dense solve of S dU = g
std::vector<double> L = R.S;
for (uint32_t j = 0; j < n; j++) {
for (uint32_t k = 0; k < j; k++)
for (uint32_t i = j; i < n; i++)
L[(size_t)i * n + j] -= L[(size_t)i * n + k] * L[(size_t)j * n + k];
const double d = std::sqrt(L[(size_t)j * n + j]);
for (uint32_t i = j; i < n; i++) L[(size_t)i * n + j] /= d;
}
R.dU = R.g;
for (uint32_t i = 0; i < n; i++) {
for (uint32_t j = 0; j < i; j++) R.dU[i] -= L[(size_t)i * n + j] * R.dU[j];
R.dU[i] /= L[(size_t)i * n + i];
}
for (int i = (int)n - 1; i >= 0; i--) {
for (uint32_t j = i + 1; j < n; j++) R.dU[i] -= L[(size_t)j * n + i] * R.dU[j];
R.dU[i] /= L[(size_t)i * n + i];
}
for (uint32_t p = 0; p < P.num_points; p++) {
double t[3];
for (int i = 0; i < 3; i++) {
double s = bp[3 * (size_t)p + i];
for (uint32_t a = 0; a < n; a++) s -= Wp[((size_t)p * n + a) * 3 + i] * R.dU[a];
t[i] = s;
}
for (int i = 0; i < 3; i++)
R.dP[3 * (size_t)p + i] = Vi[9 * (size_t)p + 3 * i] * t[0] +
Vi[9 * (size_t)p + 3 * i + 1] * t[1] +
Vi[9 * (size_t)p + 3 * i + 2] * t[2];
}
return R;
}
double relMax(const double* a, const double* b, size_t n) {
double d = 0, s = 0;
for (size_t i = 0; i < n; i++) {
d = std::max(d, std::fabs(a[i] - b[i]));
s = std::max(s, std::fabs(a[i]));
}
return d / std::max(s, 1e-300);
}
// One LM iteration of the solver against the reference: the assembled S and g,
// and the parameters the step leaves behind.
void testAgainstReference(uint32_t model, uint32_t groups, const char* loss, bool cg,
uint32_t nImg = 9, int nfree = -1) {
const double lambda = 1e-2;
BAProblem P = makeProblem(model, nImg, 90, groups, 0.15, 7 * model + groups, nfree);
if (P.num_obs < 100) {
printf("model %u: too few observations, skipped\n", model);
return;
}
BAProblem P2 = P;
SolverOptions opt;
opt.real = RealCfg::CPU;
opt.loss = loss;
opt.loss_param = 1.5f;
opt.init_damping = lambda;
opt.max_iters = 1;
opt.verbose = false;
opt.solver = cg ? SolverSel::CG : SolverSel::Dense;
opt.cg_tol = 1e-12;
opt.cg_max_iters = 4000;
opt.cg_fallback = CgFallback::Off;
Reference R = referenceSolve(P, lambda, opt.loss_param, loss);
char name[96];
bacpu::Solver solver(P, opt);
solver.init();
if (!cg) {
solver.assembleOnly(lambda);
std::vector<double> S = solver.packedS(), g = solver.gradient();
double dS = 0, sS = 0;
for (uint32_t i = 0; i < P.n_dim; i++)
for (uint32_t j = 0; j <= i; j++) {
const double ref = R.S[(size_t)i * P.n_dim + j];
dS = std::max(dS, std::fabs(S[(size_t)i * (i + 1) / 2 + j] - ref));
sS = std::max(sS, std::fabs(ref));
}
snprintf(name, sizeof name, "S model=%u groups=%u n=%u f=%d", model, groups, nImg, nfree);
report(name, dS / sS, 1e-10);
snprintf(name, sizeof name, "g model=%u groups=%u n=%u f=%d", model, groups, nImg, nfree);
report(name, relMax(g.data(), R.g.data(), P.n_dim), 1e-10);
}
solver.solve();
double dmax = 0, smax = 0;
for (uint32_t i = 0; i < P.pose_dim; i++) {
dmax = std::max(dmax, std::fabs(P.poses[i] - (P2.poses[i] - R.dU[i])));
smax = std::max(smax, std::fabs(R.dU[i]));
}
for (const BAProblem::Group& gr : P.groups)
for (uint32_t j = 0; j < gr.n_intr; j++) {
const double want = P2.intr[gr.intr_offset + j] - R.dU[gr.intr_col + j];
dmax = std::max(dmax, std::fabs(P.intr[gr.intr_offset + j] - want) /
std::max(1.0, std::fabs(want)));
smax = std::max(smax, std::fabs(R.dU[gr.intr_col + j]));
}
snprintf(name, sizeof name, "%s step model=%u groups=%u %s", cg ? "cg " : "dU ", model, groups,
loss);
report(name, dmax / std::max(smax, 1e-300), cg ? 1e-6 : 1e-9);
double pmax = 0, psc = 0;
for (uint32_t p = 0; p < 3 * P.num_points; p++) {
pmax = std::max(pmax, std::fabs(P.points[p] - (P2.points[p] - R.dP[p])));
psc = std::max(psc, std::fabs(R.dP[p]));
}
snprintf(name, sizeof name, "%s dP model=%u groups=%u", cg ? "cg " : "dU ", model, groups);
report(name, pmax / std::max(psc, 1e-300), cg ? 1e-6 : 1e-9);
}
// A full solve has to descend, and the two linear solvers have to agree on
// where it lands.
void testFullSolve(uint32_t model, uint32_t groups) {
BAProblem base = makeProblem(model, 12, 200, groups, 0.3, 31 + model);
double cost[2];
std::vector<double> poses[2];
for (int k = 0; k < 2; k++) {
BAProblem P = base;
SolverOptions opt;
opt.real = RealCfg::CPU;
opt.loss = "huber";
opt.loss_param = 2.0f;
opt.max_iters = 12;
opt.verbose = false;
opt.solver = k ? SolverSel::CG : SolverSel::Dense;
opt.cg_tol = 1e-10;
opt.cg_max_iters = 2000;
opt.cg_fallback = CgFallback::Off;
bacpu::Solver s(P, opt);
s.init();
s.solve();
cost[k] = s.stats().final_cost;
poses[k] = P.poses;
if (!(s.stats().final_cost < s.stats().initial_cost)) {
printf("full model=%u groups=%u %s: cost did not decrease FAIL\n", model, groups,
k ? "cg" : "dense");
g_fail++;
}
}
char name[96];
snprintf(name, sizeof name, "dense/cg cost model=%u groups=%u", model, groups);
report(name, std::fabs(cost[0] - cost[1]) / std::max(cost[0], 1e-300), 1e-8);
snprintf(name, sizeof name, "dense/cg poses model=%u groups=%u", model, groups);
report(name, relMax(poses[0].data(), poses[1].data(), poses[0].size()), 1e-6);
}
} // namespace
int run(int argc, char** argv) {
bool quick = false;
for (int i = 1; i < argc; i++)
if (std::string(argv[i]) == "--quick") quick = true;
testChol(37);
testChol(200);
if (!quick) testChol(400);
std::mt19937 rng(99);
{
int n;
testJacobianModel<bacpu::SnavelyModel>("jac snavely", defaultIntr(0, n), rng);
testJacobianModel<bacpu::SnavelyFModel>("jac snavely_f", defaultIntr(1, n), rng);
testJacobianModel<bacpu::PinholeRadialModel>("jac pinhole_radial", defaultIntr(2, n), rng);
testJacobianModel<bacpu::OpenCVModel>("jac opencv", defaultIntr(3, n), rng);
testJacobianModel<bacpu::SimplePinholeModel>("jac simple_pinhole", defaultIntr(4, n), rng);
testJacobianModel<bacpu::PinholeModel>("jac pinhole", defaultIntr(5, n), rng);
testJacobianModel<bacpu::FisheyeModel>("jac opencv_fisheye", defaultIntr(6, n), rng);
testJacobianModel<bacpu::FullOpenCVModel>("jac full_opencv", defaultIntr(7, n), rng);
testJacobianModel<bacpu::ThinPrismFisheyeModel>("jac thin_prism", defaultIntr(8, n), rng);
testJacobianModel<bacpu::EquirectModel>("jac equirect", defaultIntr(9, n), rng);
testZeroRotation<bacpu::OpenCVModel>("zero rotation opencv", defaultIntr(3, n));
testZeroRotation<bacpu::SnavelyModel>("zero rotation snavely", defaultIntr(0, n));
testZeroRotation<bacpu::ThinPrismFisheyeModel>("zero rotation thin_prism",
defaultIntr(8, n));
testZeroRotation<bacpu::EquirectModel>("zero rotation equirect", defaultIntr(9, n));
}
for (uint32_t model = 0; model < (uint32_t)kNumModels; model++)
for (uint32_t groups : {1u, 9u}) testAgainstReference(model, groups, "huber", false);
testAgainstReference(3, 1, "trivial", false);
testAgainstReference(3, 9, "cauchy", false);
testAgainstReference(3, 1, "huber", true);
testAgainstReference(3, 9, "huber", true);
// partial free prefixes (a held principal point) and the smallest problem
// the mapper ever hands the solver
for (int nf : {0, 2, 6}) {
testAgainstReference(3, 1, "huber", false, 9, nf);
testAgainstReference(3, 9, "huber", false, 9, nf);
}
for (uint32_t nImg : {2u, 3u}) {
testAgainstReference(3, 1, "huber", false, nImg, 6);
testAgainstReference(8, 1, "huber", false, nImg, 10);
}
if (!quick)
for (uint32_t model : {3u, 6u, 8u})
for (uint32_t groups : {1u, 12u}) testFullSolve(model, groups);
printf("%s\n", g_fail ? "FAIL" : "PASS");
return g_fail ? 1 : 0;
}
int main(int argc, char** argv) { return sfmTestMain(argc, argv, run); }
src/sfm/tests/sfm_cholesky_test.cpp
+10 -2
View File
@@ -32,6 +32,11 @@ int selftestChol(uint32_t n, SolverOptions opt) {
// init() may have stepped the scalar type down to what the device supports;
// everything below packs and unpacks against what the kernels actually use.
const RealCfg real = solver.real();
if (real == RealCfg::CPU) {
printf("selftest-chol: this device runs bundle adjustment on the host; "
"the factorization there is covered by sfm_ba_cpu_test\nPASS\n");
return 0;
}
std::mt19937 rng(12345);
std::normal_distribution<double> gauss;
@@ -122,6 +127,10 @@ int benchDispatch(SolverOptions opt) {
P.n_dim = n;
BundleSolver solver(P, opt);
solver.init();
if (solver.real() == RealCfg::CPU) {
printf("no device runs the kernels; nothing to time\n");
return 0;
}
std::vector<double> packed((size_t)n * (n + 1) / 2, 0.0);
for (uint32_t i = 0; i < n; i++) packed[(size_t)i * (i + 1) / 2 + i] = 1.0;
@@ -158,8 +167,7 @@ int run(int argc, char** argv) {
if (a == "--bench") {
bench = true;
} else if (a == "--real") {
std::string s = next();
opt.real = s == "float" ? RealCfg::F32 : s == "double" ? RealCfg::F64 : RealCfg::DF64;
opt.real = realCfgFromName(next());
} else if (a == "--device") {
opt.device = std::stoi(next());
} else if (a[0] != '-') {