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 time
00041 import numpy
00042
00043 from calibration_estimation.camera import RectifiedCamera
00044 from numpy import *
00045
00046 def DefaultParams():
00047 return {'baseline_shift':0,
00048 'frame_id': 'default_frame',
00049 'chain_id': 'default_chain',
00050 'f_shift':0,
00051 'cx_shift':0,
00052 'cy_shift':0,
00053 'cov': {'u':0.5, 'v':0.5} }
00054
00055 class TestRectifiedCamera(unittest.TestCase):
00056
00057 def test_project_easy_1(self):
00058 cam = RectifiedCamera(DefaultParams())
00059 P_list = [ 1, 0, 0, 0, \
00060 0, 1, 0, 0, \
00061 0, 0, 1, 0 ]
00062
00063 pts = matrix( [ [ 0, 1, 0, 2 ],
00064 [ 0, 1, 1, 0 ],
00065 [ 1, 1, 2, 2 ],
00066 [ 1, 1, 1, 1 ]], float )
00067
00068 expected = matrix( [ [ 0, 1, 0, 1],
00069 [ 0, 1, .5, 0] ] )
00070
00071 result = cam.project(P_list, pts)
00072
00073 print ""
00074 print result
00075
00076 self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6)
00077
00078 def test_project_easy_2(self):
00079 cam = RectifiedCamera(DefaultParams())
00080 P_list = [ 1, 0, 0,-1, \
00081 0, 1, 0, 0, \
00082 0, 0, 1, 0 ]
00083
00084 pts = matrix( [ [ 0, 1, 0, 2 ],
00085 [ 0, 1, 1, 0 ],
00086 [ 1, 1, 2, 2 ],
00087 [ 1, 1, 1, 1 ]], float )
00088
00089 expected = matrix( [ [-1, 0,-.5,.5],
00090 [ 0, 1, .5, 0] ] )
00091
00092 result = cam.project(P_list, pts)
00093
00094 print ""
00095 print result
00096
00097 self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6)
00098
00099 def test_project_easy_3(self):
00100 cam = RectifiedCamera(DefaultParams())
00101 P_list = [ 1, 0, 0, 0, \
00102 0, 1, 0, 0, \
00103 0, 0, 1, 0 ]
00104
00105 cam.inflate(matrix([-1]))
00106
00107 pts = matrix( [ [ 0, 1, 0, 2 ],
00108 [ 0, 1, 1, 0 ],
00109 [ 1, 1, 2, 2 ],
00110 [ 1, 1, 1, 1 ]], float )
00111
00112 expected = matrix( [ [-1, 0,-.5,.5],
00113 [ 0, 1, .5, 0] ] )
00114
00115 result = cam.project(P_list, pts)
00116
00117 print ""
00118 print result
00119
00120 self.assertAlmostEqual(numpy.linalg.norm(result-expected), 0.0, 6)
00121
00122
00123 def test_free(self):
00124 cam = RectifiedCamera(DefaultParams())
00125 free_list = cam.calc_free( {"baseline_shift":0, "f_shift":0, "cx_shift":0, "cy_shift":0} )
00126 self.assertEqual(free_list[0], False)
00127
00128 free_list = cam.calc_free( {"baseline_shift":1, "f_shift":0, "cx_shift":0, "cy_shift":0} )
00129 self.assertEqual(free_list[0], True)
00130
00131 self.assertEqual(len(free_list), 4)
00132
00133 def test_params_to_config(self):
00134 cam = RectifiedCamera(DefaultParams())
00135 p = matrix([1, 0, 0, 0], float).T
00136 config = cam.params_to_config(p)
00137 self.assertAlmostEqual(config["baseline_shift"], 1, 6)
00138
00139 def test_length(self):
00140 cam = RectifiedCamera(DefaultParams())
00141 self.assertEqual(cam.get_length(), 4)
00142
00143 def test_deflate(self):
00144 params = DefaultParams()
00145 params['baseline_shift'] = 10
00146 cam = RectifiedCamera(params)
00147 self.assertEqual(cam.deflate()[0,0], 10)
00148
00149 if __name__ == '__main__':
00150 import rostest
00151 rostest.unitrun('calibration_estimation', 'test_RectifiedCamera', TestRectifiedCamera, coverage_packages=['calibration_estimation.camera'])
00152