SLAM/Mapping/RGBDMapAnchors.mo
Download original .moGitHub source
Documentation: SLAM.Mapping.RGBDMapAnchors · SLAM.Mapping.RGBDMapAnchors.ProperRotation · SLAM.Mapping.RGBDMapAnchors.ValidNodes · SLAM.Mapping.RGBDMapAnchors.Reproject
within SLAM.Mapping;
// Landmark coordinates stay local to stable keyframe identities. A graph
// correction is a proposal until every persistent owner commits it together.
package RGBDMapAnchors
constant Integer imageHeight = 90;
constant Integer imageWidth = 160;
constant Integer mapCapacity = imageHeight*imageWidth;
constant Integer keyframeCapacity = 128;
constant Integer dimension = 3;
constant Integer identifierLimit = 1000000000;
function ProperRotation
input Real rotation[dimension,dimension];
output Boolean valid;
protected
Real gram[dimension,dimension]; Real determinant;
algorithm
valid := true;
for row in 1:dimension loop
for column in 1:dimension loop
valid := valid and abs(rotation[row,column]) <= 1.000001;
end for;
end for;
if valid then
gram := transpose(rotation)*rotation;
for row in 1:dimension loop
for column in 1:dimension loop
valid := valid and abs(gram[row,column]-(if row == column then 1.0 else 0.0)) <= 1e-6;
end for;
end for;
determinant := rotation[1,1]*(rotation[2,2]*rotation[3,3]-rotation[2,3]*rotation[3,2])
-rotation[1,2]*(rotation[2,1]*rotation[3,3]-rotation[2,3]*rotation[3,1])
+rotation[1,3]*(rotation[2,1]*rotation[3,2]-rotation[2,2]*rotation[3,1]);
valid := valid and abs(determinant-1.0) <= 1e-6;
end if;
end ProperRotation;
// Catalog/graph slots never relocate within a generation. Reuse changes
// the monotone identity; a cached slot alone cannot keep an anchor alive.
function ValidNodes
input Boolean enabled[:];
input Integer ids[size(enabled,1)];
input Real position[size(enabled,1),dimension];
input Real rotation[size(enabled,1),dimension,dimension];
input Real coordinateLimit;
output Boolean valid;
algorithm
valid := true;
for node in 1:size(enabled,1) loop
if enabled[node] then
valid := valid and ids[node] >= 1 and ids[node] <= identifierLimit
and ProperRotation(rotation[node,:,:]);
for axis in 1:dimension loop
valid := valid and abs(position[node,axis]) <= coordinateLimit;
end for;
for other in 1:node-1 loop
if enabled[other] then valid := valid and ids[node] <> ids[other]; end if;
end for;
end if;
end for;
end ValidNodes;
function Reproject
input Real previousPoint[:,dimension];
input Real previousLocalPoint[size(previousPoint,1),dimension];
input Real previousOccupied[size(previousPoint,1)];
input Integer previousAnchorId[size(previousPoint,1)];
input Integer previousAnchorSlot[size(previousPoint,1)];
input Integer mapGeneration; input Integer previousRevision;
input Boolean nodeEnabled[:];
input Integer nodeId[size(nodeEnabled,1)];
input Real nodePosition[size(nodeEnabled,1),dimension];
input Real nodeRotation[size(nodeEnabled,1),dimension,dimension];
input Integer graphGeneration; input Integer graphRevision;
input Boolean graphAccepted; input Boolean requested;
input Real coordinateLimit;
output Real point[size(previousPoint,1),dimension];
output Real localPoint[size(previousPoint,1),dimension];
output Real occupied[size(previousPoint,1)];
output Integer anchorId[size(previousPoint,1)];
output Integer anchorSlot[size(previousPoint,1)];
output Integer nextRevision;
output Boolean accepted;
output Integer rejectionReason;
output Integer projectedCount; output Integer prunedCount;
protected
Boolean configuration; Boolean stateValid; Boolean slotValid;
Boolean coordinatesValid; Boolean retained;
Integer owner; Real candidate[dimension];
algorithm
point := previousPoint; localPoint := previousLocalPoint;
occupied := previousOccupied; anchorId := previousAnchorId; anchorSlot := previousAnchorSlot;
nextRevision := previousRevision; accepted := false; rejectionReason := 1;
projectedCount := 0; prunedCount := 0;
if requested then
configuration := size(previousPoint,1) >= 1 and size(previousPoint,1) <= mapCapacity
and size(nodeEnabled,1) >= 1 and size(nodeEnabled,1) <= keyframeCapacity
and coordinateLimit > 0.0 and coordinateLimit <= 1e6
and mapGeneration >= 1 and mapGeneration <= identifierLimit and graphGeneration == mapGeneration
and previousRevision >= 0 and previousRevision < identifierLimit and graphRevision == previousRevision+1;
rejectionReason := 2;
if configuration then
rejectionReason := 3;
if graphAccepted then
rejectionReason := 4;
if ValidNodes(nodeEnabled,nodeId,nodePosition,nodeRotation,coordinateLimit) then
stateValid := true; coordinatesValid := true;
for slot in 1:size(previousPoint,1) loop
if previousOccupied[slot] == 0.0 then
point[slot,:] := zeros(dimension); localPoint[slot,:] := zeros(dimension);
occupied[slot] := 0.0; anchorId[slot] := 0; anchorSlot[slot] := 0;
elseif previousOccupied[slot] == 1.0 then
owner := previousAnchorSlot[slot];
slotValid := previousAnchorId[slot] >= 1 and previousAnchorId[slot] <= identifierLimit
and owner >= 1 and owner <= size(nodeEnabled,1);
for axis in 1:dimension loop
slotValid := slotValid and abs(previousPoint[slot,axis]) <= coordinateLimit
and abs(previousLocalPoint[slot,axis]) <= coordinateLimit;
end for;
stateValid := stateValid and slotValid;
if slotValid then
retained := nodeEnabled[owner] and nodeId[owner] == previousAnchorId[slot];
if retained then
candidate := nodeRotation[owner,:,:]*previousLocalPoint[slot,:]+nodePosition[owner,:];
for axis in 1:dimension loop
coordinatesValid := coordinatesValid and abs(candidate[axis]) <= coordinateLimit;
end for;
point[slot,:] := candidate; projectedCount := projectedCount+1;
else
// Eviction or slot reuse cannot transfer a landmark to a
// new keyframe that happens to occupy the same array slot.
point[slot,:] := zeros(dimension); localPoint[slot,:] := zeros(dimension);
occupied[slot] := 0.0; anchorId[slot] := 0; anchorSlot[slot] := 0;
prunedCount := prunedCount+1;
end if;
end if;
else
stateValid := false;
end if;
end for;
accepted := stateValid and coordinatesValid;
rejectionReason := if not stateValid then 5 else if not coordinatesValid then 6 else 0;
if accepted then nextRevision := graphRevision; end if;
end if;
end if;
end if;
end if;
// Late failures preserve all previous fields, including poisoned disabled
// payload. Canonicalization, pruning and version advancement commit together.
if not accepted then
point := previousPoint; localPoint := previousLocalPoint; occupied := previousOccupied;
anchorId := previousAnchorId; anchorSlot := previousAnchorSlot;
nextRevision := previousRevision; projectedCount := 0; prunedCount := 0;
end if;
end Reproject;
end RGBDMapAnchors;