#include <gtsam/geometry/Pose3.h>
#include <iostream>
int main(int argc, char** argv) {// B 的旋转量为绕 x 轴旋转 180 度gtsam::Pose3 B =gtsam::Pose3(gtsam::Rot3(0, 1, 0, 0), gtsam::Point3(1, 2, 0));// A 的旋转量为绕 z 轴旋转 180 度gtsam::Pose3 A =gtsam::Pose3(gtsam::Rot3(0, 0, 0, 1), gtsam::Point3(2, 1, 0));auto it = A.compose(B);std::cout << it << std::endl;return 0;
}

终端输出结果

说明 it 以动坐标系为基准：先从零点平移 A 的平移量 gtsam::Point3(2, 1, 0)，再旋转 A 的旋转量 gtsam::Rot3(0, 0, 0, 1)，再平移 B 的平移量 gtsam::Point3(1, 2, 0)，再旋转 B 的旋转量 gtsam::Rot3(0, 1, 0, 0)