20 #include <gtsam/config.h>
72 R_(
T(0, 0),
T(0, 1),
T(0, 2),
T(1, 0),
T(1, 1),
T(1, 2),
T(2, 0),
T(2, 1),
73 T(2, 2)), t_(
T(0, 3),
T(1, 3),
T(2, 3)) {
79 OptionalJacobian<6, 3> Ht = {});
96 void print(
const std::string&
s =
"")
const;
115 return Pose3(R_ *
T.R_, t_ + R_ *
T.t_);
142 static Vector6 Logmap(
const Pose3&
pose, OptionalJacobian<6, 6> Hpose = {});
148 Matrix6 AdjointMap()
const;
156 Vector6 Adjoint(
const Vector6& xi_b,
157 OptionalJacobian<6, 6> H_this = {},
158 OptionalJacobian<6, 6> H_xib = {})
const;
161 Vector6 AdjointTranspose(
const Vector6&
x,
162 OptionalJacobian<6, 6> H_this = {},
163 OptionalJacobian<6, 6> H_x = {})
const;
180 static Matrix6 adjointMap(
const Vector6&
xi);
185 static Vector6
adjoint(
const Vector6&
xi,
const Vector6&
y,
186 OptionalJacobian<6, 6> Hxi = {},
187 OptionalJacobian<6, 6> H_y = {});
196 static Vector6 adjointTranspose(
const Vector6&
xi,
const Vector6&
y,
198 OptionalJacobian<6, 6> H_y = {});
201 static Matrix6 ExpmapDerivative(
const Vector6&
xi);
204 static Matrix6 LogmapDerivative(
const Pose3&
xi);
221 static Matrix3 ComputeQforExpmapDerivative(
222 const Vector6&
xi,
double nearZeroThreshold = 1
e-5);
235 return (
Matrix(4, 4) << 0., -wz, wy,
vx, wz, 0., -wx,
vy, -wy, wx, 0., vz, 0., 0., 0., 0.).finished();
250 {}, OptionalJacobian<3, 3> Hpoint = {})
const;
272 {}, OptionalJacobian<3, 3> Hpoint = {})
const;
286 const Rot3&
rotation(OptionalJacobian<3, 6> Hself = {})
const;
315 OptionalJacobian<6, 6> HaTb = {})
const;
322 OptionalJacobian<6, 6> HwTb = {})
const;
330 OptionalJacobian<1, 3> Hpoint = {})
const;
337 double range(
const Pose3&
pose, OptionalJacobian<1, 6> Hself = {},
338 OptionalJacobian<1, 6> Hpose = {})
const;
345 Unit3 bearing(
const Point3&
point, OptionalJacobian<2, 6> Hself = {},
346 OptionalJacobian<2, 3> Hpoint = {})
const;
354 Unit3 bearing(
const Pose3&
pose, OptionalJacobian<2, 6> Hself = {},
355 OptionalJacobian<2, 6> Hpose = {})
const;
385 OptionalJacobian<6, 6> Hy = {})
const;
392 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
394 friend class boost::serialization::access;
395 template<
class Archive>
396 void serialize(Archive & ar,
const unsigned int ) {
397 ar & BOOST_SERIALIZATION_NVP(R_);
398 ar & BOOST_SERIALIZATION_NVP(t_);
403 #ifdef GTSAM_USE_QUATERNIONS
443 template <
typename T>