125 lines
4.0 KiB
Python
125 lines
4.0 KiB
Python
"""
|
|
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
|
Atlanta, Georgia 30332-0415
|
|
All Rights Reserved
|
|
|
|
See LICENSE for the license information
|
|
|
|
Unit tests for IMU numerical_derivative module.
|
|
Author: Frank Dellaert & Joel Truher
|
|
"""
|
|
# pylint: disable=invalid-name, no-name-in-module
|
|
|
|
import unittest
|
|
import numpy as np
|
|
|
|
from gtsam import Pose3, Rot3, Point3
|
|
from gtsam.utils.numerical_derivative import numericalDerivative11, numericalDerivative21, numericalDerivative22, numericalDerivative33
|
|
|
|
|
|
class TestNumericalDerivatives(unittest.TestCase):
|
|
def test_numericalDerivative11_scalar(self):
|
|
# Test function of one variable
|
|
def h(x):
|
|
return x ** 2
|
|
|
|
x = np.array([3.0])
|
|
# Analytical derivative: dh/dx = 2x
|
|
analytical_derivative = np.array([[2.0 * x[0]]])
|
|
|
|
# Compute numerical derivative
|
|
numerical_derivative = numericalDerivative11(h, x)
|
|
|
|
# Check if numerical derivative is close to analytical derivative
|
|
np.testing.assert_allclose(
|
|
numerical_derivative, analytical_derivative, rtol=1e-5
|
|
)
|
|
|
|
def test_numericalDerivative11_vector(self):
|
|
# Test function of one vector variable
|
|
def h(x):
|
|
return x ** 2
|
|
|
|
x = np.array([1.0, 2.0, 3.0])
|
|
# Analytical derivative: dh/dx = 2x
|
|
analytical_derivative = np.diag(2.0 * x)
|
|
|
|
numerical_derivative = numericalDerivative11(h, x)
|
|
|
|
np.testing.assert_allclose(
|
|
numerical_derivative, analytical_derivative, rtol=1e-5
|
|
)
|
|
|
|
def test_numericalDerivative21(self):
|
|
# Test function of two variables, derivative with respect to first variable
|
|
def h(x1, x2):
|
|
return x1 * np.sin(x2)
|
|
|
|
x1 = np.array([2.0])
|
|
x2 = np.array([np.pi / 4])
|
|
# Analytical derivative: dh/dx1 = sin(x2)
|
|
analytical_derivative = np.array([[np.sin(x2[0])]])
|
|
|
|
numerical_derivative = numericalDerivative21(h, x1, x2)
|
|
|
|
np.testing.assert_allclose(
|
|
numerical_derivative, analytical_derivative, rtol=1e-5
|
|
)
|
|
|
|
def test_numericalDerivative22(self):
|
|
# Test function of two variables, derivative with respect to second variable
|
|
def h(x1, x2):
|
|
return x1 * np.sin(x2)
|
|
|
|
x1 = np.array([2.0])
|
|
x2 = np.array([np.pi / 4])
|
|
# Analytical derivative: dh/dx2 = x1 * cos(x2)
|
|
analytical_derivative = np.array([[x1[0] * np.cos(x2[0])]])
|
|
|
|
numerical_derivative = numericalDerivative22(h, x1, x2)
|
|
|
|
np.testing.assert_allclose(
|
|
numerical_derivative, analytical_derivative, rtol=1e-5
|
|
)
|
|
|
|
def test_numericalDerivative33(self):
|
|
# Test function of three variables, derivative with respect to third variable
|
|
def h(x1, x2, x3):
|
|
return x1 * x2 + np.exp(x3)
|
|
|
|
x1 = np.array([1.0])
|
|
x2 = np.array([2.0])
|
|
x3 = np.array([0.5])
|
|
# Analytical derivative: dh/dx3 = exp(x3)
|
|
analytical_derivative = np.array([[np.exp(x3[0])]])
|
|
|
|
numerical_derivative = numericalDerivative33(h, x1, x2, x3)
|
|
|
|
np.testing.assert_allclose(
|
|
numerical_derivative, analytical_derivative, rtol=1e-5
|
|
)
|
|
|
|
def test_numericalDerivative_with_pose(self):
|
|
# Test function with manifold and vector inputs
|
|
|
|
def h(pose:Pose3, point:np.ndarray):
|
|
return pose.transformFrom(point)
|
|
|
|
# Values from testPose3.cpp
|
|
P = Point3(0.2,0.7,-2)
|
|
R = Rot3.Rodrigues(0.3,0,0)
|
|
P2 = Point3(3.5,-8.2,4.2)
|
|
T = Pose3(R,P2)
|
|
|
|
analytic_H1 = np.zeros((3,6), order='F', dtype=float)
|
|
analytic_H2 = np.zeros((3,3), order='F', dtype=float)
|
|
y = T.transformFrom(P, analytic_H1, analytic_H2)
|
|
|
|
numerical_H1 = numericalDerivative21(h, T, P)
|
|
numerical_H2 = numericalDerivative22(h, T, P)
|
|
|
|
np.testing.assert_allclose(numerical_H1, analytic_H1, rtol=1e-5)
|
|
np.testing.assert_allclose(numerical_H2, analytic_H2, rtol=1e-5)
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main() |