Question about symbolic computation: About addExpressionFactor

[Zkjoker]

I am performing a symbolic computation, and one of the steps involves extracting the third column from a rotation matrix. The subsequent calculations are based on this third column. Could you please help me check if the code is correct?

inline Point3 SymR3(const Rot3& rot, OptionalJacobian<3, 3> H)
{
if(H) *H = I_3x3;
return rot.r3();
}

inline Vector3_ ForwardDirection(const Pose3_& pose)
{
Rot3_ rot = Rot3_(&Pose3::rotation, pose);
Vector3_ z_direction = Point3_(&SymR3,rot);
return z_direction;
}

But I found that using this ForwardDirection function for symbolic computation seems to always yield inaccurate results. Please help me check if there is any issue, thank you.

[ProfFan]

Hi, do you mean the result OR the Jacobian is inaccurate?

[Zkjoker]

The optimize result is incorrect.
My question is: 1. In this function SymR3, what should the Jacobian matrix be set to? I have tried the identity matrix, a matrix with the third column as 1, and a matrix with the last element as 1, but none of them are correct.
2. For objects of type Rot3, I can call r3() function to get the third column. But for objects of type Expression, I cannot directly call the r3 method. It seems like I need to wrap it into a function for symbolic expression calculation. What should the Jacobian matrix be set to for the call to the r3 method? Are there any other methods to achieve the desired calculation?

[Zkjoker]

I use this way that can calculate the correct optimize result:

inline Vector3 SymR3(const Rot3& rot,OptionalJacobian<3, 3> H1)    
{
  if(H1)
  {
    Matrix3 H_d;
    H_d << 0,1,0,
           -1,0,0,
           0,0,0;
    *H1 = rot.matrix()* H_d;
  }
  return rot.rotate(Vector3(0,0,1));
}

that is because I have read the https://groups.google.com/g/gtsam-users/c/h1BcwFoh7_8, and read the math.pdf.
But I still don't know how to compute the partial derivatives if I obtain a slice of a matrix (i.e., getting a submatrix from a larger matrix).