12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'develop' into fix-docs-params-inherits

release/4.3a0
Porter Zach 2025-04-23 16:00:04 -04:00 committed by GitHub
parent 453491b78d c9498fe0c0
commit 6bf2f55952
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
27 changed files with 2862 additions and 405 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
gtsam/geometry/Pose3.cpp
View File

@ -219,8 +219,7 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
const Vector3 w = xi.head<3>(), v = xi.tail<3>();
// Instantiate functor for Dexp-related operations:
const bool nearZero = (w.dot(w) <= 1e-5);
const so3::DexpFunctor local(w, nearZero);
const so3::DexpFunctor local(w);
// Compute rotation using Expmap
#ifdef GTSAM_USE_QUATERNIONS
@ -256,25 +255,17 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
/* ************************************************************************* */
Vector6 Pose3::Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose) {
if (Hpose) *Hpose = LogmapDerivative(pose);
const Vector3 w = Rot3::Logmap(pose.rotation());
const Vector3 T = pose.translation();
const double t = w.norm();
if (t < 1e-10) {
Vector6 log;
log << w, T;
return log;
} else {
const Matrix3 W = skewSymmetric(w / t);
// Formula from Agrawal06iros, equation (14)
// simplified with Mathematica, and multiplying in T to avoid matrix math
const double Tan = tan(0.5 * t);
const Vector3 WT = W * T;
const Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
Vector6 log;
log << w, u;
return log;
}
// Instantiate functor for Dexp-related operations:
const so3::DexpFunctor local(w);
const Vector3 t = pose.translation();
const Vector3 u = local.applyLeftJacobianInverse(t);
Vector6 xi;
xi << w, u;
if (Hpose) *Hpose = LogmapDerivative(xi);
return xi;
}
/* ************************************************************************* */
@ -309,25 +300,6 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
#endif
}
/* ************************************************************************* */
Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
double nearZeroThreshold) {
const auto w = xi.head<3>();
const auto v = xi.tail<3>();
// Instantiate functor for Dexp-related operations:
bool nearZero = (w.dot(w) <= nearZeroThreshold);
so3::DexpFunctor local(w, nearZero);
// Call applyLeftJacobian to get its Jacobian
Matrix3 H;
local.applyLeftJacobian(v, H);
// Multiply with R^T to account for the Pose3::Create Jacobian.
const Matrix3 R = local.expmap();
return R.transpose() * H;
}
/* ************************************************************************* */
Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
Matrix6 J;
@ -335,16 +307,35 @@ Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
return J;
}
/* ************************************************************************* */
Matrix6 Pose3::LogmapDerivative(const Vector6& xi) {
const Vector3 w = xi.head<3>();
Vector3 v = xi.segment<3>(3);
// Instantiate functor for Dexp-related operations:
const so3::DexpFunctor local(w);
// Call applyLeftJacobian to get its Jacobians
Matrix3 H_t_w;
local.applyLeftJacobian(v, H_t_w);
// Multiply with R^T to account for NavState::Create Jacobian.
const Matrix3 R = local.expmap();
const Matrix3 Qt = R.transpose() * H_t_w;
// Now compute the blocks of the LogmapDerivative Jacobian
const Matrix3 Jw = Rot3::LogmapDerivative(w);
const Matrix3 Qt2 = -Jw * Qt * Jw;
Matrix6 J;
J << Jw, Z_3x3, Qt2, Jw;
return J;
}
/* ************************************************************************* */
Matrix6 Pose3::LogmapDerivative(const Pose3& pose) {
const Vector6 xi = Logmap(pose);
const Vector3 w = xi.head<3>();
const Matrix3 Jw = Rot3::LogmapDerivative(w);
const Matrix3 Q = ComputeQforExpmapDerivative(xi);
const Matrix3 Q2 = -Jw*Q*Jw;
Matrix6 J;
J << Jw, Z_3x3, Q2, Jw;
return J;
return LogmapDerivative(xi);
}
/* ************************************************************************* */

17
gtsam/geometry/Pose3.h
View File

@ -209,7 +209,10 @@ public:
static Matrix6 ExpmapDerivative(const Vector6& xi);
/// Derivative of Logmap
static Matrix6 LogmapDerivative(const Pose3& xi);
static Matrix6 LogmapDerivative(const Vector6& xi);
/// Derivative of Logmap, Pose3 version. TODO(Frank): deprecate?
static Matrix6 LogmapDerivative(const Pose3& pose);
// Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
struct GTSAM_EXPORT ChartAtOrigin {
@ -217,18 +220,6 @@ public:
static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {});
};
/**
* Compute the 3x3 bottom-left block Q of SE3 Expmap right derivative matrix
* J_r(xi) = [J_(w) Z_3x3;
* Q_r J_(w)]
* where J_(w) is the SO3 Expmap right derivative.
* (see Chirikjian11book2, pg 44, eq 10.95.
* The closed-form formula is identical to formula 102 in Barfoot14tro where
* Q_l of the SE3 Expmap left derivative matrix is given.
*/
static Matrix3 ComputeQforExpmapDerivative(
const Vector6& xi, double nearZeroThreshold = 1e-5);
using LieGroup<Pose3, 6>::inverse; // version with derivative
/**

13
gtsam/geometry/Rot3.h
View File

@ -374,20 +374,13 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
using LieAlgebra = Matrix3;
/**
* Exponential map at identity - create a rotation from canonical coordinates
* Exponential map - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
*/
static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = {}) {
if(H) *H = Rot3::ExpmapDerivative(v);
#ifdef GTSAM_USE_QUATERNIONS
return traits<gtsam::Quaternion>::Expmap(v);
#else
return Rot3(traits<SO3>::Expmap(v));
#endif
}
static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = {});
/**
* Log map at identity - returns the canonical coordinates
* Log map - returns the canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ of this rotation
*/
static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = {});

6
gtsam/geometry/Rot3M.cpp
View File

