[WIP] Add LM Pose3Example and G2O 3D Reader #85
Conversation
|
One important note here: the error (referring to the CGLS book Bjorck96) should be calculated as |
|
After 30 GN steps, Cost 185s of runtime, 30 iterations, cross-module optimization enabled (1.3s, 7 iterations with GTSAM). Since we still do not have a noise model, it's not a particularly fair comparison in number of iterations, but the performance difference in time per iteration is still there (6s v.s. 0.18s) Test environment: |
|
Add reference to #42 |
| /// | | | ||
| /// z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) | ||
| /// Vehicle at p0 is looking towards y axis (X-axis points towards world y) | ||
| func circlePose3(numPoses: Int = 8, radius: Double = 1.0) -> Values { |
There was a problem hiding this comment.
Could this create a VariableAssignments directly so that you don't have to convert it later?
There was a problem hiding this comment.
Modified to return (hexagonId, hexagon), but I still feel that Frank may disagree with the current TypedID API as we effectively lost control of what the ID will be.
| @@ -62,7 +62,7 @@ public struct NewJacobianFactor< | |||
| } | |||
|
|
|||
| public func errorVector(at x: Variables) -> ErrorVector { | |||
| return errorVector_linearComponent(x) + error | |||
| return error - errorVector_linearComponent(x) | |||
There was a problem hiding this comment.
I'm pretty confused by the signs.
I see that we need to calculate b - Ax, but I'm not sure if errorVector should be the method that does this.
Changing errorVector to b - Ax changes the meaning of linearization (the comment on NewFactorGraph.linearized implies that errorVector should be Ax + b).
Also, changing errorVector to b - Ax makes it so that errorVector_linearComponent isn't actually the linear component of errorVector and more.
Maybe we should make a new method called residual that calculates b - Ax?
Or we could negate the implementations of errorVector_linearComponent and errorVector_linearComponent_adjoint, and leave errorVector(at: x) = errorVector_linearComponent(x) + error. Then we'd also have to update the comment on NewFactorGraph.linearized to say:
/// The linear approximation satisfies the approximate equality:
/// `self.linearized(at: x).errorVectors(at: dx)` ≈ `self.errorVectors(at: x.moved(along: -1 * dx))`
/// where the equality is exact when `dx == x.linearizedZero`.
There was a problem hiding this comment.
Yea I feel a bit confused too :( IMHO, in CGLS the objective is to reach the solution where Ax=b, thus by moving Ax to the right we have 0 = b - Ax(the "error"). In GN we approximate the L2 error function (which should be >=0) f(x) by its linearization at x0, where f(dx+x_0)=Ax+b (Jacobian A, bias b), and we arrived at the same equation as in CGLS.
I think error here is confusing as it is actually the "error of error", as what we linearized to the JacobianFactor IS the error. However, I am not sure if changing this is actually a good idea, as if we want linear factors to be nonlinear compatible we need the protocol conformances.
There was a problem hiding this comment.
nonlinear error: |h(x)-z|
linear error |Ax-b|
|
I merged so we can start using the LM solver right now, and do optimizations later ((( |
No description provided.