def get_chessboard_pose(img, info, ncol=5, nrow=4, squareSize=0.045, useSubPix=True, publishDetection=True, isRect=True): ''' compute the pose of the chessboard in the camera frame ''' (success, chessboard_wrtImage) = find_chessboard(img, ncol=ncol, nrow=nrow, useSubPix=useSubPix); if (len(chessboard_wrtImage) == 0): rospy.logwarn(__name__ + ': no chessboard found'); return None; if (publishDetection): chessboardCV = draw_chessboard(img, chessboard_wrtImage, ncol=ncol, nrow=nrow); chessboardMsg = cvmat2msg(chessboardCV, img.encoding, frame_id=img.header.frame_id, stamp=rospy.Time().now()); chessboardDetectionPub.publish(chessboardMsg); # compute pose chessboard_wrtBoard = cv.CreateMat(ncol*nrow, 1, cv.CV_32FC3); for i in range(ncol*nrow): chessboard_wrtBoard[i,0] = ((i % ncol) * squareSize, (i / ncol) * squareSize, 0); # get camera intrinsics (cameraMatrix, distCoeffs, projectionMatrix, rotationMatrix) = info2cvmat(info); # extrinsic outputs rot = cv.CreateMat(3, 1, cv.CV_32FC1) trans = cv.CreateMat(3, 1, cv.CV_32FC1) # find chessboard pose if isRect: distCoeffs = cv.CreateMat(1, 4, cv.CV_32FC1); distCoeffs[0,0] = 0; distCoeffs[0,1] = 0; distCoeffs[0,2] = 0; distCoeffs[0,3] = 0; cv.FindExtrinsicCameraParams2(chessboard_wrtBoard, chessboard_wrtImage, projectionMatrix[:,:3], distCoeffs, rot, trans); else: cv.FindExtrinsicCameraParams2(chessboard_wrtBoard, chessboard_wrtImage, cameraMatrix, distCoeffs, rot, trans); # get transform from rot, trans rot = np.asarray(rot); trans = np.asarray(trans); th = np.linalg.norm(rot); r = rot / th; R = np.cos(th)*np.eye(3) + (1 - np.cos(th))*np.dot(r, r.T) + np.sin(th)*np.array([[0, -r[2], r[1]], [r[2], 0, -r[0]], [-r[1], r[0], 0]]); if not isRect: Trect = tr.trans(-projectionMatrix[0,3]/cameraMatrix[0,0],-projectionMatrix[1,3]/cameraMatrix[1,1], 0) Rcam = tr.rot2tr(np.asarray(rotationMatrix)); Tmodel2cam = np.dot(Trect, np.dot(Rcam, np.dot(tr.trans(trans), tr.rot2tr(R)))); else: Tmodel2cam = np.dot(tr.trans(trans), tr.rot2tr(R)); return ru.to_PoseStamped(Tmodel2cam, frame_id=info.header.frame_id, stamp=info.header.stamp);
def detcb(imagemsg): if imagemsg.header.stamp > timestart: cvimage = self.bridge.imgmsg_to_cv(imagemsg, 'mono8') corners = self.detect(cvimage) if corners is not None: with donecond: # get the pose with respect to the attached sensor tf frame if self.cvKK is None or self.cvkc is None: KK, kc, Ts, error = self.calibrateIntrinsicCamera( [self.getObjectPoints()], [corners], (cvimage.width, cvimage.height), usenonlinoptim=False) T = Ts[0] else: cvrvec = cv.CreateMat(1, 3, cv.CV_64F) cvtvec = cv.CreateMat(1, 3, cv.CV_64F) cv.FindExtrinsicCameraParams2( cv.fromarray(self.getObjectPoints()), cv.fromarray(corners), self.cvKK, self.cvkc, cvrvec, cvtvec) T = matrixFromAxisAngle(array(cvrvec)[0]) T[0:3, 3] = array(cvtvec)[0] data[0] = { 'T': T, 'corners_raw': corners, 'image_raw': array(cvimage) } if 'type' in self.pattern: data[0]['type'] = self.pattern['type'] donecond.notifyAll()
def FindExtrinsicCameraParams(imagepoints, objectpoints, KK): """ Use OpenCV to solve for the affine transformation that matches imagepoints to object points imagepoints - 2xN array objectpoints - 3xN array KK - 3x3 array or 4 element array """ imagepoints = array(imagepoints,float) objectpoints = array(objectpoints,float) if len(KK.shape) == 1: cvKK = cv.CreateMat(3,3,cv.CV_32FC1) cvKK[0,0] = KK[0]; cvKK[0,1] = 0; cvKK[0,2] = KK[2]; cvKK[1,0] = 0; cvKK[1,1] = KK[1]; cvKK[1,2] = KK[3]; cvKK[2,0] = 0; cvKK[2,1] = 0; cvKK[2,2] = 1; else: cvKK = cv.fromarray(KK) cvDist = cv.CreateMat(4,1,cv.CV_32FC1) cvDist[0,0] = 0; cvDist[1,0] = 0; cvDist[2,0] = 0; cvDist[3,0] = 0; rvec = cv.CreateMat(3,1,cv.CV_32FC1) tvec = cv.CreateMat(3,1,cv.CV_32FC1) object_points = cv.CreateMatHeader(3,objectpoints.shape[0],cv.CV_32FC1) cv.SetData(object_points,struct.pack('f'*(objectpoints.shape[0]*3),*transpose(objectpoints).flat),4*objectpoints.shape[0]) image_points = cv.CreateMatHeader(2,imagepoints.shape[0],cv.CV_32FC1) cv.SetData(image_points,struct.pack('f'*(imagepoints.shape[0]*2),*transpose(imagepoints).flat),4*imagepoints.shape[0]) cv.FindExtrinsicCameraParams2(object_points,image_points,cvKK,cvDist,rvec,tvec) T = matrixFromAxisAngle((rvec[0,0],rvec[1,0],rvec[2,0])) T[0:3,3] = [tvec[0,0],tvec[1,0],tvec[2,0]] return T
def get_target_pose(cam): # Populate object_points object_points = cv.fromarray( numpy.array([[p.x, p.y, p.z] for p in cam.features.object_points])) image_points = cv.fromarray( numpy.array([[p.x, p.y] for p in cam.features.image_points])) dist_coeffs = cv.fromarray(numpy.array([[0.0, 0.0, 0.0, 0.0]])) camera_matrix = numpy.array( [[cam.cam_info.P[0], cam.cam_info.P[1], cam.cam_info.P[2]], [cam.cam_info.P[4], cam.cam_info.P[5], cam.cam_info.P[6]], [cam.cam_info.P[8], cam.cam_info.P[9], cam.cam_info.P[10]]]) rot = cv.CreateMat(3, 1, cv.CV_32FC1) trans = cv.CreateMat(3, 1, cv.CV_32FC1) cv.FindExtrinsicCameraParams2(object_points, image_points, cv.fromarray(camera_matrix), dist_coeffs, rot, trans) rot3x3 = cv.CreateMat(3, 3, cv.CV_32FC1) cv.Rodrigues2(rot, rot3x3) frame = PyKDL.Frame() for i in range(3): frame.p[i] = trans[i, 0] for i in range(3): for j in range(3): frame.M[i, j] = rot3x3[i, j] return frame
def gotimage(self, imgmsg): if imgmsg.encoding == "bayer_bggr8": imgmsg.encoding = "8UC1" img = CvBridge().imgmsg_to_cv(imgmsg) # cv.ShowImage("DataMatrix", img) # cv.WaitKey(6) self.track(img) # monocular case print self.cams if 'l' in self.cams: for (code, corners) in self.tracking.items(): model = cv.fromarray(numpy.array([[0,0,0], [.1, 0, 0], [.1, .1, 0], [0, .1, 0]], numpy.float32)) rot = cv.CreateMat(3, 1, cv.CV_32FC1) trans = cv.CreateMat(3, 1, cv.CV_32FC1) cv.FindExtrinsicCameraParams2(model, cv.fromarray(numpy.array(corners, numpy.float32)), self.cams['l'].intrinsicMatrix(), self.cams['l'].distortionCoeffs(), rot, trans) ts = geometry_msgs.msg.TransformStamped() ts.header.frame_id = imgmsg.header.frame_id ts.header.stamp = imgmsg.header.stamp ts.child_frame_id = code posemsg = pm.toMsg(pm.fromCameraParams(cv, rot, trans)) ts.transform.translation = posemsg.position ts.transform.rotation = posemsg.orientation tfm = tf.msg.tfMessage([ts]) self.pub_tf.publish(tfm)
def handle_monocular(self, msg): def pt2line(x0, y0, x1, y1, x2, y2): """ point is (x0, y0), line is (x1, y1, x2, y2) """ return abs((x2 - x1) * (y1 - y0) - (x1 - x0) * (y2 - y1)) / math.sqrt((x2 - x1)**2 + (y2 - y1)**2) (image, camera) = msg rgb = self.mkgray(image) C = self.image_corners(rgb) if C: cc = self.board.n_cols cr = self.board.n_rows errors = [] for r in range(cr): (x1, y1) = C[(cc * r) + 0] (x2, y2) = C[(cc * r) + cc - 1] for i in range(1, cc - 1): (x0, y0) = C[(cc * r) + i] errors.append(pt2line(x0, y0, x1, y1, x2, y2)) linearity_rms = math.sqrt( sum([e**2 for e in errors]) / len(errors)) # Add in reprojection check A = cv.CreateMat(3, 3, 0) image_points = cv.fromarray(numpy.array(C)) object_points = cv.fromarray(numpy.zeros([cc * cr, 3])) for i in range(cr): for j in range(cc): object_points[i * cc + j, 0] = j * self.board.dim object_points[i * cc + j, 1] = i * self.board.dim object_points[i * cc + j, 2] = 0.0 dist_coeffs = cv.fromarray(numpy.array([[0.0, 0.0, 0.0, 0.0]])) camera_matrix = numpy.array( [[camera.P[0], camera.P[1], camera.P[2]], [camera.P[4], camera.P[5], camera.P[6]], [camera.P[8], camera.P[9], camera.P[10]]]) rot = cv.CreateMat(3, 1, cv.CV_32FC1) trans = cv.CreateMat(3, 1, cv.CV_32FC1) cv.FindExtrinsicCameraParams2(object_points, image_points, cv.fromarray(camera_matrix), dist_coeffs, rot, trans) # Convert rotation into a 3x3 Rotation Matrix rot3x3 = cv.CreateMat(3, 3, cv.CV_32FC1) cv.Rodrigues2(rot, rot3x3) # Reproject model points into image object_points_world = numpy.asmatrix(rot3x3) * ( numpy.asmatrix(object_points).T) + numpy.asmatrix(trans) reprojected_h = camera_matrix * object_points_world reprojected = (reprojected_h[0:2, :] / reprojected_h[2, :]) reprojection_errors = image_points - reprojected.T reprojection_rms = numpy.sqrt( numpy.sum(numpy.array(reprojection_errors)**2) / numpy.product(reprojection_errors.shape)) # Print the results print "Linearity RMS Error: %.3f Pixels Reprojection RMS Error: %.3f Pixels" % ( linearity_rms, reprojection_rms) else: print 'no chessboard'
def solve(C, Q, matches, dbsiftpath, dbimgpath): # open EarthMine info info = os.path.join(C.infodir, os.path.basename(dbimgpath)[:-4] + '.info') em = render_tags.EarthmineImageInfo(dbimgpath, info) map3d = C.pixelmap.open(dbsiftpath) # find non-None features vector = [] for i, m in enumerate(matches): d = m['db'] # get 3d pt of feature feature = map3d[int(d[0]), int(d[1])] if not feature: continue # convert from latlon to meters relative to earthmine camera pz, px = geom.lltom(em.lat, em.lon, feature['lat'], feature['lon']) py = feature['alt'] - em.alt vector.append((m['query'][:2], (px, py, -pz))) print vector[0] # reference camera matrix # f 0 0 # 0 f 0 # 0 0 1 A = cv.CreateMat(3, 3, cv.CV_64F) cv.SetZero(A) f = 662 # focal len? cv.Set2D(A, 0, 0, cv.Scalar(f)) cv.Set2D(A, 1, 1, cv.Scalar(f)) cv.Set2D(A, 2, 2, cv.Scalar(1)) # convert vector to to cvMats objectPoints3d = cv.CreateMat(len(vector), 1, cv.CV_64FC3) imagePoints2d = cv.CreateMat(len(vector), 1, cv.CV_64FC2) for i, (p2d, p3d) in enumerate(vector): cv.Set2D(imagePoints2d, i, 0, cv.Scalar(*p2d)) cv.Set2D(objectPoints3d, i, 0, cv.Scalar(*p3d)) coeff = cv.CreateMat(4, 1, cv.CV_64F) rvec = cv.CreateMat(3, 1, cv.CV_64F) tvec = cv.CreateMat(3, 1, cv.CV_64F) cv.SetZero(coeff) cv.SetZero(rvec) cv.SetZero(tvec) # since rvec, tvec are zero the initial guess is the earthmine camera ret = cv.FindExtrinsicCameraParams2(objectPoints3d, imagePoints2d, A, coeff, rvec, tvec, useExtrinsicGuess=False) np_rvec = np.matrix(rvec) np_tvec = np.matrix(tvec) print np_rvec print np_tvec return np_rvec, np_tvec
def backproject_depth(im, depth, corners): # Use 'extrinsic rectification' to find plane equation global obj, cam, distcc, rvec, tvec, bp obj = np.mgrid[:6, :8, :1].astype('f').reshape(3, -1) * 0.0254 f = 583.0 cx = 321 cy = 249 distcc = np.zeros((4, 1), 'f') rvec = np.zeros((3, 1), 'f') tvec = np.zeros((3, 1), 'f') cam = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]]) cv.FindExtrinsicCameraParams2(obj, corners, cam, distcc, rvec, tvec) # Back project to see how it went bp = np.array(corners) cv.ProjectPoints2(obj, rvec, tvec, cam, distcc, bp) global pts1 # Show the points in a point cloud, using rvec and tvec RT = np.eye(4).astype('f') RT[:3, :3] = expmap.axis2rot(rvec.squeeze()) RT[:3, 3] = tvec.squeeze() #RT = np.linalg.inv(RT) pts1 = np.dot(RT[:3, :3], obj) + RT[:3, 3].reshape(3, 1) pts1[1, :] *= -1 pts1[2, :] *= -1 rgb1 = np.zeros((pts1.shape[1], 4), 'f') rgb1[:, :] = [0, 0, 0, 1] # Also show the points in the region, using the calib image global mask, pts2 bounds = corners[[0, 7, 5 * 8 + 7, 5 * 8], :] polys = (tuple([tuple(x) for x in tuple(bounds)]), ) mask = np.zeros((480, 640), 'f') cv.FillPoly(mask, polys, [1, 0, 0]) mask = (mask > 0) & (depth < 2047) v, u = np.mgrid[:480, :640].astype('f') pts2 = np.vstack(project(depth[mask].astype('f'), u[mask], v[mask])) rgb2 = np.zeros((pts2.shape[1], 4), 'f') rgb2[:, :] = [0, 1, 0, 1] if np.any(np.isnan(pts2.mean(1))): return window.update_points( np.hstack((pts1, pts2)).transpose(), np.vstack((rgb1, rgb2))) window.lookat = pts1.mean(1) window.Refresh() return
def handle_mono(self, qq): (imgmsg, caminfo) = qq img = CvBridge().imgmsg_to_cv(imgmsg, "mono8") pcm = image_geometry.PinholeCameraModel() pcm.fromCameraInfo(caminfo) self.track(img) for (code, corners) in self.tracking.items(): # detector numbers vertices clockwise like this: # 1 2 # 0 3 if isinstance(self.dim, (float, int)): d = self.dim else: d = self.dim[code] # User specifies black bar length, but detector uses # full module, including the 2-unit quiet zone, # so scale by 14/10 d = d * 14 / 10 model = cv.fromarray(numpy.array([[0,0,0], [0, d, 0], [d, d, 0], [d, 0, 0]], numpy.float32)) rot = cv.CreateMat(3, 1, cv.CV_32FC1) trans = cv.CreateMat(3, 1, cv.CV_32FC1) cv.FindExtrinsicCameraParams2(model, cv.fromarray(numpy.array(corners, numpy.float32)), pcm.intrinsicMatrix(), pcm.distortionCoeffs(), rot, trans) ts = geometry_msgs.msg.TransformStamped() ts.header.frame_id = imgmsg.header.frame_id ts.header.stamp = imgmsg.header.stamp ts.child_frame_id = code posemsg = pm.toMsg(pm.fromCameraParams(cv, rot, trans)) ts.transform.translation = posemsg.position ts.transform.rotation = posemsg.orientation tfm = tf.msg.tfMessage([ts]) self.pub_tf.publish(tfm)
def fit(self, data): m = mean(data, 0) diff = data - tile(m, (data.shape[0], 1)) area0 = abs(linalg.det(dot(transpose(diff[:, 0:2]), diff[:, 0:2]))) if area0 < 0.00001: # check if point area is large enough #print 'degenerate 2d data %f'%area0 return None # have to compute if the 3d points are collinear or not eigvalues = linalg.eigvalsh(dot(transpose(diff[:, 2:5]), diff[:, 2:5])) if sum(abs(eigvalues) <= 1e-9 ) >= 2: # check if point area is large enough #print 'degenerate 3d points',eigvalues return None if data.shape[0] == self.ninitial: object_points = self.object_points image_points = self.image_points else: object_points = cv.CreateMatHeader(3, data.shape[0], cv.CV_32FC1) image_points = cv.CreateMatHeader(2, data.shape[0], cv.CV_32FC1) cv.SetData( object_points, struct.pack('f' * (data.shape[0] * 3), *transpose(data[:, 2:5]).flat), 4 * data.shape[0]) cv.SetData( image_points, struct.pack('f' * (data.shape[0] * 2), *transpose(data[:, 0:2]).flat), 4 * data.shape[0]) cv.FindExtrinsicCameraParams2(object_points, image_points, self.cvKK, self.cvDist, self.rvec, self.tvec) #cv.FindExtrinsicCameraParams2(cv.fromarray(data[:,2:5]),cv.fromarray(data[:,0:2]),self.cvKK,self.cvDist,self.rvec,self.tvec) T = matrixFromAxisAngle( (self.rvec[0, 0], self.rvec[1, 0], self.rvec[2, 0])) T[0:3, 3] = [self.tvec[0, 0], self.tvec[1, 0], self.tvec[2, 0]] # make sure that texture faces towards the image (ie, z axis has negative z component) if T[2, 2] < 0: return None return T
class CrCalibration: def __init__(self, size, dim): self.chess_size = size self.dim = dim left_ns = rospy.remap_name('left') range_ns = rospy.remap_name('range') limg_sub = message_filters.Subscriber(left_ns + '/image_rect', sensor_msgs.msg.Image) rimg_sub = message_filters.Subscriber(range_ns + '/image_rect', sensor_msgs.msg.Image) linfo_sub = message_filters.Subscriber(left_ns + '/camera_info', sensor_msgs.msg.CameraInfo) rinfo_sub = message_filters.Subscriber(range_ns + '/camera_info', sensor_msgs.msg.CameraInfo) ts = message_filters.TimeSynchronizer( [limg_sub, linfo_sub, rimg_sub, rinfo_sub], 16) ts.registerCallback(self.queue_cr) self.bridge = cv_bridge.CvBridge() self.frame_list = [] self.static_pose = None self.result_list = [] self.last_err = 0 def calc_frame(self, lst): pos = PyKDL.Vector() ex = 0 ey = 0 ez = 0 for f in lst: pos += f.p z, y, x = f.M.GetEulerZYX() ex += x ey += y ez += z size = len(lst) return PyKDL.Frame( PyKDL.Rotation.EulerZYX(ez / size, ey / size, ex / size), pos / size) def calc_distance(self, f1, f2): test = f1 * f2.Inverse() angle, axis = test.M.GetRotAngle() norm = test.p.Norm() return (norm + (angle / 10.0)) def queue_cr(self, limg, linfo, rimg, rinfo): # lpose = self.find_checkerboard_pose(limg, linfo) rpose = self.find_checkerboard_pose(rimg, rinfo) if lpose == None or rpose == None: if lpose == None: rospy.loginfo("can't find CB on left camera image") if rpose == None: rospy.loginfo("can't find CB on range image") return False # calcurate pose lframe = pm.fromMsg(lpose.pose) rframe = pm.fromMsg(rpose.pose) frame = lframe * rframe.Inverse() # check pose euniqness for r in self.result_list: if self.calc_distance(r[0], lframe) < 0.075: return False # check pose_movement if len(self.frame_list) == 0: self.static_pose = lframe self.frame_list.append(frame) return False dist = self.calc_distance(self.static_pose, lframe) print dist if dist > 0.012: self.frame_list = [] self.static_pose = None return False self.frame_list.append(frame) if len(self.frame_list) > 5: self.result_list.append( [self.static_pose, self.calc_frame(self.frame_list)]) self.frame_list = [] self.static_pose = None # check resut list num ret = self.calc_frame([r[1] for r in self.result_list]) err = 0.0 for r in self.result_list: err += self.calc_distance(r[1], ret) qx, qy, qz, qw = ret.M.GetQuaternion() rospy.loginfo("%f %f %f %f %f %f %f / err = %f" % (ret.p.x(), ret.p.y(), ret.p.z(), qx, qy, qz, qw, err / len(self.result_list))) self.last_err = err / len(self.result_list) # finish check if len(self.result_list) > 7 and self.last_err < 0.1: rospy.loginfo("Finished size = %d, err = %f" % (len(self.result_list), self.last_err)) print "translation: [%f, %f, %f]\nrotation: [%f, %f, %f, %f]" % ( ret.p.x(), ret.p.y(), ret.p.z(), qx, qy, qz, qw) print "(make-coords :pos #f(%f %f %f) :rot (quaternion2matrix #f(%f %f %f %f)))" % ( 1000 * ret.p.x(), 1000 * ret.p.y(), 1000 * ret.p.z(), qw, qx, qy, qz) #print "<node pkg=\"tf\" type=\"static_transform_publisher\" name=\"cam_link_broadcaster\" args=\"%f %f %f %f %f %f %f link1 link2 30\" />" % (ret.p.x(), ret.p.y(), ret.p.z(), qw, qx, qy, qz) exit(-1) return True return True def detect(self, image): corners_x = self.chess_size[0] corners_y = self.chess_size[1] #Here, we'll actually call the openCV detector found, corners = cv.FindChessboardCorners( image, (corners_x, corners_y), cv.CV_CALIB_CB_ADAPTIVE_THRESH) if found: board_corners = (corners[0], corners[corners_x - 1], corners[(corners_y - 1) * corners_x], corners[len(corners) - 1]) #find the perimeter of the checkerboard perimeter = 0.0 for i in range(len(board_corners)): next = (i + 1) % 4 xdiff = board_corners[i][0] - board_corners[next][0] ydiff = board_corners[i][1] - board_corners[next][1] perimeter += math.sqrt(xdiff * xdiff + ydiff * ydiff) #estimate the square size in pixels square_size = perimeter / ((corners_x - 1 + corners_y - 1) * 2) radius = int(square_size * 0.5 + 0.5) corners = cv.FindCornerSubPix( image, corners, (radius, radius), (-1, -1), (cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 30, 0.1)) #uncomment to debug chessboard detection #cv.DrawChessboardCorners(image, (corners_x, corners_y), corners, 1) #cv.NamedWindow("image") #cv.ShowImage("image", image) #cv.WaitKey(600) #we'll also generate the object points if the user has specified spacing object_points = cv.CreateMat(3, corners_x * corners_y, cv.CV_32FC1) for y in range(corners_y): for x in range(corners_x): cv.SetReal2D(object_points, 0, y * corners_x + x, x * self.dim) cv.SetReal2D(object_points, 1, y * corners_x + x, y * self.dim) cv.SetReal2D(object_points, 2, y * corners_x + x, 0.0) #not sure why opencv functions return non opencv compatible datatypes... but they do so we'll convert corners_cv = cv.CreateMat(2, corners_x * corners_y, cv.CV_32FC1) for i in range(corners_x * corners_y): cv.SetReal2D(corners_cv, 0, i, corners[i][0]) cv.SetReal2D(corners_cv, 1, i, corners[i][1]) return (corners_cv, object_points) else: #cv.NamedWindow("image_scaled") #cv.ShowImage("image_scaled", image_scaled) #cv.WaitKey(600) rospy.logwarn("Didn't find checkerboard") return (None, None) return def intrinsic_matrix_from_info(self, cam_info): intrinsic_matrix = cv.CreateMat(3, 3, cv.CV_32FC1) #Because we only want the upper 3x3 (normal) portion of the rectified intrinsic matrix for i in range(0, 3): for j in range(0, 3): intrinsic_matrix[i, j] = cam_info.P[4 * i + j] return intrinsic_matrix def find_checkerboard_pose(self, ros_image, cam_info): #we need to convert the ros image to an opencv image try: image = self.bridge.imgmsg_to_cv(ros_image, "mono8") except CvBridgeError, e: rospy.logerror("Error importing image %s" % e) return corners, model = self.detect(image) if corners == None or model == None: return None else: #find the pose of the checkerboard rot = cv.CreateMat(3, 1, cv.CV_32FC1) trans = cv.CreateMat(3, 1, cv.CV_32FC1) kc = cv.CreateMat(1, 4, cv.CV_32FC1) cv.Set(kc, 0.0) intrinsic_matrix = self.intrinsic_matrix_from_info(cam_info) cv.FindExtrinsicCameraParams2(model, corners, intrinsic_matrix, kc, rot, trans) #We want to build a transform now, but first we have to convert the #rotation vector we get back from OpenCV into a rotation matrix rot_mat = cv.CreateMat(3, 3, cv.CV_32FC1) cv.Rodrigues2(rot, rot_mat) #Now we need to convert this rotation matrix into a quaternion #This can be done better in never versions of opencv, but we need to be boxturtle compatible numpy_mat = numpy.fromstring(rot_mat.tostring(), dtype=numpy.float32).reshape((3, 3)) #of course, tf expects all matricies passed to it to be 4x4... so we'll build a full matrix here full_pose = numpy.zeros((4, 4)) #first, copy in the rotation matrix full_pose[0:3, 0:3] = numpy_mat[0:3, 0:3] #next, we'll copy in the translation full_pose[0:3, 3] = [trans[i, 0] for i in range(3)] #and make sure to add a 1 in the lower right corner full_pose[3][3] = 1.0 rospy.logdebug("%s" % numpy_mat) rospy.logdebug("%s" % full_pose) tf_trans = tf.transformations.translation_from_matrix(full_pose) tf_rot = tf.transformations.quaternion_from_matrix(full_pose) #and now we'll actually build our pose stamped board_pose = geometry_msgs.msg.PoseStamped() board_pose.header = ros_image.header board_pose.pose.position.x = tf_trans[0] board_pose.pose.position.y = tf_trans[1] board_pose.pose.position.z = tf_trans[2] board_pose.pose.orientation.x = tf_rot[0] board_pose.pose.orientation.y = tf_rot[1] board_pose.pose.orientation.z = tf_rot[2] board_pose.pose.orientation.w = tf_rot[3] rospy.logdebug("%s" % board_pose) #we'll publish the pose so we can display it in rviz # self.pose_pub.publish(board_pose) return board_pose
class ImageCbDetectorNode: def __init__(self): self.corners_x = rospy.get_param('~corners_x', 6) self.corners_y = rospy.get_param('~corners_y', 6) self.spacing_x = rospy.get_param('~spacing_x', None) self.spacing_y = rospy.get_param('~spacing_y', None) self.width_scaling = rospy.get_param('~width_scaling', 1) self.height_scaling = rospy.get_param('~height_scaling', 1) self.im_cb_detector = ImageCbDetector() self.image_sub = rospy.Subscriber("image_stream", Image, self.callback) self.caminfo_sub = rospy.Subscriber("camera_info", CameraInfo, self.cam_info_cb) self.pose_pub = rospy.Publisher("board_pose", PoseStamped, latch=True) self.pose_srv = None self.bridge = CvBridge() self.mutex = threading.RLock() self.cam_info = None self.ros_image = None def intrinsic_matrix_from_info(self, cam_info): intrinsic_matrix = cv.CreateMat(3, 3, cv.CV_32FC1) #Because we only want the upper 3x3 (normal) portion of the rectified intrinsic matrix for i in range(0, 3): for j in range(0, 3): intrinsic_matrix[i, j] = cam_info.P[4 * i + j] return intrinsic_matrix def attempt_to_advertise(self): #we'll only advertise the service after we've received a cam_info/image pair if self.ros_image != None and self.cam_info != None: self.pose_srv = rospy.Service("get_checkerboard_pose", GetCheckerboardPose, self.find_checkerboard_service) def cam_info_cb(self, cam_info): with self.mutex: self.cam_info = cam_info if self.pose_srv == None: self.attempt_to_advertise() def callback(self, ros_image): #copy over the latest image to be used when a service call is made with self.mutex: self.ros_image = ros_image if self.pose_srv == None: self.attempt_to_advertise() def find_checkerboard_service(self, req): poses = [] min_x = self.ros_image.width min_y = self.ros_image.height max_x = 0 max_y = 0 for i in range(10): rospy.sleep(0.5) #copy the image over with self.mutex: image = self.ros_image result = self.find_checkerboard_pose(image, req.corners_x, req.corners_y, req.spacing_x, req.spacing_y, self.width_scaling, self.height_scaling) if result is None: rospy.logerr("Couldn't find a checkerboard") continue p, corners = result poses.append(p) for j in range(corners.cols): x = cv.GetReal2D(corners, 0, j) y = cv.GetReal2D(corners, 1, j) min_x = min(min_x, x) max_x = max(max_x, x) min_y = min(min_y, y) max_y = max(max_y, y) # convert all poses to twists twists = [] for p in poses: twists.append( PyKDL.diff(PyKDL.Frame.Identity(), posemath.fromMsg(p.pose))) # get average twist twist_res = PyKDL.Twist() PyKDL.SetToZero(twist_res) for t in twists: for i in range(3): twist_res.vel[i] += t.vel[i] / len(poses) twist_res.rot[i] += t.rot[i] / len(poses) # get noise noise_rot = 0 noise_vel = 0 for t in twists: n_vel = (t.vel - twist_res.vel).Norm() n_rot = (t.rot - twist_res.rot).Norm() if n_vel > noise_vel: noise_vel = n_vel if n_rot > noise_rot: noise_rot = n_rot # set service resul pose_res = PyKDL.addDelta(PyKDL.Frame.Identity(), twist_res, 1.0) pose_msg = PoseStamped() pose_msg.header = poses[0].header pose_msg.pose = posemath.toMsg(pose_res) return GetCheckerboardPoseResponse(pose_msg, min_x, max_x, min_y, max_y, noise_vel, noise_rot) def find_checkerboard_pose(self, ros_image, corners_x, corners_y, spacing_x, spacing_y, width_scaling, height_scaling): #we need to convert the ros image to an opencv image try: image = self.bridge.imgmsg_to_cv(ros_image, "mono8") except CvBridgeError, e: rospy.logerror("Error importing image %s" % e) return corners, model = self.im_cb_detector.detect(image, corners_x, corners_y, spacing_x, spacing_y, width_scaling, height_scaling) if corners != None and model != None and self.cam_info != None: #find the pose of the checkerboard rot = cv.CreateMat(3, 1, cv.CV_32FC1) trans = cv.CreateMat(3, 1, cv.CV_32FC1) with self.mutex: intrinsic_matrix = self.intrinsic_matrix_from_info( self.cam_info) kc = cv.CreateMat(1, 4, cv.CV_32FC1) cv.Set(kc, 0.0) cv.FindExtrinsicCameraParams2(model, corners, intrinsic_matrix, kc, rot, trans) #We want to build a transform now, but first we have to convert the #rotation vector we get back from OpenCV into a rotation matrix rot_mat = cv.CreateMat(3, 3, cv.CV_32FC1) cv.Rodrigues2(rot, rot_mat) #Now we need to convert this rotation matrix into a quaternion #This can be done better in never versions of opencv, but we need to be boxturtle compatible numpy_mat = numpy.fromstring(rot_mat.tostring(), dtype=numpy.float32).reshape((3, 3)) #of course, tf expects all matricies passed to it to be 4x4... so we'll build a full matrix here full_pose = numpy.zeros((4, 4)) #first, copy in the rotation matrix full_pose[0:3, 0:3] = numpy_mat[0:3, 0:3] #next, we'll copy in the translation full_pose[0:3, 3] = [trans[i, 0] for i in range(3)] #and make sure to add a 1 in the lower right corner full_pose[3][3] = 1.0 rospy.logdebug("%s" % numpy_mat) rospy.logdebug("%s" % full_pose) tf_trans = tf.transformations.translation_from_matrix(full_pose) tf_rot = tf.transformations.quaternion_from_matrix(full_pose) #and now we'll actually build our pose stamped board_pose = PoseStamped() board_pose.header = ros_image.header board_pose.pose.position.x = tf_trans[0] board_pose.pose.position.y = tf_trans[1] board_pose.pose.position.z = tf_trans[2] board_pose.pose.orientation.x = tf_rot[0] board_pose.pose.orientation.y = tf_rot[1] board_pose.pose.orientation.z = tf_rot[2] board_pose.pose.orientation.w = tf_rot[3] rospy.logdebug("%s" % board_pose) #we'll publish the pose so we can display it in rviz self.pose_pub.publish(board_pose) return (board_pose, corners)
def calibrate(self): self.projector_to_camera_translation_vector = cv.CreateMat( 3, 1, cv.CV_32FC1) self.projector_to_camera_rotation_vector = cv.CreateMat( 3, 1, cv.CV_32FC1) cv.SetZero(self.projector_to_camera_translation_vector) cv.SetZero(self.projector_to_camera_rotation_vector) for i in range(self.number_of_scenes): camera_tvec = cv.CreateMat(3, 1, cv.CV_32FC1) camera_rvec = cv.CreateMat(3, 1, cv.CV_32FC1) cv.FindExtrinsicCameraParams2(self.object_points[i], self.camera_image_points[i], self.camera_model.intrinsicMatrix(), self.camera_model.distortionCoeffs(), camera_rvec, camera_tvec) print "Camera To Board Vector:" for row in range(camera_tvec.height): for col in range(camera_tvec.width): print camera_tvec[row, col], print print projector_tvec = cv.CreateMat(3, 1, cv.CV_32FC1) projector_rvec = cv.CreateMat(3, 1, cv.CV_32FC1) cv.FindExtrinsicCameraParams2( self.object_points[i], self.projector_image_points[i], self.projector_model.intrinsicMatrix(), self.projector_model.distortionCoeffs(), projector_rvec, projector_tvec) print "Projector To Board Vector:" for row in range(projector_tvec.height): for col in range(projector_tvec.width): print projector_tvec[row, col], print print camera_rmat = cv.CreateMat(3, 3, cv.CV_32FC1) cv.Rodrigues2(camera_rvec, camera_rmat) projector_rmat = cv.CreateMat(3, 3, cv.CV_32FC1) cv.Rodrigues2(projector_rvec, projector_rmat) scene_projector_to_camera_rotation_matrix = cv.CreateMat( 3, 3, cv.CV_32FC1) cv.GEMM(camera_rmat, projector_rmat, 1, None, 0, scene_projector_to_camera_rotation_matrix, cv.CV_GEMM_B_T) scene_projector_to_camera_rotation_vector = cv.CreateMat( 3, 1, cv.CV_32FC1) for i in range(3): for j in range(3): print scene_projector_to_camera_rotation_matrix[i, j], print print cv.Rodrigues2(scene_projector_to_camera_rotation_matrix, scene_projector_to_camera_rotation_vector) print "Scene Rotation Vector:" for row in range(scene_projector_to_camera_rotation_vector.height): for col in range( scene_projector_to_camera_rotation_vector.width): print scene_projector_to_camera_rotation_vector[row, col], print print scene_projector_to_camera_translation_vector = cv.CreateMat( 3, 1, cv.CV_32FC1) cv.GEMM(projector_rmat, projector_tvec, -1, None, 0, scene_projector_to_camera_translation_vector, cv.CV_GEMM_A_T) cv.GEMM(camera_rmat, scene_projector_to_camera_translation_vector, 1, camera_tvec, 1, scene_projector_to_camera_translation_vector, 0) print "Scene Translation Vector:" for row in range( scene_projector_to_camera_translation_vector.height): for col in range( scene_projector_to_camera_translation_vector.width): print scene_projector_to_camera_translation_vector[row, col], print print cv.Add(scene_projector_to_camera_translation_vector, self.projector_to_camera_translation_vector, self.projector_to_camera_translation_vector) cv.Add(scene_projector_to_camera_rotation_vector, self.projector_to_camera_rotation_vector, self.projector_to_camera_rotation_vector) cv.ConvertScale(self.projector_to_camera_translation_vector, self.projector_to_camera_translation_vector, scale=1.0 / self.number_of_scenes) cv.ConvertScale(self.projector_to_camera_rotation_vector, self.projector_to_camera_rotation_vector, scale=1.0 / self.number_of_scenes) print "Final Translation Vector:" for row in range(self.projector_to_camera_translation_vector.height): for col in range( self.projector_to_camera_translation_vector.width): print self.projector_to_camera_translation_vector[row, col], print print print "Final Rotation Vector:" for row in range(self.projector_to_camera_rotation_vector.height): for col in range(self.projector_to_camera_rotation_vector.width): print self.projector_to_camera_rotation_vector[row, col], print
def qsolve(C, Q, matches, dbsiftpath, dbimgpath): # Get image information. info = os.path.join(C.infodir, os.path.basename(dbimgpath)[:-4] + '.info') qsource = render_tags.QueryImageInfo(Q.datasource) dbsource = render_tags.EarthmineImageInfo(dbimgpath, info) map3d = C.pixelmap.open(dbsiftpath) # Get 2d pixels and 3d locations of feature inliers matches = [(m['query'][0],m['query'][1],m['db'][0],m['db'][1]) for m in matches] matches = list(set(matches)) q2d = [arr([m[0],m[1]]) for m in matches] db2d = [arr([m[2],m[3]]) for m in matches] db3d = [map3d[int(d[0]),int(d[1])] for d in db2d] i = 0 while i < len(db3d): if db3d[i] is None: q2d.pop(i) db2d.pop(i) db3d.pop(i) else: i = i+1 olat,olon,oalt = dbsource.lat,dbsource.lon,dbsource.alt qlat,qlon = qsource.lat,qsource.lon qzx = geom.lltom(olat,olon,qlat,qlon) zx = [geom.lltom(olat,olon,d['lat'],d['lon']) for d in db3d] y = [dbsource.alt-d['alt'] for d in db3d] xyz = [[zx[i][1],y[i],zx[i][0]] for i in range(len(y))] print len(xyz) # Set K, Rhat wx,wy = qsource.pgmsize[0], qsource.pgmsize[1] tx,ty = qsource.view_angle[0], qsource.view_angle[1] f1, f2 = (wx-1)/(2*np.tan(tx/2)), (wy-1)/(2*np.tan(ty/2)) f = (f1+f2) / 2 K = arr([[f,0,(wx-1)/2.0], [0,f,(wy-1)/2.0], [0,0,1]]) y,p,r = qsource.yaw, qsource.pitch, qsource.roll print [180*y/np.pi,180*p/np.pi,180*r/np.pi] Ry = arr([[np.cos(y),0,np.sin(y)], [0,1,0], [-np.sin(y),0,np.cos(y)]]) Rx = arr([[1,0,0], [0,np.cos(p),-np.sin(p)], [0,np.sin(p),np.cos(p)]]) Rz = arr([[np.cos(r),-np.sin(r),0], [np.sin(r),np.cos(r),0], [0,0,1]]) Rhat = dot(Ry,dot(Rx,Rz)) # camera orientation (camera to world) tRhat = tp(Rhat) # reference camera matrix # f 0 cx # 0 f cy # 0 0 1 A = cv.CreateMat(3, 3, cv.CV_64F) cv.SetZero(A) cv.Set2D(A, 0, 0, cv.Scalar(f)) cv.Set2D(A, 1, 1, cv.Scalar(f)) cv.Set2D(A, 2, 2, cv.Scalar(1)) cv.Set2D(A, 0, 2, cv.Scalar((wx-1)/2.0)) cv.Set2D(A, 1, 2, cv.Scalar((wy-1)/2.0)) # convert 2d, 3d points to cvMats objectPoints3d = cv.CreateMat(len(xyz), 1, cv.CV_64FC3) imagePoints2d = cv.CreateMat(len(xyz), 1, cv.CV_64FC2) for i in range(len(xyz)): cv.Set2D(imagePoints2d, i, 0, cv.Scalar(*q2d[i])) cv.Set2D(objectPoints3d, i, 0, cv.Scalar(*xyz[i])) # set initial rotation and translation vectors, distortion coefficients coeff = cv.CreateMat(4, 1, cv.CV_64F) cv.SetZero(coeff) rmat = cv.CreateMat(3, 3, cv.CV_64F) cv.Set2D(rmat, 0, 0, cv.Scalar(tRhat[0,0])) cv.Set2D(rmat, 0, 1, cv.Scalar(tRhat[0,1])) cv.Set2D(rmat, 0, 2, cv.Scalar(tRhat[0,2])) cv.Set2D(rmat, 1, 0, cv.Scalar(tRhat[1,0])) cv.Set2D(rmat, 1, 1, cv.Scalar(tRhat[1,1])) cv.Set2D(rmat, 1, 2, cv.Scalar(tRhat[1,2])) cv.Set2D(rmat, 2, 0, cv.Scalar(tRhat[2,0])) cv.Set2D(rmat, 2, 1, cv.Scalar(tRhat[2,1])) cv.Set2D(rmat, 2, 2, cv.Scalar(tRhat[2,2])) print 'YPR init for PnP' print 180*np.array(geom.ypr_fromR(Rhat))/np.pi rvec = cv.CreateMat(3, 1, cv.CV_64F) cv.SetZero(rvec) cv.Rodrigues2(rmat,rvec) # convert from rotation matrix to Rodrigues vector tvec = cv.CreateMat(3, 1, cv.CV_64F) cv.SetZero(tvec) # solvepnp ret = cv.FindExtrinsicCameraParams2(objectPoints3d, imagePoints2d, A, coeff, rvec, tvec, useExtrinsicGuess=False) np_rvec = np.matrix(rvec).A cv.Rodrigues2(rvec,rmat) np_rmat = np.transpose(np.matrix(rmat)).A np_tvec = np.dot(np_rmat,np.squeeze(np.matrix(tvec).A)) print 'Rq from PnP' print np_rmat print 'YPR from PnP' print 180*np.array(geom.ypr_fromR(np_rmat))/np.pi return np_rmat, np_tvec
def find_checkerboard_pose(self, ros_image, cam_info): #we need to convert the ros image to an opencv image try: image = self.bridge.imgmsg_to_cv(ros_image, "mono8") except CvBridgeError as e: rospy.logerror("Error importing image %s" % e) return corners, model = self.detect(image) if corners == None or model == None: return None else: #find the pose of the checkerboard rot = cv.CreateMat(3, 1, cv.CV_32FC1) trans = cv.CreateMat(3, 1, cv.CV_32FC1) kc = cv.CreateMat(1, 4, cv.CV_32FC1) cv.Set(kc, 0.0) intrinsic_matrix = self.intrinsic_matrix_from_info(cam_info) cv.FindExtrinsicCameraParams2(model, corners, intrinsic_matrix, kc, rot, trans) #We want to build a transform now, but first we have to convert the #rotation vector we get back from OpenCV into a rotation matrix rot_mat = cv.CreateMat(3, 3, cv.CV_32FC1) cv.Rodrigues2(rot, rot_mat) #Now we need to convert this rotation matrix into a quaternion #This can be done better in never versions of opencv, but we need to be boxturtle compatible numpy_mat = numpy.fromstring(rot_mat.tostring(), dtype=numpy.float32).reshape((3, 3)) #of course, tf expects all matricies passed to it to be 4x4... so we'll build a full matrix here full_pose = numpy.zeros((4, 4)) #first, copy in the rotation matrix full_pose[0:3, 0:3] = numpy_mat[0:3, 0:3] #next, we'll copy in the translation full_pose[0:3, 3] = [trans[i, 0] for i in range(3)] #and make sure to add a 1 in the lower right corner full_pose[3][3] = 1.0 rospy.logdebug("%s" % numpy_mat) rospy.logdebug("%s" % full_pose) tf_trans = tf.transformations.translation_from_matrix(full_pose) tf_rot = tf.transformations.quaternion_from_matrix(full_pose) #and now we'll actually build our pose stamped board_pose = geometry_msgs.msg.PoseStamped() board_pose.header = ros_image.header board_pose.pose.position.x = tf_trans[0] board_pose.pose.position.y = tf_trans[1] board_pose.pose.position.z = tf_trans[2] board_pose.pose.orientation.x = tf_rot[0] board_pose.pose.orientation.y = tf_rot[1] board_pose.pose.orientation.z = tf_rot[2] board_pose.pose.orientation.w = tf_rot[3] rospy.logdebug("%s" % board_pose) #we'll publish the pose so we can display it in rviz # self.pose_pub.publish(board_pose) return board_pose
def dbsolve(C, Rhat, matches, dbsiftpath, dbimgpath): # Get image information. info = os.path.join(C.infodir, os.path.basename(dbimgpath)[:-4] + '.info') dbsource = render_tags.EarthmineImageInfo(dbimgpath, info) map3d = C.pixelmap.open(dbsiftpath) # Get 2d pixels and 3d locations of feature inliers matches = [(m['query'][0],m['query'][1],m['db'][0],m['db'][1]) for m in matches] matches = list(set(matches)) db2d = [arr([m[2],m[3]]) for m in matches] db3d = [map3d[int(d[0]),int(d[1])] for d in db2d] i = 0 while i < len(db3d): if db3d[i] is None: db2d.pop(i) db3d.pop(i) else: i = i+1 olat,olon,oalt = dbsource.lat,dbsource.lon,dbsource.alt zx = [geom.lltom(olat,olon,d['lat'],d['lon']) for d in db3d] y = [dbsource.alt-d['alt'] for d in db3d] xyz = [[zx[i][1],y[i],zx[i][0]] for i in range(len(y))] # reference camera matrix Kd wx,wy = dbsource.image.size fov = dbsource.fov f = (wx-1)/(2*np.tan(fov/2)) # f 0 cx # 0 f cy # 0 0 1 A = cv.CreateMat(3, 3, cv.CV_64F) cv.SetZero(A) cv.Set2D(A, 0, 0, cv.Scalar(f)) cv.Set2D(A, 1, 1, cv.Scalar(f)) cv.Set2D(A, 2, 2, cv.Scalar(1)) cv.Set2D(A, 0, 2, cv.Scalar((wx-1)/2.0)) cv.Set2D(A, 1, 2, cv.Scalar((wy-1)/2.0)) # convert 2d, 3d points to cvMats objectPoints3d = cv.CreateMat(len(xyz), 1, cv.CV_64FC3) imagePoints2d = cv.CreateMat(len(xyz), 1, cv.CV_64FC2) for i in range(len(xyz)): cv.Set2D(imagePoints2d, i, 0, cv.Scalar(*db2d[i])) cv.Set2D(objectPoints3d, i, 0, cv.Scalar(*xyz[i])) # set initial rotation and translation vectors, distortion coefficients coeff = cv.CreateMat(4, 1, cv.CV_64F) cv.SetZero(coeff) rmat = cv.CreateMat(3, 3, cv.CV_64F) tRhat = tp(Rhat) cv.Set2D(rmat, 0, 0, cv.Scalar(tRhat[0,0])) cv.Set2D(rmat, 0, 1, cv.Scalar(tRhat[0,1])) cv.Set2D(rmat, 0, 2, cv.Scalar(tRhat[0,2])) cv.Set2D(rmat, 1, 0, cv.Scalar(tRhat[1,0])) cv.Set2D(rmat, 1, 1, cv.Scalar(tRhat[1,1])) cv.Set2D(rmat, 1, 2, cv.Scalar(tRhat[1,2])) cv.Set2D(rmat, 2, 0, cv.Scalar(tRhat[2,0])) cv.Set2D(rmat, 2, 1, cv.Scalar(tRhat[2,1])) cv.Set2D(rmat, 2, 2, cv.Scalar(tRhat[2,2])) rvec = cv.CreateMat(3, 1, cv.CV_64F) cv.SetZero(rvec) cv.Rodrigues2(rmat,rvec) # convert from rotation matrix to Rodrigues vector tvec = cv.CreateMat(3, 1, cv.CV_64F) cv.SetZero(tvec) #print Rhat # solvepnp ret = cv.FindExtrinsicCameraParams2(objectPoints3d, imagePoints2d, A, coeff, rvec, tvec, useExtrinsicGuess=False) np_rvec = np.matrix(rvec).A cv.Rodrigues2(rvec,rmat) np_rmat = np.transpose(np.matrix(rmat)).A np_tvec = np.dot(np_rmat,np.squeeze(np.matrix(tvec).A)) return np_rmat, np_tvec
def calibrateFromObservations(self, observations, calibextrinsic=True, pruneoutliers=True, full_output=True, fixprincipalpoint=False, Tcameraestimate=None): while True: if len(observations) < 3: raise self.CalibrationError('very little observations: %d!' % len(observations), action=1) object_points = self.getObjectPoints() all_object_points = [] all_corners = [] imagesize = None for o in observations: all_object_points.append(object_points) all_corners.append(o['corners_raw']) if imagesize: assert imagesize[0] == o['image_raw'].shape[ 1] and imagesize[1] == o['image_raw'].shape[0] else: imagesize = (o['image_raw'].shape[1], o['image_raw'].shape[0]) intrinsiccondition = self.validateCalibrationData( all_object_points, all_corners) if intrinsiccondition is None: raise self.CalibrationError( 'bad condition number, %d observations' % (len(observations)), action=1) KKorig, kcorig, Traws, error_intrinsic, error_grad = self.calibrateIntrinsicCamera( all_object_points, all_corners, imagesize, fixprincipalpoint=fixprincipalpoint, computegradients=True) cvKKorig = cv.fromarray(KKorig) cvkcorig = cv.fromarray(kcorig) thresh = median(error_intrinsic) + 0.5 if any(error_intrinsic > thresh): # compute again using a pruned set of observations print 'pruning observations (thresh=%f) because intrinsic error is: ' % thresh, error_intrinsic observations = [ o for i, o in enumerate(observations) if error_intrinsic[i] <= thresh ] else: if mean(error_intrinsic) > 0.6: raise self.CalibrationError( 'intrinsic error is huge (%s)! giving up, KK=(%s)' % (str(error_intrinsic), str(KKorig))) break if not calibextrinsic: return KKorig, kcorig, None, { 'error_intrinsic': error_intrinsic, 'intrinsiccondition': intrinsiccondition, 'error_grad': error_grad } if Tcameraestimate is None: raise TypeError( 'Tcameraestimate needs to be initialized to a 4x4 matrix') # unwarp all images and re-detect the checkerboard patterns cvKK = cv.CreateMat(3, 3, cv.CV_64F) cv.GetOptimalNewCameraMatrix(cvKKorig, cvkcorig, imagesize, 0, cvKK) cvUndistortionMapX = cv.CreateMat(imagesize[1], imagesize[0], cv.CV_32F) cvUndistortionMapY = cv.CreateMat(imagesize[1], imagesize[0], cv.CV_32F) cv.InitUndistortRectifyMap(cvKKorig, cvkcorig, cv.fromarray(eye(3)), cvKK, cvUndistortionMapX, cvUndistortionMapY) KK = array(cvKK) KKinv = linalg.inv(KK) if full_output: print 'KKorig: ', KKorig, ' KK:', KK cvimage = None cvkczero = cv.fromarray(zeros(kcorig.shape)) cvrvec = cv.CreateMat(1, 3, cv.CV_64F) cvtvec = cv.CreateMat(1, 3, cv.CV_64F) all_optimization_data = [] Tworldpatternestimates = [] for i, o in enumerate(observations): cvimage_dist = cv.fromarray(o['image_raw']) if not cvimage: cvimage = cv.CloneMat(cvimage_dist) cv.Remap(cvimage_dist, cvimage, cvUndistortionMapX, cvUndistortionMapY, cv.CV_INTER_LINEAR + cv.CV_WARP_FILL_OUTLIERS) corners = self.detect(cvimage) if corners is not None: cv.FindExtrinsicCameraParams2(cv.fromarray(object_points), cv.fromarray(corners), cvKK, cvkczero, cvrvec, cvtvec) T = matrixFromAxisAngle(array(cvrvec)[0]) T[0:3, 3] = array(cvtvec)[0] Tworldpatternestimates.append( dot(dot(o['Tlink'], Tcameraestimate), T)) all_optimization_data.append( (transformPoints(KKinv, corners), linalg.inv(o['Tlink']))) else: print 'could not detect original pattern ', i # have the following equation: Tlink * Tcamera * Tdetectedpattern * corners3d = Tworldpattern * corners3d # need to solve for Tcamera and Tworldpattern # instead of using Tdetectedpattern, use projected difference: # corners - proj( inv(Tcamera) * inv(Tlink) * Tworldpattern *corners3d) corners3d = self.getObjectPoints() quatWorldPatternEstimates = array([ quatFromRotationMatrix(T[0:3, 0:3]) for T in Tworldpatternestimates ]) quatWorldPatternInitial, success = leastsq( lambda q: quatArrayTDist(q / sqrt(sum(q**2)), quatWorldPatternEstimates), quatWorldPatternEstimates[0], maxfev=100000, epsfcn=1e-6) rWorldPatternInitial = zeros(6, float64) rWorldPatternInitial[0:3] = axisAngleFromQuat(quatWorldPatternInitial) rWorldPatternInitial[3:6] = mean( array([T[0:3, 3] for T in Tworldpatternestimates]), 0) Tcameraestimateinv = linalg.inv(Tcameraestimate) rCameraInitial = zeros(6, float64) rCameraInitial[0:3] = axisAngleFromRotationMatrix( Tcameraestimateinv[0:3, 0:3]) rCameraInitial[3:6] = Tcameraestimateinv[0:3, 3] def errorfn(x, optimization_data): Tworldpattern = matrixFromAxisAngle(x[0:3]) Tworldpattern[0:3, 3] = x[3:6] Tcamerainv = matrixFromAxisAngle(x[6:9]) Tcamerainv[0:3, 3] = x[9:12] err = zeros(len(optimization_data) * len(corners3d) * 2) off = 0 for measuredcameracorners, Tlinkinv in optimization_data: cameracorners3d = transformPoints( dot(dot(Tcamerainv, Tlinkinv), Tworldpattern), corners3d) iz = 1.0 / cameracorners3d[:, 2] err[off:( off + len(corners3d) )] = measuredcameracorners[:, 0] - cameracorners3d[:, 0] * iz off += len(corners3d) err[off:( off + len(corners3d) )] = measuredcameracorners[:, 1] - cameracorners3d[:, 1] * iz off += len(corners3d) if full_output: print 'rms: ', sqrt(sum(err**2)) return err optimization_data = all_optimization_data xorig, cov_x, infodict, mesg, iter = leastsq( lambda x: errorfn(x, all_optimization_data), r_[rWorldPatternInitial, rCameraInitial], maxfev=100000, epsfcn=1e-6, full_output=1) if pruneoutliers: # prune the images with the most error errors = reshape( errorfn(xorig, all_optimization_data)**2, (len(all_optimization_data), len(corners3d) * 2)) errorreprojection = mean( sqrt(KK[0, 0]**2 * errors[:, 0:len(corners3d)] + KK[1, 1]**2 * errors[:, len(corners3d):]), 1) #thresh=mean(errorreprojection)+std(errorreprojection) thresh = median( errorreprojection) + 20 #0.5*std(errorreprojection) print 'thresh:', thresh, 'errors:', errorreprojection optimization_data = [ all_optimization_data[i] for i in flatnonzero(errorreprojection <= thresh) ] x, cov_x, infodict, mesg, iter = leastsq( lambda x: errorfn(x, optimization_data), xorig, maxfev=100000, epsfcn=1e-6, full_output=1) else: x = xorig Tcamerainv = matrixFromAxisAngle(x[6:9]) Tcamerainv[0:3, 3] = x[9:12] Tcamera = linalg.inv(Tcamerainv) Tworldpattern = matrixFromAxisAngle(x[0:3]) Tworldpattern[0:3, 3] = x[3:6] points3d = self.Compute3DObservationPoints(Tcamerainv, optimization_data) if full_output: errors = reshape( errorfn(x, optimization_data)**2, (len(optimization_data), len(corners3d) * 2)) error_reprojection = sqrt( KK[0, 0]**2 * errors[:, 0:len(corners3d)] + KK[1, 1]**2 * errors[:, len(corners3d):]).flatten() print 'final reprojection error (pixels): ', mean( error_reprojection), std(error_reprojection) error_3d = sqrt( sum((transformPoints(Tworldpattern, corners3d) - points3d)**2, 1)) print '3d error: ', mean(error_3d), std(error_3d) return KKorig, kcorig, Tcamera, { 'error_reprojection': error_reprojection, 'error_3d': error_3d, 'error_intrinsic': error_intrinsic, 'intrinsiccondition': intrinsiccondition, 'error_grad': error_grad, 'KK': array(cvKK) } return KKorig, kcorig, Tcamera, None
class ImageCbDetectorNode: def __init__(self): self.mutex = threading.RLock() self.corners_x = rospy.get_param('~corners_x', 6) self.corners_y = rospy.get_param('~corners_y', 6) self.spacing_x = rospy.get_param('~spacing_x', 0.022) self.spacing_y = rospy.get_param('~spacing_y', 0.022) self.publish_period = rospy.Duration( rospy.get_param('~publish_period', 0)) self.last_publish_time = rospy.Time() self.width_scaling = rospy.get_param('~width_scaling', 1) self.height_scaling = rospy.get_param('~height_scaling', 1) self.im_cb_detector = ImageCbDetector() self.pose_srv = None self.bridge = CvBridge() self.cam_info = None self.ros_image = None self.image_sub = rospy.Subscriber("image_stream", Image, self.callback) self.caminfo_sub = rospy.Subscriber("camera_info", CameraInfo, self.cam_info_cb) self.pose_pub = rospy.Publisher("board_pose", PoseStamped) def intrinsic_matrix_from_info(self, cam_info): intrinsic_matrix = cv.CreateMat(3, 3, cv.CV_32FC1) #Because we only want the upper 3x3 (normal) portion of the rectified intrinsic matrix for i in range(0, 3): for j in range(0, 3): intrinsic_matrix[i, j] = cam_info.P[4 * i + j] return intrinsic_matrix def attempt_to_advertise(self): #we'll only advertise the service after we've received a cam_info/image pair if self.ros_image != None and self.cam_info != None: self.pose_srv = rospy.Service("get_checkerboard_pose", GetCheckerboardPose, self.find_checkerboard_service) def cam_info_cb(self, cam_info): with self.mutex: self.cam_info = cam_info if self.pose_srv == None: self.attempt_to_advertise() def callback(self, ros_image): #copy over the latest image to be used when a service call is made with self.mutex: self.ros_image = ros_image if self.pose_srv == None: self.attempt_to_advertise() if rospy.Duration( ) != self.publish_period and rospy.rostime.get_rostime( ) - self.last_publish_time > self.publish_period: pose = self.find_checkerboard_pose( ros_image, self.corners_x, self.corners_y, self.spacing_x, self.spacing_y, self.width_scaling, self.height_scaling) if pose: self.pose_pub.publish(pose) self.last_publish_time = rospy.rostime.get_rostime() def find_checkerboard_service(self, req): #copy the image over with self.mutex: image = self.ros_image pose = self.find_checkerboard_pose(image, req.corners_x, req.corners_y, req.spacing_x, req.spacing_y, self.width_scaling, self.height_scaling) return GetCheckerboardPoseResponse(pose) def find_checkerboard_pose(self, ros_image, corners_x, corners_y, spacing_x, spacing_y, width_scaling, height_scaling): #we need to convert the ros image to an opencv image try: image = self.bridge.imgmsg_to_cv(ros_image, "mono8") except CvBridgeError, e: rospy.logerror("Error importing image %s" % e) return corners, model = self.im_cb_detector.detect(image, corners_x, corners_y, spacing_x, spacing_y, width_scaling, height_scaling) if corners != None and model != None and self.cam_info != None: #find the pose of the checkerboard rot = cv.CreateMat(3, 1, cv.CV_32FC1) trans = cv.CreateMat(3, 1, cv.CV_32FC1) with self.mutex: kc = cv.CreateMat(1, 4, cv.CV_32FC1) cv.Set(kc, 0.0) intrinsic_matrix = self.intrinsic_matrix_from_info( self.cam_info) cv.FindExtrinsicCameraParams2(model, corners, intrinsic_matrix, kc, rot, trans) #We want to build a transform now, but first we have to convert the #rotation vector we get back from OpenCV into a rotation matrix rot_mat = cv.CreateMat(3, 3, cv.CV_32FC1) cv.Rodrigues2(rot, rot_mat) #Now we need to convert this rotation matrix into a quaternion #This can be done better in never versions of opencv, but we need to be boxturtle compatible numpy_mat = numpy.fromstring(rot_mat.tostring(), dtype=numpy.float32).reshape((3, 3)) #of course, tf expects all matricies passed to it to be 4x4... so we'll build a full matrix here full_pose = numpy.zeros((4, 4)) #first, copy in the rotation matrix full_pose[0:3, 0:3] = numpy_mat[0:3, 0:3] #next, we'll copy in the translation full_pose[0:3, 3] = [trans[i, 0] for i in range(3)] #and make sure to add a 1 in the lower right corner full_pose[3][3] = 1.0 rospy.logdebug("%s" % numpy_mat) rospy.logdebug("%s" % full_pose) tf_trans = tf.transformations.translation_from_matrix(full_pose) tf_rot = tf.transformations.quaternion_from_matrix(full_pose) #and now we'll actually build our pose stamped board_pose = PoseStamped() board_pose.header = ros_image.header board_pose.pose.position.x = tf_trans[0] board_pose.pose.position.y = tf_trans[1] board_pose.pose.position.z = tf_trans[2] board_pose.pose.orientation.x = tf_rot[0] board_pose.pose.orientation.y = tf_rot[1] board_pose.pose.orientation.z = tf_rot[2] board_pose.pose.orientation.w = tf_rot[3] rospy.logdebug("%s" % board_pose) #we'll publish the pose so we can display it in rviz self.pose_pub.publish(board_pose) return board_pose