#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)