def main():
odom_global = [
[0, 0, 0], [-0.0300632963656, 0.0, 1.3448275862099877],
[-0.06607260741470895, -0.0008453538851817006, 2.91379310344999],
[-0.10714376028563645, -0.00293584149525012, 4.931034482759998],
[-0.14218198242984106, -0.005958795717176256, 6.724137931039991]
]
odom_relative = [[-0.0300632963656, 0, 1.34482758621],
[-0.036019232438, 0, 1.56896551724],
[-0.0411243205001, 0, 2.01724137931],
[-0.0351683844277, 0, 1.79310344828]]
visual_global = [
[0, 0, 0], [0.0, 0.0, 0.0],
[0.18664165536919183, 0.0, -0.1036353915413315],
[0.45068738580529732, -0.0004776008413494182, -0.099896609254045643],
[0.71659621910361793, -0.00094121994138562253, 0.31771146544934936]
]
visual_relative = [[0, 0, 0],
[0.18664165536919183, 0, -0.10363539154133775],
[0.26404616237336254, 0, 0.0037387822872720043],
[0.26590923746410905, 0, 0.41760807470338812]]
try:
s = Slam(odom_global, odom_relative, visual_global, visual_relative)
estimated_global = s.estimate_global()
print(estimated_global)
except Exception as e:
print("opencv test fail - fail")
def __init__(self, datadir, ppi, writeData=False):
# Initialise data parameters
self.ppi = ppi
self.ppi.set_velocity(0, 0)
self.img = np.zeros([240, 320, 3], dtype=np.uint8)
self.aruco_img = np.zeros([240, 320, 3], dtype=np.uint8)
self.previous_time = -1
# Set up subsystems
camera_matrix, dist_coeffs, scale, baseline = self.getCalibParams(
datadir)
# Control subsystem
self.keyboard = Keyboard.Keyboard(self.ppi)
# SLAM subsystem
self.pibot = Robot.Robot(baseline, scale, camera_matrix, dist_coeffs)
self.aruco_det = aruco.aruco_detector(self.pibot, marker_length=0.07)
self.slam = Slam.Slam(self.pibot)
# Optionally record input data to a dataset
if writeData:
self.data = dh.DatasetWriter('test')
else:
self.data = None
self.output = dh.OutputWriter('system_output')
def setUp(self):
self.world = sim_framework.World()
# Walls for RANSAC.
self.world.add_obs(sim_framework.Wall(-15, '-x'))
self.world.add_obs(sim_framework.Wall(15, '+x'))
self.world.add_obs(sim_framework.Wall(-15, '-y'))
self.world.add_obs(sim_framework.Wall(15, '+y'))
# Small box for Spikes.
self.world.add_obs(sim_framework.Box(3, 3.2, -3.2, -3))
self.bot = sim_framework.CircleBot(1, 0, 0, 0)
self.world.add_ent(self.bot)
self.bot_control = sim_framework.SimBotControl(self.world, self.bot)
self.slam = Slam(self.bot_control)
def __init__(self):
#robot parameters
self.omtrek_wiel = 0.08 * np.pi
self.afstand_wielen = 0.26
self.correctiefactor = 0.95
self.scan_hoek = 365 / 4 / 60
self.Robot_parts = [
'linkermotor', 'rechtermotor', 'Laser_Sensor', 'Laser_Sensor0',
'Laser_Sensor1', 'Laser_Sensor2', 'Lidar_Motor', 'Dummy'
]
self.motors = ['linkermotor', 'rechtermotor']
self.sensors = [
'Laser_Sensor', 'Laser_Sensor0', 'Laser_Sensor1', 'Laser_Sensor2'
]
self.rejection_distance = 0.2
#initialize slam
self.ObjSlam = Slam()
#startup parameters
self.modus = 0 #0 is ronde scannen
#1 is stukje rijden
self.ticks = 0
self.locatie = [0, 0, 0]
self.distance = {
'Laser_Sensor': [],
'Laser_Sensor0': [],
'Laser_Sensor1': [],
'Laser_Sensor2': []
}
self.map_landmark = []
self.geschiedenis_locatie_x = []
self.geschiedenis_locatie_y = []
self.slam_position = [0, 0, 0]
self.real_position = [0, 0]
self.clientID = make_connection()
self.handle = get_handle(self.clientID, self.Robot_parts)
self.old_motor_position = position_motor(self.handle, self.motors,
self.clientID)
vrep.simxSynchronousTrigger(self.clientID)
plt.ion()
plt.show()
def __init__(self, datadir, ppi):
# Initialise
self.ppi = ppi
self.ppi.set_velocity(0, 0)
self.img = np.zeros([240, 320, 3], dtype=np.uint8)
self.aruco_img = np.zeros([240, 320, 3], dtype=np.uint8)
# Keyboard teleoperation components
# Get camera / wheel calibration info for SLAM
camera_matrix, dist_coeffs, scale, baseline = self.getCalibParams(
datadir)
# SLAM components
self.pibot = Robot.Robot(baseline, scale, camera_matrix, dist_coeffs)
self.aruco_det = aruco.aruco_detector(self.pibot, marker_length=0.1)
self.slam = Slam.Slam(self.pibot)
tracker_type = FeatureTrackerTypes.DES_BF # descriptor-based, brute force matching with knn
#tracker_type = FeatureTrackerTypes.DES_FLANN # descriptor-based, FLANN-based matching
# select your tracker configuration (see the file feature_tracker_configs.py)
tracker_config = FeatureTrackerConfigs.TEST
tracker_config['num_features'] = num_features
tracker_config['match_ratio_test'] = 0.8 # 0.7 is the default
tracker_config['tracker_type'] = tracker_type
feature_tracker = feature_tracker_factory(**tracker_config)
#============================================
# create SLAM object
#============================================
slam = Slam(cam, feature_tracker) #, grountruth)
timer = Timer()
#============================================
# N.B.: keep this coherent with the above forced camera settings
img_ref = cv2.imread('../data/kitti06-12.png', cv2.IMREAD_COLOR)
#img_cur = cv2.imread('../data/kitti06-12-01.png',cv2.IMREAD_COLOR)
img_cur = cv2.imread('../data/kitti06-13.png', cv2.IMREAD_COLOR)
slam.track(img_ref, frame_id=0)
slam.track(img_cur, frame_id=1)
f_ref = slam.map.get_frame(-2)
f_cur = slam.map.get_frame(-1)
num_features = 2000
tracker_type = FeatureTrackerTypes.DES_BF # descriptor-based, brute force matching with knn
#tracker_type = FeatureTrackerTypes.DES_FLANN # descriptor-based, FLANN-based matching
# select your tracker configuration (see the file feature_tracker_configs.py)
# FeatureTrackerConfigs: SHI_TOMASI_ORB, FAST_ORB, ORB, ORB2, ORB2_FREAK, ORB2_BEBLID, BRISK, AKAZE, FAST_FREAK, SIFT, ROOT_SIFT, SURF, SUPERPOINT, FAST_TFEAT, CONTEXTDESC
tracker_config = FeatureTrackerConfigs.TEST
tracker_config['num_features'] = num_features
tracker_config['tracker_type'] = tracker_type
print('tracker_config: ', tracker_config)
feature_tracker = feature_tracker_factory(**tracker_config)
# create SLAM object
slam = Slam(cam, feature_tracker, groundtruth)
time.sleep(1) # to show initial messages
viewer3D = Viewer3D()
if platform.system() == 'Linux':
display2d = Display2D(cam.width, cam.height) # pygame interface
else:
display2d = None # enable this if you want to use opencv window
matched_points_plt = Mplot2d(xlabel='img id',
ylabel='# matches',
title='# matches')
do_step = False
is_paused = False
class Robot:
def __init__(self):
#robot parameters
self.omtrek_wiel = 0.08 * np.pi
self.afstand_wielen = 0.26
self.correctiefactor = 0.95
self.scan_hoek = 365 / 4 / 60
self.Robot_parts = [
'linkermotor', 'rechtermotor', 'Laser_Sensor', 'Laser_Sensor0',
'Laser_Sensor1', 'Laser_Sensor2', 'Lidar_Motor', 'Dummy'
]
self.motors = ['linkermotor', 'rechtermotor']
self.sensors = [
'Laser_Sensor', 'Laser_Sensor0', 'Laser_Sensor1', 'Laser_Sensor2'
]
self.rejection_distance = 0.2
#initialize slam
self.ObjSlam = Slam()
#startup parameters
self.modus = 0 #0 is ronde scannen
#1 is stukje rijden
self.ticks = 0
self.locatie = [0, 0, 0]
self.distance = {
'Laser_Sensor': [],
'Laser_Sensor0': [],
'Laser_Sensor1': [],
'Laser_Sensor2': []
}
self.map_landmark = []
self.geschiedenis_locatie_x = []
self.geschiedenis_locatie_y = []
self.slam_position = [0, 0, 0]
self.real_position = [0, 0]
self.clientID = make_connection()
self.handle = get_handle(self.clientID, self.Robot_parts)
self.old_motor_position = position_motor(self.handle, self.motors,
self.clientID)
vrep.simxSynchronousTrigger(self.clientID)
plt.ion()
plt.show()
def pos_calculate(self):
self.motor_position = position_motor(self.handle, self.motors,
self.clientID)
self.locatie, self.old_motor_position = drive_calc(
self.motor_position, self.old_motor_position, self.motors,
self.omtrek_wiel, self.afstand_wielen, self.locatie,
self.correctiefactor)
self.geschiedenis_locatie_x.append(self.locatie[0])
self.geschiedenis_locatie_y.append(self.locatie[1])
#echte positie ophalen uit vrep, eerst rotatie dan positie
orrientatie = get_orrientation(self.handle, self.clientID, 'Dummy')
self.real_position = get_position(self.handle, self.clientID, 'Dummy')
def drive(self):
self.driving(3, 6)
self.ticks += 1
self.pos_calculate()
ObjRobot.plotting()
if self.ticks > 20:
self.modus = 0
self.ticks = 1
def driving(self, left_motor, right_motor):
move_motor(self.handle, 'linkermotor', left_motor, self.clientID)
move_motor(self.handle, 'rechtermotor', right_motor, self.clientID)
def scanning(self):
self.driving(0, 0)
lidar_positie = position_lidar_motor(self.handle, 'Lidar_Motor',
self.clientID)
lidar_positie += (self.scan_hoek / 365) * 2 * np.pi
self.distance = distance_sensor(self.handle, self.sensors,
self.clientID, self.distance)
if lidar_positie > np.pi / 2:
lidar_positie = 0
self.measured_points_x, self.measured_points_y, self.map_x, self.map_y, self.robot_map_x, self.robot_map_y, self.new_location = self.ObjSlam.calculations(
self.distance, self.locatie)
real_locatie = get_position(self.handle, self.clientID, 'Dummy')
ObjRobot.plotting()
self.distance = {
'Laser_Sensor': [],
'Laser_Sensor0': [],
'Laser_Sensor1': [],
'Laser_Sensor2': []
}
self.modus = 1
move_lidar_motor(self.handle, 'Lidar_Motor', lidar_positie,
self.clientID)
def plotting(self):
#rood is berekende locatie
#groen is werkelijke locatie
plt.gcf().clear()
plt.scatter(self.locatie[0], self.locatie[1], color='red')
if self.ticks > 0:
plt.scatter(self.real_position[1][1],
-self.real_position[1][0],
color='green')
if self.new_location is not False:
plt.scatter(self.new_location[0],
self.new_location[1],
color='orange')
plt.scatter(self.measured_points_x,
self.measured_points_y,
color='purple')
plt.scatter(self.robot_map_x, self.robot_map_y, color='blue')
plt.scatter(self.map_x, self.map_y, color='yellow')
plt.axis('equal')
plt.draw
plt.pause(0.001)
def __init__(self):
Slam.__init__(self)
class TestSlamMethods(unittest.TestCase):
def setUp(self):
self.world = sim_framework.World()
# Walls for RANSAC.
self.world.add_obs(sim_framework.Wall(-15, '-x'))
self.world.add_obs(sim_framework.Wall(15, '+x'))
self.world.add_obs(sim_framework.Wall(-15, '-y'))
self.world.add_obs(sim_framework.Wall(15, '+y'))
# Small box for Spikes.
self.world.add_obs(sim_framework.Box(3, 3.2, -3.2, -3))
self.bot = sim_framework.CircleBot(1, 0, 0, 0)
self.world.add_ent(self.bot)
self.bot_control = sim_framework.SimBotControl(self.world, self.bot)
self.slam = Slam(self.bot_control)
def tearDown(self):
del self.slam
del self.bot_control
del self.bot
del self.world
def test_init(self):
self.assertIsInstance(self.world, sim_framework.World)
self.assertIsInstance(self.bot, sim_framework.CircleBot)
self.assertIsInstance(self.bot_control, sim_framework.SimBotControl)
self.assertIsInstance(self.slam, Slam)
def test_landmark_extraction(self):
spikes = self.slam.extract_spike(
self.slam.control.get_distance_reading())
self.assertEqual(len(spikes), 2)
self.assertTrue(
min(map(lambda p: point_distance(p, (3.1, -3)), spikes)) < 0.01)
self.assertTrue(
min(map(lambda p: point_distance(p, (3, -3.1)), spikes)) < 0.01)
ransacs = self.slam.extract_ransac(
self.slam.control.get_distance_reading(), seed_override=0)
self.assertEqual(len(ransacs), 4)
self.assertTrue(
min(map(lambda p: point_distance(p, (15, 0)), ransacs)) < 1)
self.assertTrue(
min(map(lambda p: point_distance(p, (-15, 0)), ransacs)) < 1)
self.assertTrue(
min(map(lambda p: point_distance(p, (0, 15)), ransacs)) < 0.1)
self.assertTrue(
min(map(lambda p: point_distance(p, (0, -15)), ransacs)) < 0.1)
def test_landmark_association(self):
# Landmark DB should be empty, so this should insert a new spike landmark.
result = self.slam.associate_landmarks(LandmarkType.SPIKE, [(0, 0)])
self.assertEqual(result,
[(Landmark(LandmarkType.SPIKE, 0, 0), (0, 0))])
self.assertEqual(
self.slam.landmarks.find_nearest(LandmarkType.SPIKE, 10, 10),
Landmark(LandmarkType.SPIKE, 0, 0))
# Make sure the landmark observation is associated with the closest landmark.
# Also test that the old landmark's times seen is incremented.
self.slam.landmarks.insert_landmark(Landmark(LandmarkType.SPIKE, 1, 0))
result = self.slam.associate_landmarks(LandmarkType.SPIKE, [(-1, 0)])
self.assertEqual(result,
[(Landmark(LandmarkType.SPIKE, 0, 0), (-1, 0))])
self.assertEqual(result[0][0].times_seen, 2)
# Test that far away associations are discarded.
result = self.slam.associate_landmarks(LandmarkType.SPIKE, [(10, 0)])
self.assertEqual(result,
[(Landmark(LandmarkType.SPIKE, 10, 0), (10, 0))])
self.assertEqual(
self.slam.landmarks.find_nearest(LandmarkType.SPIKE, 10, 10),
Landmark(LandmarkType.SPIKE, 10, 0))
# Test that incorrect types are not associated.
result = self.slam.associate_landmarks(LandmarkType.RANSAC, [(0, 1)])
self.assertEqual(result,
[(Landmark(LandmarkType.RANSAC, 0, 1), (0, 1))])
self.assertEqual(
self.slam.landmarks.find_nearest(LandmarkType.RANSAC, 0, 0),
Landmark(LandmarkType.RANSAC, 0, 1))
# import setup_path
import airsim
from slam import Slam
import assistant
from clients import Clients
import numpy as np
tmp_dir = "./img"
assistant.create_directory(tmp_dir)
clients = Clients()
slam = Slam()
airsim.wait_key('Press any key to takeoff')
slam.calibrate_camera(clients.client_2)
clients.takeoff()
assistant.print_state(clients.client_1)
clients.move_and_take_photos()
slam.append(np.array(clients.queue.queue))
#print(slam.images)
#print(slam.images.shape)
slam.estimate_trajectory_dataset()
slam.plot_trajectory()
clients.end()
async def main(sl, im, od):
collector = DataCollector(sl, im, od)
collector_future = asyncio.ensure_future(collector.run())
uri = f"ws://{SERVER['HOST']}:{SERVER['PORT']}/collector"
async with websockets.connect(uri) as ws:
print('connected')
await ws.send(json.dumps({"action": "register", "data": "sensors"}))
while True:
raw_resp = await ws.recv()
resp = json.loads(raw_resp)
if resp['action'] == 'set_saving':
print(f'Saving switch {resp["data"]}')
if resp['data'] and not await collector.is_saver():
await collector.set_saver(await
Saver.create(f'{ROOT}/data/'))
else:
await collector.finish()
await ws.send(
json.dumps({
'action': 'saving',
'data': await collector.is_saver()
}))
if __name__ == "__main__":
loop = asyncio.get_event_loop()
imu = IMU()
slam = Slam()
odometer = loop.run_until_complete(Odometer.create(loop))
loop.run_until_complete(main(slam, imu, odometer))