Namespace sym#

namespace sym

Typedefs

using ATANCameraCald = ATANCameraCal<double>
using ATANCameraCalf = ATANCameraCal<float>
using DoubleSphereCameraCald = DoubleSphereCameraCal<double>
using DoubleSphereCameraCalf = DoubleSphereCameraCal<float>
using EquirectangularCameraCald = EquirectangularCameraCal<double>
using EquirectangularCameraCalf = EquirectangularCameraCal<float>
using LinearCameraCald = LinearCameraCal<double>#
using LinearCameraCalf = LinearCameraCal<float>#
using PolynomialCameraCald = PolynomialCameraCal<double>#
using PolynomialCameraCalf = PolynomialCameraCal<float>#
using Pose2d = Pose2<double>#
using Pose2f = Pose2<float>#
using Pose3d = Pose3<double>#
using Pose3f = Pose3<float>#
using Rot2d = Rot2<double>#
using Rot2f = Rot2<float>#
using Rot3d = Rot3<double>#
using Rot3f = Rot3<float>#
using SphericalCameraCald = SphericalCameraCal<double>
using SphericalCameraCalf = SphericalCameraCal<float>
using Unit3d = Unit3<double>
using Unit3f = Unit3<float>
template<typename Scalar>
using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>
template<typename Scalar>
using MatrixX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>
template<typename Scalar>
using Vector1 = Eigen::Matrix<Scalar, 1, 1>
template<typename Scalar>
using Vector2 = Eigen::Matrix<Scalar, 2, 1>
template<typename Scalar>
using Vector3 = Eigen::Matrix<Scalar, 3, 1>
template<typename Scalar>
using Vector4 = Eigen::Matrix<Scalar, 4, 1>
template<typename Scalar>
using Vector5 = Eigen::Matrix<Scalar, 5, 1>
template<typename Scalar>
using Vector6 = Eigen::Matrix<Scalar, 6, 1>
template<typename Scalar>
using Vector7 = Eigen::Matrix<Scalar, 7, 1>
template<typename Scalar>
using Vector8 = Eigen::Matrix<Scalar, 8, 1>
template<typename Scalar>
using Vector9 = Eigen::Matrix<Scalar, 9, 1>
template<typename Scalar>
using Matrix11 = Eigen::Matrix<Scalar, 1, 1>
template<typename Scalar>
using Matrix12 = Eigen::Matrix<Scalar, 1, 2>
template<typename Scalar>
using Matrix13 = Eigen::Matrix<Scalar, 1, 3>
template<typename Scalar>
using Matrix14 = Eigen::Matrix<Scalar, 1, 4>
template<typename Scalar>
using Matrix15 = Eigen::Matrix<Scalar, 1, 5>
template<typename Scalar>
using Matrix16 = Eigen::Matrix<Scalar, 1, 6>
template<typename Scalar>
using Matrix17 = Eigen::Matrix<Scalar, 1, 7>
template<typename Scalar>
using Matrix18 = Eigen::Matrix<Scalar, 1, 8>
template<typename Scalar>
using Matrix19 = Eigen::Matrix<Scalar, 1, 9>
template<typename Scalar>
using Matrix21 = Eigen::Matrix<Scalar, 2, 1>
template<typename Scalar>
using Matrix22 = Eigen::Matrix<Scalar, 2, 2>
template<typename Scalar>
using Matrix23 = Eigen::Matrix<Scalar, 2, 3>
template<typename Scalar>
using Matrix24 = Eigen::Matrix<Scalar, 2, 4>
template<typename Scalar>
using Matrix25 = Eigen::Matrix<Scalar, 2, 5>
template<typename Scalar>
using Matrix26 = Eigen::Matrix<Scalar, 2, 6>
template<typename Scalar>
using Matrix27 = Eigen::Matrix<Scalar, 2, 7>
template<typename Scalar>
using Matrix28 = Eigen::Matrix<Scalar, 2, 8>
template<typename Scalar>
using Matrix29 = Eigen::Matrix<Scalar, 2, 9>
template<typename Scalar>
using Matrix31 = Eigen::Matrix<Scalar, 3, 1>
template<typename Scalar>
using Matrix32 = Eigen::Matrix<Scalar, 3, 2>
template<typename Scalar>
using Matrix33 = Eigen::Matrix<Scalar, 3, 3>
template<typename Scalar>
using Matrix34 = Eigen::Matrix<Scalar, 3, 4>
template<typename Scalar>
using Matrix35 = Eigen::Matrix<Scalar, 3, 5>
template<typename Scalar>
using Matrix36 = Eigen::Matrix<Scalar, 3, 6>
template<typename Scalar>
using Matrix37 = Eigen::Matrix<Scalar, 3, 7>
template<typename Scalar>
using Matrix38 = Eigen::Matrix<Scalar, 3, 8>
template<typename Scalar>
using Matrix39 = Eigen::Matrix<Scalar, 3, 9>
template<typename Scalar>
using Matrix41 = Eigen::Matrix<Scalar, 4, 1>
template<typename Scalar>
using Matrix42 = Eigen::Matrix<Scalar, 4, 2>
template<typename Scalar>
using Matrix43 = Eigen::Matrix<Scalar, 4, 3>
template<typename Scalar>
using Matrix44 = Eigen::Matrix<Scalar, 4, 4>
template<typename Scalar>
using Matrix45 = Eigen::Matrix<Scalar, 4, 5>
template<typename Scalar>
using Matrix46 = Eigen::Matrix<Scalar, 4, 6>
template<typename Scalar>
using Matrix47 = Eigen::Matrix<Scalar, 4, 7>
template<typename Scalar>
using Matrix48 = Eigen::Matrix<Scalar, 4, 8>
template<typename Scalar>
using Matrix49 = Eigen::Matrix<Scalar, 4, 9>
template<typename Scalar>
using Matrix51 = Eigen::Matrix<Scalar, 5, 1>
template<typename Scalar>
using Matrix52 = Eigen::Matrix<Scalar, 5, 2>
template<typename Scalar>
using Matrix53 = Eigen::Matrix<Scalar, 5, 3>
template<typename Scalar>
using Matrix54 = Eigen::Matrix<Scalar, 5, 4>
template<typename Scalar>
using Matrix55 = Eigen::Matrix<Scalar, 5, 5>
template<typename Scalar>
using Matrix56 = Eigen::Matrix<Scalar, 5, 6>
template<typename Scalar>
using Matrix57 = Eigen::Matrix<Scalar, 5, 7>
template<typename Scalar>
using Matrix58 = Eigen::Matrix<Scalar, 5, 8>
template<typename Scalar>
using Matrix59 = Eigen::Matrix<Scalar, 5, 9>
template<typename Scalar>
using Matrix61 = Eigen::Matrix<Scalar, 6, 1>
template<typename Scalar>
using Matrix62 = Eigen::Matrix<Scalar, 6, 2>
template<typename Scalar>
using Matrix63 = Eigen::Matrix<Scalar, 6, 3>
template<typename Scalar>
using Matrix64 = Eigen::Matrix<Scalar, 6, 4>
template<typename Scalar>
using Matrix65 = Eigen::Matrix<Scalar, 6, 5>
template<typename Scalar>
using Matrix66 = Eigen::Matrix<Scalar, 6, 6>
template<typename Scalar>
using Matrix67 = Eigen::Matrix<Scalar, 6, 7>
template<typename Scalar>
using Matrix68 = Eigen::Matrix<Scalar, 6, 8>
template<typename Scalar>
using Matrix69 = Eigen::Matrix<Scalar, 6, 9>
template<typename Scalar>
using Matrix71 = Eigen::Matrix<Scalar, 7, 1>
template<typename Scalar>
using Matrix72 = Eigen::Matrix<Scalar, 7, 2>
template<typename Scalar>
using Matrix73 = Eigen::Matrix<Scalar, 7, 3>
template<typename Scalar>
using Matrix74 = Eigen::Matrix<Scalar, 7, 4>
template<typename Scalar>
using Matrix75 = Eigen::Matrix<Scalar, 7, 5>
template<typename Scalar>
using Matrix76 = Eigen::Matrix<Scalar, 7, 6>
template<typename Scalar>
using Matrix77 = Eigen::Matrix<Scalar, 7, 7>
template<typename Scalar>
using Matrix78 = Eigen::Matrix<Scalar, 7, 8>
template<typename Scalar>
using Matrix79 = Eigen::Matrix<Scalar, 7, 9>
template<typename Scalar>
using Matrix81 = Eigen::Matrix<Scalar, 8, 1>
template<typename Scalar>
using Matrix82 = Eigen::Matrix<Scalar, 8, 2>
template<typename Scalar>
using Matrix83 = Eigen::Matrix<Scalar, 8, 3>
template<typename Scalar>
using Matrix84 = Eigen::Matrix<Scalar, 8, 4>
template<typename Scalar>
using Matrix85 = Eigen::Matrix<Scalar, 8, 5>
template<typename Scalar>
using Matrix86 = Eigen::Matrix<Scalar, 8, 6>
template<typename Scalar>
using Matrix87 = Eigen::Matrix<Scalar, 8, 7>
template<typename Scalar>
using Matrix88 = Eigen::Matrix<Scalar, 8, 8>
template<typename Scalar>
using Matrix89 = Eigen::Matrix<Scalar, 8, 9>
template<typename Scalar>
using Matrix91 = Eigen::Matrix<Scalar, 9, 1>
template<typename Scalar>
using Matrix92 = Eigen::Matrix<Scalar, 9, 2>
template<typename Scalar>
using Matrix93 = Eigen::Matrix<Scalar, 9, 3>
template<typename Scalar>
using Matrix94 = Eigen::Matrix<Scalar, 9, 4>
template<typename Scalar>
using Matrix95 = Eigen::Matrix<Scalar, 9, 5>
template<typename Scalar>
using Matrix96 = Eigen::Matrix<Scalar, 9, 6>
template<typename Scalar>
using Matrix97 = Eigen::Matrix<Scalar, 9, 7>
template<typename Scalar>
using Matrix98 = Eigen::Matrix<Scalar, 9, 8>
template<typename Scalar>
using Matrix99 = Eigen::Matrix<Scalar, 9, 9>
using Vector1d = Vector1<double>
using Vector1f = Vector1<float>
using Vector2d = Vector2<double>
using Vector2f = Vector2<float>
using Vector3d = Vector3<double>
using Vector3f = Vector3<float>
using Vector4d = Vector4<double>
using Vector4f = Vector4<float>
using Vector5d = Vector5<double>
using Vector5f = Vector5<float>
using Vector6d = Vector6<double>
using Vector6f = Vector6<float>
using Vector7d = Vector7<double>
using Vector7f = Vector7<float>
using Vector8d = Vector8<double>
using Vector8f = Vector8<float>
using Vector9d = Vector9<double>
using Vector9f = Vector9<float>
using Matrix11d = Matrix11<double>
using Matrix11f = Matrix11<float>
using Matrix12d = Matrix12<double>
using Matrix12f = Matrix12<float>
using Matrix13d = Matrix13<double>
using Matrix13f = Matrix13<float>
using Matrix14d = Matrix14<double>
using Matrix14f = Matrix14<float>
using Matrix15d = Matrix15<double>
using Matrix15f = Matrix15<float>
using Matrix16d = Matrix16<double>
using Matrix16f = Matrix16<float>
using Matrix17d = Matrix17<double>
using Matrix17f = Matrix17<float>
using Matrix18d = Matrix18<double>
using Matrix18f = Matrix18<float>
using Matrix19d = Matrix19<double>
using Matrix19f = Matrix19<float>
using Matrix21d = Matrix21<double>
using Matrix21f = Matrix21<float>
using Matrix22d = Matrix22<double>
using Matrix22f = Matrix22<float>
using Matrix23d = Matrix23<double>
using Matrix23f = Matrix23<float>
using Matrix24d = Matrix24<double>
using Matrix24f = Matrix24<float>
using Matrix25d = Matrix25<double>
using Matrix25f = Matrix25<float>
using Matrix26d = Matrix26<double>
using Matrix26f = Matrix26<float>
using Matrix27d = Matrix27<double>
using Matrix27f = Matrix27<float>
using Matrix28d = Matrix28<double>
using Matrix28f = Matrix28<float>
using Matrix29d = Matrix29<double>
using Matrix29f = Matrix29<float>
using Matrix31d = Matrix31<double>
using Matrix31f = Matrix31<float>
using Matrix32d = Matrix32<double>
using Matrix32f = Matrix32<float>
using Matrix33d = Matrix33<double>
using Matrix33f = Matrix33<float>
using Matrix34d = Matrix34<double>
using Matrix34f = Matrix34<float>
using Matrix35d = Matrix35<double>
using Matrix35f = Matrix35<float>
using Matrix36d = Matrix36<double>
using Matrix36f = Matrix36<float>
using Matrix37d = Matrix37<double>
using Matrix37f = Matrix37<float>
using Matrix38d = Matrix38<double>
using Matrix38f = Matrix38<float>
using Matrix39d = Matrix39<double>
using Matrix39f = Matrix39<float>
using Matrix41d = Matrix41<double>
using Matrix41f = Matrix41<float>
using Matrix42d = Matrix42<double>
using Matrix42f = Matrix42<float>
using Matrix43d = Matrix43<double>
using Matrix43f = Matrix43<float>
using Matrix44d = Matrix44<double>
using Matrix44f = Matrix44<float>
using Matrix45d = Matrix45<double>
using Matrix45f = Matrix45<float>
using Matrix46d = Matrix46<double>
using Matrix46f = Matrix46<float>
using Matrix47d = Matrix47<double>
using Matrix47f = Matrix47<float>
using Matrix48d = Matrix48<double>
using Matrix48f = Matrix48<float>
using Matrix49d = Matrix49<double>
using Matrix49f = Matrix49<float>
using Matrix51d = Matrix51<double>
using Matrix51f = Matrix51<float>
using Matrix52d = Matrix52<double>
using Matrix52f = Matrix52<float>
using Matrix53d = Matrix53<double>
using Matrix53f = Matrix53<float>
using Matrix54d = Matrix54<double>
using Matrix54f = Matrix54<float>
using Matrix55d = Matrix55<double>
using Matrix55f = Matrix55<float>
using Matrix56d = Matrix56<double>
using Matrix56f = Matrix56<float>
using Matrix57d = Matrix57<double>
using Matrix57f = Matrix57<float>
using Matrix58d = Matrix58<double>
using Matrix58f = Matrix58<float>
using Matrix59d = Matrix59<double>
using Matrix59f = Matrix59<float>
using Matrix61d = Matrix61<double>
using Matrix61f = Matrix61<float>
using Matrix62d = Matrix62<double>
using Matrix62f = Matrix62<float>
using Matrix63d = Matrix63<double>
using Matrix63f = Matrix63<float>
using Matrix64d = Matrix64<double>
using Matrix64f = Matrix64<float>
using Matrix65d = Matrix65<double>
using Matrix65f = Matrix65<float>
using Matrix66d = Matrix66<double>
using Matrix66f = Matrix66<float>
using Matrix67d = Matrix67<double>
using Matrix67f = Matrix67<float>
using Matrix68d = Matrix68<double>
using Matrix68f = Matrix68<float>
using Matrix69d = Matrix69<double>
using Matrix69f = Matrix69<float>
using Matrix71d = Matrix71<double>
using Matrix71f = Matrix71<float>
using Matrix72d = Matrix72<double>
using Matrix72f = Matrix72<float>
using Matrix73d = Matrix73<double>
using Matrix73f = Matrix73<float>
using Matrix74d = Matrix74<double>
using Matrix74f = Matrix74<float>
using Matrix75d = Matrix75<double>
using Matrix75f = Matrix75<float>
using Matrix76d = Matrix76<double>
using Matrix76f = Matrix76<float>
using Matrix77d = Matrix77<double>
using Matrix77f = Matrix77<float>
using Matrix78d = Matrix78<double>
using Matrix78f = Matrix78<float>
using Matrix79d = Matrix79<double>
using Matrix79f = Matrix79<float>
using Matrix81d = Matrix81<double>
using Matrix81f = Matrix81<float>
using Matrix82d = Matrix82<double>
using Matrix82f = Matrix82<float>
using Matrix83d = Matrix83<double>
using Matrix83f = Matrix83<float>
using Matrix84d = Matrix84<double>
using Matrix84f = Matrix84<float>
using Matrix85d = Matrix85<double>
using Matrix85f = Matrix85<float>
using Matrix86d = Matrix86<double>
using Matrix86f = Matrix86<float>
using Matrix87d = Matrix87<double>
using Matrix87f = Matrix87<float>
using Matrix88d = Matrix88<double>
using Matrix88f = Matrix88<float>
using Matrix89d = Matrix89<double>
using Matrix89f = Matrix89<float>
using Matrix91d = Matrix91<double>
using Matrix91f = Matrix91<float>
using Matrix92d = Matrix92<double>
using Matrix92f = Matrix92<float>
using Matrix93d = Matrix93<double>
using Matrix93f = Matrix93<float>
using Matrix94d = Matrix94<double>
using Matrix94f = Matrix94<float>
using Matrix95d = Matrix95<double>
using Matrix95f = Matrix95<float>
using Matrix96d = Matrix96<double>
using Matrix96f = Matrix96<float>
using Matrix97d = Matrix97<double>
using Matrix97f = Matrix97<float>
using Matrix98d = Matrix98<double>
using Matrix98f = Matrix98<float>
using Matrix99d = Matrix99<double>
using Matrix99f = Matrix99<float>

