Convert a SE3 matrix to Eigen::Matrixf matrix?

[Mechazo11]

Hi,

Is there a method built into Sophus library that converts a SE3 matrix from Sophus to Eigen::Matrixf?

For computing Frobenius norm, I was thinking of converting a 4x4 SE3 matrix into an Eigen equivalent for being able to use Eigen's JacobiSVD class for computing singular values and from them, the Frobenius norm.

With best,
@Mechazo11

[NikolausDemmel]

Sophus stores an SE3 element as quaternion and translation vector, but you can get the corresponding 4x4 transformation matrix with the .matrix() member function:

Sophus/sophus/se3.hpp

Lines 263 to 279 in caf27cf

	/// Returns 4x4 matrix representation of the instance.
	///
	/// It has the following form:
	///
	/// | R t |
	/// | o 1 |
	///
	/// where ``R`` is a 3x3 rotation matrix, ``t`` a translation 3-vector and
	/// ``o`` a 3-column vector of zeros.
	///
	SOPHUS_FUNC Transformation matrix() const {
	Transformation homogenious_matrix;
	homogenious_matrix.template topLeftCorner<3, 4>() = matrix3x4();
	homogenious_matrix.row(3) =
	Matrix<Scalar, 1, 4>(Scalar(0), Scalar(0), Scalar(0), Scalar(1));
	return homogenious_matrix;
	}

[Mechazo11]

@NikolausDemmel thank you for the information.

If I understood the code, there is a statement using Transformation = Matrix<Scalar, N, N>; at the beginning of the se3.hpp file. Thus, the return homogenious_matrix is giving back a matrix in Eigen::Matrix<> correct?

Just want to make sure as I am fairly new to OOP in C++ and usage of the Eigen library.

[NikolausDemmel]

Yes.

[Mechazo11]

@NikolausDemmel Thanks for helping me in figuring this out. Yes, I can confirm Sophus::SE3f.matrix() method returns a Eigen::Matrixf object