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)))

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)))

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.

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.

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.

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.

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)

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.

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

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 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.

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

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.

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

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.

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

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).

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.

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.

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.

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 T &a, const T &b, const typename StorageOps<T>::Scalar tol)#

template<typename T, typename Generator> T Random(Generator &gen)#

std::ostream &operator<<(std::ostream &os, const PolynomialCameraCald &a)#

std::ostream &operator<<(std::ostream &os, const PolynomialCameraCalf &a)#

std::ostream &operator<<(std::ostream &os, const Pose2d &a)#

std::ostream &operator<<(std::ostream &os, const Pose2f &a)#

std::ostream &operator<<(std::ostream &os, const Pose3d &a)#

std::ostream &operator<<(std::ostream &os, const Pose3f &a)#

std::ostream &operator<<(std::ostream &os, const Rot2d &a)#

std::ostream &operator<<(std::ostream &os, const Rot2f &a)#

std::ostream &operator<<(std::ostream &os, const Rot3d &a)#

std::ostream &operator<<(std::ostream &os, const Rot3f &a)#

std::ostream &operator<<(std::ostream &os, const SphericalCameraCald &a)

std::ostream &operator<<(std::ostream &os, const SphericalCameraCalf &a)

std::ostream &operator<<(std::ostream &os, const Unit3d &a)

std::ostream &operator<<(std::ostream &os, const Unit3f &a)

inline bool IsEigenType(const type_t type)

inline std::pair<int, int> EigenTypeShape(const type_t type): Returns the shape of an Eigen type as a pair of ints (rows, cols)

Variables

template<typename Scalar> constexpr Scalar kDefaultEpsilon = Scalar(10.0) * std::numeric_limits<Scalar>::epsilon()

kDefaultEpsilon is a small positive number for avoiding numerical singularities.

It is based on numerical epsilon (the difference between 1.0 and the next floating point number), scaled up for safety.

An example of where kDefaultEpsilon might be used is in the evaluation of sin(x - 1) / (x - 1), which, at x = 1, evaluates to 0/0, i.e., NaN. This is despite the fact that the limit as x -> 1 is well defined (itself equaling 1).

If we used x + copysign(kDefaultEpsilon, x) instead of x, we’d avoid the singularity and get a nearly correct answer (due to the continuity of the function).

For more information on epsilons and how to use them, see symforce/notebooks/epsilon_sandbox.ipynb

const template double kDefaultEpsilon< double >

const template float kDefaultEpsilon< float >

constexpr double kDefaultEpsilond = kDefaultEpsilon<double>

constexpr float kDefaultEpsilonf = kDefaultEpsilon<float>

template<typename T> static constexpr const bool kIsEigenType = std::is_base_of<Eigen::MatrixBase<T>, T>::value

template<typename T> static constexpr const bool kIsSparseEigenType = std::is_base_of<Eigen::SparseMatrix<typename T::Scalar>, T>::value

template<typename ScalarType> class ATANCameraCal

#include <atan_camera_cal.h>

Autogenerated C++ implementation of symforce.cam.atan_camera_cal.ATANCameraCal.

ATAN camera with 5 parameters [fx, fy, cx, cy, omega].

(fx, fy) representing focal length, (cx, cy) representing principal point, and omega representing the distortion parameter.

See here for more details: https://hal.inria.fr/inria-00267247/file/distcalib.pdf

Public Types

using Scalar = ScalarType

using Self = ATANCameraCal<Scalar>

using DataVec = Eigen::Matrix<Scalar, 5, 1>

Public Functions

inline ATANCameraCal(const Eigen::Matrix<Scalar, 2, 1> &focal_length, const Eigen::Matrix<Scalar, 2, 1> &principal_point, const Scalar omega)

inline explicit ATANCameraCal(const DataVec &data, bool normalize = true)

Construct from data vec

Parameters:: normalize – Project to the manifold on construction. This ensures numerical stability as this constructor is called after each codegen operation. Constructing from a normalized vector may be faster, e.g. with FromStorage.

inline const DataVec &Data() const

inline void ToStorage(Scalar *const vec) const

Eigen::Matrix<Scalar, 2, 1> FocalLength() const: Return the focal length.

Eigen::Matrix<Scalar, 2, 1> PrincipalPoint() const: Return the principal point.

Eigen::Matrix<Scalar, 2, 1> PixelFromCameraPoint(const Eigen::Matrix<Scalar, 3, 1> &point, const Scalar epsilon, Scalar *const is_valid = nullptr) const

Project a 3D point in the camera frame into 2D pixel coordinates.

Returns: pixel: (x, y) coordinate in pixels if valid is_valid: 1 if the operation is within bounds else 0

Eigen::Matrix<Scalar, 2, 1> PixelFromCameraPointWithJacobians(const Eigen::Matrix<Scalar, 3, 1> &point, const Scalar epsilon, Scalar *const is_valid = nullptr, Eigen::Matrix<Scalar, 2, 5> *const pixel_D_cal = nullptr, Eigen::Matrix<Scalar, 2, 3> *const pixel_D_point = nullptr) const

Project a 3D point in the camera frame into 2D pixel coordinates.

Returns: pixel: (x, y) coordinate in pixels if valid is_valid: 1 if the operation is within bounds else 0 pixel_D_cal: Derivative of pixel with respect to intrinsic calibration parameters pixel_D_point: Derivative of pixel with respect to point

Eigen::Matrix<Scalar, 3, 1> CameraRayFromPixel(const Eigen::Matrix<Scalar, 2, 1> &pixel, const Scalar epsilon, Scalar *const is_valid = nullptr) const

Backproject a 2D pixel coordinate into a 3D ray in the camera frame.

Returns: camera_ray: The ray in the camera frame (NOT normalized) is_valid: 1 if the operation is within bounds else 0

