Hybrid/simplify

[dellaert]

This PR implements the scheme in the (included) doc/Hybrid.pdf, where GaussianMixture::likelihood returns a single GaussianMixtureFactor instance, that encodes the log-normalization constant as well - by adding a row to A and b, respectively. This is only needed when the noise is mode-dependent.

We also now enforce error>0, and define the normalization constant for the GaussianMixture as the max of all normalization constants of its components.

With this scheme, there is no more need for FactorAndConstant and GraphAndConstant, which have been removed.

To make the elimination work, I use the correct formula in the hybrid elimination:

q(μ;m) = exp(-error(μ;m)) * sqrt(det(2π Σ_m))

There are two tests I had to disable:

1. a pruning test that now yields infinity. @varunagrawal look into that?
2. a complex example where both noise model and motion model are governed by discrete variables, fails the ratio test after elimination. Please take a detailed look there whether I made a silly mistake? Hoping this one does not point to yet another issue with elimination.

[dellaert]

OK @varunagrawal , @ProfFan I fixed the last issue. One of the things GraphAndConstant did was to correct for the newly created GaussianConditional normalization factors. I now did this in a second path in the hybrid elimination step, by using HessianFactor::constantTerm().

Could you review? A bit of a deadline, as I want to land 4.2, and this was the missing piece. Also, @varunagrawal , consult on the pruning? I had to change the regression tests in testHybridBayesNet, but I believe the results to be correct now (and not before).

[ProfFan]

How could this branch be taken?

[dellaert]

Yeah, whenever we do BayesTree there is some way (that I don't yet understand) that nullptrs are used, and we are dealing with all over the place. Can you explain? Or @varunagrawal ?

[varunagrawal]

This could be broken down into smaller PRs...

[varunagrawal]

@dellaert I'm a bit confused. Have we gone from using probabilities errors in the discrete trees to logProbabilities? If that's the case, then the pruning methods need to be updated.

[dellaert]

@dellaert I'm a bit confused. Have we gone from using probabilities errors in the discrete trees to logProbabilities? If that's the case, then the pruning methods need to be updated.

Yeah, this is why I asked your consult. I'm looking at it now.

[dellaert]

@dellaert I'm a bit confused. Have we gone from using probabilities errors in the discrete trees to logProbabilities? If that's the case, then the pruning methods need to be updated.

Yeah, this is why I asked your consult. I'm looking at it now.

Unfortunately, I am not able to decode the pruning code. Can we chat?

[varunagrawal]

@dellaert I'm a bit confused. Have we gone from using probabilities errors in the discrete trees to logProbabilities? If that's the case, then the pruning methods need to be updated.

Yeah, this is why I asked your consult. I'm looking at it now.

Unfortunately, I am not able to decode the pruning code. Can we chat?

Yeah the fix is easy: the pruning code sets the probability to 0, so log(0) = -inf. We have to just update the regression parameter.

[varunagrawal]

@dellaert I'm making a PR for this now.

No please don't, I'd like to chat

[dellaert]

@dellaert I'm making a PR for this now.

@varunagrawal No please don't, I'd like to chat to get on the same page first.

[dellaert]

OK,

[varunagrawal]

@dellaert about this TODO, the explanation is given above. The assembleGraphTree method causes an empty GaussianFactorGraph to be created when there is a nullptr, hence we need to explicitly check and remove any factor graph with nullptrs (by setting it to nullptr).