@ -153,7 +153,11 @@ Point3 Rot3::rotate(const Point3& p,
}
/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Rot3 Rot3::Expmap(const Vector3& v, OptionalJacobian<3,3> H) {
return Rot3(SO3::Expmap(v, H));
}
/* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
return SO3::Logmap(R.rot_,H);
}

6
gtsam/geometry/Rot3Q.cpp
View File

@ -116,6 +116,12 @@ namespace gtsam {
return Point3(r.x(), r.y(), r.z());
}
/* ************************************************************************* */
Rot3 Rot3::Expmap(const Vector3& v, OptionalJacobian<3,3> H) {
if(H) *H = Rot3::ExpmapDerivative(v);
return traits<gtsam::Quaternion>::Expmap(v);
}
/* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
return traits<gtsam::Quaternion>::Logmap(R.quaternion_, H);

184
gtsam/geometry/SO3.cpp
View File

@ -42,6 +42,20 @@ static constexpr double one_180th = 1.0 / 180.0;
static constexpr double one_720th = 1.0 / 720.0;
static constexpr double one_1260th = 1.0 / 1260.0;
static constexpr double kPi_inv = 1.0 / M_PI;
static constexpr double kPi2 = M_PI * M_PI;
static constexpr double k1_Pi2 = 1.0 / kPi2;
static constexpr double kPi3 = M_PI * kPi2;
static constexpr double k1_Pi3 = 1.0 / kPi3;
static constexpr double k2_Pi3 = 2.0 * k1_Pi3;
static constexpr double k1_4Pi = 0.25 * kPi_inv; // 1/(4*pi)
// --- Thresholds ---
// Tolerance for near zero (theta^2)
static constexpr double kNearZeroThresholdSq = 1e-6;
// Tolerance for near pi (delta^2 = (pi - theta)^2)
static constexpr double kNearPiThresholdSq = 1e-6;
GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) {
Matrix99 H;
auto R = Q.matrix();
@ -58,10 +72,11 @@ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
return MR;
}
void ExpmapFunctor::init(bool nearZeroApprox) {
nearZero =
nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
void ExpmapFunctor::init(double nearZeroThresholdSq) {
nearZero = (theta2 <= nearZeroThresholdSq);
if (!nearZero) {
// General case: Use standard stable formulas for A and B
const double sin_theta = std::sin(theta);
A = sin_theta / theta;
const double s2 = std::sin(theta / 2.0);
@ -69,36 +84,51 @@ void ExpmapFunctor::init(bool nearZeroApprox) {
2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
B = one_minus_cos / theta2;
} else {
// Taylor expansion at 0
// Taylor expansion at 0 for A, B (Order theta^2)
A = 1.0 - theta2 * one_6th;
B = 0.5 - theta2 * one_24th;
}
}
ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
ExpmapFunctor::ExpmapFunctor(const Vector3& omega) :ExpmapFunctor(kNearZeroThresholdSq, omega) {}
ExpmapFunctor::ExpmapFunctor(double nearZeroThresholdSq, const Vector3& omega)
: theta2(omega.dot(omega)),
theta(std::sqrt(theta2)),
W(skewSymmetric(omega)),
WW(W * W) {
init(nearZeroApprox);
init(nearZeroThresholdSq);
}
ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
bool nearZeroApprox)
ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle)
: theta2(angle * angle),
theta(angle),
W(skewSymmetric(axis * angle)),
WW(W * W) {
init(nearZeroApprox);
init(kNearZeroThresholdSq);
}
Matrix3 ExpmapFunctor::expmap() const { return I_3x3 + A * W + B * WW; }
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
DexpFunctor::DexpFunctor(const Vector3& omega, double nearZeroThresholdSq, double nearPiThresholdSq)
: ExpmapFunctor(nearZeroThresholdSq, omega), omega(omega) {
if (!nearZero) {
C = (1 - A) / theta2;
D = (1.0 - A / (2.0 * B)) / theta2;
// General case or nearPi: Use standard stable formulas first
C = (1.0 - A) / theta2; // Usually stable, even near pi (1-0)/pi^2
// Calculate delta = pi - theta (non-negative) for nearPi check
const double delta = M_PI > theta ? M_PI - theta : 0.0;
const double delta2 = delta * delta;
const bool nearPi = (delta2 < nearPiThresholdSq);
if (nearPi) {
// Taylor expansion near pi *only for D* (Order delta)
D = k1_Pi2 + (k2_Pi3 - k1_4Pi) * delta; // D ~ 1/pi^2 + delta*(2/pi^3 - 1/(4*pi))
} else {
// General case D:
D = (1.0 - A / (2.0 * B)) / theta2;
}
// Calculate E and F using standard formulas (stable near pi)
E = (2.0 * B - A) / theta2;
F = (3.0 * C - B) / theta2;
} else {
@ -111,51 +141,46 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
}
}
Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const {
// Wv = omega x v
const Vector3 Wv = gtsam::cross(omega, v);
if (H) {
// Apply product rule to (B Wv)
// - E * omega.transpose() is 1x3 Jacobian of B with respect to omega
// - skewSymmetric(v) is 3x3 Jacobian of Wv = gtsam::cross(omega, v)
*H = - Wv * E * omega.transpose() - B * skewSymmetric(v);
}
return B * Wv;
DexpFunctor::DexpFunctor(const Vector3& omega)
: DexpFunctor(omega, kNearZeroThresholdSq, kNearPiThresholdSq) {}
Matrix3 DexpFunctor::rightJacobianInverse() const {
if (theta > M_PI) return rightJacobian().inverse();
return I_3x3 + 0.5 * W + D * WW;
}
Vector3 DexpFunctor::doubleCrossC(const Vector3& v,
OptionalJacobian<3, 3> H) const {
// WWv = omega x (omega x * v)
Matrix3 doubleCrossJacobian;
const Vector3 WWv =
gtsam::doubleCross(omega, v, H ? &doubleCrossJacobian : nullptr);
if (H) {
// Apply product rule to (C WWv)
// - F * omega.transpose() is 1x3 Jacobian of C with respect to omega
// doubleCrossJacobian is 3x3 Jacobian of WWv = gtsam::doubleCross(omega, v)
*H = - WWv * F * omega.transpose() + C * doubleCrossJacobian;
}
return C * WWv;
Matrix3 DexpFunctor::leftJacobianInverse() const {
if (theta > M_PI) return leftJacobian().inverse();
return I_3x3 - 0.5 * W + D * WW;
}
// Multiplies v with left Jacobian through vector operations only.
Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
Matrix3 D_BWv_w, D_CWWv_w;
const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr);
const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr);
if (H1) *H1 = -D_BWv_w + D_CWWv_w;
Vector3 DexpFunctor::applyRightJacobian(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
const Vector3 Wv = gtsam::cross(omega, v);
Matrix3 WWv_H_w;
const Vector3 WWv = gtsam::doubleCross(omega, v, H1 ? &WWv_H_w : nullptr);
if (H1) {
// - E * omega.transpose() is 1x3 Jacobian of B with respect to omega
Matrix3 BWv_H_w = -Wv * E * omega.transpose() - B * skewSymmetric(v);
// - F * omega.transpose() is 1x3 Jacobian of C with respect to omega
Matrix3 CWWv_H_w = -WWv * F * omega.transpose() + C * WWv_H_w;
*H1 = -BWv_H_w + CWWv_H_w;
}
if (H2) *H2 = rightJacobian();
return v - BWv + CWWv;
return v - B * Wv + C * WWv;
}
Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
Vector3 DexpFunctor::applyRightJacobianInverse(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
const Matrix3 invJr = rightJacobianInverse();
const Vector3 c = invJr * v;
if (H1) {
Matrix3 H;
applyDexp(c, H); // get derivative H of forward mapping
applyRightJacobian(c, H); // get derivative H of forward mapping
*H1 = -invJr * H;
}
if (H2) *H2 = invJr;
@ -163,19 +188,26 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
}
Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
Matrix3 D_BWv_w, D_CWWv_w;
const Vector3 BWv = crossB(v, H1 ? &D_BWv_w : nullptr);
const Vector3 CWWv = doubleCrossC(v, H1 ? &D_CWWv_w : nullptr);
if (H1) *H1 = D_BWv_w + D_CWWv_w;
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
const Vector3 Wv = gtsam::cross(omega, v);
Matrix3 WWv_H_w;
const Vector3 WWv = gtsam::doubleCross(omega, v, H1 ? &WWv_H_w : nullptr);
if (H1) {
// - E * omega.transpose() is 1x3 Jacobian of B with respect to omega
Matrix3 BWv_H_w = -Wv * E * omega.transpose() - B * skewSymmetric(v);
// - F * omega.transpose() is 1x3 Jacobian of C with respect to omega
Matrix3 CWWv_H_w = -WWv * F * omega.transpose() + C * WWv_H_w;
*H1 = BWv_H_w + CWWv_H_w;
}
if (H2) *H2 = leftJacobian();
return v + BWv + CWWv;
return v + B * Wv + C * WWv;
}
Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
const Matrix3 invJl = leftJacobianInverse();
const Vector3 c = invJl * v;
if (H1) {
@ -250,50 +282,24 @@ Matrix3 SO3::AdjointMap() const {
template <>
GTSAM_EXPORT
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
if (H) {
so3::DexpFunctor local(omega);
*H = local.dexp();
return SO3(local.expmap());
} else {
return SO3(so3::ExpmapFunctor(omega).expmap());
}
so3::DexpFunctor local(omega);
if (H) *H = local.rightJacobian();
return SO3(local.expmap());
}
template <>
GTSAM_EXPORT
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
return so3::DexpFunctor(omega).dexp();
return so3::DexpFunctor(omega).rightJacobian();
}
//******************************************************************************
/* Right Jacobian for Log map in SO(3) - equation (10.86) and following
equations in G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie
Groups", Volume 2, 2008.
logmap( Rhat * expmap(omega) ) \approx logmap(Rhat) + Jrinv * omega
where Jrinv = LogmapDerivative(omega). This maps a perturbation on the
manifold (expmap(omega)) to a perturbation in the tangent space (Jrinv *
omega)
*/
template <>
GTSAM_EXPORT
Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
using std::cos;
using std::sin;
double theta2 = omega.dot(omega);
if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
double theta = std::sqrt(theta2); // rotation angle
// element of Lie algebra so(3): W = omega^
const Matrix3 W = Hat(omega);
return I_3x3 + 0.5 * W +
(1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) *
W * W;
return so3::DexpFunctor(omega).rightJacobianInverse();
}
//******************************************************************************
template <>
GTSAM_EXPORT
Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
@ -306,9 +312,7 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
const double &R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
const double &R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
// Get trace(R)
const double tr = R.trace();
Vector3 omega;
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
@ -357,7 +361,7 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
} else {
double magnitude;
const double tr_3 = tr - 3.0; // could be non-negative if the matrix is off orthogonal
if (tr_3 < -1e-6) {
if (tr_3 < -so3::kNearZeroThresholdSq) {
// this is the normal case -1 < trace < 3
double theta = acos((tr - 1.0) / 2.0);
magnitude = theta / (2.0 * sin(theta));
@ -365,7 +369,7 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
// use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
// see https://github.com/borglab/gtsam/issues/746 for details
magnitude = 0.5 - tr_3 / 12.0 + tr_3*tr_3/60.0;
magnitude = 0.5 - tr_3 * so3::one_12th + tr_3 * tr_3 * so3::one_60th;
}
omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
}

63
gtsam/geometry/SO3.h
View File

@ -137,23 +137,26 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
struct GTSAM_EXPORT ExpmapFunctor {
const double theta2, theta;
const Matrix3 W, WW;
bool nearZero;
bool nearZero{ false };
// Ethan Eade's constants:
double A; // A = sin(theta) / theta
double B; // B = (1 - cos(theta))
/// Constructor with element of Lie algebra so(3)
explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
explicit ExpmapFunctor(const Vector3& omega);
/// Constructor with threshold (advanced)
ExpmapFunctor(double nearZeroThresholdSq, const Vector3& axis);
/// Constructor with axis-angle
ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
ExpmapFunctor(const Vector3& axis, double angle);
/// Rodrigues formula
Matrix3 expmap() const;
protected:
void init(bool nearZeroApprox = false);
protected:
void init(double nearZeroThresholdSq);
};
/// Functor that implements Exponential map *and* its derivatives
@ -173,7 +176,10 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
double F; // (3C - B) / theta2
/// Constructor with element of Lie algebra so(3)
explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
explicit DexpFunctor(const Vector3& omega);
/// Constructor with custom thresholds (advanced)
explicit DexpFunctor(const Vector3& omega, double nearZeroThresholdSq, double nearPiThresholdSq);
// NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
// (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
@ -186,40 +192,37 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
// Compute the left Jacobian for Exponential map in SO(3)
Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; }
/// Differential of expmap == right Jacobian
inline Matrix3 dexp() const { return rightJacobian(); }
/// Inverse of right Jacobian
Matrix3 rightJacobianInverse() const { return I_3x3 + 0.5 * W + D * WW; }
/// For |omega|>pi uses rightJacobian().inverse(), as unstable beyond pi!
Matrix3 rightJacobianInverse() const;
// Inverse of left Jacobian
Matrix3 leftJacobianInverse() const { return I_3x3 - 0.5 * W + D * WW; }
/// For |omega|>pi uses leftJacobian().inverse(), as unstable beyond pi!
Matrix3 leftJacobianInverse() const;
/// Synonym for rightJacobianInverse
inline Matrix3 invDexp() const { return rightJacobianInverse(); }
/// Multiplies with rightJacobian(), with optional derivatives
Vector3 applyRightJacobian(const Vector3& v,
OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const;
/// Computes B * (omega x v).
Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
/// Computes C * (omega x (omega x v)).
Vector3 doubleCrossC(const Vector3& v, OptionalJacobian<3, 3> H = {}) const;
/// Multiplies with dexp(), with optional derivatives
Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
/// Multiplies with dexp().inverse(), with optional derivatives
Vector3 applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
/// Multiplies with rightJacobian().inverse(), with optional derivatives
Vector3 applyRightJacobianInverse(const Vector3& v,
OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const;
/// Multiplies with leftJacobian(), with optional derivatives
Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
Vector3 applyLeftJacobian(const Vector3& v,
OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const;
/// Multiplies with leftJacobianInverse(), with optional derivatives
Vector3 applyLeftJacobianInverse(const Vector3& v,
OptionalJacobian<3, 3> H1 = {},
OptionalJacobian<3, 3> H2 = {}) const;
OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) const;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
/// @deprecated: use rightJacobian
inline Matrix3 dexp() const { return rightJacobian(); }
/// @deprecated: use rightJacobianInverse
inline Matrix3 invDexp() const { return rightJacobianInverse(); }
#endif
};
} // namespace so3

95
gtsam/geometry/doc/Rot3.ipynb
View File

