tilting_laser_sensor_unittest.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
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         # Test Sparsity
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'])


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Tue Sep 27 2016 04:06:33