def estimateTrajectoryGradientDescent(self): CAMERA_FOV = 44.0 * math.pi / 180.0 HALF_CAMERA_FOV = CAMERA_FOV / 2.0 COS_OF_HALF_CAMERA_FOV = math.cos(HALF_CAMERA_FOV) CAMERA_WIDTH = 320.0 HALF_CAMERA_WIDTH = CAMERA_WIDTH / 2.0 CAMERA_HEIGHT = 240.0 HALF_CAMERA_HEIGHT = CAMERA_HEIGHT / 2.0 FOCAL_DISTANCE = CAMERA_WIDTH / (2.0 * math.tan(CAMERA_FOV / 2.0)) FOCAL_HYP_X = math.sqrt(FOCAL_DISTANCE * FOCAL_DISTANCE + HALF_CAMERA_WIDTH * HALF_CAMERA_WIDTH) # Initial guesses for position and orientation poseGuesses = [] for poseIdx in range(self.numFrames): poseGuesses.append([0.0, 0.0, 0.0]) # For each frame work out where the landmarks are projected numEntities = len(self.testSequence.fixedEntities) landmarkPositions = [(e.x, e.y) for e in self.testSequence.fixedEntities] landmarkBearings = [] landmarkVisibility = [] for frameIdx in range(self.numFrames): frameData = self.testSequence.frames[frameIdx] bearingArray = [] visibilityArray = [] for entityIdx in range(numEntities): entity = self.testSequence.fixedEntities[entityIdx] bearing = self.bearingToFeature(frameData.subX, frameData.subY, frameData.subYaw, entity.x, entity.y) bearingArray.append(bearing) # Entity is visible in this frame visibilityArray.append(math.cos(bearing) > COS_OF_HALF_CAMERA_FOV) landmarkBearings.append(bearingArray) landmarkVisibility.append(visibilityArray) # Use bundle adjustment to estimate camera positions self.estimator = TrajectoryEstimator() totalError = self.estimator.estimateTrajectory( poseGuesses, landmarkPositions, landmarkBearings, landmarkVisibility ) print "Average Error =", totalError / self.numFrames
class MainWindow: SCALE_UP = 1.25 SCALE_DOWN = 1.0 / SCALE_UP MIN_SCALE = 10.0 MAX_SCALE = 1000000.0 # --------------------------------------------------------------------------- def __init__(self): self.cameraDisplayPixBuf = None self.settingFrame = False # Load in a test sequence self.testSequenceDir = "../../data/Sequences" self.testSequence = TestSequence.loadSequenceFromFile(self.testSequenceDir + "/BRLSimTest.yaml") self.numFrames = len(self.testSequence.frames) self.displayOriginX = 150 self.displayOriginY = 200 self.displayScale = 100 self.draggingDisplay = False self.lastDragPos = None # Setup the GUI builder = gtk.Builder() builder.add_from_file(os.path.dirname(__file__) + "/TrajectoryViewer.glade") self.window = builder.get_object("winMain") self.dwgTrajectoryDisplay = builder.get_object("dwgTrajectoryDisplay") self.dwgCameraDisplay = builder.get_object("dwgCameraDisplay") self.tbxFrameNumber = builder.get_object("tbxFrameNumber") self.lblNumFrames = builder.get_object("lblNumFrames") self.lblNumFrames.set_text("/" + str(self.numFrames)) builder.connect_signals(self) self.setCurFrameIdx(0) self.window.show() self.estimateTrajectoryGradientDescent() # --------------------------------------------------------------------------- def onWinMainDestroy(self, widget, data=None): gtk.main_quit() # --------------------------------------------------------------------------- def main(self): # All PyGTK applications must have a gtk.main(). Control ends here # and waits for an event to occur (like a key press or mouse event). gtk.main() # --------------------------------------------------------------------------- def onDwgTrajectoryDisplayExposeEvent(self, widget, event): (windowWidth, windowHeight) = widget.window.get_size() originX = self.displayOriginX originY = self.displayOriginY scaleX = self.displayScale scaleY = -self.displayScale numPoints = self.curFrameIdx - 1 pointList = [ ( int(originX + scaleX * self.testSequence.frames[i].subX), int(originY + scaleY * self.testSequence.frames[i].subY), ) for i in range(self.curFrameIdx) ] estimatedTrajectoryPointList = [ ( int(originX + scaleX * self.estimator.poseGuesses[i][0]), int(originY + scaleY * self.estimator.poseGuesses[i][1]), ) for i in range(self.curFrameIdx) ] graphicsContext = widget.window.new_gc() graphicsContext.set_rgb_fg_color(gtk.gdk.Color(65535, 65535, 65535)) widget.window.draw_rectangle(graphicsContext, filled=True, x=0, y=0, width=windowWidth, height=windowHeight) # Draw the landmarks for fixedEntity in self.testSequence.fixedEntities: x = int(originX + scaleX * fixedEntity.x) y = int(originY + scaleY * fixedEntity.y) innerRadius = 0.05 * scaleX outerRadius = 0.25 * scaleX graphicsContext.set_rgb_fg_color(gtk.gdk.Color(0, 0, 0)) self.drawCircle(widget.window, graphicsContext, x, y, innerRadius, filled=True) # graphicsContext.set_rgb_fg_color( gtk.gdk.Color( 65535/2, 65535/2, 65535/2 ) ) # self.drawCircle( widget.window, graphicsContext, x, y, outerRadius, filled = False ) # Draw the Sub... # graphicsContext.set_rgb_fg_color( gtk.gdk.Color( 0, 0, 0 ) ) # subPos = ( originX + scaleX*0.0, originY + scaleY*0.0 ) # self.drawCircle( widget.window, graphicsContext, # subPos[ 0 ], subPos[ 1 ], # 0.05*self.displayScale, filled=True ) # yaw = math.pi/3.0 # headingX = math.cos( yaw + math.pi/2.0 )*scaleX*0.10 # headingY = math.sin( yaw + math.pi/2.0 )*scaleY*0.10 # widget.window.draw_line( graphicsContext, # subPos[ 0 ], subPos[ 1 ], # subPos[ 0 ] + headingX, subPos[ 1 ] + headingY ) landmarkPositions = [(e.x, e.y) for e in self.testSequence.fixedEntities] if numPoints > 0: graphicsContext.set_rgb_fg_color(gtk.gdk.Color(0, 0, 0)) graphicsContext.set_line_attributes(2, gtk.gdk.LINE_SOLID, gtk.gdk.CAP_BUTT, gtk.gdk.JOIN_BEVEL) widget.window.draw_points(graphicsContext, pointList) lastPoint = pointList[-1] self.drawCircle( widget.window, graphicsContext, lastPoint[0], lastPoint[1], 0.05 * self.displayScale, filled=True ) yaw = self.testSequence.frames[self.curFrameIdx - 1].subYaw headingX = math.cos(yaw + math.pi / 2.0) * scaleX * 0.10 headingY = math.sin(yaw + math.pi / 2.0) * scaleY * 0.10 widget.window.draw_line( graphicsContext, lastPoint[0], lastPoint[1], lastPoint[0] + headingX, lastPoint[1] + headingY ) graphicsContext.set_rgb_fg_color(gtk.gdk.Color(65535, 0, 0)) graphicsContext.set_line_attributes(2, gtk.gdk.LINE_SOLID, gtk.gdk.CAP_BUTT, gtk.gdk.JOIN_BEVEL) widget.window.draw_points(graphicsContext, estimatedTrajectoryPointList) lastEstimatedPoint = estimatedTrajectoryPointList[-1] self.drawCircle( widget.window, graphicsContext, lastEstimatedPoint[0], lastEstimatedPoint[1], 0.05 * self.displayScale, filled=True, ) yaw = self.estimator.poseGuesses[self.curFrameIdx - 1][2] headingX = math.cos(yaw + math.pi / 2.0) * scaleX * 0.10 headingY = math.sin(yaw + math.pi / 2.0) * scaleY * 0.10 widget.window.draw_line( graphicsContext, lastEstimatedPoint[0], lastEstimatedPoint[1], lastEstimatedPoint[0] + headingX, lastEstimatedPoint[1] + headingY, ) # --------------------------------------------------------------------------- def onDwgCameraDisplayExposeEvent(self, widget, event): if self.cameraDisplayPixBuf != None: imgRect = self.getImageRectangleInWidget(widget) imgOffsetX = imgRect.x imgOffsetY = imgRect.y # Get the total area that needs to be redrawn imgRect = imgRect.intersect(event.area) srcX = imgRect.x - imgOffsetX srcY = imgRect.y - imgOffsetY widget.window.draw_pixbuf( widget.get_style().fg_gc[gtk.STATE_NORMAL], self.cameraDisplayPixBuf, srcX, srcY, imgRect.x, imgRect.y, imgRect.width, imgRect.height, ) # --------------------------------------------------------------------------- def onBtnPrevFrameClicked(self, widget, data=None): self.setCurFrameIdx(self.curFrameIdx - 1) # --------------------------------------------------------------------------- def onBtnNextFrameClicked(self, widget, data=None): self.setCurFrameIdx(self.curFrameIdx + 1) # --------------------------------------------------------------------------- def onTbxFrameNumberFocusOut(self, widget, data=None): try: self.setCurFrameIdx(int(self.tbxFrameNumber.get_text()) - 1) except: pass # Catch errors that may occur whilst parsing an integer # --------------------------------------------------------------------------- def onTbxFrameNumberKeyPressed(self, widget, keyPressEvent): if gtk.gdk.keyval_name(keyPressEvent.keyval) == "Return": self.onTbxFrameNumberFocusOut(widget) # --------------------------------------------------------------------------- def onDwgTrajectoryDisplayScrollEvent(self, widget, event=None): if event.direction == gtk.gdk.SCROLL_UP: self.displayScale *= self.SCALE_UP else: self.displayScale *= self.SCALE_DOWN if self.displayScale < self.MIN_SCALE: self.displayScale = self.MIN_SCALE if self.displayScale > self.MAX_SCALE: self.displayScale = self.MAX_SCALE self.dwgTrajectoryDisplay.queue_draw() # --------------------------------------------------------------------------- def onDwgTrajectoryDisplayButtonPressEvent(self, widget, event=None): if event.button == 1: self.draggingDisplay = True self.lastDragPos = (event.x, event.y) # --------------------------------------------------------------------------- def onDwgTrajectoryDisplayButtonReleaseEvent(self, widget, event=None): if event.button == 1: self.draggingDisplay = False # --------------------------------------------------------------------------- def onDwgTrajectoryDisplayMotionNotifyEvent(self, widget, event=None): if self.draggingDisplay == True: newDragPos = (event.x, event.y) if newDragPos != self.lastDragPos: self.displayOriginX += newDragPos[0] - self.lastDragPos[0] self.displayOriginY += newDragPos[1] - self.lastDragPos[1] self.lastDragPos = newDragPos self.dwgTrajectoryDisplay.queue_draw() # --------------------------------------------------------------------------- def drawCircle(self, drawable, graphicsContext, x, y, radius, filled): topLeftX = int(x - radius) topLeftY = int(y - radius) width = height = int(radius * 2) drawable.draw_arc(graphicsContext, filled, topLeftX, topLeftY, width, height, 0, 360 * 64) # --------------------------------------------------------------------------- def getImageRectangleInWidget(self, widget): # Centre the image inside the widget widgetX, widgetY, widgetWidth, widgetHeight = widget.get_allocation() imgRect = gtk.gdk.Rectangle(0, 0, widgetWidth, widgetHeight) (requestedWidth, requestedHeight) = widget.get_size_request() if widgetWidth > requestedWidth: imgRect.x = (widgetWidth - requestedWidth) / 2 imgRect.width = requestedWidth if widgetHeight > requestedHeight: imgRect.y = (widgetHeight - requestedHeight) / 2 imgRect.height = requestedHeight return imgRect # --------------------------------------------------------------------------- def setCurFrameIdx(self, frameIdx): if self.settingFrame: raise "NotReentrant" self.settingFrame = True # Clip the frame index to a valid number if frameIdx < 0: frameIdx = 0 elif frameIdx >= self.numFrames: frameIdx = self.numFrames - 1 # Display the selected frame self.curFrameIdx = frameIdx imageFilename = self.testSequenceDir + "/" + self.testSequence.frames[self.curFrameIdx].imageFilename bgrImage = cv.LoadImage(imageFilename) self.curFrameImage = cv.CreateImage((bgrImage.width, bgrImage.height), cv.IPL_DEPTH_8U, 3) cv.CvtColor(bgrImage, self.curFrameImage, cv.CV_BGR2RGB) self.cameraDisplayPixBuf = gtk.gdk.pixbuf_new_from_data( self.curFrameImage.tostring(), gtk.gdk.COLORSPACE_RGB, False, self.curFrameImage.depth, self.curFrameImage.width, self.curFrameImage.height, self.curFrameImage.width * self.curFrameImage.nChannels, ) # Update the text-box that displays the current frame number self.tbxFrameNumber.set_text(str(self.curFrameIdx + 1)) # Resize the drawing area if necessary if self.dwgCameraDisplay.get_size_request() != (self.curFrameImage.width, self.curFrameImage.height): self.dwgCameraDisplay.set_size_request(self.curFrameImage.width, self.curFrameImage.height) # Redraw the frames self.dwgTrajectoryDisplay.queue_draw() self.dwgCameraDisplay.queue_draw() self.settingFrame = False # --------------------------------------------------------------------------- def estimateTrajectory(self): CAMERA_FOV = 44.0 * math.pi / 180.0 HALF_CAMERA_FOV = CAMERA_FOV / 2.0 COS_OF_HALF_CAMERA_FOV = math.cos(HALF_CAMERA_FOV) CAMERA_WIDTH = 320.0 HALF_CAMERA_WIDTH = CAMERA_WIDTH / 2.0 CAMERA_HEIGHT = 240.0 HALF_CAMERA_HEIGHT = CAMERA_HEIGHT / 2.0 FOCAL_DISTANCE = CAMERA_WIDTH / (2.0 * math.tan(CAMERA_FOV / 2.0)) FOCAL_HYP_X = math.sqrt(FOCAL_DISTANCE * FOCAL_DISTANCE + HALF_CAMERA_WIDTH * HALF_CAMERA_WIDTH) # Create camera parameters # cameraParams = [ FOCAL_DISTANCE, 0.0, CAMERA_WIDTH / 2.0, # 0.0, FOCAL_DISTANCE, CAMERA_HEIGHT / 2.0, # 0.0, 0.0, 1.0 ] cameraParams = [ FOCAL_DISTANCE, # fu CAMERA_WIDTH / 2.0, # u0 CAMERA_HEIGHT / 2.0, # v0 CAMERA_HEIGHT / CAMERA_WIDTH, # Ratio to get from width to height 0.0, ] # Skew # Create initial guesses for the camera poses and landmark positions motionStruct = [0] * 6 * self.numFrames # Place camera guesses at the origin for entity in self.testSequence.fixedEntities: motionStruct.extend([entity.x, entity.y, 0.0]) # Repeat camera rotation guesses as quaternions initialRotations = [1.0, 0.0, 0.0, 0.0] * self.numFrames # For each frame work out where the landmarks are projected numEntities = len(self.testSequence.fixedEntities) landmarkPoints = [] landmarkVisibility = [0] * self.numFrames * numEntities for entityIdx in range(numEntities): entity = self.testSequence.fixedEntities[entityIdx] for frameIdx in range(self.numFrames): frameData = self.testSequence.frames[frameIdx] subHeading = Vector2D(-math.sin(frameData.subYaw), math.cos(frameData.subYaw)) dirToEntity = Normalise(Vector2D(entity.x - frameData.subX, entity.y - frameData.subY)) cosOfAngleToEntity = subHeading.Dot(dirToEntity) if cosOfAngleToEntity > COS_OF_HALF_CAMERA_FOV: # Entity is visible in this frame landmarkVisibility[entityIdx * self.numFrames + frameIdx] = 1 subXAxis = Vector2D(subHeading.y, -subHeading.x) if subXAxis.Dot(dirToEntity) > 0.0: entityPixelX = HALF_CAMERA_WIDTH + cosOfAngleToEntity * FOCAL_HYP_X else: entityPixelX = HALF_CAMERA_WIDTH - cosOfAngleToEntity * FOCAL_HYP_X landmarkPoints.extend([entityPixelX, HALF_CAMERA_HEIGHT]) # Use bundle adjustment to estimate camera positions print len(landmarkPoints) poseResults = RoBoardControl.bundleAdjustment( self.numFrames, numEntities, len(landmarkPoints) / 2, motionStruct, initialRotations, landmarkPoints, landmarkVisibility, cameraParams, ) print poseResults # --------------------------------------------------------------------------- def bearingToFeature(self, subX, subY, subYaw, featureX, featureY): return math.atan2(featureY - subY, featureX - subX) - subYaw # --------------------------------------------------------------------------- def estimateTrajectoryGradientDescent(self): CAMERA_FOV = 44.0 * math.pi / 180.0 HALF_CAMERA_FOV = CAMERA_FOV / 2.0 COS_OF_HALF_CAMERA_FOV = math.cos(HALF_CAMERA_FOV) CAMERA_WIDTH = 320.0 HALF_CAMERA_WIDTH = CAMERA_WIDTH / 2.0 CAMERA_HEIGHT = 240.0 HALF_CAMERA_HEIGHT = CAMERA_HEIGHT / 2.0 FOCAL_DISTANCE = CAMERA_WIDTH / (2.0 * math.tan(CAMERA_FOV / 2.0)) FOCAL_HYP_X = math.sqrt(FOCAL_DISTANCE * FOCAL_DISTANCE + HALF_CAMERA_WIDTH * HALF_CAMERA_WIDTH) # Initial guesses for position and orientation poseGuesses = [] for poseIdx in range(self.numFrames): poseGuesses.append([0.0, 0.0, 0.0]) # For each frame work out where the landmarks are projected numEntities = len(self.testSequence.fixedEntities) landmarkPositions = [(e.x, e.y) for e in self.testSequence.fixedEntities] landmarkBearings = [] landmarkVisibility = [] for frameIdx in range(self.numFrames): frameData = self.testSequence.frames[frameIdx] bearingArray = [] visibilityArray = [] for entityIdx in range(numEntities): entity = self.testSequence.fixedEntities[entityIdx] bearing = self.bearingToFeature(frameData.subX, frameData.subY, frameData.subYaw, entity.x, entity.y) bearingArray.append(bearing) # Entity is visible in this frame visibilityArray.append(math.cos(bearing) > COS_OF_HALF_CAMERA_FOV) landmarkBearings.append(bearingArray) landmarkVisibility.append(visibilityArray) # Use bundle adjustment to estimate camera positions self.estimator = TrajectoryEstimator() totalError = self.estimator.estimateTrajectory( poseGuesses, landmarkPositions, landmarkBearings, landmarkVisibility ) print "Average Error =", totalError / self.numFrames