def drawPoint(self, point, color=(0, 0, 0), current=None):
pointJde = jderobot.Point()
pointJde.x = float(point[0] / 100.0)
pointJde.y = float(point[1] / 100.0)
pointJde.z = float(point[2] / 100.0)
colorJDE = jderobot.Color()
colorJDE.r = float(color[0])
colorJDE.g = float(color[1])
colorJDE.b = float(color[2])
getbufferPoint(pointJde, colorJDE)
def drawPoint(self, point, color=(0, 0, 0)):
pointJde = jderobot.Point()
pointJde.x = float(point[0]) / 100.0
pointJde.y = float(point[1] / 100.0)
pointJde.z = float(point[2] / 100.0)
colorJDE = jderobot.Color()
colorJDE.r = float(color[0])
colorJDE.g = float(color[1])
colorJDE.b = float(color[2])
self.viewerProxy.drawPoint(pointJde, colorJDE)
def drawSegment(self, point_a, point_b, color=(0, 0, 0)):
pointJde_a = jderobot.Point()
pointJde_a.x = float(point_a[0] / 100.0)
pointJde_a.y = float(point_a[1] / 100.0)
pointJde_a.z = float(point_a[2] / 100.0)
pointJde_b = jderobot.Point()
pointJde_b.x = float(point_b[0] / 100.0)
pointJde_b.y = float(point_b[1] / 100.0)
pointJde_b.z = float(point_b[2] / 100.0)
segJde = jderobot.Segment()
segJde.fromPoint = pointJde_a
segJde.toPoint = pointJde_b
colorJDE = jderobot.Color()
colorJDE.r = float(color[0])
colorJDE.g = float(color[1])
colorJDE.b = float(color[2])
getbufferSegment(segJde, colorJDE, True)
def __init__(self):
self.lock = threading.Lock()
self.playButton = False
try:
cfg = config.load(sys.argv[1])
#starting comm
jdrc = comm.init(cfg, '3DReconstruction')
ic = jdrc.getIc()
properties = ic.getProperties()
proxyStrCL = jdrc.getConfig().getProperty(
"3DReconstruction.CameraLeft.Proxy")
basecameraL = ic.stringToProxy(proxyStrCL)
#ic = EasyIce.initialize(sys.argv)
#properties = ic.getProperties()
#basecameraL = ic.propertyToProxy("FollowLine.CameraLeft.Proxy")
self.cameraProxyL = jderobot.CameraPrx.checkedCast(basecameraL)
if self.cameraProxyL:
self.imageLeft = self.cameraProxyL.getImageData("RGB8")
self.imageLeft_h = self.imageLeft.description.height
self.imageLeft_w = self.imageLeft.description.width
else:
print('Interface for left camera not connected')
proxyStrCR = jdrc.getConfig().getProperty(
"3DReconstruction.CameraRight.Proxy")
basecameraR = ic.stringToProxy(proxyStrCL)
#basecameraR = ic.propertyToProxy("FollowLine.CameraRight.Proxy")
self.cameraProxyR = jderobot.CameraPrx.checkedCast(basecameraR)
if self.cameraProxyR:
self.imageRight = self.cameraProxyR.getImageData("RGB8")
self.imageRight_h = self.imageRight.description.height
self.imageRight_w = self.imageRight.description.width
else:
print('Interface for right camera not connected')
proxyStrM = jdrc.getConfig().getProperty(
"3DReconstruction.Motors.Proxy")
motorsBase = ic.stringToProxy(proxyStrM)
#motorsBase = ic.propertyToProxy("FolowLine.motors.Proxy")
self.motorsProxy = jderobot.MotorsPrx.checkedCast(motorsBase)
if self.motorsProxy:
print('Interface for motors connected')
else:
print('Interface for motors not connected')
self.maxSpeedV = 1
self.maxSpeedW = 1
#visualization
proxyStrV = jdrc.getConfig().getProperty(
"3DReconstruction.Viewer.Proxy")
baseViewer = ic.stringToProxy(proxyStrV)
#baseViewer = ic.propertyToProxy("FollowLine.Viewer.Proxy")
self.viewerProxy = jderobot.VisualizationPrx.checkedCast(
baseViewer)
if self.viewerProxy:
print('Interface for viewer connected')
else:
print('Interface for viewer not connected')
#draw floor:
self.MAXWORLD = 30
colorJDE = jderobot.Color()
colorJDE.r = 0
colorJDE.g = 0
colorJDE.b = 0
for i in range(0, self.MAXWORLD + 1):
pointJde1 = jderobot.Point()
pointJde2 = jderobot.Point()
pointJde3 = jderobot.Point()
pointJde4 = jderobot.Point()
pointJde1.x = -self.MAXWORLD * 10 / 2 + i * 10
pointJde1.y = -self.MAXWORLD * 10 / 2
pointJde1.z = 0
pointJde2.x = -self.MAXWORLD * 10 / 2 + i * 10
pointJde2.y = self.MAXWORLD * 10 / 2
pointJde2.z = 0
pointJde3.x = -self.MAXWORLD * 10 / 2
pointJde3.y = -self.MAXWORLD * 10 / 2. + i * 10
pointJde3.z = 0
pointJde4.x = self.MAXWORLD * 10 / 2
pointJde4.y = -self.MAXWORLD * 10 / 2. + i * 10
pointJde4.z = 0
seg1 = jderobot.Segment()
seg1.fromPoint = pointJde1
seg1.toPoint = pointJde2
seg2 = jderobot.Segment()
seg2.fromPoint = pointJde3
seg2.toPoint = pointJde4
self.viewerProxy.drawSegment(seg1, colorJDE)
self.viewerProxy.drawSegment(seg2, colorJDE)
except:
traceback.print_exc()
exit()
status = 1
stereoCalibrationData['cameraMatrix1'],
np.identity(3),
np.array([[.0], [.0], [.0]])
)
right_3d_point = right_3d_point[:3] / 10
left_3d_point = left_3d_point[:3] / 10
left_3d_point = transform_points_to_real_world(left_3d_point)
right_3d_point = transform_points_to_real_world(right_3d_point)
left_3d_point = left_3d_point * -10000
center_a = np.array([.0, .0, .0])
camera_color = jderobot.Color(0.0, 0.0, 1.0)
segments += [jderobot.RGBSegment(
jderobot.Segment(jderobot.Point(0, 0, 0),
jderobot.Point(left_3d_point[0], left_3d_point[1], left_3d_point[2])),
camera_color)]
center_b = rotate_points(center_a, stereoCalibrationData['R'])
center_b = translate_points(center_b, stereoCalibrationData['T'].reshape(3) / 10)
center_b = transform_points_to_real_world(center_b)
print('center', center_b)
right_3d_point[0] = center_b[0] + (right_3d_point[0] - center_b[0]) * -10000
right_3d_point[1] = center_b[1] + (right_3d_point[1] - center_b[1]) * -10000
right_3d_point[2] = center_b[2] + (right_3d_point[2] - center_b[2]) * -10000
if ret1 is False or ret2 is False:
break
matcher.set_images(image1, image2)
points = matcher.get_matching_points()
points.append(
jderobot.RGBPoint(3.44965908, 1.22125194e-17, 0.0, 0.0,
0.0, 0.0))
points.append(jderobot.RGBPoint(0.0, 0.0, 0.0, 0.0, 0.0, 0.0))
segments = [
jderobot.RGBSegment(
jderobot.Segment(jderobot.Point(10.0, 0.0, 0.0),
jderobot.Point(0.0, 0.0, 0.0)),
jderobot.Color(1.0, 0.0, 0.0)),
jderobot.RGBSegment(
jderobot.Segment(jderobot.Point(0.0, 10.0, 0.0),
jderobot.Point(0.0, 0.0, 0.0)),
jderobot.Color(0.0, 1.0, 0.0)),
jderobot.RGBSegment(
jderobot.Segment(jderobot.Point(0.0, 0.0, 10.0),
jderobot.Point(0.0, 0.0, 0.0)),
jderobot.Color(0.0, 0.0, 1.0))
]
segments.append(
jderobot.RGBSegment(
jderobot.Segment(jderobot.Point(50.0, 50.0, -55.0),
jderobot.Point(50.0, -50.0, -55.0)),
jderobot.Color(1.0, 0.0, 0.0)))