Fix HybridGaussianFactorGraph error

[varunagrawal]

With Frank:

• Store a FactorAndConstant in GaussianMixtureFactor, to properly account for different noise models for different modes. Small extra compute if they are all the same.

Frank fixed the ratios, and the marginals work out as well:

• used HybridValues where possible.
• define, unit test, and use logNormalizationConstant
• define virtual error method.
• because HybridDiscreteFactor now also defines an error, unequal mode priors are also properly handled (which was not the case before).

Now we are in a position where the factor graph is proportional to the joint, and we can think about elimination.

[dellaert]

I will continue working in this branch. Please do not force-push ever.

[varunagrawal]

You got it!

[dellaert]

Okidoke, I fixed the ratios, and the marginals work out as well. My main changes:

• define, unit test, and use logNormalizationConstant
• define virtual error method.
• because HybridDiscreteFactor now also defines an error, unequal mode priors are also properly handled (which was not the case before).

Now we are in a position where the factor graph is proportional to the joint, and we can think about elimination.

[dellaert]

I looked over everything - but of course I made a lot of the changes.

[varunagrawal]

Awesome sauce!

[varunagrawal]

I would probably have had one PR for all the virtual methods and overloads and another PR for the logNormalizationConstant to help keep track. Reviewing this was a bit of work.

[dellaert]

Will merge and fix comments in next PR.