gtsam使用

1、关于里面的pose的between的说明
gtSAMgraph.add(gtsam::BetweenFactorgtsam::Pose3(prev_node_idx, curr_node_idx, poseFrom.between(poseTo), odomNoise));

这个poseFrom.between(poseTo)得到的是poseTo相对于poseFrom的姿态变化，即以poseFrom为基准坐标，poseTo在poseFrom基准坐标下的坐标。
即如果poseFrom的在world下的姿态为Tf_to_w，poseTo在world下的姿态为Tt_to_w，那么
poseFrom.between(poseTo) = Tf_to_w.inverse()*Tt_to_w = Tt_to_f

这个也符合正向理解，以from为原点出发，到达to节点的姿态变化。