How to creating a Sophus::SE3f variable from a Eigen::Matrixf variable?

[Mechazo11]

Hi, I have a 4x4 transformation matrix defined as a Eigen::Matrixf variable. I perused into the SE3.hpp file that shows that there is a copy constructor defined as follows

/// Constructor from 4x4 matrix
  ///
  /// Precondition: Rotation matrix needs to be orthogonal with determinant
  ///               of 1. The last row must be ``(0, 0, 0, 1)``.
  ///
  SOPHUS_FUNC explicit SE3(Matrix4<Scalar> const& T)
      : so3_(T.template topLeftCorner<3, 3>()),
        translation_(T.template block<3, 1>(0, 3)) {
    SOPHUS_ENSURE((T.row(3) - Matrix<Scalar, 1, 4>(Scalar(0), Scalar(0),
                                                   Scalar(0), Scalar(1)))
                          .squaredNorm() < Constants<Scalar>::epsilon(),
                  "Last row is not (0,0,0,1), but ({}).",
                  SOPHUS_FMT_ARG(T.row(3)));
  }

However when I invoke this constructor using the following statement

Sophus::SE3f mTca = Sophus::SE3(Tcaru); where Tcaru is a 4x4 Eigen Matrix, I am getting the following error

/home/icore_base/catkin_ws/src/orb_slam3_ros/orb_slam3/src/Tracking.cc:2596:36: error: missing template arguments before ‘(’ token
 2596 |     Sophus::SE3f mTca = Sophus::SE3(Tcaru); // 4x4 constructor

How do I solve this error?

With best
@Mechazo11

[DmitriyKorchemkin]

Sophus::SE3f is a specialization of template class Sophus::SE3<T>

You need to use either

Sophus::SE3f mTca = Sophus::SE3f(Tcaru);

or just

Sophus::SE3f mTca(Tcaru);

[Mechazo11]

@DmitriyKorchemkin Thanks, it works now. Before passing this SE3f object out to another node, I want to double-check if I copied the rotation and translation vector correctly from the Tcaru Eigen matrix. Is there any command to print a SE3f object into the console?

[DmitriyKorchemkin]

There is a method Sophus::SE3<T>::matrix() that returns 4x4 matrix: https://github.com/strasdat/Sophus/blob/main-1.x/sophus/se3.hpp#L273-L279
It should be almost identical (up to non-orthogonality of top-left corner of the input matrix).

[Mechazo11]

@DmitriyKorchemkin thank you very much. I got the initialization and printing to console problems sorted thanks to your direction.

Have a wonderful day.