printErrors method for HybridFactorGraph

[varunagrawal]

Added printErrors to HybridNonlinearFactorGraph and HybridGaussianFactorGraph to aid with debugging.

Also added error method to HybridBayesNet.

Example output for a HybridGaussianFactorGraph with 3 factors. Note that it prints the factors along with the error right after each factor.

HybridGaussianFactorGraph: size: 3
Factor 0: Hybrid [x0; m0]{
 Choice(m0) 
 0 Leaf :
  A[x0] = [
	-1, -0, -0;
	-0, -1, -0;
	-0, -0, -1;
	0, 0, 0
]
  b = [      -1      -2      -5 5.25652 ]
  Noise model: diagonal sigmas [10; 10; 10; 1];
 1 Leaf :
  A[x0] = [
	-1, -0, -0;
	-0, -1, -0;
	-0, -0, -1
]
  b = [ -1 -2 -5 ]
isotropic dim=3 sigma=0.1
}
error =  Choice(m0) 
 0 Leaf 13.815511
 1 Leaf    0


Factor 1: p(x0)
  R = [ 1 0 0 ]
      [ 0 1 0 ]
      [ 0 0 1 ]
  d = [ 1 2 5 ]
  mean: 1 elements
  x0: 1 2 5
isotropic dim=3 sigma=0.5
error = 0

Factor 2:  P( m0 ):
 Choice(m0) 
 0 Leaf  0.4
 1 Leaf  0.6
error =  Choice(m0) 
 0 Leaf 0.91629073
 1 Leaf 0.51082562

[dellaert]

LGTM!
Might be nice to show example output in the MR comment before merging, for posterity,