def local_intersect(self, ray):
if abs(ray.direction.y) < EPSILON:
return Intersections([])
else:
t1 = -ray.origin.y / ray.direction.y
i1 = Intersection(t1, self)
return Intersections([i1])
def local_intersect(self, ray):
xtmin, xtmax = self.check_axis(ray.origin.x, ray.direction.x)
ytmin, ytmax = self.check_axis(ray.origin.y, ray.direction.y)
ztmin, ztmax = self.check_axis(ray.origin.z, ray.direction.z)
tmin = max(xtmin, ytmin, ztmin)
tmax = min(xtmax, ytmax, ztmax)
if tmin > tmax:
return Intersections([])
ls = [Intersection(tmin, self), Intersection(tmax, self)]
xs = Intersections(ls)
return xs
def __init__(self,
speed=0.15,
lane_offset=140,
wait_period=10,
hard_coded_turns=True):
self.hard_coded_turns = hard_coded_turns
self.speed = speed
self.pid = SteeringPid(lane_offset, kp=0.1, ki=0.006, kd=1.2)
self.intersections_pid = SteeringPid(1, kp=0.1, ki=0.006, kd=1.2)
self.current_turn = None
self.current_turn_direction = None
self.handling_intersection = False
self.inter_start_time = time.time()
self.go_straight = False
self.angle = 0
self.waypoint = Waypoint()
self.car = Car_Control()
self.detector = Image_Process()
self.vs = cv2.VideoCapture(
"/dev/video2",
cv2.CAP_V4L) # TODO: figure out a good way to do this
self.intersections = Intersections(wait_period=wait_period)
self.pipeline = rs.pipeline()
self.config = rs.config()
self.config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
self.pipeline.start(self.config)
def local_intersect(self, ray):
# ray2 = self.transform.inverse() * ray
# ray = Ray(self.transform.inverse()*ray.origin, self.transform.inverse()*ray.direction)
sphere2ray = ray.origin - Point(0, 0, 0)
a = ray.direction.dot(ray.direction)
b = 2 * ray.direction.dot(sphere2ray)
c = sphere2ray.dot(sphere2ray) - 1
discriminant = b * b - 4 * a * c
if discriminant < 0:
return Intersections([])
else:
t1 = (-b - sqrt(discriminant)) / (2 * a)
t2 = (-b + sqrt(discriminant)) / (2 * a)
i1 = Intersection(t1, self)
i2 = Intersection(t2, self)
return Intersections([i1, i2])
def test_intersect3(self):
origin = Point(0, 2, -5)
direction = Vector(0, 0, 1)
r = Ray(origin, direction)
s = Sphere()
xs = Intersections([])
self.assertTrue(s.intersect(r) == xs)
def test_hit3(self):
s = Sphere()
i1 = Intersection(-2, s)
i2 = Intersection(-1, s)
xs = Intersections([i1, i2])
i = xs.hit()
self.assertTrue(i == None)
def test_hit1(self):
s = Sphere()
i1 = Intersection(1, s)
i2 = Intersection(2, s)
xs = Intersections([i1, i2])
i = xs.hit()
self.assertTrue(i == i1)
def test_int2(self):
s = Sphere()
i1 = Intersection(1, s)
i2 = Intersection(2, s)
xs = Intersections([i1, i2])
self.assertEqual(xs.count, 2)
self.assertEqual(xs[0].t, 1)
self.assertEqual(xs[1].t, 2)
def test_intersect5(self):
origin = Point(0, 0, 5)
direction = Vector(0, 0, 1)
r = Ray(origin, direction)
s = Sphere()
i1 = Intersection(-6, s)
i2 = Intersection(-4, s)
xs = Intersections([i1, i2])
self.assertTrue(s.intersect(r) == xs)
def test_hit4(self):
s = Sphere()
i1 = Intersection(5, s)
i2 = Intersection(7, s)
i3 = Intersection(-3, s)
i4 = Intersection(2, s)
xs = Intersections([i1, i2, i3, i4])
i = xs.hit()
self.assertTrue(i == i4)
def test_schlick_approximation_with_small_angle_and_n2_larger_than_n1(self):
shape = GlassSphere()
r = Ray(Point(0, 0.99, -2), Vector(0, 0, 1))
ls = [
Intersection(1.8589, shape),
]
xs = Intersections(ls)
comps = xs[0].prepare_computations(r, xs)
reflectance = comps.schlick()
self.assertTrue(equals(reflectance, 0.48873))
def test_schlick_approximation_with_perpendicular_viewing_angle(self):
shape = GlassSphere()
r = Ray(Point(0, 0, 0), Vector(0, 1, 0))
ls = [
Intersection(-1, shape),
Intersection(1, shape)
]
xs = Intersections(ls)
comps = xs[1].prepare_computations(r, xs)
reflectance = comps.schlick()
self.assertTrue(equals(reflectance, 0.04))
def test_schlick_approximation_under_total_internal_reflection(self):
shape = GlassSphere()
r = Ray(Point(0, 0, sqrt(2)/2), Vector(0, 1, 0))
ls = [
Intersection(-sqrt(2)/2, shape),
Intersection(sqrt(2)/2, shape)
]
xs = Intersections(ls)
comps = xs[1].prepare_computations(r, xs)
reflectance = comps.schlick()
self.assertTrue(equals(reflectance, 1.0))
def test_under_point1(self):
r = Ray(Point(0, 0, -5), Vector(0, 0, 1))
shape = GlassSphere()
shape.set_transform(translate(0, 0, 1))
i = Intersection(5, shape)
xs = Intersections([i])
comps = i.prepare_computations(r, xs)
# print(comps.normalv)
# print(comps.point)
# print(comps.under_point.z)
self.assertTrue(comps.under_point.z > EPSILON / 2)
self.assertTrue(comps.point.z < comps.under_point.z)
def test_refracted_color_opaque_surface(self):
w = World()
shape = w.objs[0]
r = Ray(Point(0, 0, -5), Vector(0, 0, -1))
ls = [
Intersection(4, shape),
Intersection(6, shape)
]
xs = Intersections(ls)
comps = xs[0].prepare_computations(r, xs)
c = w.refracted_color(comps, 5)
self.assertTrue(Color(0, 0, 0) == c)
def test_refracted_color_at_maximum_recursive_depth(self):
w = World()
shape = w.objs[0]
shape.material.transparency = 1.0
shape.material.refractive_index = 1.5
r = Ray(Point(0, 0, -5), Vector(0, 0, -1))
ls = [
Intersection(4, shape),
Intersection(6, shape)
]
xs = Intersections(ls)
comps = xs[0].prepare_computations(r, xs)
c = w.refracted_color(comps, 0)
self.assertTrue(Color(0, 0, 0) == c)
def test_refract1(self):
A = GlassSphere()
A.set_transform(scale(2, 2, 2))
A.material.refractive_index = 1.5
B = GlassSphere()
B.set_transform(translate(0, 0, -0.25))
B.material.refractive_index = 2.0
C = GlassSphere()
C.set_transform(translate(0, 0, 0.25))
C.material.refractive_index = 2.5
r = Ray(Point(0, 0, -4), Vector(0, 0, 1))
ls = [
Intersection(2, A),
Intersection(2.75, B),
Intersection(3.25, C),
Intersection(4.75, B),
Intersection(5.25, C),
Intersection(6, A)
]
xs = Intersections(ls)
comps0 = xs[0].prepare_computations(r, xs)
self.assertEqual(comps0.n1, 1.0)
self.assertEqual(comps0.n2, 1.5)
comps1 = xs[1].prepare_computations(r, xs)
self.assertEqual(comps1.n1, 1.5)
self.assertEqual(comps1.n2, 2.0)
comps2 = xs[2].prepare_computations(r, xs)
self.assertEqual(comps2.n1, 2.0)
self.assertEqual(comps2.n2, 2.5)
comps3 = xs[3].prepare_computations(r, xs)
self.assertEqual(comps3.n1, 2.5)
self.assertEqual(comps3.n2, 2.5)
comps4 = xs[4].prepare_computations(r, xs)
self.assertEqual(comps4.n1, 2.5)
self.assertEqual(comps4.n2, 1.5)
comps5 = xs[5].prepare_computations(r, xs)
self.assertEqual(comps5.n1, 1.5)
self.assertEqual(comps5.n2, 1.0)
def test_refracted_color_under_total_internal_reflection(self):
w = World()
shape = w.objs[0]
shape.material.transparency = 1.0
shape.material.refractive_index = 1.5
r = Ray(Point(0, 0, sqrt(2)/2), Vector(0, 1, 0))
ls = [
Intersection(-sqrt(2)/2, shape),
Intersection(sqrt(2)/2, shape)
]
xs = Intersections(ls)
comps = xs[1].prepare_computations(r, xs)
c = w.refracted_color(comps, 5)
self.assertTrue(c == Color(0, 0, 0))
def intersect(self, ray):
def swap(l, i1, i2):
temp = l[i1]
l[i1] = l[i2]
l[i2] = temp
return l
ls = []
for obj in self.objs:
intersections = obj.intersect(ray)
if intersections.count != 0:
for i in range(intersections.count):
ls.append(intersections[i])
n = len(ls)
for i in range(1, n):
target = ls[i].t
for j in range(i):
if ls[j].t > ls[i].t:
l = swap(ls, i, j)
return Intersections(ls)
def test_shade_hit_with_reflective_transparent_material(self):
w = World()
r = Ray(Point(0, 0, -3), Vector(0, -sqrt(2)/2, sqrt(2)/2))
floor = Plane()
floor.set_transform(translate(0, -1, 0))
floor.material.reflective = 0.5
floor.material.transparency = 0.5
floor.material.refractive_index = 1.5
w.objs.append(floor)
ball = Sphere()
ball.material.color = Color(1, 0, 0)
ball.material.ambient = 0.5
ball.set_transform(translate(0, -3.5, -0.5))
w.objs.append(ball)
ls = [Intersection(sqrt(2), floor)]
xs = Intersections(ls)
comps = xs[0].prepare_computations(r, xs)
color = w.shade_hit(comps, 5)
# print(color)
self.assertTrue(Color(0.93391, 0.69643, 0.69243) == color)
def test_shade_hit_with_transparent_material(self):
w = World()
floor = Plane()
floor.set_transform(translate(0, -1, 0))
floor.material.transparency = 0.5
floor.material.refractive_index = 1.5
ball = Sphere()
ball.material.color = Color(1., 0., 0.)
ball.material.ambient = 0.5
ball.set_transform(translate(0, -3.5, -0.5))
w.objs = [floor, ball]
r = Ray(Point(0, 0, -3), Vector(0, -sqrt(2)/2, sqrt(2)/2))
ls = [Intersection(sqrt(2), floor)]
xs = Intersections(ls)
comps = xs[0].prepare_computations(r, xs)
color = w.shade_hit(comps, 5)
self.assertTrue(Color(0.93642, 0.68642, 0.68642) == color)
def test_refracted_color_with_refracted_ray(self):
# this test failed!
w = World()
A = w.objs[0]
A.material.ambient = 1
A.material.pattern = TestPattern()
B = w.objs[1]
B.material.transparency = 1.0
B.material.refractive_index = 1.5
r = Ray(Point(0, 0, 0.1), Vector(0, 1, 0))
ls = [
Intersection(-0.9899, A),
Intersection(-0.4899, B),
Intersection(0.4899, B),
Intersection(0.9899, A)
]
xs = Intersections(ls)
comps = xs[2].prepare_computations(r, xs)
c = w.refracted_color(comps, 5)
# the expected answer is color(0, 0.99888, 0.04725)
self.assertTrue(c == Color(0, 0.99888, 0.04721))
def scan(main_color, intermediates=None):
main_gray = utility.shrink_img(main_color) # 処理速度を上げるため縮小する
main_gray = cv2.cvtColor(main_gray, cv2.COLOR_BGR2GRAY)
size_color = main_color.shape[:2]
size_gray = main_gray.shape
length_threshold = 20
distance_threshold = 1.4142
# canny_th1 = 5.0
# canny_th2 = 50.0
canny_th1 = 1.0
canny_th2 = 10
canny_size = 3
do_merge = False
do_merge = False
fld = cv2.ximgproc.createFastLineDetector(length_threshold,
distance_threshold, canny_th1,
canny_th2, canny_size, do_merge)
line_pnts = fld.detect(main_gray)
line_pnts = np.array(line_pnts).reshape((-1, 4))
lines = Lines(line_pnts)
print(f"Num of lines: {lines.num} => ", end="")
lines.remove_central(size_gray)
print(lines.num)
equal = lines.equal()
labels = partition.partition(lines.num, equal)
labels_num = len(np.unique(labels))
if intermediates is not None:
intermediates['lines'] = utility.draw_lines(main_gray, lines, labels,
labels_num)
cv2.imwrite("lines.jpeg", intermediates['lines'])
segments = Segments(lines, labels, labels_num, size_gray)
print(f"Num of segments: {segments.num} => ", end="")
segments.remove_central(size_gray)
print(segments.num)
if intermediates is not None:
intermediates['segments'] = utility.draw_segments(main_gray, segments)
cv2.imwrite("segments.jpeg", intermediates['segments'])
intersections = Intersections(segments, size_gray)
print(f"Num of intersections: {intersections.num}")
if intermediates is not None:
intermediates['intersections'] = utility.draw_intersections(
main_gray, intersections)
cv2.imwrite("intersections.jpeg", intermediates['intersections'])
df = ml_model.prepare_data(intersections, size_gray)
scores = ml_model.get_score(df)
indice = np.argsort(scores)[::-1]
points_per_section = 3
vertex_lt = []
vertex_rt = []
vertex_lb = []
vertex_rb = []
for idx in indice:
if intersections.is_left[idx] and intersections.is_top[idx] and len(
vertex_lt) < points_per_section:
vertex_lt.append(idx)
elif intersections.is_right[idx] and intersections.is_top[idx] and len(
vertex_rt) < points_per_section:
vertex_rt.append(idx)
elif intersections.is_left[idx] and intersections.is_bottom[
idx] and len(vertex_lb) < points_per_section:
vertex_lb.append(idx)
elif intersections.is_right[idx] and intersections.is_bottom[
idx] and len(vertex_rb) < points_per_section:
vertex_rb.append(idx)
if len(vertex_lt) >= points_per_section and \
len(vertex_rt) >= points_per_section and \
len(vertex_lb) >= points_per_section and \
len(vertex_rb) >= points_per_section:
break
if len(vertex_lt) == 0:
print("no vertex at left top was found")
return None
if len(vertex_rt) == 0:
print("no vertex at right top was found")
return None
if len(vertex_lb) == 0:
print("no vertex at left bottom was found")
return None
if len(vertex_rb) == 0:
print("no vertex at right bottom was found")
return None
idx_lt, idx_rt, idx_lb, idx_rb = utility.get_best_set(
intersections, scores, vertex_lt, vertex_rt, vertex_lb, vertex_rb)
# print(f"selected vertexes: ({idx_lt}, {idx_rt}, {idx_lb}, {idx_rb})")
if intermediates is not None:
intermediates['detected'] = utility.draw_detected(
main_gray, intersections, idx_lt, idx_rt, idx_lb, idx_rb)
src_points_gray = np.array([ \
intersections.cross_pnt[idx_lt], \
intersections.cross_pnt[idx_rt], \
intersections.cross_pnt[idx_lb], \
intersections.cross_pnt[idx_rb]
], dtype=np.float32)
dst_points_gray = np.array([ \
(0, 0),
(size_gray[1], 0),
(0, size_gray[0]),
(size_gray[1], size_gray[0])
], dtype=np.float32)
scale = np.array(size_color, dtype=np.float32) / np.array(size_gray,
dtype=np.float32)
scale = scale[::-1]
src_points_color = src_points_gray * scale
dst_points_color = dst_points_gray * scale
M_color = cv2.getPerspectiveTransform(src_points_color, dst_points_color)
main_color = cv2.warpPerspective(main_color, M_color, size_color[::-1])
return main_color
def houghlinemethod(self, edges):
lines = cv2.HoughLines(edges, 0.7, np.pi / 500, 130)
for rho, theta in lines[0]:
a = np.cos(theta)
b = np.sin(theta)
x0 = a * rho
y0 = b * rho
x1 = int(x0 + 1000 * (-b))
y1 = int(y0 + 1000 * (a))
x2 = int(x0 - 1000 * (-b))
y2 = int(y0 - 1000 * (a))
cv2.line(self.source_color, (x1, y1), (x2, y2), (0, 255, 255), 1)
# Initial Intersection class
inter = Intersections()
for i in range(0, lines.shape[1]):
for j in range(i + 1, lines.shape[1]):
line1 = lines[0][i]
line2 = lines[0][j]
# check the lines as a pair to compute their intersections
if inter.acceptLinePair(line1, line2, np.pi / 32):
intersection = inter.computeintersect(line1, line2)
#print intersection
if (intersection[0] > 0 and intersection[1] > 0):
inter.append(intersection[0], intersection[1])
cv2.circle(self.source_color,
(intersection[0], intersection[1]), 6,
(0, 255, 255), 2)
# combination of the extracted intersection with harris corner detetction
gray = cv2.cvtColor(self.source, cv2.COLOR_BGR2GRAY)
'''harris corner detector and draw red circle around them'''
self.features = cv2.goodFeaturesToTrack(gray,
1000,
0.001,
5,
None,
None,
2,
useHarrisDetector=True,
k=0.00009)
if len(self.features.shape) == 3.0:
assert (self.features.shape[1:] == (1, 2))
self.features.shape = (self.features.shape[0], 2)
for x, y in self.features:
self.corners += [(x, y)]
inter.append_features(x, y)
cv2.circle(self.source_color, (x, y), 8, (255, 255, 255))
cv2.imwrite("images/result_images/Lines_intersection.jpg",
self.source_color)
#cv2.imshow("lines and intersections", self.source_color)
#cv2.waitKey(0)
# subpixel accuracy
'''define the criteria to stop and refine the corners'''
criteria = (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 100,
0.03)
#criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001)
cv2.cornerSubPix(gray, self.features, (5, 5), (-1, -1), criteria)
for x, y in self.features:
self.refined_corners += [(x, y)]
inter.append_refined_features(x, y)
cv2.circle(self.source_color, (x, y), 4, (0, 255, 0))
'''Now draw them'''
res = np.hstack((self.corners, self.refined_corners))
res = np.int0(res)
self.source_color[res[:, 1], res[:, 0]] = [0, 0, 255]
self.source_color[res[:, 3], res[:, 2]] = [0, 255, 0]
#cv2.imwrite(str(self.IMAGE_NAME) + '_subpixel.png', self.im_new)
#cv2.imshow("corners", self.source_color)
#cv2.waitKey(0)
return inter
def houghlinePmethod(self, edges):
minLineLength = int(self.source_color.shape[0] / 5.0)
maxLineGap = int(self.source_color.shape[0] / 2.0)
linesP = cv2.HoughLinesP(edges, 1, np.pi / 180, 95, np.array([]),
minLineLength, maxLineGap)
print "linesP are: " + str(linesP)
for x1, y1, x2, y2 in linesP[0]:
#cv2.circle(self.source_color, (x1, y1), 6, (0, 255, 255), 1)
#cv2.circle(self.source_color, (x2, y2), 6, (255, 0, 255), 1)
cv2.line(self.source_color, (x1, y1), (x2, y2), (0, 0, 255))
#cv2.imshow("lines", self.source_color)
#cv2.waitKey(0)
# Initial Intersection class
inter = Intersections()
d = []
for i in range(0, linesP.shape[1]):
for j in range(i + 1, linesP.shape[1]):
line1 = linesP[0][i]
line2 = linesP[0][j]
# check the lines as a pair to compute their intersections
if inter.acceptLinesPPair(line1, line2, np.pi / 20.0):
intersection = inter.computeintersectP(line1, line2)
print intersection
if ((intersection[0] > 0
and intersection[0] < self.source_color.shape[1]) and
(intersection[1] > 0
and intersection[1] < self.source_color.shape[0])):
#if ((intersection[0] > 0) and (intersection[1] > 0)):
inter.append(intersection[0], intersection[1])
cv2.circle(self.source_color,
(intersection[0], intersection[1]), 10,
(0, 255, 0))
#cv2.line(self.source_color,(line1[0],line1[1]),(line1[2],line1[3]),(255, 255, 0))
#cv2.line(self.source_color,(line2[0],line2[1]),(line2[2],line2[3]),(255, 255, 0))
cv2.imwrite("images/result_images/Lines_intersection.jpg",
self.source_color)
#cv2.imshow("lines and intersections", self.source_color)
#cv2.waitKey(0)
# combination of the extracted intersection with harris corner detetction
gray = cv2.cvtColor(self.source, cv2.COLOR_BGR2GRAY)
'''harris corner detector and draw red circle around them'''
self.features = cv2.goodFeaturesToTrack(gray,
1000,
0.001,
5,
None,
None,
2,
useHarrisDetector=True,
k=0.00009)
if len(self.features.shape) == 3.0:
assert (self.features.shape[1:] == (1, 2))
self.features.shape = (self.features.shape[0], 2)
for x, y in self.features:
self.corners += [(x, y)]
inter.append_features(x, y)
cv2.circle(self.source_color, (x, y), 8, (255, 255, 255))
# subpixel accuracy
'''define the criteria to stop and refine the corners'''
criteria = (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 100,
0.03)
#criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001)
cv2.cornerSubPix(gray, self.features, (5, 5), (-1, -1), criteria)
for x, y in self.features:
self.refined_corners += [(x, y)]
inter.append_refined_features(x, y)
cv2.circle(self.source_color, (x, y), 4, (0, 255, 0))
'''Now draw them'''
res = np.hstack((self.corners, self.refined_corners))
res = np.int0(res)
self.source_color[res[:, 1], res[:, 0]] = [0, 0, 255]
self.source_color[res[:, 3], res[:, 2]] = [0, 255, 0]
#cv2.imwrite(str(self.IMAGE_NAME) + '_subpixel.png', self.im_new)
cv2.imwrite("images/result_images/Features and subpixel accuracy.jpg",
self.source_color)
#cv2.imshow("Features and subpixel accuracy", self.source_color)
#cv2.waitKey(0)
return inter