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('calibration_estimation')
00036
00037 import sys
00038 import unittest
00039 import rospy
00040 import numpy
00041 import yaml
00042 from calibration_estimation.sensors.tilting_laser_sensor import TiltingLaserBundler, TiltingLaserSensor
00043 from calibration_msgs.msg import *
00044 from sensor_msgs.msg import JointState, CameraInfo
00045 from calibration_estimation.urdf_params import UrdfParams
00046
00047 from calibration_estimation.tilting_laser import TiltingLaser
00048
00049 import numpy
00050 from numpy import *
00051
00052 def loadConfigList():
00053
00054 config_yaml = '''
00055 - laser_id: laser1
00056 sensor_id: laser1
00057 '''
00058 config_dict = yaml.load(config_yaml)
00059 return config_dict
00060
00061 class TestTiltingLaserBundler(unittest.TestCase):
00062 def test_basic(self):
00063 config_list = loadConfigList()
00064
00065 bundler = TiltingLaserBundler(config_list)
00066
00067 M_robot = RobotMeasurement()
00068 M_robot.M_laser.append( LaserMeasurement(laser_id="laser1"))\
00069
00070 blocks = bundler.build_blocks(M_robot)
00071
00072 self.assertEqual( len(blocks), 1)
00073 self.assertEqual( blocks[0]._M_laser.laser_id, "laser1")
00074
00075 def loadSystem():
00076 urdf = '''
00077 <robot>
00078 <link name="base_link"/>
00079 <joint name="j0" type="fixed">
00080 <origin xyz="0 0 0" rpy="0 0 0"/>
00081 <parent link="base_link"/>
00082 <child link="j0_link"/>
00083 </joint>
00084 <link name="j0_link"/>
00085 <joint name="j1" type="revolute">
00086 <axis xyz="0 0 1"/>
00087 <origin xyz="0 0 0" rpy="0 0 0"/>
00088 <parent link="j0_link"/>
00089 <child link="j1_link"/>
00090 </joint>
00091 <link name="j1_link"/>
00092 <joint name="j2" type="fixed">
00093 <origin xyz="0 0 0" rpy="0 0 0"/>
00094 <parent link="j1_link"/>
00095 <child link="j2_link"/>
00096 </joint>
00097 <link name="j2_link"/>
00098 </robot>
00099 '''
00100 config = yaml.load('''
00101 sensors:
00102 chains: {}
00103 rectified_cams: {}
00104 tilting_lasers:
00105 laserA:
00106 sensor_id: laserA
00107 joint: j1
00108 frame_id: j2_link
00109 gearing: 1
00110 cov:
00111 bearing: 1
00112 range: 1
00113 tilt: 1
00114 transforms: {}
00115 checkerboards: {}
00116 ''')
00117
00118 return config["sensors"], UrdfParams(urdf, config)
00119
00120
00121 class TestTiltingLaser(unittest.TestCase):
00122
00123 def test_cov(self):
00124 print ""
00125 config, robot_params = loadSystem()
00126
00127 joint_points = [ JointState(position=[0,0,1]),
00128 JointState(position=[pi/2,0,2]) ]
00129
00130 sensor = TiltingLaserSensor(config["tilting_lasers"]["laserA"], LaserMeasurement(laser_id="laserA",
00131 joint_points=joint_points))
00132
00133 sensor.update_config(robot_params)
00134
00135 cov = sensor.compute_cov(None)
00136
00137 print "Cov:"
00138 print cov
00139
00140 self.assertAlmostEqual(cov[0,0], 1.0, 6)
00141 self.assertAlmostEqual(cov[1,1], 1.0, 6)
00142 self.assertAlmostEqual(cov[2,2], 1.0, 6)
00143 self.assertAlmostEqual(cov[3,3], 4.0, 6)
00144 self.assertAlmostEqual(cov[4,4], 4.0, 6)
00145 self.assertAlmostEqual(cov[5,5], 1.0, 6)
00146
00147 def test_gamma(self):
00148 print ""
00149 config, robot_params = loadSystem()
00150
00151 joint_points = [ JointState(position=[0,0,1]),
00152 JointState(position=[pi/2,0,2]) ]
00153
00154 sensor = TiltingLaserSensor(config["tilting_lasers"]["laserA"], LaserMeasurement(laser_id="laserA",
00155 joint_points=joint_points))
00156
00157 sensor.update_config(robot_params)
00158 gamma = sensor.compute_marginal_gamma_sqrt(None)
00159
00160 print "Gamma:"
00161 print gamma
00162 self.assertAlmostEqual(gamma[0,0], 1.0, 6)
00163 self.assertAlmostEqual(gamma[1,1], 1.0, 6)
00164 self.assertAlmostEqual(gamma[2,2], 1.0, 6)
00165 self.assertAlmostEqual(gamma[3,3], 0.5, 6)
00166 self.assertAlmostEqual(gamma[4,4], 0.5, 6)
00167 self.assertAlmostEqual(gamma[5,5], 1.0, 6)
00168
00169 def test_tilting_laser_1(self):
00170 print ""
00171 config, robot_params = loadSystem()
00172
00173 joint_points = [ JointState(position=[0,0,0]),
00174 JointState(position=[0,pi/2,1]),
00175 JointState(position=[pi/2,0,1]) ]
00176
00177 sensor = TiltingLaserSensor(config["tilting_lasers"]["laserA"], LaserMeasurement(laser_id="laserA",
00178 joint_points=joint_points))
00179
00180 sensor.update_config(robot_params)
00181
00182 target_pts = matrix( [ [ 0, 0, 0 ],
00183 [ 0, 1, 0 ],
00184 [ 0, 0, -1 ],
00185 [ 1, 1, 1 ] ] )
00186
00187 h = sensor.compute_expected(target_pts)
00188 z = sensor.get_measurement()
00189 r = sensor.compute_residual(target_pts)
00190
00191 self.assertAlmostEqual(numpy.linalg.norm(h-target_pts), 0.0, 6)
00192 self.assertAlmostEqual(numpy.linalg.norm(z-target_pts), 0.0, 6)
00193 self.assertAlmostEqual(numpy.linalg.norm(r), 0.0, 6)
00194
00195
00196 sparsity = sensor.build_sparsity_dict()
00197 self.assertEqual(sparsity['transforms']['j0'], [1,1,1,1,1,1])
00198 self.assertEqual(sparsity['transforms']['j1'], [1,1,1,1,1,1])
00199 self.assertEqual(sparsity['transforms']['j2'], [1,1,1,1,1,1])
00200 self.assertEqual(sparsity['tilting_lasers']['laserA']['gearing'], 1)
00201
00202
00203 if __name__ == '__main__':
00204 import rostest
00205 rostest.unitrun('calibration_estimation', 'test_TiltingLaserBundler', TestTiltingLaserBundler, coverage_packages=['calibration_estimation.sensors.tilting_laser_sensor'])
00206 rostest.unitrun('calibration_estimation', 'test_TiltingLaser', TestTiltingLaser, coverage_packages=['calibration_estimation.sensors.tilting_laser_sensor'])