Eigen::Matrix<Scalar, 3, 1> CameraRayFromPixelWithJacobians(const Eigen::Matrix<Scalar, 2, 1> &pixel, const Scalar epsilon, Scalar *const is_valid = nullptr, Eigen::Matrix<Scalar, 3, 5> *const point_D_cal = nullptr, Eigen::Matrix<Scalar, 3, 2> *const point_D_pixel = nullptr) const

Backproject a 2D pixel coordinate into a 3D ray in the camera frame.

Returns: camera_ray: The ray in the camera frame (NOT normalized) is_valid: 1 if the operation is within bounds else 0 point_D_cal: Derivative of point with respect to intrinsic calibration parameters point_D_pixel: Derivation of point with respect to pixel

inline bool IsApprox(const Self &b, const Scalar tol) const

template<typename ToScalar> inline ATANCameraCal<ToScalar> Cast() const

inline bool operator==(const ATANCameraCal &rhs) const

Public Static Functions

static inline constexpr int32_t StorageDim()

static inline ATANCameraCal FromStorage(const Scalar *const vec)

template<typename CameraCalType> class Camera

#include <camera.h>

Camera with a given camera calibration and an optionally specified image size (width, height).

If the image size is specified, we use it to check whether pixels (either given or computed by projection of 3D points into the image frame) are in the image frame and thus valid/invalid.

Subclassed by sym::PosedCamera< CameraCalType >

Public Types

using Scalar = typename CameraCalType::Scalar

Public Functions

inline Camera(const CameraCalType &calibration, const Eigen::Vector2i &image_size = Eigen::Vector2i(-1, -1))

inline Eigen::Matrix<Scalar, 2, 1> FocalLength() const

inline Eigen::Matrix<Scalar, 2, 1> PrincipalPoint() const

inline Eigen::Matrix<Scalar, 2, 1> PixelFromCameraPoint(const Eigen::Matrix<Scalar, 3, 1> &point, const Scalar epsilon, Scalar *const is_valid) const

Project a 3D point in the camera frame into 2D pixel coordinates.

Returns: pixel: (x, y) coordinate in pixels if valid is_valid: 1 if the operation is within bounds (including image_size bounds) else 0

inline Eigen::Matrix<Scalar, 3, 1> CameraRayFromPixel(const Eigen::Matrix<Scalar, 2, 1> &pixel, const Scalar epsilon, Scalar *const is_valid) const

Backproject a 2D pixel coordinate into a 3D ray in the camera frame.

NOTE: If image_size is specified and the given pixel is out of bounds, is_valid will be set to zero.

Args: normalize: Whether camera_ray will be normalized (False by default)

Returns: camera_ray: The ray in the camera frame is_valid: 1 if the operation is within bounds else 0

inline Scalar MaybeCheckInView(const Eigen::Matrix<Scalar, 2, 1> &pixel) const

inline CameraCalType Calibration() const

inline Eigen::Matrix<int, 2, 1> ImageSize() const

Public Static Functions

static inline Scalar InView(const Eigen::Matrix<Scalar, 2, 1> &pixel, const Eigen::Matrix<int, 2, 1> &image_size): Returns 1.0 if the pixel coords are in bounds of the image, 0.0 otherwise.

template<typename ScalarType> class DoubleSphereCameraCal

#include <double_sphere_camera_cal.h>

Autogenerated C++ implementation of symforce.cam.double_sphere_camera_cal.DoubleSphereCameraCal.

Camera model where a point is consecutively projected onto two unit spheres with centers shifted by xi, then projected into the image plane using the pinhole model shifted by alpha / (1 - alpha).

There are important differences here from the derivation in the paper and in other open-source packages with double sphere models; see notebooks/double_sphere_derivation.ipynb for more information.

The storage for this class is:

[ fx fy cx cy xi alpha ]

TODO(aaron): Create double_sphere_derivation.ipynb

TODO(aaron): Probably want to check that values and derivatives are correct (or at least sane) on the valid side of the is_valid boundary

Reference: https://vision.in.tum.de/research/vslam/double-sphere

Public Types

using Scalar = ScalarType

using Self = DoubleSphereCameraCal<Scalar>

using DataVec = Eigen::Matrix<Scalar, 6, 1>

Public Functions

inline DoubleSphereCameraCal(const Eigen::Matrix<Scalar, 2, 1> &focal_length, const Eigen::Matrix<Scalar, 2, 1> &principal_point, const Scalar xi, const Scalar alpha)

inline explicit DoubleSphereCameraCal(const DataVec &data, bool normalize = true)

Construct from data vec

Parameters:: normalize – Project to the manifold on construction. This ensures numerical stability as this constructor is called after each codegen operation. Constructing from a normalized vector may be faster, e.g. with FromStorage.

inline const DataVec &Data() const

inline void ToStorage(Scalar *const vec) const

Eigen::Matrix<Scalar, 2, 1> FocalLength() const: Return the focal length.

Eigen::Matrix<Scalar, 2, 1> PrincipalPoint() const: Return the principal point.

Eigen::Matrix<Scalar, 2, 1> PixelFromCameraPoint(const Eigen::Matrix<Scalar, 3, 1> &point, const Scalar epsilon, Scalar *const is_valid = nullptr) const

Project a 3D point in the camera frame into 2D pixel coordinates.

Returns: pixel: (x, y) coordinate in pixels if valid is_valid: 1 if the operation is within bounds else 0

Eigen::Matrix<Scalar, 2, 1> PixelFromCameraPointWithJacobians(const Eigen::Matrix<Scalar, 3, 1> &point, const Scalar epsilon, Scalar *const is_valid = nullptr, Eigen::Matrix<Scalar, 2, 6> *const pixel_D_cal = nullptr, Eigen::Matrix<Scalar, 2, 3> *const pixel_D_point = nullptr) const