@ -1014,7 +1014,7 @@
"source": [
"### Lie group operations\n",
"\n",
"`Rot3` implements the Lie group operations for exponential mapping and log mapping. For more information on Lie groups and their use here, see [GTSAM concepts](https://gtsam.org/notes/GTSAM-Concepts.html)."
"`Rot3` implements the Lie group operations for exponential mapping and log mapping. For more information on Lie groups and their use here, see [GTSAM concepts](https://gtsam.org/notes/GTSAM-Concepts.html), and some detailed math in the [`SO3`](SO3.ipynb) class."
]
},
{
@ -1023,51 +1023,7 @@
"id": "MmBfK0ad1KZ6"
},
"source": [
"The exponential map for $\\text{SO}(3)$ converts a 3D rotation vector (Lie algebra element in $\\mathfrak{so}(3)$) into a rotation matrix (Lie group element in $\\text{SO}(3)$). This is used to map a rotation vector $\\boldsymbol{\\omega} \\in \\mathbb{R}^3$ to a rotation matrix $R \\in \\text{SO}(3)$.\n",
"\n",
"Given a rotation vector $\\boldsymbol{\\omega} = (\\omega_x, \\omega_y, \\omega_z)$, define:\n",
"\n",
"- The rotation axis as the unit vector:\n",
" \n",
" $$\n",
" \\hat{\\omega} = \\frac{\\boldsymbol{\\omega}}{\\theta}, \\quad \\text{where } \\theta = \\|\\boldsymbol{\\omega}\\|\n",
" $$\n",
"\n",
"- The skew-symmetric matrix $[\\boldsymbol{\\omega}]_\\times$:\n",
"\n",
" $$\n",
" [\\boldsymbol{\\omega}]_\\times =\n",
" \\begin{bmatrix}\n",
" 0 & -\\omega_z & \\omega_y \\\\\n",
" \\omega_z & 0 & -\\omega_x \\\\\n",
" -\\omega_y & \\omega_x & 0\n",
" \\end{bmatrix}\n",
" $$\n",
"\n",
"Using Rodrigues' formula, the exponential map is:\n",
"\n",
"$$\n",
"\\exp([\\boldsymbol{\\omega}]_\\times) = I + \\frac{\\sin\\theta}{\\theta} [\\boldsymbol{\\omega}]_\\times + \\frac{1 - \\cos\\theta}{\\theta^2} [\\boldsymbol{\\omega}]_\\times^2\n",
"$$\n",
"\n",
"which results in the rotation matrix:\n",
"\n",
"$$\n",
"R = I + \\frac{\\sin\\theta}{\\theta} [\\boldsymbol{\\omega}]_\\times + \\frac{1 - \\cos\\theta}{\\theta^2} [\\boldsymbol{\\omega}]_\\times^2\n",
"$$\n",
"\n",
"where:\n",
"- $ I $ is the $ 3 \\times 3 $ identity matrix.\n",
"- $ \\theta $ is the magnitude of the rotation vector (rotation angle).\n",
"- $ [\\boldsymbol{\\omega}]_\\times $ represents the skew-symmetric matrix, the infinitesimal rotation generator.\n",
"\n",
"For very small $ \\theta $, we use the Taylor series approximation:\n",
"\n",
"$$\n",
"R \\approx I + [\\boldsymbol{\\omega}]_\\times\n",
"$$\n",
"\n",
"since $ \\sin\\theta \\approx \\theta$ and $ 1 - \\cos\\theta \\approx \\frac{\\theta^2}{2} $."
"The exponential map for $\\text{SO}(3)$ converts a 3D rotation vector (corresponding to a Lie algebra element in $\\mathfrak{so}(3)$) into a rotation matrix (Lie group element in $\\text{SO}(3)$). I.e., we map a rotation vector $\\boldsymbol{\\omega} \\in \\mathbb{R}^3$ to a rotation matrix $R \\in \\text{SO}(3)$."
]
},
{
@ -1126,52 +1082,7 @@
"id": "Yk2nazsK6ixV"
},
"source": [
"The logarithm map for $ \\text{SO}(3) $ is the inverse of the exponential map It converts a rotation matrix $ R \\in SO(3) $ into a 3D rotation vector (a Lie algebra element in $ \\mathfrak{so}(3) $).\n",
"\n",
"Given a rotation matrix $ R $, the corresponding rotation vector $ \\boldsymbol{\\omega} \\in \\mathbb{R}^3 $ is computed as:\n",
"\n",
"$$\n",
"\\boldsymbol{\\omega} = \\theta \\hat{\\omega}\n",
"$$\n",
"\n",
"where:\n",
"- $ \\theta $ is the rotation angle:\n",
" \n",
" $$\n",
" \\theta = \\cos^{-1} \\left( \\frac{\\text{Tr}(R) - 1}{2} \\right)\n",
" $$\n",
"\n",
"- $ \\hat{\\omega} $ is the rotation axis, obtained from the skew-symmetric matrix $ [\\boldsymbol{\\omega}]_\\times $:\n",
"\n",
" $$\n",
" [\\boldsymbol{\\omega}]_\\times = \\frac{\\theta}{2 \\sin\\theta} (R - R^T)\n",
" $$\n",
"\n",
"- Extracting the components:\n",
"\n",
" $$\n",
" \\boldsymbol{\\omega} =\n",
" \\frac{\\theta}{2 \\sin\\theta} (R - R^T)^\\vee\n",
" $$\n",
"\n",
"For small $ \\theta $, we use the Taylor series approximation:\n",
"\n",
"$$\n",
"\\boldsymbol{\\omega} \\approx \\frac{1}{2} (R - R^T)^\\vee\n",
"$$\n",
"\n",
"In both cases, $ (R - R^T)^\\vee $ extracts the unique 3D vector from a skew-symmetric matrix:\n",
"\n",
"$$\n",
"(R - R^T)^\\vee=\n",
"\\begin{bmatrix}\n",
" R_{32} - R_{23} \\\\\n",
" R_{13} - R_{31} \\\\\n",
" R_{21} - R_{12}\n",
" \\end{bmatrix}\n",
"$$\n",
"\n",
"where $ R_{ij} $ are the elements of $ R $."
"The logarithm map for $ \\text{SO}(3) $ is the inverse of the exponential map It converts a rotation matrix $ R \\in SO(3) $ into a 3D rotation vector (corresponding to a Lie algebra element in $ \\mathfrak{so}(3) $)."
]
},
{

558
gtsam/geometry/doc/SO3.ipynb Normal file
View File

File diff suppressed because one or more lines are too long

66
gtsam/geometry/geometry.i
View File

@ -198,6 +198,39 @@ class Rot2 {
};
#include <gtsam/geometry/SO3.h>
namespace so3 {
class ExpmapFunctor {
const double theta2;
const double theta;
const Matrix3 W;
const Matrix3 WW;
const bool nearZero;
double A; // A = sin(theta) / theta
double B; // B = (1 - cos(theta))
ExpmapFunctor(const gtsam::Vector3& omega);
ExpmapFunctor(double nearZeroThresholdSq, const gtsam::Vector3& axis);
ExpmapFunctor(const gtsam::Vector3& axis, double angle);
gtsam::Matrix3 expmap() const;
};
virtual class DexpFunctor : gtsam::so3::ExpmapFunctor {
const gtsam::Vector3 omega;
const double C; // (1 - A) / theta^2
const double D; // (1 - A/2B) / theta2
DexpFunctor(const gtsam::Vector3& omega);
DexpFunctor(const gtsam::Vector3& omega, double nearZeroThresholdSq, double nearPiThresholdSq);
gtsam::Matrix3 rightJacobian() const;
gtsam::Matrix3 leftJacobian() const;
gtsam::Matrix3 rightJacobianInverse() const;
gtsam::Matrix3 leftJacobianInverse() const;
gtsam::Vector3 applyRightJacobian(const gtsam::Vector3& v) const;
gtsam::Vector3 applyRightJacobianInverse(const gtsam::Vector3& v) const;
gtsam::Vector3 applyLeftJacobian(const gtsam::Vector3& v) const;
gtsam::Vector3 applyLeftJacobianInverse(const gtsam::Vector3& v) const;
};
}
class SO3 {
// Standard Constructors
SO3();
@ -220,21 +253,22 @@ class SO3 {
gtsam::SO3 operator*(const gtsam::SO3& R) const;
// Lie Group
static gtsam::SO3 Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::SO3& p);
gtsam::SO3 expmap(gtsam::Vector v);
gtsam::Vector logmap(const gtsam::SO3& p);
static gtsam::Matrix Hat(const gtsam::Vector& xi);
static gtsam::Vector Vee(const gtsam::Matrix& xi);
static gtsam::SO3 Expmap(gtsam::Vector3 v);
static gtsam::Vector3 Logmap(const gtsam::SO3& p);
static gtsam::Matrix3 ExpmapDerivative(const gtsam::Vector3& omega);
static gtsam::Matrix3 LogmapDerivative(const gtsam::Vector3& omega);
gtsam::SO3 expmap(gtsam::Vector3 v);
gtsam::Vector3 logmap(const gtsam::SO3& p);
static gtsam::Matrix3 Hat(const gtsam::Vector3& xi);
static gtsam::Vector3 Vee(const gtsam::Matrix3& xi);
// Manifold
gtsam::SO3 retract(gtsam::Vector v) const;
gtsam::Vector localCoordinates(const gtsam::SO3& R) const;
static gtsam::SO3 Expmap(gtsam::Vector v);
gtsam::SO3 retract(gtsam::Vector3 v) const;
gtsam::Vector3 localCoordinates(const gtsam::SO3& R) const;
// Other methods
gtsam::Vector vec() const;
gtsam::Matrix matrix() const;
gtsam::Vector3 vec() const;
gtsam::Matrix3 matrix() const;
};
#include <gtsam/geometry/SO4.h>
@ -376,6 +410,8 @@ class Rot3 {
// Lie group
static gtsam::Rot3 Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::Rot3& R);
static gtsam::Matrix3 ExpmapDerivative(const gtsam::Vector3& omega);
static gtsam::Matrix3 LogmapDerivative(const gtsam::Vector3& omega);
gtsam::Rot3 expmap(const gtsam::Vector& v);
gtsam::Vector logmap(const gtsam::Rot3& p);
static gtsam::Matrix Hat(const gtsam::Vector& xi);
@ -532,12 +568,17 @@ class Pose3 {
// Lie Group
static gtsam::Pose3 Expmap(gtsam::Vector xi);
static gtsam::Vector Logmap(const gtsam::Pose3& p);
static gtsam::Matrix6 ExpmapDerivative(const gtsam::Vector6& xi);
static gtsam::Matrix6 LogmapDerivative(const gtsam::Vector6& xi);
static gtsam::Matrix6 LogmapDerivative(const gtsam::Pose3& xi);
static gtsam::Pose3 Expmap(gtsam::Vector xi, Eigen::Ref<Eigen::MatrixXd> Hxi);
static gtsam::Vector Logmap(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Hpose);
gtsam::Pose3 expmap(gtsam::Vector v);
gtsam::Pose3 expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2);
gtsam::Vector logmap(const gtsam::Pose3& p);
gtsam::Vector logmap(const gtsam::Pose3& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2);
gtsam::Matrix AdjointMap() const;
gtsam::Vector Adjoint(gtsam::Vector xi_b) const;
gtsam::Vector Adjoint(gtsam::Vector xi_b, Eigen::Ref<Eigen::MatrixXd> H_this,
@ -550,8 +591,7 @@ class Pose3 {
static gtsam::Matrix adjointMap_(gtsam::Vector xi);
static gtsam::Vector adjoint_(gtsam::Vector xi, gtsam::Vector y);
static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y);
static gtsam::Matrix ExpmapDerivative(gtsam::Vector xi);
static gtsam::Matrix LogmapDerivative(const gtsam::Pose3& xi);
static gtsam::Matrix Hat(const gtsam::Vector& xi);
static gtsam::Vector Vee(const gtsam::Matrix& X);

71
gtsam/geometry/tests/testPose3.cpp
View File

@ -891,14 +891,14 @@ TEST(Pose3, ExpmapDerivative) {
//******************************************************************************
namespace test_cases {
std::vector<Vector3> small{{0, 0, 0}, //
{1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //,
{1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}};
std::vector<Vector3> large{{0, 0, 0}, {1, 0, 0}, {0, 1, 0},
{0, 0, 1}, {.1, .2, .3}, {1, -2, 3}};
auto omegas = [](bool nearZero) { return nearZero ? small : large; };
std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1},
{.4, .3, .2}, {4, 5, 6}, {-10, -20, 30}};
static const std::vector<Vector3> small{ {0, 0, 0}, //
{1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //,
{1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4} };
static const std::vector<Vector3> large{ {0, 0, 0}, {1, 0, 0}, {0, 1, 0},
{0, 0, 1}, {.1, .2, .3}, {1, -2, 3} };
auto omegas = [](bool nearZero) -> const std::vector<Vector3>&{ return nearZero ? small : large; };
static const std::vector<Vector3> vs{ {1, 0, 0}, {0, 1, 0}, {0, 0, 1},
{.4, .3, .2}, {4, 5, 6}, {-10, -20, 30} };
} // namespace test_cases
//******************************************************************************
@ -936,7 +936,9 @@ TEST(Pose3, Logmap) {
TEST(Pose3, LogmapDerivatives) {
for (bool nearZero : {true, false}) {
for (const Vector3& w : test_cases::omegas(nearZero)) {
std::cout << "w: " << w.transpose() << std::endl;
for (Vector3 v : test_cases::vs) {
std::cout << "v: " << v.transpose() << std::endl;
const Vector6 xi = (Vector6() << w, v).finished();
Pose3 pose = Pose3::Expmap(xi);
const Matrix6 expectedH =
@ -956,6 +958,59 @@ TEST(Pose3, LogmapDerivatives) {
}
}
//******************************************************************************
TEST(Pose3, LogmapDerivative) {
// Copied from testSO3.cpp
const Rot3 R2((Matrix3() << // Near pi
-0.750767, -0.0285082, -0.659952,
-0.0102558, -0.998445, 0.0547974,
-0.660487, 0.0479084, 0.749307).finished());
const Rot3 R3((Matrix3() << // Near pi
-0.747473, -0.00190019, -0.664289,
-0.0385114, -0.99819, 0.0461892,
-0.663175, 0.060108, 0.746047).finished());
const Rot3 R4((Matrix3() << // Final pose in a drone experiment
0.324237, 0.902975, 0.281968,
-0.674322, 0.429668, -0.600562,
-0.663445, 0.00458662, 0.748211).finished());
// Now creates poses
const Pose3 T0; // Identity
const Vector6 xi(0.1, -0.1, 0.1, 0.1, -0.1, 0.1);
const Pose3 T1 = Pose3::Expmap(xi); // Small rotation
const Pose3 T2(R2, Point3(1, 2, 3));
const Pose3 T3(R3, Point3(1, 2, 3));
const Pose3 T4(R4, Point3(1, 2, 3));
size_t i = 0;
for (const Pose3& T : { T0, T1, T2, T3, T4 }) {
const bool nearPi = (i == 2 || i == 3); // Flag cases near pi
Matrix6 actualH; // H computed by Logmap(T, H) using LogmapDerivative(xi)
const Vector6 xi = Pose3::Logmap(T, actualH);
// 1. Check self-consistency of analytical derivative calculation:
// Does the H returned by Logmap match an independent calculation
// of J_r^{-1} using ExpmapDerivative with the computed xi?
Matrix6 J_r_inv = Pose3::ExpmapDerivative(xi).inverse(); // J_r^{-1}
EXPECT(assert_equal(J_r_inv, actualH)); // This test is crucial and should pass
// 2. Check analytical derivative against numerical derivative:
// Only perform this check AWAY from the pi singularity, where
// numerical differentiation of Logmap is expected to be reliable
// and should match the analytical derivative.
if (!nearPi) {
const Matrix expectedH = numericalDerivative11<Vector6, Pose3>(
std::bind(&Pose3::Logmap, std::placeholders::_1, nullptr), T, 1e-7);
EXPECT(assert_equal(expectedH, actualH, 1e-5)); // 1e-5 needed to pass R4
}
else {
// We accept that the numerical derivative of this specific Logmap implementation
// near pi will not match the standard analytical derivative J_r^{-1}.
}
i++;
}
}
/* ************************************************************************* */
Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
return Pose3::adjointMap(xi) * v;

178
gtsam/geometry/tests/testSO3.cpp
View File

@ -274,44 +274,72 @@ TEST(SO3, ExpmapDerivative5) {
//******************************************************************************
TEST(SO3, ExpmapDerivative6) {
const Vector3 thetahat(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
std::bind(&SO3::Expmap, std::placeholders::_1, nullptr), thetahat);
Matrix3 Jactual;
SO3::Expmap(thetahat, Jactual);
EXPECT(assert_equal(Jexpected, Jactual));
const Vector3 theta(0.1, 0, 0.1);
const Matrix expectedH = numericalDerivative11<SO3, Vector3>(
std::bind(&SO3::Expmap, std::placeholders::_1, nullptr), theta);
Matrix3 actualH;
SO3::Expmap(theta, actualH);
EXPECT(assert_equal(expectedH, actualH));
}
//******************************************************************************
TEST(SO3, LogmapDerivative) {
const Vector3 thetahat(0.1, 0, 0.1);
const SO3 R = SO3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
std::bind(&SO3::Logmap, std::placeholders::_1, nullptr), R);
const Matrix3 Jactual = SO3::LogmapDerivative(thetahat);
EXPECT(assert_equal(Jexpected, Jactual));
}
const SO3 R0; // Identity
const Vector3 omega1(0.1, 0, 0.1);
const SO3 R1 = SO3::Expmap(omega1); // Small rotation
const SO3 R2((Matrix3() << // Near pi
-0.750767, -0.0285082, -0.659952,
-0.0102558, -0.998445, 0.0547974,
-0.660487, 0.0479084, 0.749307).finished());
const SO3 R3((Matrix3() << // Near pi
-0.747473, -0.00190019, -0.664289,
-0.0385114, -0.99819, 0.0461892,
-0.663175, 0.060108, 0.746047).finished());
const SO3 R4((Matrix3() << // Final pose in a drone experiment
0.324237, 0.902975, 0.281968,
-0.674322, 0.429668, -0.600562,
-0.663445, 0.00458662, 0.748211).finished());
size_t i = 0;
for (const SO3& R : { R0, R1, R2, R3, R4 }) {
const bool nearPi = (i == 2 || i == 3); // Flag cases near pi
//******************************************************************************
TEST(SO3, JacobianLogmap) {
const Vector3 thetahat(0.1, 0, 0.1);
const SO3 R = SO3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
std::bind(&SO3::Logmap, std::placeholders::_1, nullptr), R);
Matrix3 Jactual;
SO3::Logmap(R, Jactual);
EXPECT(assert_equal(Jexpected, Jactual));
}
Matrix3 actualH; // H computed by Logmap(R, H) using LogmapDerivative(omega)
const Vector3 omega = SO3::Logmap(R, actualH);
// 1. Check self-consistency of analytical derivative calculation:
// Does the H returned by Logmap match an independent calculation
// of J_r^{-1} using DexpFunctor with the computed omega?
so3::DexpFunctor local(omega);
Matrix3 J_r_inv = local.rightJacobianInverse(); // J_r^{-1} via DexpFunctor
EXPECT(assert_equal(J_r_inv, actualH)); // This test is crucial and should pass
// 2. Check analytical derivative against numerical derivative:
// Only perform this check AWAY from the pi singularity, where
// numerical differentiation of Logmap is expected to be reliable
// and should match the analytical derivative.
if (!nearPi) {
const Matrix expectedH = numericalDerivative11<Vector3, SO3>(
std::bind(&SO3::Logmap, std::placeholders::_1, nullptr), R, 1e-7);
EXPECT(assert_equal(expectedH, actualH, 1e-6)); // 1e-6 needed to pass R4
}
else {
// We accept that the numerical derivative of this specific Logmap implementation
// near pi will not match the standard analytical derivative J_r^{-1}.
}
i++;
}
}
//******************************************************************************
namespace test_cases {
std::vector<Vector3> small{{0, 0, 0}, //
{1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //,
{1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4}};
std::vector<Vector3> large{
{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3}};
auto omegas = [](bool nearZero) { return nearZero ? small : large; };
std::vector<Vector3> vs{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2}};
std::vector<Vector3> small{ {0, 0, 0}, //
{1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //,
{1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4} };
std::vector<Vector3> large{
{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3},
{0.4, 0.5, 0.6}, {0.7, 0.8, 0.9}, {1.1, 1.2, 1.3}, {1.4, 1.5, 1.6},
{1.7, 1.8, 1.9}, {2, 2, 2}, {3, 3, 3}, {4, 4, 4}, {5, 5, 5} };
auto omegas = [](bool nearZero) { return nearZero ? small : large; };
std::vector<Vector3> vs{ {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2} };
} // namespace test_cases
//******************************************************************************
@ -319,86 +347,50 @@ TEST(SO3, JacobianInverses) {
Matrix HR, HL;
for (bool nearZero : {true, false}) {
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5);
EXPECT(assert_equal<Matrix3>(local.rightJacobian().inverse(),
local.rightJacobianInverse()));
local.rightJacobianInverse()));
EXPECT(assert_equal<Matrix3>(local.leftJacobian().inverse(),
local.leftJacobianInverse()));
local.leftJacobianInverse()));
}
}
}
//******************************************************************************
TEST(SO3, CrossB) {
Matrix aH1;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).crossB(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
for (const Vector3& v : test_cases::vs) {
local.crossB(v, aH1);
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
}
}
}
}
//******************************************************************************
TEST(SO3, DoubleCrossC) {
Matrix aH1;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).doubleCrossC(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
for (const Vector3& v : test_cases::vs) {
local.doubleCrossC(v, aH1);
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
}
}
}
}
//******************************************************************************
TEST(SO3, ApplyDexp) {
TEST(SO3, ApplyRightJacobian) {
Matrix aH1, aH2;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyDexp(v);
};
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyRightJacobian(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5);
for (const Vector3& v : test_cases::vs) {
EXPECT(assert_equal(Vector3(local.dexp() * v),
local.applyDexp(v, aH1, aH2)));
EXPECT(assert_equal(Vector3(local.rightJacobian() * v),
local.applyRightJacobian(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(local.dexp(), aH2));
EXPECT(assert_equal(local.rightJacobian(), aH2));
}
}
}
}
//******************************************************************************
TEST(SO3, ApplyInvDexp) {
TEST(SO3, ApplyRightJacobianInverse) {
Matrix aH1, aH2;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyInvDexp(v);
};
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyRightJacobianInverse(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5);
Matrix invJr = local.rightJacobianInverse();
for (const Vector3& v : test_cases::vs) {
EXPECT(
assert_equal(Vector3(invJr * v), local.applyInvDexp(v, aH1, aH2)));
assert_equal(Vector3(invJr * v), local.applyRightJacobianInverse(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(invJr, aH2));
@ -412,14 +404,14 @@ TEST(SO3, ApplyLeftJacobian) {
Matrix aH1, aH2;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyLeftJacobian(v);
};
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyLeftJacobian(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5);
for (const Vector3& v : test_cases::vs) {
EXPECT(assert_equal(Vector3(local.leftJacobian() * v),
local.applyLeftJacobian(v, aH1, aH2)));
local.applyLeftJacobian(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(local.leftJacobian(), aH2));
@ -433,15 +425,15 @@ TEST(SO3, ApplyLeftJacobianInverse) {
Matrix aH1, aH2;
for (bool nearZero : {true, false}) {
std::function<Vector3(const Vector3&, const Vector3&)> f =
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero).applyLeftJacobianInverse(v);
};
[nearZero](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZero ? 1.0 : 0.0, 1e-5).applyLeftJacobianInverse(v);
};
for (const Vector3& omega : test_cases::omegas(nearZero)) {
so3::DexpFunctor local(omega, nearZero);
so3::DexpFunctor local(omega, nearZero ? 1.0 : 0.0, 1e-5);
Matrix invJl = local.leftJacobianInverse();
for (const Vector3& v : test_cases::vs) {
EXPECT(assert_equal(Vector3(invJl * v),
local.applyLeftJacobianInverse(v, aH1, aH2)));
local.applyLeftJacobianInverse(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(invJl, aH2));

17
gtsam/linear/linear.i
View File

@ -657,6 +657,21 @@ virtual class GaussianBayesNet {
};
#include <gtsam/linear/GaussianBayesTree.h>
class GaussianBayesTreeClique {
GaussianBayesTreeClique();
GaussianBayesTreeClique(const gtsam::GaussianConditional* conditional);
bool equals(const gtsam::GaussianBayesTreeClique& other, double tol) const;
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter);
const gtsam::GaussianConditional* conditional() const;
bool isRoot() const;
gtsam::GaussianBayesTreeClique* parent() const;
size_t nrChildren() const;
gtsam::GaussianBayesTreeClique* operator[](size_t j) const;
size_t treeSize() const;
size_t numCachedSeparatorMarginals() const;
void deleteCachedShortcuts();
};
virtual class GaussianBayesTree {
// Standard Constructors and Named Constructors
GaussianBayesTree();
@ -666,6 +681,8 @@ virtual class GaussianBayesTree {
gtsam::DefaultKeyFormatter);
size_t size() const;
bool empty() const;
const GaussianBayesTree::Roots& roots() const;
const gtsam::GaussianBayesTreeClique* operator[](size_t j) const;
size_t numCachedSeparatorMarginals() const;
string dot(const gtsam::KeyFormatter& keyFormatter =

15
gtsam/navigation/NavState.cpp
View File

@ -118,8 +118,7 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>();
// Instantiate functor for Dexp-related operations:
const bool nearZero = (w.dot(w) <= 1e-5);
const so3::DexpFunctor local(w, nearZero);
const so3::DexpFunctor local(w);
// Compute rotation using Expmap
#ifdef GTSAM_USE_QUATERNIONS
@ -238,15 +237,13 @@ Matrix9 NavState::ExpmapDerivative(const Vector9& xi) {
}
//------------------------------------------------------------------------------
Matrix9 NavState::LogmapDerivative(const NavState& state) {
const Vector9 xi = Logmap(state);
Matrix9 NavState::LogmapDerivative(const Vector9& xi) {
const Vector3 w = xi.head<3>();
Vector3 rho = xi.segment<3>(3);
Vector3 nu = xi.tail<3>();
// Instantiate functor for Dexp-related operations:
const bool nearZero = (w.dot(w) <= 1e-5);
const so3::DexpFunctor local(w, nearZero);
const so3::DexpFunctor local(w);
// Call applyLeftJacobian to get its Jacobians
Matrix3 H_t_w, H_v_w;
@ -270,6 +267,12 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
return J;
}
//------------------------------------------------------------------------------
Matrix9 NavState::LogmapDerivative(const NavState& state) {
const Vector9 xi = Logmap(state);
return LogmapDerivative(xi);
}
//------------------------------------------------------------------------------
Matrix5 NavState::Hat(const Vector9& xi) {
Matrix5 X;

3
gtsam/navigation/NavState.h
View File

@ -245,6 +245,9 @@ public:
static Matrix9 ExpmapDerivative(const Vector9& xi);
/// Derivative of Logmap
static Matrix9 LogmapDerivative(const Vector9& xi);
/// Derivative of Logmap, NavState version
static Matrix9 LogmapDerivative(const NavState& xi);
// Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP

4
gtsam/navigation/TangentPreintegration.cpp
View File

@ -66,7 +66,7 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
// Calculate exact mean propagation
Matrix3 w_tangent_H_theta, invH;
const Vector3 w_tangent = // angular velocity mapped back to tangent space
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
local.applyRightJacobianInverse(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
const Rot3 R(local.expmap()); // nRb: rotation of body in nav frame
const Vector3 a_nav = R * a_body;
const double dt22 = 0.5 * dt * dt;
@ -79,7 +79,7 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
if (A) {
// Exact derivative of R*a with respect to theta:
const Matrix3 a_nav_H_theta = R.matrix() * skewSymmetric(-a_body) * local.dexp();
const Matrix3 a_nav_H_theta = R.matrix() * skewSymmetric(-a_body) * local.rightJacobian();
A->setIdentity();
A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta

231
gtsam/symbolic/doc/SymbolicBayesNet.ipynb Normal file
View File

@ -0,0 +1,231 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# SymbolicBayesNet"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"A `SymbolicBayesNet` is a directed acyclic graph (DAG) composed of `SymbolicConditional` objects. It represents the structure of a factorized probability distribution P(X) = Π P(Xi | Parents(Xi)) purely in terms of variable connectivity.\n",
"\n",
"It is typically the result of running sequential variable elimination on a `SymbolicFactorGraph`."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/symbolic/doc/SymbolicBayesNet.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": 13,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Note: you may need to restart the kernel to use updated packages.\n"
]
}
],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "code",
"execution_count": 14,
"metadata": {},
"outputs": [],
"source": [
"from gtsam import SymbolicConditional, SymbolicFactorGraph, Ordering\n",
"from gtsam.symbol_shorthand import X, L\n",
"import graphviz"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Creating a SymbolicBayesNet\n",
"\n",
"SymbolicBayesNets are usually created by eliminating a [SymbolicFactorGraph](SymbolicFactorGraph.ipynb). But you can also build them directly:"
]
},
{
"cell_type": "code",
"execution_count": 15,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Directly Built Symbolic Bayes Net:\n",
" \n",
"size: 5\n",
"conditional 0: P( l1 | x0)\n",
"conditional 1: P( x0 | x1)\n",
"conditional 2: P( l2 | x1)\n",
"conditional 3: P( x1 | x2)\n",
"conditional 4: P( x2)\n"
]
}
],
"source": [
"from gtsam import SymbolicBayesNet\n",
"\n",
"# Create a new Bayes Net\n",
"symbolic_bayes_net = SymbolicBayesNet()\n",
"\n",
"# Add conditionals directly\n",
"symbolic_bayes_net.push_back(SymbolicConditional(L(1), X(0))) # P(l1 | x0)\n",
"symbolic_bayes_net.push_back(SymbolicConditional(X(0), X(1))) # P(x0 | x1)\n",
"symbolic_bayes_net.push_back(SymbolicConditional(L(2), X(1))) # P(l2 | x1)\n",
"symbolic_bayes_net.push_back(SymbolicConditional(X(1), X(2))) # P(x1 | x2)\n",
"symbolic_bayes_net.push_back(SymbolicConditional(X(2))) # P(x2)\n",
"\n",
"symbolic_bayes_net.print(\"Directly Built Symbolic Bayes Net:\\n\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Accessing Conditionals and Visualization"
]
},
{
"cell_type": "code",
"execution_count": 16,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Conditional at index 1: P( x0 | x1)\n"
]
},
{
"data": {
"image/svg+xml": [
"<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
"<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
" \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
"<!-- Generated by graphviz version 12.0.0 (0)\n",
" -->\n",
"<!-- Pages: 1 -->\n",
"<svg width=\"134pt\" height=\"260pt\"\n",
" viewBox=\"0.00 0.00 134.00 260.00\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
"<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 256)\">\n",
"<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-256 130,-256 130,4 -4,4\"/>\n",
"<!-- var7782220156096217089 -->\n",
"<g id=\"node1\" class=\"node\">\n",
"<title>var7782220156096217089</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-18\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"99\" y=\"-12.95\" font-family=\"Times,serif\" font-size=\"14.00\">l1</text>\n",
"</g>\n",
"<!-- var7782220156096217090 -->\n",
"<g id=\"node2\" class=\"node\">\n",
"<title>var7782220156096217090</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-90\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"27\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">l2</text>\n",
"</g>\n",
"<!-- var8646911284551352320 -->\n",
"<g id=\"node3\" class=\"node\">\n",
"<title>var8646911284551352320</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-90\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"99\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">x0</text>\n",
"</g>\n",
"<!-- var8646911284551352320&#45;&gt;var7782220156096217089 -->\n",
"<g id=\"edge4\" class=\"edge\">\n",
"<title>var8646911284551352320&#45;&gt;var7782220156096217089</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M99,-71.7C99,-64.41 99,-55.73 99,-47.54\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"102.5,-47.62 99,-37.62 95.5,-47.62 102.5,-47.62\"/>\n",
"</g>\n",
"<!-- var8646911284551352321 -->\n",
"<g id=\"node4\" class=\"node\">\n",
"<title>var8646911284551352321</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"63\" cy=\"-162\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"63\" y=\"-156.95\" font-family=\"Times,serif\" font-size=\"14.00\">x1</text>\n",
"</g>\n",
"<!-- var8646911284551352321&#45;&gt;var7782220156096217090 -->\n",
"<g id=\"edge2\" class=\"edge\">\n",
"<title>var8646911284551352321&#45;&gt;var7782220156096217090</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M54.65,-144.76C50.42,-136.55 45.19,-126.37 40.42,-117.09\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"43.68,-115.79 36,-108.49 37.46,-118.99 43.68,-115.79\"/>\n",
"</g>\n",
"<!-- var8646911284551352321&#45;&gt;var8646911284551352320 -->\n",
"<g id=\"edge3\" class=\"edge\">\n",
"<title>var8646911284551352321&#45;&gt;var8646911284551352320</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M71.35,-144.76C75.58,-136.55 80.81,-126.37 85.58,-117.09\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"88.54,-118.99 90,-108.49 82.32,-115.79 88.54,-118.99\"/>\n",
"</g>\n",
"<!-- var8646911284551352322 -->\n",
"<g id=\"node5\" class=\"node\">\n",
"<title>var8646911284551352322</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"63\" cy=\"-234\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"63\" y=\"-228.95\" font-family=\"Times,serif\" font-size=\"14.00\">x2</text>\n",
"</g>\n",
"<!-- var8646911284551352322&#45;&gt;var8646911284551352321 -->\n",
"<g id=\"edge1\" class=\"edge\">\n",
"<title>var8646911284551352322&#45;&gt;var8646911284551352321</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M63,-215.7C63,-208.41 63,-199.73 63,-191.54\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"66.5,-191.62 63,-181.62 59.5,-191.62 66.5,-191.62\"/>\n",
"</g>\n",
"</g>\n",
"</svg>\n"
],
"text/plain": [
"<graphviz.sources.Source at 0x10c18fda0>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"# Access a conditional by index\n",
"conditional_1 = bayes_net.at(1) # P(x0 | l1)\n",
"conditional_1.print(\"Conditional at index 1: \")\n",
"\n",
"# Visualize the Bayes Net structure\n",
"display(graphviz.Source(bayes_net.dot()))"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "py312",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.12.6"
}
},
"nbformat": 4,
"nbformat_minor": 4
}

325
gtsam/symbolic/doc/SymbolicBayesTree.ipynb Normal file
View File

@ -0,0 +1,325 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# SymbolicBayesTree"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"A `SymbolicBayesTree` is a tree structure where each node (`SymbolicBayesTreeClique`) represents a clique of variables that were eliminated together during multifrontal elimination. Unlike a `SymbolicJunctionTree` which stores factors, a `SymbolicBayesTree` stores the resulting `SymbolicConditional` for each clique.\n",
"\n",
"It represents the factored structure $P(X) = Π P(C_j | S_j)$, where $C_j$ are the frontal variables of clique $j$ and $S_j$ are its separator variables (parents in the Bayes Tree). This structure is computationally advantageous for inference, especially for calculating marginals or performing incremental updates (like in iSAM)."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/symbolic/doc/SymbolicBayesTree.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Note: you may need to restart the kernel to use updated packages.\n"
]
}
],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
"from gtsam import SymbolicFactorGraph, Ordering\n",
"from gtsam.symbol_shorthand import X, L\n",
"import graphviz # For visualization"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Creating a SymbolicBayesTree\n",
"\n",
"A Bayes tree is typically created by eliminating a `SymbolicFactorGraph` using the multifrontal method."
]
},
{
"cell_type": "code",
"execution_count": 20,
"metadata": {},
"outputs": [],
"source": [
"# Create a factor graph from a GTSFM problem\n",
"graph = SymbolicFactorGraph()\n",
"\n",
"edges = [(2, 4), (2, 5), (2, 27), (4, 6), (5, 6), (5, 7), (5, 8), (5, 9), (6, 7), (6, 8), (6, 9), \n",
" (7, 8), (7, 9), (8, 9), (9, 12), (9, 24), (9, 28), (10, 12), (10, 29), (20, 21), (20, 22), \n",
" (20, 23), (20, 24), (21, 22), (21, 23), (21, 24), (22, 23), (22, 24), (23, 24), (25, 26), \n",
" (25, 27), (25, 28), (25, 29), (26, 27), (26, 28), (26, 29), (27, 28), (27, 29), (28, 29)]\n",
"\n",
"for i,j in edges:\n",
" graph.push_factor(X(i), X(j))\n",
"\n",
"# Define an elimination ordering\n",
"ordering = Ordering.MetisSymbolicFactorGraph(graph) # Use METIS for this example\n",
"\n",
"# Eliminate using Multifrontal method\n",
"bayes_tree = graph.eliminateMultifrontal(ordering)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Visualization and Properties\n",
"\n",
"The Bayes tree structure can be visualized, and we can query its properties."
]
},
{
"cell_type": "code",
"execution_count": 21,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Bayes Tree size (number of cliques): 8\n",
"Is the Bayes Tree empty? False\n"
]
}
],
"source": [
"print(f\"\\nBayes Tree size (number of cliques): {bayes_tree.size()}\")\n",
"print(f\"Is the Bayes Tree empty? {bayes_tree.empty()}\")"
]
},
{
"cell_type": "code",
"execution_count": 22,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
"<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
" \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
"<!-- Generated by graphviz version 12.0.0 (0)\n",
" -->\n",
"<!-- Title: G Pages: 1 -->\n",
"<svg width=\"695pt\" height=\"260pt\"\n",
" viewBox=\"0.00 0.00 694.66 260.00\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
"<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 256)\">\n",
"<title>G</title>\n",
"<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-256 690.66,-256 690.66,4 -4,4\"/>\n",
"<!-- 20 -->\n",
"<g id=\"node1\" class=\"node\">\n",
"<title>20</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"373.29\" cy=\"-234\" rx=\"114.82\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"373.29\" y=\"-228.95\" font-family=\"Times,serif\" font-size=\"14.00\">x28, x25, x26, x9, x29, x27</text>\n",
"</g>\n",
"<!-- 21 -->\n",
"<g id=\"node2\" class=\"node\">\n",
"<title>21</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"224.29\" cy=\"-162\" rx=\"70.29\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"224.29\" y=\"-156.95\" font-family=\"Times,serif\" font-size=\"14.00\">x5, x4 : x9, x27</text>\n",
"</g>\n",
"<!-- 20&#45;&gt;21 -->\n",
"<g id=\"edge1\" class=\"edge\">\n",
"<title>20&#45;&gt;21</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M337.98,-216.41C316.8,-206.46 289.73,-193.74 267.31,-183.21\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"268.96,-180.12 258.42,-179.04 265.98,-186.46 268.96,-180.12\"/>\n",
"</g>\n",
"<!-- 25 -->\n",
"<g id=\"node6\" class=\"node\">\n",
"<title>25</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"373.29\" cy=\"-162\" rx=\"60.56\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"373.29\" y=\"-156.95\" font-family=\"Times,serif\" font-size=\"14.00\">x12 : x9, x29</text>\n",
"</g>\n",
"<!-- 20&#45;&gt;25 -->\n",
"<g id=\"edge5\" class=\"edge\">\n",
"<title>20&#45;&gt;25</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M373.29,-215.7C373.29,-208.41 373.29,-199.73 373.29,-191.54\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"376.79,-191.62 373.29,-181.62 369.79,-191.62 376.79,-191.62\"/>\n",
"</g>\n",
"<!-- 27 -->\n",
"<g id=\"node8\" class=\"node\">\n",
"<title>27</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"569.29\" cy=\"-162\" rx=\"117.37\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"569.29\" y=\"-156.95\" font-family=\"Times,serif\" font-size=\"14.00\">x24, x23, x22, x21, x20 : x9</text>\n",
"</g>\n",
"<!-- 20&#45;&gt;27 -->\n",
"<g id=\"edge7\" class=\"edge\">\n",
"<title>20&#45;&gt;27</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M417.76,-217.12C446.26,-206.94 483.45,-193.66 513.86,-182.79\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"514.64,-186.23 522.89,-179.57 512.29,-179.64 514.64,-186.23\"/>\n",
"</g>\n",
"<!-- 22 -->\n",
"<g id=\"node3\" class=\"node\">\n",
"<title>22</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"70.29\" cy=\"-90\" rx=\"70.29\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"70.29\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">x2 : x4, x5, x27</text>\n",
"</g>\n",
"<!-- 21&#45;&gt;22 -->\n",
"<g id=\"edge2\" class=\"edge\">\n",
"<title>21&#45;&gt;22</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M190.87,-145.81C168.45,-135.62 138.67,-122.08 114.32,-111.01\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"115.83,-107.86 105.28,-106.91 112.94,-114.23 115.83,-107.86\"/>\n",
"</g>\n",
"<!-- 23 -->\n",
"<g id=\"node4\" class=\"node\">\n",
"<title>23</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"224.29\" cy=\"-90\" rx=\"65.68\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"224.29\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">x6 : x4, x5, x9</text>\n",
"</g>\n",
"<!-- 21&#45;&gt;23 -->\n",
"<g id=\"edge3\" class=\"edge\">\n",
"<title>21&#45;&gt;23</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M224.29,-143.7C224.29,-136.41 224.29,-127.73 224.29,-119.54\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"227.79,-119.62 224.29,-109.62 220.79,-119.62 227.79,-119.62\"/>\n",
"</g>\n",
"<!-- 24 -->\n",
"<g id=\"node5\" class=\"node\">\n",
"<title>24</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"224.29\" cy=\"-18\" rx=\"80.01\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"224.29\" y=\"-12.95\" font-family=\"Times,serif\" font-size=\"14.00\">x8, x7 : x5, x6, x9</text>\n",
"</g>\n",
"<!-- 23&#45;&gt;24 -->\n",
"<g id=\"edge4\" class=\"edge\">\n",
"<title>23&#45;&gt;24</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M224.29,-71.7C224.29,-64.41 224.29,-55.73 224.29,-47.54\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"227.79,-47.62 224.29,-37.62 220.79,-47.62 227.79,-47.62\"/>\n",
"</g>\n",
"<!-- 26 -->\n",
"<g id=\"node7\" class=\"node\">\n",
"<title>26</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"373.29\" cy=\"-90\" rx=\"65.17\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"373.29\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">x10 : x12, x29</text>\n",
"</g>\n",
"<!-- 25&#45;&gt;26 -->\n",
"<g id=\"edge6\" class=\"edge\">\n",
"<title>25&#45;&gt;26</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M373.29,-143.7C373.29,-136.41 373.29,-127.73 373.29,-119.54\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"376.79,-119.62 373.29,-109.62 369.79,-119.62 376.79,-119.62\"/>\n",
"</g>\n",
"</g>\n",
"</svg>\n"
],
"text/plain": [
"<graphviz.sources.Source at 0x1120de8a0>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"# Visualize the Bayes tree using graphviz\n",
"display(graphviz.Source(bayes_tree.dot()))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Traversing a Bayes Tree"
]
},
{
"cell_type": "code",
"execution_count": 23,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Number of roots: 1\n",
"\n",
"Clique:\n",
" P( x28 x25 x26 x9 x29 x27)\n",
"\n",
"Clique:\n",
" P( x5 x4 | x9 x27)\n",
"\n",
"Clique:\n",
" P( x2 | x4 x5 x27)\n",
"\n",
"Clique:\n",
" P( x6 | x4 x5 x9)\n",
"\n",
"Clique:\n",
" P( x8 x7 | x5 x6 x9)\n",
"\n",
"Clique:\n",
" P( x12 | x9 x29)\n",
"\n",
"Clique:\n",
" P( x10 | x12 x29)\n",
"\n",
"Clique:\n",
" P( x24 x23 x22 x21 x20 | x9)\n"
]
}
],
"source": [
"roots = bayes_tree.roots()\n",
"print(f\"Number of roots: {len(roots)}\")\n",
"\n",
"def traverse_clique(clique):\n",
" if clique:\n",
" clique.print(\"\\nClique:\\n\")\n",
" for j in range(clique.nrChildren()):\n",
" traverse_clique(clique[j])\n",
"\n",
"for root in roots:\n",
" traverse_clique(root)"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "py312",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.12.6"
}
},
"nbformat": 4,
"nbformat_minor": 4
}

