Fan/prototype hybrid tr #42

ProfFan · 2022-03-11T21:00:17Z

Prototyping a GTSAM-native hybrid graph that uses the CLG formulation described in Murphy02 and Salmeron18.

Multifrontal just works, though no actual functionality is added yet everything works.

I believe this is a better way to do hybrid, since everything now lies in accordance with other modules (discrete, linear, etc).

Also this is also how LinearContainerFactor is designed...

dellaert

I don’t understand: is this a completely new framework? How does it relate to what we already have?

ProfFan

Let me explain what is happening here.

In a historical perspective, when we merged in DCSAM code we started by making HybridFactorGraph there a FactorGraph<T> in GTSAM. However, that class is not designed in that way. If you look at that you can see that it is not a children of FactorGraph.

The native way in GTSAM how we make Factor graphs is by inheriting FactorGraph<DERIVED>. By doing this you automatically gets a container of DERIVED factors that comes with all the goodies. However, since the DCSAM version of HybridFactorGraph is not using this underlying storage, we have to work around the existing methods in FactorGraph, which is very tedious and error-prone.

A proper implementation, as I propose here, is to just use the underlying storage of FactorGraph and build logic only in the EliminateHybrid method. This allows us to have a minimal change in GTSAM to enable the same algorithms in DCSAM.

Actually, everything in this PR except the elimination functions are merely boilerplate: they 100% resembles their counterparts in linear and discrete.

ProfFan · 2022-03-11T21:35:11Z

gtsam/hybrid/CGMixtureFactor.h