Project a 3D point in the camera frame into 2D pixel coordinates.

Eigen::Matrix<Scalar, 3, 1> CameraRayFromPixel(const Eigen::Matrix<Scalar, 2, 1> &pixel, const Scalar epsilon, Scalar *const is_valid = nullptr) const

Backproject a 2D pixel coordinate into a 3D ray in the camera frame.

Returns: camera_ray: The ray in the camera frame (NOT normalized) is_valid: 1 if the operation is within bounds else 0

Eigen::Matrix<Scalar, 3, 1> CameraRayFromPixelWithJacobians(const Eigen::Matrix<Scalar, 2, 1> &pixel, const Scalar epsilon, Scalar *const is_valid = nullptr, Eigen::Matrix<Scalar, 3, 6> *const point_D_cal = nullptr, Eigen::Matrix<Scalar, 3, 2> *const point_D_pixel = nullptr) const

Backproject a 2D pixel coordinate into a 3D ray in the camera frame.

inline bool IsApprox(const Self &b, const Scalar tol) const

template<typename ToScalar> inline DoubleSphereCameraCal<ToScalar> Cast() const

inline bool operator==(const DoubleSphereCameraCal &rhs) const

Public Static Functions

static inline constexpr int32_t StorageDim()

static inline DoubleSphereCameraCal FromStorage(const Scalar *const vec)

template<typename ScalarType> class EquirectangularCameraCal

#include <equirectangular_camera_cal.h>

Autogenerated C++ implementation of symforce.cam.equirectangular_camera_cal.EquirectangularCameraCal.

Equirectangular camera model with parameters [fx, fy, cx, cy].

(fx, fy) representing focal length; (cx, cy) representing principal point.

Public Types

using Scalar = ScalarType

using Self = EquirectangularCameraCal<Scalar>

using DataVec = Eigen::Matrix<Scalar, 4, 1>

Public Functions

inline EquirectangularCameraCal(const Eigen::Matrix<Scalar, 2, 1> &focal_length, const Eigen::Matrix<Scalar, 2, 1> &principal_point)

inline explicit EquirectangularCameraCal(const DataVec &data, bool normalize = true)

Construct from data vec

Parameters:: normalize – Project to the manifold on construction. This ensures numerical stability as this constructor is called after each codegen operation. Constructing from a normalized vector may be faster, e.g. with FromStorage.

inline const DataVec &Data() const

inline void ToStorage(Scalar *const vec) const

Eigen::Matrix<Scalar, 2, 1> FocalLength() const: Return the focal length.

Eigen::Matrix<Scalar, 2, 1> PrincipalPoint() const: Return the principal point.

Eigen::Matrix<Scalar, 2, 1> PixelFromCameraPoint(const Eigen::Matrix<Scalar, 3, 1> &point, const Scalar epsilon, Scalar *const is_valid = nullptr) const

Project a 3D point in the camera frame into 2D pixel coordinates.

Returns: pixel: (x, y) coordinate in pixels if valid is_valid: 1 if the operation is within bounds else 0

Eigen::Matrix<Scalar, 2, 1> PixelFromCameraPointWithJacobians(const Eigen::Matrix<Scalar, 3, 1> &point, const Scalar epsilon, Scalar *const is_valid = nullptr, Eigen::Matrix<Scalar, 2, 4> *const pixel_D_cal = nullptr, Eigen::Matrix<Scalar, 2, 3> *const pixel_D_point = nullptr) const

Project a 3D point in the camera frame into 2D pixel coordinates.

Eigen::Matrix<Scalar, 3, 1> CameraRayFromPixel(const Eigen::Matrix<Scalar, 2, 1> &pixel, const Scalar epsilon, Scalar *const is_valid = nullptr) const

Backproject a 2D pixel coordinate into a 3D ray in the camera frame.

Returns: camera_ray: The ray in the camera frame (NOT normalized) is_valid: 1 if the operation is within bounds else 0

Eigen::Matrix<Scalar, 3, 1> CameraRayFromPixelWithJacobians(const Eigen::Matrix<Scalar, 2, 1> &pixel, const Scalar epsilon, Scalar *const is_valid = nullptr, Eigen::Matrix<Scalar, 3, 4> *const point_D_cal = nullptr, Eigen::Matrix<Scalar, 3, 2> *const point_D_pixel = nullptr) const

Backproject a 2D pixel coordinate into a 3D ray in the camera frame.

inline bool IsApprox(const Self &b, const Scalar tol) const

template<typename ToScalar> inline EquirectangularCameraCal<ToScalar> Cast() const

inline bool operator==(const EquirectangularCameraCal &rhs) const

Public Static Functions

static inline constexpr int32_t StorageDim()

static inline EquirectangularCameraCal FromStorage(const Scalar *const vec)

template<typename T> struct GroupOps#

#include <group_ops.h>

C++ GroupOps concept, specialized per type. See symforce.ops.group_ops for details.

Public Types

using SelfJacobian = Eigen::Matrix<typename T::Scalar, LieGroupOps<T>::TangentDim(), LieGroupOps<T>::TangentDim()>#

Public Static Functions

static T Identity()#

static T Inverse(const T &a)#

static T Compose(const T &a, const T &b)#

static T Between(const T &a, const T &b)#

static T InverseWithJacobian(const T &a, SelfJacobian *const res_D_a)#

static T ComposeWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a, SelfJacobian *const res_D_b)#

static T BetweenWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a, SelfJacobian *const res_D_b)#

template<typename Scalar> struct GroupOps<ATANCameraCal<Scalar>>#

#include <group_ops.h>

C++ GroupOps implementation for <class ‘symforce.cam.atan_camera_cal.ATANCameraCal’>.

Public Types

using T = ATANCameraCal<Scalar>#