262
gtsam/symbolic/doc/SymbolicBayesTreeClique.ipynb Normal file
View File

@ -0,0 +1,262 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# SymbolicBayesTreeClique"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"A `SymbolicBayesTreeClique` represents a single node within a `SymbolicBayesTree`. Each clique corresponds to a set of variables (frontal variables) that were eliminated together during the multifrontal elimination process.\n",
"\n",
"Key aspects of a clique:\n",
"* **Conditional:** It stores the `SymbolicConditional` P(Frontals | Parents/Separator) that results from eliminating the frontal variables.\n",
"* **Tree Structure:** It maintains pointers to its parent and children cliques within the Bayes Tree, defining the tree's topology.\n",
"* **Frontal and Separator Variables:** Implicitly defines the frontal variables (eliminated in this clique) and separator variables (parents in the Bayes Tree, connecting it to its parent clique).\n",
"\n",
"Users typically interact with the `SymbolicBayesTree` as a whole, but understanding the clique structure is helpful for comprehending how the Bayes Tree represents the factored distribution and facilitates efficient inference."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/symbolic/doc/SymbolicBayesTreeClique.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "code",
"execution_count": 11,
"metadata": {},
"outputs": [],
"source": [
"from gtsam import SymbolicFactorGraph, Ordering\n",
"# SymbolicBayesTreeClique is accessed *through* a SymbolicBayesTree\n",
"from gtsam.symbol_shorthand import X, L\n",
"import graphviz"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Accessing and Inspecting Cliques\n",
"\n",
"Cliques are obtained by first creating a `SymbolicBayesTree` (usually via elimination) and then accessing its nodes."
]
},
{
"cell_type": "code",
"execution_count": 16,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Bayes Tree has 4 cliques.\n",
"Number of roots: 1\n"
]
},
{
"data": {
"image/svg+xml": [
"<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
"<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
" \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
"<!-- Generated by graphviz version 12.0.0 (0)\n",
" -->\n",
"<!-- Title: G Pages: 1 -->\n",
"<svg width=\"170pt\" height=\"188pt\"\n",
" viewBox=\"0.00 0.00 169.99 188.00\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
"<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 184)\">\n",
"<title>G</title>\n",
"<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-184 165.99,-184 165.99,4 -4,4\"/>\n",
"<!-- 6 -->\n",
"<g id=\"node1\" class=\"node\">\n",
"<title>6</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"82.02\" cy=\"-162\" rx=\"34.46\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"82.02\" y=\"-156.95\" font-family=\"Times,serif\" font-size=\"14.00\">x1, x2</text>\n",
"</g>\n",
"<!-- 7 -->\n",
"<g id=\"node2\" class=\"node\">\n",
"<title>7</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"37.02\" cy=\"-90\" rx=\"37.02\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"37.02\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">x0 : x1</text>\n",
"</g>\n",
"<!-- 6&#45;&gt;7 -->\n",
"<g id=\"edge1\" class=\"edge\">\n",
"<title>6&#45;&gt;7</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M71.36,-144.41C66.09,-136.22 59.61,-126.14 53.71,-116.95\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"56.78,-115.26 48.43,-108.74 50.89,-119.05 56.78,-115.26\"/>\n",
"</g>\n",
"<!-- 9 -->\n",
"<g id=\"node4\" class=\"node\">\n",
"<title>9</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"127.02\" cy=\"-90\" rx=\"34.97\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"127.02\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">l2 : x1</text>\n",
"</g>\n",
"<!-- 6&#45;&gt;9 -->\n",
"<g id=\"edge3\" class=\"edge\">\n",
"<title>6&#45;&gt;9</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M92.69,-144.41C97.95,-136.22 104.43,-126.14 110.34,-116.95\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"113.15,-119.05 115.61,-108.74 107.26,-115.26 113.15,-119.05\"/>\n",
"</g>\n",
"<!-- 8 -->\n",
"<g id=\"node3\" class=\"node\">\n",
"<title>8</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"37.02\" cy=\"-18\" rx=\"34.97\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"37.02\" y=\"-12.95\" font-family=\"Times,serif\" font-size=\"14.00\">l1 : x0</text>\n",
"</g>\n",
"<!-- 7&#45;&gt;8 -->\n",
"<g id=\"edge2\" class=\"edge\">\n",
"<title>7&#45;&gt;8</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M37.02,-71.7C37.02,-64.41 37.02,-55.73 37.02,-47.54\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"40.52,-47.62 37.02,-37.62 33.52,-47.62 40.52,-47.62\"/>\n",
"</g>\n",
"</g>\n",
"</svg>\n"
],
"text/plain": [
"<graphviz.sources.Source at 0x105c27740>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"# Create a factor graph\n",
"graph = SymbolicFactorGraph()\n",
"graph.push_factor(X(0))\n",
"graph.push_factor(X(0), X(1))\n",
"graph.push_factor(X(1), X(2))\n",
"graph.push_factor(X(0), L(1))\n",
"graph.push_factor(X(1), L(2))\n",
"\n",
"# Eliminate to get a Bayes Tree\n",
"ordering = Ordering.ColamdSymbolicFactorGraph(graph)\n",
"bayes_tree = graph.eliminateMultifrontal(ordering)\n",
"\n",
"print(f\"Bayes Tree has {bayes_tree.size()} cliques.\")\n",
"\n",
"roots = bayes_tree.roots()\n",
"print(f\"Number of roots: {len(roots)}\")\n",
"\n",
"# Visualize the Bayes tree using graphviz\n",
"display(graphviz.Source(bayes_tree.dot()))"
]
},
{
"cell_type": "code",
"execution_count": 17,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Inspecting Clique 0:\n",
" Clique Structure: P( x1 x2)\n",
" Associated Conditional: P( P( x1 x2)\n",
" Is Root? True\n",
" Parent Clique is None (likely a root clique).\n",
" Number of Children: 2\n",
"\n",
"Inspecting Clique 0:\n",
" Clique Structure: P( x0 | x1)\n",
" Associated Conditional: P( P( x0 | x1)\n",
" Is Root? False\n",
" Parent Clique exists.\n",
" Number of Children: 1\n",
"\n",
"Inspecting Clique 0:\n",
" Clique Structure: P( l1 | x0)\n",
" Associated Conditional: P( P( l1 | x0)\n",
" Is Root? False\n",
" Parent Clique exists.\n",
" Number of Children: 0\n",
"\n",
"Inspecting Clique 0:\n",
" Clique Structure: P( l2 | x1)\n",
" Associated Conditional: P( P( l2 | x1)\n",
" Is Root? False\n",
" Parent Clique exists.\n",
" Number of Children: 0\n"
]
}
],
"source": [
"def inspect(clique):\n",
" print(\"\\nInspecting Clique 0:\")\n",
" clique.print(\" Clique Structure: \")\n",
" \n",
" # Get the conditional stored in the clique\n",
" conditional = clique.conditional()\n",
" if conditional:\n",
" conditional.print(\" Associated Conditional: P(\")\n",
" else:\n",
" print(\" Clique has no associated conditional (might be empty root)\")\n",
" \n",
" # Check properties\n",
" print(f\" Is Root? {clique.isRoot()}\")\n",
" # Accessing parent/children is possible in C++ but might be less direct or typical in Python wrapper usage\n",
" # Parent clique (careful, might be null for root)\n",
" parent_clique = clique.parent() \n",
" if parent_clique:\n",
" print(\" Parent Clique exists.\")\n",
" else:\n",
" print(\" Parent Clique is None (likely a root clique).\")\n",
" \n",
" print(f\" Number of Children: {clique.nrChildren()}\") # Example if method existed\n",
"\n",
"def traverse_clique(clique):\n",
" inspect(clique)\n",
" for j in range(clique.nrChildren()):\n",
" traverse_clique(clique[j])\n",
"\n",
"for root in roots:\n",
" traverse_clique(root)"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "py312",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.12.6"
}
},
"nbformat": 4,
"nbformat_minor": 4
}

