(나중에 추가) 아래 코드는 원작자도 에라가 있는 걸 못 고친 듯 합니다. 알고보니 원본 데모 영상에도 똑같은 에러가 있더군요.
별로 가치가 없는 내용이었습니다.
===============================================
알고보틱의 https://www.youtube.com/watch?v=ZxaXfahaP2s 비디오 코드인데 작가의 코드와 똑같지 않은듯 합니다.
약간 결과가 다르게 나오더군요.
총 5편의 비디오 강의의 결과물인데 강의 영상과는 결과가 다르게 부실하게 나왔습니다.
알고보틱은 예전에는 코드를 판매했는데 살만한 가치까지는 없어보여서 그냥 웹에 뒤지니 대충 나왔습니다.
요즘에는 판매하는 account도 없애버린 것 같더군요.
env.py
import math
import pygame
class buildEnvironment:
def __init__(self, MapDimensions):
pygame.init()
self.pointCloud = []
self.externalMap = pygame.image.load('floorplan.png')
self.maph, self.mapw = MapDimensions
self.MapWindowName = 'RRT path planning'
pygame.display.set_caption(self.MapWindowName)
self.map = pygame.display.set_mode((self.mapw,self.maph))
self.map.blit(self.externalMap,(0,0))
#Colours
self.black = (0, 0, 0)
self.grey = (70, 70, 70)
self.Blue = (0, 0, 255)
self.Green = (0, 255, 0)
self.Red = (255, 0, 0)
self.white = (255, 255, 255)
def AD2pos(self, distance, angle, robotPosition):
x = distance * math.cos(angle) + robotPosition[0]
y = -distance * math.sin(angle) + robotPosition[1]
return (int(x), int(y))
def dataStorage(self, data):
print(len(self.pointCloud))
for element in data:
point = self.AD2pos(element[0], element[1], element[2])
if point not in self.pointCloud:
self.pointCloud.append(point)
def show_sensorData(self):
self.infomap = self.map.copy()
for point in self.pointCloud:
self.infomap.set_at((int(point[0]), int(point[1])), (255, 0, 0))
sensors.py
import math
import pygame
import numpy as np
def uncertaninty_add(distance, angle, sigma):
mean = np.array([distance, angle])
covariance = np.diag(sigma**2)
distance, angle = np.random.multivariate_normal(mean, covariance)
distance = max(distance, 0)
angle = max(angle, 0)
return [distance, angle]
class LaserSensor:
def __init__(self, Range, map, uncertainty):
self.Range = Range
self.speed = 3 #rounds per second
self.sigma = np.array([uncertainty[0], uncertainty[1]])
self.position = (0,0)
self.map = map
self.W, self.H = pygame.display.get_surface().get_size()
self.sensedObstacles = []
def distance(self, obsatclePosition):
px = (obsatclePosition[0] - self.position[0])**2
py = (obsatclePosition[1] - self.position[1])**2
return math.sqrt(px + py)
def sense_obstacles(self):
data = []
x1, y1 = self.position[0], self.position[1]
for angle in np.linspace(0, 2*math.pi, 180, False):
x2, y2 = (x1 + self.Range * math.cos(angle), y1 - self.Range * math.sin(angle))
for i in range(0,100):
u = i / 100
x = int(x2*u + x1*(1-u))
y = int(y2*u + y1*(1-u))
if 0 < x < self.W and 0 < y < self.H:
color = self.map.get_at((x,y))
if (color[0], color[1], color[2]) == (0,0,0):
distance = self.distance((x,y))
output = uncertaninty_add(distance, angle, self.sigma)
output.append(self.position)
# store the mesurement
data.append(output)
break
if len(data) > 0:
return data
else:
return False
Features.py
# Features.py
import numpy as np
import math
from fractions import Fraction
from scipy.odr import *
# landmarks
Landmarks = []
class featuresDetection:
def __init__(self):
# variables
self.EPSILON = 10
self.DELTA = 10
self.SNUM = 6
self.PMIN = 20
self.GMAX = 20
self.SEED_SEGMENTS = []
self.LINE_SEGMENTS = []
self.LASERPOINTS = []
self.LINE_PARAMS = None
self.NP = len(self.LASERPOINTS) - 1
self.LMIN = 5 # minimum length of a line segment
self.LR = 0 # real length of a line segment
self.PR = 0 # the number of laser points contained
self.FEATURES = []
# euclidian distance from point1 to point2
def dist_point2point(self, point1, point2):
#def dist_point2point(point1, point2):
Px = (point1[0] - point2[0]) ** 2
Py = (point1[1] - point2[1]) ** 2
return math.sqrt(Px + Py)
# distance point to line written in general form
def dist_point2line(self, params, point):
A, B, C = params
distance = abs(A * point[0] + B * point[1] + C) / math.sqrt(A ** 2 + B ** 2)
return distance
# extract two points from a line equation under the slope intercepts form
def line_2points(self, m, b):
x = 5
y = m * x + b
x2 = 2000
y2 = m * x2 + b
return [(x, y), (x2, y2)]
# general form to slope-intercept
def lineForm_G2SI(self, A, B, C):
m = -A / B
B = -C / B
return m, B
# slope-intercept to general form
def lineForm_Si2G(self, m, B):
A, B, C = -m, 1, -B
if A < 0:
A, B, C = -A, -B, -C
den_a = Fraction(A).limit_denominator(1000).as_integer_ratio()[1]
den_c = Fraction(C).limit_denominator(1000).as_integer_ratio()[1]
gcd = np.gcd(den_a, den_c)
lcm = den_a * den_c / gcd
A = A * lcm
B = B * lcm
C = C * lcm
return A, B, C
def line_intersect_general(self, params1, params2):
a1, b1, c1 = params1
a2, b2, c2 = params2
if (b1 * a2 - a1 * b2) != 0:
x = (c1 * b2 - b1 * c2) / (b1 * a2 - a1 * b2)
y = (a1 * c2 - a2 * c1) / (b1 * a2 - a1 * b2)
return x, y
def points_2line(self, point1, point2):
m, b = 0, 0
if point2[0] == point1[0]:
pass
else:
m = (point2[1] - point1[1]) / (point2[0] - point1[0])
b = point2[1] - m * point2[0]
return m, b
def projection_point2line(self, point, m, b):
x, y = point
m2 = -1 / m
c2 = y - m2 * x
intersection_x = - (b - c2) / (m - m2)
intersection_y = m2 * intersection_x + c2
return intersection_x, intersection_y
def AD2pos(self, distance, angle, robot_position):
x = distance * math.cos(angle) + robot_position[0]
y = -distance * math.sin(angle) + robot_position[1]
return (int(x), int(y))
def laser_points_set(self, data):
self.LASERPOINTS = []
if not data:
pass
else:
for point in data:
coordinates = self.AD2pos(point[0], point[1], point[2])
self.LASERPOINTS.append([coordinates, point[1]])
self.NP = len(self.LASERPOINTS) - 1
# Define a function (quadratic in our case) to fit the data with.
def linear_func(self, p, x):
m, b = p
return m * x + b
def odr_fit(self, laser_points):
x = np.array([i[0][0] for i in laser_points])
y = np.array([i[0][1] for i in laser_points])
# Create a model for fitting.
linear_model = Model(self.linear_func)
# Create a RealData object using our initiated data from above.
data = RealData(x, y)
# Set up ODR with the model and data.
odr_model = ODR(data, linear_model, beta0=[0., 0.])
# Run the regression.
out = odr_model.run()
m, b = out.beta
return m, b
def predictPoint(self, line_params, sensed_point, robotpos):
m, b = self.points_2line(robotpos, sensed_point)
params1 = self.lineForm_Si2G(m, b)
predx, predy = self.line_intersect_general(params1, line_params)
return predx, predy
def seed_segment_detection(self, robot_position, break_point_ind):
flag = True
self.NP = max(0, self.NP)
self.SEED_SEGMENTS = []
for i in range(break_point_ind, (self.NP - self.PMIN)):
predicted_points_to_draw = []
j = i + self.SNUM
m, c = self.odr_fit(self.LASERPOINTS[i:j])
params = self.lineForm_Si2G(m, c)
for k in range(i, j):
predicted_point = self.predictPoint(params, self.LASERPOINTS[k][0], robot_position)
predicted_points_to_draw.append(predicted_point)
d1 = self.dist_point2point(predicted_point, self.LASERPOINTS[k][0])
if d1 > self.DELTA:
flag = False
break
d2 = self.dist_point2line(params, predicted_point)
if d2 > self.EPSILON:
flag = False
break
if flag:
self.LINE_PARAMS = params
return [self.LASERPOINTS[i:j], predicted_points_to_draw, (i, j)]
return False
def seed_segment_growing(self, indices, break_point):
line_eq = self.LINE_PARAMS
i, j = indices
# Beginning and Final points in the line segment
PB, PF = max(break_point, i - 1), min(j + 1, len(self.LASERPOINTS) - 1)
while self.dist_point2line(line_eq, self.LASERPOINTS[PF][0]) < self.EPSILON:
if PF > self.NP - 1:
break
else:
m, b = self.odr_fit(self.LASERPOINTS[PB:PF])
line_eq = self.lineForm_Si2G(m, b)
POINT = self.LASERPOINTS[PF][0]
PF = PF + 1
NEXTPOINT = self.LASERPOINTS[PF][0]
if self.dist_point2point(POINT, NEXTPOINT) > self.GMAX:
break
PF = PF - 1
while self.dist_point2line(line_eq, self.LASERPOINTS[PB][0]) < self.EPSILON:
if PB < break_point:
break
else:
m, b = self.odr_fit(self.LASERPOINTS[PB:PF])
line_eq = self.lineForm_Si2G(m, b)
POINT = self.LASERPOINTS[PB][0]
PB = PB - 1
NEXTPOINT = self.LASERPOINTS[PB][0]
if self.dist_point2point(POINT, NEXTPOINT) > self.GMAX:
break
PB = PB + 1
LR = self.dist_point2point(self.LASERPOINTS[PB][0], self.LASERPOINTS[PF][0])
PR = len(self.LASERPOINTS[PB:PF])
if (LR >= self.LMIN) and (PR >= self.PMIN):
self.LINE_PARAMS = line_eq
m, b = self.lineForm_G2SI(line_eq[0], line_eq[1], line_eq[2])
self.two_points = self.line_2points(m, b)
self.LINE_SEGMENTS.append((self.LASERPOINTS[PB + 1][0], self.LASERPOINTS[PF - 1][0]))
return [self.LASERPOINTS[PB:PF], self.two_points,
(self.LASERPOINTS[PB + 1][0], self.LASERPOINTS[PF - 1][0]), PF, line_eq, (m, b)]
else:
return False
def lineFeats2point(self):
new_rep = [] # the new representation of the features
for feature in self.FEATURES:
projection = self.projection_point2line((0,0), feature[0][0], feature[0][1])
new_rep.append([feature[0], feature[1], projection])
return new_rep
def landmark_association(landmarks):
thresh = 10 # 10 was default
for l in landmarks:
flag = False
for i, Landmark in enumerate(Landmarks):
dist = featuresDetection.dist_point2point(l[2], Landmark[2])
if dist < thresh:
if not is_overlap(l[1], Landmark[1]):
continue
else:
Landmarks.pop(i)
Landmarks.insert(i, l)
flag = True
break
if not flag:
Landmarks.append(l)
def is_overlap(seg1, seg2):
length1 = featuresDetection.dist_point2point(seg1[0], seg1[1])
length2 = featuresDetection.dist_point2point(seg2[0], seg2[1])
center1 = ((seg1[0][0] + seg1[1][0]) / 2, (seg1[0][1] + seg1[1][1]) / 2)
center2 = ((seg2[0][0] + seg2[1][0]) / 2, (seg2[0][1] + seg2[1][1]) / 2)
dist = featuresDetection.dist_point2point(center1, center2)
if dist > (length1 + length2) / 2:
return False
else:
return True
main.py
# Main for Feature Extraction from 2D LIDAR
#import SLAM_YTB import env, sensors, Features
import env, sensors, Features
import random
import pygame
def random_color():
levels = range(32,256,32)
return tuple(random.choice(levels) for _ in range(3))
FeatureMAP=Features.featuresDetection()
environment = env.buildEnvironment((600,1200))
originalMap = environment.map.copy()
laser = sensors.LaserSensor(200, originalMap, uncertainty=(0.5, 0.01))
environment.map.fill((255,255,255))
environment.infomap = environment.map.copy()
originalMap = environment.map.copy()
running = True
FEATURE_DETECTION=True
BREAK_POINT_IND = 0
while running :
#environment.infomap = originalMap.copy()
FEATURE_DETECTION = True
BREAK_POINT_IND = 0
ENDPOINTS=[0,0]
sensorOn = False
PREDICTED_POINTS_TODRAW=[]
for envent in pygame.event.get():
if envent.type == pygame.QUIT:
running = False
if pygame.mouse.get_focused():
sensorOn = True
elif not pygame.mouse.get_focused():
sensorOn = False
if sensorOn:
position = pygame.mouse.get_pos()
laser.position = position
sensor_data = laser.sense_obstacles()
FeatureMAP.laser_points_set(sensor_data)
while BREAK_POINT_IND < (FeatureMAP.NP - FeatureMAP.PMIN):
seedSeg = FeatureMAP.seed_segment_detection(laser.position,BREAK_POINT_IND)
if seedSeg == False:
break
else:
seedSegment = seedSeg[0]
PREDICTED_POINTS_TODRAW = seedSeg[1]
INDICES = seedSeg[2]
results = FeatureMAP.seed_segment_growing(INDICES, BREAK_POINT_IND)
if results == False:
BREAK_POINT_IND = INDICES[1]
continue
else:
line_eq = results[1]
m, c = results[5]
line_seg = results[0]
OUTERMOST = results[2]
BREAK_POINT_IND = results[3]
ENDPOINTS[0] = FeatureMAP.projection_point2line(OUTERMOST[0], m, c)
ENDPOINTS[1] = FeatureMAP.projection_point2line(OUTERMOST[1], m, c)
FeatureMAP.FEATURES.append([[m, c], ENDPOINTS])
COLOR = random_color()
for point in line_seg:
#environment.infomap.set_at((int(point[0][0]), int(point[0][1])), (0, 255, 0))
pygame.draw.circle(environment.infomap, COLOR, (int(point[0][0]), int(point[0][1])), 2, 0)
#pygame.draw.line(environment.infomap, (255, 0, 0), ENDPOINTS[0], ENDPOINTS[1], 2)
pygame.draw.line(environment.infomap, (0, 255, 100), ENDPOINTS[0], ENDPOINTS[1], 1)
environment.dataStorage(sensor_data)
FeatureMAP.FEATURES=FeatureMAP.lineFeats2point()
Features.landmark_association(FeatureMAP.FEATURES)
for landmark in Features.Landmarks:
pygame.draw.line(environment.infomap, (100, 100, 155), landmark[1][0], landmark[1][1], 4)
environment.map.blit(environment.infomap, (0, 0))
pygame.display.update()
원본을 다운받은 곳은 초기 영상분이었습니다. 이 곳에 Map 화일들이 있습니다. https://github.com/kabbel/SLAM
GitHub - kabbel/SLAM: SLAM simulation - code heavily inspired from a YouTube video by Algobotics
SLAM simulation - code heavily inspired from a YouTube video by Algobotics - GitHub - kabbel/SLAM: SLAM simulation - code heavily inspired from a YouTube video by Algobotics
github.com
'Pathfinding' 카테고리의 다른 글
| 앞 로봇 꽁무니 따라가는 로봇. (0) | 2022.06.02 |
|---|---|
| 파이션으로 만드는 2D differential 드라이브 코드 예제. (0) | 2022.05.30 |