camera_chain_sensor_unittest.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2008, Willow Garage, Inc.
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of Willow Garage, Inc. nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 
34 
35 import roslib; roslib.load_manifest('calibration_estimation')
36 
37 import sys
38 import unittest
39 import rospy
40 import numpy
41 import yaml
42 from calibration_estimation.sensors.camera_chain_sensor import CameraChainBundler, CameraChainSensor
43 
44 from calibration_msgs.msg import *
45 from sensor_msgs.msg import JointState, CameraInfo
46 
47 from calibration_estimation.single_transform import SingleTransform
48 from calibration_estimation.joint_chain import JointChain
49 from calibration_estimation.camera import RectifiedCamera
50 from calibration_estimation.tilting_laser import TiltingLaser
51 from calibration_estimation.full_chain import FullChainCalcBlock
52 from calibration_estimation.checkerboard import Checkerboard
53 
54 from numpy import *
55 
57 
58  config_yaml = '''
59 - camera_id: camA
60  chain:
61  before_chain: [transformA]
62  chain_id: chainA
63  link_num: 1
64  after_chain: [transformB]
65 - camera_id: camB
66  chain:
67  before_chain: [transformA]
68  chain_id: chainB
69  link_num: 1
70  after_chain: [transformB]
71 '''
72  config_dict = yaml.load(config_yaml)
73 
74  return config_dict
75 
76 class TestCameraChainBundler(unittest.TestCase):
77  def test_basic_match(self):
78  config_list = loadConfigList()
79 
80  bundler = CameraChainBundler(config_list)
81 
82  M_robot = RobotMeasurement( target_id = "targetA",
83  chain_id = "chainB",
84  M_cam = [ CameraMeasurement(camera_id="camA")],
85  M_chain = [ ChainMeasurement(chain_id="chainA"),
86  ChainMeasurement(chain_id="chainB") ])
87 
88  blocks = bundler.build_blocks(M_robot)
89 
90  self.assertEqual( len(blocks), 1)
91  block = blocks[0]
92  self.assertEqual( block._M_cam.camera_id, "camA" )
93  self.assertEqual( block._M_chain.chain_id, "chainA")
94 
96  config_list = loadConfigList()
97 
98  bundler = CameraChainBundler(config_list)
99 
100  M_robot = RobotMeasurement( target_id = "targetA",
101  chain_id = "chainA",
102  M_cam = [ CameraMeasurement(camera_id="camB")],
103  M_chain = [ ChainMeasurement(chain_id="chainA")] )
104 
105  blocks = bundler.build_blocks(M_robot)
106 
107  self.assertEqual( len(blocks), 0)
108 
109 
110 from calibration_estimation.robot_params import RobotParams
111 
112 class TestCameraChainSensor(unittest.TestCase):
113  def load(self):
114  config = yaml.load('''
115  camera_id: camA
116  chain:
117  before_chain: [transformA]
118  chain_id: chainA
119  link_num: 1
120  after_chain: [transformB]
121  ''')
122 
123  robot_params = RobotParams()
124  robot_params.configure( yaml.load('''
125  chains:
126  chainA:
127  transforms:
128  - [ 1, 0, 0, 0, 0, 0 ]
129  axis: [6]
130  gearing: [1]
131  cov:
132  joint_angles: [1.0]
133  chainB:
134  transforms:
135  - [ 2, 0, 0, 0, 0, 0 ]
136  axis: [6]
137  gearing: [1]
138  cov:
139  joint_angles: [1.0]
140  tilting_lasers: {}
141  rectified_cams:
142  camA:
143  baseline_shift: 0.0
144  f_shift: 0.0
145  cx_shift: 0.0
146  cy_shift: 0.0
147  cov: {u: 1.0, v: 1.0}
148  transforms:
149  transformA: [0, 0, 0, 0, 0, 0]
150  transformB: [0, 0, 0, 0, 0, 0]
151  transformC: [0, 0, 0, 0, 0, 0]
152  transformD: [0, 0, 0, 0, 0, 0]
153  checkerboards: {}
154  ''' ) )
155 
156  P = [ 1, 0, 0, 0,
157  0, 1, 0, 0,
158  0, 0, 1, 0 ]
159 
160  return config, robot_params, P
161 
162  def test_cov(self):
163  config, robot_params, P = self.load()
164 
165  camera_points = [ ImagePoint(0, 0),
166  ImagePoint(1, 0) ]
167 
168  sensor = CameraChainSensor(config,
169  CameraMeasurement(camera_id="camA",
170  cam_info=CameraInfo(P=P),
171  image_points=camera_points),
172  ChainMeasurement(chain_id="chainA",
173  chain_state=JointState(position=[0])) )
174  sensor.update_config(robot_params)
175 
176  target = matrix([[1, 2],
177  [0, 0],
178  [1, 1],
179  [1, 1]])
180 
181  cov = sensor.compute_cov(target)
182  print "\ncov:\n", cov
183 
184  self.assertAlmostEqual(cov[0,0], 1.0, 6)
185  self.assertAlmostEqual(cov[1,0], 0.0, 6)
186  self.assertAlmostEqual(cov[2,0], 0.0, 6)
187  self.assertAlmostEqual(cov[3,0], 0.0, 6)
188  self.assertAlmostEqual(cov[1,1], 2.0, 6)
189  self.assertAlmostEqual(cov[3,3], 5.0, 6)
190 
191  def test_update(self):
192  config, robot_params, P = self.load()
193 
194  camera_points = [ ImagePoint(0, 0),
195  ImagePoint(1, 0),
196  ImagePoint(0, 1),
197  ImagePoint(1, 1) ]
198 
199  block = CameraChainSensor(config,
200  CameraMeasurement(camera_id="camA",
201  cam_info=CameraInfo(P=P),
202  image_points=camera_points),
203  ChainMeasurement(chain_id="chainA",
204  chain_state=JointState(position=[0])) )
205  block.update_config(robot_params)
206 
207  target = matrix([[1, 2, 1, 2],
208  [0, 0, 1, 1],
209  [1, 1, 1, 1],
210  [1, 1, 1, 1]])
211 
212  h = block.compute_expected(target)
213  z = block.get_measurement()
214  r = block.compute_residual(target)
215 
216  self.assertAlmostEqual(linalg.norm( h - matrix( [ [0,1,0,1],
217  [0,0,1,1] ] ).T), 0.0, 6)
218  self.assertAlmostEqual(linalg.norm( z - matrix( [ [0,1,0,1],
219  [0,0,1,1] ] ).T), 0.0, 6)
220  self.assertAlmostEqual(linalg.norm( r ), 0.0, 6)
221 
222  # Test Sparsity
223  sparsity = block.build_sparsity_dict()
224  self.assertEqual(sparsity['transforms']['transformA'], [1,1,1,1,1,1])
225  self.assertEqual(sparsity['transforms']['transformB'], [1,1,1,1,1,1])
226  self.assertEqual(sparsity['chains']['chainA']['transforms'], [[1,1,1,1,1,1]])
227  self.assertEqual(sparsity['rectified_cams']['camA']['baseline_shift'], 1)
228 
229 if __name__ == '__main__':
230  import rostest
231  rostest.unitrun('calibration_estimation', 'test_CameraChainBundler', TestCameraChainBundler, coverage_packages=['calibration_estimation.blocks.camera_chain'])
232  rostest.unitrun('calibration_estimation', 'test_CameraChainSensor', TestCameraChainSensor, coverage_packages=['calibration_estimation.blocks.camera_chain'])


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Fri Apr 2 2021 02:12:53