00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 import roslib; roslib.load_manifest('pr2_calibration_estimation')
00036
00037 import sys
00038 import unittest
00039 import rospy
00040 import numpy
00041 import yaml
00042 from pr2_calibration_estimation.sensors.camera_chain_sensor import CameraChainBundler, CameraChainSensor
00043
00044 from calibration_msgs.msg import *
00045 from sensor_msgs.msg import JointState, CameraInfo
00046
00047 from pr2_calibration_estimation.single_transform import SingleTransform
00048 from pr2_calibration_estimation.dh_chain import DhChain
00049 from pr2_calibration_estimation.camera import RectifiedCamera
00050 from pr2_calibration_estimation.tilting_laser import TiltingLaser
00051 from pr2_calibration_estimation.full_chain import FullChainCalcBlock
00052 from pr2_calibration_estimation.checkerboard import Checkerboard
00053
00054 from numpy import *
00055
00056 def loadConfigList():
00057
00058 config_yaml = '''
00059 - camera_id: camA
00060 chain:
00061 before_chain: [transformA]
00062 chain_id: chainA
00063 dh_link_num: 1
00064 after_chain: [transformB]
00065 - camera_id: camB
00066 chain:
00067 before_chain: [transformA]
00068 chain_id: chainB
00069 dh_link_num: 1
00070 after_chain: [transformB]
00071 '''
00072 config_dict = yaml.load(config_yaml)
00073
00074
00075 return config_dict
00076
00077 class TestCameraChainBundler(unittest.TestCase):
00078 def test_basic_match(self):
00079 config_list = loadConfigList()
00080
00081 bundler = CameraChainBundler(config_list)
00082
00083 M_robot = RobotMeasurement( target_id = "targetA",
00084 chain_id = "chainB",
00085 M_cam = [ CameraMeasurement(camera_id="camA")],
00086 M_chain = [ ChainMeasurement(chain_id="chainA"),
00087 ChainMeasurement(chain_id="chainB") ])
00088
00089 blocks = bundler.build_blocks(M_robot)
00090
00091 self.assertEqual( len(blocks), 1)
00092 block = blocks[0]
00093 self.assertEqual( block._M_cam.camera_id, "camA" )
00094 self.assertEqual( block._M_chain.chain_id, "chainA")
00095
00096 def test_basic_no_match(self):
00097 config_list = loadConfigList()
00098
00099 bundler = CameraChainBundler(config_list)
00100
00101 M_robot = RobotMeasurement( target_id = "targetA",
00102 chain_id = "chainA",
00103 M_cam = [ CameraMeasurement(camera_id="camB")],
00104 M_chain = [ ChainMeasurement(chain_id="chainA")] )
00105
00106 blocks = bundler.build_blocks(M_robot)
00107
00108 self.assertEqual( len(blocks), 0)
00109
00110
00111 from pr2_calibration_estimation.robot_params import RobotParams
00112
00113 class TestCameraChainSensor(unittest.TestCase):
00114 def load(self):
00115 config = yaml.load('''
00116 camera_id: camA
00117 chain:
00118 before_chain: [transformA]
00119 chain_id: chainA
00120 dh_link_num: 1
00121 after_chain: [transformB]
00122 ''')
00123
00124 robot_params = RobotParams()
00125 robot_params.configure( yaml.load('''
00126 dh_chains:
00127 chainA:
00128 dh:
00129 - [ 0, 0, 1, 0 ]
00130 gearing: [1]
00131 cov:
00132 joint_angles: [1.0]
00133 chainB:
00134 dh:
00135 - [ 0, 0, 2, 0 ]
00136 gearing: [1]
00137 cov:
00138 joint_angles: [1.0]
00139 tilting_lasers: {}
00140 rectified_cams:
00141 camA:
00142 baseline_shift: 0.0
00143 f_shift: 0.0
00144 cx_shift: 0.0
00145 cy_shift: 0.0
00146 cov: {u: 1.0, v: 1.0}
00147 transforms:
00148 transformA: [0, 0, 0, 0, 0, 0]
00149 transformB: [0, 0, 0, 0, 0, 0]
00150 transformC: [0, 0, 0, 0, 0, 0]
00151 transformD: [0, 0, 0, 0, 0, 0]
00152 checkerboards: {}
00153 ''' ) )
00154
00155 P = [ 1, 0, 0, 0,
00156 0, 1, 0, 0,
00157 0, 0, 1, 0 ]
00158
00159 return config, robot_params, P
00160
00161 def test_cov(self):
00162 config, robot_params, P = self.load()
00163
00164 camera_points = [ ImagePoint(0, 0),
00165 ImagePoint(1, 0) ]
00166
00167 sensor = CameraChainSensor(config,
00168 CameraMeasurement(camera_id="camA",
00169 cam_info=CameraInfo(P=P),
00170 image_points=camera_points),
00171 ChainMeasurement(chain_id="chainA",
00172 chain_state=JointState(position=[0])) )
00173 sensor.update_config(robot_params)
00174
00175 target = matrix([[1, 2],
00176 [0, 0],
00177 [1, 1],
00178 [1, 1]])
00179
00180 cov = sensor.compute_cov(target)
00181 print "\ncov:\n", cov
00182
00183 self.assertAlmostEqual(cov[0,0], 1.0, 6)
00184 self.assertAlmostEqual(cov[1,0], 0.0, 6)
00185 self.assertAlmostEqual(cov[2,0], 0.0, 6)
00186 self.assertAlmostEqual(cov[3,0], 0.0, 6)
00187 self.assertAlmostEqual(cov[1,1], 2.0, 6)
00188 self.assertAlmostEqual(cov[3,3], 5.0, 6)
00189
00190 def test_update(self):
00191 config, robot_params, P = self.load()
00192
00193 camera_points = [ ImagePoint(0, 0),
00194 ImagePoint(1, 0),
00195 ImagePoint(0, 1),
00196 ImagePoint(1, 1) ]
00197
00198 block = CameraChainSensor(config,
00199 CameraMeasurement(camera_id="camA",
00200 cam_info=CameraInfo(P=P),
00201 image_points=camera_points),
00202 ChainMeasurement(chain_id="chainA",
00203 chain_state=JointState(position=[0])) )
00204 block.update_config(robot_params)
00205
00206 target = matrix([[1, 2, 1, 2],
00207 [0, 0, 1, 1],
00208 [1, 1, 1, 1],
00209 [1, 1, 1, 1]])
00210
00211 h = block.compute_expected(target)
00212 z = block.get_measurement()
00213 r = block.compute_residual(target)
00214
00215 self.assertAlmostEqual(linalg.norm( h - matrix( [ [0,1,0,1],
00216 [0,0,1,1] ] ).T), 0.0, 6)
00217 self.assertAlmostEqual(linalg.norm( z - matrix( [ [0,1,0,1],
00218 [0,0,1,1] ] ).T), 0.0, 6)
00219 self.assertAlmostEqual(linalg.norm( r ), 0.0, 6)
00220
00221
00222 sparsity = block.build_sparsity_dict()
00223 self.assertEqual(sparsity['transforms']['transformA'], [1,1,1,1,1,1])
00224 self.assertEqual(sparsity['transforms']['transformB'], [1,1,1,1,1,1])
00225 self.assertEqual(sparsity['dh_chains']['chainA']['dh'], [[1,1,1,1]])
00226 self.assertEqual(sparsity['rectified_cams']['camA']['baseline_shift'], 1)
00227
00228 if __name__ == '__main__':
00229 import rostest
00230 rostest.unitrun('pr2_calibration_estimation', 'test_CameraChainBundler', TestCameraChainBundler, coverage_packages=['pr2_calibration_estimation.blocks.camera_chain'])
00231 rostest.unitrun('pr2_calibration_estimation', 'test_CameraChainSensor', TestCameraChainSensor, coverage_packages=['pr2_calibration_estimation.blocks.camera_chain'])
