From bc8fc45e9b590c03dc35f01f1b21f798700507de Mon Sep 17 00:00:00 2001
From: Shisato Yano <shisatoyano@gmail.com>
Date: Fri, 8 Dec 2023 03:37:42 +0000
Subject: [PATCH] calibration and draw process test passed

---
 test/test_ukf_params_calibrator.py | 36 ++++++++++++++++++++++++++++++
 1 file changed, 36 insertions(+)

diff --git a/test/test_ukf_params_calibrator.py b/test/test_ukf_params_calibrator.py
index 1dee19a..eefce08 100644
--- a/test/test_ukf_params_calibrator.py
+++ b/test/test_ukf_params_calibrator.py
@@ -13,7 +13,17 @@
 from pathlib import Path
 
 sys.path.append(str(Path(__file__).absolute().parent) + "/../src/components/sensors/lidar")
+sys.path.append(str(Path(__file__).absolute().parent) + "/../src/components/common")
 from ukf_ext_params_calibrator import UkfExtParamsCalibrator
+from matrix_lib import hom_mat_33
+
+
+class MockState:
+    def __init__(self):
+        pass
+    
+    def x_y_yaw(self):
+        return np.array([[0.0], [0.0], [0.0]])
 
 
 def test_initialization():
@@ -44,3 +54,29 @@ def test_initialization():
     assert calibrator.cov[0, 0] == 1.0
     assert calibrator.cov[1, 1] == 1.0
     assert calibrator.cov[2, 2] == 1.0
+
+
+def test_calibrate_extrinsic_params():
+    calibrator = UkfExtParamsCalibrator()
+    
+    sensor_odom_tf = hom_mat_33(0.0, 0.0, 0.0)
+    vehicle_odom_tf = hom_mat_33(0.0, 0.0, 0.0)
+    calibrator.calibrate_extrinsic_params(sensor_odom_tf, vehicle_odom_tf)
+
+    assert calibrator.state[0, 0] == 0.0
+    assert calibrator.state[1, 0] == 0.0
+    assert calibrator.state[2, 0] == 0.0
+    assert calibrator.cov[0, 0] != 1.0
+    assert calibrator.cov[1, 1] != 1.0
+    assert calibrator.cov[2, 2] != 1.0
+
+
+def test_draw_calib_result():
+    calibrator = UkfExtParamsCalibrator()
+
+    plt.clf()
+    plt.close()
+    figure = plt.figure(figsize=(8, 8))
+    axes = figure.add_subplot(111)
+
+    calibrator.draw_calib_result(axes, [], MockState(), 0.0, 0.0, 0.0)