using SelfJacobian = Eigen::Matrix<Scalar, LieGroupOps<T>::TangentDim(), LieGroupOps<T>::TangentDim()>#

Public Static Functions

static T Identity()#

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

Symbolic function: <lambda>

Args:

Outputs: res: ATANCameraCal

static T Inverse(const T &a)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity

static T Compose(const T &a, const T &b)#

Composition of two elements in the group.

Returns: Element: a @ b

static T Between(const T &a, const T &b)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b

static T InverseWithJacobian(const T &a, SelfJacobian *const res_D_a = nullptr)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity res_D_a: (5x5) jacobian of res (5) wrt arg a (5)

static T ComposeWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Composition of two elements in the group.

Returns: Element: a @ b res_D_a: (5x5) jacobian of res (5) wrt arg a (5) res_D_b: (5x5) jacobian of res (5) wrt arg b (5)

static T BetweenWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b res_D_a: (5x5) jacobian of res (5) wrt arg a (5) res_D_b: (5x5) jacobian of res (5) wrt arg b (5)

template<> struct GroupOps<double> : public sym::scalar::GroupOps<double>#

template<typename Scalar> struct GroupOps<DoubleSphereCameraCal<Scalar>>#

#include <group_ops.h>

C++ GroupOps implementation for <class ‘symforce.cam.double_sphere_camera_cal.DoubleSphereCameraCal’>.

Public Types

using T = DoubleSphereCameraCal<Scalar>#

using SelfJacobian = Eigen::Matrix<Scalar, LieGroupOps<T>::TangentDim(), LieGroupOps<T>::TangentDim()>#

Public Static Functions

static T Identity()#

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

Symbolic function: <lambda>

Args:

Outputs: res: DoubleSphereCameraCal

static T Inverse(const T &a)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity

static T Compose(const T &a, const T &b)#

Composition of two elements in the group.

Returns: Element: a @ b

static T Between(const T &a, const T &b)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b

static T InverseWithJacobian(const T &a, SelfJacobian *const res_D_a = nullptr)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity res_D_a: (6x6) jacobian of res (6) wrt arg a (6)

static T ComposeWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Composition of two elements in the group.

Returns: Element: a @ b res_D_a: (6x6) jacobian of res (6) wrt arg a (6) res_D_b: (6x6) jacobian of res (6) wrt arg b (6)

static T BetweenWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b res_D_a: (6x6) jacobian of res (6) wrt arg a (6) res_D_b: (6x6) jacobian of res (6) wrt arg b (6)

template<typename ScalarType, int Rows, int Cols> Matrix< ScalarType, Rows, Cols > >

#include <group_ops.h>

C++ GroupOps implementation for matrices.

Public Types

using Scalar = ScalarType#

using T = Eigen::Matrix<Scalar, Rows, Cols>#

Public Static Functions

static inline T Identity()#

static inline T Inverse(const T &a)#

static inline T Compose(const T &a, const T &b)#

static inline T Between(const T &a, const T &b)#

template<typename Scalar> struct GroupOps<EquirectangularCameraCal<Scalar>>#

#include <group_ops.h>

C++ GroupOps implementation for <class ‘symforce.cam.equirectangular_camera_cal.EquirectangularCameraCal’>.

Public Types

using T = EquirectangularCameraCal<Scalar>#

using SelfJacobian = Eigen::Matrix<Scalar, LieGroupOps<T>::TangentDim(), LieGroupOps<T>::TangentDim()>#

Public Static Functions

static T Identity()#

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

Symbolic function: <lambda>

Args:

Outputs: res: EquirectangularCameraCal

static T Inverse(const T &a)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity

static T Compose(const T &a, const T &b)#

Composition of two elements in the group.

Returns: Element: a @ b

static T Between(const T &a, const T &b)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b

static T InverseWithJacobian(const T &a, SelfJacobian *const res_D_a = nullptr)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity res_D_a: (4x4) jacobian of res (4) wrt arg a (4)

static T ComposeWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Composition of two elements in the group.

Returns: Element: a @ b res_D_a: (4x4) jacobian of res (4) wrt arg a (4) res_D_b: (4x4) jacobian of res (4) wrt arg b (4)

static T BetweenWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b res_D_a: (4x4) jacobian of res (4) wrt arg a (4) res_D_b: (4x4) jacobian of res (4) wrt arg b (4)

template<> struct GroupOps<float> : public sym::scalar::GroupOps<float>#

template<typename Scalar> struct GroupOps<LinearCameraCal<Scalar>>#

#include <group_ops.h>

C++ GroupOps implementation for <class ‘symforce.cam.linear_camera_cal.LinearCameraCal’>.

Public Types

using T = LinearCameraCal<Scalar>#

using SelfJacobian = Eigen::Matrix<Scalar, LieGroupOps<T>::TangentDim(), LieGroupOps<T>::TangentDim()>#

Public Static Functions

static T Identity()#

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

Symbolic function: <lambda>

Args:

Outputs: res: LinearCameraCal

static T Inverse(const T &a)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity

static T Compose(const T &a, const T &b)#

Composition of two elements in the group.

Returns: Element: a @ b

static T Between(const T &a, const T &b)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b

static T InverseWithJacobian(const T &a, SelfJacobian *const res_D_a = nullptr)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity res_D_a: (4x4) jacobian of res (4) wrt arg a (4)

static T ComposeWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Composition of two elements in the group.

Returns: Element: a @ b res_D_a: (4x4) jacobian of res (4) wrt arg a (4) res_D_b: (4x4) jacobian of res (4) wrt arg b (4)

static T BetweenWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b res_D_a: (4x4) jacobian of res (4) wrt arg a (4) res_D_b: (4x4) jacobian of res (4) wrt arg b (4)

template<typename Scalar> struct GroupOps<PolynomialCameraCal<Scalar>>#

#include <group_ops.h>

