init_optimization_prior.py
Go to the documentation of this file.
00001 import roslib; roslib.load_manifest('camera_pose_calibration')
00002 import itertools
00003 import numpy
00004 import collections
00005 import cv
00006 import PyKDL
00007 from tf_conversions import posemath
00008 
00009 
00010 def get_target_pose(cam):
00011     # Populate object_points
00012     object_points = cv.fromarray(numpy.array([ [p.x, p.y, p.z ] for p in cam.features.object_points]))
00013     image_points = cv.fromarray(numpy.array([[p.x, p.y] for p in cam.features.image_points]))
00014     dist_coeffs = cv.fromarray(numpy.array([ [0.0, 0.0, 0.0, 0.0] ]))
00015     camera_matrix = numpy.array( [ [ cam.cam_info.P[0], cam.cam_info.P[1], cam.cam_info.P[2]  ],
00016                                    [ cam.cam_info.P[4], cam.cam_info.P[5], cam.cam_info.P[6]  ],
00017                                    [ cam.cam_info.P[8], cam.cam_info.P[9], cam.cam_info.P[10] ] ] )
00018     rot = cv.CreateMat(3, 1, cv.CV_32FC1)
00019     trans = cv.CreateMat(3, 1, cv.CV_32FC1)
00020     cv.FindExtrinsicCameraParams2(object_points, image_points, cv.fromarray(camera_matrix), dist_coeffs, rot, trans)
00021     # print "Rot: %f, %f, %f" % ( rot[0,0], rot[1,0], rot[2,0] )
00022     # print "Trans: %f, %f, %f" % ( trans[0,0], trans[1,0], trans[2,0] )
00023     rot3x3 = cv.CreateMat(3, 3, cv.CV_32FC1)
00024     cv.Rodrigues2(rot, rot3x3)
00025     frame = PyKDL.Frame()
00026     for i in range(3):
00027         frame.p[i] = trans[i,0]
00028     for i in range(3):
00029         for j in range(3):
00030             frame.M[i,j] = rot3x3[i,j]
00031     return frame
00032 
00033 def read_observations(meas):
00034     # Stores the checkerboards observed by two cameras
00035     # camera_id -> camera_id -> [ (cb pose, cb pose, cb id) ]
00036     mutual_observations = collections.defaultdict(lambda: collections.defaultdict(list))
00037     
00038     checkerboard_id = 0
00039     for msg in meas:
00040         for M_cam1, M_cam2 in itertools.combinations(msg.M_cam, 2):
00041             cam1 = M_cam1.camera_id
00042             cam2 = M_cam2.camera_id
00043             p1 = get_target_pose(M_cam1)
00044             p2 = get_target_pose(M_cam2)
00045 
00046             mutual_observations[cam1][cam2].append( (p1, p2, checkerboard_id) )
00047             mutual_observations[cam2][cam1].append( (p2, p1, checkerboard_id) )
00048 
00049         checkerboard_id += 1
00050     return mutual_observations
00051 
00052 # Populates cameras_seen and checkerboards_seen
00053 # root_cam: camera id to start from
00054 # observations: camera_id -> camera_id -> [ (cb pose, cb pose, checkerbord id) ]
00055 # cameras_seen: camera_id -> pose
00056 # checkerboards_seen: checkerboard_id -> pose
00057 def bfs(root_cam, observations, cameras_seen, checkerboards_seen):
00058     q = [root_cam]
00059     cameras_seen[root_cam] = PyKDL.Frame()
00060     while q:
00061         cam1, q = q[0], q[1:]  # pop
00062         cam1_pose = cameras_seen[cam1]
00063 
00064         for cam2, checkerboards in observations[cam1].iteritems():
00065             assert checkerboards
00066             if cam2 not in cameras_seen:
00067                 q.append(cam2)
00068 
00069                 # Cam2Pose = Cam1Pose * Cam1->CB * CB->Cam2
00070                 cameras_seen[cam2] = cam1_pose * checkerboards[0][0] * checkerboards[0][1].Inverse()
00071 
00072             for cb in checkerboards:
00073                 if cb[2] not in checkerboards_seen:
00074                     checkerboards_seen[cb[2]] = cam1_pose * cb[0]
00075 
00076 def find_initial_poses(meas, root_cam = None):
00077     mutual_observations = read_observations(meas)
00078 
00079     if not root_cam:
00080         root_cam = mutual_observations.keys()[0]
00081     camera_poses = {}
00082     checkerboard_poses = {}
00083     bfs(root_cam, mutual_observations, camera_poses, checkerboard_poses)
00084     return camera_poses, checkerboard_poses
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


camera_pose_calibration
Author(s): Vijay Pradeep, Wim Meeussen
autogenerated on Thu Aug 15 2013 10:18:08