142
gtsam/symbolic/doc/SymbolicConditional.ipynb Normal file
View File

@ -0,0 +1,142 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# SymbolicConditional"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"A `SymbolicConditional` represents a conditional probability distribution P(Frontals | Parents) in purely symbolic form. It only stores the keys of the frontal variables and the parent variables, without any associated numerical probability function.\n",
"\n",
"`SymbolicConditional` objects are typically produced as the result of symbolic elimination on a `SymbolicFactorGraph`. A collection of these forms a `SymbolicBayesNet`."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/symbolic/doc/SymbolicConditional.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Note: you may need to restart the kernel to use updated packages.\n"
]
}
],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"from gtsam import SymbolicConditional\n",
"from gtsam.symbol_shorthand import X, L"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Creating SymbolicConditionals\n",
"\n",
"Conditionals specify frontal (conditioned) variables and parent variables."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"P(X(0)): P( x0)\n",
" Nr Frontals: 1, Nr Parents: 0\n",
"\n",
"P(X(1) | X(0)): P( x1 | x0)\n",
" Nr Frontals: 1, Nr Parents: 1\n",
"\n",
"P(X(2) | X(0), X(1)): P( x2 | x0 x1)\n",
" Nr Frontals: 1, Nr Parents: 2\n",
"\n",
"P(L(1) | X(0), X(1)): P( l1 | x0 x1)\n",
" Nr Frontals: 1, Nr Parents: 2\n",
"\n",
"P(X(2), L(1) | X(0), X(1)): P( x2 l1 | x0 x1)\n",
" Nr Frontals: 2, Nr Parents: 2\n"
]
}
],
"source": [
"# P(X(0))\n",
"c0 = SymbolicConditional(X(0))\n",
"c0.print(\"P(X(0)): \")\n",
"print(f\" Nr Frontals: {c0.nrFrontals()}, Nr Parents: {c0.nrParents()}\\n\")\n",
"\n",
"# P(X(1) | X(0))\n",
"c1 = SymbolicConditional(X(1), X(0))\n",
"c1.print(\"P(X(1) | X(0)): \")\n",
"print(f\" Nr Frontals: {c1.nrFrontals()}, Nr Parents: {c1.nrParents()}\\n\")\n",
"\n",
"# P(X(2) | X(0), X(1))\n",
"c2 = SymbolicConditional(X(2), X(0), X(1))\n",
"c2.print(\"P(X(2) | X(0), X(1)): \")\n",
"print(f\" Nr Frontals: {c2.nrFrontals()}, Nr Parents: {c2.nrParents()}\\n\")\n",
"\n",
"# P(L(1) | X(0), X(1))\n",
"c3 = SymbolicConditional(L(1), X(0), X(1))\n",
"c3.print(\"P(L(1) | X(0), X(1)): \")\n",
"print(f\" Nr Frontals: {c3.nrFrontals()}, Nr Parents: {c3.nrParents()}\\n\")\n",
"\n",
"# Create from keys and number of frontals, e.g. P(X(2), L(1) | X(0), X(1))\n",
"# Keys = [Frontals..., Parents...]\n",
"c4 = SymbolicConditional.FromKeys([X(2), L(1), X(0), X(1)], 2)\n",
"c4.print(\"P(X(2), L(1) | X(0), X(1)): \")\n",
"print(f\" Nr Frontals: {c4.nrFrontals()}, Nr Parents: {c4.nrParents()}\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "py312",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.12.6"
}
},
"nbformat": 4,
"nbformat_minor": 4
}

