def __createFeatureMatchers(self):
self._fm = list()
self._fm.append(FeatureMatcher(Box(Point(1250, 650), Point(1250 + 300, 650 + 200)))) # middle right
self._fm.append(FeatureMatcher(Box(Point(700, 600), Point(700 + 400, 600 + 300)))) # center
self._fm.append(FeatureMatcher(Box(Point(1250, 125), Point(1450 + 200, 125 + 400)))) #top right
self._fm.append(FeatureMatcher(Box(Point(1200, 300), Point(1200 + 500, 300 + 300)))) #top right
self._fm.append(FeatureMatcher(Box(Point(200, 50), Point(200 + 600, 50 + 400))))
self._fm.append(FeatureMatcher(Box(Point(800, 50), Point(800 + 300, 50 + 200))))
self._fm.append(FeatureMatcher(Box(Point(300, 400), Point(300 + 250, 400 + 350)))) # middle left
self._fm.append(FeatureMatcher(Box(Point(800, 300), Point(800 + 300, 300 + 200))))
self._fm.append(FeatureMatcher(Box(Point(200, 650), Point(200 + 300, 650 + 200))))
def trimSides(self, newWidth):
# type: (int) -> Image
widthToCutOutLeft = int((self.width() - newWidth) / 2)
areaToCut = Box(Point(widthToCutOutLeft, 0),
Point(widthToCutOutLeft + newWidth, self.height()))
return self.subImage(areaToCut)
def __boxAroundFeatureForFrame(self, frameID):
topLeftPoint = self.__getTopLeftForFrame(frameID)
box = Box(
topLeftPoint,
Point(topLeftPoint.x + self.__startingBox.width(),
topLeftPoint.y + self.__startingBox.hight()))
return box
def __boundingBoxAroundContour(self, contour):
Xmin = int(np.min(contour[:, 1]))
Xmax = int(np.max(contour[:, 1]))
Ymin = int(np.min(contour[:, 0]))
Ymax = int(np.max(contour[:, 0]))
box = Box(Point(Xmin, Ymin), Point(Xmax, Ymax))
return box
def shiftImageHorizontally(self, pixelsToShift):
# type: (int) -> Image
boxSizeOfImage = Box.createUsingDimesions(self.width(), self.height())
fillerWidth = abs(pixelsToShift)
imageWithFillerOnBothSides = self.padSidesToMakeWider(fillerWidth * 2)
boxEncompasingImage = boxSizeOfImage.translateBy(Vector(
fillerWidth, 0))
#slide "boxEncompasingImage" to the left or to the right depending if xDrift is negative or positive
boxAroundAreaThatWeNeedToCrop = boxEncompasingImage.translateBy(
Vector(pixelsToShift, 0))
return imageWithFillerOnBothSides.subImage(
boxAroundAreaThatWeNeedToCrop)
def showWindowAndWaitForTwoClicks(self, image):
keyPress = self.showWindowAndWaitForClick(image)
if not self.userClickedMouse():
return keyPress
point1 = self.featureCoordiate
img = Image(image)
img.drawBoxOnImage(point1.boxAroundPoint(3))
keyPress = self.showWindowAndWaitForClick(image)
if not self.userClickedMouse():
return keyPress
point2 = self.featureCoordiate
if point1.x <= point2.x:
self.featureBox = Box(point1, point2)
else:
#second Click was to the left of the first. Reverse the two points
self.featureBox = Box(point2, point1)
return keyPress
def __updateRedDotsSearchArea(self, boxAroundRedDots):
dotsShift = int(self.__distanceBetweenRedPoints() / 2)
bottomRightLimit_x = self.__getImage().width() - 200
bottomRightLimit_y = self.__getImage().height() - 200
topLeftX = min(max(boxAroundRedDots.topLeft.x - dotsShift, 1),
bottomRightLimit_x - 100)
topLeftY = min(max(boxAroundRedDots.topLeft.y - dotsShift, 1),
bottomRightLimit_y - 100)
bottomRightX = min(boxAroundRedDots.bottomRight.x + dotsShift,
bottomRightLimit_x)
bottomRightY = min(boxAroundRedDots.bottomRight.y + dotsShift,
bottomRightLimit_y)
redDotsSearchArea = Box(Point(topLeftX, topLeftY),
Point(bottomRightX, bottomRightY))
return redDotsSearchArea
def __initial_search_area2(self):
# type: () -> Box
if (self.__frame.is_high_resolution()):
return Box(Point(1400, 1000), Point(1900, 1400))
else:
return Box(Point(900, 300), Point(1400, 800))
def __defaultBoxAroundFeature(self):
box = Box(
self.__getTopLeft(),
Point(self.__getTopLeft().x + self.__startingBox.width(),
self.__getTopLeft().y + self.__startingBox.hight()))
return box
#/usr/bin/env python
from math import sqrt
from hpp.corbaserver.manipulation import ProblemSolver, Constraints, \
ConstraintGraph, ConstraintGraphFactory, Rule
from hpp.gepetto import PathPlayer
from hpp.gepetto.manipulation import ViewerFactory
from hpp import Transform
import CORBA, sys, numpy as np
from common import generateRandomConfig, writeTrajInYaml, robot, ps, \
Box, Table, vf
objects = list()
objects.append(Box(name='box', vf=vf))
robot.setJointBounds("box/root_joint", [-1, 1, -1, 1, 0, 2])
# Loaded as an object to get the visual tags at the right position.
table = Table(name='table', vf=vf)
q_init = [
0.6000022397992977, -0.6781023282014177, 1.0227731536350761,
-0.026501735815611785, 0.026519221784192294, 0.7066117724333544,
0.7066075232640208, -7.425303327690214e-06, -3.104979264370574e-05,
-0.5336970777582547, 0.7998833793330343, -0.3411868362719986,
-0.001701417910656863, -7.425312934111446e-06, -3.104992049186117e-05,
-0.5337106744153133, 0.799914920807006, -0.341204781088912,
-0.0017014177824483088, 0.0, 0.5558260964981129, 0.28504014455571497,
0.19401590944614103, 0.003941049852447548, -0.5228672465592601,
-6.095997813993427e-05, 0.001283145908800703, 0.10028131572105346, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.28502261804473694, -0.1940282332221976,
-0.0038866982587039012, -0.5226794809165282, 0.00011731350732173029,
-0.00127685849617846, 0.10046668823166613, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
def topPart(self, height):
# type: (integer) -> Image
box = Box(Point(0, 0), Point(self.width(), height))
return self.subImage(box)
def bottomPart(self, height):
# type: (integer) -> Image
box = Box(Point(0,
self.height() - height),
Point(self.width(), self.height()))
return self.subImage(box)
def drawDriftVectorOnImage(self, driftVector):
if driftVector is not None:
vectorStart = Point(100, 100)
vectorEnd = vectorStart.translateBy(driftVector)
vectorBox = Box(vectorStart, vectorEnd)
self.drawBoxOnImage(vectorBox)
def drawFrameID(self, frameID):
self.drawTextInBox(Box(Point(0, 0), Point(80, 50)), frameID)