/* ************************************************************************* */
Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
OptionalJacobian<6,6> H) {
if (H) {
H->setZero();
for (int i = 0; i < 6; ++i) {
Vector6 dxi;
dxi.setZero();
dxi(i) = 1.0;
Matrix6 GTi = adjointMap(dxi).transpose();
H->col(i) = GTi * y;
}
}
return adjointMap(xi).transpose() * y;
}
/* ************************************************************************* */
PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
OptionalJacobian<9, 6> Dtrans) const {
// Pose3 transform is just compose
Matrix6 D_newpose_trans, D_newpose_pose;
Pose3 newpose = trans.compose(pose(), D_newpose_trans, D_newpose_pose);
// Note that we rotate the velocity
Matrix3 D_newvel_R, D_newvel_v;
Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v);
if (Dglobal) {
Dglobal->setZero();
Dglobal->topLeftCorner<6,6>() = D_newpose_pose;
Dglobal->bottomRightCorner<3,3>() = D_newvel_v;
}
if (Dtrans) {
Dtrans->setZero();
Dtrans->topLeftCorner<6,6>() = D_newpose_trans;
Dtrans->bottomLeftCorner<3,3>() = D_newvel_R;
}
return PoseRTV(newpose, newvel);
}