C++ GroupOps implementation for <class ‘symforce.cam.polynomial_camera_cal.PolynomialCameraCal’>.

Public Types

using T = PolynomialCameraCal<Scalar>#

using SelfJacobian = Eigen::Matrix<Scalar, LieGroupOps<T>::TangentDim(), LieGroupOps<T>::TangentDim()>#

Public Static Functions

static T Identity()#

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

Symbolic function: <lambda>

Args:

Outputs: res: PolynomialCameraCal

static T Inverse(const T &a)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity

static T Compose(const T &a, const T &b)#

Composition of two elements in the group.

Returns: Element: a @ b

static T Between(const T &a, const T &b)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b

static T InverseWithJacobian(const T &a, SelfJacobian *const res_D_a = nullptr)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity res_D_a: (8x8) jacobian of res (8) wrt arg a (8)

static T ComposeWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Composition of two elements in the group.

Returns: Element: a @ b res_D_a: (8x8) jacobian of res (8) wrt arg a (8) res_D_b: (8x8) jacobian of res (8) wrt arg b (8)

static T BetweenWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b res_D_a: (8x8) jacobian of res (8) wrt arg a (8) res_D_b: (8x8) jacobian of res (8) wrt arg b (8)

template<typename Scalar> struct GroupOps<Pose2<Scalar>>#

#include <group_ops.h>

C++ GroupOps implementation for <class ‘symforce.geo.pose2.Pose2’>.

Public Types

using T = Pose2<Scalar>#

using SelfJacobian = Eigen::Matrix<Scalar, LieGroupOps<T>::TangentDim(), LieGroupOps<T>::TangentDim()>#

Public Static Functions

static T Identity()#

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

Symbolic function: <lambda>

Args:

Outputs: res: Pose2

static T Inverse(const T &a)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity

static T Compose(const T &a, const T &b)#

Composition of two elements in the group.

Returns: Element: a @ b

static T Between(const T &a, const T &b)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b

static T InverseWithJacobian(const T &a, SelfJacobian *const res_D_a = nullptr)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity res_D_a: (3x3) jacobian of res (3) wrt arg a (3)

static T ComposeWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Composition of two elements in the group.

Returns: Element: a @ b res_D_a: (3x3) jacobian of res (3) wrt arg a (3) res_D_b: (3x3) jacobian of res (3) wrt arg b (3)

static T BetweenWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b res_D_a: (3x3) jacobian of res (3) wrt arg a (3) res_D_b: (3x3) jacobian of res (3) wrt arg b (3)

template<typename Scalar> struct GroupOps<Pose3<Scalar>>#

#include <group_ops.h>

C++ GroupOps implementation for <class ‘symforce.geo.pose3.Pose3’>.

Public Types

using T = Pose3<Scalar>#

using SelfJacobian = Eigen::Matrix<Scalar, LieGroupOps<T>::TangentDim(), LieGroupOps<T>::TangentDim()>#

Public Static Functions

static T Identity()#

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

Symbolic function: <lambda>

Args:

Outputs: res: Pose3

static T Inverse(const T &a)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity

static T Compose(const T &a, const T &b)#

Composition of two elements in the group.

Returns: Element: a @ b

static T Between(const T &a, const T &b)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b

static T InverseWithJacobian(const T &a, SelfJacobian *const res_D_a = nullptr)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity res_D_a: (6x6) jacobian of res (6) wrt arg a (6)

static T ComposeWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Composition of two elements in the group.

Returns: Element: a @ b res_D_a: (6x6) jacobian of res (6) wrt arg a (6) res_D_b: (6x6) jacobian of res (6) wrt arg b (6)

static T BetweenWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b res_D_a: (6x6) jacobian of res (6) wrt arg a (6) res_D_b: (6x6) jacobian of res (6) wrt arg b (6)

template<typename Scalar> struct GroupOps<Rot2<Scalar>>#

#include <group_ops.h>

C++ GroupOps implementation for <class ‘symforce.geo.rot2.Rot2’>.

Public Types

using T = Rot2<Scalar>#

using SelfJacobian = Eigen::Matrix<Scalar, LieGroupOps<T>::TangentDim(), LieGroupOps<T>::TangentDim()>#

Public Static Functions

static T Identity()#

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

Symbolic function: <lambda>

Args:

Outputs: res: Rot2

static T Inverse(const T &a)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity

static T Compose(const T &a, const T &b)#

Composition of two elements in the group.

Returns: Element: a @ b

static T Between(const T &a, const T &b)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b

static T InverseWithJacobian(const T &a, SelfJacobian *const res_D_a = nullptr)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity res_D_a: (1x1) jacobian of res (1) wrt arg a (1)

static T ComposeWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Composition of two elements in the group.

Returns: Element: a @ b res_D_a: (1x1) jacobian of res (1) wrt arg a (1) res_D_b: (1x1) jacobian of res (1) wrt arg b (1)

static T BetweenWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b res_D_a: (1x1) jacobian of res (1) wrt arg a (1) res_D_b: (1x1) jacobian of res (1) wrt arg b (1)

template<typename Scalar> struct GroupOps<Rot3<Scalar>>#

#include <group_ops.h>

C++ GroupOps implementation for <class ‘symforce.geo.rot3.Rot3’>.

Public Types

using T = Rot3<Scalar>#

using SelfJacobian = Eigen::Matrix<Scalar, LieGroupOps<T>::TangentDim(), LieGroupOps<T>::TangentDim()>#

Public Static Functions

static T Identity()#

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

Symbolic function: <lambda>

Args:

Outputs: res: Rot3

static T Inverse(const T &a)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity

static T Compose(const T &a, const T &b)#

Composition of two elements in the group.

Returns: Element: a @ b

static T Between(const T &a, const T &b)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b