+
+namespace gtsam {
+
+class CGMixtureFactor : HybridFactor {


This will be analogous to the current DCMixtureFactor

gtsam/hybrid/HybridBayesNet.h

ProfFan · 2022-03-11T21:36:06Z

gtsam/hybrid/HybridBayesTree.cpp

+
+namespace gtsam {
+
+// Instantiate base class


No need to look at this

gtsam/hybrid/HybridBayesTree.h

ProfFan · 2022-03-11T21:39:28Z

gtsam/hybrid/HybridConditional.h

+ * - DiscreteConditional
+ * - GaussianMixture
+ * - GaussianConditional
+ */


HybridConditional is a type-erased wrapper for all:

DiscreteConditional

GaussianMixture

GaussianConditional

Gist is that by unifying all these types the only work need in HybridBayesTree is boilerplate (same to linear and discrete), the hard work (type-based dispatch) is offloaded to this class.

This eliminates the necessity of AbstractConditional, which is good because now we don't need to touch other modules, especially inference, at all.

gtsam/hybrid/HybridDiscreteFactor.h

ProfFan · 2022-03-11T21:41:49Z

gtsam/hybrid/HybridFactor.h

+/**
+ * Base class for hybrid probabilistic factors
+ */
+class GTSAM_EXPORT HybridFactor: public Factor {


Just the base for all hybrid factors

ProfFan · 2022-03-11T21:43:29Z

gtsam/hybrid/HybridFactorGraph.cpp

+
+/* ************************************************************************ */
+std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>  //
+EliminateHybrid(const HybridFactorGraph& factors,


Because we are in the Conditional Gaussian regime there are only
few cases:

continuous variable, we make a GM if there are hybrid factors;

continuous variable, we make a GF if there are no hybrid factors;

discrete variable, no continuous factor is allowed (escapes CG regime), so we panic

if discrete only we do the discrete elimination.

We can know which variable is discrete by querying the factors.

ProfFan · 2022-03-11T21:47:11Z

gtsam/hybrid/tests/testHybridConditional.cpp

+  GTSAM_PRINT(*result.first);
+}
+
+TEST(HybridFactorGraph, eliminateMultifrontal) {


By employing this scheme we don't need any special code for handling the templated JunctionTree and BayesTrees, so eliminateMultifrontal just work. In contrast, I spent tens of hours to make the old code to compile with JunctionTree and failed.

dellaert · 2022-03-11T23:37:17Z

OK, these explanations shed a lot of light, so thank you! I will do an in-depth look tomorrow, and then we can talk.

gtsam/hybrid/CLGaussianConditional.h

dellaert

I took 40 minutes to review. Please address every comment by either implementing suggestion and resolving OR providing a rationale and not resolving.

gtsam/CMakeLists.txt

gtsam/hybrid/CGMixtureFactor.cpp

dellaert · 2022-03-15T15:11:04Z

gtsam/hybrid/HybridFactor.h

+  typedef boost::shared_ptr<HybridFactor> shared_ptr; ///< shared_ptr to this class
+  typedef Factor Base; ///< Our base class
+
+  bool isDiscrete_ = false;


Would each derived class not know this automatically?

This is to avoid the need to dynamic casts each time we need to know what is the type of one factor.

gtsam/hybrid/HybridFactor.h

gtsam/hybrid/HybridFactorGraph.h

gtsam/hybrid/HybridGaussianFactor.cpp

gtsam/hybrid/HybridGaussianFactor.h

gtsam/hybrid/tests/CMakeLists.txt

gtsam/hybrid/CGMixtureFactor.h

ProfFan · 2022-03-24T00:20:20Z

@dellaert I now believe I have full multifrontal elimination working. The code still needs some cleaning up though.

BTW, I am now thinking a bit more about the decision to use DecisionTree as the base structure. Maybe a hash table keyed on the discrete assignment will be better both in performance and in difficulty of programming, if we have pruning?

ProfFan · 2022-03-24T00:21:19Z

@dellaert @varunagrawal Feel free to play with the code while I add new unit tests :)

varunagrawal · 2022-03-24T15:26:17Z

Also, I guess it would be a good idea to add a massive comment on HybridConditional about how the type-erasure works? Basically explain why we typecast everything to HybridConditional in order to make things seamless.

dellaert

some comments

dellaert · 2022-03-26T15:40:18Z

gtsam/hybrid/tests/testHybridConditional.cpp

@@ -0,0 +1,273 @@
+/* ----------------------------------------------------------------------------


Many tests should be in testHybridFactorGraph

dellaert · 2022-03-26T15:44:20Z

gtsam/hybrid/tests/testHybridConditional.cpp

+                               X(0), Z_3x1, I_3x3, X(1), I_3x3),
+                           boost::make_shared<GaussianConditional>(
+                               X(0), Vector3::Ones(), I_3x3, X(1), I_3x3)));
+  GTSAM_PRINT(clgc);


Convert print to real tests.

dellaert · 2022-03-26T15:46:03Z

gtsam/hybrid/tests/testHybridConditional.cpp

+                               X(0), Z_3x1, I_3x3, X(1), I_3x3),
+                           boost::make_shared<GaussianConditional>(
+                               X(0), Vector3::Ones(), I_3x3, X(1), I_3x3)));
+  GTSAM_PRINT(clgc);


use curly braces for discrete keys

dellaert · 2022-03-26T15:47:33Z

gtsam/hybrid/tests/testHybridConditional.cpp

+
+  DiscreteKey c1(C(1), 2);
+
+  hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));


don't copy paste

dellaert · 2022-03-26T15:51:19Z

gtsam/hybrid/tests/testHybridConditional.cpp

+
+  HybridFactorGraph hfg;
+
+  hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));


Add comments to show example.

…id-tr

ProfFan · 2022-05-03T18:42:36Z

gtsam/hybrid/HybridBayesTree.h

+  boost::shared_ptr<CliqueType> clique;
+
+  BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& clique)
+      : clique(clique) {


make it a constructor for Base

ProfFan · 2022-05-03T18:43:19Z

gtsam/hybrid/HybridConditional.h

+
+#pragma once
+
+#include <gtsam/discrete/DiscreteConditional.h>


Clean them up

Check other headers

…id-tr

gtsam/hybrid/GaussianMixture.cpp

gtsam/hybrid/GaussianMixture.h

gtsam/hybrid/HybridFactor.cpp

varunagrawal · 2022-05-16T14:31:46Z

gtsam/hybrid/HybridFactorGraph.cpp

+
+/* ************************************************************************ */
+std::pair<HybridConditional::shared_ptr, HybridFactor::shared_ptr>  //
+EliminateHybrid(const HybridFactorGraph &factors, const Ordering &frontalKeys) {


We should really consider breaking this method up into smaller chunks. There is already a logical demarcation.

gtsam/hybrid/HybridGaussianFactor.h

varunagrawal · 2022-05-16T14:34:33Z

gtsam/hybrid/tests/testHybridConditional.cpp

@@ -0,0 +1,273 @@
+/* ----------------------------------------------------------------------------


varunagrawal · 2022-05-16T14:34:46Z

gtsam/hybrid/tests/Switching.h

+  return {new_order, levels};
+}
+
+}  // namespace gtsam


Please add a newline.

dellaert · 2022-05-21T17:25:29Z

@ProfFan after you address these relatively minor comments (please do so ASAP) please close this PR and create a PR from this branch directly to GTSAM develop (after merging in that develop).

…id-tr

ProfFan added 2 commits March 11, 2022 15:51

Fix typo

ea4ebb6

Prototype a type-erased hybrid machinery

0567303

ProfFan requested review from varunagrawal and dellaert March 11, 2022 21:00

dellaert reviewed Mar 11, 2022

View reviewed changes

ProfFan commented Mar 11, 2022

View reviewed changes

ProfFan requested a review from dellaert March 11, 2022 22:35

ProfFan added 9 commits March 11, 2022 19:20

Add better mocking for actual elimination

efa37ff

Fix compilation error

3aac52c

Add shorthand for inserting raw JacobianFactor

53551e0

Add full elimination

095f6ad

More mock-ups added

2bae286

Added mixture factor functionality

ee4f9d1

Added more mockups and color output of the elimination process

5ea614a

add more comments

0aeb596

even better printing and comments

fe5dde7

varunagrawal reviewed Mar 14, 2022

View reviewed changes

gtsam/hybrid/CLGaussianConditional.h Outdated Show resolved Hide resolved

dellaert reviewed Mar 15, 2022

View reviewed changes

ProfFan added 5 commits March 15, 2022 12:50

Address comments

f237438

Missing pragma onces

36ee4ce

Now we have real multifrontal!

7ad2031

Fully working Multifrontal

d42fc21

Add a test for EliminateSequential

1e8aae3

ProfFan added 3 commits March 23, 2022 20:36

Address comments

b4f8eea

Remove warning

4f8dfeb

Address comments

293ef61

ProfFan requested a review from dellaert March 24, 2022 00:42

ProfFan added 2 commits March 25, 2022 17:14

Add Python bindings

d2dc620

Added more Python examples

7f2fa61

dellaert reviewed Mar 26, 2022

View reviewed changes

ProfFan added 4 commits March 26, 2022 23:13

Added plotting for nested dissection

0f69b4c

Working iSAM for Hybrid

a9c1d43

Merge remote-tracking branch 'origin/develop' into fan/prototype-hybr…

1fe7e74

…id-tr

Fix compile

2de3169

ProfFan commented May 3, 2022

View reviewed changes

ProfFan and others added 5 commits May 8, 2022 11:45

Merge remote-tracking branch 'origin/develop' into fan/prototype-hybr…

1bf9952

…id-tr

Fix the enable_if_t absence

4e481ea

Remove most debug messages

d012bcd

Bump version

30412b8

Merge pull request borglab#1196 from borglab/release/4.2a7

da00870

varunagrawal reviewed May 16, 2022

View reviewed changes

ProfFan added 2 commits May 21, 2022 13:56

Merge remote-tracking branch 'origin/develop' into fan/prototype-hybr…

d5fd279

…id-tr

Address Varun's comments

2c4990b

ProfFan closed this May 21, 2022

ProfFan mentioned this pull request May 21, 2022

Hybrid Factor Graph Implementation borglab/gtsam#1203

Merged

ProfFan added 5 commits May 21, 2022 15:04

Fix compile error

04593cc

Don't crash anymore

2ae2cb6

Don't use Python dict method since it is not

b8299d7

Trying to make MSVC happy

74af969

Address PR comments

b215d3a

varunagrawal reopened this May 24, 2022

include missing headers for msvc and fix warning

e36583e

varunagrawal force-pushed the fan/prototype-hybrid-tr branch from 453be44 to e36583e Compare May 24, 2022 02:20

varunagrawal closed this May 24, 2022

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fan/prototype hybrid tr #42

Fan/prototype hybrid tr #42

ProfFan commented Mar 11, 2022 •

edited

Loading

dellaert left a comment

ProfFan left a comment

ProfFan Mar 11, 2022

ProfFan Mar 11, 2022

ProfFan Mar 11, 2022

ProfFan Mar 11, 2022

ProfFan Mar 11, 2022

ProfFan Mar 11, 2022

ProfFan Mar 11, 2022

dellaert commented Mar 11, 2022

dellaert left a comment

dellaert Mar 15, 2022

ProfFan Mar 24, 2022

ProfFan commented Mar 24, 2022

ProfFan commented Mar 24, 2022 •

edited

Loading

varunagrawal commented Mar 24, 2022

dellaert left a comment

dellaert Mar 26, 2022

varunagrawal May 16, 2022

dellaert Mar 26, 2022

dellaert Mar 26, 2022

dellaert Mar 26, 2022

dellaert Mar 26, 2022

ProfFan May 3, 2022

ProfFan May 3, 2022

ProfFan May 3, 2022

varunagrawal May 16, 2022

varunagrawal May 16, 2022

varunagrawal May 16, 2022

dellaert commented May 21, 2022

		@@ -0,0 +1,273 @@
		/* ----------------------------------------------------------------------------


		DiscreteKey c1(C(1), 2);

		hfg.add(JacobianFactor(X(0), I_3x3, Z_3x1));


		HybridFactorGraph hfg;

		hfg.add(JacobianFactor(X(0), I_3x3, X(1), -I_3x3, Z_3x1));


		#pragma once

		#include <gtsam/discrete/DiscreteConditional.h>

Fan/prototype hybrid tr #42

Fan/prototype hybrid tr #42

Conversation

ProfFan commented Mar 11, 2022 • edited Loading

dellaert left a comment

Choose a reason for hiding this comment

ProfFan left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dellaert commented Mar 11, 2022

dellaert left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ProfFan commented Mar 24, 2022

ProfFan commented Mar 24, 2022 • edited Loading

varunagrawal commented Mar 24, 2022

dellaert left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dellaert commented May 21, 2022

ProfFan commented Mar 11, 2022 •

edited

Loading

ProfFan commented Mar 24, 2022 •

edited

Loading