# Dual constraint non-linear optimization for SLAM

I am trying to achieve a nonlinear optimization for SLAM on a function that contains two different constraints, like this : $$T_{opt} = \underset{T}{\arg\min} \sum_i(\left\lVert T.X_i - x_i \right\rVert)^2 + \sum_j(\left\lVert\pi(T.Y_i) - y_i\right\rVert)^2$$

• T : Affine transformation (Rotation + Translation),
• X_i some source's 3d points, x_i are target's 3d points that correspond to X_i,
• Y_i some source's 3d points (different than X_i), y_i are target's 2d points that correspond to Y_i,
• PI function is the projection of a 3d point to the image plane.

My questions are :

• The two constraints have different units, first give an error in meter, second give an error in pixel. Is it a problem for the optimization to minimize two constraints with different scale units ?
• Is it a problem if one constraint have lot more data to deal with ? For example, first constraint would have 10 points for the optimization, second constraint would have 50 points. Is it a problem if sets of data have complete different sizes ?

Regards

The two constraints have different units, first give an error in meter, second give an error in pixel. Is it a problem for the optimization to minimize two constraints with different scale units ?

For your problem I think it will be fine(if your optimizer uses Levenberg Marquadt it will implicitly handle it). But generally yes this is a problem you have to deal with any sort of optimization algorithm(not limited to SLAM). The topic is called feature scaling and comes up a lot in machine learning.

Is it a problem if one constraint have lot more data to deal with ? For example, first constraint would have 10 points for the optimization, second constraint would have 50 points. Is it a problem if sets of data have complete different sizes ?

Other things being equal then the constraint with more samples will have a bigger effect on your output. One way to fix this is to add a weighting($$\lambda$$) parameter that will reduce the effect of that constraint.

So your equation would become something like this

$$T_{opt} = \underset{T}{\arg\min} \sum_i(\left\lVert T.X_i - x_i \right\rVert)^2 + \lambda \sum_j(\left\lVert\pi(T.Y_i) - y_i\right\rVert)^2$$

• Thank you for this very detailled answer. Two questions : 1) Why does Levenberg Marquardt handle scale differences in data ? 2) Do you know a theorital way to choose Lambda or do I have to set it after experimentations ? Thanks again Apr 2 '20 at 7:52
• 1) An in depth explanation is beyond me. Essentially the damping parameter will adjust differently for each value, and will therefore scale certain values more or less depending on what is needed. 2) Theoretical really doesn't exist. If you know you want to weight the 2 parts the same you could use something as simple as the average($\lambda=\frac{1}{j}$). To get the best value you would do something called hyperparameter optimization. Again pops up in machine learning a lot, and there are a bunch of python libraries that can do it. Apr 2 '20 at 18:58
• Though I do want to stress that your problem is simple enough that you don't have to worry about these details. Just put your problem as you stated it into Ceres and it should give you an answer. Apr 2 '20 at 19:01