Skip to content
Hide whitespace changes
Inline Side-by-side
src/main/java/data_acquisition/help_fncs/helperMatchFeaturesInRadius.m deleted 100644 → 0
View file @ 34e4eb38
function indexPairs = helperMatchFeaturesInRadius(features1, features2, ...
projectedPoints1, points2, radius, minScales, maxScales)
%helperMatchFeaturesInRadius Match features within a radius
% indexPairs = helperMatchFeaturesInRadius(feature1, feature2,
% projectedPoints1, points2, radius, minScales, maxScales) returns a
% P-by-2 matrix, indexPairs, containing the indices to the features most
% likely to correspond between the two input feature matrices satisfying
% the distance and the scale constraints.
%
% This is an example helper function that is subject to change or removal
% in future releases.
%
% Inputs
% ------
% features1 - Feature matrices in the first image
% features2 - Feature matrices in the second image
% projectedPoints1 - The projection of the world points in the
% second image that correspond to features1
% points2 - Feature points corresponding to features2
% radius - Searching radius
% minScales - Minimum scales of feature points points1
% maxScales - Maximum scales of feature points points1
%
% Output
% ------
% indexPairs - Indices of corresponding features
% Copyright 2019-2020 The MathWorks, Inc.
matchThreshold = 100;
maxRatio = 0.8;
numPoints = size(projectedPoints1, 1);
indexPairs = zeros(numPoints, 2, 'uint32');
neighborScales = points2.Scale;
neighborPoints = points2.Location;
if isscalar(radius)
radius = radius * ones(numPoints, 1);
end
for i = 1: numPoints
% Find points within a radius subjected to the scale constraint
pointIdx = findPointsInRadius(neighborPoints, projectedPoints1(i,:), ...
radius(i), neighborScales, minScales(i), maxScales(i));
if ~isempty(pointIdx)
centerFeature = features1(i,:);
nearbyFeatures = features2(pointIdx,:);
if numel(pointIdx) == 1
bestIndex = pointIdx;
else
scores = helperHammingDistance(centerFeature, nearbyFeatures);
% Find the best two matches
[minScore, index] = mink(scores, 2);
if minScore(1) < matchThreshold
% Ratio test when the best two matches have the same scale
if minScore(1) > maxRatio * minScore(2)
continue
else
bestIndex = pointIdx(index(1));
end
else
continue
end
end
indexPairs(i,:) = uint32([i, bestIndex]);
end
end
isFound = indexPairs(:, 2) > 0;
indexPairs = indexPairs(isFound, :);
[~, ia] = unique(indexPairs(:, 2), 'stable');
indexPairs = indexPairs(ia, :);
end
function index = findPointsInRadius(neighborPoints, centerPoint, radius, ...
neighborScales, minScalse, maxScales)
sqrDist = sum((neighborPoints - centerPoint).^2 , 2);
index = find(sqrDist < radius^2 & neighborScales >= minScalse & neighborScales <= maxScales);
end
src/main/java/data_acquisition/help_fncs/helperSURFFeatureExtractorFunction.m deleted 100644 → 0
View file @ 34e4eb38
function [features, featureMetrics, varargout] = helperSURFFeatureExtractorFunction(I)
% helperSURFFeatureExtractorFunction Implements the SURF feature extraction
% used in bagOfFeatures.
%
% This is an example helper function that is subject to change or removal
% in future releases.
% Copyright 2019 The MathWorks, Inc.
% Preprocess the Image
grayImage = rgb2gray(I);
% Feature Extraction
points = detectSURFFeatures(grayImage);
features = extractFeatures(grayImage, points);
% Compute the Feature Metric. Use the variance of features as the metric
featureMetrics = var(features,[],2);
% Optionally return the feature location information. The feature location
% information is used for image search applications. See the retrieveImages
% and indexImages functions.
if nargout > 2
% Return feature location information
varargout{1} = points.Location;
end
src/main/java/data_acquisition/help_fncs/helperSelectStrongConnections.m deleted 100644 → 0
View file @ 34e4eb38
function isStrong = helperSelectStrongConnections(connections, viewIds, currViewId, threshold)
%helperSelectStrongConnections select strong connections with more than
% a specified number of matches
%
% This is an example helper function that is subject to change or removal
% in future releases.
% Copyright 2019 The MathWorks, Inc.
[~,~,ib] = intersect([viewIds, currViewId*ones(numel(viewIds),1,'uint32')],...
connections{:,1:2}, 'stable', 'row');
isStrong = cellfun(@(x) size(x, 1) > threshold, connections.Matches(ib));
src/main/java/data_acquisition/help_fncs/helperTrackLastKeyFrame.m deleted 100644 → 0
View file @ 34e4eb38
%helperTrackLastKeyFrame Estimate the camera pose by tracking the last key frame
% [currPose, mapPointIdx, featureIdx] = helperTrackLastKeyFrame(mapPoints,
% views, currFeatures, currPoints, lastKeyFrameId, intrinsics) estimates
% the camera pose of the current frame by matching features with the
% previous key frame.
%
% This is an example helper function that is subject to change or removal
% in future releases.
%
% Inputs
% ------
% mapPoints - A helperMapPoints objects storing map points
% views - View attributes of key frames
% currFeatures - Features in the current frame
% currPoints - Feature points in the current frame
% lastKeyFrameId - ViewId of the last key frame
% intrinsics - Camera intrinsics
% scaleFactor - scale factor of features
%
% Outputs
% -------
% currPose - Estimated camera pose of the current frame
% mapPointIdx - Indices of map points observed in the current frame
% featureIdx - Indices of features corresponding to mapPointIdx
% Copyright 2019-2020 The MathWorks, Inc.
function [currPose, mapPointIdx, featureIdx] = helperTrackLastKeyFrame(...
mapPoints, views, currFeatures, currPoints, lastKeyFrameId, intrinsics, scaleFactor)
imageSize = intrinsics.ImageSize;
% Match features from the previous key frame with known world locations
[index3d, index2d] = findWorldPointsInView(mapPoints, lastKeyFrameId);
lastKeyFrameFeatures = views.Features{lastKeyFrameId}(index2d,:);
lastKeyFramePoints = views.Points{lastKeyFrameId}(index2d);
indexPairs = matchFeatures(currFeatures, binaryFeatures(lastKeyFrameFeatures),...
'Unique', true, 'MaxRatio', 0.9, 'MatchThreshold', 40);
% Estimate the camera pose
matchedImagePoints = currPoints.Location(indexPairs(:,1),:);
matchedWorldPoints = mapPoints.WorldPoints(index3d(indexPairs(:,2)), :);
matchedImagePoints = cast(matchedImagePoints, 'like', matchedWorldPoints);
[worldOri, worldLoc, inlier] = estimateWorldCameraPose(...
matchedImagePoints, matchedWorldPoints, intrinsics, ...
'Confidence', 95, 'MaxReprojectionError', 3, 'MaxNumTrials', 1e4);
currPose = rigid3d(worldOri, worldLoc);
% Refine camera pose only
currPose = bundleAdjustmentMotion(matchedWorldPoints(inlier,:), ...
matchedImagePoints(inlier,:), currPose, intrinsics, ...
'PointsUndistorted', true, 'AbsoluteTolerance', 1e-7,...
'RelativeTolerance', 1e-15, 'MaxIteration', 20);
% Search for more matches with the map points in the previous key frame
xyzPoints = mapPoints.WorldPoints(index3d,:);
[isInImage, projectedPoints] = helperFindProjectedPointsInImage(xyzPoints, ...
currPose, intrinsics, imageSize);
minScales = max(1, lastKeyFramePoints.Scale(isInImage)/scaleFactor);
maxScales = lastKeyFramePoints.Scale(isInImage)*scaleFactor;
r = 4;
searchRadius = r*lastKeyFramePoints.Scale(isInImage);
indexPairs = helperMatchFeaturesInRadius(lastKeyFrameFeatures(isInImage,:), ...
currFeatures.Features, projectedPoints, currPoints, searchRadius, ...
minScales, maxScales);
% Obtain the index of matched map points and features
tempIdx = find(isInImage); % Convert to linear index
mapPointIdx = index3d(tempIdx(indexPairs(:,1)));
featureIdx = indexPairs(:,2);
% Refine the camera pose again
matchedWorldPoints = mapPoints.WorldPoints(mapPointIdx, :);
matchedImagePoints = currPoints.Location(featureIdx, :);
currPose = bundleAdjustmentMotion(matchedWorldPoints, matchedImagePoints, ...
currPose, intrinsics, 'PointsUndistorted', true, 'AbsoluteTolerance', 1e-7,...
'RelativeTolerance', 1e-15, 'MaxIteration', 20);
end
\ No newline at end of file
src/main/java/data_acquisition/help_fncs/helperTrackLocalMap.m deleted 100644 → 0
View file @ 34e4eb38
function [refKeyFrameId, localKeyFrameIds, currPose, mapPointIdx, featureIdx] = ...
helperTrackLocalMap(mapPoints, directionAndDepth, vSetKeyFrames, mapPointIdx, ...
featureIdx, currPose, currFeatures, currPoints, intrinsics, scaleFactor, numLevels)
%helperTrackLocalMap Refine camera pose by tracking the local map
%
% This is an example helper function that is subject to change or removal
% in future releases.
%
% Inputs
% ------
% mapPoints - A worldpointset object storing map points
% directionAndDepth - A helperDirectionAndDepth object of map point attributes
% vSetKeyFrames - An imageviewset storing key frames
% mapPointsIndices - Indices of map points observed in the current frame
% featureIndices - Indices of features in the current frame
% corresponding to map points denoted by mapPointsIndices
% currPose - Current camera pose
% currFeatures - ORB Features in the current frame
% currPoints - Feature points in the current frame
% intrinsics - Camera intrinsics
% scaleFactor - scale factor of features
% numLevels - number of levels in feature exatraction
%
% Outputs
% -------
% mapPoints - A helperMapPointSet objects storing map points
% localKeyFrameIds - ViewIds of the local key frames
% currPose - Refined camera pose of the current frame
% mapPointIdx - Indices of map points observed in the current frame
% featureIdx - Indices of features in the current frame corresponding
% to mapPointIdx
% Copyright 2019-2020 The MathWorks, Inc.
imageSize = intrinsics.ImageSize;
[refKeyFrameId, localPointsIndices, localKeyFrameIds] = ...
updateRefKeyFrameAndLocalPoints(mapPoints, vSetKeyFrames, mapPointIdx);
% Project the map into the frame and search for more map point correspondences
newmapPointIdx = setdiff(localPointsIndices, mapPointIdx,'stable');
localMapPoints = mapPoints.WorldPoints(newmapPointIdx, :);
localFeatures = getFeatures(directionAndDepth, vSetKeyFrames.Views, newmapPointIdx);
% Filter out outliers
[inlierIndex, predictedScales, viewAngles] = removeOutlierMapPoints(mapPoints, ...
directionAndDepth, currPose, intrinsics, newmapPointIdx, scaleFactor, numLevels, imageSize);
newmapPointIdx = newmapPointIdx(inlierIndex);
localMapPoints = localMapPoints(inlierIndex,:);
localFeatures = localFeatures(inlierIndex,:);
unmatchedfeatureIdx = setdiff(cast((1:size( currFeatures.Features, 1)).', 'uint32'), ...
featureIdx,'stable');
unmatchedFeatures = currFeatures.Features(unmatchedfeatureIdx, :);
unmatchedValidPoints = currPoints(unmatchedfeatureIdx);
[R, t] = cameraPoseToExtrinsics(currPose.Rotation, currPose.Translation);
projectedPoints = worldToImage(intrinsics, R, t, localMapPoints);
% Search radius depends on scale and view direction
searchRadius = 4*ones(size(localFeatures, 1), 1);
searchRadius(viewAngles<3) = 2.5;
searchRadius = searchRadius.*predictedScales;
indexPairs = helperMatchFeaturesInRadius(localFeatures, unmatchedFeatures, ...
projectedPoints, unmatchedValidPoints, searchRadius, ...
max(1, predictedScales/scaleFactor), predictedScales);
% Refine camera pose with more 3D-to-2D correspondences
mapPointIdx = [newmapPointIdx(indexPairs(:,1)); mapPointIdx];
featureIdx = [unmatchedfeatureIdx(indexPairs(:,2)); featureIdx];
matchedMapPoints = mapPoints.WorldPoints(mapPointIdx,:);
matchedImagePoints = currPoints.Location(featureIdx,:);
% Refine camera pose only
currPose = bundleAdjustmentMotion(matchedMapPoints, matchedImagePoints, ...
currPose, intrinsics, 'PointsUndistorted', true, ...
'AbsoluteTolerance', 1e-7, 'RelativeTolerance', 1e-16,'MaxIteration', 20);
end
function [refKeyFrameId, localPointsIndices, localKeyFrameIds] = ...
updateRefKeyFrameAndLocalPoints(mapPoints, vSetKeyFrames, pointIndices)
% Get key frames K1 that observe map points in the current key frame
viewIds = findViewsOfWorldPoint(mapPoints, pointIndices);
K1IDs = vertcat(viewIds{:});
% The reference key frame has the most covisible map points
refKeyFrameId = mode(K1IDs);
% Retrieve key frames K2 that are connected to K1
K1IDs = unique(K1IDs);
localKeyFrameIds = K1IDs;
for i = 1:numel(K1IDs)
views = connectedViews(vSetKeyFrames, K1IDs(i));
K2IDs = setdiff(views.ViewId, localKeyFrameIds);
localKeyFrameIds = [localKeyFrameIds; K2IDs]; %#ok<AGROW>
end
pointIdx = findWorldPointsInView(mapPoints, localKeyFrameIds);
localPointsIndices = sort(vertcat(pointIdx{:}));
end
function features = getFeatures(directionAndDepth, views, mapPointIdx)
% Efficiently retrieve features and image points corresponding to map points
% denoted by mapPointIdx
allIndices = zeros(1, numel(mapPointIdx));
% ViewId and offset pair
count = []; % (ViewId, NumFeatures)
viewsFeatures = views.Features;
majorViewIds = directionAndDepth.MajorViewId;
majorFeatureindices = directionAndDepth.MajorFeatureIndex;
for i = 1:numel(mapPointIdx)
index3d = mapPointIdx(i);
viewId = double(majorViewIds(index3d));
if isempty(count)
count = [viewId, size(viewsFeatures{viewId},1)];
elseif ~any(count(:,1) == viewId)
count = [count; viewId, size(viewsFeatures{viewId},1)];
end
idx = find(count(:,1)==viewId);
if idx > 1
offset = sum(count(1:idx-1,2));
else
offset = 0;
end
allIndices(i) = majorFeatureindices(index3d) + offset;
end
uIds = count(:,1);
% Concatenating features and indexing once is faster than accessing via a for loop
allFeatures = vertcat(viewsFeatures{uIds});
features = allFeatures(allIndices, :);
end
function [inliers, predictedScales, viewAngles] = removeOutlierMapPoints(...
mapPoints, directionAndDepth, pose, intrinsics, localPointsIndices, scaleFactor, ...
numLevels, imageSize)
% 1) Points within the image bounds
xyzPoints = mapPoints.WorldPoints(localPointsIndices, :);
isInImage = helperFindProjectedPointsInImage(xyzPoints, pose, intrinsics, imageSize);
% 2) Parallax less than 60 degrees
cameraNormVector = [0 0 1] * pose.Rotation;
cameraToPoints = xyzPoints - pose.Translation;
viewDirection = directionAndDepth.ViewDirection(localPointsIndices, :);
validByView = sum(viewDirection.*cameraToPoints, 2) > ...
cosd(60)*(vecnorm(cameraToPoints, 2, 2));
% 3) Distance from map point to camera center is in the range of scale
% invariant depth
minDist = directionAndDepth.MinDistance(localPointsIndices);
maxDist = directionAndDepth.MaxDistance(localPointsIndices);
dist = vecnorm(xyzPoints - pose.Translation, 2, 2);
validByDistance = dist > minDist & dist < maxDist;
inliers = isInImage & validByView & validByDistance;
% Predicted scales
level= ceil(log(maxDist ./ dist)./log(scaleFactor));
level(level<0) = 0;
level(level>=numLevels-1) = numLevels-1;
predictedScales = scaleFactor.^level;
% View angles
viewAngles = acosd(sum(cameraNormVector.*cameraToPoints, 2) ./ ...
vecnorm(cameraToPoints, 2, 2));
predictedScales = predictedScales(inliers);
viewAngles = viewAngles(inliers);
end
src/main/java/data_acquisition/help_fncs/helperTriangulateTwoFrames.m deleted 100644 → 0
View file @ 34e4eb38
function [isValid, xyzPoints, inlierIdx] = helperTriangulateTwoFrames(...
pose1, pose2, matchedPoints1, matchedPoints2, intrinsics, minParallax)
[R1, t1] = cameraPoseToExtrinsics(pose1.Rotation, pose1.Translation);
camMatrix1 = cameraMatrix(intrinsics, R1, t1);
[R2, t2] = cameraPoseToExtrinsics(pose2.Rotation, pose2.Translation);
camMatrix2 = cameraMatrix(intrinsics, R2, t2);
[xyzPoints, reprojectionErrors, isInFront] = triangulate(matchedPoints1, ...
matchedPoints2, camMatrix1, camMatrix2);
% Filter points by view direction and reprojection error
minReprojError = 1;
inlierIdx = isInFront & reprojectionErrors < minReprojError;
xyzPoints = xyzPoints(inlierIdx ,:);
% A good two-view with significant parallax
ray1 = xyzPoints - pose1.Translation;
ray2 = xyzPoints - pose2.Translation;
cosAngle = sum(ray1 .* ray2, 2) ./ (vecnorm(ray1, 2, 2) .* vecnorm(ray2, 2, 2));
% Check parallax
isValid = all(cosAngle < cosd(minParallax) & cosAngle>0);
end
\ No newline at end of file
src/main/java/data_acquisition/help_fncs/helperUpdateGlobalMap.m deleted 100644 → 0
View file @ 34e4eb38
function [mapPointSet, directionAndDepth] = helperUpdateGlobalMap(...
mapPointSet, directionAndDepth, vSetKeyFrames, vSetKeyFramesOptim)
%helperUpdateGlobalMap update map points after pose graph optimization
posesOld = vSetKeyFrames.Views.AbsolutePose;
posesNew = vSetKeyFramesOptim.Views.AbsolutePose;
positionsOld = mapPointSet.WorldPoints;
positionsNew = positionsOld;
indices = 1:mapPointSet.Count;
% Update world location of each map point based on the new absolute pose of
% the corresponding major view
for i = 1: mapPointSet.Count
majorViewIds = directionAndDepth.MajorViewId(i);
tform = posesOld(majorViewIds).T \ posesNew(majorViewIds).T ;
positionsNew(i, :) = positionsOld(i, :) * tform(1:3,1:3) + tform(4, 1:3);
end
mapPointSet = updateWorldPoints(mapPointSet, indices, positionsNew);
end
\ No newline at end of file
src/main/java/data_acquisition/help_fncs/helperViewDirectionAndDepth.m deleted 100644 → 0
View file @ 34e4eb38
% helperViewDirectionAndDepth Object for storing view direction and depth
% Use this object to store map points attributes, such as view direction,
% predicted depth range, and the ID of the major view that contains the
% representative feature descriptor.
%
% This is an example helper class that is subject to change or removal
% in future releases.
%
% viewAndDepth = helperViewDirectionAndDepth(numPoints) returns a
% helperViewDirectionAndDepth object.
%
% helperViewDirectionAndDepth properties:
% ViewDirection - An M-by-3 matrix representing the view direction
% MaxDistance - An M-by-1 vector representing the maximum scale
% invariant distance
% MinDistance - An M-by-1 vector representing the minimum scale
% invariant distance
% MajorViewId - An M-by-1 vector containing the Id of the major view
% that contains the representative feature descriptor
% MajorFeatureIndex - An M-by-1 vector containing the index of the
% representative feature in the major view
%
% helperViewDirectionAndDepth methods:
%
% update - Add view direction and depth
% remove - Remove view direction and depth
% Copyright 2020 The MathWorks, Inc.
classdef helperViewDirectionAndDepth
properties
%ViewDirection An M-by-3 matrix representing the view direction of
% each map point
ViewDirection
%MaxDistance An M-by-1 vector representing the maximum scale
% invariant distance for each map point
MaxDistance
%MinDistance An M-by-1 vector representing the minimum scale
% invariant distance for each map point
MinDistance
%MajorViewId An M-by-1 vector containing the Id of the major view
% that contains the representative feature descriptor for each
% map point
MajorViewId
%MajorFeatureIndex An M-by-1 vector containing the index of the
% representative feature in the major view for each map point
MajorFeatureIndex
end
methods (Access = public)
function this = helperViewDirectionAndDepth(numPoints)
this.ViewDirection = zeros(numPoints, 3);
this.MaxDistance = zeros(numPoints, 1);
this.MinDistance = zeros(numPoints, 1);
this.MajorFeatureIndex = ones(numPoints, 1);
this.MajorViewId = ones(numPoints, 1, 'uint8');
end
function this = initialize(this, numPoints)
numOldPoints = numel(this.MaxDistance);
if numPoints > numOldPoints
idx = numOldPoints+1:numPoints;
this.ViewDirection(idx, :) = zeros(numel(idx),3);
this.MaxDistance(idx) = 0;
this.MinDistance(idx) = 0;
this.MajorFeatureIndex(idx) = 1;
this.MajorViewId(idx, :) = uint8(1);
end
end
function this = remove(this, idx)
this.ViewDirection(idx, :) = [];
this.MaxDistance(idx) = [];
this.MinDistance(idx) = [];
this.MajorFeatureIndex(idx) = [];
this.MajorViewId(idx, :) = [];
end
function this = update(this, mapPointSet, views, mapPointsIndices, updateMajorFeatureIndex)
this = initialize(this, mapPointSet.Count);
% Extract the columns for faster query
viewsLocations= vertcat(views.AbsolutePose.Translation);
viewsFeatures = views.Features;
viewsPoints = views.Points;
viewsScales = cellfun(@(x) x.Scale, viewsPoints, 'UniformOutput', false);
[keyFrameIds, allFeatureIdx] = findViewsOfWorldPoint(mapPointSet, mapPointsIndices);
% Update for each map point
for j = 1: numel(mapPointsIndices)
pointIdx = mapPointsIndices(j);
keyFrameIdsOfPoint = keyFrameIds{j};
featureIdxOfPoints = allFeatureIdx{j};
numCameras = numel(keyFrameIdsOfPoint);
% Update mean viewing direction
allFeatures = zeros(numCameras, 32, 'uint8');
allScales = zeros(numCameras, 1);
for k = 1:numCameras
tempId = keyFrameIdsOfPoint(k);
features = viewsFeatures{tempId};
scales = viewsScales{tempId};
featureIndex = featureIdxOfPoints(k);
allFeatures(k, :) = features(featureIndex, :);
allScales(k) = scales(featureIndex);
end
directionVec = mapPointSet.WorldPoints(pointIdx,:) - viewsLocations(keyFrameIdsOfPoint,:);
% Update view direction
meanViewVec = mean(directionVec ./ (vecnorm(directionVec, 2, 2)), 1);
this.ViewDirection(pointIdx, :) = meanViewVec/norm(meanViewVec);
if updateMajorFeatureIndex
% Identify the distinctive descriptor and the associated key frame
distIndex = computeDistinctiveDescriptors(allFeatures);
% Update the distinctive key frame index
this.MajorViewId(pointIdx) = keyFrameIdsOfPoint(distIndex);
this.MajorFeatureIndex(pointIdx) = featureIdxOfPoints(distIndex);
else
majorViewId = this.MajorViewId(pointIdx);
distIndex = find(keyFrameIdsOfPoint == majorViewId);
end
% Update depth range
distDirectionVec = directionVec(distIndex,:);
distKeyFrameId = keyFrameIdsOfPoint(distIndex);
maxDist = norm(distDirectionVec)* allScales(distIndex);
minDist = maxDist/max(viewsScales{distKeyFrameId});
this.MaxDistance(pointIdx, 1) = maxDist;
this.MinDistance(pointIdx, 1) = minDist;
end
end
end
end
%------------------------------------------------------------------
function index = computeDistinctiveDescriptors(features)
%computeDistinctiveDescriptors Find the distinctive discriptor
if size(features, 1) < 3
index = 2;
else
scores = helperHammingDistance(features, features);
[~, index] = min(sum(scores, 2));
end
end
\ No newline at end of file
src/main/java/data_acquisition/help_fncs/helperVisualizeMatchedFeatures.m deleted 100644 → 0
View file @ 34e4eb38
classdef helperVisualizeMatchedFeatures < handle
%helperVisualizeMatchedFeatures show the matched features in a frame
%
% This is an example helper class that is subject to change or removal
% in future releases.
% Copyright 2019-2020 The MathWorks, Inc.
properties (Access = private)
Image
Feature
end
methods (Access = public)
function obj = helperVisualizeMatchedFeatures(I, featurePoints)
locations= featurePoints.Location;
% Plot image
hFig = figure;
hAxes = newplot(hFig);
% Set figure visibility and position
hFig.Visible = 'on';
movegui(hFig, [300 220]);
% Show the image
obj.Image = imshow(I, 'Parent', hAxes, 'Border', 'tight');
title(hAxes, 'Matched Features in Current Frame');
hold(hAxes, 'on');
% Plot features
plot(featurePoints, hAxes, 'ShowOrientation',false, ...
'ShowScale',false);
obj.Feature = findobj(hAxes.Parent,'Type','Line');
end
function updatePlot(obj, I, featurePoints)
locations = featurePoints.Location;
obj.Image.CData = I;
obj.Feature.XData = locations(:,1);
obj.Feature.YData = locations(:,2);
drawnow limitrate
end
end
end
src/main/java/data_acquisition/help_fncs/helperVisualizeMotionAndStructure.m deleted 100644 → 0
View file @ 34e4eb38
classdef helperVisualizeMotionAndStructure < handle
%helperVisualizeMatchedFeatures show map points and camera trajectory
%
% This is an example helper class that is subject to change or removal
% in future releases.
% Copyright 2019-2020 The MathWorks, Inc.
properties
XLim = [-1.5 1.5]
YLim = [-1 0.5]
ZLim = [-0.5 2]
Axes
end
properties (Access = private)
MapPointsPlot
EstimatedTrajectory
OptimizedTrajectory
CameraPlot
end
methods (Access = public)
function obj = helperVisualizeMotionAndStructure(vSetKeyFrames, mapPoints)
[xyzPoints, currPose, trajectory] = retrievePlottedData(obj, vSetKeyFrames, mapPoints);
obj.MapPointsPlot = pcplayer(obj.XLim, obj.YLim, obj.ZLim, ...
'VerticalAxis', 'y', 'VerticalAxisDir', 'down');
obj.Axes = obj.MapPointsPlot.Axes;
obj.MapPointsPlot.view(xyzPoints);
obj.Axes.Children.DisplayName = 'Map points';
hold(obj.Axes, 'on');
% Set figure position on the screen
movegui(obj.Axes.Parent, [1000 200]);
% Plot camera trajectory
obj.EstimatedTrajectory = plot3(obj.Axes, trajectory(:,1), trajectory(:,2), ...
trajectory(:,3), 'r', 'LineWidth', 2 , 'DisplayName', 'Estimated trajectory');
% Plot the current cameras
obj.CameraPlot = plotCamera(currPose, 'Parent', obj.Axes, 'Size', 0.05);
end
function updatePlot(obj, vSetKeyFrames, mapPoints)
[xyzPoints, currPose, trajectory] = retrievePlottedData(obj, vSetKeyFrames, mapPoints);
% Update the point cloud
obj.MapPointsPlot.view(xyzPoints);
% Update the camera trajectory
set(obj.EstimatedTrajectory, 'XData', trajectory(:,1), 'YData', ...
trajectory(:,2), 'ZData', trajectory(:,3));
% Update the current camera pose since the first camera is fixed
obj.CameraPlot.AbsolutePose = currPose.AbsolutePose;
obj.CameraPlot.Label = num2str(currPose.ViewId);
drawnow limitrate
end
function plotOptimizedTrajectory(obj, poses)
% Delete the camera plot
delete(obj.CameraPlot);
% Plot the optimized trajectory
trans = vertcat(poses.AbsolutePose.Translation);
obj.OptimizedTrajectory = plot3(obj.Axes, trans(:, 1), trans(:, 2), trans(:, 3), 'm', ...
'LineWidth', 2, 'DisplayName', 'Optimized trajectory');
end
function plotActualTrajectory(obj, gTruth, optimizedPoses)
estimatedCams = vertcat(optimizedPoses.AbsolutePose.Translation);
actualCams = vertcat(gTruth.Translation);
scale = median(vecnorm(actualCams, 2, 2))/ median(vecnorm(estimatedCams, 2, 2));
% Update the plot based on the ground truth
updatePlotScale(obj, scale);
% Plot the ground truth
plot3(obj.Axes, actualCams(:,1), actualCams(:,2), actualCams(:,3), ...
'g','LineWidth',2, 'DisplayName', 'Actual trajectory');
drawnow limitrate
end
function showLegend(obj)
% Add a legend to the axes
hLegend = legend(obj.Axes, 'Location', 'northeast', ...
'TextColor', [1 1 1], 'FontWeight', 'bold');
end
end
methods (Access = private)
function [xyzPoints, currPose, trajectory] = retrievePlottedData(obj, vSetKeyFrames, mapPoints)
camPoses = poses(vSetKeyFrames);
currPose = camPoses(end,:); % Contains both ViewId and Pose
trajectory = vertcat(camPoses.AbsolutePose.Translation);
xyzPoints = mapPoints.WorldPoints;%(mapPoints.UserData.Validity,:);
% Only plot the points within the limit
inPlotRange = xyzPoints(:, 1) > obj.XLim(1) & ...
xyzPoints(:, 1) < obj.XLim(2) & xyzPoints(:, 2) > obj.YLim(1) & ...
xyzPoints(:, 2) < obj.YLim(2) & xyzPoints(:, 3) > obj.ZLim(1) & ...
xyzPoints(:, 3) < obj.ZLim(2);
xyzPoints = xyzPoints(inPlotRange, :);
end
function updatePlotScale(obj, scale)
% Update the map points and camera trajectory based on the
% ground truth scale
obj.Axes.XLim = obj.Axes.XLim * scale;
obj.Axes.YLim = obj.Axes.YLim * scale;
obj.Axes.ZLim = obj.Axes.ZLim * scale;
% Map points
obj.Axes.Children(end).XData = obj.Axes.Children(end).XData * scale;
obj.Axes.Children(end).YData = obj.Axes.Children(end).YData * scale;
obj.Axes.Children(end).ZData = obj.Axes.Children(end).ZData * scale;
% Estiamted and optimized Camera trajectory
obj.EstimatedTrajectory.XData = obj.EstimatedTrajectory.XData * scale;
obj.EstimatedTrajectory.YData = obj.EstimatedTrajectory.YData * scale;
obj.EstimatedTrajectory.ZData = obj.EstimatedTrajectory.ZData * scale;
obj.OptimizedTrajectory.XData = obj.OptimizedTrajectory.XData * scale;
obj.OptimizedTrajectory.YData = obj.OptimizedTrajectory.YData * scale;
obj.OptimizedTrajectory.ZData = obj.OptimizedTrajectory.ZData * scale;
end
end
end
src/main/java/data_acquisition/main.m deleted 100644 → 0
View file @ 34e4eb38
%% Considerations
%
% Environment: Tello, OpenCV face detection, and vSLAM are extremely sensitive
% to lighting in order to function properly. Tello doesn't counteract
% drafts and turbulence from flying in small, hard-surfaced spaces -->
% blown off course often. Theoretically, the best space for this
% problem is a spacious, well-lit, draftless, indoor area... make do with
% what you got.
%
% Connectivity: Tello programming depends on a ground station's connection
% to Tello's local wifi, which isn't great even for what it is. Often
% times, clearing the environment
%
% Battery: Tello only has 12 minutes of continous flight time which feels
% significantly smaller with lots of starts and stops. Make sure it's
% fully charged to start with and I suggest no flying at <20%. Low battery
% can cause a whole host of problems and is probably tied to overheating as
% well so beware.
%
% Reset: Tello's destructor includes auto-landing. No need to manually land
% the drone if an error is raised and it's left hovering. Run the cleanup
% section and it will force it to land and clean the environment.
%
% Common errors often solvable on reset:
% 1. "Unable to receive response from the drone..." because the wifi is
% spotty and will often bug out in Matlab even though it technically still
% has connection to the drone and can subsequently send move commands.
% 2. "Unable to execute 'move'. Execute 'takeoff' command first." because,
% in my experience, there is a lot of overhead in passing the drone object
% to function to control the drone within the function instead of the
% top-level code, which may not be the case for the others. I attempted to
% handle these errors where they occur.
%
% https://www.mathworks.com/matlabcentral/fileexchange/74434-matlab-support-package-for-ryze-tello-drones?s_tid=FX_rc2_behav
% The package is new and not widely used so the forum is small. Many of the
% above issues may be caused by poor connection, overheating, low battery,
% and a flurry of other picky statuses. Just clear the environment and
% rerun from the start, no need to disconnect from its wifi unless it needs
% to be turned off to cool down.
%
% Face detection %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The source is in follow.m but main executes the move commands. It works
% pretty well as is but if it's misclassifying too many objects as faces
% then tweak the parameters of the detector (e.g. increase the minimum box
% size, increase the merge threshold, change the model, etc.). To make the
% drone adjust more "responsively" tweak the thresholds for the vector
% components in follow.m.
%
% vSLAM %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The source is vslam.m and is not handled by main. Hence, the drone is
% moved from within the function, which adds some overhead. I added error
% handling to a reasonable extent for these problems, but they may still
% reoccur. This process is slower than face tracking and much buggier
% because there are a lot of moving parts. Since it takes longer, there is
% a higher probability that the drone will lose connection to the ground
% station along the way. Small values for the number of cycles and the
% lenght of the move sequence are recommended until the toolkit / hardware
% has more reliability.
%
% Important: Matlab will crash if you disconnect from the drone's wifi
% while there is still a drone object in the environment. Make sure you
% clear the environment soon after resetting the run.
%
% Matlab's toolkit doesn't provide support for principal axes control.
% Instead, movement is split between x, y, z (see move() documentation for
% axes directions) and then turn in lieu of yaw. Although not the end of
% the world, it is nonetheless a limitation.
%% initial cleanup
close all
clear
%% connect, setup, and takeoff
drone = ryze();
cam = camera(drone);
%fprintf("Battery: %d\n",drone.Battery)
takeoff(drone)
pause(2) % after takeoff, the drone needs a grace period for connection
moveup(drone,1.75,'WaitUntilDone',false)
%% face detection
% setup face detector s.t. false positives are minimized
faceDetector = vision.CascadeObjectDetector;
faceDetector.ClassificationModel = 'FrontalFaceLBP';
faceDetector.MinSize = [60 60];
faceDetector.MergeThreshold = 10;
stop = 60;
count = 0;
tic
while toc < stop
[dX,dY,dZ,angle,faces] = follow(cam,faceDetector,2);
imshow(faces)
try
imwrite(faces,sprintf('./imgs/faces/faces%d.png',count))
count = count + 1;
catch
imshow(faces)
end
% try the command
% if not received, continue and try again
try
if dX || dY || dZ
% move
move(drone,[dX,dY,dZ],'WaitUntilDone',false)
fprintf("Moving with relative coords:\n")
fprintf("dX = %d\n",dX)
fprintf("dY = %d\n",dY)
fprintf("dZ = %d\n",dZ)
% turn to face
turn(drone,angle)
fprintf("Turning by %d radians\n",angle)
end
catch
fprintf("Command not received. Continuing.\n")
end
end
land(drone)
%% vSLAM
% see toolkit documentation for move() axes
% values are in meters
moveseq = {[0, -0.4, 0], 0;
% [0.5, 0, 0], 0;
[0, 0.5, 0], 0};
% [-0.5, 0, 0], 0};
% vslam
% drone: drone object
% cam: drone's camera
% moveseq: cell array with rows of a vector [x,y,z] and an angle in radians
% cycles: total number of cycles for movement
% minMatches: minimum number of ORB feature matches for triangulation
vslam(drone, cam, moveseq, 5, 10)
land(drone)
%% final cleanup
close all
clear
src/main/java/data_acquisition/vslam.m deleted 100644 → 0
View file @ 34e4eb38
%% Visual SLAM
% Modified monocular visual ORB SLAM from the Matlab example
% https://www.mathworks.com/help/vision/ug/monocular-visual-simultaneous-localization-and-mapping.html
%
% Main modifications:
%
% Image acquisiton and handling: uses the drone's camera to acquire new
% images and doesn't store them in an imageDatastore
%
% Camera intrinsics: changed the camera intrinsics to those of Tello's
% camera (including distortion)
%
% Figures: grabbed handles to the feature image and particle maps at every
% iteration to save.
%
% Main loop: loop for a given number of cycles through the predetermined
% move sequence --> no loop closure based on a predetermined dataset
%
% Error handling: in case of low lighting and too few ORB features are
% detected, catch in map initialization and main loops and continue; in
% case of poor connection to the drone, catch error for move sequence and
% try 10 times before throwing the error.
%% vslam
% drone: drone object
% cam: drone's camera
% moveseq: cell array with rows containing a vector and an angle
% cycles: total number cycles for movement
% minMatches: minimum number of ORB feature matches for triangulation
function vslam(drone, cam, moveseq, cycles, minMatches)
% Inspect the first image
currFrameIdx = 1;
currI = snapshot(cam);
himage = imshow(currI);
% Set random seed for reproducibility
rng('default');
% Create a cameraIntrinsics object to store the camera intrinsic parameters.
% The intrinsics for the dataset can be found at the following page:
% https://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats
% Note that the images in the dataset are already undistorted, hence there
% is no need to specify the distortion coefficients.
% Tello camera calibration parameters from: https://tellopilots.com/threads/camera-intrinsic-parameter.2620/
focalLength = [921.170702, 919.018377]; % in units of pixels
principalPoint = [459.904354, 351.238301]; % in units of pixels
imageSize = size(currI,[1 2]); % in units of pixels
rad_distort = [-0.033458, 0.105152];
tan_distort = [0.001256, -0.006647];
intrinsics = cameraIntrinsics(focalLength, principalPoint, imageSize,'RadialDistortion',rad_distort,'TangentialDistortion',tan_distort);
% Detect and extract ORB features
scaleFactor = 1.2;
numLevels = 8;
[preFeatures, prePoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels);
currFrameIdx = currFrameIdx + 1;
firstI = currI; % Preserve the first frame
isMapInitialized = false;
% Map initialization loop
while ~isMapInitialized && currFrameIdx < 10
currI = snapshot(cam);
% jiggle the drone around a bit to initialize map
if mod(currFrameIdx,2) == 0
movedown(drone,0.5)
else
moveup(drone,0.5)
end
% error handle if the subcall to detectORBFeatures fails
try
[currFeatures, currPoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels);
catch
currFrameIdx = currFrameIdx + 1;
continue
end
currFrameIdx = currFrameIdx + 1;
% Find putative feature matches
indexPairs = matchFeatures(preFeatures, currFeatures, 'Unique', true, ...
'MaxRatio', 0.9, 'MatchThreshold', 40);
preMatchedPoints = prePoints(indexPairs(:,1),:);
currMatchedPoints = currPoints(indexPairs(:,2),:);
% If not enough matches are found, check the next frame
minMatches = 100;
if size(indexPairs, 1) < minMatches
continue
end
preMatchedPoints = prePoints(indexPairs(:,1),:);
currMatchedPoints = currPoints(indexPairs(:,2),:);
% Compute homography and evaluate reconstruction
[tformH, scoreH, inliersIdxH] = helperComputeHomography(preMatchedPoints, currMatchedPoints);
% Compute fundamental matrix and evaluate reconstruction
[tformF, scoreF, inliersIdxF] = helperComputeFundamentalMatrix(preMatchedPoints, currMatchedPoints);
% Select the model based on a heuristic
ratio = scoreH/(scoreH + scoreF);
ratioThreshold = 0.45;
if ratio > ratioThreshold
inlierTformIdx = inliersIdxH;
tform = tformH;
else
inlierTformIdx = inliersIdxF;
tform = tformF;
end
% Computes the camera location up to scale. Use half of the
% points to reduce computation
inlierPrePoints = preMatchedPoints(inlierTformIdx);
inlierCurrPoints = currMatchedPoints(inlierTformIdx);
[relOrient, relLoc, validFraction] = relativeCameraPose(tform, intrinsics, ...
inlierPrePoints(1:2:end), inlierCurrPoints(1:2:end));
% If not enough inliers are found, move to the next frame
if validFraction < 0.9 || numel(size(relOrient))==3
continue
end
% Triangulate two views to obtain 3-D map points
relPose = rigid3d(relOrient, relLoc);
minParallax = 3; % In degrees
[isValid, xyzWorldPoints, inlierTriangulationIdx] = helperTriangulateTwoFrames(...
rigid3d, relPose, inlierPrePoints, inlierCurrPoints, intrinsics, minParallax);
if ~isValid
continue
end
% Get the original index of features in the two key frames
indexPairs = indexPairs(inlierTformIdx(inlierTriangulationIdx),:);
isMapInitialized = true;
disp(['Map initialized with frame 1 and frame ', num2str(currFrameIdx-1)])
end % End of map initialization loop
if isMapInitialized
close(himage.Parent.Parent); % Close the previous figure
% Show matched features
hfeature = showMatchedFeatures(firstI, currI, prePoints(indexPairs(:,1)), ...
currPoints(indexPairs(:, 2)), 'Montage');
saveas(gcf,'./imgs/slam/matches.png')
else
error('Unable to initialize map.')
end
% Create an empty imageviewset object to store key frames
vSetKeyFrames = imageviewset;
% Create an empty worldpointset object to store 3-D map points
mapPointSet = worldpointset;
% Create a helperViewDirectionAndDepth object to store view direction and depth
directionAndDepth = helperViewDirectionAndDepth(size(xyzWorldPoints, 1));
% Add the first key frame. Place the camera associated with the first
% key frame at the origin, oriented along the Z-axis
preViewId = 1;
vSetKeyFrames = addView(vSetKeyFrames, preViewId, rigid3d, 'Points', prePoints,...
'Features', preFeatures.Features);
% Add the second key frame
currViewId = 2;
vSetKeyFrames = addView(vSetKeyFrames, currViewId, relPose, 'Points', currPoints,...
'Features', currFeatures.Features);
% Add connection between the first and the second key frame
vSetKeyFrames = addConnection(vSetKeyFrames, preViewId, currViewId, relPose, 'Matches', indexPairs);
% Add 3-D map points
[mapPointSet, newPointIdx] = addWorldPoints(mapPointSet, xyzWorldPoints);
% Add observations of the map points
preLocations = prePoints.Location;
currLocations = currPoints.Location;
preScales = prePoints.Scale;
currScales = currPoints.Scale;
% Add image points corresponding to the map points in the first key frame
mapPointSet = addCorrespondences(mapPointSet, preViewId, newPointIdx, indexPairs(:,1));
% Add image points corresponding to the map points in the second key frame
mapPointSet = addCorrespondences(mapPointSet, currViewId, newPointIdx, indexPairs(:,2));
% Run full bundle adjustment on the first two key frames
tracks = findTracks(vSetKeyFrames);
cameraPoses = poses(vSetKeyFrames);
[refinedPoints, refinedAbsPoses] = bundleAdjustment(xyzWorldPoints, tracks, ...
cameraPoses, intrinsics, 'FixedViewIDs', 1, ...
'PointsUndistorted', true, 'AbsoluteTolerance', 1e-7,...
'RelativeTolerance', 1e-15, 'MaxIteration', 50);
% Scale the map and the camera pose using the median depth of map points
medianDepth = median(vecnorm(refinedPoints.'));
refinedPoints = refinedPoints / medianDepth;
refinedAbsPoses.AbsolutePose(currViewId).Translation = ...
refinedAbsPoses.AbsolutePose(currViewId).Translation / medianDepth;
relPose.Translation = relPose.Translation/medianDepth;
% Update key frames with the refined poses
vSetKeyFrames = updateView(vSetKeyFrames, refinedAbsPoses);
vSetKeyFrames = updateConnection(vSetKeyFrames, preViewId, currViewId, relPose);
% Update map points with the refined positions
mapPointSet = updateWorldPoints(mapPointSet, newPointIdx, refinedPoints);
% Update view direction and depth
directionAndDepth = update(directionAndDepth, mapPointSet, vSetKeyFrames.Views, newPointIdx, true);
% Visualize matched features in the current frame
close(hfeature.Parent.Parent);
featurePlot = helperVisualizeMatchedFeatures(currI, currPoints(indexPairs(:,2)));
% Visualize initial map points and camera trajectory
mapPlot = helperVisualizeMotionAndStructure(vSetKeyFrames, mapPointSet);
% Show legend
showLegend(mapPlot);
% ViewId of the current key frame
currKeyFrameId = currViewId;
% ViewId of the last key frame
lastKeyFrameId = currViewId;
% ViewId of the reference key frame that has the most co-visible
% map points with the current key frame
refKeyFrameId = currViewId;
% Index of the last key frame in the input image sequence
lastKeyFrameIdx = currFrameIdx - 1;
% Indices of all the key frames in the input image sequence
addedFramesIdx = [1; lastKeyFrameIdx];
hs = findall(groot,'Type','Figure');
fplot = hs(1);
mplot = hs(2);
saveas(fplot,'./imgs/slam/fplot0.png')
saveas(mplot,'./imgs/slam/mplot0.png')
% Main loop
move_idx = 0;
total_moves = cycles*length(moveseq);
break_iter = 0;
while move_idx < total_moves
try
% cycle through movement vector
[vec, angle] = moveseq{mod(move_idx,length(moveseq)) + 1,:};
move(drone,vec) % may throw error
turn(drone,angle) % may throw error
% guaranteed no error
fprintf('Movement %d of %d\n',move_idx,total_moves)
fprintf("dX = %d\n",vec(1))
fprintf("dY = %d\n",vec(2))
fprintf("dZ = %d\n",vec(3))
move_idx = move_idx + 1;
break_iter = 0;
catch
if break_iter < 10
break_iter = break_iter + 1;
continue
else
error('Movement command not received by Tello on iteration %d.\n',move_idx)
end
end
currI = snapshot(cam);
try
[currFeatures, currPoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels);
% Track the last key frame
% mapPointsIdx: Indices of the map points observed in the current frame
% featureIdx: Indices of the corresponding feature points in the
% current frame
[currPose, mapPointsIdx, featureIdx] = helperTrackLastKeyFrame(mapPointSet, ...
vSetKeyFrames.Views, currFeatures, currPoints, lastKeyFrameId, intrinsics, scaleFactor);
% Track the local map
% refKeyFrameId: ViewId of the reference key frame that has the most
% co-visible map points with the current frame
% localKeyFrameIds: ViewId of the connected key frames of the current frame
[refKeyFrameId, localKeyFrameIds, currPose, mapPointsIdx, featureIdx] = ...
helperTrackLocalMap(mapPointSet, directionAndDepth, vSetKeyFrames, mapPointsIdx, ...
featureIdx, currPose, currFeatures, currPoints, intrinsics, scaleFactor, numLevels);
% Check if the current frame is a key frame.
% A frame is a key frame if both of the following conditions are satisfied:
%
% 1. At least 20 frames have passed since the last key frame or the
% current frame tracks fewer than 80 map points
% 2. The map points tracked by the current frame are fewer than 90% of
% points tracked by the reference key frame
isKeyFrame = helperIsKeyFrame(mapPointSet, refKeyFrameId, lastKeyFrameIdx, ...
currFrameIdx, mapPointsIdx);
% Visualize matched features
updatePlot(featurePlot, currI, currPoints(featureIdx));
saveas(fplot,sprintf('./imgs/slam/fplot_%d.png',move_idx))
if ~isKeyFrame
currFrameIdx = currFrameIdx + 1;
continue
end
% Update current key frame ID
currKeyFrameId = currKeyFrameId + 1;
% Add the new key frame
[mapPointSet, vSetKeyFrames] = helperAddNewKeyFrame(mapPointSet, vSetKeyFrames, ...
currPose, currFeatures, currPoints, mapPointsIdx, featureIdx, localKeyFrameIds);
% Remove outlier map points that are observed in fewer than 3 key frames
[mapPointSet, directionAndDepth, mapPointsIdx] = helperCullRecentMapPoints(mapPointSet, directionAndDepth, mapPointsIdx, newPointIdx);
% Create new map points by triangulation
minNumMatches = minMatches;
minParallax = 3;
[mapPointSet, vSetKeyFrames, newPointIdx] = helperCreateNewMapPoints(mapPointSet, vSetKeyFrames, ...
currKeyFrameId, intrinsics, scaleFactor, minNumMatches, minParallax);
% Update view direction and depth
directionAndDepth = update(directionAndDepth, mapPointSet, vSetKeyFrames.Views, [mapPointsIdx; newPointIdx], true);
% Local bundle adjustment
[mapPointSet, directionAndDepth, vSetKeyFrames, newPointIdx] = helperLocalBundleAdjustment(mapPointSet, directionAndDepth, vSetKeyFrames, ...
currKeyFrameId, intrinsics, newPointIdx);
% Visualize 3D world points and camera trajectory
updatePlot(mapPlot, vSetKeyFrames, mapPointSet);
saveas(mplot,sprintf('./imgs/slam/mplot_%d.png',move_idx))
catch
currFrameIdx = currFrameIdx + 1;
continue
end
% Update IDs and indices
lastKeyFrameId = currKeyFrameId;
lastKeyFrameIdx = currFrameIdx;
addedFramesIdx = [addedFramesIdx; currFrameIdx]; %#ok<AGROW>
currFrameIdx = currFrameIdx + 1;
end % End of main loop
%% Only works if point graph is well-formed, continous --> probably won't happen for Tello but keeping it here just in case
% % Optimize the poses
% vSetKeyFramesOptim = optimizePoses(vSetKeyFrames, minNumMatches, 'Tolerance', 1e-16, 'Verbose', true);
%
% % Update map points after optimizing the poses
% mapPointSet = helperUpdateGlobalMap(mapPointSet, directionAndDepth, ...
% vSetKeyFrames, vSetKeyFramesOptim);
%
% updatePlot(mapPlot, vSetKeyFrames, mapPointSet);
%
% % Plot the optimized camera trajectory
% optimizedPoses = poses(vSetKeyFramesOptim);
% plotOptimizedTrajectory(mapPlot, optimizedPoses)
%
% % Update legend
% showLegend(mapPlot);
% saveas(mplot,'./imgs/final_mplot.png')
end
\ No newline at end of file
src/main/java/database/DatabaseUpdater.java deleted 100644 → 0
View file @ 34e4eb38
package database;
import object_detection.types.ObjectSet;
public class DatabaseUpdater {
public static boolean updateDB(ObjectSet objSet){
return true;
}
}
src/main/java/database/MongoDBAtlasConnection.java deleted 100644 → 0
View file @ 34e4eb38
package database;
import com.mongodb.ConnectionString;
import com.mongodb.MongoClientSettings;
import com.mongodb.MongoException;
import com.mongodb.ServerApi;
import com.mongodb.ServerApiVersion;
import com.mongodb.client.MongoClient;
import com.mongodb.client.MongoClients;
import com.mongodb.client.MongoDatabase;
import org.bson.Document;
public class MongoDBAtlasConnection {
public static void main(String[] args) {
String connectionString = "mongodb+srv://zanem:<YXQiSFkSVqxPTs3M>@cluster0.axhv9kg.mongodb.net/?retryWrites=true&w=majority&appName=Cluster0";
ServerApi serverApi = ServerApi.builder()
.version(ServerApiVersion.V1)
.build();
MongoClientSettings settings = MongoClientSettings.builder()
.applyConnectionString(new ConnectionString(connectionString))
.serverApi(serverApi)
.build();
try (MongoClient mongoClient = MongoClients.create(settings)) {
try {
MongoDatabase database = mongoClient.getDatabase("admin");
database.runCommand(new Document("ping", 1));
System.out.println("Pinged your deployment. You successfully connected to MongoDB!");
} catch (MongoException e) {
e.printStackTrace();
}
}
}
}
//YXQiSFkSVqxPTs3M
src/main/java/database/MongoDBInteraction.java 0 → 100644
View file @ 8d04e48b
package database;
import com.mongodb.ConnectionString;
import com.mongodb.MongoClientSettings;
import com.mongodb.ServerApi;
import com.mongodb.ServerApiVersion;
import com.mongodb.client.*;
import com.mongodb.client.model.Filters;
import com.mongodb.client.model.ReplaceOptions;
import object_detection.types.ObjectSet;
import object_detection.types.Point;
import object_detection.types.PointSet;
import org.bson.Document;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
public class MongoDBInteraction {
private MongoClient mongoClient;
private MongoDatabase database;
private MongoCollection<Document> objectCollection;
public MongoDBInteraction() {
String uri = "mongodb+srv://Cluster03790:dlVzT2Z2bEh9@cluster03790.tk4cwyy.mongodb.net/myFirstDatabase?retryWrites=true";
// Construct a ServerApi instance using the ServerApi.builder() method
MongoClientSettings settings = MongoClientSettings.builder()
.applyConnectionString(new ConnectionString(uri))
.serverApi(ServerApi.builder().version(ServerApiVersion.V1).build())
.build();
this.mongoClient = MongoClients.create(settings);
this.database = mongoClient.getDatabase("Objects");
this.objectCollection = database.getCollection("objectSets");
}
/**
* calls convertDocumentToObjectSet
* @return
*/
public ObjectSet retrieveLatestObjectSet() {
try {
Document doc = objectCollection.find().sort(new Document("index", -1)).first();
if (doc == null) {
return null;
} else {
return convertDocumentToObjectSet(doc);
}
} catch (Exception e) {
System.out.println("Error retrieving document: " + e.getMessage());
return null;
}
}
private ObjectSet convertDocumentToObjectSet(Document doc) {
if (doc == null) {
return null;
}
List<Document> pointSetDocs = doc.getList("objectSets", Document.class);
if (pointSetDocs == null || pointSetDocs.isEmpty()) {
return new ObjectSet();
}
ObjectSet objectSet = new ObjectSet();
for (Document pointSetDoc : pointSetDocs) {
PointSet pointSet = convertDocumentToPointSet(pointSetDoc);
if (pointSet != null) {
objectSet.objects.add(pointSet);
} else {
System.out.println("Failed to convert point set document");
}
}
return objectSet;
}
private PointSet convertDocumentToPointSet(Document doc) {
if (doc == null) {
return null;
}
// Using Integer.parseInt to safely convert String to Integer if necessary
int idx;
String pred;
try {
idx = doc.get("setId") instanceof Integer ? (Integer) doc.get("setId") : Integer.parseInt((String) doc.get("setId"));
pred = (String) doc.get("predName");
} catch (NumberFormatException e) {
System.err.println("Invalid format for setId, must be an integer: " + doc.get("setId"));
return null;
}
List<Document> pointsDocs = doc.getList("points", Document.class);
if (pointsDocs == null) {
return new PointSet(idx, pred);
}
PointSet pointSet = new PointSet(idx, pred);
for (Document pointDoc : pointsDocs) {
Point point = new Point(
pointDoc.getDouble("x").floatValue(),
pointDoc.getDouble("y").floatValue(),
pointDoc.getDouble("z").floatValue(),
pointDoc.getInteger("R"),
pointDoc.getInteger("G"),
pointDoc.getInteger("B")
);
pointSet.addPoint(point);
}
return pointSet;
}
private Document pointSetToDocument(String setId, PointSet pointSet) {
List<Document> pointsList = new ArrayList<>();
for (Point p : pointSet.getPoints()) {
pointsList.add(new Document("x", p.getX())
.append("y", p.getY())
.append("z", p.getZ())
.append("R", p.R)
.append("G", p.G)
.append("B", p.B));
}
return new Document("setId", setId)
.append("predName", pointSet.getPred())
.append("points", pointsList);
}
private Document objectSetToDocument(int index, ObjectSet objectSet) {
List<Document> objectList = new ArrayList<>();
for (PointSet ps : objectSet.objects) {
Document pointSetDoc = pointSetToDocument(Integer.toString(ps.getIDX()), ps);
objectList.add(pointSetDoc);
}
return new Document("index", index)
.append("objectSets", objectList);
}
public void updateObjectSet(int index, ObjectSet objectSet) {
Document doc = objectSetToDocument(index, objectSet);
ReplaceOptions options = new ReplaceOptions().upsert(true);
objectCollection.replaceOne(Filters.eq("index", index), doc, options);
}
}
src/main/java/gui_server/app.js deleted 100644 → 0
View file @ 34e4eb38
function createTableFromData(data) {
let table = '<table>';
// Table header row
table += '<tr><th></th>';
for (let key in data) {
table += '<th>' + key + '</th>';
}
table += '</tr>';
// Table body
let maxRows = getMaxRows(data);
for (let i = 0; i < maxRows; i++) {
table += '<tr>';
table += '<td>' + i + '</td>';
for (let key in data) {
let row = data[key][i] || [];
table += '<td>' + row.join(', ') + '</td>';
}
table += '</tr>';
}
table += '</table>';
return table;
}
// Function to get the maximum number of rows in the data
function getMaxRows(data) {
let max = 0;
for (let key in data) {
let numRows = Object.keys(data[key]).length;
if (numRows > max) {
max = numRows;
}
}
return max;
}
function displayJSON(){
$.getJSON("http://127.0.0.1:5000/getJSON", function (data) {
console.log(data);
// Create the table
let tableHtml = createTableFromData(data);
// Display the table
document.getElementById('container').innerHTML = tableHtml;
});
}
// listen for mouse events
$("#canvas").mousedown(function (e) {
handleMouseDown(e);
});
$("#canvas").mousemove(function (e) {
handleMouseMove(e);
});
$("#canvas").mouseup(function (e) {
handleMouseUp(e);
});
$("#canvas").mouseout(function (e) {
handleMouseOut(e);
});
src/main/java/gui_server/detect.js deleted 100644 → 0
View file @ 34e4eb38
function detect() {
for(let i=0; i < items.length; i++){
var bbox = items[i]["bbox"];
drawBox(bbox[0], bbox[1], bbox[2], bbox[3]);
}
}
const img = document.getElementById('img');
var items;
// Load the model.
cocoSsd.load().then(model => {
// detect objects in the image.
model.detect(img).then(predictions => {
items = predictions;
console.log('Predictions: ', predictions);
});
});
\ No newline at end of file
src/main/java/gui_server/draw.js deleted 100644 → 0
View file @ 34e4eb38
var canvas = document.getElementById("canvas");
var overlay = document.getElementById("overlay");
var ctx = canvas.getContext("2d");
var ctxo = overlay.getContext("2d");
var $canvas = $("#canvas");
var canvasOffset = $canvas.offset();
var offsetX = canvasOffset.left;
var offsetY = canvasOffset.top;
var scrollX = $canvas.scrollLeft();
var scrollY = $canvas.scrollTop();
var isDown = false;
var startX;
var startY;
var prevStartX = 0;
var prevStartY = 0;
var prevWidth = 0;
var prevHeight = 0;
ctx.canvas.width = img.width;
ctxo.canvas.width= img.width;
ctx.canvas.height = img.height;
ctxo.canvas.height = img.height;
ctxo.drawImage(img, 0, 0);
// style the context
ctx.strokeStyle = "red";
ctx.lineWidth = 3;
ctxo.strokeStyle = "red";
ctxo.lineWidth = 3;
// Draw a box from existing x:y coordinates
function drawBox(x, y, w, h) {
if(w / img.width < 0.2){
ctxo.strokeRect(x, y, w, h);
}
}
function handleMouseDown(e) {
e.preventDefault();
e.stopPropagation();
// save the starting x/y of the rectangle
startX = parseInt(e.clientX - offsetX);
startY = parseInt(e.clientY - offsetY);
// set a flag indicating the drag has begun
isDown = true;
}
function handleMouseUp(e) {
e.preventDefault();
e.stopPropagation();
// the drag is over, clear the dragging flag
isDown = false;
ctxo.strokeRect(prevStartX, prevStartY, prevWidth, prevHeight);
}
function handleMouseOut(e) {
e.preventDefault();
e.stopPropagation();
// the drag is over, clear the dragging flag
isDown = false;
}
function handleMouseMove(e) {
e.preventDefault();
e.stopPropagation();
// if we're not dragging, just return
if (!isDown) {
return;
}
// get the current mouse position
mouseX = parseInt(e.clientX - offsetX);
mouseY = parseInt(e.clientY - offsetY);
var width = mouseX - startX;
var height = mouseY - startY;
// clear the canvas
ctx.clearRect(0, 0, canvas.width, canvas.height);
// draw a new rect from the start position
// to the current mouse position
ctx.strokeRect(startX, startY, width, height);
prevStartX = startX;
prevStartY = startY;
prevWidth = width;
prevHeight = height;
}
// remove current boxes
function clearCanvas(){
ctx.clearRect(0, 0, canvas.width, canvas.height);
ctxo.clearRect(0, 0, canvas.width, canvas.height);
ctxo.drawImage(img, 0, 0);
}