towards fixing static array factor eval

Author: dehann

test/testMultiHypo3Door.jl

@@ -52,7 +52,20 @@ addFactor!(fg, [:l3], Prior(Normal(l3, lm_prior_noise)))
 addVariable!(fg, :x0, ContinuousScalar, N=n_samples)
 
 # Make first "door" measurement
-addFactor!(fg, [:x0; :l0; :l1; :l2; :l3], LinearRelative(Normal(0, meas_noise)), multihypo=[1.0; (1/4 for _=1:4)...])
+f1 = addFactor!(fg, [:x0; :l0; :l1; :l2; :l3], LinearRelative(Normal(0, meas_noise)), multihypo=[1.0; (1/4 for _=1:4)...])
+
+# make sure approxConv is as expected
+@test isInitialized.(fg, [:l0;:l1;:l2;:l3]) |> all
+X0 = approxConvBelief(fg, getLabel(f1), :x0)
+# smpls = sampleFactor(fg, f1.label,200)
+
+# should have four equal sized peaks at landmark locations
+@test 0.1 < X0([l0])[1]
+@test 0.1 < X0([l1])[1]
+@test 0.1 < X0([l2])[1]
+@test 0.1 < X0([l3])[1]
+
+
 
 # Add second pose
 addVariable!(fg, :x1, ContinuousScalar, N=n_samples)