camera_chain_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.camera_chain_sensor import CameraChainBundler, CameraChainSensor
00043 
00044 from calibration_msgs.msg import *
00045 from sensor_msgs.msg import JointState, CameraInfo
00046 
00047 from calibration_estimation.single_transform import SingleTransform
00048 from calibration_estimation.joint_chain import JointChain
00049 from calibration_estimation.camera import RectifiedCamera
00050 from calibration_estimation.tilting_laser import TiltingLaser
00051 from calibration_estimation.full_chain import FullChainCalcBlock
00052 from 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     link_num:     1
00064     after_chain:  [transformB]
00065 - camera_id: camB
00066   chain:
00067     before_chain: [transformA]
00068     chain_id:     chainB
00069     link_num:     1
00070     after_chain:  [transformB]
00071 '''
00072     config_dict = yaml.load(config_yaml)
00073 
00074     return config_dict
00075 
00076 class TestCameraChainBundler(unittest.TestCase):
00077     def test_basic_match(self):
00078         config_list = loadConfigList()
00079 
00080         bundler = CameraChainBundler(config_list)
00081 
00082         M_robot = RobotMeasurement( target_id = "targetA",
00083                                     chain_id = "chainB",
00084                                     M_cam   = [ CameraMeasurement(camera_id="camA")],
00085                                     M_chain = [ ChainMeasurement(chain_id="chainA"),
00086                                                 ChainMeasurement(chain_id="chainB") ])
00087 
00088         blocks = bundler.build_blocks(M_robot)
00089 
00090         self.assertEqual( len(blocks), 1)
00091         block = blocks[0]
00092         self.assertEqual( block._M_cam.camera_id,  "camA"  )
00093         self.assertEqual( block._M_chain.chain_id, "chainA")
00094 
00095     def test_basic_no_match(self):
00096         config_list = loadConfigList()
00097 
00098         bundler = CameraChainBundler(config_list)
00099 
00100         M_robot = RobotMeasurement( target_id = "targetA",
00101                                     chain_id = "chainA",
00102                                     M_cam   = [ CameraMeasurement(camera_id="camB")],
00103                                     M_chain = [ ChainMeasurement(chain_id="chainA")] )
00104 
00105         blocks = bundler.build_blocks(M_robot)
00106 
00107         self.assertEqual( len(blocks), 0)
00108 
00109 
00110 from calibration_estimation.robot_params import RobotParams
00111 
00112 class TestCameraChainSensor(unittest.TestCase):
00113     def load(self):
00114         config = yaml.load('''
00115             camera_id: camA
00116             chain:
00117               before_chain: [transformA]
00118               chain_id:     chainA
00119               link_num:     1
00120               after_chain:  [transformB]
00121             ''')
00122 
00123         robot_params = RobotParams()
00124         robot_params.configure( yaml.load('''
00125             chains:
00126               chainA:
00127                 transforms:
00128                   - [ 1, 0, 0, 0, 0, 0 ]
00129                 axis: [6]
00130                 gearing: [1]
00131                 cov:
00132                   joint_angles: [1.0]
00133               chainB:
00134                 transforms:
00135                   - [ 2, 0, 0, 0, 0, 0 ]
00136                 axis: [6]
00137                 gearing: [1]
00138                 cov:
00139                   joint_angles: [1.0]
00140             tilting_lasers: {}
00141             rectified_cams:
00142               camA:
00143                 baseline_shift: 0.0
00144                 f_shift: 0.0
00145                 cx_shift: 0.0
00146                 cy_shift: 0.0
00147                 cov: {u: 1.0, v: 1.0}
00148             transforms:
00149                 transformA: [0, 0, 0, 0, 0, 0]
00150                 transformB: [0, 0, 0, 0, 0, 0]
00151                 transformC: [0, 0, 0, 0, 0, 0]
00152                 transformD: [0, 0, 0, 0, 0, 0]
00153             checkerboards: {}
00154             ''' ) )
00155 
00156         P = [ 1, 0, 0, 0,
00157               0, 1, 0, 0,
00158               0, 0, 1, 0 ]
00159 
00160         return config, robot_params, P
00161 
00162     def test_cov(self):
00163         config, robot_params, P = self.load()
00164 
00165         camera_points      = [ ImagePoint(0, 0),
00166                                ImagePoint(1, 0) ]
00167 
00168         sensor = CameraChainSensor(config,
00169                                    CameraMeasurement(camera_id="camA",
00170                                                      cam_info=CameraInfo(P=P),
00171                                                      image_points=camera_points),
00172                                    ChainMeasurement(chain_id="chainA",
00173                                                     chain_state=JointState(position=[0])) )
00174         sensor.update_config(robot_params)
00175 
00176         target = matrix([[1, 2],
00177                          [0, 0],
00178                          [1, 1],
00179                          [1, 1]])
00180 
00181         cov = sensor.compute_cov(target)
00182         print "\ncov:\n", cov
00183 
00184         self.assertAlmostEqual(cov[0,0], 1.0, 6)
00185         self.assertAlmostEqual(cov[1,0], 0.0, 6)
00186         self.assertAlmostEqual(cov[2,0], 0.0, 6)
00187         self.assertAlmostEqual(cov[3,0], 0.0, 6)
00188         self.assertAlmostEqual(cov[1,1], 2.0, 6)
00189         self.assertAlmostEqual(cov[3,3], 5.0, 6)
00190 
00191     def test_update(self):
00192         config, robot_params, P = self.load()
00193 
00194         camera_points      = [ ImagePoint(0, 0),
00195                                ImagePoint(1, 0),
00196                                ImagePoint(0, 1),
00197                                ImagePoint(1, 1) ]
00198 
00199         block = CameraChainSensor(config,
00200                                   CameraMeasurement(camera_id="camA",
00201                                                     cam_info=CameraInfo(P=P),
00202                                                     image_points=camera_points),
00203                                   ChainMeasurement(chain_id="chainA",
00204                                                    chain_state=JointState(position=[0])) )
00205         block.update_config(robot_params)
00206 
00207         target = matrix([[1, 2, 1, 2],
00208                          [0, 0, 1, 1],
00209                          [1, 1, 1, 1],
00210                          [1, 1, 1, 1]])
00211 
00212         h = block.compute_expected(target)
00213         z = block.get_measurement()
00214         r = block.compute_residual(target)
00215 
00216         self.assertAlmostEqual(linalg.norm( h - matrix( [ [0,1,0,1],
00217                                                           [0,0,1,1] ] ).T), 0.0, 6)
00218         self.assertAlmostEqual(linalg.norm( z - matrix( [ [0,1,0,1],
00219                                                           [0,0,1,1] ] ).T), 0.0, 6)
00220         self.assertAlmostEqual(linalg.norm( r ), 0.0, 6)
00221 
00222         # Test Sparsity
00223         sparsity = block.build_sparsity_dict()
00224         self.assertEqual(sparsity['transforms']['transformA'], [1,1,1,1,1,1])
00225         self.assertEqual(sparsity['transforms']['transformB'], [1,1,1,1,1,1])
00226         self.assertEqual(sparsity['chains']['chainA']['transforms'], [[1,1,1,1,1,1]])
00227         self.assertEqual(sparsity['rectified_cams']['camA']['baseline_shift'], 1)
00228 
00229 if __name__ == '__main__':
00230     import rostest
00231     rostest.unitrun('calibration_estimation', 'test_CameraChainBundler', TestCameraChainBundler, coverage_packages=['calibration_estimation.blocks.camera_chain'])
00232     rostest.unitrun('calibration_estimation', 'test_CameraChainSensor',  TestCameraChainSensor,  coverage_packages=['calibration_estimation.blocks.camera_chain'])


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Wed Aug 26 2015 10:56:21