Covariances for GTSAM point-features

gtsam_point_covariance_test.py

import gtsam
import gtsam_unstable

def point_covaraince_test():
    
    # graph with point-features
    # ---------------------------
    
    p_obs = gtsam.Point2(1, 2)
    cov = np.array(((2., -1.), (-1., 3.)))
    p_0 = gtsam.Pose2(0,0,np.pi/2)

    graph = gtsam.NonlinearFactorGraph()
    initial = gtsam.Values()

    graph.add(gtsam.PriorFactorPose2(0, p_0, gtsam.noiseModel.Diagonal.Sigmas(np.array((1e-5, 1e-5, 1e-5)))))
    graph.add(gtsam.PriorFactorPoint2(1, p_obs, gtsam.noiseModel.Gaussian.Covariance(cov)))
    graph.add(gtsam_unstable.DeltaFactor(0, 1, gtsam.Point2(1, -1), gtsam.noiseModel.Diagonal.Sigmas(np.array((120, 120)))))

    initial.insert_pose2(0, p_0)
    initial.insert_point2(1, p_obs)


    params = gtsam.LevenbergMarquardtParams()
    optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
    result = optimizer.optimize()
    marginals = gtsam.Marginals(graph, result)

    posterior_0 = marginals.marginalCovariance(0)
    posterior_1 = marginals.marginalCovariance(1)
    result_0 = result.atPose2(0)
    result_1 = result.atPoint2(1)

    #plot.plot_trajectory(0, result, marginals=marginals)

    
    # graph with pose-features
    # ---------------------------
    p_obs = gtsam.Pose2(1, 2, np.pi / 2)
    cov = np.array(((2., -1.), (-1., 3.)))
    p_0 = gtsam.Pose2(0, 0, np.pi / 2)

    graph = gtsam.NonlinearFactorGraph()
    initial = gtsam.Values()

    graph.add(gtsam.PriorFactorPose2(0, p_0, gtsam.noiseModel.Diagonal.Sigmas(np.array((1e-5, 1e-5, 1e-5)))))
    graph.add(gtsam.PoseTranslationPrior2D(1, p_obs, gtsam.noiseModel.Gaussian.Covariance(cov)))
    graph.add(gtsam.BetweenFactorPose2(0, 1, gtsam.Pose2(1, -1, 0), gtsam.noiseModel.Diagonal.Sigmas(np.array((120, 120, 12)))))

    initial.insert_pose2(0, p_0)
    initial.insert_pose2(1, p_obs)

    params = gtsam.LevenbergMarquardtParams()
    optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
    result = optimizer.optimize()
    marginals = gtsam.Marginals(graph, result)

    posterior2_0 = marginals.marginalCovariance(0)
    posterior2_1 = marginals.marginalCovariance(1)
    result2_0 = result.atPose2(0)
    result2_1 = result.atPose2(1)
    
    # from plot.py
    gRp = result2_1.rotation().matrix()
    pPp = posterior2_1[0:2, 0:2]
    posterior2_1_global = np.matmul(np.matmul(gRp, pPp), gRp.T)
    
    assert np.allclose(posterior2_1_global, posterior_1)

  
if __name__ == '__main__':
    point_covaraince_test()