Functions

std::ostream &operator<<(std::ostream &os, const ATANCameraCald &a)
std::ostream &operator<<(std::ostream &os, const ATANCameraCalf &a)
std::ostream &operator<<(std::ostream &os, const DoubleSphereCameraCald &a)
std::ostream &operator<<(std::ostream &os, const DoubleSphereCameraCalf &a)
std::ostream &operator<<(std::ostream &os, const EquirectangularCameraCald &a)
std::ostream &operator<<(std::ostream &os, const EquirectangularCameraCalf &a)
template<typename Scalar>
void AtanReprojectionDelta(const sym::Pose3<Scalar> &source_pose, const sym::ATANCameraCal<Scalar> &source_calibration, const sym::Pose3<Scalar> &target_pose, const sym::ATANCameraCal<Scalar> &target_calibration, const Scalar source_inverse_range, const Eigen::Matrix<Scalar, 2, 1> &source_pixel, const Eigen::Matrix<Scalar, 2, 1> &target_pixel, const Scalar epsilon, Eigen::Matrix<Scalar, 2, 1> *const reprojection_delta = nullptr, Scalar *const is_valid = nullptr)

Reprojects the landmark into the target camera and returns the delta from the correspondence to the reprojection.

The landmark is specified as a pixel in the source camera and an inverse range; this means the landmark is fixed in the source camera and always has residual 0 there (this 0 residual is not returned, only the residual in the target camera is returned).

Args: source_pose: The pose of the source camera source_calibration: The source camera calibration target_pose: The pose of the target camera target_calibration: The target camera calibration source_inverse_range: The inverse range of the landmark in the source camera source_pixel: The location of the landmark in the source camera target_pixel: The location of the correspondence in the target camera epsilon: Small positive value camera_model_class: The subclass of CameraCal to use as the camera model

Outputs: res: 2dof pixel reprojection error valid: is valid projection or not

template<typename Scalar>
void BetweenFactorMatrix31(const Eigen::Matrix<Scalar, 3, 1> &a, const Eigen::Matrix<Scalar, 3, 1> &b, const Eigen::Matrix<Scalar, 3, 1> &a_T_b, const Eigen::Matrix<Scalar, 3, 3> &sqrt_info, const Scalar epsilon, Eigen::Matrix<Scalar, 3, 1> *const res = nullptr, Eigen::Matrix<Scalar, 3, 6> *const jacobian = nullptr, Eigen::Matrix<Scalar, 6, 6> *const hessian = nullptr, Eigen::Matrix<Scalar, 6, 1> *const rhs = nullptr)

