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