From 4a372d66dfa021544e9a0b441e19b9c9669c4422 Mon Sep 17 00:00:00 2001 From: "Oliver J. Woodford" Date: Sat, 4 Nov 2023 19:51:21 +0000 Subject: [PATCH] Release 4.0.0 * Remove deprecated stuff (gauss-newton iterator; store cost & trajectory in options/result) * Remove unused marginalization code * Remove visual geometry functionality (and VisualGeometryDatasets dependency) --- Project.toml | 6 +- README.md | 4 - examples/bundleadjustment.jl | 135 --------------------------- src/NLLSsolver.jl | 10 +- src/autodiff.jl | 20 ---- src/marginalize.jl | 162 --------------------------------- src/optimize.jl | 19 +--- src/residual.jl | 41 +++++++++ src/structs.jl | 30 ++---- src/visualgeometry/camera.jl | 144 ----------------------------- src/visualgeometry/geometry.jl | 141 ---------------------------- test/VectorRepo.jl | 4 +- test/camera.jl | 49 ---------- test/geometry.jl | 45 --------- test/marginalize.jl | 92 ------------------- test/optimizeba.jl | 37 +++----- test/runtests.jl | 3 - 17 files changed, 72 insertions(+), 870 deletions(-) delete mode 100644 examples/bundleadjustment.jl delete mode 100644 src/marginalize.jl delete mode 100644 src/visualgeometry/camera.jl delete mode 100644 src/visualgeometry/geometry.jl delete mode 100644 test/camera.jl delete mode 100644 test/geometry.jl delete mode 100644 test/marginalize.jl diff --git a/Project.toml b/Project.toml index 763cc4ed..750d8755 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "NLLSsolver" uuid = "50b5cf9e-bf7a-4e29-8981-4280cbba70cb" authors = ["Oliver Woodford"] -version = "3.3.2" +version = "4.0.0" [deps] DiffResults = "163ba53b-c6d8-5494-b064-1a9d43ac40c5" @@ -30,15 +30,13 @@ SparseArrays = "1.6" Static = "0.8.7" StaticArrays = "1" Test = "1.6" -VisualGeometryDatasets = "0.2.1" julia = "1.6" [extras] GLMakie = "e9467ef8-e4e7-5192-8a1a-b1aee30e663a" Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" -VisualGeometryDatasets = "962a53e2-84f2-440b-bfbc-4e96ec28357f" [targets] -examples = ["GLMakie", "VisualGeometryDatasets"] +examples = ["GLMakie"] test = ["Test", "Random"] diff --git a/README.md b/README.md index 10b03fcd..33ab5c44 100644 --- a/README.md +++ b/README.md @@ -57,12 +57,8 @@ Various optimizer `options` can be defined. During optimization, the optimizer u ## Examples The following examples of problem definition, creation and optimization are included: - **Rosenbrock function** (examples/rosenbrock.jl): Visualizes optimization of the Rosenbrock function using some of the available optimizers. Click on the parameter space to interactively select a new start point. -- **Bundle adjustment** (examples/bundleadjustment.jl): Optimization of large scale [Bundle Adjustment in the Large](https://grail.cs.washington.edu/projects/bal/) problems, with non-Euclidean variables. ## Future work & collaboration -- **Add Schur complement** to speed up optimization of bipartite problems. -- **Add Variable Projection method** for solving bipartite problems. -- **Implement reduced memory Variable Projection** for solving very large scale bipartite problems. - **Allow residuals to dynamically change the variables they depend on** to broaden the types of problems that can be optimized. - **Allow residual blocks to depend on a dynamic number of variables** to further broaden the types of problems that can be optimized. - **Add additional solvers** diff --git a/examples/bundleadjustment.jl b/examples/bundleadjustment.jl deleted file mode 100644 index a0a4426b..00000000 --- a/examples/bundleadjustment.jl +++ /dev/null @@ -1,135 +0,0 @@ -# This example: -# 1. Defines the variable and residual blocks for "Bundle Adjustment in the Large" (BAL) problems -# 2. Loads a BAL dataset and constructs an NLLSsolver problem from this -# 3. Optimizes the bundle adjustment problem and prints the optimization summary, as well as the start and end AUC. -# AUC = area under curve, specifically the area under the error-recall curve, thresholded at 2 pixel error. - -using VisualGeometryDatasets, StaticArrays, Static, LinearAlgebra -import NLLSsolver -export optimizeBALproblem - -# Description of BAL image, and function to transform a landmark from world coordinates to pixel coordinates -struct BALImage{T} - pose::NLLSsolver.EffPose3D{T} # Extrinsic parameters (rotation & translation) - sensor::NLLSsolver.SimpleCamera{T} # Intrinsic sensor parameters (just a single focal length) - lens::NLLSsolver.BarrelDistortion{T} # Intrinsic lens parameters (k1 & k2 barrel distortion) -end -NLLSsolver.nvars(::BALImage) = static(9) # 9 DoF in total for the image (6 extrinsic, 3 intrinsic) -function NLLSsolver.update(var::BALImage, updatevec, start=1) - return BALImage(NLLSsolver.update(var.pose, updatevec, start), - NLLSsolver.update(var.sensor, updatevec, start+6), - NLLSsolver.update(var.lens, updatevec, start+7)) -end -function BALImage(rx::T, ry::T, rz::T, tx::T, ty::T, tz::T, f::T, k1::T, k2::T) where T<:Real - return BALImage{T}(NLLSsolver.EffPose3D(NLLSsolver.Pose3D(rx, ry, rz, tx, ty, tz)), - NLLSsolver.SimpleCamera(f), - NLLSsolver.BarrelDistortion(k1, k2)) -end -BALImage(v) = BALImage(v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8], v[9]) -transform(im::BALImage, X::NLLSsolver.Point3D) = NLLSsolver.ideal2image(im.sensor, NLLSsolver.ideal2distorted(im.lens, -NLLSsolver.project(im.pose * X))) - -# Residual that defines the reprojection error of a BAL measurement -struct BALResidual{T} <: NLLSsolver.AbstractResidual - measurement::SVector{2, T} # Landmark image coordinates (x, y) - varind::SVector{2, Int} # Variable indices for the residual (image, landmark) -end -BALResidual(m, v) = BALResidual(SVector{2}(m[1], m[2]), SVector{2, Int}(v[1], v[2])) -NLLSsolver.ndeps(::BALResidual) = static(2) # Residual depends on 2 variables -NLLSsolver.nres(::BALResidual) = static(2) # Residual vector has length 2 -NLLSsolver.varindices(res::BALResidual) = res.varind -NLLSsolver.getvars(res::BALResidual{T}, vars::Vector) where T = vars[res.varind[1]]::BALImage{T}, vars[res.varind[2]]::NLLSsolver.Point3D{T} -NLLSsolver.computeresidual(res::BALResidual, im::BALImage, X::NLLSsolver.Point3D) = transform(im, X) - res.measurement -const balrobustifier = NLLSsolver.HuberKernel(2.) -NLLSsolver.robustkernel(::BALResidual) = balrobustifier -Base.eltype(::BALResidual{T}) where T = T - -function NLLSsolver.computeresjac(::StaticInt{3}, residual::BALResidual, im, point) - # Exploit the parameterization to make the jacobian computation more efficient - res, jac = NLLSsolver.computeresjac(static(1), residual, im, point) - return res, hcat(jac, @inbounds(-view(jac, :, NLLSsolver.SR(4, 6)))) -end - -# Define the type of the problem -BALProblem = NLLSsolver.NLLSProblem{Union{BALImage{Float64}, NLLSsolver.Point3D{Float64}}, BALResidual{Float64}} - -# Function to create a NLLSsolver problem from a BAL dataset -function makeBALproblem(data)::BALProblem - # Create the problem - problem = BALProblem() - - # Add the camera variable blocks - for cam in data.cameras - NLLSsolver.addvariable!(problem, BALImage(cam)) - end - numcameras = length(data.cameras) - # Add the landmark variable blocks - for lm in data.landmarks - NLLSsolver.addvariable!(problem, NLLSsolver.Point3D(lm[1], lm[2], lm[3])) - end - - # Add the measurement cost blocks - for meas in data.measurements - NLLSsolver.addcost!(problem, BALResidual(SVector(meas.x, meas.y), SVector(meas.camera, meas.landmark + numcameras))) - end - - # Return the optimization problem - return problem -end - -function loadBALproblem(name)::BALProblem - # Create the problem - t = @elapsed begin - data = loadbaldataset(name) - problem = makeBALproblem(data) - end - show(data) - println("Data loading and problem construction took ", t, " seconds.") - return problem -end - -# Compute the Area Under Curve for reprojection errors, truncated at a given threshold -function computeauc(problem, threshold=10.0, residuals=problem.costs.data[BALResidual{Float64}]) - # Compute all the errors - invthreshold = 1.0 / threshold - errors = Vector{Float64}(undef, length(residuals)+1) - ind = 1 - errors[ind] = 0.0 - for res in residuals - ind += 1 - errors[ind] = norm(NLLSsolver.computeresidual(res, NLLSsolver.getvars(res, problem.variables)...)) * invthreshold - end - - # Sort the errors and truncate, compute recall - sort!(errors) - if errors[end] > 1.0 - cutoff = findfirst(x -> x > 1.0, errors) - recall = (cutoff - 1.0) / length(residuals) - recallfinal = (1.0 - errors[cutoff-1]) / ((errors[cutoff] - errors[cutoff-1]) * length(residuals)) - errors[cutoff] = 1.0 - resize!(errors, cutoff) - recall = vcat(range(0.0, recall, cutoff-1), recall + recallfinal) - else - recall = vcat(range(0.0, 1.0, length(errors)), 1.0) - errors = vcat(errors, 1.0) - end - - # Compute the AUC - return 0.5 * sum(diff(errors) .* (recall[1:end-1] .+ recall[2:end])) -end - -# Function to optimize a BAL problem -optimizeBALproblem(name::String) = optimizeBALproblem(loadBALproblem(name)) -function optimizeBALproblem(problem::NLLSsolver.NLLSProblem) - # Compute the starting AUC - println(" Start AUC: ", computeauc(problem, 2.0)) - # Optimize the cost - result = NLLSsolver.optimize!(problem, NLLSsolver.NLLSOptions(iterator=NLLSsolver.levenbergmarquardt, reldcost=1.0e-6), nothing, NLLSsolver.printoutcallback) - # Compute the final AUC - println(" Final AUC: ", computeauc(problem, 2.0)) - # Print out the solver summary - display(result) - # Return the optimized problem - return problem -end - -optimizeBALproblem("problem-16-22106"); diff --git a/src/NLLSsolver.jl b/src/NLLSsolver.jl index 1fa8e035..fa4266fd 100644 --- a/src/NLLSsolver.jl +++ b/src/NLLSsolver.jl @@ -6,18 +6,15 @@ export NLLSProblem, NLLSOptions, NLLSResult, NLLSIterator # Concrete problem & s export AbstractRobustifier, NoRobust, Scaled, HuberKernel, Huber2oKernel, GemanMcclureKernel # Robustifiers export ContaminatedGaussian # Adaptive robustifiers export EuclideanVector, ZeroToInfScalar, ZeroToOneScalar # Concrete general variables -export Rotation3DR, Rotation3DL, Point3D, Pose3D, EffPose3D, UnitVec3D, UnitPose3D # Concrete 3D geometry variable types -export SimpleCamera, NoDistortionCamera, ExtendedUnifiedCamera, BarrelDistortion, EULensDistortion # Concrete camera sensor & lens variable types +export SimpleError2, SimpleError3, SimpleError4 # Measurement error residuals depending on 2, 3, and 4 variables export CostTrajectory # Object for storing opimization trajectory and costs # Functions export addcost!, addvariable!, updatevarcostmap!, subproblem, subproblem!, nvars, nres # Construct a problem export update, nvars # Variable interface -export nres, ndeps, varindices, getvars # Residual interface +export nres, ndeps, varindices, getvars, generatemeasurement # Residual interface export robustkernel, robustify, robustifydcost, robustifydkernel # Robustifier interface export cost, computeresidual, computeresjac, computecost, computecostgradhess # Compute the objective export optimize! # Optimize the objective -export rodrigues, project, epipolarerror, proj2orthonormal # Multi-view geometry helper functions -export ideal2image, image2ideal, pixel2image, image2pixel, ideal2distorted, distorted2ideal, convertlens export nullcallback, printoutcallback, storecostscallback # Callbacks to be called once per optimization iteration # Exported abstract types @@ -42,8 +39,6 @@ include("problem.jl") # Variables include("variable.jl") -include("visualgeometry/camera.jl") -include("visualgeometry/geometry.jl") # Types include("BlockSparseMatrix.jl") @@ -53,7 +48,6 @@ include("structs.jl") # Optimization include("residual.jl") -include("marginalize.jl") include("robust.jl") include("robustadaptive.jl") include("autodiff.jl") diff --git a/src/autodiff.jl b/src/autodiff.jl index 1d606fa5..57353339 100644 --- a/src/autodiff.jl +++ b/src/autodiff.jl @@ -163,23 +163,3 @@ computegradhesshelper(varflags, residual, vars, x) = computecost(residual, updat @inline autorobustifydcost(kernel::AbstractRobustifier, cost) = computehessian(Base.Fix1(robustify, kernel), cost) @inline autorobustifydkernel(kernel::AbstractAdaptiveRobustifier, cost::T) where T = computehessian(fixallbutlast(computerobustgradhesshelper, kernel, cost), zeros(SVector{nvars(kernel)+1, T})) computerobustgradhesshelper(kernel, cost, x) = robustify(update(kernel, x), cost+x[end]) - -# Overloads of some specific functions -struct RotMatLie{T} - x::T - y::T - z::T -end - -function rodrigues(x::T, y::T, z::T) where T<:ForwardDiff.Dual - @assert x == 0 && y == 0 && z == 0 - return RotMatLie{T}(x, y, z) -end - -du(x) = ForwardDiff.partials(x) -Base.:*(r::AbstractMatrix, u::RotMatLie{T}) where T = SMatrix{3, 3, T, 9}(T(r[1,1], du(u.z)*r[1,2]-du(u.y)*r[1,3]), T(r[2,1], du(u.z)*r[2,2]-du(u.y)*r[2,3]), T(r[3,1], du(u.z)*r[3,2]-du(u.y)*r[3,3]), - T(r[1,2], du(u.x)*r[1,3]-du(u.z)*r[1,1]), T(r[2,2], du(u.x)*r[2,3]-du(u.z)*r[2,1]), T(r[3,2], du(u.x)*r[3,3]-du(u.z)*r[3,1]), - T(r[1,3], du(u.y)*r[1,1]-du(u.x)*r[1,2]), T(r[2,3], du(u.y)*r[2,1]-du(u.x)*r[2,2]), T(r[3,3], du(u.y)*r[3,1]-du(u.x)*r[3,2])) -Base.:*(u::RotMatLie{T}, r::AbstractMatrix) where T = SMatrix{3, 3, T, 9}(T(r[1,1], du(u.y)*r[3,1]-du(u.z)*r[2,1]), T(r[2,1], du(u.z)*r[1,1]-du(u.x)*r[3,1]), T(r[3,1], du(u.x)*r[2,1]-du(u.y)*r[1,1]), - T(r[1,2], du(u.y)*r[3,2]-du(u.z)*r[2,2]), T(r[2,2], du(u.z)*r[1,2]-du(u.x)*r[3,2]), T(r[3,2], du(u.x)*r[2,2]-du(u.y)*r[1,2]), - T(r[1,3], du(u.y)*r[3,3]-du(u.z)*r[2,3]), T(r[2,3], du(u.z)*r[1,3]-du(u.x)*r[3,3]), T(r[3,3], du(u.x)*r[2,3]-du(u.y)*r[1,3])) diff --git a/src/marginalize.jl b/src/marginalize.jl deleted file mode 100644 index fbef2f0f..00000000 --- a/src/marginalize.jl +++ /dev/null @@ -1,162 +0,0 @@ -using StaticArrays, HybridArrays, Static, LinearAlgebra - -function marginalize!(to::MultiVariateLS, from::MultiVariateLSsparse, blockind::Int, lambda::Float64, blocksz::Int) - # Get the list of blocks to marginalize out - ind = from.A.indicestransposed.colptr[blockind]:from.A.indicestransposed.colptr[blockind+1]-1 - blocks = view(from.A.indicestransposed.rowval, ind) - @assert blocks[end] == blockind && from.A.rowblocksizes[blockind] == blocksz - @assert !isa(to, MultiVariateLSsparse) || all(view(blocks, 1:lastindex(blocks)-1) .<= length(to.A.rowblocksizes)) - @assert !isa(to, MultiVariateLSdense) || all(view(blocks, 1:lastindex(blocks)-1) .<= (length(to.A.rowblockoffsets) - 1)) - N = length(blocks) - 1 - dataindices = view(from.A.indicestransposed.nzval, ind) - # Get the diagonal block (to be marginalized) - diagblock = reshape(view(from.A.data, (0:blocksz*blocksz-1) .+ dataindices[end]), blocksz, blocksz) + I * lambda - @static if VERSION ≥ v"1.9" - diagblock = bunchkaufman(diagblock) - end - # For each non-marginalized block - blockgrad = view(from.b, (0:blocksz-1) .+ from.boffsets[blockind]) - for a in 1:N - # Multiply inverse by first block - blocka = blocks[a] - lena = Int(from.A.rowblocksizes[blocka]) - S = (diagblock \ reshape(view(from.A.data, (0:blocksz*lena-1) .+ dataindices[a]), blocksz, lena))' - # Update gradient - view(to.b, (0:lena-1) .+ to.boffsets[blocka]) .-= S * blockgrad - # Update Hessian blocks - for b in 1:a - blockb = blocks[b] - lenb = Int(from.A.rowblocksizes[blockb]) - B = reshape(view(from.A.data, (0:blocksz*lenb-1) .+ dataindices[b]), blocksz, lenb) - block(to.A, blocka, blockb, lena, lenb) .-= S * B - end - end -end - -function marginalize!(to::MultiVariateLS, from::MultiVariateLSsparse, blockind::Int, lambda::Float64, ::StaticInt{blocksz}) where blocksz - # Get the list of blocks to marginalize out - ind = from.A.indicestransposed.colptr[blockind]:from.A.indicestransposed.colptr[blockind+1]-1 - blocks = view(from.A.indicestransposed.rowval, ind) - @assert blocks[end] == blockind && from.A.rowblocksizes[blockind] == blocksz - @assert !isa(to, MultiVariateLSsparse) || all(view(blocks, 1:lastindex(blocks)-1) .<= length(to.A.rowblocksizes)) - @assert !isa(to, MultiVariateLSdense) || all(view(blocks, 1:lastindex(blocks)-1) .<= (length(to.A.rowblockoffsets) - 1)) - N = length(blocks) - 1 - dataindices = view(from.A.indicestransposed.nzval, ind) - # Get the diagonal block (to be marginalized) - diagblock = SMatrix{blocksz, blocksz}(SizedMatrix{blocksz, blocksz}(view(from.A.data, SR(0, blocksz*blocksz-1).+dataindices[end]))) + I * lambda - @static if VERSION ≥ v"1.9" - diagblock = bunchkaufman(diagblock) - end - # For each non-marginalized block - blockgrad = SizedVector{blocksz}(view(from.b, SR(0, blocksz-1) .+ from.boffsets[blockind])) - for a in 1:N - # Multiply inverse by first block - blocka = blocks[a] - lena = Int(from.A.rowblocksizes[blocka]) - S = (diagblock \ HybridArray{Tuple{blocksz, StaticArrays.Dynamic()}}(reshape(view(from.A.data, (0:blocksz*lena-1) .+ dataindices[a]), blocksz, lena)))' - # Update gradient - view(to.b, SR(0, lena-1) .+ to.boffsets[blocka]) .-= S * blockgrad - # Update Hessian blocks - for b in 1:a - blockb = blocks[b] - lenb = Int(from.A.rowblocksizes[blockb]) - block(to.A, blocka, blockb, lena, lenb) .-= S * HybridArray{Tuple{blocksz, StaticArrays.Dynamic()}}(reshape(view(from.A.data, (0:blocksz*lenb-1) .+ dataindices[b]), blocksz, lenb)) - end - end -end - -function marginalize!(to::MultiVariateLS, from::MultiVariateLSsparse, blocks::AbstractRange, lambdas, blocksz) - for block in blocks - marginalize!(to, from, block, @inbounds(lambdas[min(block, end)]), blocksz) - end -end - -function marginalize!(to::MultiVariateLS, from::MultiVariateLSsparse, lambdas, fromblock = isa(to, MultiVariateLSsparse) ? length(to.A.rowblocksizes)+1 : length(to.A.rowblockoffsets)) - last = fromblock - finish = length(from.A.rowblocksizes) - while last <= finish - first = last - blocksz = Int(from.A.rowblocksizes[last]) - while true - last += 1 - if last > finish || blocksz != Int(from.A.rowblocksizes[last]) - break - end - end - range = first:last-1 - if blocksz <= MAX_BLOCK_SZ - # marginalize!(to, from, first:last, static(blocksz)) - valuedispatch(static(1), static(MAX_BLOCK_SZ), blocksz, fixallbutlast(marginalize!, to, from, range, lambdas)) - else - marginalize!(to, from, range, lambdas, blocksz) - end - end - return to -end - -function initcrop!(to::MultiVariateLSsparse, from::MultiVariateLSsparse, fromblock=length(to.A.rowblocksizes)+1) - # Reset the linear system to all zeros - zero!(to) - # Copy over all the cropped bits - to.b .= view(from.b, 1:lastindex(to.b)) - endind = from.A.indicestransposed.nzval[from.A.indicestransposed.colptr[fromblock]] - 1 - view(to.A.data, 1:endind) .= view(from.A.data, 1:endind) - return to -end - -function initcrop!(to::MultiVariateLSdense, from::MultiVariateLSsparse, fromblock=length(to.A.rowblockoffsets)) - # Reset the linear system to all zeros - zero!(to) - # Copy over all the cropped bits - @inbounds to.b .= view(from.b, 1:lastindex(to.b)) - @inbounds for row = 1:fromblock-1 - lenr = Int(from.A.rowblocksizes[row]) - for colind = from.A.indicestransposed.colptr[row]:from.A.indicestransposed.colptr[row+1]-1 - col = from.A.indicestransposed.rowval[colind] - lenc = Int(from.A.columnblocksizes[col]) - block(to.A, row, col, lenr, lenc) .= reshape(view(from.A.data, (0:lenr*lenc-1) .+ from.A.indicestransposed.nzval[colind]), lenr, lenc) - end - end - return to -end - -function constructcrop(from::MultiVariateLSsparse, fromblock, forcesparse=false) - toblocksizes = view(from.A.rowblocksizes, 1:fromblock-1) - - # Decide whether to have a sparse or a dense system - len = sum(toblocksizes) - @inbounds if forcesparse || len >= 40 - # Compute the crop sparsity - cacheindices(from.A) - toblock = fromblock - 1 - cropsparsity = view(from.A.indices + from.A.indicestransposed, :, 1:toblock) # Do view before sum when this issue is fixed: https://github.com/JuliaSparse/SparseArrays.jl/issues/441 - cropsparsity = triu(cropsparsity' * cropsparsity) - - # Check sparsity level - if forcesparse || sparse_dense_decision(len, block_sparse_nnz(cropsparsity, toblocksizes)) - # Add any missing blocks to the cropped region - start = from.A.indicestransposed.nzval[from.A.indicestransposed.colptr[fromblock]] - blocksizes = convert.(Int, @inbounds view(from.A.rowblocksizes, 1:toblock)) - for c = 1:toblock - nc = blocksizes[c] - for rind = cropsparsity.colptr[c]:cropsparsity.colptr[c+1]-1 - r = cropsparsity.rowval[rind] - v = from.A.indicestransposed[r,c] # Improve this line - if v == 0 - # We need to add a block - v = start - start += nc * blocksizes[r] - end - cropsparsity.nzval[rind] = v - end - end - A = BlockSparseMatrix{Float64}(start-1, cropsparsity, blocksizes, blocksizes) - - # Construct the sparse linear system - return MultiVariateLSsparse(A, from.blockindices[1:findfirst(isequal(fromblock), from.blockindices)-1]) - end - end - - # Construct a dense linear system - return MultiVariateLSdense(toblocksizes, from.blockindices[1:findfirst(isequal(fromblock), from.blockindices)-1]) -end diff --git a/src/optimize.jl b/src/optimize.jl index 76147d5d..3c1dc7b3 100644 --- a/src/optimize.jl +++ b/src/optimize.jl @@ -1,5 +1,5 @@ # Uni-variate optimization (single unfixed variable) -optimize!(problem::NLLSProblem, options::NLLSOptions, unfixed::Integer, callback=nullcallback, starttimens=Base.time_ns())::NLLSResult = getresult(setupiterator(optimizeinternal!, problem, options, NLLSInternal(UInt(unfixed), nvars(problem.variables[unfixed]), starttimens), checkcallback(options, callback))) +optimize!(problem::NLLSProblem, options::NLLSOptions, unfixed::Integer, callback=nullcallback, starttimens=Base.time_ns())::NLLSResult = getresult(setupiterator(optimizeinternal!, problem, options, NLLSInternal(UInt(unfixed), nvars(problem.variables[unfixed]), starttimens), callback)) # Multi-variate optimization function optimize!(problem::NLLSProblem, options::NLLSOptions, unfixed::AbstractVector, callback)::NLLSResult @@ -13,7 +13,7 @@ function optimize!(problem::NLLSProblem, options::NLLSOptions, unfixed::Abstract return optimize!(problem, options, unfixed, callback, starttimens) end # Multiple variables - return getresult(setupiterator(optimizeinternal!, problem, options, NLLSInternal(makesymmvls(problem, unfixed, nblocks), starttimens), checkcallback(options, callback))) + return getresult(setupiterator(optimizeinternal!, problem, options, NLLSInternal(makesymmvls(problem, unfixed, nblocks), starttimens), callback)) end # Conversions for different types of "unfixed" @@ -43,9 +43,6 @@ function optimizesingles!(problem::NLLSProblem{VT, CT}, options::NLLSOptions, in return end -checkcallback(::NLLSOptions{Nothing}, callback) = callback -checkcallback(options::NLLSOptions, ::Any) = options.callback - function setupiterator(func, problem::NLLSProblem, options::NLLSOptions, data::NLLSInternal, trailingargs...) # Copy the variables, if need be if length(problem.variables) != length(problem.varnext) @@ -53,8 +50,8 @@ function setupiterator(func, problem::NLLSProblem, options::NLLSOptions, data::N end # Call the optimizer with the required iterator struct - if options.iterator == newton || options.iterator == gaussnewton - # Newton's method, using Gauss' approximation to the Hessian (optimizing Hessian form) + if options.iterator == newton + # Newton's method newtondata = NewtonData(problem, data) return func(problem, options, data, newtondata, trailingargs...) end @@ -97,10 +94,6 @@ function optimizeinternal!(problem::NLLSProblem, options::NLLSOptions, data, ite cost = iterate!(iteratedata, data, problem, options)::Float64 # Call the user-defined callback cost, terminate = callback(cost, problem, data, iteratedata)::Tuple{Float64, Int} - # Store the cost if necessary - if options.storecosts - push!(data.costs, cost) - end # Check for cost increase (only some iterators will do this) dcost = data.bestcost - cost if dcost >= 0 @@ -120,10 +113,6 @@ function optimizeinternal!(problem::NLLSProblem, options::NLLSOptions, data, ite end # Update the variables updatefromnext!(problem, data) - if options.storetrajectory - # Store the variable trajectory (as update vectors) - push!(data.trajectory, copy(data.linsystem.x)) - end # Check for termination maxstep = maximum(abs, data.linsystem.x) converged = 0 diff --git a/src/residual.jl b/src/residual.jl index 99af6e4d..d9220cfa 100644 --- a/src/residual.jl +++ b/src/residual.jl @@ -1,5 +1,46 @@ using Static +# Simple residuals modelling measurement noise errors +struct SimpleError2{N, T, V1, V2} <: AbstractResidual + measurement::SVector{N, T} # Measurement + varind::SVector{2, Int} # Variable indices +end +SimpleError2{V1, V2}(meas::SVector{N, T}, vi1, vi2) where {N, T, V1, V2} = SimpleError2{N, T, V1, V2}(meas, SVector{2, Int}(vi1, vi2)) +ndeps(::SimpleError2) = static(2) # Residual depends on 2 variables +nres(::SimpleError2{N, T, V1, V2}) where {N, T, V1, V2} = static(N) # Residual vector has length N +varindices(res::SimpleError2) = res.varind +getvars(res::SimpleError2{N, T, V1, V2}, vars::Vector) where {N, T, V1, V2} = vars[@inbounds(res.varind[1])]::V1, vars[@inbounds(res.varind[2])]::V2 +computeresidual(res::SimpleError2, arg1, arg2) = generatemeasurement(arg1, arg2) - res.measurement +Base.eltype(::SimpleError2{N, T, V1, V2}) where {N, T, V1, V2} = T + +struct SimpleError3{N, T, V1, V2, V3} <: AbstractResidual + measurement::SVector{N, T} # Measurement + varind::SVector{3, Int} # Variable indices +end +SimpleError3{V1, V2, V3}(meas::SVector{N, T}, vi1, vi2, vi3) where {N, T, V1, V2, V3} = SimpleError3{N, T, V1, V2, V3}(meas, SVector{3, Int}(vi1, vi2, vi3)) +ndeps(::SimpleError3) = static(3) # Residual depends on 3 variables +nres(::SimpleError3{N, T, V1, V2, V3}) where {N, T, V1, V2, V3} = static(N) # Residual vector has length N +varindices(res::SimpleError3) = res.varind +getvars(res::SimpleError3{N, T, V1, V2, V3}, vars::Vector) where {N, T, V1, V2, V3} = vars[@inbounds(res.varind[1])]::V1, vars[@inbounds(res.varind[2])]::V2, vars[@inbounds(res.varind[3])]::V3 +computeresidual(res::SimpleError3, arg1, arg2, arg3) = generatemeasurement(arg1, arg2, arg3) - res.measurement +Base.eltype(::SimpleError3{N, T, V1, V2, V3}) where {N, T, V1, V2, V3} = T + +struct SimpleError4{N, T, V1, V2, V3, V4} <: AbstractResidual + measurement::SVector{N, T} # Measurement + varind::SVector{4, Int} # Variable indices +end +SimpleError4{V1, V2, V3, V4}(meas::SVector{N, T}, vi1, vi2, vi3, vi4) where {N, T, V1, V2, V3, V4} = SimpleError4{N, T, V1, V2, V3, V4}(meas, SVector{4, Int}(vi1, vi2, vi3, vi4)) +ndeps(::SimpleError4) = static(4) # Residual depends on 4 variables +nres(::SimpleError4{N, T, V1, V2, V3, V4}) where {N, T, V1, V2, V3, V4} = static(N) # Residual vector has length N +varindices(res::SimpleError4) = res.varind +getvars(res::SimpleError4{N, T, V1, V2, V3, V4}, vars::Vector) where {N, T, V1, V2, V3, V4} = vars[@inbounds(res.varind[1])]::V1, vars[@inbounds(res.varind[2])]::V2, vars[@inbounds(res.varind[3])]::V3, vars[@inbounds(res.varind[4])]::V4 +computeresidual(res::SimpleError4, arg1, arg2, arg3, arg4) = generatemeasurement(arg1, arg2, arg3, arg4) - res.measurement +Base.eltype(::SimpleError4{N, T, V1, V2, V3, V4}) where {N, T, V1, V2, V3, V4} = T + +# Dummy definition +generatemeasurement() = nothing + +# Functions for computing costs and gradients of residuals computecost(residual::AbstractResidual, vars...) = computerescost(residual, robustkernel(residual), vars) computecostgradhess(varflags, residual::AbstractResidual, vars...) = computerescostgradhess(varflags << static(1), residual, robustkernel(residual), vars) computecost(residual::AbstractAdaptiveResidual, kernel, vars...) = computerescost(residual, kernel, vars) diff --git a/src/structs.jl b/src/structs.jl index d9eb2085..78ea2cdb 100644 --- a/src/structs.jl +++ b/src/structs.jl @@ -2,9 +2,9 @@ using SparseArrays, Static import IfElse.ifelse import Printf.@printf -@enum NLLSIterator gaussnewton newton levenbergmarquardt dogleg gradientdescent +@enum NLLSIterator newton levenbergmarquardt dogleg gradientdescent function Base.String(iterator::NLLSIterator) - if iterator == newton || iterator == gaussnewton + if iterator == newton return "Newton" end if iterator == levenbergmarquardt @@ -29,20 +29,9 @@ struct NLLSOptions{T} iterator::NLLSIterator # Inner iterator (see above for options) callback::T # Callback called every outer iteration - (cost, problem, data) -> (newcost, terminate::Bool) where terminate == true ends the optimization iteratordata # Iterator-specific data, to be passed to the iterator - storecosts::Bool # Indicates whether the cost per outer iteration should be stored - storetrajectory::Bool # Indicates whether the step per outer iteration should be stored end -function NLLSOptions(; maxiters=100, reldcost=1.e-15, absdcost=1.e-15, dstep=1.e-15, maxfails=3, maxtime=30.0, iterator=levenbergmarquardt, callback::T=nothing, iteratordata=nothing, storecosts=false, storetrajectory=false) where T - if iterator == gaussnewton - Base.depwarn("gaussnewton is deprecated. Use newton instead", :NLLSOptions) - end - if storecosts || storetrajectory - Base.depwarn("storecosts and storetrajectory are deprecated. Use storecostscallback instead", :NLLSOptions) - end - if !isnothing(callback) - Base.depwarn("Setting callback in options is deprecated. Pass the callback directly to optimize!() instead", :NLLSOptions) - end - NLLSOptions{T}(reldcost, absdcost, dstep, maxfails, maxiters, UInt64(round(maxtime * 1e9)), iterator, callback, iteratordata, storecosts, storetrajectory) +function NLLSOptions(; maxiters=100, reldcost=1.e-15, absdcost=1.e-15, dstep=1.e-15, maxfails=3, maxtime=30.0, iterator=levenbergmarquardt, callback::T=nothing, iteratordata=nothing) where T + NLLSOptions{T}(reldcost, absdcost, dstep, maxfails, maxiters, UInt64(round(maxtime * 1e9)), iterator, callback, iteratordata) end struct NLLSResult @@ -58,8 +47,6 @@ struct NLLSResult costcomputations::Int # Number of cost computations performed gradientcomputations::Int # Number of residual gradient computations performed linearsolvers::Int # Number of linear solves performed - costs::Vector{Float64} # Vector of costs at the end of each outer iteration - trajectory::Vector{Vector{Float64}} # Vector of update vectors at the end of each outer iteration end function Base.show(io::IO, x::NLLSResult) @@ -109,18 +96,15 @@ mutable struct NLLSInternal{LSType} linearsolvers::Int converged::Int # Linear system - linsystem::LSType - # Intermediate cost / trajectory storage (deprecated) - costs::Vector{Float64} - trajectory::Vector{Vector{Float64}} + linsystem::LSType function NLLSInternal(linsystem::LSType, starttimens) where LSType - return new{LSType}(0., 0., starttimens, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, linsystem, Vector{Float64}(), Vector{Vector{Float64}}()) + return new{LSType}(0., 0., starttimens, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, linsystem) end end NLLSInternal(unfixed::UInt, varlen, starttimens) = NLLSInternal(ifelse(is_static(varlen), UniVariateLSstatic{dynamic(varlen), dynamic(varlen*varlen)}(unfixed), UniVariateLSdynamic(unfixed, dynamic(varlen))), starttimens) -getresult(data::NLLSInternal) = NLLSResult(data.startcost, data.bestcost, data.timetotal*1.e-9, data.timeinit*1.e-9, data.timecost*1.e-9, data.timegradient*1.e-9, data.timesolver*1.e-9, data.converged, data.iternum, data.costcomputations, data.gradientcomputations, data.linearsolvers, data.costs, data.trajectory) +getresult(data::NLLSInternal) = NLLSResult(data.startcost, data.bestcost, data.timetotal*1.e-9, data.timeinit*1.e-9, data.timecost*1.e-9, data.timegradient*1.e-9, data.timesolver*1.e-9, data.converged, data.iternum, data.costcomputations, data.gradientcomputations, data.linearsolvers) NLLSInternalMultiVar = Union{NLLSInternal{MultiVariateLSdense}, NLLSInternal{MultiVariateLSsparse}} NLLSInternalSingleVar = Union{NLLSInternal{UniVariateLSstatic{N, N2}}, NLLSInternal{UniVariateLSdynamic}} where {N, N2} diff --git a/src/visualgeometry/camera.jl b/src/visualgeometry/camera.jl deleted file mode 100644 index 4992b50b..00000000 --- a/src/visualgeometry/camera.jl +++ /dev/null @@ -1,144 +0,0 @@ -using StaticArrays, LinearAlgebra, Static - - -function image2pixel(halfimsz, x) - return x .* halfimsz[1] .+ halfimsz -end - -function pixel2image(halfimsz, x) - return (x .- halfimsz) ./ halfimsz[1] -end - -function pixel2image(halfimsz, x, W) - return ((x .- halfimsz) ./ halfimsz[1], W .* halfimsz[1]) -end - -function ideal2image(camera, x) - return x .* focallength(camera) .+ cameracenter(camera) -end - -function image2ideal(camera, x) - return (x .- cameracenter(camera)) ./ focallength(camera) -end - -function image2ideal(camera, x, W) - return (x .- cameracenter(camera)) ./ focallength(camera), W .* focallength(camera)' -end - -struct SimpleCamera{T} - f::ZeroToInfScalar{T} - function SimpleCamera{T}(v::T) where T - return new{T}(ZeroToInfScalar{T}(v)) - end -end -SimpleCamera(v::T) where T = SimpleCamera{T}(v::T) -nvars(::SimpleCamera) = static(1) -update(var::SimpleCamera, updatevec, start=1) = SimpleCamera(update(var.f, updatevec, start).val) -@inline cameracenter(::SimpleCamera) = 0 -@inline focallength(camera::SimpleCamera) = camera.f.val - - -struct NoDistortionCamera{T} - fx::ZeroToInfScalar{T} - fy::ZeroToInfScalar{T} - c::EuclideanVector{2, T} -end -NoDistortionCamera(fx::T, fy::T, cx::T, cy::T) where T = NoDistortionCamera(ZeroToInfScalar(fx), ZeroToInfScalar(fy), EuclideanVector(cx, cy)) -NoDistortionCamera(f, c) = NoDistortionCamera(f[1], f[2], c[1], c[2]) -nvars(::NoDistortionCamera) = static(4) -update(var::NoDistortionCamera, updatevec, start=1) = NoDistortionCamera(update(var.fx, updatevec, start), update(var.fy, updatevec, start+1), update(var.c, updatevec, start+2)) -@inline cameracenter(camera::NoDistortionCamera) = camera.c -@inline focallength(camera::NoDistortionCamera{T}) where T = SVector{2, T}(camera.fx.val, camera.fy.val) - - -struct EULensDistortion{T} - alpha::ZeroToOneScalar{T} - beta::ZeroToInfScalar{T} -end -EULensDistortion(alpha::T, beta::T) where T = EULensDistortion{T}(ZeroToOneScalar{T}(alpha), ZeroToInfScalar{T}(beta)) -nvars(::EULensDistortion) = static(2) -update(var::EULensDistortion, updatevec, start=1) = EULensDistortion(update(var.alpha, updatevec, start), update(var.beta, updatevec, start+1)) -Base.eltype(::EULensDistortion{T}) where T = T - -function ideal2distorted(lens::EULensDistortion, x) - z = 1 / (1 + lens.alpha.val * (sqrt(lens.beta.val * (x' * x) + 1) - 1)) - return x * z -end - -function distorted2ideal(lens::EULensDistortion, x) - z = lens.beta.val * (x' * x) - z = (1 + lens.alpha.val * (sqrt(1 + z * (1 - 2 * lens.alpha.val)) - 1)) / (1 - z * (lens.alpha.val ^ 2)) - return x * z -end - -function distorted2ideal(lens::EULensDistortion, x, W) - t = 1 - 2 * lens.alpha.val - n = x' * x - u = lens.beta.val * n - v = 1 / (1 - u * (lens.alpha.val ^ 2)) - w = sqrt(t * u + 1) - z = (1 + lens.alpha.val * (w - 1)) * v - zi = 1 / z - dzdn = zi * 2 * lens.alpha.val * lens.beta.val * v * (z * lens.alpha.val + t / (2 * w)) - return x * z, (W - (W * (x * x')) * (dzdn / (1 + n * dzdn))) * zi -end - -struct ExtendedUnifiedCamera{T<:Number} - sensor::NoDistortionCamera{T} - lens::EULensDistortion{T} -end -ExtendedUnifiedCamera(f, c, a, b) = ExtendedUnifiedCamera(NoDistortionCamera(f[1], f[2], c[1], c[2]), EULensDistortion(a, b)) -nvars(::ExtendedUnifiedCamera) = static(6) -update(var::ExtendedUnifiedCamera, updatevec, start=1) = ExtendedUnifiedCamera(update(var.sensor, updatevec, start), update(var.lens, updatevec, start+4)) - -function ideal2image(camera::ExtendedUnifiedCamera, x) - return ideal2image(camera.sensor, ideal2distorted(camera.lens, x)) -end - -function image2ideal(camera::ExtendedUnifiedCamera, x) - return distorted2ideal(camera.lens, image2ideal(camera.sensor, x)) -end - -function image2ideal(camera::ExtendedUnifiedCamera, x, W) - y, W_ = image2ideal(camera.sensor, x, W) - return distorted2ideal(camera.lens, y, W_) -end - -struct BarrelDistortion{T} - k1::T - k2::T -end -nvars(::BarrelDistortion) = static(2) -update(var::BarrelDistortion, updatevec, start=1) = BarrelDistortion(var.k1 + updatevec[start], var.k2 + updatevec[start+1]) -Base.eltype(::BarrelDistortion{T}) where T = T - -function ideal2distorted(lens::BarrelDistortion, x) - z = x' * x - z = z * (lens.k1 + z * lens.k2) + 1 - return x * z -end - -struct LensDistortResidual{T} <: AbstractResidual - rlinear::T - rdistort::T -end -ndeps(::LensDistortResidual) = static(1) # The residual depends on one variable -nres(::LensDistortResidual) = 1 # The residual vector has length one -varindices(::LensDistortResidual) = SVector(1) -computeresidual(residual::LensDistortResidual, lens::EULensDistortion) = SVector(residual.rdistort - ideal2distorted(lens, residual.rlinear)) -getvars(::LensDistortResidual{T}, vars::Vector{LT}) where {T, LT} = (vars[1]::LT,) -Base.eltype(::LensDistortResidual{T}) where T = T - -function convertlens(tolens, fromlens, halfimsz) - # Create an optimization problem to convert the lens distortion - @assert eltype(tolens)<:AbstractFloat - problem = NLLSsolver.NLLSProblem(typeof(tolens), LensDistortResidual{eltype(tolens)}) - NLLSsolver.addvariable!(problem, tolens) - for x in LinRange(0., convert(Float64, halfimsz), 100) - NLLSsolver.addcost!(problem, LensDistortResidual(x, ideal2distorted(fromlens, x))) - end - # Optimize - NLLSsolver.optimize!(problem, NLLSsolver.NLLSOptions(iterator=NLLSsolver.dogleg)) - # Return the lens - return problem.variables[1] -end diff --git a/src/visualgeometry/geometry.jl b/src/visualgeometry/geometry.jl deleted file mode 100644 index 8c84de6d..00000000 --- a/src/visualgeometry/geometry.jl +++ /dev/null @@ -1,141 +0,0 @@ -using StaticArrays, LinearAlgebra - -function rodrigues(x::T, y::T, z::T) where T<:Number - theta2 = x * x + y * y + z * z - cosf = T(0.5) - sinc = T(1) - if theta2 > T(2.23e-16) - theta = sqrt(theta2) - sinc, cosf = sincos(theta) - cosf -= 1 - sinc /= theta - cosf /= -theta2 - end - a = x * y * cosf - b = sinc * z - c = x * z * cosf - d = sinc * y - e = y * z * cosf - f = sinc * x - return SMatrix{3, 3, T, 9}((x * x - theta2) * cosf + 1, a + b, c - d, - a - b, (y * y - theta2) * cosf + 1, e + f, - c + d, e - f, (z * z - theta2) * cosf + 1) -end - -function proj2orthonormal(M) - s = svd(M); - return s.U * s.V'; -end - -function epipolarerror(RX, T, x, W=nothing) - Tp = proj(T) - xe = x - Tp - RXp = proj(RX) - Tp - RXp = SVector(RXp[2], -RXp[1]) - if !isnothing(W) - RXp = W' \ RXp - xe = W * xe - end - return dot(xe, normalize(RXp)) -end - -abstract type AbstractPoint3D end -struct Point3D{T<:Real} <: AbstractPoint3D - v::SVector{3, T} -end -Point3D(x::T, y::T, z::T) where T = Point3D{T}(SVector{3, T}(x, y, z)) -Point3D() = Point3D{Float64}() -Point3D{T}() where T = Point3D{T}(SVector{3, T}(0, 0, 0)) -nvars(::Point3D) = static(3) -update(var::Point3D, updatevec, start=1) = Point3D(var.v + view(updatevec, SR(0, 2) .+ start)) -@inline getvec(x::Point3D) = x.v -@inline getind(x::Point3D, ind) = x.v[ind] - - -abstract type AbstractRotation3D end -struct Rotation3DR{T<:Real} <: AbstractRotation3D - m::SMatrix{3, 3, T, 9} -end -Rotation3DR(x, y, z) = Rotation3DR(rodrigues(x, y, z)) -Rotation3DR() = Rotation3DR(SMatrix{3, 3, Float64}(1., 0., 0., 0., 1., 0., 0., 0., 1.)) -nvars(::Rotation3DR) = static(3) -update(var::Rotation3DR, updatevec, start=1) = Rotation3DR(var.m * rodrigues(updatevec[start], updatevec[start+1], updatevec[start+2])) - -struct Rotation3DL{T<:Real} <: AbstractRotation3D - m::SMatrix{3, 3, T, 9} -end -Rotation3DL(x, y, z) = Rotation3DL(rodrigues(x, y, z)) -Rotation3DL() = Rotation3DL(SMatrix{3, 3, Float64}(1., 0., 0., 0., 1., 0., 0., 0., 1.)) -nvars(::Rotation3DL) = static(3) -update(var::Rotation3DL, updatevec, start=1) = Rotation3DL(rodrigues(updatevec[start], updatevec[start+1], updatevec[start+2]) * var.m) - -struct UnitVec3D{T<:Real} <: AbstractPoint3D - v::Rotation3DR{T} -end -UnitVec3D() = UnitVec3D(Rotation3DR()) -function UnitVec3D(x::SVector{3, T}) where T - # Initialize y & z axes - ind = findmin(abs, x)[2] - y = cross(SVector{3, T}(1==ind, 2==ind, 3==ind), x) - z = cross(x, y) - # Normalize all the vectors - return UnitVec3D{T}(Rotation3DR(hcat(normalize(x), normalize(y), normalize(z)))) -end -UnitVec3D(x::T, y::T, z::T) where T = UnitVec3D(SVector{3, T}(x, y, z)) -nvars(::UnitVec3D) = static(2) -update(var::UnitVec3D, updatevec, start=1) = UnitVec3D(update(var.v, SVector(0, updatevec[start], updatevec[start+1]))) -getvec(x::UnitVec3D) = view(x.v.m, :, 1) -getind(x::UnitVec3D, ind) = x.v.m[ind,1] - - -abstract type AbstractPose3D end -struct Pose3D{T<:Real} <: AbstractPose3D - rot::Rotation3DR{T} - trans::Point3D{T} -end -Pose3D(rx, ry, rz, tx, ty, tz) = Pose3D(Rotation3DR(rx, ry, rz), Point3D(tx, ty, tz)) -Pose3D() = Pose3D(Rotation3DR(), Point3D()) -nvars(::Pose3D) = static(6) -update(var::Pose3D, updatevec, start=1) = Pose3D(update(var.rot, updatevec, start), update(var.trans, updatevec, start+3)) - -struct UnitPose3D{T<:Real} <: AbstractPose3D - rot::Rotation3DR{T} - trans::UnitVec3D{T} -end -UnitPose3D() = Pose3D(Rotation3DR(), UnitVec3D()) -UnitPose3D(rx::T, ry::T, rz::T, tx::T, ty::T, tz::T) where T = Pose3D(Rotation3DR(rx, ry, rz), UnitVec3D(tx, ty, tz)) -nvars(::UnitPose3D) = static(5) -update(var::UnitPose3D, updatevec, start=1) = UnitPose3D(update(var.rot, updatevec, start), update(var.trans, updatevec, start+3)) - -struct EffPose3D{T<:Real} <: AbstractPose3D - rot::Rotation3DL{T} - camcenter::Point3D{T} -end -EffPose3D(rx, ry, rz, cx, cy, cz) = EffPose3D(Rotation3DL(rx, ry, rz), Point3D(cx, cy, cz)) -EffPose3D() = EffPose3D(Rotation3DL(), Point3D()) -nvars(::EffPose3D) = static(6) -update(var::EffPose3D, updatevec, start=1) = EffPose3D(update(var.rot, updatevec, start), update(var.camcenter, updatevec, start+3)) -inverse(var::EffPose3D) = Pose3D(Rotation3DR(var.rot.m'), var.camcenter) -transform(pose::EffPose3D, point::AbstractPoint3D) = Point3D(pose.rot.m * (getvec(point) - pose.camcenter.v)) - -# Pose conversions -Pose3D(pose::EffPose3D) = Pose3D(Rotation3DR(pose.rot.m), Point3D(pose.rot.m * -pose.camcenter.v)) -Pose3D(pose::Pose3D) = pose -EffPose3D(pose::Pose3D) = EffPose3D(Rotation3DL(pose.rot.m), Point3D(pose.rot.m' * -pose.trans.v)) -EffPose3D(pose::EffPose3D) = pose - -# Transformations on abstract types -project(x::AbstractPoint3D) = SVector(getind(x, 1), getind(x, 2)) ./ getind(x, 3) -inverse(rot::T) where T<:AbstractRotation3D = T(rot.m') -inverse(var::AbstractPose3D) = Pose3D(Rotation3DR(var.rot.m'), Point3D(var.rot.m' * -getvec(var.trans))) -transform(rota::T, rotb::AbstractRotation3D) where T<:AbstractRotation3D = T(rota.m * rotb.m) -transform(rot::AbstractRotation3D, point::AbstractPoint3D) = Point3D(rot.m * getvec(point)) -transform(pose::AbstractPose3D, point::AbstractPoint3D) = pose.rot * point + pose.trans - -# Overload arithmetic operators -Base.:*(rota::AbstractRotation3D, rotb::AbstractRotation3D) = transform(rota, rotb) -Base.:*(rot::AbstractRotation3D, point::AbstractPoint3D) = transform(rot, point) -Base.:*(posea::AbstractPose3D, poseb::AbstractPose3D) = transform(posea, poseb) -Base.:*(pose::AbstractPose3D, point::AbstractPoint3D) = transform(pose, point) -Base.:+(pointa::AbstractPoint3D, pointb::AbstractPoint3D) = Point3D(getvec(pointa) + getvec(pointb)) -Base.:-(pointa::AbstractPoint3D, pointb::AbstractPoint3D) = Point3D(getvec(pointa) - getvec(pointb)) diff --git a/test/VectorRepo.jl b/test/VectorRepo.jl index a79eb1bf..eb31a399 100644 --- a/test/VectorRepo.jl +++ b/test/VectorRepo.jl @@ -40,7 +40,7 @@ end @test NLLSsolver.sumsubset(Float64, rangefun, vr1) == halftotal @test NLLSsolver.sumsubset(Float64, indicesfun, vr1) == halftotal @test NLLSsolver.sumsubset(Float64, bitvecfun, vr1) == halftotal - @test NLLSsolver.sumsubset(Float64, boolvecfun, vr1) == halftotal + @test NLLSsolver.sumsubset(Float64, boolvecfun, vr1) ≈ halftotal # Union container vr2 = NLLSsolver.VectorRepo{Union{Float64, Int, Char}}() @@ -58,5 +58,5 @@ end @test NLLSsolver.sumsubset(Float64, rangefun, vr2) == halftotal @test NLLSsolver.sumsubset(Float64, indicesfun, vr2) == halftotal @test NLLSsolver.sumsubset(Float64, bitvecfun, vr2) == halftotal - @test NLLSsolver.sumsubset(Float64, boolvecfun, vr2) == halftotal + @test NLLSsolver.sumsubset(Float64, boolvecfun, vr2) ≈ halftotal end diff --git a/test/camera.jl b/test/camera.jl deleted file mode 100644 index 51e99ccc..00000000 --- a/test/camera.jl +++ /dev/null @@ -1,49 +0,0 @@ -using NLLSsolver, StaticArrays, ForwardDiff, Test, Static - -function testcamera(cam, x) - # Test image to ideal transformations - y = ideal2image(cam, x) - x_ = image2ideal(cam, y) - @test isapprox(x, x_) - - # Test warping of the weight matrix - x__, W = image2ideal(cam, y, @SMatrix [1. 0.; 0. 1.]) - @test x__ == x_ - @test isapprox(W, ForwardDiff.jacobian(x -> ideal2image(cam, x), x)) - - # Test the update of all zeros returns the same camera - @test cam == update(cam, zeros(dynamic(nvars(cam)))) -end - -@testset "camera.jl" begin - halfimsz = SA[640, 480] - x = SVector(7.0/11., 2.0/3.) - xi = x .* (0.3 * halfimsz) - - # Test pixel to image transformations - y = pixel2image(halfimsz, xi) - @test isapprox(image2pixel(halfimsz, y), xi) - - # Test warping of the weight matrix - y_, W = pixel2image(halfimsz, xi, @SMatrix [1. 0.; 0. 1.]) - @test y_ == y - @test isapprox(W, ForwardDiff.jacobian(x -> image2pixel(halfimsz, x), y)) - - # Test cameras - f = SVector(11.0/13., 17.0/13.) - c = SVector(3.0/5., 5.0/7.) * 0.05 - testcamera(SimpleCamera(f[1]), x) - testcamera(NoDistortionCamera(f, c), x) - testcamera(ExtendedUnifiedCamera(f, c, 0.1, 0.1), x) - testcamera(ExtendedUnifiedCamera(f, c, 0.5, 0.2), x) - - # Test lens distortion model conversion - halfimsz = 1. - fromlens = BarrelDistortion(-1.e-5, -1.e-7) - tolens = convertlens(EULensDistortion(0.1, 0.1), fromlens, halfimsz) - maxerror = 0. - for x in LinRange(0., halfimsz, 100) - maxerror = max(maxerror, abs(ideal2distorted(tolens, x) - ideal2distorted(fromlens, x))) - end - @test maxerror < 1.e-6 -end diff --git a/test/geometry.jl b/test/geometry.jl deleted file mode 100644 index a43213ca..00000000 --- a/test/geometry.jl +++ /dev/null @@ -1,45 +0,0 @@ -using NLLSsolver, LinearAlgebra, StaticArrays, Test - -checkduals(x, y) = NLLSsolver.extractvaldual(vec(x)) == NLLSsolver.extractvaldual(vec(y)) - -@testset "geometry.jl" begin - # Test utility functions - @test NLLSsolver.rodrigues(0., 0., 0.) == I - @test isapprox(NLLSsolver.rodrigues(Float64(pi), 0., 0.), Diagonal(SVector(1., -1., -1.))) - O = NLLSsolver.proj2orthonormal(randn(5, 5)) - @test isapprox(O' * O, I) - - # Test points - updatevec = zeros(3) - point = NLLSsolver.update(NLLSsolver.Point3D(normalize(SVector(0.9, 1.1, -1.0))), updatevec) - pointu = NLLSsolver.update(NLLSsolver.UnitVec3D(NLLSsolver.getvec(point)), updatevec) - @test project(point) == SVector(-0.9, -1.1) - @test project(pointu) == SVector(-0.9, -1.1) - @test point - pointu == Point3D() - - # Test rotations - updatevec = randn(3) - rotr = update(NLLSsolver.Rotation3DR(), updatevec) - rotl = update(NLLSsolver.Rotation3DL(), -updatevec) - @test isapprox(NLLSsolver.getvec((rotl * rotr) * point), NLLSsolver.getvec(point)) - @test isapprox(NLLSsolver.getvec((NLLSsolver.inverse(rotl) * NLLSsolver.inverse(rotr)) * point), NLLSsolver.getvec(point)) - - # Test rotation autodiff updates - updatevec = NLLSsolver.dualzeros(Float64, Val(4)) - T = eltype(updatevec) - updatemat = SMatrix{3, 3, T, 9}(T(1), updatevec[3], -updatevec[2], -updatevec[3], T(1), updatevec[1], updatevec[2], -updatevec[1], T(1)) - rotr = NLLSsolver.Rotation3DR(randn(), randn(), randn()) - rotl = NLLSsolver.Rotation3DL(randn(), randn(), randn()) - @test checkduals(NLLSsolver.update(rotr, updatevec).m, rotr.m * updatemat) - @test checkduals(NLLSsolver.update(rotl, updatevec).m, updatemat * rotl.m) - - # Test poses - updatevec = zeros(6) - pose = NLLSsolver.update(NLLSsolver.Pose3D(rotr, point), updatevec) - poseu = NLLSsolver.update(NLLSsolver.UnitPose3D(rotr, pointu), updatevec) - @test isapprox(NLLSsolver.getvec(pose * (NLLSsolver.inverse(poseu) * point)), NLLSsolver.getvec(point)) - @test isapprox(NLLSsolver.getvec(poseu * (NLLSsolver.inverse(pose) * point)), NLLSsolver.getvec(point)) - posee = NLLSsolver.update(NLLSsolver.EffPose3D(pose), updatevec) - @test isapprox(NLLSsolver.getvec(NLLSsolver.inverse(pose) * (posee * point)), NLLSsolver.getvec(point)) - @test isapprox(NLLSsolver.getvec(pose * (NLLSsolver.inverse(posee) * point)), NLLSsolver.getvec(point)) -end \ No newline at end of file diff --git a/test/marginalize.jl b/test/marginalize.jl deleted file mode 100644 index 6c54e0eb..00000000 --- a/test/marginalize.jl +++ /dev/null @@ -1,92 +0,0 @@ -using NLLSsolver, SparseArrays, StaticArrays, Test, Random, LinearAlgebra - -function test_marginalize(lambda) - # Define the size of test problem - blocksizes = [1, 1, 2, 2, 3, 3, 3, 3, 2, 2, 1, 1] - fromblock = 7 - rows = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 2, 6, 7, 9, 8, 7, 8, 10, 12, 4, 9, 10, 11, 6, 6, 11, 12] - cols = [1, 2, 3, 2, 5, 6, 7, 8, 9, 10, 11, 12, 1, 1, 1, 1, 2, 3, 3, 3, 4, 3, 5, 5, 5, 5, 4, 6, 6] - - # Intitialize a sparse linear system randomly - from = NLLSsolver.MultiVariateLSsparse(NLLSsolver.BlockSparseMatrix{Float64}(sparse(cols, rows, trues(length(rows))), blocksizes, blocksizes), 1:length(blocksizes)) - from.A.data .= randn(length(from.A.data)) - from.b .= randn(length(from.b)) - # Make the diagonal blocks symmetric - for (ind, sz) in enumerate(blocksizes) - if NLLSsolver.validblock(from.A, ind, ind) - diagblock = NLLSsolver.block(from.A, ind, ind, sz, sz) - diagblock .= diagblock * diagblock' - end - end - - # Construct the cropped system - to_d = NLLSsolver.constructcrop(from, fromblock) - @test isa(to_d, NLLSsolver.MultiVariateLSdense) - to_s = NLLSsolver.constructcrop(from, fromblock, true) - @test isa(to_s, NLLSsolver.MultiVariateLSsparse) - NLLSsolver.initcrop!(to_d, from) - NLLSsolver.initcrop!(to_s, from) - - # Check that the crops are correct - hessian = NLLSsolver.symmetrifyfull(from.A) - croplen = sum(blocksizes[1:fromblock-1]) - @test view(hessian, 1:croplen, 1:croplen) == NLLSsolver.symmetrifyfull(to_d.A) - @test view(from.b, 1:croplen) == to_d.b - @test all((to_d.boffsets[1:end-1] .+ blocksizes[1:fromblock-1]) .<= (length(to_d.b) + 1)) - @test view(hessian, 1:croplen, 1:croplen) == NLLSsolver.symmetrifyfull(to_s.A) - @test view(from.b, 1:croplen) == to_s.b - @test all((to_s.boffsets .+ blocksizes[1:fromblock-1]) .<= (length(to_s.b) + 1)) - - # Compute the ground truth variable update - hessian += I * lambda - gtupdate = hessian \ from.b - gtupdate = gtupdate[1:croplen] - - # Compute the ground truth reduced system - N = size(hessian, 2) - S = view(hessian, 1:croplen, croplen+1:N) / view(hessian, croplen+1:N, croplen+1:N) - hessian = view(hessian, 1:croplen, 1:croplen) - S * view(hessian, croplen+1:N, 1:croplen) - gradient = view(from.b, 1:croplen) - S * view(from.b, croplen+1:N) - - # Compute the marginalized system using dynamic block sizes - for block in fromblock:length(from.A.rowblocksizes) - NLLSsolver.marginalize!(to_d, from, block, lambda, Int(from.A.rowblocksizes[block])) - NLLSsolver.marginalize!(to_s, from, block, lambda, Int(from.A.rowblocksizes[block])) - end - - # Check that the results are the same - hess_d = NLLSsolver.symmetrifyfull(to_d.A) - hess_s = NLLSsolver.symmetrifyfull(to_s.A) - @test hess_d ≈ hess_s - @test to_d.b == to_s.b - - # Check that the reduced systems give the correct variable update - @test hessian \ gradient ≈ gtupdate - @test (hess_d + I * lambda) \ to_d.b ≈ gtupdate - @test (hess_s + I * lambda) \ to_s.b ≈ gtupdate - - # Reset the 'to' systems - NLLSsolver.initcrop!(to_d, from) - NLLSsolver.initcrop!(to_s, from) - - # Compute the marginalized system using static block sizes - NLLSsolver.marginalize!(to_d, from, SVector(lambda)) - NLLSsolver.marginalize!(to_s, from, SVector(lambda)) - - # Check that the results are the same - hess_d = NLLSsolver.symmetrifyfull(to_d.A) - hess_s = NLLSsolver.symmetrifyfull(to_s.A) - @test hess_d ≈ hess_s - @test to_d.b == to_s.b - - # Check that the reduced systems give the correct variable update - @test (hess_d + I * lambda) \ to_d.b ≈ gtupdate - @test (hess_s + I * lambda) \ to_s.b ≈ gtupdate - return -end - -@testset "marginalize.jl" begin - Random.seed!(1) - test_marginalize(0.0) - test_marginalize(1.0) -end diff --git a/test/optimizeba.jl b/test/optimizeba.jl index 257bc013..615692d4 100644 --- a/test/optimizeba.jl +++ b/test/optimizeba.jl @@ -1,41 +1,32 @@ using NLLSsolver, Test, Static, StaticArrays, SparseArrays, Random, LinearAlgebra -# Simple reprojection residual -struct ProjError <: AbstractResidual - measurement::SVector{2, Float64} # Landmark image coordinates (x, y) - varind::SVector{2, Int} # Variable indices for the residual (image, landmark) -end -ProjError(meas, cam, lndm) = ProjError(SVector{2}(meas[1], meas[2]), SVector{2, Int}(cam, lndm)) -NLLSsolver.ndeps(::ProjError) = static(2) # Residual depends on 2 variables -NLLSsolver.nres(::ProjError) = static(2) # Residual vector has length 2 -NLLSsolver.varindices(res::ProjError) = res.varind -NLLSsolver.getvars(res::ProjError, vars::Vector) = vars[res.varind[1]]::EffPose3D{Float64}, vars[res.varind[2]]::Point3D{Float64} -NLLSsolver.computeresidual(res::ProjError, pose::EffPose3D, X::Point3D) = project(pose * X) - res.measurement -Base.eltype(::ProjError) = Float64 +# Simple affine projection transform +NLLSsolver.generatemeasurement(pose::EuclideanVector{6, T}, X::EuclideanVector{3, U}) where {T, U} = SVector(dot(@inbounds(view(pose, NLLSsolver.SR(1, 3))), X), dot(@inbounds(view(pose, NLLSsolver.SR(4, 6))), X)) function create_ba_problem(ncameras, nlandmarks, propvisible) - problem = NLLSProblem(Union{EffPose3D{Float64}, Point3D{Float64}}, ProjError) + problem = NLLSProblem(Union{EuclideanVector{6, Float64}, EuclideanVector{3, Float64}}, SimpleError2{2, Float64, EuclideanVector{6, Float64}, EuclideanVector{3, Float64}}) # Generate the cameras on a unit sphere, pointing to the origin + camoffset = SVector(1.0, 0.0, 0.0, 0.0, 1.0, 0.0) for i = 1:ncameras - camcenter = normalize(randn(SVector{3, Float64})) - addvariable!(problem, EffPose3D(Rotation3DL(UnitVec3D(-camcenter).v.m), Point3D(camcenter))) + addvariable!(problem, randn(EuclideanVector{6, Float64}) .+ camoffset) end # Generate the landmarks in a unit cube centered on the origin + lmoffset = SVector(-0.5, -0.5, 10.0) for i = 1:nlandmarks - addvariable!(problem, Point3D{Float64}(rand(SVector{3, Float64}) .- 0.5)) + addvariable!(problem, rand(EuclideanVector{3, Float64}) .+ lmoffset) end # Generate the measurements visibility = abs.(repeat(vec(1:ncameras), outer=(1, nlandmarks)) .- LinRange(2, ncameras-1, nlandmarks)') visibility = visibility .<= sort(vec(visibility))[Int(ceil(length(visibility)*propvisible))] for camind = 1:ncameras - camera = problem.variables[camind]::EffPose3D{Float64} + camera = problem.variables[camind]::EuclideanVector{6, Float64} for (landmark, tf) in enumerate(view(visibility, camind, :)') if tf landmarkind = landmark + ncameras - addcost!(problem, ProjError(project(camera * problem.variables[landmarkind]::Point3D{Float64}), camind, landmarkind)) + addcost!(problem, SimpleError2{EuclideanVector{6, Float64}, EuclideanVector{3, Float64}}(generatemeasurement(camera, problem.variables[landmarkind]::EuclideanVector{3, Float64}), camind, landmarkind)) end end end @@ -46,10 +37,10 @@ end function perturb_ba_problem(problem, pointnoise, posenoise) for ind in 1:lastindex(problem.variables) - if isa(problem.variables[ind], Point3D) - problem.variables[ind]::Point3D{Float64} = update(problem.variables[ind]::Point3D{Float64}, randn(SVector{3, Float64}) * pointnoise) + if isa(problem.variables[ind], EuclideanVector{3, Float64}) + problem.variables[ind]::EuclideanVector{3, Float64} += randn(SVector{3, Float64}) * pointnoise else - problem.variables[ind]::EffPose3D{Float64} = update(problem.variables[ind]::EffPose3D{Float64}, randn(SVector{6, Float64}) * posenoise) + problem.variables[ind]::EuclideanVector{6, Float64} += randn(SVector{6, Float64}) * posenoise end end return problem @@ -63,11 +54,11 @@ end # Test reordering the costs problem = perturb_ba_problem(problem, 0.003, 0.0) costbefore = cost(problem) - NLLSsolver.reordercostsforschur!(problem, isa.(problem.variables, Point3D{Float64})) + NLLSsolver.reordercostsforschur!(problem, isa.(problem.variables, EuclideanVector{3, Float64})) @test cost(problem) ≈ costbefore # Optimze just the landmarks - NLLSsolver.optimizesingles!(problem, NLLSOptions(), Point3D{Float64}) + NLLSsolver.optimizesingles!(problem, NLLSOptions(), EuclideanVector{3, Float64}) @test NLLSsolver.cost(problem) < 1.e-15 # Optimize problem diff --git a/test/runtests.jl b/test/runtests.jl index b9787617..cc853fc0 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1,8 +1,6 @@ using Test @testset "NLLSsolver.jl" begin - include("camera.jl") - include("geometry.jl") include("BlockSparseMatrix.jl") include("VectorRepo.jl") include("robust.jl") @@ -13,5 +11,4 @@ using Test include("nonsquaredcost.jl") include("adaptivecost.jl") include("utils.jl") - include("marginalize.jl") end