def getbufferSegment(seg, color, plane):
rgbsegment = jderobot.RGBSegment()
rgbsegment.seg = seg
if not plane:
rgbsegment.seg.fromPoint.z = rgbsegment.seg.fromPoint.z * uniform(
1, 10)
rgbsegment.seg.toPoint.z = rgbsegment.seg.toPoint.z * uniform(1, 10)
rgbsegment.seg.fromPoint.y = rgbsegment.seg.fromPoint.y * uniform(
1, 10)
rgbsegment.seg.toPoint.y = rgbsegment.seg.toPoint.y * uniform(1, 10)
rgbsegment.seg.fromPoint.x = rgbsegment.seg.fromPoint.x * uniform(
1, 10)
rgbsegment.seg.toPoint.x = rgbsegment.seg.toPoint.x * uniform(1, 10)
rgbsegment.c = color
bufferline.append(rgbsegment)
print('ret1: ' + str(ret1))
print('ret2: ' + str(ret2))
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)),
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
segments += [jderobot.RGBSegment(
def getbufferSegment(seg, color):
rgbsegment = jderobot.RGBSegment()
rgbsegment.seg = seg
rgbsegment.c = color
bufferline.append(rgbsegment)