138
gtsam/symbolic/doc/SymbolicEliminationTree.ipynb Normal file
View File

@ -0,0 +1,138 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# SymbolicEliminationTree"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"A `SymbolicEliminationTree` represents the computational structure used in variable elimination, particularly in sparse Cholesky or QR factorization. Each node in the tree corresponds to the elimination of a single variable.\n",
"\n",
"The tree structure reveals dependencies: the elimination of a variable (node) depends on the results from its children in the tree. The root of the tree corresponds to the last variable eliminated. This structure is closely related to the resulting Bayes net or Bayes tree."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/symbolic/doc/SymbolicEliminationTree.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"from gtsam import SymbolicEliminationTree, SymbolicFactorGraph, Ordering\n",
"from gtsam.symbol_shorthand import X, L"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Creating a SymbolicEliminationTree\n",
"\n",
"An elimination tree is constructed from a `SymbolicFactorGraph` and an `Ordering`."
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Symbolic Elimination Tree:\n",
"-(x2)\n",
"Symbolic Elimination Tree:\n",
"| -(x1)\n",
"Symbolic Elimination Tree:\n",
"| - x1 x2\n",
"Symbolic Elimination Tree:\n",
"| | -(x0)\n",
"Symbolic Elimination Tree:\n",
"| | - x0\n",
"Symbolic Elimination Tree:\n",
"| | - x0 x1\n",
"Symbolic Elimination Tree:\n",
"| | | -(l1)\n",
"Symbolic Elimination Tree:\n",
"| | | - x0 l1\n",
"Symbolic Elimination Tree:\n",
"| | -(l2)\n",
"Symbolic Elimination Tree:\n",
"| | - x1 l2\n"
]
}
],
"source": [
"# Create a factor graph\n",
"graph = SymbolicFactorGraph()\n",
"graph.push_factor(X(0))\n",
"graph.push_factor(X(0), X(1))\n",
"graph.push_factor(X(1), X(2))\n",
"graph.push_factor(X(0), L(1))\n",
"graph.push_factor(X(1), L(2))\n",
"\n",
"# Define an elimination ordering\n",
"ordering = Ordering([L(1), L(2), X(0), X(1), X(2)]) # Eliminate L(1) first, then X(0), X(1), X(2) last\n",
"\n",
"# Construct the elimination tree\n",
"elimination_tree = SymbolicEliminationTree(graph, ordering)\n",
"\n",
"# Print the tree structure (text representation)\n",
"elimination_tree.print(\"Symbolic Elimination Tree:\\n\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"*(Note: Direct visualization of the elimination tree structure isn't available via a simple `.dot()` method like factor graphs or Bayes nets/trees in the Python wrapper, but the print output shows the parent-child relationships.)*"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "py312",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.12.6"
}
},
"nbformat": 4,
"nbformat_minor": 4
}