Residual that penalizes the difference between between(a, b) and a_T_b.

In vector space terms that would be: (b - a) - a_T_b

In lie group terms: local_coordinates(a_T_b, between(a, b)) to_tangent(compose(inverse(a_T_b), compose(inverse(a), b)))

Args: sqrt_info: Square root information matrix to whiten residual. This can be computed from a covariance matrix as the cholesky decomposition of the inverse. In the case of a diagonal it will contain 1/sigma values. Must match the tangent dim. jacobian: (3x6) jacobian of res wrt args a (3), b (3) hessian: (6x6) Gauss-Newton hessian for args a (3), b (3) rhs: (6x1) Gauss-Newton rhs for args a (3), b (3)

template<typename Scalar>
void BetweenFactorPose2(const sym::Pose2<Scalar> &a, const sym::Pose2<Scalar> &b, const sym::Pose2<Scalar> &a_T_b, const Eigen::Matrix<Scalar, 3, 3> &sqrt_info, const Scalar epsilon, Eigen::Matrix<Scalar, 3, 1> *const res = nullptr, Eigen::Matrix<Scalar, 3, 6> *const jacobian = nullptr, Eigen::Matrix<Scalar, 6, 6> *const hessian = nullptr, Eigen::Matrix<Scalar, 6, 1> *const rhs = nullptr)

Residual that penalizes the difference between between(a, b) and a_T_b.

In vector space terms that would be: (b - a) - a_T_b

In lie group terms: local_coordinates(a_T_b, between(a, b)) to_tangent(compose(inverse(a_T_b), compose(inverse(a), b)))

Args: sqrt_info: Square root information matrix to whiten residual. This can be computed from a covariance matrix as the cholesky decomposition of the inverse. In the case of a diagonal it will contain 1/sigma values. Must match the tangent dim. jacobian: (3x6) jacobian of res wrt args a (3), b (3) hessian: (6x6) Gauss-Newton hessian for args a (3), b (3) rhs: (6x1) Gauss-Newton rhs for args a (3), b (3)

template<typename Scalar>
void BetweenFactorPose3(const sym::Pose3<Scalar> &a, const sym::Pose3<Scalar> &b, const sym::Pose3<Scalar> &a_T_b, const Eigen::Matrix<Scalar, 6, 6> &sqrt_info, const Scalar epsilon, Eigen::Matrix<Scalar, 6, 1> *const res = nullptr, Eigen::Matrix<Scalar, 6, 12> *const jacobian = nullptr, Eigen::Matrix<Scalar, 12, 12> *const hessian = nullptr, Eigen::Matrix<Scalar, 12, 1> *const rhs = nullptr)

Residual that penalizes the difference between between(a, b) and a_T_b.

In vector space terms that would be: (b - a) - a_T_b

In lie group terms: local_coordinates(a_T_b, between(a, b)) to_tangent(compose(inverse(a_T_b), compose(inverse(a), b)))

Args: sqrt_info: Square root information matrix to whiten residual. This can be computed from a covariance matrix as the cholesky decomposition of the inverse. In the case of a diagonal it will contain 1/sigma values. Must match the tangent dim. jacobian: (6x12) jacobian of res wrt args a (6), b (6) hessian: (12x12) Gauss-Newton hessian for args a (6), b (6) rhs: (12x1) Gauss-Newton rhs for args a (6), b (6)

template<typename Scalar>
void BetweenFactorPose3Position(const sym::Pose3<Scalar> &a, const sym::Pose3<Scalar> &b, const Eigen::Matrix<Scalar, 3, 1> &a_t_b, const Eigen::Matrix<Scalar, 3, 3> &sqrt_info, const Scalar epsilon, Eigen::Matrix<Scalar, 3, 1> *const res = nullptr, Eigen::Matrix<Scalar, 3, 12> *const jacobian = nullptr, Eigen::Matrix<Scalar, 12, 12> *const hessian = nullptr, Eigen::Matrix<Scalar, 12, 1> *const rhs = nullptr)

Residual that penalizes the difference between between(a, b) and a_t_b.

In vector space terms that would be: (b - a) - a_t_b

In lie group terms: local_coordinates(a_t_b, between(a, b)) to_tangent(compose(inverse(a_t_b), compose(inverse(a), b)))

Args: sqrt_info: Square root information matrix to whiten residual. This can be computed from a covariance matrix as the cholesky decomposition of the inverse. In the case of a diagonal it will contain 1/sigma values. Must match the tangent dim. jacobian: (3x12) jacobian of res wrt args a (6), b (6) hessian: (12x12) Gauss-Newton hessian for args a (6), b (6) rhs: (12x1) Gauss-Newton rhs for args a (6), b (6)

template<typename Scalar>
void BetweenFactorPose3Rotation(const sym::Pose3<Scalar> &a, const sym::Pose3<Scalar> &b, const sym::Rot3<Scalar> &a_R_b, const Eigen::Matrix<Scalar, 3, 3> &sqrt_info, const Scalar epsilon, Eigen::Matrix<Scalar, 3, 1> *const res = nullptr, Eigen::Matrix<Scalar, 3, 12> *const jacobian = nullptr, Eigen::Matrix<Scalar, 12, 12> *const hessian = nullptr, Eigen::Matrix<Scalar, 12, 1> *const rhs = nullptr)

Residual that penalizes the difference between between(a, b) and a_R_b.

In vector space terms that would be: (b - a) - a_R_b

In lie group terms: local_coordinates(a_R_b, between(a, b)) to_tangent(compose(inverse(a_R_b), compose(inverse(a), b)))

Args: sqrt_info: Square root information matrix to whiten residual. This can be computed from a covariance matrix as the cholesky decomposition of the inverse. In the case of a diagonal it will contain 1/sigma values. Must match the tangent dim. jacobian: (3x12) jacobian of res wrt args a (6), b (6) hessian: (12x12) Gauss-Newton hessian for args a (6), b (6) rhs: (12x1) Gauss-Newton rhs for args a (6), b (6)

template<typename Scalar>
void BetweenFactorPose3TranslationNorm(const sym::Pose3<Scalar> &a, const sym::Pose3<Scalar> &b, const Scalar translation_norm, const Eigen::Matrix<Scalar, 1, 1> &sqrt_info, const Scalar epsilon, Eigen::Matrix<Scalar, 1, 1> *const res = nullptr, Eigen::Matrix<Scalar, 1, 12> *const jacobian = nullptr, Eigen::Matrix<Scalar, 12, 12> *const hessian = nullptr, Eigen::Matrix<Scalar, 12, 1> *const rhs = nullptr)

Residual that penalizes the difference between translation_norm and (a.t - b.t).norm().

Args: sqrt_info: Square root information matrix to whiten residual. In this one dimensional case this is just 1/sigma. jacobian: (1x12) jacobian of res wrt args a (6), b (6) hessian: (12x12) Gauss-Newton hessian for args a (6), b (6) rhs: (12x1) Gauss-Newton rhs for args a (6), b (6)

template<typename Scalar>
void BetweenFactorRot2(const sym::Rot2<Scalar> &a, const sym::Rot2<Scalar> &b, const sym::Rot2<Scalar> &a_T_b, const Eigen::Matrix<Scalar, 1, 1> &sqrt_info, const Scalar epsilon, Eigen::Matrix<Scalar, 1, 1> *const res = nullptr, Eigen::Matrix<Scalar, 1, 2> *const jacobian = nullptr, Eigen::Matrix<Scalar, 2, 2> *const hessian = nullptr, Eigen::Matrix<Scalar, 2, 1> *const rhs = nullptr)

Residual that penalizes the difference between between(a, b) and a_T_b.

In vector space terms that would be: (b - a) - a_T_b

In lie group terms: local_coordinates(a_T_b, between(a, b)) to_tangent(compose(inverse(a_T_b), compose(inverse(a), b)))

Args: sqrt_info: Square root information matrix to whiten residual. This can be computed from a covariance matrix as the cholesky decomposition of the inverse. In the case of a diagonal it will contain 1/sigma values. Must match the tangent dim. jacobian: (1x2) jacobian of res wrt args a (1), b (1) hessian: (2x2) Gauss-Newton hessian for args a (1), b (1) rhs: (2x1) Gauss-Newton rhs for args a (1), b (1)

template<typename Scalar>
void BetweenFactorRot3(const sym::Rot3<Scalar> &a, const sym::Rot3<Scalar> &b, const sym::Rot3<Scalar> &a_T_b, const Eigen::Matrix<Scalar, 3, 3> &sqrt_info, const Scalar epsilon, Eigen::Matrix<Scalar, 3, 1> *const res = nullptr, Eigen::Matrix<Scalar, 3, 6> *const jacobian = nullptr, Eigen::Matrix<Scalar, 6, 6> *const hessian = nullptr, Eigen::Matrix<Scalar, 6, 1> *const rhs = nullptr)

Residual that penalizes the difference between between(a, b) and a_T_b.

In vector space terms that would be: (b - a) - a_T_b

In lie group terms: local_coordinates(a_T_b, between(a, b)) to_tangent(compose(inverse(a_T_b), compose(inverse(a), b)))

Args: sqrt_info: Square root information matrix to whiten residual. This can be computed from a covariance matrix as the cholesky decomposition of the inverse. In the case of a diagonal it will contain 1/sigma values. Must match the tangent dim. jacobian: (3x6) jacobian of res wrt args a (3), b (3) hessian: (6x6) Gauss-Newton hessian for args a (3), b (3) rhs: (6x1) Gauss-Newton rhs for args a (3), b (3)

template<typename Scalar>
void DoubleSphereReprojectionDelta(const sym::Pose3<Scalar> &source_pose, const sym::DoubleSphereCameraCal<Scalar> &source_calibration, const sym::Pose3<Scalar> &target_pose, const sym::DoubleSphereCameraCal<Scalar> &target_calibration, const Scalar source_inverse_range, const Eigen::Matrix<Scalar, 2, 1> &source_pixel, const Eigen::Matrix<Scalar, 2, 1> &target_pixel, const Scalar epsilon, Eigen::Matrix<Scalar, 2, 1> *const reprojection_delta = nullptr, Scalar *const is_valid = nullptr)