static T InverseWithJacobian(const T &a, SelfJacobian *const res_D_a = nullptr)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity res_D_a: (3x3) jacobian of res (3) wrt arg a (3)

static T ComposeWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Composition of two elements in the group.

Returns: Element: a @ b res_D_a: (3x3) jacobian of res (3) wrt arg a (3) res_D_b: (3x3) jacobian of res (3) wrt arg b (3)

static T BetweenWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b res_D_a: (3x3) jacobian of res (3) wrt arg a (3) res_D_b: (3x3) jacobian of res (3) wrt arg b (3)

template<typename Scalar> struct GroupOps<SphericalCameraCal<Scalar>>#

#include <group_ops.h>

C++ GroupOps implementation for <class ‘symforce.cam.spherical_camera_cal.SphericalCameraCal’>.

Public Types

using T = SphericalCameraCal<Scalar>#

using SelfJacobian = Eigen::Matrix<Scalar, LieGroupOps<T>::TangentDim(), LieGroupOps<T>::TangentDim()>#

Public Static Functions

static T Identity()#

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

Symbolic function: <lambda>

Args:

Outputs: res: SphericalCameraCal

static T Inverse(const T &a)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity

static T Compose(const T &a, const T &b)#

Composition of two elements in the group.

Returns: Element: a @ b

static T Between(const T &a, const T &b)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b

static T InverseWithJacobian(const T &a, SelfJacobian *const res_D_a = nullptr)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity res_D_a: (11x11) jacobian of res (11) wrt arg a (11)

static T ComposeWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Composition of two elements in the group.

Returns: Element: a @ b res_D_a: (11x11) jacobian of res (11) wrt arg a (11) res_D_b: (11x11) jacobian of res (11) wrt arg b (11)

static T BetweenWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b res_D_a: (11x11) jacobian of res (11) wrt arg a (11) res_D_b: (11x11) jacobian of res (11) wrt arg b (11)

template<typename Scalar> struct GroupOps<Unit3<Scalar>>#

#include <group_ops.h>

C++ GroupOps implementation for <class ‘symforce.geo.unit3.Unit3’>.

Public Types

using T = Unit3<Scalar>#

using SelfJacobian = Eigen::Matrix<Scalar, LieGroupOps<T>::TangentDim(), LieGroupOps<T>::TangentDim()>#

Public Static Functions

static T Identity()#

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

Symbolic function: <lambda>

Args:

Outputs: res: Unit3

static T Inverse(const T &a)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity

static T Compose(const T &a, const T &b)#

Composition of two elements in the group.

Returns: Element: a @ b

static T Between(const T &a, const T &b)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b

static T InverseWithJacobian(const T &a, SelfJacobian *const res_D_a = nullptr)#

Inverse of the element a.

Returns: Element: b such that a @ b = identity res_D_a: (2x2) jacobian of res (2) wrt arg a (2)

static T ComposeWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Composition of two elements in the group.

Returns: Element: a @ b res_D_a: (2x2) jacobian of res (2) wrt arg a (2) res_D_b: (2x2) jacobian of res (2) wrt arg b (2)

static T BetweenWithJacobians(const T &a, const T &b, SelfJacobian *const res_D_a = nullptr, SelfJacobian *const res_D_b = nullptr)#

Returns the element that when composed with a produces b. For vector spaces it is b - a.

Implementation is simply compose(inverse(a), b).

Returns: Element: c such that a @ c = b res_D_a: (2x2) jacobian of res (2) wrt arg a (2) res_D_b: (2x2) jacobian of res (2) wrt arg b (2)

template<typename T> struct LieGroupOps#

#include <lie_group_ops.h>

C++ LieGroupOps concept, specialized per type. See symforce.ops.lie_group_ops for details.

Public Types

using Scalar = typename T::Scalar#

using TangentVec = Eigen::Matrix<Scalar, TangentDim(), 1>#

Public Static Functions

static constexpr int32_t TangentDim()#

static T FromTangent(const TangentVec &vec, const Scalar epsilon)#

static TangentVec ToTangent(const T &a, const Scalar epsilon)#

static T Retract(const T &a, const TangentVec &vec, const Scalar epsilon)#

static TangentVec LocalCoordinates(const T &a, const T &b, const Scalar epsilon)#

static T Interpolate(const T &a, const T &b, const Scalar alpha, const Scalar epsilon)#

static bool IsClose(const T &a, const T &b, const Scalar epsilon, const Scalar tol)#

template<typename Scalar> struct LieGroupOps<ATANCameraCal<Scalar>> : public sym::internal::LieGroupOpsBase<ATANCameraCal<Scalar>, Scalar>#

#include <lie_group_ops.h>

C++ LieGroupOps implementation for <class ‘symforce.cam.atan_camera_cal.ATANCameraCal’>.

Public Types

using T = ATANCameraCal<Scalar>#

using TangentVec = Eigen::Matrix<Scalar, TangentDim(), 1>#

Public Static Functions

static inline constexpr int32_t TangentDim()#

static T FromTangent(const TangentVec &vec, const Scalar epsilon)#

static TangentVec ToTangent(const T &a, const Scalar epsilon)#

static T Retract(const T &a, const TangentVec &vec, const Scalar epsilon)#

static TangentVec LocalCoordinates(const T &a, const T &b, const Scalar epsilon)#

static T Interpolate(const T &a, const T &b, const Scalar alpha, const Scalar epsilon)#

template<> struct LieGroupOps<double> : public sym::scalar::LieGroupOps<double>#

template<typename Scalar> struct LieGroupOps<DoubleSphereCameraCal<Scalar>> : public sym::internal::LieGroupOpsBase<DoubleSphereCameraCal<Scalar>, Scalar>#

#include <lie_group_ops.h>

C++ LieGroupOps implementation for <class ‘symforce.cam.double_sphere_camera_cal.DoubleSphereCameraCal’>.