131
gtsam/symbolic/doc/SymbolicFactor.ipynb Normal file
View File

@ -0,0 +1,131 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# SymbolicFactor"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"A `SymbolicFactor` represents the connectivity (topology) between variables in a factor graph without any specific numerical function associated with it. It's primarily to *illustrate* symbolic elimination. Internally, GTSAM does analyze the structure of other factor graph types without explicitly converting to a symbolic factor graph.\n",
"\n",
"It inherits from `gtsam.Factor` and stores the keys (indices) of the variables it connects."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/symbolic/doc/SymbolicFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Note: you may need to restart the kernel to use updated packages.\n"
]
}
],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"import gtsam\n",
"from gtsam import SymbolicFactor\n",
"from gtsam.symbol_shorthand import X\n",
"\n",
"# Example Keys\n",
"x0, x1, x2 = X(0), X(1), X(2)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Creating SymbolicFactors\n",
"\n",
"SymbolicFactors can be created by specifying the keys they involve."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Unary Factor f0: \n",
" x0\n",
"Binary Factor f1: \n",
" x0 x1\n",
"Ternary Factor f2: \n",
" x1 x2 x0\n",
"Factor f3 from KeyVector: \n",
" x0 x1 x2\n"
]
}
],
"source": [
"# Unary factor\n",
"f0 = SymbolicFactor(x0)\n",
"f0.print(\"Unary Factor f0: \\n\")\n",
"\n",
"# Binary factor\n",
"f1 = SymbolicFactor(x0, x1)\n",
"f1.print(\"Binary Factor f1: \\n\")\n",
"\n",
"# Ternary factor\n",
"f2 = SymbolicFactor(x1, x2, x0)\n",
"f2.print(\"Ternary Factor f2: \\n\")\n",
"\n",
"# From a list of keys\n",
"keys = gtsam.KeyVector([x0, x1, x2])\n",
"f3 = SymbolicFactor.FromKeys(keys)\n",
"f3.print(\"Factor f3 from KeyVector: \\n\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "py312",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.12.6"
}
},
"nbformat": 4,
"nbformat_minor": 4
}

479
gtsam/symbolic/doc/SymbolicFactorGraph.ipynb Normal file
View File

@ -0,0 +1,479 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# SymbolicFactorGraph"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"A `SymbolicFactorGraph` is a factor graph consisting purely of `SymbolicFactor` objects. It represents the structure or topology of a probabilistic graphical model (like a Markov Random Field) without the numerical details.\n",
"\n",
"It's primarily used to *illustrate* symbolic elimination, which determines the structure of the resulting Bayes net or Bayes tree and finds an efficient variable elimination ordering (e.g., using COLAMD or METIS)."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/symbolic/doc/SymbolicFactorGraph.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Note: you may need to restart the kernel to use updated packages.\n"
]
}
],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "code",
"execution_count": 13,
"metadata": {},
"outputs": [],
"source": [
"from gtsam import SymbolicFactorGraph, Ordering\n",
"from gtsam.symbol_shorthand import X, L\n",
"import graphviz"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Creating and Manipulating Symbolic Factor Graphs"
]
},
{
"cell_type": "code",
"execution_count": 12,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Symbolic Factor Graph:\n",
" \n",
"size: 5\n",
"factor 0: x0\n",
"factor 1: x0 x1\n",
"factor 2: x1 x2\n",
"factor 3: x0 l1\n",
"factor 4: x1 l2\n"
]
},
{
"data": {
"image/svg+xml": [
"<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
"<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
" \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
"<!-- Generated by graphviz version 12.0.0 (0)\n",
" -->\n",
"<!-- Pages: 1 -->\n",
"<svg width=\"350pt\" height=\"84pt\"\n",
" viewBox=\"0.00 0.00 350.00 83.60\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
"<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 79.6)\">\n",
"<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-79.6 346,-79.6 346,4 -4,4\"/>\n",
"<!-- var7782220156096217089 -->\n",
"<g id=\"node1\" class=\"node\">\n",
"<title>var7782220156096217089</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"27\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">l1</text>\n",
"</g>\n",
"<!-- factor3 -->\n",
"<g id=\"node9\" class=\"node\">\n",
"<title>factor3</title>\n",
"<ellipse fill=\"black\" stroke=\"black\" cx=\"52\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
"</g>\n",
"<!-- var7782220156096217089&#45;&#45;factor3 -->\n",
"<g id=\"edge7\" class=\"edge\">\n",
"<title>var7782220156096217089&#45;&#45;factor3</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M34.77,-39.87C41.08,-26.3 49.27,-8.68 51.45,-3.99\"/>\n",
"</g>\n",
"<!-- var7782220156096217090 -->\n",
"<g id=\"node2\" class=\"node\">\n",
"<title>var7782220156096217090</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"315\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"315\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">l2</text>\n",
"</g>\n",
"<!-- factor4 -->\n",
"<g id=\"node10\" class=\"node\">\n",
"<title>factor4</title>\n",
"<ellipse fill=\"black\" stroke=\"black\" cx=\"279\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
"</g>\n",
"<!-- var7782220156096217090&#45;&#45;factor4 -->\n",
"<g id=\"edge9\" class=\"edge\">\n",
"<title>var7782220156096217090&#45;&#45;factor4</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M304.39,-40.75C295.1,-26.86 282.63,-8.22 279.66,-3.78\"/>\n",
"</g>\n",
"<!-- var8646911284551352320 -->\n",
"<g id=\"node3\" class=\"node\">\n",
"<title>var8646911284551352320</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"99\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">x0</text>\n",
"</g>\n",
"<!-- factor0 -->\n",
"<g id=\"node6\" class=\"node\">\n",
"<title>factor0</title>\n",
"<ellipse fill=\"black\" stroke=\"black\" cx=\"99\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
"</g>\n",
"<!-- var8646911284551352320&#45;&#45;factor0 -->\n",
"<g id=\"edge1\" class=\"edge\">\n",
"<title>var8646911284551352320&#45;&#45;factor0</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M99,-39.28C99,-25.77 99,-8.54 99,-3.96\"/>\n",
"</g>\n",
"<!-- factor1 -->\n",
"<g id=\"node7\" class=\"node\">\n",
"<title>factor1</title>\n",
"<ellipse fill=\"black\" stroke=\"black\" cx=\"160\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
"</g>\n",
"<!-- var8646911284551352320&#45;&#45;factor1 -->\n",
"<g id=\"edge2\" class=\"edge\">\n",
"<title>var8646911284551352320&#45;&#45;factor1</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M114.71,-42.75C131.09,-28.3 154.98,-7.23 159.31,-3.41\"/>\n",
"</g>\n",
"<!-- var8646911284551352320&#45;&#45;factor3 -->\n",
"<g id=\"edge6\" class=\"edge\">\n",
"<title>var8646911284551352320&#45;&#45;factor3</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M85.91,-41.61C73.74,-27.68 56.91,-8.41 52.89,-3.81\"/>\n",
"</g>\n",
"<!-- var8646911284551352321 -->\n",
"<g id=\"node4\" class=\"node\">\n",
"<title>var8646911284551352321</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"243\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"243\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">x1</text>\n",
"</g>\n",
"<!-- var8646911284551352321&#45;&#45;factor1 -->\n",
"<g id=\"edge3\" class=\"edge\">\n",
"<title>var8646911284551352321&#45;&#45;factor1</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M224.17,-44.39C201.97,-30.01 167.31,-7.54 161,-3.45\"/>\n",
"</g>\n",
"<!-- factor2 -->\n",
"<g id=\"node8\" class=\"node\">\n",
"<title>factor2</title>\n",
"<ellipse fill=\"black\" stroke=\"black\" cx=\"196\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
"</g>\n",
"<!-- var8646911284551352321&#45;&#45;factor2 -->\n",
"<g id=\"edge4\" class=\"edge\">\n",
"<title>var8646911284551352321&#45;&#45;factor2</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M229.91,-41.61C217.74,-27.68 200.91,-8.41 196.89,-3.81\"/>\n",
"</g>\n",
"<!-- var8646911284551352321&#45;&#45;factor4 -->\n",
"<g id=\"edge8\" class=\"edge\">\n",
"<title>var8646911284551352321&#45;&#45;factor4</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M253.61,-40.75C262.9,-26.86 275.37,-8.22 278.34,-3.78\"/>\n",
"</g>\n",
"<!-- var8646911284551352322 -->\n",
"<g id=\"node5\" class=\"node\">\n",
"<title>var8646911284551352322</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"171\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"171\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">x2</text>\n",
"</g>\n",
"<!-- var8646911284551352322&#45;&#45;factor2 -->\n",
"<g id=\"edge5\" class=\"edge\">\n",
"<title>var8646911284551352322&#45;&#45;factor2</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M178.77,-39.87C185.08,-26.3 193.27,-8.68 195.45,-3.99\"/>\n",
"</g>\n",
"</g>\n",
"</svg>\n"
],
"text/plain": [
"<graphviz.sources.Source at 0x107b7c890>"
]
},
"execution_count": 12,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"# Create an empty graph\n",
"graph = SymbolicFactorGraph()\n",
"\n",
"# Add factors (using convenience methods)\n",
"graph.push_factor(X(0)) # Unary\n",
"graph.push_factor(X(0), X(1))\n",
"graph.push_factor(X(1), X(2))\n",
"graph.push_factor(X(0), L(1))\n",
"graph.push_factor(X(1), L(2))\n",
"\n",
"# Print the graph structure\n",
"graph.print(\"Symbolic Factor Graph:\\n\")\n",
"\n",
"# Visualize the graph using Graphviz\n",
"graphviz.Source(graph.dot())"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Symbolic Elimination\n",
"\n",
"We can perform symbolic elimination to get the structure of the resulting Bayes net or Bayes tree."
]
},
{
"cell_type": "code",
"execution_count": 16,
"metadata": {},
"outputs": [],
"source": [
"# Define an elimination ordering (can also be computed automatically)\n",
"ordering = Ordering([L(1), L(2), X(0), X(1), X(2)])"
]
},
{
"cell_type": "code",
"execution_count": 18,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Resulting Symbolic Bayes Net:\n",
" \n",
"size: 5\n",
"conditional 0: P( l1 | x0)\n",
"conditional 1: P( x0 | x1)\n",
"conditional 2: P( l2 | x1)\n",
"conditional 3: P( x1 | x2)\n",
"conditional 4: P( x2)\n"
]
},
{
"data": {
"image/svg+xml": [
"<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
"<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
" \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
"<!-- Generated by graphviz version 12.0.0 (0)\n",
" -->\n",
"<!-- Pages: 1 -->\n",
"<svg width=\"134pt\" height=\"260pt\"\n",
" viewBox=\"0.00 0.00 134.00 260.00\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
"<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 256)\">\n",
"<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-256 130,-256 130,4 -4,4\"/>\n",
"<!-- var7782220156096217089 -->\n",
"<g id=\"node1\" class=\"node\">\n",
"<title>var7782220156096217089</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-18\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"99\" y=\"-12.95\" font-family=\"Times,serif\" font-size=\"14.00\">l1</text>\n",
"</g>\n",
"<!-- var7782220156096217090 -->\n",
"<g id=\"node2\" class=\"node\">\n",
"<title>var7782220156096217090</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-90\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"27\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">l2</text>\n",
"</g>\n",
"<!-- var8646911284551352320 -->\n",
"<g id=\"node3\" class=\"node\">\n",
"<title>var8646911284551352320</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-90\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"99\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">x0</text>\n",
"</g>\n",
"<!-- var8646911284551352320&#45;&gt;var7782220156096217089 -->\n",
"<g id=\"edge4\" class=\"edge\">\n",
"<title>var8646911284551352320&#45;&gt;var7782220156096217089</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M99,-71.7C99,-64.41 99,-55.73 99,-47.54\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"102.5,-47.62 99,-37.62 95.5,-47.62 102.5,-47.62\"/>\n",
"</g>\n",
"<!-- var8646911284551352321 -->\n",
"<g id=\"node4\" class=\"node\">\n",
"<title>var8646911284551352321</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"63\" cy=\"-162\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"63\" y=\"-156.95\" font-family=\"Times,serif\" font-size=\"14.00\">x1</text>\n",
"</g>\n",
"<!-- var8646911284551352321&#45;&gt;var7782220156096217090 -->\n",
"<g id=\"edge2\" class=\"edge\">\n",
"<title>var8646911284551352321&#45;&gt;var7782220156096217090</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M54.65,-144.76C50.42,-136.55 45.19,-126.37 40.42,-117.09\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"43.68,-115.79 36,-108.49 37.46,-118.99 43.68,-115.79\"/>\n",
"</g>\n",
"<!-- var8646911284551352321&#45;&gt;var8646911284551352320 -->\n",
"<g id=\"edge3\" class=\"edge\">\n",
"<title>var8646911284551352321&#45;&gt;var8646911284551352320</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M71.35,-144.76C75.58,-136.55 80.81,-126.37 85.58,-117.09\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"88.54,-118.99 90,-108.49 82.32,-115.79 88.54,-118.99\"/>\n",
"</g>\n",
"<!-- var8646911284551352322 -->\n",
"<g id=\"node5\" class=\"node\">\n",
"<title>var8646911284551352322</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"63\" cy=\"-234\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"63\" y=\"-228.95\" font-family=\"Times,serif\" font-size=\"14.00\">x2</text>\n",
"</g>\n",
"<!-- var8646911284551352322&#45;&gt;var8646911284551352321 -->\n",
"<g id=\"edge1\" class=\"edge\">\n",
"<title>var8646911284551352322&#45;&gt;var8646911284551352321</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M63,-215.7C63,-208.41 63,-199.73 63,-191.54\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"66.5,-191.62 63,-181.62 59.5,-191.62 66.5,-191.62\"/>\n",
"</g>\n",
"</g>\n",
"</svg>\n"
],
"text/plain": [
"<graphviz.sources.Source at 0x107815d30>"
]
},
"execution_count": 18,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"# Eliminate sequentially to get a Bayes Net structure\n",
"bayes_net = graph.eliminateSequential(ordering)\n",
"bayes_net.print(\"\\nResulting Symbolic Bayes Net:\\n\")\n",
"\n",
"# Visualize the Bayes Net using Graphviz\n",
"graphviz.Source(bayes_net.dot())"
]
},
{
"cell_type": "code",
"execution_count": 19,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Resulting Symbolic Bayes Tree:\n",
": cliques: 4, variables: 5\n",
"\n",
"Resulting Symbolic Bayes Tree:\n",
"- P( x1 x2)\n",
"\n",
"Resulting Symbolic Bayes Tree:\n",
"| - P( x0 | x1)\n",
"\n",
"Resulting Symbolic Bayes Tree:\n",
"| | - P( l1 | x0)\n",
"\n",
"Resulting Symbolic Bayes Tree:\n",
"| - P( l2 | x1)\n"
]
},
{
"data": {
"image/svg+xml": [
"<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
"<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
" \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
"<!-- Generated by graphviz version 12.0.0 (0)\n",
" -->\n",
"<!-- Title: G Pages: 1 -->\n",
"<svg width=\"170pt\" height=\"188pt\"\n",
" viewBox=\"0.00 0.00 169.99 188.00\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
"<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 184)\">\n",
"<title>G</title>\n",
"<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-184 165.99,-184 165.99,4 -4,4\"/>\n",
"<!-- 0 -->\n",
"<g id=\"node1\" class=\"node\">\n",
"<title>0</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"82.02\" cy=\"-162\" rx=\"34.46\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"82.02\" y=\"-156.95\" font-family=\"Times,serif\" font-size=\"14.00\">x1, x2</text>\n",
"</g>\n",
"<!-- 1 -->\n",
"<g id=\"node2\" class=\"node\">\n",
"<title>1</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"37.02\" cy=\"-90\" rx=\"37.02\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"37.02\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">x0 : x1</text>\n",
"</g>\n",
"<!-- 0&#45;&gt;1 -->\n",
"<g id=\"edge1\" class=\"edge\">\n",
"<title>0&#45;&gt;1</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M71.36,-144.41C66.09,-136.22 59.61,-126.14 53.71,-116.95\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"56.78,-115.26 48.43,-108.74 50.89,-119.05 56.78,-115.26\"/>\n",
"</g>\n",
"<!-- 3 -->\n",
"<g id=\"node4\" class=\"node\">\n",
"<title>3</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"127.02\" cy=\"-90\" rx=\"34.97\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"127.02\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">l2 : x1</text>\n",
"</g>\n",
"<!-- 0&#45;&gt;3 -->\n",
"<g id=\"edge3\" class=\"edge\">\n",
"<title>0&#45;&gt;3</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M92.69,-144.41C97.95,-136.22 104.43,-126.14 110.34,-116.95\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"113.15,-119.05 115.61,-108.74 107.26,-115.26 113.15,-119.05\"/>\n",
"</g>\n",
"<!-- 2 -->\n",
"<g id=\"node3\" class=\"node\">\n",
"<title>2</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"37.02\" cy=\"-18\" rx=\"34.97\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"37.02\" y=\"-12.95\" font-family=\"Times,serif\" font-size=\"14.00\">l1 : x0</text>\n",
"</g>\n",
"<!-- 1&#45;&gt;2 -->\n",
"<g id=\"edge2\" class=\"edge\">\n",
"<title>1&#45;&gt;2</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M37.02,-71.7C37.02,-64.41 37.02,-55.73 37.02,-47.54\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"40.52,-47.62 37.02,-37.62 33.52,-47.62 40.52,-47.62\"/>\n",
"</g>\n",
"</g>\n",
"</svg>\n"
],
"text/plain": [
"<graphviz.sources.Source at 0x111534470>"
]
},
"execution_count": 19,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"# Eliminate using Multifrontal method to get a Bayes Tree structure\n",
"bayes_tree = graph.eliminateMultifrontal(ordering)\n",
"bayes_tree.print(\"\\nResulting Symbolic Bayes Tree:\\n\")\n",
"\n",
"# Visualize the Bayes Tree using Graphviz\n",
"graphviz.Source(bayes_tree.dot())"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "py312",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.12.6"
}
},
"nbformat": 4,
"nbformat_minor": 4
}

