Having Trouble Using BearingRangeFactor3D

I have some trouble using BearingRangeFactor3D. In the following example with large elevation noise, I got unexpected results.

typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;

NonlinearFactorGraph graph;

// Add prior
Values initialEstimate;
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Isotropic::Sigma(6, 1e-6);
Symbol x0('x', 0);
graph.add(PriorFactor<Pose3>(x0, Pose3(), priorNoise));
initialEstimate.insert(x0, Pose3());

// Add large elevation noise
Vector3 sigmas;
sigmas << 1e-6, 0.3, 1e-6;
noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(sigmas);

// Add bearing range factors
for (int i = 0; i < 3; ++i) {
    Symbol l('l', i);
    Unit3 bearing(cos(i), sin(i), 0);
    graph.add(BearingRangeFactor3D(x0, l, bearing, 1.0, measurementNoise));
    initialEstimate.insert(l, bearing.point3());
}

LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
Values result = optimizer.optimize();

Marginals marginals(graph, result, Marginals::Factorization::QR);
print(marginals.marginalCovariance(Symbol('l', 0)), "l0 covariance");
print(marginals.marginalCovariance(Symbol('l', 1)), "l1 covariance");
print(marginals.marginalCovariance(Symbol('l', 2)), "l2 covariance");

I expected the uncertainty of z to be large, but the results were different.

l0 covariance[
          2e-12           0           0;
                  0        0.09 1.30967e-22;
                  0 1.30967e-22       3e-12
  ]
l1 covariance[
     2.70807e-12 -4.54649e-13  -2.9388e-23;
        -4.54649e-13  2.29193e-12  1.88698e-23;
         -2.9388e-23  1.88698e-23         0.09
  ]
l2 covariance[
    2.82682e-12 3.78401e-13           0;
        3.78401e-13 2.17318e-12           0;
                  0           0        0.09
  ]

Any help will be appreciated.

Comments (3)