Reprojects the landmark into the target camera and returns the delta from the correspondence to the reprojection.

The landmark is specified as a pixel in the source camera and an inverse range; this means the landmark is fixed in the source camera and always has residual 0 there (this 0 residual is not returned, only the residual in the target camera is returned).

Args: source_pose: The pose of the source camera source_calibration: The source camera calibration target_pose: The pose of the target camera target_calibration: The target camera calibration source_inverse_range: The inverse range of the landmark in the source camera source_pixel: The location of the landmark in the source camera target_pixel: The location of the correspondence in the target camera epsilon: Small positive value camera_model_class: The subclass of CameraCal to use as the camera model

Outputs: res: 2dof pixel reprojection error valid: is valid projection or not

template<typename Scalar>
void EquirectangularReprojectionDelta(const sym::Pose3<Scalar> &source_pose, const sym::EquirectangularCameraCal<Scalar> &source_calibration, const sym::Pose3<Scalar> &target_pose, const sym::EquirectangularCameraCal<Scalar> &target_calibration, const Scalar source_inverse_range, const Eigen::Matrix<Scalar, 2, 1> &source_pixel, const Eigen::Matrix<Scalar, 2, 1> &target_pixel, const Scalar epsilon, Eigen::Matrix<Scalar, 2, 1> *const reprojection_delta = nullptr, Scalar *const is_valid = nullptr)

Reprojects the landmark into the target camera and returns the delta from the correspondence to the reprojection.

The landmark is specified as a pixel in the source camera and an inverse range; this means the landmark is fixed in the source camera and always has residual 0 there (this 0 residual is not returned, only the residual in the target camera is returned).

Args: source_pose: The pose of the source camera source_calibration: The source camera calibration target_pose: The pose of the target camera target_calibration: The target camera calibration source_inverse_range: The inverse range of the landmark in the source camera source_pixel: The location of the landmark in the source camera target_pixel: The location of the correspondence in the target camera epsilon: Small positive value camera_model_class: The subclass of CameraCal to use as the camera model

Outputs: res: 2dof pixel reprojection error valid: is valid projection or not

template<typename Scalar>
void ImuManifoldPreintegrationUpdate(const sym::Rot3<Scalar> &DR, const Eigen::Matrix<Scalar, 3, 1> &Dv, const Eigen::Matrix<Scalar, 3, 1> &Dp, const Eigen::Matrix<Scalar, 9, 9> &covariance, const Eigen::Matrix<Scalar, 3, 3> &DR_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 3> &Dv_D_accel_bias, const Eigen::Matrix<Scalar, 3, 3> &Dv_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 3> &Dp_D_accel_bias, const Eigen::Matrix<Scalar, 3, 3> &Dp_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 1> &accel_bias, const Eigen::Matrix<Scalar, 3, 1> &gyro_bias, const Eigen::Matrix<Scalar, 3, 1> &accel_cov_diagonal, const Eigen::Matrix<Scalar, 3, 1> &gyro_cov_diagonal, const Eigen::Matrix<Scalar, 3, 1> &accel_measurement, const Eigen::Matrix<Scalar, 3, 1> &gyro_measurement, const Scalar dt, const Scalar epsilon, sym::Rot3<Scalar> *const new_DR = nullptr, Eigen::Matrix<Scalar, 3, 1> *const new_Dv = nullptr, Eigen::Matrix<Scalar, 3, 1> *const new_Dp = nullptr, Eigen::Matrix<Scalar, 9, 9> *const new_covariance = nullptr, Eigen::Matrix<Scalar, 3, 3> *const new_DR_D_gyro_bias = nullptr, Eigen::Matrix<Scalar, 3, 3> *const new_Dv_D_accel_bias = nullptr, Eigen::Matrix<Scalar, 3, 3> *const new_Dv_D_gyro_bias = nullptr, Eigen::Matrix<Scalar, 3, 3> *const new_Dp_D_accel_bias = nullptr, Eigen::Matrix<Scalar, 3, 3> *const new_Dp_D_gyro_bias = nullptr)

An internal helper function to update a set of preintegrated IMU measurements with a new pair of gyroscope and accelerometer measurements. Returns the updated preintegrated measurements.

NOTE: If you are interested in this function, you should likely be using the IntegrateMeasurement method of the PreintegratedImuMeasurements class located in symforce/slam/imu_preintegration/preintegrated_imu_measurements.h.

When integrating the first measurement, DR should be the identity, Dv, Dp, covariance, and the derivatives w.r.t. to bias should all be 0.

Args: DR: Preintegrated change in orientation Dv: Preintegrated change in velocity Dp: Preintegrated change in position covariance: Covariance [DR, Dv, Dp] in local coordinates of mean DR_D_gyro_bias: Derivative of DR w.r.t. gyro_bias Dv_D_accel_bias: Derivative of Dv w.r.t. accel_bias Dv_D_gyro_bias: Derivative of Dv w.r.t. gyro_bias Dp_D_accel_bias: Derivative of Dp w.r.t. accel_bias Dp_D_gyro_bias: Derivative of Dp w.r.t. gyro_bias accel_bias: Initial guess for the accelerometer measurement bias gyro_bias: Initial guess for the gyroscope measurement bias accel_cov_diagonal: The diagonal of the covariance of the accelerometer measurement gyro_cov_diagonal: The diagonal of the covariance of the gyroscope measurement accel_measurement: The accelerometer measurement gyro_measurement: The gyroscope measurement dt: The time between the new measurements and the last epsilon: epsilon for numerical stability

Returns: T.Tuple[sf.Rot3, sf.V3, sf.V3, sf.M99, sf.M33, sf.M33, sf.M33, sf.M33, sf.M33]: new_DR, new_Dv, new_Dp, new_covariance, new_DR_D_gyro_bias, new_Dv_D_accel_bias, new_Dv_D_gyro_bias, new_Dp_D_accel_bias new_Dp_D_gyro_bias,

template<typename Scalar>
void ImuManifoldPreintegrationUpdateAutoDerivative(const sym::Rot3<Scalar> &DR, const Eigen::Matrix<Scalar, 3, 1> &Dv, const Eigen::Matrix<Scalar, 3, 1> &Dp, const Eigen::Matrix<Scalar, 9, 9> &covariance, const Eigen::Matrix<Scalar, 3, 3> &DR_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 3> &Dv_D_accel_bias, const Eigen::Matrix<Scalar, 3, 3> &Dv_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 3> &Dp_D_accel_bias, const Eigen::Matrix<Scalar, 3, 3> &Dp_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 1> &accel_bias, const Eigen::Matrix<Scalar, 3, 1> &gyro_bias, const Eigen::Matrix<Scalar, 3, 1> &accel_cov_diagonal, const Eigen::Matrix<Scalar, 3, 1> &gyro_cov_diagonal, const Eigen::Matrix<Scalar, 3, 1> &accel_measurement, const Eigen::Matrix<Scalar, 3, 1> &gyro_measurement, const Scalar dt, const Scalar epsilon, sym::Rot3<Scalar> *const new_DR = nullptr, Eigen::Matrix<Scalar, 3, 1> *const new_Dv = nullptr, Eigen::Matrix<Scalar, 3, 1> *const new_Dp = nullptr, Eigen::Matrix<Scalar, 9, 9> *const new_covariance = nullptr, Eigen::Matrix<Scalar, 3, 3> *const new_DR_D_gyro_bias = nullptr, Eigen::Matrix<Scalar, 3, 3> *const new_Dv_D_accel_bias = nullptr, Eigen::Matrix<Scalar, 3, 3> *const new_Dv_D_gyro_bias = nullptr, Eigen::Matrix<Scalar, 3, 3> *const new_Dp_D_accel_bias = nullptr, Eigen::Matrix<Scalar, 3, 3> *const new_Dp_D_gyro_bias = nullptr)

Alternative to ImuManifoldPreintegrationUpdate that uses auto-derivatives. Exists only to help verify correctness of ImuManifoldPreintegrationUpdate. Should have the same output. Since this function is more expensive, there is no reason to use it instead.

An internal helper function to update a set of preintegrated IMU measurements with a new pair of gyroscope and accelerometer measurements. Returns the updated preintegrated measurements.

NOTE: If you are interested in this function, you should likely be using the IntegrateMeasurement method of the PreintegratedImuMeasurements class located in symforce/slam/imu_preintegration/preintegrated_imu_measurements.h.

When integrating the first measurement, DR should be the identity, Dv, Dp, covariance, and the derivatives w.r.t. to bias should all be 0.

Args: DR: Preintegrated change in orientation Dv: Preintegrated change in velocity Dp: Preintegrated change in position covariance: Covariance [DR, Dv, Dp] in local coordinates of mean DR_D_gyro_bias: Derivative of DR w.r.t. gyro_bias Dv_D_accel_bias: Derivative of Dv w.r.t. accel_bias Dv_D_gyro_bias: Derivative of Dv w.r.t. gyro_bias Dp_D_accel_bias: Derivative of Dp w.r.t. accel_bias Dp_D_gyro_bias: Derivative of Dp w.r.t. gyro_bias accel_bias: Initial guess for the accelerometer measurement bias gyro_bias: Initial guess for the gyroscope measurement bias accel_cov_diagonal: The diagonal of the covariance of the accelerometer measurement gyro_cov_diagonal: The diagonal of the covariance of the gyroscope measurement accel_measurement: The accelerometer measurement gyro_measurement: The gyroscope measurement dt: The time between the new measurements and the last epsilon: epsilon for numerical stability

Returns: T.Tuple[sf.Rot3, sf.V3, sf.V3, sf.M99, sf.M33, sf.M33, sf.M33, sf.M33, sf.M33]: new_DR, new_Dv, new_Dp, new_covariance, new_DR_D_gyro_bias, new_Dv_D_accel_bias, new_Dv_D_gyro_bias, new_Dp_D_accel_bias new_Dp_D_gyro_bias,

