Main Content

updatePose

Update pose of mesh in TSDF

Since R2024a

collapse all in page

    Syntax

    isUpdated = updatePose(mTSDF,poseStruct)

    Description

    isUpdated = updatePose(mTSDF,poseStruct) updates the poses of meshes in the truncated signed distance field (TSDF) and returns an indication of which mesh poses were successfully updated.

    example

    Examples

    collapse all

    Open Live Script

    Create two collision boxes and one collision sphere. The collision boxes represent a static environment and the sphere represents a dynamic obstacle with a pose that could change at any time.

    box1 = collisionBox(0.5,1,0.1);
box2 = collisionBox(0.5,0.1,0.2,Pose=trvec2tform([0 -0.45 0.15]));
sph = collisionSphere(0.125,Pose=trvec2tform([-0.1 0.25 0.75]));
showCollisionArray({box1,box2,sph});
title("Static Environment and Dynamic Obstacle")
v = [110 10];
view(v);

    Figure contains an axes object. The axes object with title Static Environment and Dynamic Obstacle, xlabel X, ylabel Y contains 3 objects of type patch.

    Create a mesh TSDF manager with a resolution of 25 cells per meter.

    tsdfs = meshtsdf(Resolution=25);

    To improve the efficiency of signed distance field computation, combine meshes that represent the static environment.

    staticMeshes = geom2struct({box1,box2});
staticEnv = staticMeshes(1);
staticEnv.Pose = eye(4);
staticEnv.Vertices = [];
staticEnv.Faces = [];
for i = 1:numel(staticMeshes)
    H = staticMeshes(i).Pose;
    V = staticMeshes(i).Vertices*H(1:3,1:3)'+ H(1:3,end)';
    nVert = size(staticEnv.Vertices,1);
    staticEnv.Vertices = [staticEnv.Vertices; V];
    staticEnv.Faces = [staticEnv.Faces; staticMeshes(i).Faces+nVert];
end
staticEnv.ID = 1;

    Add the static environment mesh to the TSDF manager.

    addMesh(tsdfs,staticEnv);

    Convert the sphere collision geometry into a structure for the mesh TSDF manager. Assign it an ID of 2 and add it to the mesh TSDF manager.

    obstacleID = 2;
dynamicObstacle = geom2struct(sph,obstacleID);
addMesh(tsdfs,dynamicObstacle);
show(tsdfs)
view(v)
axis equal
title("Mesh TSDFs of Static Environment and Dynamic Obstacle")

    Figure contains an axes object. The axes object with title Mesh TSDFs of Static Environment and Dynamic Obstacle contains 2 objects of type scatter.

    Update the pose of the dynamic obstacle in the mesh TSDF manager by changing Pose property of the object handle of the obstacle. Then use the updatePose function to update the pose of the mesh in the TSDF manager.

    dynamicObstacle.Pose = trvec2tform([0.2 0.25 0.2]);
updatePose(tsdfs,dynamicObstacle)
    ans = 
1

    show(tsdfs)
view(v)
axis equal
title("Updated Dynamic Obstacle Pose")

    Figure contains an axes object. The axes object with title Updated Dynamic Obstacle Pose contains 2 objects of type scatter.

    Input Arguments

    collapse all

    Truncated signed distance field for 3-D meshes, specified as a meshtsdf object.

    Example: meshtsdf(meshes,TruncationDistance=5) creates a TSDF for the specified meshes with a truncation distance of 5 meters.

    Pose information for each mesh in the TSDF, returned as a structure or N-element array of structures. Each structure represents the pose information for each mesh in the TSDF, and contains these fields:

    • ID — ID of the mesh in the TSDF.

    • Pose — Pose of the mesh as a 4-by-4 homogeneous transformation matrix.

    Output Arguments

    collapse all

    Indication of whether the pose of the mesh was updated or not, returned as logical 1 (true) if the mesh was successfully updated or 0 (false) if the ID field of the poseStruct specifies the ID of a mesh that does not exist in mTSDF. If poseStruct is an N-element of structures, then isUpdated is an N-element vector of logical scalars corresponding to each of the N pose structures in poseStruct.

    Extended Capabilities

    C/C++ Code Generation
    Generate C and C++ code using MATLAB® Coder™.

    Version History

    Introduced in R2024a

    See Also

    |