test_Factors.py

Go to the documentation of this file.

"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
 
See LICENSE for the license information
 
Unit tests for various factors.
 
Author: Varun Agrawal
"""
import unittest
 
import gtsam
import numpy as np
from gtsam.utils.test_case import GtsamTestCase
 
 
class TestNonlinearEquality2Factor(GtsamTestCase):
    """
    Test various instantiations of NonlinearEquality2.
    """
 
    def test_point3(self):
        """Test for Point3 version."""
        factor = gtsam.NonlinearEquality2Point3(0, 1)
        error = factor.evaluateError(gtsam.Point3(0, 0, 0),
                                     gtsam.Point3(0, 0, 0))
 
        np.testing.assert_allclose(error, np.zeros(3))
 
 
class TestJacobianFactor(GtsamTestCase):
    """Test JacobianFactor"""
 
    def test_gaussian_factor_graph(self):
        """Test construction from GaussianFactorGraph."""
        gfg = gtsam.GaussianFactorGraph()
        jf = gtsam.JacobianFactor(gfg)
        self.assertIsInstance(jf, gtsam.JacobianFactor)
 
        nfg = gtsam.NonlinearFactorGraph()
        nfg.push_back(gtsam.PriorFactorDouble(1, 0.0, gtsam.noiseModel.Isotropic.Sigma(1, 1.0)))
        values = gtsam.Values()
        values.insert(1, 0.0)
        gfg = nfg.linearize(values)
        jf = gtsam.JacobianFactor(gfg)
        self.assertIsInstance(jf, gtsam.JacobianFactor)
 
 
if __name__ == "__main__":
    unittest.main()