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