From 17237becb46e5e0ee12855f3cfb5f85d1b49c72d Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Thu, 24 Mar 2022 11:58:06 -0400
Subject: [PATCH] print parents when printing GaussianConditional

---
 gtsam/linear/GaussianConditional.cpp          | 10 +++--
 gtsam/linear/GaussianConditional.h            |  4 +-
 .../linear/tests/testGaussianConditional.cpp  | 38 +++++++++++++++++++
 3 files changed, 47 insertions(+), 5 deletions(-)

diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp
index cf3f78b73..8d616443a 100644
--- a/gtsam/linear/GaussianConditional.cpp
+++ b/gtsam/linear/GaussianConditional.cpp
@@ -89,11 +89,15 @@ namespace gtsam {
 
   /* ************************************************************************ */
   void GaussianConditional::print(const string &s, const KeyFormatter& formatter) const {
-    cout << s << "  Conditional density ";
+    cout << s << "Conditional density [";
     for (const_iterator it = beginFrontals(); it != endFrontals(); ++it) {
-      cout << (boost::format("[%1%]")%(formatter(*it))).str() << " ";
+      cout << (boost::format("%1%")%(formatter(*it))).str() << " ";
     }
-    cout << endl;
+    cout << "|";
+    for (const_iterator it = beginParents(); it != endParents(); ++it) {
+      cout << " " << (boost::format("%1%")%(formatter(*it))).str();
+    }
+    cout << "]" << endl;
     cout << formatMatrixIndented("  R = ", R()) << endl;
     for (const_iterator it = beginParents() ; it != endParents() ; ++it) {
       cout << formatMatrixIndented((boost::format("  S[%1%] = ")%(formatter(*it))).str(), getA(it))
diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h
index 6dd278536..e8aae4c31 100644
--- a/gtsam/linear/GaussianConditional.h
+++ b/gtsam/linear/GaussianConditional.h
@@ -109,8 +109,8 @@ namespace gtsam {
     /// @{
 
     /** print */
-    void print(const std::string& = "GaussianConditional",
-      const KeyFormatter& formatter = DefaultKeyFormatter) const override;
+    void print(const std::string& = "", const KeyFormatter& formatter =
+                                            DefaultKeyFormatter) const override;
 
     /** equals function */
     bool equals(const GaussianFactor&cg, double tol = 1e-9) const override;
diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp
index 8525cf9e0..52771960a 100644
--- a/gtsam/linear/tests/testGaussianConditional.cpp
+++ b/gtsam/linear/tests/testGaussianConditional.cpp
@@ -40,6 +40,7 @@ using namespace gtsam;
 using namespace std;
 using namespace boost::assign;
 using symbol_shorthand::X;
+using symbol_shorthand::Y;
 
 static const double tol = 1e-5;
 
@@ -396,6 +397,43 @@ TEST(GaussianConditional, sample) {
   // EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5));
 }
 
+/* ************************************************************************* */
+TEST(GaussianConditional, Print) {
+  Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();
+  Matrix A2 = (Matrix(2, 2) << 5., 6., 7., 8.).finished();
+  const Vector2 b(20, 40);
+  const double sigma = 3;
+
+  auto conditional =
+      GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), b, sigma);
+
+  // Test printing for single parent.
+  std::string expected =
+    "Conditional density [x0 | x1]\n"
+    "  R = [ 1 0 ]\n"
+    "      [ 0 1 ]\n"
+    "  S[x1] = [ -1 -2 ]\n"
+    "          [ -3 -4 ]\n"
+    "  d = [ 20 40 ]\n"
+    "isotropic dim=2 sigma=3\n";
+  EXPECT(assert_print_equal(expected, conditional));
+
+  // Test printing for multiple parents.
+  auto conditional2 = GaussianConditional::FromMeanAndStddev(X(0), A1, Y(0), A2,
+                                                             Y(1), b, sigma);
+  std::string expected2 =
+    "Conditional density [x0 | y0 y1]\n"
+    "  R = [ 1 0 ]\n"
+    "      [ 0 1 ]\n"
+    "  S[y0] = [ -1 -2 ]\n"
+    "          [ -3 -4 ]\n"
+    "  S[y1] = [ -5 -6 ]\n"
+    "          [ -7 -8 ]\n"
+    "  d = [ 20 40 ]\n"
+    "isotropic dim=2 sigma=3\n";
+  EXPECT(assert_print_equal(expected2, conditional2));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;