Public Types

using T = DoubleSphereCameraCal<Scalar>#

using TangentVec = Eigen::Matrix<Scalar, TangentDim(), 1>#

Public Static Functions

static inline constexpr int32_t TangentDim()#

static T FromTangent(const TangentVec &vec, const Scalar epsilon)#

static TangentVec ToTangent(const T &a, const Scalar epsilon)#

static T Retract(const T &a, const TangentVec &vec, const Scalar epsilon)#

static TangentVec LocalCoordinates(const T &a, const T &b, const Scalar epsilon)#

static T Interpolate(const T &a, const T &b, const Scalar alpha, const Scalar epsilon)#

template<typename ScalarType, int Rows, int Cols> Matrix< ScalarType, Rows, Cols > > : public sym::internal::LieGroupOpsBase< Eigen::Matrix< ScalarType, Rows, Cols >, ScalarType >

#include <lie_group_ops.h>

C++ LieGroupOps implementation for matrices.

Public Types

using Scalar = ScalarType

using T = Eigen::Matrix<Scalar, Rows, Cols>

using TangentVec = Eigen::Matrix<Scalar, TangentDim(), 1>#

Public Static Functions

static inline constexpr int32_t TangentDim()#

static inline T FromTangent(const TangentVec &vec, const Scalar epsilon)#

static inline TangentVec ToTangent(const T &a, const Scalar epsilon)#

static inline T Retract(const T &a, const TangentVec &vec, const Scalar epsilon)#

static inline TangentVec LocalCoordinates(const T &a, const T &b, const Scalar epsilon)#

static inline T Interpolate(const T &a, const T &b, const Scalar alpha, const Scalar epsilon)#

template<typename Scalar> struct LieGroupOps<EquirectangularCameraCal<Scalar>> : public sym::internal::LieGroupOpsBase<EquirectangularCameraCal<Scalar>, Scalar>#

#include <lie_group_ops.h>

C++ LieGroupOps implementation for <class ‘symforce.cam.equirectangular_camera_cal.EquirectangularCameraCal’>.

Public Types

using T = EquirectangularCameraCal<Scalar>#

using TangentVec = Eigen::Matrix<Scalar, TangentDim(), 1>#

Public Static Functions

static inline constexpr int32_t TangentDim()#

static T FromTangent(const TangentVec &vec, const Scalar epsilon)#

static TangentVec ToTangent(const T &a, const Scalar epsilon)#

static T Retract(const T &a, const TangentVec &vec, const Scalar epsilon)#

static TangentVec LocalCoordinates(const T &a, const T &b, const Scalar epsilon)#

static T Interpolate(const T &a, const T &b, const Scalar alpha, const Scalar epsilon)#

template<> struct LieGroupOps<float> : public sym::scalar::LieGroupOps<float>#

template<typename Scalar> struct LieGroupOps<LinearCameraCal<Scalar>> : public sym::internal::LieGroupOpsBase<LinearCameraCal<Scalar>, Scalar>#

#include <lie_group_ops.h>

C++ LieGroupOps implementation for <class ‘symforce.cam.linear_camera_cal.LinearCameraCal’>.

Public Types

using T = LinearCameraCal<Scalar>#

using TangentVec = Eigen::Matrix<Scalar, TangentDim(), 1>#

Public Static Functions

static inline constexpr int32_t TangentDim()#

static T FromTangent(const TangentVec &vec, const Scalar epsilon)#

static TangentVec ToTangent(const T &a, const Scalar epsilon)#

static T Retract(const T &a, const TangentVec &vec, const Scalar epsilon)#

static TangentVec LocalCoordinates(const T &a, const T &b, const Scalar epsilon)#

static T Interpolate(const T &a, const T &b, const Scalar alpha, const Scalar epsilon)#

template<typename Scalar> struct LieGroupOps<PolynomialCameraCal<Scalar>> : public sym::internal::LieGroupOpsBase<PolynomialCameraCal<Scalar>, Scalar>#

#include <lie_group_ops.h>

C++ LieGroupOps implementation for <class ‘symforce.cam.polynomial_camera_cal.PolynomialCameraCal’>.

Public Types

using T = PolynomialCameraCal<Scalar>#

using TangentVec = Eigen::Matrix<Scalar, TangentDim(), 1>#

Public Static Functions

static inline constexpr int32_t TangentDim()#

static T FromTangent(const TangentVec &vec, const Scalar epsilon)#

static TangentVec ToTangent(const T &a, const Scalar epsilon)#

static T Retract(const T &a, const TangentVec &vec, const Scalar epsilon)#

static TangentVec LocalCoordinates(const T &a, const T &b, const Scalar epsilon)#

static T Interpolate(const T &a, const T &b, const Scalar alpha, const Scalar epsilon)#

template<typename Scalar> struct LieGroupOps<Pose2<Scalar>> : public sym::internal::LieGroupOpsBase<Pose2<Scalar>, Scalar>#

#include <lie_group_ops.h>

C++ LieGroupOps implementation for <class ‘symforce.geo.pose2.Pose2’>.

Public Types

using T = Pose2<Scalar>#

using TangentVec = Eigen::Matrix<Scalar, TangentDim(), 1>#

Public Static Functions

static inline constexpr int32_t TangentDim()#

static T FromTangent(const TangentVec &vec, const Scalar epsilon)#

static TangentVec ToTangent(const T &a, const Scalar epsilon)#

static T Retract(const T &a, const TangentVec &vec, const Scalar epsilon)#

static TangentVec LocalCoordinates(const T &a, const T &b, const Scalar epsilon)#

static T Interpolate(const T &a, const T &b, const Scalar alpha, const Scalar epsilon)#

template<typename Scalar> struct LieGroupOps<Pose3<Scalar>> : public sym::internal::LieGroupOpsBase<Pose3<Scalar>, Scalar>#