133
gtsam/symbolic/doc/SymbolicJunctionTree.ipynb Normal file
View File

@ -0,0 +1,133 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# SymbolicJunctionTree"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"A `SymbolicJunctionTree` (often used interchangeably with Clique Tree in GTSAM documentation) is a data structure used in multifrontal variable elimination. It's derived from an `EliminationTree`.\n",
"\n",
"Key differences from an Elimination Tree:\n",
"* **Nodes are Cliques:** Each node in a Junction Tree represents a *clique* (a group of variables eliminated together), not just a single variable.\n",
"* **Stores Factors:** Nodes (`SymbolicCluster` objects) store the symbolic factors associated with the variables in that clique.\n",
"\n",
"The Junction Tree organizes the factors and variables for efficient multifrontal elimination, which processes variables in these larger cliques simultaneously. The result of eliminating a Junction Tree is a `SymbolicBayesTree`."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/symbolic/doc/SymbolicJunctionTree.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"from gtsam import SymbolicJunctionTree, SymbolicEliminationTree, SymbolicFactorGraph, Ordering\n",
"from gtsam.symbol_shorthand import X, L"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Creating a SymbolicJunctionTree\n",
"\n",
"A Junction Tree is constructed from a `SymbolicEliminationTree`."
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Symbolic Junction Tree:\n",
"- (6) x1 x2 \n",
"Symbolic Junction Tree:\n",
"| - (6) x0 \n",
"Symbolic Junction Tree:\n",
"| | - (2) l1 \n",
"Symbolic Junction Tree:\n",
"| - (2) l2 \n"
]
}
],
"source": [
"# Create a factor graph\n",
"graph = SymbolicFactorGraph()\n",
"graph.push_factor(X(0))\n",
"graph.push_factor(X(0), X(1))\n",
"graph.push_factor(X(1), X(2))\n",
"graph.push_factor(X(0), L(1))\n",
"graph.push_factor(X(1), L(2))\n",
"\n",
"# Define an elimination ordering\n",
"ordering = Ordering([L(1), L(2), X(0), X(1), X(2)]) \n",
"\n",
"# Construct the elimination tree first\n",
"elimination_tree = SymbolicEliminationTree(graph, ordering)\n",
"\n",
"# Construct the junction tree from the elimination tree\n",
"junction_tree = SymbolicJunctionTree(elimination_tree)\n",
"\n",
"# Print the tree structure (text representation)\n",
"junction_tree.print(\"Symbolic Junction Tree:\\n\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"*(Note: The `SymbolicCluster` class represents the nodes within the Junction Tree, containing the factors and frontal/separator keys for that clique. Direct visualization is usually done via the resulting Bayes Tree.)*"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "py312",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.12.6"
}
},
"nbformat": 4,
"nbformat_minor": 4
}

4
gtsam/symbolic/symbolic.i
View File

@ -185,6 +185,8 @@ class SymbolicBayesTreeClique {
const gtsam::SymbolicConditional* conditional() const;
bool isRoot() const;
gtsam::SymbolicBayesTreeClique* parent() const;
size_t nrChildren() const;
gtsam::SymbolicBayesTreeClique* operator[](size_t j) const;
size_t treeSize() const;
size_t numCachedSeparatorMarginals() const;
void deleteCachedShortcuts();
@ -204,7 +206,7 @@ class SymbolicBayesTree {
// Standard Interface
bool empty() const;
size_t size() const;
const SymbolicBayesTree::Roots& roots() const;
const gtsam::SymbolicBayesTreeClique* operator[](size_t j) const;
void saveGraph(string s,

40
gtsam/symbolic/symbolic.md Normal file
View File

@ -0,0 +1,40 @@
# Symbolic Module
The `symbolic` module in GTSAM deals with the *structure* of factor graphs and Bayesian networks, independent of the specific numerical types of factors (like Gaussian or discrete). It allows for analyzing graph connectivity, determining optimal variable elimination orders, and understanding the sparsity structure of the resulting inference objects.
This is crucial for efficient inference, as the symbolic elimination steps determine the computational complexity and memory requirements of the numerical factorization.
The classes here are used primarily to *illustrate* symbolic elimination. Internally, GTSAM does analyze the structure of other factor graph types without explicitly converting to a symbolic factor graph.
## Classes
Here's an overview of the key classes in the `symbolic` module:
### Factor Graph
- **[SymbolicFactor](doc/SymbolicFactor.ipynb)**: Represents the connectivity between a set of variables (keys) in a factor graph, without any specific numerical function associated with it. It defines the hyperedges of the graph.
- **[SymbolicFactorGraph](doc/SymbolicFactorGraph.ipynb)**: A collection of `SymbolicFactor` objects, representing the overall structure of a factor graph.
### Elimination Products
These classes represent the results of symbolic variable elimination:
- **[SymbolicConditional](doc/SymbolicConditional.ipynb)**: Represents the structure of a conditional probability distribution P(Frontals | Parents). It stores the keys of the frontal (conditioned) and parent variables resulting from eliminating one or more variables from a set of factors.
- **[SymbolicBayesNet](doc/SymbolicBayesNet.ipynb)**: A directed acyclic graph composed of `SymbolicConditional` objects, representing the structure of a factorized distribution P(X) = Π P(Xi | Parents(Xi)). Typically results from sequential elimination.
- **[SymbolicBayesTree](doc/SymbolicBayesTree.ipynb)**: A tree structure where each node (`SymbolicBayesTreeClique`) represents a `SymbolicConditional` P(Frontals | Separator). This is the result of multifrontal elimination and is the underlying structure for efficient incremental updates (iSAM) and exact marginal computation.
- **[SymbolicBayesTreeClique](doc/SymbolicBayesTreeClique.ipynb)**: Represents a single clique (node) within a `SymbolicBayesTree`, containing a `SymbolicConditional`.
### Elimination Structures
These classes represent intermediate structures used during the elimination process:
- **[SymbolicEliminationTree](doc/SymbolicEliminationTree.ipynb)**: Represents the dependency structure of sequential variable elimination. Each node corresponds to eliminating a single variable.
- **[SymbolicJunctionTree](doc/SymbolicJunctionTree.ipynb)** (Clique Tree): An intermediate structure used in multifrontal elimination, derived from an elimination tree. Each node (`SymbolicCluster`) represents a clique of variables eliminated together and stores the *factors* associated with that clique.
**Importance**
Performing symbolic analysis can be used for:
1. Choosing an Optimal Ordering: Finding an ordering that minimizes fill-in (new connections created during elimination) is key to efficient numerical factorization.
2. Predicting Computational Cost: The structure of the Bayes net or Bayes tree determines the complexity of subsequent numerical operations like solving or marginalization.
3. Memory Allocation: Knowing the structure allows for pre-allocation of memory for the numerical factorization.
The symbolic module provides the foundation for GTSAM's efficient inference algorithms.

3
myst.yml
View File

@ -19,6 +19,9 @@ project:
- file: ./gtsam/nonlinear/nonlinear.md
children:
- pattern: ./gtsam/nonlinear/doc/*
- file: ./gtsam/symbolic/symbolic.md
children:
- pattern: ./gtsam/symbolic/doc/*
- file: ./gtsam/navigation/navigation.md
children:
- pattern: ./gtsam/navigation/doc/*