calibration_estimation: single_transform

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 os
00040 import rospy
00041 import time
00042 import numpy
00043 
00044 from calibration_estimation.single_transform import SingleTransform
00045 from numpy import *
00046 
00047 class TestSingleTransform(unittest.TestCase):
00048 
00049     def test_free(self):
00050 
00051         st = SingleTransform([0, 0, 0, 0, 0, 0])
00052         free_list = st.calc_free( [0, 0, 1, 1, 0, 0] )
00053 
00054         self.assertEqual(free_list[0], False)
00055         self.assertEqual(free_list[1], False)
00056         self.assertEqual(free_list[2], True)
00057         self.assertEqual(free_list[3], True)
00058         self.assertEqual(free_list[4], False)
00059         self.assertEqual(free_list[5], False)
00060 
00061     def test_inflate(self):
00062         st = SingleTransform([0, 0, 0, 0, 0, 0])
00063         st.inflate( reshape( matrix([1, 0, 0, 0, 0, 0], float), (-1,1) ))
00064         expected = numpy.matrix( [[ 1, 0, 0, 1],
00065                                   [ 0, 1, 0, 0],
00066                                   [ 0, 0, 1, 0],
00067                                   [ 0, 0, 0, 1]], float )
00068 
00069         print ""
00070         print st.transform
00071 
00072         self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6)
00073 
00074     def test_deflate(self):
00075         st = SingleTransform([0, 0, 0, 0, 0, 0])
00076         p = reshape( matrix([1, 0, 0, 0, 0, 0], float), (-1,1) )
00077         st.inflate(p)
00078         result = st.deflate()
00079         self.assertAlmostEqual(numpy.linalg.norm(p-result), 0.0, 6)
00080 
00081     def test_params_to_config(self):
00082         st = SingleTransform()
00083         p = reshape( matrix([1, 0, 0, 0, 0, 0], float), (-1,1) )
00084         config = st.params_to_config(p)
00085         self.assertAlmostEqual( config[0], 1, 6)
00086         self.assertAlmostEqual( config[1], 0, 6)
00087 
00088 
00089     def test_easy_trans_1(self):
00090         st = SingleTransform([1, 2, 3, 0, 0, 0])
00091         expected = numpy.matrix( [[ 1, 0, 0, 1],
00092                                   [ 0, 1, 0, 2],
00093                                   [ 0, 0, 1, 3],
00094                                   [ 0, 0, 0, 1]], float )
00095 
00096         print ""
00097         print st.transform
00098 
00099         self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6)
00100 
00101     def test_easy_rot_1(self):
00102         st = SingleTransform([0, 0, 0, 0, 0, pi/2])
00103         expected = numpy.matrix( [[ 0,-1, 0, 0],
00104                                   [ 1, 0, 0, 0],
00105                                   [ 0, 0, 1, 0],
00106                                   [ 0, 0, 0, 1]], float )
00107         print ""
00108         print st.transform
00109 
00110         self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6)
00111 
00112     def test_easy_rot_2(self):
00113         st = SingleTransform([0, 0, 0, 0, pi/2, 0])
00114         expected = numpy.matrix( [[ 0, 0, 1, 0],
00115                                   [ 0, 1, 0, 0],
00116                                   [-1, 0, 0, 0],
00117                                   [ 0, 0, 0, 1]], float )
00118         print ""
00119         print st.transform
00120 
00121         self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6)
00122 
00123     def test_easy_rot_3(self):
00124         st = SingleTransform([0, 0, 0, pi/2, 0, 0])
00125         expected = numpy.matrix( [[ 1, 0, 0, 0],
00126                                   [ 0, 0,-1, 0],
00127                                   [ 0, 1, 0, 0],
00128                                   [ 0, 0, 0, 1]], float )
00129         print ""
00130         print st.transform
00131 
00132         self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected), 0.0, 6)
00133 
00134     def test_length(self):
00135         st = SingleTransform([0, 0, 0, 0, 0, 0])
00136         self.assertEqual(st.get_length(), 6)
00137 
00138     def test_hard(self):
00139         sample_nums = range(1,6)
00140         dir_name = os.path.dirname(os.path.realpath(__file__))
00141         params_filenames    = [os.path.join(dir_name, 'data', 'single_transform_data', 'params_%02u.txt' % n) for n in sample_nums]
00142         transform_filenames = [os.path.join(dir_name, 'data', 'single_transform_data', 'transform_%02u.txt' % n) for n in sample_nums]
00143 
00144         for params_filename, transform_filename in zip(params_filenames, transform_filenames):
00145             f_params = open(params_filename)
00146             f_transforms = open(transform_filename)
00147             param_elems     = [float(x) for x in f_params.read().split()]
00148             transform_elems = [float(x) for x in f_transforms.read().split()]
00149 
00150             st = SingleTransform(param_elems)
00151             expected_result = numpy.matrix(transform_elems)
00152             expected_result.shape = 4,4
00153 
00154             self.assertAlmostEqual(numpy.linalg.norm(st.transform-expected_result), 0.0, 4, "Failed on %s" % params_filename)
00155 
00156 if __name__ == '__main__':
00157     import rostest
00158     rostest.unitrun('pr2_calibration_estimation', 'test_SingleTransform', TestSingleTransform, coverage_packages=['pr2_calibration_estimation.single_transform'])
00159