WIP: add homogenization term to D = 1 lifting to QCQP factor

[avinashresearch1]

No description provided.

[avinashresearch1]

Discussion on conversion to QCQP:

  static Matrix RightProductMatrix(const Matrix& right) {
    constexpr int AmbientDim = N * N;
    const Matrix I = Matrix::Identity(N, N);
    Matrix result = Matrix::Zero(AmbientDim, AmbientDim);
    for (int column = 0; column < N; ++column) {
      for (int sourceColumn = 0; sourceColumn < N; ++sourceColumn) {
        result.block(column * N, sourceColumn * N, N, N) =
            right(sourceColumn, column) * I;
      }
    }
    return result;
  }

would it be reasonable to use: return Eigen::kroneckerProduct(right.transpose(), Matrix::Identity(N, N)).eval(); The downside is it involves including #include <unsupported/Eigen/KroneckerProduct>

[avinashresearch1]

To keep the math consistent, we keep the two steps: 1. lifting, 2. homogenization & truncation separate. This is because the lifting step is identical for SE(2), SE(3) and so on, but homogenization & truncatio is lie group specific

Step 1: Lifting

Derivation:
Start from the Frobenius between-factor error

e(R1, R2) = 0.5 * || R2 - R1 M ||_F^2,

where M is the measured relative transform/rotation. Using column-major
vectorization,

  || R2 - R1 M ||_F^2
    = || vec(R2 - R1 M) ||_2^2
    = || vec(R2) - vec(R1 M) ||_2^2.

The standard vectorization identity is

vec(X Y Z) = (Z^T kron X) vec(Y).

For the right-product term R1 M, take X = I, Y = R1, Z = M:
vec(R1 M) = (M^T kron I) vec(R1).
So define
A = M^T kron I,
which is what RightProductMatrix(M) constructs. This A variable is also used in the qcqpFactorsForVec function.

Then the residual can be written
linearly in the full vectorized variables:

r = vec(R2) - A vec(R1).
For the stacked vector

  z = [vec(R1);
       vec(R2)],

we build

  B = [-A  I],

  B z = vec(R2) - A vec(R1)
      = vec(R2 - R1 M).

The noise model whitens this residual by left-multiplying B:
B_w = whiten(B).
The quadratic cost matrix is then
Q = B_w^T B_w,
so the factor error is represented as
'
e(z) = 0.5 * z^T Q z.
This derivation intentionally happens in full vectorized coordinates. The identity
vec(X Y Z) = (Z^T kron X) vec(Y)
`
only has the stated dimensions and simple Kronecker form for full
vectorization. If we insert homogenization or remove fixed matrix rows before
this derivation, the dimensions no longer match the standard identity and every
group needs custom selection/projection algebra inside the Kronecker formula.

2. Lie-group specific homogenization and truncation:

For Rot2, there is no truncation.

the lifted coordinate layout is
x = [1; vec(R)] in R^5.
So after computing the old full-vectorized Q over
[vec(R1); vec(R2)],
we embed it into the larger lifted coordinate layout
[1; vec(R1); 1; vec(R2)]
by shifting each vec block by one row/column. In code this embedded matrix is
called Q_trunc_hom.

[dellaert]

On Kronecker: Yes, but one caveat: GTSAM_WITH_EIGEN_UNSUPPORTED defaults OFF for installing bundled unsupported headers. So avoid adding unsupported Eigen includes to public GTSAM headers. Inside .cpp files is fine.

[dellaert]

Please use Vector5

[dellaert]

we should get rid of AmbientDim! It's already defined as Dim in this class.

[dellaert]

This works for SO(2), SO(3), but dow we truly use 17 rows for SE(3)?

[avinashresearch1]

No, that is what I'm trying to explain in the comment: #2549 (comment)

we should separate lifting and homogenization/truncation. So we should lift to 16 rows, then truncate the 4 rows and then add the homogenization row. Because the lifting math is the same. It is only that we truncate for SE(3) but not for SO(2), SO(3).

[dellaert]

Yes - I think I am saying the same thing. We commit to 17 for SE(3) in the D=1 case, and make sure factors adhere to it. So, we should go down to 17 here, in FrobeniusFactor

[dellaert]

Apologies: commit to 13 !

[avinashresearch1]

The 17 is a temporary thing that exists in memory before it is truncated to 13

[dellaert]

Excellent. BTW my comment about AmbientDim still holds. Please remove and change to Dim everywhere.