Simple bundle adjustment: array in nonlinear expression?

Hi there!

I’m here since I find Julia, and JuMP in particular, very impressive and I suspect it should be a very good fit for SLAM (Simultaneous Localization and Mapping), where C++ is still the popular choice. In particular, I think Julia would be ideal to create programs and algorithms that are both performant and readable/learnable.

Now, I’m trying my hand at implementing a basic part of many SLAM algorithms, which is bundle adjustment. I read a bunch of papers and, long story short, I’m trying to use JuMP to solve the problem:

\underset{\mathbf{C} ,\ \mathbf{W}}{\min}\sum\nolimits ^{n_{c}}_{i=1}\sum\nolimits ^{n_{p}}_{j=1} d^{2}(\mathbf{F}_{i,j} ,\mathbf{C}_{i}\mathbf{W}_{j})

where:

  • \mathbf{F}_{i,j} = projection of j-th 3D point to the i-th camera
  • \mathbf{C}_{i} = i-th camera matrix (composition of the calibration matrix \mathbf{K}, and the camera’s own rotation and translation)
  • \mathbf{W}_j = j-th 3D point in world coordinate frame, homogeneous coordinates

I modeled this problem in JuMP as the following non-linear objective (all cam* and wpoints are @variables):

    @variable(model, cost)
    @NLconstraint(model, cost ==
        sum(distance_sq(
                matches[j, i, :],
                project(
                    camfl[i],
                    campp[i,1], campp[i,2],
                    camloc[i,:],
                    camrot[i,:],
                    wpoints[j,:]))
            for i = 1:ncameras
            for j = 1:npoints))
    @NLobjective(model, Min, cost)

The code is in this Gist.

The problem is that I get the following error:

Unexpected array JuMP.VariableRef[camloc[1,1], camloc[1,2], camloc[1,3]] in nonlinear expression. Nonlinear expressions may contain only scalar expressions.

As far as I can tell, I’m using the “auxiliary variable + constraint”, like the documentation suggests (it also mentions splatting, but I don’t see how it’s applicable).

So… what am I getting wrong? Can I express the optimization objective in any other way?

I should mention that I’m only experienced in computer engineering, and my knowledge of geometry and mathematical optimization is rusty.

I realize some extra context that I forgot to add might be necessary to understand the problem; of course, I’m available to clarify anything as soon as I can.

Thank you very much in advance. Keep up the (very) good work!

Sebastiano, changing the subject a bit, could you try our solver CaNNOLeS.jl? We developed this package for Constrained Nonlinear Least Squares and we hoped to test with bundle adjustment but never got around to do it.
We’d love to see how our method fares in the problem.

Regarding the JuMP issue, I think you need to register your function: http://www.juliaopt.org/JuMP.jl/v0.19.2/nlp/#User-defined-Functions-1

Best

2 Likes

As a Sicilian, I can’t forego the opportunity of using a package with such name! :joy: This being said, please keep in mind that I’m a total beginner in bundle adjustment, nonlinear optimization, and possibly more of the maths involved. I’ll do it for sure as soon as I get the chance, just don’t expect a paper from me any time soon :wink:

Ah! Well, I did have that problem (with a more explicit error message), but I think I solved it… I called register for project, distance_sq and their dependencies. It’s possible to see that in the gist. After that, it switched to complaining about the array in a nonlinear expression. Thank you, nevertheless; I’ll check the code again as soon as I get to my laptop again.

Your functions must take scalar inputs: http://www.juliaopt.org/JuMP.jl/v0.19.2/nlp/#User-defined-functions-with-vector-inputs-1

1 Like

Ohhh damn it! I totally overlooked that when I read the docs!

That’s definitely a mistake that is there in the code. I’ll fix it as soon as I can.

I’ll report back! Thanks a lot!