template<typename Scalar>
void InternalImuFactor(const sym::Pose3<Scalar> &pose_i, const Eigen::Matrix<Scalar, 3, 1> &vel_i, const sym::Pose3<Scalar> &pose_j, const Eigen::Matrix<Scalar, 3, 1> &vel_j, const Eigen::Matrix<Scalar, 3, 1> &accel_bias_i, const Eigen::Matrix<Scalar, 3, 1> &gyro_bias_i, const sym::Rot3<Scalar> &DR, const Eigen::Matrix<Scalar, 3, 1> &Dv, const Eigen::Matrix<Scalar, 3, 1> &Dp, const Eigen::Matrix<Scalar, 9, 9> &sqrt_info, const Eigen::Matrix<Scalar, 3, 3> &DR_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 3> &Dv_D_accel_bias, const Eigen::Matrix<Scalar, 3, 3> &Dv_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 3> &Dp_D_accel_bias, const Eigen::Matrix<Scalar, 3, 3> &Dp_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 1> &accel_bias_hat, const Eigen::Matrix<Scalar, 3, 1> &gyro_bias_hat, const Eigen::Matrix<Scalar, 3, 1> &gravity, const Scalar dt, const Scalar epsilon, Eigen::Matrix<Scalar, 9, 1> *const res = nullptr, Eigen::Matrix<Scalar, 9, 24> *const jacobian = nullptr, Eigen::Matrix<Scalar, 24, 24> *const hessian = nullptr, Eigen::Matrix<Scalar, 24, 1> *const rhs = nullptr)

An internal helper function to linearize the difference between the orientation, velocity, and position at one time step and those of another time step. The expected difference is calculated from the preintegrated IMU measurements between those time steps.

NOTE: If you are looking for a residual for an IMU factor, do not use this. Instead use the one found in symforce/slam/imu_preintegration/imu_factor.h.

Time step i is the time of the first IMU measurement of the interval. Time step j is the time after the last IMU measurement of the interval.

Assumes sqrt_info is lower triangular and only reads the lower triangle.

Args: pose_i: Pose at time step i (world_T_body) vel_i: Velocity at time step i (world frame) pose_j: Pose at time step j (world_T_body) vel_j: Velocity at time step j (world frame) accel_bias_i: The accelerometer bias between timesteps i and j gyro_bias_i: The gyroscope bias between timesteps i and j DR: Preintegrated estimate for pose_i.R.inverse() * pose_j.R Dv: Preintegrated estimate for vel_j - vel_i in the body frame at timestep i Dp: Preintegrated estimate for position change (before velocity and gravity corrections) in the body frame at timestep i::

R_i^T (p_j - p_i - v_i Delta t - 1/2 g Delta t^2),
where R_i = pose_i.R, p_i = pose_i.t, p_j = pose_j.t, and v_i = vel_i
sqrt_info: sqrt info matrix of DR(‘s tangent space), Dv, Dp DR_D_gyro_bias: Derivative of DR w.r.t. gyro_bias evaluated at gyro_bias_hat Dv_D_accel_bias: Derivative of Dv w.r.t. accel_bias evaluated at accel_bias_hat Dv_D_gyro_bias: Derivative of Dv w.r.t. gyro_bias evaluated at gyro_bias_hat Dp_D_accel_bias: Derivative of Dp w.r.t. accel_bias evaluated at accel_bias_hat Dp_D_gyro_bias: Derivative of Dp w.r.t. gyro_bias evaluated at gyro_bias_hat accel_bias_hat: The accelerometer bias used during preintegration gyro_bias_hat: The gyroscope bias used during preintegration gravity: Acceleration due to gravity (in the same frame as pose_x and vel_x), i.e., the vector which when added to the accelerometer measurements gives the true acceleration (up to bias and noise) of the IMU. dt: The time between timestep i and j: t_j - t_i epsilon: epsilon used for numerical stability

Outputs: res: 9dof product residual between the orientations, then velocities, then positions. jacobian: (9x24) jacobian of res wrt args pose_i (6), vel_i (3), pose_j (6), vel_j (3), accel_bias_i (3), gyro_bias_i (3) hessian: (24x24) Gauss-Newton hessian for args pose_i (6), vel_i (3), pose_j (6), vel_j (3), accel_bias_i (3), gyro_bias_i (3) rhs: (24x1) Gauss-Newton rhs for args pose_i (6), vel_i (3), pose_j (6), vel_j (3), accel_bias_i (3), gyro_bias_i (3)

template<typename Scalar>
void InternalImuUnitGravityFactor(const sym::Pose3<Scalar> &pose_i, const Eigen::Matrix<Scalar, 3, 1> &vel_i, const sym::Pose3<Scalar> &pose_j, const Eigen::Matrix<Scalar, 3, 1> &vel_j, const Eigen::Matrix<Scalar, 3, 1> &accel_bias_i, const Eigen::Matrix<Scalar, 3, 1> &gyro_bias_i, const sym::Rot3<Scalar> &DR, const Eigen::Matrix<Scalar, 3, 1> &Dv, const Eigen::Matrix<Scalar, 3, 1> &Dp, const Eigen::Matrix<Scalar, 9, 9> &sqrt_info, const Eigen::Matrix<Scalar, 3, 3> &DR_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 3> &Dv_D_accel_bias, const Eigen::Matrix<Scalar, 3, 3> &Dv_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 3> &Dp_D_accel_bias, const Eigen::Matrix<Scalar, 3, 3> &Dp_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 1> &accel_bias_hat, const Eigen::Matrix<Scalar, 3, 1> &gyro_bias_hat, const sym::Unit3<Scalar> &gravity_direction, const Scalar gravity_norm, const Scalar dt, const Scalar epsilon, Eigen::Matrix<Scalar, 9, 1> *const res = nullptr, Eigen::Matrix<Scalar, 9, 26> *const jacobian = nullptr, Eigen::Matrix<Scalar, 26, 26> *const hessian = nullptr, Eigen::Matrix<Scalar, 26, 1> *const rhs = nullptr)

This function was autogenerated from a symbolic function. Do not modify by hand.

Symbolic function: internal_imu_unit_gravity_residual

Args: pose_i: Pose3 vel_i: Matrix31 pose_j: Pose3 vel_j: Matrix31 accel_bias_i: Matrix31 gyro_bias_i: Matrix31 DR: Rot3 Dv: Matrix31 Dp: Matrix31 sqrt_info: Matrix99 DR_D_gyro_bias: Matrix33 Dv_D_accel_bias: Matrix33 Dv_D_gyro_bias: Matrix33 Dp_D_accel_bias: Matrix33 Dp_D_gyro_bias: Matrix33 accel_bias_hat: Matrix31 gyro_bias_hat: Matrix31 gravity_direction: Unit3 gravity_norm: Scalar dt: Scalar epsilon: Scalar

Outputs: res: Matrix91 jacobian: (9x26) jacobian of res wrt args pose_i (6), vel_i (3), pose_j (6), vel_j (3), accel_bias_i (3), gyro_bias_i (3), gravity_direction (2) hessian: (26x26) Gauss-Newton hessian for args pose_i (6), vel_i (3), pose_j (6), vel_j (3), accel_bias_i (3), gyro_bias_i (3), gravity_direction (2) rhs: (26x1) Gauss-Newton rhs for args pose_i (6), vel_i (3), pose_j (6), vel_j (3), accel_bias_i (3), gyro_bias_i (3), gravity_direction (2)

template<typename Scalar>
void InternalImuWithGravityFactor(const sym::Pose3<Scalar> &pose_i, const Eigen::Matrix<Scalar, 3, 1> &vel_i, const sym::Pose3<Scalar> &pose_j, const Eigen::Matrix<Scalar, 3, 1> &vel_j, const Eigen::Matrix<Scalar, 3, 1> &accel_bias_i, const Eigen::Matrix<Scalar, 3, 1> &gyro_bias_i, const sym::Rot3<Scalar> &DR, const Eigen::Matrix<Scalar, 3, 1> &Dv, const Eigen::Matrix<Scalar, 3, 1> &Dp, const Eigen::Matrix<Scalar, 9, 9> &sqrt_info, const Eigen::Matrix<Scalar, 3, 3> &DR_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 3> &Dv_D_accel_bias, const Eigen::Matrix<Scalar, 3, 3> &Dv_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 3> &Dp_D_accel_bias, const Eigen::Matrix<Scalar, 3, 3> &Dp_D_gyro_bias, const Eigen::Matrix<Scalar, 3, 1> &accel_bias_hat, const Eigen::Matrix<Scalar, 3, 1> &gyro_bias_hat, const Eigen::Matrix<Scalar, 3, 1> &gravity, const Scalar dt, const Scalar epsilon, Eigen::Matrix<Scalar, 9, 1> *const res = nullptr, Eigen::Matrix<Scalar, 9, 27> *const jacobian = nullptr, Eigen::Matrix<Scalar, 27, 27> *const hessian = nullptr, Eigen::Matrix<Scalar, 27, 1> *const rhs = nullptr)

An internal helper function to linearize the difference between the orientation, velocity, and position at one time step and those of another time step. The expected difference is calculated from the preintegrated IMU measurements between those time steps.

NOTE: If you are looking for a residual for an IMU factor, do not use this. Instead use the one found in symforce/slam/imu_preintegration/imu_factor.h.

Time step i is the time of the first IMU measurement of the interval. Time step j is the time after the last IMU measurement of the interval.

Assumes sqrt_info is lower triangular and only reads the lower triangle.

Args: pose_i: Pose at time step i (world_T_body) vel_i: Velocity at time step i (world frame) pose_j: Pose at time step j (world_T_body) vel_j: Velocity at time step j (world frame) accel_bias_i: The accelerometer bias between timesteps i and j gyro_bias_i: The gyroscope bias between timesteps i and j DR: Preintegrated estimate for pose_i.R.inverse() * pose_j.R Dv: Preintegrated estimate for vel_j - vel_i in the body frame at timestep i Dp: Preintegrated estimate for position change (before velocity and gravity corrections) in the body frame at timestep i::

