diff --git a/laser_scan_matcher/models/model_conv_16.pth b/laser_scan_matcher/models/model_conv_16.pth new file mode 100644 index 00000000..7f42df8d Binary files /dev/null and b/laser_scan_matcher/models/model_conv_16.pth differ diff --git a/laser_scan_matcher/models/model_match_16.pth b/laser_scan_matcher/models/model_match_16.pth new file mode 100644 index 00000000..d2732dd3 Binary files /dev/null and b/laser_scan_matcher/models/model_match_16.pth differ diff --git a/laser_scan_matcher/scripts/__pycache__/config.cpython-38.pyc b/laser_scan_matcher/scripts/__pycache__/config.cpython-38.pyc deleted file mode 100644 index 13ecbd24..00000000 Binary files a/laser_scan_matcher/scripts/__pycache__/config.cpython-38.pyc and /dev/null differ diff --git a/laser_scan_matcher/scripts/__pycache__/helpers.cpython-38.pyc b/laser_scan_matcher/scripts/__pycache__/helpers.cpython-38.pyc deleted file mode 100644 index afa989b7..00000000 Binary files a/laser_scan_matcher/scripts/__pycache__/helpers.cpython-38.pyc and /dev/null differ diff --git a/laser_scan_matcher/scripts/__pycache__/laser_scan_matcher.cpython-38.pyc b/laser_scan_matcher/scripts/__pycache__/laser_scan_matcher.cpython-38.pyc deleted file mode 100644 index 97160fb6..00000000 Binary files a/laser_scan_matcher/scripts/__pycache__/laser_scan_matcher.cpython-38.pyc and /dev/null differ diff --git a/laser_scan_matcher/scripts/__pycache__/model.cpython-38.pyc b/laser_scan_matcher/scripts/__pycache__/model.cpython-38.pyc deleted file mode 100644 index 102190a6..00000000 Binary files a/laser_scan_matcher/scripts/__pycache__/model.cpython-38.pyc and /dev/null differ diff --git a/laser_scan_matcher/scripts/config.py b/laser_scan_matcher/scripts/config.py deleted file mode 100644 index 603b00b2..00000000 --- a/laser_scan_matcher/scripts/config.py +++ /dev/null @@ -1,116 +0,0 @@ -import numpy as np - -import argparse - -class Configuration: - def __init__(self, train=False, laser=True, evaluation=False, data_processing=False): - parser = argparse.ArgumentParser() - - parser.add_argument('--num_workers', default=16, type=int) - parser.add_argument('--batch_size', default=1024, type=int) - parser.add_argument('--distance_cache', type=str, default=None, help='cached overlap info to start with') - - if train: - parser.add_argument('--num_epoch', default=45, type=int) - parser.add_argument('--outf', type=str, default='matcher', help='output folder') - parser.add_argument('--lr', type=float, default=1e-3, help='learning rate for optimizer') - if laser: - parser.add_argument('--name', type=str, default='laser_dataset', help="name for the created dataset/models") - parser.add_argument('--train_transform',action='store_true') - parser.add_argument('--train_match',action='store_true') - parser.add_argument('--lock_conv', action='store_true') - parser.add_argument('--curriculum', action='store_true') - parser.add_argument('--augmentation_probability', type=float, default=0.8) - parser.add_argument('--dist_weight_ratio', type=int, default=2) - parser.add_argument('--edge_trimming', type=int, default=12) - else: - parser.add_argument('--train_set', default='train', help='Which subset of the dataset to use for training') - parser.add_argument('--validation_set', default='val', help='Which subset of the dataset to use for validation') - - if laser: - parser.add_argument('--bag_file', type=str, help="bag file") - parser.add_argument('--bag_files', nargs='+', help="bag file") - parser.add_argument('--model_dir', type=str, default='', help='directory containing pretrained model to start with') - parser.add_argument('--model_epoch', type=str, default='', help='epoch number for pretrained model to start with') - parser.add_argument('--lidar_topic', type=str, default='/Cobot/Laser') - parser.add_argument('--localization_topic', type=str, default='/Cobot/Localization') - parser.add_argument('--lidar_max_range', type=float, default=10) - - if evaluation and not laser: - parser.add_argument('--threshold_min', type=float, default=0, help='minimum threshold to test for evaluation') - parser.add_argument('--threshold_max', type=float, default=16, help='maximum threshold to test for evaluation') - parser.add_argument('--evaluation_set', default='val', help='Which subset of the dataset to use for evaluation') - - if data_processing: - parser.add_argument('--lidar_fov', type=float, default=np.pi) - parser.add_argument('--overlap_radius', type=float, default=4) - parser.add_argument('--overlap_sample_resolution', type=int, default=10) - parser.add_argument('--close_distance_threshold', type=float, default=2.0) - parser.add_argument('--far_distance_threshold', type=float, default=3.5) - parser.add_argument('--overlap_similarity_threshold', type=float, default=0.7) - parser.add_argument('--time_ignore_threshold', type=float, default=1.0) - if not laser: - parser.add_argument('--augmentation_probability', type=float, default=0.8) - parser.add_argument('--lidar_max_range', type=float, default=10) - - - self.parser = parser - - def add_argument(self, *args, **kwargs): - self.parser.add_argument(*args, **kwargs) - - def parse(self): - return self.parser.parse_args() - - -execution_config = { - 'BATCH_SIZE': 768, - 'NUM_WORKERS': 8, -} - -training_config = { - 'TRAIN_SET': 'train', - 'VALIDATION_SET': 'val', - 'NUM_EPOCH': 90, - 'LASER_MATCH_WEIGHT': 0.8, - 'LASER_TRANS_WEIGHT': 0.2 -} - -evaluation_config = { - 'THRESHOLD_MIN': 0, - 'THRESHOLD_MAX': 16, - 'EVALUATION_SET': 'val' -} - -lidar_config = { - 'FOV': np.pi, - 'MAX_RANGE': 10, -} - -data_config = { - 'OVERLAP_RADIUS': 4, - 'OVERLAP_SAMPLE_RESOLUTION': 10, - 'CLOSE_DISTANCE_THRESHOLD': 1.5, - 'FAR_DISTANCE_THRESHOLD': 3, - 'OVERLAP_SIMILARITY_THRESHOLD': 0.7, - 'TIME_IGNORE_THRESHOLD': 1, - 'MATCH_REPEAT_FACTOR': 1, - 'AUGMENTATION_PROBABILITY': 0.8 -} - -DEV_SPLIT = 0.8 -data_generation_config = { - 'TIME_SPACING': 0.075, - 'TRAIN_SPLIT': 0.15, - 'DEV_SPLIT': DEV_SPLIT, - 'VAL_SPLIT': 1 - DEV_SPLIT, - 'LIDAR_TOPIC': '/Cobot/Laser', - 'LOCALIZATION_TOPIC': '/Cobot/Localization', - 'MAX_PARTITION_COUNT': 10, - 'MAX_PARTITION_SIZE': 1200, - 'MIN_PARTITION_SIZE': 50 -} - -visualization_config = { - 'TIMESTEP': 1.5 -} diff --git a/laser_scan_matcher/scripts/match_laser_scans.py b/laser_scan_matcher/scripts/match_laser_scans.py index 0f7d182c..178993eb 100755 --- a/laser_scan_matcher/scripts/match_laser_scans.py +++ b/laser_scan_matcher/scripts/match_laser_scans.py @@ -4,49 +4,18 @@ from sensor_msgs.msg import LaserScan from laser_scan_matcher.srv import MatchLaserScans, MatchLaserScansResponse import sys -import os from os import path import torch # TODO fix this hack -sys.path.append(path.dirname(path.dirname(path.dirname( path.dirname( path.abspath(__file__) ) ) ))) -sys.path.append(path.dirname(__file__)) - -from model import FullNet, EmbeddingNet, LCCNet, DistanceNet, StructuredEmbeddingNet, ScanMatchNet, ScanConvNet, ScanTransformNet, ScanSingleConvNet, ScanUncertaintyNet - -def create_laser_networks(model_dir, model_epoch, multi_gpu=True): - scan_conv = ScanConvNet() - if model_dir: - scan_conv.load_state_dict(torch.load(os.path.join(model_dir, 'model_conv_' + model_epoch + '.pth'))) - - scan_transform = ScanTransformNet() - if model_dir: - transform_path = os.path.join(model_dir, 'model_transform_' + model_epoch + '.pth') - if os.path.exists(transform_path): - scan_transform.load_state_dict(torch.load(transform_path)) - else: - print("Warning: no `transform` network found for provided model_dir and epoch") - - scan_match = ScanMatchNet() - if model_dir: - scan_match.load_state_dict(torch.load(os.path.join(model_dir, 'model_match_' + model_epoch + '.pth'))) - - if multi_gpu and torch.cuda.device_count() > 1: - print("Let's use", torch.cuda.device_count(), "GPUs!") - scan_conv = torch.nn.DataParallel(scan_conv) - scan_match = torch.nn.DataParallel(scan_match) - scan_transform = torch.nn.DataParallel(scan_transform) - - scan_conv.cuda() - scan_match.cuda() - scan_transform.cuda() - return scan_conv, scan_match, scan_transform +sys.path.append(path.join(path.dirname(path.dirname(path.dirname(path.abspath(__file__)))), 'learning_loop_closure')) +from helpers import create_laser_networks def create_scan_match_helper(scan_conv, scan_match): def match_scans(req): - scan_np = np.array(req.scan.ranges).astype(np.float32) - alt_scan_np = np.array(req.alt_scan.ranges).astype(np.float32) + scan_np = np.minimum(np.array(req.scan.ranges).astype(np.float32), 30) + alt_scan_np = np.minimum(np.array(req.alt_scan.ranges).astype(np.float32), 30) scan = torch.tensor(scan_np).cuda() alt_scan = torch.tensor(alt_scan_np).cuda() @@ -75,4 +44,4 @@ def service(): try: service() except rospy.ROSInterruptException: - pass + pass \ No newline at end of file diff --git a/laser_scan_matcher/scripts/model.py b/laser_scan_matcher/scripts/model.py deleted file mode 100644 index c6e6d048..00000000 --- a/laser_scan_matcher/scripts/model.py +++ /dev/null @@ -1,379 +0,0 @@ -import torch -import math -from torch import nn -import torch.nn.functional as F - -import os -import sys -sys.path.append(os.path.join(os.getcwd(), '..')) -from config import data_generation_config - -# Predicts a 3-length vector [0:2] are x,y translation -# [2] is theta -class TransformPredictionNetwork(nn.Module): - def __init__(self): - super(TransformPredictionNetwork, self).__init__() - self.conv1 = torch.nn.Conv1d(2, 16, 1) - self.conv2 = torch.nn.Conv1d(16, 32, 1) - self.bn1 = nn.BatchNorm1d(16) - self.bn2 = nn.BatchNorm1d(32) - self.bn3 = nn.BatchNorm1d(24) - self.fc1 = nn.Linear(32, 24) - self.fc2 = nn.Linear(24, 3) - - def forward(self, x): - x = self.bn1(self.conv1(x)) - x = self.bn2(self.conv2(x)) - x = torch.max(x, 2, keepdim=True)[0] - x = x.view(-1, 32) - x = self.bn3(self.fc1(x)) - x = self.fc2(x) - trans = x[:,0:2] - theta = x[:,2] - theta = torch.clamp(theta, min=0, max=2 * math.pi) - return trans, theta - -class TransformNet(nn.Module): - def __init__(self): - super(TransformNet, self).__init__() - self.transform_pred = TransformPredictionNetwork() - - def forward(self, x): - translation, theta = self.transform_pred(x) - - rotations = torch.zeros(x.shape[0], 2, 2).cuda() - - c = torch.cos(theta) - s = torch.sin(theta) - - rotations[:, 0, 0] = c.squeeze() - rotations[:, 1, 0] = s.squeeze() - rotations[:, 0, 1] = -s.squeeze() - rotations[:, 1, 1] = c.squeeze() - - rotated = torch.bmm(rotations, x) - translations = translation.unsqueeze(2).expand(x.shape) - - transformed = rotated + translations - return transformed, translation, theta - -EMBEDDING_SIZE = 32 -class EmbeddingNet(nn.Module): - def __init__(self): - super(EmbeddingNet, self).__init__() - self.conv1 = torch.nn.Conv1d(2, EMBEDDING_SIZE, 5, 1) - self.conv2 = torch.nn.Conv1d(EMBEDDING_SIZE, EMBEDDING_SIZE*2, 3, 2) - self.conv3 = torch.nn.Conv1d(EMBEDDING_SIZE*2, EMBEDDING_SIZE, 3, 1) - self.conv4 = torch.nn.Conv1d(EMBEDDING_SIZE, EMBEDDING_SIZE, 3) - self.conv5 = torch.nn.Conv1d(EMBEDDING_SIZE, EMBEDDING_SIZE, 1) - self.dropout = nn.Dropout(0.25) - self.bn1 = nn.BatchNorm1d(EMBEDDING_SIZE) - self.bn2 = nn.BatchNorm1d(EMBEDDING_SIZE*2) - self.bn3 = nn.BatchNorm1d(EMBEDDING_SIZE) - self.bn4 = nn.BatchNorm1d(EMBEDDING_SIZE) - self.bn5 = nn.BatchNorm1d(EMBEDDING_SIZE) - - def forward(self, x): - x = self.bn1(F.relu(self.conv1(x))) - x = self.bn2(F.relu(self.conv2(x))) - x = self.bn3(F.relu(self.conv3(x))) - x = self.bn4(F.relu(self.conv4(x))) - x = self.bn5(F.relu(self.conv5(x))) - x = F.max_pool1d(x, x.shape[2]) - x = self.dropout(x) - return x, None, None - -class DistanceNet(nn.Module): - def __init__(self, embedding=EmbeddingNet()): - super(DistanceNet, self).__init__() - self.embedding = embedding - self.dropout = nn.Dropout(0.2) - self.ff = nn.Linear(32, 1) - nn.init.xavier_uniform_(self.ff.weight) - - def forward(self, x, y): - x_emb, x_translation, x_theta = self.embedding(x) - y_emb, y_translation, y_theta = self.embedding(y) - dist = F.relu(self.ff(self.dropout(torch.cat([x_emb, y_emb], dim=1)))) - - translation = y_translation - x_translation - theta = y_theta - x_theta - - return dist, translation, theta - -class FullNet(nn.Module): - def __init__(self, embedding=EmbeddingNet()): - super(FullNet, self).__init__() - self.embedding = embedding - self.dropout = nn.Dropout(0.4) - self.ff = nn.Linear(64, 2) - self.softmax = nn.LogSoftmax(dim=1) - nn.init.xavier_uniform_(self.ff.weight) - - def forward(self, x, y): - x_emb, x_translation, x_theta = self.embedding(x) - y_emb, y_translation, y_theta = self.embedding(y) - - scores = self.ff(self.dropout(torch.cat([x_emb, y_emb], dim=1))) - - out = self.softmax(scores) - - translation = y_translation - x_translation - theta = y_theta - x_theta - - return out, (translation, theta) - -class AttentionEmbeddingNet(nn.Module): - def __init__(self, threshold=0.75, embedding=EmbeddingNet()): - super(AttentionEmbeddingNet, self).__init__() - self.embedding = embedding - # self.conv = torch.nn.Conv1d(32, 32, 1) - # self.lstm = torch.nn.LSTM(EMBEDDING_SIZE, 32, batch_first=True) - self.att_weights = torch.nn.Parameter(torch.Tensor(1, EMBEDDING_SIZE), - requires_grad=True) - - torch.nn.init.xavier_uniform_(self.att_weights.data) - - def forward(self, x, l): - batch_size, partitions, partition_and_center_size, dims = x.shape - partition_size = partition_and_center_size - 1 - centers = x[:, :, partition_size:, :] - c_in = (x[:batch_size, :partitions, :partition_size, :dims] + centers.repeat(1, 1, partition_size, 1)).view(batch_size * partitions, dims, partition_size) - c_out = self.embedding(c_in)[0] - r_in = c_out.view(batch_size, partitions, EMBEDDING_SIZE) - - weights = torch.bmm(r_in, - self.att_weights # (1, hidden_size) - .permute(1, 0) # (hidden_size, 1) - .unsqueeze(0) # (1, hidden_size, 1) - .repeat(batch_size, 1, 1) # (batch_size, hidden_size, 1) - ) - - attentions = F.softmax(F.relu(weights.squeeze(-1))) - # create mask based on the sentence lengths - mask = torch.autograd.Variable(torch.ones(attentions.size())).cuda() - for i, l in enumerate(l): - if l < partition_and_center_size - 2: - mask[i, l:] = 0 - - # apply mask and renormalize attention scores (weights) - masked = attentions * mask - _sums = masked.sum(-1).unsqueeze(1).expand_as(masked) # sums per row - attentions = masked.div(_sums) - - # apply weights - weighted = torch.mul(r_in, attentions.unsqueeze(-1).expand_as(r_in)) - - # get the final fixed vector representations of the sentences - representations = weighted.sum(1).squeeze(-1) - return representations - -class StructuredEmbeddingNet(nn.Module): - def __init__(self, threshold=0.75, embedding=EmbeddingNet()): - super(StructuredEmbeddingNet, self).__init__() - self.embedding = embedding - # self.conv = torch.nn.Conv1d(32, 32, 1) - # self.lstm = torch.nn.LSTM(EMBEDDING_SIZE, 32, batch_first=True) - self.att_weights = torch.nn.Parameter(torch.Tensor(1, EMBEDDING_SIZE), - requires_grad=True) - - torch.nn.init.xavier_uniform_(self.att_weights.data) - - def forward(self, x, l): - batch_size, partitions, partition_and_center_size, dims = x.shape - partition_size = partition_and_center_size - 1 - centers = x[:, :, partition_size:, :].squeeze() - c_in = (x[:batch_size, :partitions, :partition_size, :dims]).view(batch_size * partitions, dims, partition_size) - c_out = self.embedding(c_in)[0] - h_in = c_out.view(batch_size, partitions, EMBEDDING_SIZE) - h_out = torch.zeros(batch_size, EMBEDDING_SIZE).cuda() - - # create a mask for each part of the batch - indices = torch.arange(0, partitions).unsqueeze(0).repeat(batch_size, 1).cuda() - limits = l.unsqueeze(1).expand(batch_size, partitions) - mask = indices <= limits - - norm_dist = torch.distributions.Normal(0, 0.5) - for i in range(partitions): - distances = torch.norm(centers[:, i, :].unsqueeze(1).repeat(1, partitions, 1) - centers[:, :, :], dim=2) - weights = torch.exp(norm_dist.log_prob(distances)) - # Zero out weights for things we shouldnt care about - weights *= mask - - h_out += torch.sum(weights.unsqueeze(2) * h_in, dim=1) - - # get the final fixed vector representations of the sentences - return h_out - -class LCCNet(nn.Module): - def __init__(self, embedding=EmbeddingNet()): - super(LCCNet, self).__init__() - self.embedding = embedding - # self.conv1 = torch.nn.Conv1d(16, 12, 1) - # self.conv2 = torch.nn.Conv1d(12, 8, 1) - # self.bn1 = nn.BatchNorm1d(12) - # self.bn2 = nn.BatchNorm1d(8) - self.ff = nn.Linear(16, 2) - nn.init.xavier_uniform_(self.ff.weight) - - def forward(self, x): - emb, _, _ = self.embedding(x) - - # x = self.bn1(self.conv1(emb.unsqueeze(2))) - # x = self.bn2(self.conv2(x)) - out = self.ff(emb) - - return out - -class ScanTransformNet(nn.Module): - def __init__(self, scan_size=1081): - super(ScanTransformNet, self).__init__() - - self.ff = nn.Sequential( - nn.Linear(52 * 64, 1024), - nn.Dropout(p=0.3), - nn.Linear(1024, 1024), - nn.Dropout(p=0.3), - nn.Linear(1024, 512), - nn.Dropout(p=0.3), - nn.Linear(512, 3) - ) - - def forward(self, x): - return self.ff(x).squeeze(1) - -class ScanMatchNet(nn.Module): - def __init__(self): - super(ScanMatchNet, self).__init__() - - self.ff = nn.Sequential( - nn.Linear(52 * 64, 1024), - nn.Dropout(p=0.3), - nn.Linear(1024, 1024), - nn.Dropout(p=0.3), - nn.Linear(1024, 512), - nn.Dropout(p=0.3), - nn.Linear(512, 2) - ) - - def forward(self, x): - return self.ff(x).squeeze(1) - -class ScanUncertaintyNet(nn.Module): - def __init__(self): - super(ScanUncertaintyNet, self).__init__() - - self.ff = nn.Sequential( - nn.Linear(52 * 64, 1024), - nn.Dropout(p=0.3), - nn.Linear(1024, 1024), - nn.Dropout(p=0.3), - nn.Linear(1024, 512), - nn.Dropout(p=0.3), - nn.Linear(512, 2) - ) - - def forward(self, x): - return self.ff(x).squeeze(1) - -class ScanConvNet(nn.Module): - def __init__(self): - super(ScanConvNet, self).__init__() - - self.conv = nn.Conv1d(64, 64, 3, padding=1) - self.relu = nn.ReLU() - self.initialConv = nn.Sequential( - nn.Conv1d(2, 64, 7), - nn.MaxPool1d(kernel_size=1, stride=3) - ) - - self.avgPool = nn.AvgPool1d(1, 7) - - def forward(self, x, y): - xy = torch.cat((x.unsqueeze(1), y.unsqueeze(1)), dim=1) - last_checkpoint = self.initialConv(xy) - - #First block - conv = self.conv(last_checkpoint) - conv = self.relu(conv) - conv = self.conv(conv) - conv = conv + last_checkpoint - last_checkpoint = self.relu(conv) - - #Second block - conv = self.conv(last_checkpoint) - conv = self.relu(conv) - conv = self.conv(conv) - conv = conv + last_checkpoint - last_checkpoint = self.relu(conv) - - #Third block - conv = self.conv(last_checkpoint) - conv = self.relu(conv) - conv = self.conv(conv) - conv = conv + last_checkpoint - last_checkpoint = self.relu(conv) - - #Fourth block - conv = self.conv(last_checkpoint) - conv = self.relu(conv) - conv = self.conv(conv) - conv = conv * last_checkpoint - last_checkpoint = self.relu(conv) - - conv = self.avgPool(conv) - - output = conv.view(xy.shape[0], -1) - - return output - -class ScanSingleConvNet(nn.Module): - def __init__(self): - super(ScanSingleConvNet, self).__init__() - - self.conv = nn.Conv1d(64, 64, 3, padding=1) - self.relu = nn.ReLU() - self.initialConv = nn.Sequential( - nn.Conv1d(1, 64, 7), - nn.MaxPool1d(kernel_size=1, stride=3) - ) - - self.avgPool = nn.AvgPool1d(1, 7) - - def forward(self, x): - last_checkpoint = self.initialConv(x) - - #First block - conv = self.conv(last_checkpoint) - conv = self.relu(conv) - conv = self.conv(conv) - conv = conv + last_checkpoint - last_checkpoint = self.relu(conv) - - #Second block - conv = self.conv(last_checkpoint) - conv = self.relu(conv) - conv = self.conv(conv) - conv = conv + last_checkpoint - last_checkpoint = self.relu(conv) - - #Third block - conv = self.conv(last_checkpoint) - conv = self.relu(conv) - conv = self.conv(conv) - conv = conv + last_checkpoint - last_checkpoint = self.relu(conv) - - #Fourth block - conv = self.conv(last_checkpoint) - conv = self.relu(conv) - conv = self.conv(conv) - conv = conv * last_checkpoint - last_checkpoint = self.relu(conv) - - conv = self.avgPool(conv) - - output = conv.view(x.shape[0], -1) - - return output - \ No newline at end of file diff --git a/launch/run_stack.launch b/launch/run_stack.launch index d0c535b3..f32408b9 100644 --- a/launch/run_stack.launch +++ b/launch/run_stack.launch @@ -1,5 +1,6 @@ - - + + + diff --git a/local_uncertainty_estimator/CMakeLists.txt b/local_uncertainty_estimator/CMakeLists.txt new file mode 100644 index 00000000..eb1ea19b --- /dev/null +++ b/local_uncertainty_estimator/CMakeLists.txt @@ -0,0 +1,16 @@ +PROJECT(local_uncertainty_estimator) +CMAKE_MINIMUM_REQUIRED(VERSION 2.8.3) + + +FIND_PACKAGE(catkin REQUIRED COMPONENTS message_generation std_msgs sensor_msgs rospy) + +add_service_files( + DIRECTORY srv + FILES EstimateLocalUncertainty.srv +) + +generate_messages(DEPENDENCIES std_msgs sensor_msgs) +catkin_package(CATKIN_DEPENDS std_msgs sensor_msgs message_runtime) +catkin_install_python(PROGRAMS scripts/estimate_local_uncertainty.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) \ No newline at end of file diff --git a/local_uncertainty_estimator/Makefile b/local_uncertainty_estimator/Makefile new file mode 100644 index 00000000..dd10ca69 --- /dev/null +++ b/local_uncertainty_estimator/Makefile @@ -0,0 +1,17 @@ +# acceptable build_types: Release/Debug/Profile +# build_type=Release +build_type=Release + +all: build/CMakeLists.txt.copy + $(info Build_type is [${build_type}]) + $(MAKE) --no-print-directory -C build + +clean: + rm -rf bin lib build srv_gen + +build/CMakeLists.txt.copy: build CMakeLists.txt Makefile + cd build && cmake -DCMAKE_BUILD_TYPE=$(build_type) .. + cp CMakeLists.txt build/CMakeLists.txt.copy + +build: + mkdir -p build diff --git a/local_uncertainty_estimator/manifest.xml b/local_uncertainty_estimator/manifest.xml new file mode 100644 index 00000000..98907911 --- /dev/null +++ b/local_uncertainty_estimator/manifest.xml @@ -0,0 +1,18 @@ + + + PyTorch network for predicting the local uncertainty of a given scan + + local_uncertainty_estimator + Kavan Sikand + MIT + + + + + + message_generation + message_runtime + + + + diff --git a/local_uncertainty_estimator/models/model_conv_44.pth b/local_uncertainty_estimator/models/model_conv_44.pth new file mode 100644 index 00000000..781c2893 Binary files /dev/null and b/local_uncertainty_estimator/models/model_conv_44.pth differ diff --git a/local_uncertainty_estimator/models/model_uncertainty_44.pth b/local_uncertainty_estimator/models/model_uncertainty_44.pth new file mode 100644 index 00000000..976dd4da Binary files /dev/null and b/local_uncertainty_estimator/models/model_uncertainty_44.pth differ diff --git a/local_uncertainty_estimator/package.xml b/local_uncertainty_estimator/package.xml new file mode 100644 index 00000000..ab840534 --- /dev/null +++ b/local_uncertainty_estimator/package.xml @@ -0,0 +1,20 @@ + + 1.0.0 + + PyTorch network for predicting local uncertainty in a given laser scan + + local_uncertainty_estimator + Kavan Sikand + Kavan Sikand + MIT + + catkin + message_generation + std_msgs + sensor_msgs + message_runtime + std_msgs + sensor_msgs + + + diff --git a/local_uncertainty_estimator/scripts/estimate_local_uncertainty.py b/local_uncertainty_estimator/scripts/estimate_local_uncertainty.py new file mode 100755 index 00000000..4aec5c5b --- /dev/null +++ b/local_uncertainty_estimator/scripts/estimate_local_uncertainty.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python +import rospy +import numpy as np +from sensor_msgs.msg import LaserScan +from local_uncertainty_estimator.srv import EstimateLocalUncertainty, EstimateLocalUncertaintyResponse +import sys +from os import path +import torch + +# TODO fix this hack +sys.path.append(path.join(path.dirname(path.dirname(path.dirname(path.abspath(__file__)))), 'learning_loop_closure')) +from helpers import create_lu_networks + +def create_lu_helper(scan_conv, scan_uncertainty): + def match_scans(req): + scan_np = np.minimum(np.array(req.scan.ranges).astype(np.float32), 30) + + scan = torch.tensor(scan_np).cuda() + with torch.no_grad(): + conv = scan_conv(scan.unsqueeze(0).unsqueeze(0)) + condition, scale = scan_uncertainty(conv)[0] + + return EstimateLocalUncertaintyResponse(condition, scale) + + return match_scans + + +def service(): + rospy.init_node('local_uncertainty_estimator', anonymous=True) + model_dir = sys.argv[1] + model_epoch = sys.argv[2] + # transform_model = sys.argv[3] + scan_conv, scan_uncertainty = create_lu_networks(model_dir, model_epoch) + scan_conv.eval() + scan_uncertainty.eval() + service = rospy.Service('estimate_local_uncertainty', EstimateLocalUncertainty, create_lu_helper(scan_conv, scan_uncertainty), buff_size=2) + rospy.spin() + +if __name__ == '__main__': + try: + service() + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/local_uncertainty_estimator/srv/EstimateLocalUncertainty.srv b/local_uncertainty_estimator/srv/EstimateLocalUncertainty.srv new file mode 100644 index 00000000..8a1319bb --- /dev/null +++ b/local_uncertainty_estimator/srv/EstimateLocalUncertainty.srv @@ -0,0 +1,4 @@ +sensor_msgs/LaserScan scan +--- +float32 condition_num +float32 scale \ No newline at end of file