Inverse using Expressions differs from normal inverse

[thorstink]

Description

I tried inverting rotations and poses using gtsam expressions. They don't seem to work.. (giving different values from expected). However, it might also be my lack of knowledge on the inner workings of the lie-stuff and I am just making a really novice faulty assumption on what I'm doing?

I also noticed some commit-messages from @dellaert about Pose3 losing mojo and wrong derivatives around identity? I guess this might be something related?

Steps to reproduce

#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/expressions.h>

using namespace gtsam;

int main() {

  using symbol_shorthand::R;
  using symbol_shorthand::T;

  double y_offset = -1;
  Rot3 bRs = Rot3(Rot3::Ry(M_PI / 2)) * Rot3::Rz(0.2);
  Pose3 bTs(bRs, Point3(0, y_offset, 0));

  Values values;
  values.insert(T(0), bTs);
  values.insert(R(0), bRs);

  Pose3_((&LieGroup<Pose3, 6>::inverse, Pose3_(T(0))))
      .value(values)
      .print("\nT expression liegroup value\n");

  (values.at<Pose3>(T(0)).inverse()).print("\nT just composition\n");

  Rot3_((&LieGroup<Rot3, 3>::inverse, Rot3_(R(0))))
      .value(values)
      .print("\nR expression liegroup value\n");

  (values.at<Rot3>(R(0)).inverse()).print("\nR just composition\n");

  
  return 0;
}

T expression liegroup value
R:
[
 	6.00118e-17 -1.2165e-17           1;
    	   0.198669    0.980067           0;
    	  -0.980067    0.198669 6.12323e-17
  ]
[0, -1, 0]';
T just composition
R:
[
 	6.00118e-17    0.198669   -0.980067;
    	-1.2165e-17    0.980067    0.198669;
    	          1           0 6.12323e-17
  ]
[0.198669, 0.980067, 0]';
R expression liegroup value
[
 	6.00118e-17 -1.2165e-17           1;
    	   0.198669    0.980067           0;
    	  -0.980067    0.198669 6.12323e-17
  ]

R just composition
[
 	6.00118e-17    0.198669   -0.980067;
    	-1.2165e-17    0.980067    0.198669;
    	          1           0 6.12323e-17
  ]

Expected behavior

I expected that the inverses would be the same?

Environment

Latest and greatest GTSAM. Ubuntu 18.04.

Additional information

[dellaert]

Nothing to do with Pose3 mojo. But the above is concerning! What happens if you use &Pose3::inverse Rather than &LieGroup<Pose3, 6>::inverse ?

[thorstink]

In that case the compiler doesn't get it;

expr.cc:20:11: error: left operand of comma operator cannot resolve address of overloaded function
   Pose3_((&Pose3::inverse, Pose3_(T(0))))
           ^~~~~~
expr.cc:26:10: error: left operand of comma operator cannot resolve address of overloaded function
   Rot3_((&Rot3::inverse, Rot3_(R(0))))

I also did it the same way recently some expressions were added to the navigation-part; but that gave the same incorrect results. it looks as if it simply doesn’t invert the rotation..?

[thduynguyen]

You have extra pairs of parentheses. I guess it's a rare pitfall of C++ called comma operator: https://en.cppreference.com/w/cpp/language/operator_other

[thduynguyen]

This works fine for me:

double y_offset = -1;
  Rot3 bRs = Rot3(Rot3::Ry(M_PI / 2)) * Rot3::Rz(0.2);
  Pose3 bTs(bRs, Point3(0, y_offset, 0));

  Values values;
  values.insert(T(0), bTs);
  values.insert(R(0), bRs);

  Pose3_(&LieGroup<Pose3, 6>::inverse, Pose3_(T(0)))
      .value(values)
      .print("\nT expression liegroup value\n");

  (values.at<Pose3>(T(0)).inverse()).print("\nT just composition\n");

  Rot3_(&LieGroup<Rot3, 3>::inverse, Rot3_(R(0)))
      .value(values)
      .print("\nR expression liegroup value\n");

  (values.at<Rot3>(R(0)).inverse()).print("\nR just composition\n");

[thorstink]

I can confirm it works :-)

I thought I knew C++....