PCL点类型转换为gtsam姿态类型

将PCL中的点类型转换为gtsam中的姿态类型。

gtsam::Pose3 pclPointTogtsamPose3(PointTypePose thisPoint) {
    return gtsam::Pose3(gtsam::Rot3::RzRyRx(double(thisPoint.roll), double(thisPoint.pitch), double(thisPoint.yaw)),
                        gtsam::Point3(double(thisPoint.x), double(thisPoint.y), double(thisPoint.z)));
}

该代码使用gtsam::Pose3表示姿态，并使用gtsam::Rot3::RzRyRx函数将欧拉角转换为旋转矩阵。

参数:

• thisPoint: PCL中的点类型，包含x, y, z坐标以及roll, pitch, yaw欧拉角。

返回值:

• gtsam::Pose3类型，表示该点的姿态。