R_i^T (p_j - p_i - v_i Delta t - 1/2 g Delta t^2),
where R_i = pose_i.R, p_i = pose_i.t, p_j = pose_j.t, and v_i = vel_i
sqrt_info: sqrt info matrix of DR(‘s tangent space), Dv, Dp DR_D_gyro_bias: Derivative of DR w.r.t. gyro_bias evaluated at gyro_bias_hat Dv_D_accel_bias: Derivative of Dv w.r.t. accel_bias evaluated at accel_bias_hat Dv_D_gyro_bias: Derivative of Dv w.r.t. gyro_bias evaluated at gyro_bias_hat Dp_D_accel_bias: Derivative of Dp w.r.t. accel_bias evaluated at accel_bias_hat Dp_D_gyro_bias: Derivative of Dp w.r.t. gyro_bias evaluated at gyro_bias_hat accel_bias_hat: The accelerometer bias used during preintegration gyro_bias_hat: The gyroscope bias used during preintegration gravity: Acceleration due to gravity (in the same frame as pose_x and vel_x), i.e., the vector which when added to the accelerometer measurements gives the true acceleration (up to bias and noise) of the IMU. dt: The time between timestep i and j: t_j - t_i epsilon: epsilon used for numerical stability

Outputs: res: 9dof product residual between the orientations, then velocities, then positions. jacobian: (9x27) jacobian of res wrt args pose_i (6), vel_i (3), pose_j (6), vel_j (3), accel_bias_i (3), gyro_bias_i (3), gravity (3) hessian: (27x27) Gauss-Newton hessian for args pose_i (6), vel_i (3), pose_j (6), vel_j (3), accel_bias_i (3), gyro_bias_i (3), gravity (3) rhs: (27x1) Gauss-Newton rhs for args pose_i (6), vel_i (3), pose_j (6), vel_j (3), accel_bias_i (3), gyro_bias_i (3), gravity (3)

template<typename Scalar>
void RollForwardState(const sym::Pose3<Scalar> &pose_i, const Eigen::Matrix<Scalar, 3, 1> &vel_i, const sym::Rot3<Scalar> &DR, const Eigen::Matrix<Scalar, 3, 1> &Dv, const Eigen::Matrix<Scalar, 3, 1> &Dp, const Eigen::Matrix<Scalar, 3, 1> &gravity, const Scalar dt, sym::Pose3<Scalar> *const res0 = nullptr, Eigen::Matrix<Scalar, 3, 1> *const res1 = nullptr)#

Roll forward the given state by the given preintegrated measurements

This returns the pose_j and vel_j that give 0 residual