#include <lie_group_ops.h>

C++ LieGroupOps implementation for <class ‘symforce.geo.pose3.Pose3’>.

Public Types

using T = Pose3<Scalar>#

using TangentVec = Eigen::Matrix<Scalar, TangentDim(), 1>#

Public Static Functions

static inline constexpr int32_t TangentDim()#

static T FromTangent(const TangentVec &vec, const Scalar epsilon)#

static TangentVec ToTangent(const T &a, const Scalar epsilon)#

static T Retract(const T &a, const TangentVec &vec, const Scalar epsilon)#

static TangentVec LocalCoordinates(const T &a, const T &b, const Scalar epsilon)#

static T Interpolate(const T &a, const T &b, const Scalar alpha, const Scalar epsilon)#

template<typename Scalar> struct LieGroupOps<Rot2<Scalar>> : public sym::internal::LieGroupOpsBase<Rot2<Scalar>, Scalar>#

#include <lie_group_ops.h>

C++ LieGroupOps implementation for <class ‘symforce.geo.rot2.Rot2’>.

Public Types

using T = Rot2<Scalar>#

using TangentVec = Eigen::Matrix<Scalar, TangentDim(), 1>#

Public Static Functions

static inline constexpr int32_t TangentDim()#

static T FromTangent(const TangentVec &vec, const Scalar epsilon)#

static TangentVec ToTangent(const T &a, const Scalar epsilon)#

static T Retract(const T &a, const TangentVec &vec, const Scalar epsilon)#

static TangentVec LocalCoordinates(const T &a, const T &b, const Scalar epsilon)#

static T Interpolate(const T &a, const T &b, const Scalar alpha, const Scalar epsilon)#

template<typename Scalar> struct LieGroupOps<Rot3<Scalar>> : public sym::internal::LieGroupOpsBase<Rot3<Scalar>, Scalar>#

#include <lie_group_ops.h>

C++ LieGroupOps implementation for <class ‘symforce.geo.rot3.Rot3’>.

Public Types

using T = Rot3<Scalar>#

using TangentVec = Eigen::Matrix<Scalar, TangentDim(), 1>#

Public Static Functions

static inline constexpr int32_t TangentDim()#

static T FromTangent(const TangentVec &vec, const Scalar epsilon)#

static TangentVec ToTangent(const T &a, const Scalar epsilon)#

static T Retract(const T &a, const TangentVec &vec, const Scalar epsilon)#

static TangentVec LocalCoordinates(const T &a, const T &b, const Scalar epsilon)#

static T Interpolate(const T &a, const T &b, const Scalar alpha, const Scalar epsilon)#

template<typename Scalar> struct LieGroupOps<SphericalCameraCal<Scalar>> : public sym::internal::LieGroupOpsBase<SphericalCameraCal<Scalar>, Scalar>#

#include <lie_group_ops.h>

C++ LieGroupOps implementation for <class ‘symforce.cam.spherical_camera_cal.SphericalCameraCal’>.

Public Types

using T = SphericalCameraCal<Scalar>#

using TangentVec = Eigen::Matrix<Scalar, TangentDim(), 1>#

Public Static Functions

static inline constexpr int32_t TangentDim()#

static T FromTangent(const TangentVec &vec, const Scalar epsilon)#

static TangentVec ToTangent(const T &a, const Scalar epsilon)#

static T Retract(const T &a, const TangentVec &vec, const Scalar epsilon)#

static TangentVec LocalCoordinates(const T &a, const T &b, const Scalar epsilon)#

static T Interpolate(const T &a, const T &b, const Scalar alpha, const Scalar epsilon)#

template<typename Scalar> struct LieGroupOps<Unit3<Scalar>> : public sym::internal::LieGroupOpsBase<Unit3<Scalar>, Scalar>#

#include <lie_group_ops.h>

C++ LieGroupOps implementation for <class ‘symforce.geo.unit3.Unit3’>.

Public Types

using T = Unit3<Scalar>#

using TangentVec = Eigen::Matrix<Scalar, TangentDim(), 1>#

Public Static Functions

static inline constexpr int32_t TangentDim()#

static T FromTangent(const TangentVec &vec, const Scalar epsilon)#

static TangentVec ToTangent(const T &a, const Scalar epsilon)#

static T Retract(const T &a, const TangentVec &vec, const Scalar epsilon)#

static TangentVec LocalCoordinates(const T &a, const T &b, const Scalar epsilon)#

static T Interpolate(const T &a, const T &b, const Scalar alpha, const Scalar epsilon)#

template<typename ScalarType> class LinearCameraCal

#include <linear_camera_cal.h>

Autogenerated C++ implementation of symforce.cam.linear_camera_cal.LinearCameraCal.

Standard pinhole camera w/ four parameters [fx, fy, cx, cy].

(fx, fy) representing focal length; (cx, cy) representing principal point.

Public Types

using Scalar = ScalarType

using Self = LinearCameraCal<Scalar>

using DataVec = Eigen::Matrix<Scalar, 4, 1>

Public Functions

inline LinearCameraCal(const Eigen::Matrix<Scalar, 2, 1> &focal_length, const Eigen::Matrix<Scalar, 2, 1> &principal_point)

inline explicit LinearCameraCal(const DataVec &data, bool normalize = true)

Construct from data vec

Parameters:: normalize – Project to the manifold on construction. This ensures numerical stability as this constructor is called after each codegen operation. Constructing from a normalized vector may be faster, e.g. with FromStorage.

inline const DataVec &Data() const

inline void ToStorage(Scalar *const vec) const

Eigen::Matrix<Scalar, 2, 1> FocalLength() const: Return the focal length.

Eigen::Matrix<Scalar, 2, 1> PrincipalPoint() const: Return the principal point.