test_PinholeCamera.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
 
PinholeCamera unit tests.
Author: Fan Jiang
"""
import unittest
from math import pi
 
import numpy as np
 
import gtsam
from gtsam.utils.test_case import GtsamTestCase
 
 
class TestPinholeCamera(GtsamTestCase):
    """
    Tests if we can correctly get the camera Jacobians in Python
    """
    def test_jacobian(self):
        cam1 = gtsam.PinholeCameraCal3Bundler()
 
        # order is important because Eigen is column major!
        Dpose = np.zeros((2, 6), order='F')
        Dpoint = np.zeros((2, 3), order='F')
        Dcal = np.zeros((2, 3), order='F')
        cam1.project(np.array([1, 1, 1]), Dpose, Dpoint, Dcal)
 
        self.gtsamAssertEquals(Dpoint, np.array([[1, 0, -1], [0, 1, -1]]))
 
        self.gtsamAssertEquals(
            Dpose,
            np.array([
                [1., -2., 1., -1., 0., 1.],  #
                [2., -1., -1., 0., -1., 1.]
            ]))
 
        self.gtsamAssertEquals(Dcal, np.array([[1., 2., 4.], [1., 2., 4.]]))
 
 
if __name__ == "__main__":
    unittest.main()