Args: pose_i: Pose at time step i (world_T_body) vel_i: Velocity at time step i (world frame) DR: Preintegrated estimate for pose_i.R.inverse() * pose_j.R Dv: Preintegrated estimate for vel_j - vel_i in the body frame at timestep i Dp: Preintegrated estimate for position change (before velocity and gravity gravity: Acceleration due to gravity (in the same frame as pose_x and vel_x), i.e., the vector which when added to the accelerometer measurements gives the true acceleration (up to bias and noise) of the IMU. dt: The time between timestep i and j: t_j - t_i

Returns: T.Tuple[sf.Pose3, sf.V3]: pose_j, vel_j

template<typename Scalar>
void InverseRangeLandmarkAtanGncFactor(const sym::Pose3<Scalar> &source_pose, const sym::ATANCameraCal<Scalar> &source_calibration, const sym::Pose3<Scalar> &target_pose, const sym::ATANCameraCal<Scalar> &target_calibration, const Scalar source_inverse_range, const Eigen::Matrix<Scalar, 2, 1> &source_pixel, const Eigen::Matrix<Scalar, 2, 1> &target_pixel, const Scalar weight, const Scalar gnc_mu, const Scalar gnc_scale, const Scalar epsilon, Eigen::Matrix<Scalar, 2, 1> *const res = nullptr, Eigen::Matrix<Scalar, 2, 13> *const jacobian = nullptr, Eigen::Matrix<Scalar, 13, 13> *const hessian = nullptr, Eigen::Matrix<Scalar, 13, 1> *const rhs = nullptr)

Return the 2dof residual of reprojecting the landmark into the target camera and comparing against the correspondence in the target camera.

The landmark is specified as a pixel in the source camera and an inverse range; this means the landmark is fixed in the source camera and always has residual 0 there (this 0 residual is not returned, only the residual in the target camera is returned).

The norm of the residual is whitened using the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel>. Whitening each component of the reprojection error separately would result in rejecting individual components as outliers. Instead, we minimize the whitened norm of the full reprojection error for each point. See :meth:ScalarNoiseModel.whiten_norm <symforce.opt.noise_models.ScalarNoiseModel.whiten_norm> for more information on this, and :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> for more information on the noise model.

Args: source_pose: The pose of the source camera source_calibration: The source camera calibration target_pose: The pose of the target camera target_calibration: The target camera calibration source_inverse_range: The inverse range of the landmark in the source camera source_pixel: The location of the landmark in the source camera target_pixel: The location of the correspondence in the target camera weight: The weight of the factor gnc_mu: The mu convexity parameter for the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> gnc_scale: The scale parameter for the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> epsilon: Small positive value

Outputs: res: 2dof residual of the reprojection jacobian: (2x13) jacobian of res wrt args source_pose (6), target_pose (6), source_inverse_range (1) hessian: (13x13) Gauss-Newton hessian for args source_pose (6), target_pose (6), source_inverse_range (1) rhs: (13x1) Gauss-Newton rhs for args source_pose (6), target_pose (6), source_inverse_range (1)

template<typename Scalar>
void InverseRangeLandmarkDoubleSphereGncFactor(const sym::Pose3<Scalar> &source_pose, const sym::DoubleSphereCameraCal<Scalar> &source_calibration, const sym::Pose3<Scalar> &target_pose, const sym::DoubleSphereCameraCal<Scalar> &target_calibration, const Scalar source_inverse_range, const Eigen::Matrix<Scalar, 2, 1> &source_pixel, const Eigen::Matrix<Scalar, 2, 1> &target_pixel, const Scalar weight, const Scalar gnc_mu, const Scalar gnc_scale, const Scalar epsilon, Eigen::Matrix<Scalar, 2, 1> *const res = nullptr, Eigen::Matrix<Scalar, 2, 13> *const jacobian = nullptr, Eigen::Matrix<Scalar, 13, 13> *const hessian = nullptr, Eigen::Matrix<Scalar, 13, 1> *const rhs = nullptr)

Return the 2dof residual of reprojecting the landmark into the target camera and comparing against the correspondence in the target camera.

The landmark is specified as a pixel in the source camera and an inverse range; this means the landmark is fixed in the source camera and always has residual 0 there (this 0 residual is not returned, only the residual in the target camera is returned).

The norm of the residual is whitened using the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel>. Whitening each component of the reprojection error separately would result in rejecting individual components as outliers. Instead, we minimize the whitened norm of the full reprojection error for each point. See :meth:ScalarNoiseModel.whiten_norm <symforce.opt.noise_models.ScalarNoiseModel.whiten_norm> for more information on this, and :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> for more information on the noise model.

Args: source_pose: The pose of the source camera source_calibration: The source camera calibration target_pose: The pose of the target camera target_calibration: The target camera calibration source_inverse_range: The inverse range of the landmark in the source camera source_pixel: The location of the landmark in the source camera target_pixel: The location of the correspondence in the target camera weight: The weight of the factor gnc_mu: The mu convexity parameter for the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> gnc_scale: The scale parameter for the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> epsilon: Small positive value

Outputs: res: 2dof residual of the reprojection jacobian: (2x13) jacobian of res wrt args source_pose (6), target_pose (6), source_inverse_range (1) hessian: (13x13) Gauss-Newton hessian for args source_pose (6), target_pose (6), source_inverse_range (1) rhs: (13x1) Gauss-Newton rhs for args source_pose (6), target_pose (6), source_inverse_range (1)

template<typename Scalar>
void InverseRangeLandmarkEquirectangularGncFactor(const sym::Pose3<Scalar> &source_pose, const sym::EquirectangularCameraCal<Scalar> &source_calibration, const sym::Pose3<Scalar> &target_pose, const sym::EquirectangularCameraCal<Scalar> &target_calibration, const Scalar source_inverse_range, const Eigen::Matrix<Scalar, 2, 1> &source_pixel, const Eigen::Matrix<Scalar, 2, 1> &target_pixel, const Scalar weight, const Scalar gnc_mu, const Scalar gnc_scale, const Scalar epsilon, Eigen::Matrix<Scalar, 2, 1> *const res = nullptr, Eigen::Matrix<Scalar, 2, 13> *const jacobian = nullptr, Eigen::Matrix<Scalar, 13, 13> *const hessian = nullptr, Eigen::Matrix<Scalar, 13, 1> *const rhs = nullptr)

Return the 2dof residual of reprojecting the landmark into the target camera and comparing against the correspondence in the target camera.

The landmark is specified as a pixel in the source camera and an inverse range; this means the landmark is fixed in the source camera and always has residual 0 there (this 0 residual is not returned, only the residual in the target camera is returned).

The norm of the residual is whitened using the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel>. Whitening each component of the reprojection error separately would result in rejecting individual components as outliers. Instead, we minimize the whitened norm of the full reprojection error for each point. See :meth:ScalarNoiseModel.whiten_norm <symforce.opt.noise_models.ScalarNoiseModel.whiten_norm> for more information on this, and :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> for more information on the noise model.

Args: source_pose: The pose of the source camera source_calibration: The source camera calibration target_pose: The pose of the target camera target_calibration: The target camera calibration source_inverse_range: The inverse range of the landmark in the source camera source_pixel: The location of the landmark in the source camera target_pixel: The location of the correspondence in the target camera weight: The weight of the factor gnc_mu: The mu convexity parameter for the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> gnc_scale: The scale parameter for the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> epsilon: Small positive value

Outputs: res: 2dof residual of the reprojection jacobian: (2x13) jacobian of res wrt args source_pose (6), target_pose (6), source_inverse_range (1) hessian: (13x13) Gauss-Newton hessian for args source_pose (6), target_pose (6), source_inverse_range (1) rhs: (13x1) Gauss-Newton rhs for args source_pose (6), target_pose (6), source_inverse_range (1)

template<typename Scalar>
void InverseRangeLandmarkLinearGncFactor(const sym::Pose3<Scalar> &source_pose, const sym::LinearCameraCal<Scalar> &source_calibration, const sym::Pose3<Scalar> &target_pose, const sym::LinearCameraCal<Scalar> &target_calibration, const Scalar source_inverse_range, const Eigen::Matrix<Scalar, 2, 1> &source_pixel, const Eigen::Matrix<Scalar, 2, 1> &target_pixel, const Scalar weight, const Scalar gnc_mu, const Scalar gnc_scale, const Scalar epsilon, Eigen::Matrix<Scalar, 2, 1> *const res = nullptr, Eigen::Matrix<Scalar, 2, 13> *const jacobian = nullptr, Eigen::Matrix<Scalar, 13, 13> *const hessian = nullptr, Eigen::Matrix<Scalar, 13, 1> *const rhs = nullptr)

Return the 2dof residual of reprojecting the landmark into the target camera and comparing against the correspondence in the target camera.

The landmark is specified as a pixel in the source camera and an inverse range; this means the landmark is fixed in the source camera and always has residual 0 there (this 0 residual is not returned, only the residual in the target camera is returned).

The norm of the residual is whitened using the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel>. Whitening each component of the reprojection error separately would result in rejecting individual components as outliers. Instead, we minimize the whitened norm of the full reprojection error for each point. See :meth:ScalarNoiseModel.whiten_norm <symforce.opt.noise_models.ScalarNoiseModel.whiten_norm> for more information on this, and :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> for more information on the noise model.

Args: source_pose: The pose of the source camera source_calibration: The source camera calibration target_pose: The pose of the target camera target_calibration: The target camera calibration source_inverse_range: The inverse range of the landmark in the source camera source_pixel: The location of the landmark in the source camera target_pixel: The location of the correspondence in the target camera weight: The weight of the factor gnc_mu: The mu convexity parameter for the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> gnc_scale: The scale parameter for the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> epsilon: Small positive value

Outputs: res: 2dof residual of the reprojection jacobian: (2x13) jacobian of res wrt args source_pose (6), target_pose (6), source_inverse_range (1) hessian: (13x13) Gauss-Newton hessian for args source_pose (6), target_pose (6), source_inverse_range (1) rhs: (13x1) Gauss-Newton rhs for args source_pose (6), target_pose (6), source_inverse_range (1)

template<typename Scalar>
void InverseRangeLandmarkPolynomialGncFactor(const sym::Pose3<Scalar> &source_pose, const sym::Pose3<Scalar> &target_pose, const sym::PolynomialCameraCal<Scalar> &target_calibration, const Scalar source_inverse_range, const Eigen::Matrix<Scalar, 3, 1> &p_camera_source, const Eigen::Matrix<Scalar, 2, 1> &target_pixel, const Scalar weight, const Scalar gnc_mu, const Scalar gnc_scale, const Scalar epsilon, Eigen::Matrix<Scalar, 2, 1> *const res = nullptr, Eigen::Matrix<Scalar, 2, 13> *const jacobian = nullptr, Eigen::Matrix<Scalar, 13, 13> *const hessian = nullptr, Eigen::Matrix<Scalar, 13, 1> *const rhs = nullptr)

Return the 2dof residual of reprojecting the landmark ray into the target spherical camera and comparing it against the correspondence.

The landmark is specified as a camera point in the source camera with an inverse range; this means the landmark is fixed in the source camera and always has residual 0 there (this 0 residual is not returned, only the residual in the target camera is returned).

The norm of the residual is whitened using the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel>. Whitening each component of the reprojection error separately would result in rejecting individual components as outliers. Instead, we minimize the whitened norm of the full reprojection error for each point. See :meth:ScalarNoiseModel.whiten_norm <symforce.opt.noise_models.ScalarNoiseModel.whiten_norm> for more information on this, and :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> for more information on the noise model.

Args: source_pose: The pose of the source camera target_pose: The pose of the target camera target_calibration: The target spherical camera calibration source_inverse_range: The inverse range of the landmark in the source camera p_camera_source: The location of the landmark in the source camera coordinate, will be normalized target_pixel: The location of the correspondence in the target camera weight: The weight of the factor gnc_mu: The mu convexity parameter for the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> gnc_scale: The scale parameter for the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> epsilon: Small positive value

Outputs: res: 2dof whiten residual of the reprojection jacobian: (2x13) jacobian of res wrt args source_pose (6), target_pose (6), source_inverse_range (1) hessian: (13x13) Gauss-Newton hessian for args source_pose (6), target_pose (6), source_inverse_range (1) rhs: (13x1) Gauss-Newton rhs for args source_pose (6), target_pose (6), source_inverse_range (1)

template<typename Scalar>
void InverseRangeLandmarkPriorFactor(const Scalar landmark_inverse_range, const Scalar inverse_range_prior, const Scalar weight, const Scalar sigma, const Scalar epsilon, Eigen::Matrix<Scalar, 1, 1> *const res = nullptr, Eigen::Matrix<Scalar, 1, 1> *const jacobian = nullptr, Eigen::Matrix<Scalar, 1, 1> *const hessian = nullptr, Eigen::Matrix<Scalar, 1, 1> *const rhs = nullptr)#

Factor representing a Gaussian prior on the inverse range of a landmark

Args: landmark_inverse_range: The current inverse range estimate inverse_range_prior: The mean of the inverse range prior weight: The weight of the prior sigma: The standard deviation of the prior epsilon: Small positive value

Outputs: res: 1dof residual of the prior jacobian: (1x1) jacobian of res wrt arg landmark_inverse_range (1) hessian: (1x1) Gauss-Newton hessian for arg landmark_inverse_range (1) rhs: (1x1) Gauss-Newton rhs for arg landmark_inverse_range (1)

template<typename Scalar>
void InverseRangeLandmarkSphericalGncFactor(const sym::Pose3<Scalar> &source_pose, const sym::Pose3<Scalar> &target_pose, const sym::SphericalCameraCal<Scalar> &target_calibration, const Scalar source_inverse_range, const Eigen::Matrix<Scalar, 3, 1> &p_camera_source, const Eigen::Matrix<Scalar, 2, 1> &target_pixel, const Scalar weight, const Scalar gnc_mu, const Scalar gnc_scale, const Scalar epsilon, Eigen::Matrix<Scalar, 2, 1> *const res = nullptr, Eigen::Matrix<Scalar, 2, 13> *const jacobian = nullptr, Eigen::Matrix<Scalar, 13, 13> *const hessian = nullptr, Eigen::Matrix<Scalar, 13, 1> *const rhs = nullptr)#

Return the 2dof residual of reprojecting the landmark ray into the target spherical camera and comparing it against the correspondence.

The landmark is specified as a camera point in the source camera with an inverse range; this means the landmark is fixed in the source camera and always has residual 0 there (this 0 residual is not returned, only the residual in the target camera is returned).

The norm of the residual is whitened using the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel>. Whitening each component of the reprojection error separately would result in rejecting individual components as outliers. Instead, we minimize the whitened norm of the full reprojection error for each point. See :meth:ScalarNoiseModel.whiten_norm <symforce.opt.noise_models.ScalarNoiseModel.whiten_norm> for more information on this, and :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> for more information on the noise model.

Args: source_pose: The pose of the source camera target_pose: The pose of the target camera target_calibration: The target spherical camera calibration source_inverse_range: The inverse range of the landmark in the source camera p_camera_source: The location of the landmark in the source camera coordinate, will be normalized target_pixel: The location of the correspondence in the target camera weight: The weight of the factor gnc_mu: The mu convexity parameter for the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> gnc_scale: The scale parameter for the :class:BarronNoiseModel <symforce.opt.noise_models.BarronNoiseModel> epsilon: Small positive value

Outputs: res: 2dof whiten residual of the reprojection jacobian: (2x13) jacobian of res wrt args source_pose (6), target_pose (6), source_inverse_range (1) hessian: (13x13) Gauss-Newton hessian for args source_pose (6), target_pose (6), source_inverse_range (1) rhs: (13x1) Gauss-Newton rhs for args source_pose (6), target_pose (6), source_inverse_range (1)

template<typename Scalar>
void LinearReprojectionDelta(const sym::Pose3<Scalar> &source_pose, const sym::LinearCameraCal<Scalar> &source_calibration, const sym::Pose3<Scalar> &target_pose, const sym::LinearCameraCal<Scalar> &target_calibration, const Scalar source_inverse_range, const Eigen::Matrix<Scalar, 2, 1> &source_pixel, const Eigen::Matrix<Scalar, 2, 1> &target_pixel, const Scalar epsilon, Eigen::Matrix<Scalar, 2, 1> *const reprojection_delta = nullptr, Scalar *const is_valid = nullptr)#

Reprojects the landmark into the target camera and returns the delta from the correspondence to the reprojection.

The landmark is specified as a pixel in the source camera and an inverse range; this means the landmark is fixed in the source camera and always has residual 0 there (this 0 residual is not returned, only the residual in the target camera is returned).

Args: source_pose: The pose of the source camera source_calibration: The source camera calibration target_pose: The pose of the target camera target_calibration: The target camera calibration source_inverse_range: The inverse range of the landmark in the source camera source_pixel: The location of the landmark in the source camera target_pixel: The location of the correspondence in the target camera epsilon: Small positive value camera_model_class: The subclass of CameraCal to use as the camera model

Outputs: res: 2dof pixel reprojection error valid: is valid projection or not

template<typename Scalar>
void PolynomialReprojectionDelta(const sym::Pose3<Scalar> &source_pose, const sym::Pose3<Scalar> &target_pose, const sym::PolynomialCameraCal<Scalar> &target_calibration, const Scalar source_inverse_range, const Eigen::Matrix<Scalar, 3, 1> &p_camera_source, const Eigen::Matrix<Scalar, 2, 1> &target_pixel, const Scalar epsilon, Eigen::Matrix<Scalar, 2, 1> *const reprojection_delta = nullptr, Scalar *const is_valid = nullptr)#

Reprojects the landmark ray into the target camera and returns the delta between the correspondence and the reprojection.

The landmark is specified as a 3D point or ray (will be normalized) in the source camera; this means the landmark is fixed in the source camera and always has residual 0 there (this 0 residual is not returned, only the residual in the target camera is returned).

Args: source_pose: The pose of the source camera target_pose: The pose of the target camera target_calibration: The target camera calibration source_inverse_range: The inverse range of the landmark in the source camera p_camera_source: The location of the landmark in the source camera coordinate, will be normalized target_pixel: The location of the correspondence in the target camera epsilon: Small positive value

Outputs: res: 2dof reprojection delta valid: is valid projection or not

template<typename Scalar>
void PriorFactorMatrix31(const Eigen::Matrix<Scalar, 3, 1> &value, const Eigen::Matrix<Scalar, 3, 1> &prior, const Eigen::Matrix<Scalar, 3, 3> &sqrt_info, const Scalar epsilon, Eigen::Matrix<Scalar, 3, 1> *const res = nullptr, Eigen::Matrix<Scalar, 3, 3> *const jacobian = nullptr, Eigen::Matrix<Scalar, 3, 3> *const hessian = nullptr, Eigen::Matrix<Scalar, 3, 1> *const rhs = nullptr)#

Residual that penalizes the difference between a value and prior (desired / measured value).

In vector space terms that would be: prior - value

In lie group terms: to_tangent(compose(inverse(value), prior))

Args: sqrt_info: Square root information matrix to whiten residual. This can be computed from a covariance matrix as the cholesky decomposition of the inverse. In the case of a diagonal it will contain 1/sigma values. Must match the tangent dim. jacobian: (3x3) jacobian of res wrt arg value (3) hessian: (3x3) Gauss-Newton hessian for arg value (3) rhs: (3x1) Gauss-Newton rhs for arg value (3)

template<typename Scalar>
void PriorFactorPose2(const sym::Pose2<Scalar> &value, const sym::Pose2<Scalar> &prior, const Eigen::Matrix<Scalar, 3, 3> &sqrt_info, const Scalar epsilon, Eigen::Matrix<Scalar, 3, 1> *const res = nullptr, Eigen::Matrix<Scalar, 3, 3> *const jacobian = nullptr, Eigen::Matrix<Scalar, 3, 3> *const hessian = nullptr, Eigen::Matrix<Scalar, 3, 1> *const rhs = nullptr)#

Residual that penalizes the difference between a value and prior (desired / measured value).

In vector space terms that would be: prior - value

In lie group terms: to_tangent(compose(inverse(value), prior))

Args: sqrt_info: Square root information matrix to whiten residual. This can be computed from a covariance matrix as the cholesky decomposition of the inverse. In the case of a diagonal it will contain 1/sigma values. Must match the tangent dim. jacobian: (3x3) jacobian of res wrt arg value (3) hessian: (3x3) Gauss-Newton hessian for arg value (3) rhs: (3x1) Gauss-Newton rhs for arg value (3)

template<typename Scalar>
void PriorFactorPose3(const sym::Pose3<Scalar> &value, const sym::Pose3<Scalar> &prior, const Eigen::Matrix<Scalar, 6, 6> &sqrt_info, const Scalar epsilon, Eigen::Matrix<Scalar, 6, 1> *const res = nullptr, Eigen::Matrix<Scalar, 6, 6> *const jacobian = nullptr, Eigen::Matrix<Scalar, 6, 6> *const hessian = nullptr, Eigen::Matrix<Scalar, 6, 1> *const rhs = nullptr)#

Residual that penalizes the difference between a value and prior (desired / measured value).

In vector space terms that would be: prior - value

In lie group terms: to_tangent(compose(inverse(value), prior))

Args: sqrt_info: Square root information matrix to whiten residual. This can be computed from a covariance matrix as the cholesky decomposition of the inverse. In the case of a diagonal it will contain 1/sigma values. Must match the tangent dim. jacobian: (6x6) jacobian of res wrt arg value (6) hessian: (6x6) Gauss-Newton hessian for arg value (6) rhs: (6x1) Gauss-Newton rhs for arg value (6)

template<typename Scalar>
void PriorFactorPose3Position(const sym::Pose3<Scalar> &value, const Eigen::Matrix<Scalar, 3, 1> &prior, const Eigen::Matrix<Scalar, 3, 3> &sqrt_info, const Scalar epsilon, Eigen::Matrix<Scalar, 3, 1> *const res = nullptr, Eigen::Matrix<Scalar, 3, 6> *const jacobian = nullptr, Eigen::Matrix<Scalar, 6, 6> *const hessian = nullptr, Eigen::Matrix<Scalar, 6, 1> *const rhs = nullptr)#

Residual that penalizes the difference between a value and prior (desired / measured value).

In vector space terms that would be: prior - value

In lie group terms: to_tangent(compose(inverse(value), prior))

Args: sqrt_info: Square root information matrix to whiten residual. This can be computed from a covariance matrix as the cholesky decomposition of the inverse. In the case of a diagonal it will contain 1/sigma values. Must match the tangent dim. jacobian: (3x6) jacobian of res wrt arg value (6) hessian: (6x6) Gauss-Newton hessian for arg value (6) rhs: (6x1) Gauss-Newton rhs for arg value (6)

template<typename Scalar>
void PriorFactorPose3Rotation(const sym::Pose3<Scalar> &value, const sym::Rot3<Scalar> &prior, const Eigen::Matrix<Scalar, 3, 3> &sqrt_info, const Scalar epsilon, Eigen::Matrix<Scalar, 3, 1> *const res = nullptr, Eigen::Matrix<Scalar, 3, 6> *const jacobian = nullptr, Eigen::Matrix<Scalar, 6, 6> *const hessian = nullptr, Eigen::Matrix<Scalar, 6, 1> *const rhs = nullptr)#

Residual that penalizes the difference between a value and prior (desired / measured value).

In vector space terms that would be: prior - value

In lie group terms: to_tangent(compose(inverse(value), prior))

Args: sqrt_info: Square root information matrix to whiten residual. This can be computed from a covariance matrix as the cholesky decomposition of the inverse. In the case of a diagonal it will contain 1/sigma values. Must match the tangent dim. jacobian: (3x6) jacobian of res wrt arg value (6) hessian: (6x6) Gauss-Newton hessian for arg value (6) rhs: (6x1) Gauss-Newton rhs for arg value (6)

template<typename Scalar>
void PriorFactorRot2(const sym::Rot2<Scalar> &value, const sym::Rot2<Scalar> &prior, const Eigen::Matrix<Scalar, 1, 1> &sqrt_info, const Scalar epsilon, Eigen::Matrix<Scalar, 1, 1> *const res = nullptr, Eigen::Matrix<Scalar, 1, 1> *const jacobian = nullptr, Eigen::Matrix<Scalar, 1, 1> *const hessian = nullptr, Eigen::Matrix<Scalar, 1, 1> *const rhs = nullptr)#

Residual that penalizes the difference between a value and prior (desired / measured value).

In vector space terms that would be: prior - value

In lie group terms: to_tangent(compose(inverse(value), prior))

Args: sqrt_info: Square root information matrix to whiten residual. This can be computed from a covariance matrix as the cholesky decomposition of the inverse. In the case of a diagonal it will contain 1/sigma values. Must match the tangent dim. jacobian: (1x1) jacobian of res wrt arg value (1) hessian: (1x1) Gauss-Newton hessian for arg value (1) rhs: (1x1) Gauss-Newton rhs for arg value (1)

template<typename Scalar>
void PriorFactorRot3(const sym::Rot3<Scalar> &value, const sym::Rot3<Scalar> &prior, const Eigen::Matrix<Scalar, 3, 3> &sqrt_info, const Scalar epsilon, Eigen::Matrix<Scalar, 3, 1> *const res = nullptr, Eigen::Matrix<Scalar, 3, 3> *const jacobian = nullptr, Eigen::Matrix<Scalar, 3, 3> *const hessian = nullptr, Eigen::Matrix<Scalar, 3, 1> *const rhs = nullptr)#

Residual that penalizes the difference between a value and prior (desired / measured value).

In vector space terms that would be: prior - value

In lie group terms: to_tangent(compose(inverse(value), prior))

Args: sqrt_info: Square root information matrix to whiten residual. This can be computed from a covariance matrix as the cholesky decomposition of the inverse. In the case of a diagonal it will contain 1/sigma values. Must match the tangent dim. jacobian: (3x3) jacobian of res wrt arg value (3) hessian: (3x3) Gauss-Newton hessian for arg value (3) rhs: (3x1) Gauss-Newton rhs for arg value (3)

template<typename Scalar>
void SphericalReprojectionDelta(const sym::Pose3<Scalar> &source_pose, const sym::Pose3<Scalar> &target_pose, const sym::SphericalCameraCal<Scalar> &target_calibration, const Scalar source_inverse_range, const Eigen::Matrix<Scalar, 3, 1> &p_camera_source, const Eigen::Matrix<Scalar, 2, 1> &target_pixel, const Scalar epsilon, Eigen::Matrix<Scalar, 2, 1> *const reprojection_delta = nullptr, Scalar *const is_valid = nullptr)

Reprojects the landmark ray into the target camera and returns the delta between the correspondence and the reprojection.

The landmark is specified as a 3D point or ray (will be normalized) in the source camera; this means the landmark is fixed in the source camera and always has residual 0 there (this 0 residual is not returned, only the residual in the target camera is returned).

Args: source_pose: The pose of the source camera target_pose: The pose of the target camera target_calibration: The target camera calibration source_inverse_range: The inverse range of the landmark in the source camera p_camera_source: The location of the landmark in the source camera coordinate, will be normalized target_pixel: The location of the correspondence in the target camera epsilon: Small positive value

Outputs: res: 2dof reprojection delta valid: is valid projection or not

std::ostream &operator<<(std::ostream &os, const LinearCameraCald &a)#
std::ostream &operator<<(std::ostream &os, const LinearCameraCalf &a)#
template<typename T>
bool IsClose(const