Skip to content

Commit

Permalink
Bugfix: Some objects have invalid scales when replay rendering (#2200)
Browse files Browse the repository at this point in the history 
* gfx-replay scaling fix

* Encode URDF link scaling in creationInfo rather than directly in nodes.

* Remove comment in Recorder.cpp.

* Remove unnecessary std::move.

* Add test for replay rendering of scaled articulated objects.

* Update MetadataMediatorTest ao file count.

* Disable classic replay AO test.

---------

Co-authored-by: Eric Undersander <[email protected]>
  • Loading branch information
@0mdc @eundersander
0mdc and eundersander authored Sep 27, 2023
1 parent 7ed84da commit 8f5d58d
Show file tree
Hide file tree
Showing 6 changed files with 258 additions and 30 deletions.
Binary file added data/test_assets/screenshots/ReplayBatchRendererTest_AO_batch.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
134 changes: 134 additions & 0 deletions data/test_assets/urdf/fridge/fridge_scaled.urdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,134 @@
<?xml version="1.0" ?>
<robot name="fridge">
<link name="root">
</link>
<joint name="root_rotation" type="fixed">
<origin rpy="1.565 0 0" xyz="0 0 0"/>
<parent link="root"/>
<child link="body"/>
</joint>
<link name="body">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="70.0"/>
<!-- Warning: dummy inertia. Expect that inertia diagonal will be computed during load. -->
<inertia ixx="1.0" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1.0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="body.glb" scale="1.5 1.5 1.5"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 -0.002765 -0.94159"/>
<geometry>
<box size="0.643 0.691 0.027"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.002765 0.94936"/>
<geometry>
<box size="0.643 0.691 0.027"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.002765 -0.585528"/>
<geometry>
<box size="0.643 0.691 0.027"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.002765 -0.226826"/>
<geometry>
<box size="0.643 0.691 0.027"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.002765 -0.033778"/>
<geometry>
<box size="0.643 0.691 0.027"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.002765 0.255839"/>
<geometry>
<box size="0.643 0.691 0.042"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.337538 0.005784"/>
<geometry>
<box size="0.643 0.03 1.89126"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0.329773 0.005784"/>
<geometry>
<box size="0.643 0.03 1.89126"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="-0.316502 -0.002765 0"/>
<geometry>
<box size="0.029014 0.690834 1.91906"/>
</geometry>
</collision>
</link>
<joint name="top_door_hinge" type="revolute">
<origin rpy="0 0 0" xyz="0.34 0.326 -0.375"/>
<parent link="body"/>
<child link="top_door"/>
<axis xyz="0 0 1"/>
<dynamics damping="0.01" friction="0.01"/>
<limit effort="44.4" lower="0.0" upper="2.3" velocity="40"/>
</joint>
<link name="top_door">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="8"/>
<!-- Warning: dummy inertia. Expect that inertia diagonal will be computed during load. -->
<inertia ixx="1.0" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1.0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 -0.33 0"/>
<geometry>
<mesh filename="top_door.glb" scale="0.75 0.75 0.75"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 -0.33 -0.02"/>
<geometry>
<box size="0.097 0.712 1.103"/>
</geometry>
</collision>
</link>
<joint name="bottom_door_hinge" type="revolute">
<origin rpy="0 0 0" xyz="0.34 0.326 0.546"/>
<parent link="body"/>
<child link="bottom_door"/>
<axis xyz="0 0 1"/>
<dynamics damping="0.01" friction="0.01"/>
<limit effort="44.4" lower="0.0" upper="2.5" velocity="40"/>
</joint>
<link name="bottom_door">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="6"/>
<!-- Warning: dummy inertia. Expect that inertia diagonal will be computed during load. -->
<inertia ixx="1.0" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1.0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.01 -0.33 0"/>
<geometry>
<mesh filename="bottom_door.glb" scale="1.0 1.0 1.0"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.01 -0.33 0.01"/>
<geometry>
<box size="0.08 0.686 0.788"/>
</geometry>
</collision>
</link>
</robot>
17 changes: 1 addition & 16 deletions src/esp/gfx/replay/Recorder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,22 +55,7 @@ void Recorder::onCreateRenderAssetInstance(
CORRADE_INTERNAL_ASSERT(findInstance(node) == ID_UNDEFINED);

RenderAssetInstanceKey instanceKey = getNewInstanceKey();

auto adjustedCreation = creation;

// We assume constant node scaling over the node's lifetime. Bake node scale
// into creation.
auto nodeScale = node->absoluteTransformation().scaling();
// todo: check for reflection (rotationShear.determinant() < 0.0f) and bake
// into scaling (negate X scale).
if (nodeScale != Mn::Vector3(1.f, 1.f, 1.f)) {
adjustedCreation.scale = adjustedCreation.scale
? *adjustedCreation.scale * nodeScale
: nodeScale;
}

getKeyframe().creations.emplace_back(instanceKey,
std::move(adjustedCreation));
getKeyframe().creations.emplace_back(instanceKey, creation);

// Constructing NodeDeletionHelper here is equivalent to calling
// node->addFeature. We keep a pointer to deletionHelper so we can delete it
Expand Down
20 changes: 9 additions & 11 deletions src/esp/physics/bullet/BulletPhysicsManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -296,14 +296,14 @@ bool BulletPhysicsManager::attachLinkGeometry(
Mn::Color4(material->m_matColor.m_specularColor);
}

auto scale = Mn::Vector3{1.0f, 1.0f, 1.0f};
switch (visual.m_geometry.m_type) {
case metadata::URDF::GEOM_CAPSULE:
visualMeshInfo.type = esp::assets::AssetType::PRIMITIVE;
// should be registered and cached already
visualMeshInfo.filepath = visual.m_geometry.m_meshFileName;
// scale by radius as suggested by magnum docs
visualGeomComponent.scale(
Mn::Vector3(visual.m_geometry.m_capsuleRadius));
scale = Mn::Vector3(visual.m_geometry.m_capsuleRadius);
// Magnum capsule is Y up, URDF is Z up
visualGeomComponent.setTransformation(
visualGeomComponent.transformation() *
Expand All @@ -316,10 +316,9 @@ bool BulletPhysicsManager::attachLinkGeometry(
visualMeshInfo.filepath =
"cylinderSolid_rings_1_segments_12_halfLen_1_useTexCoords_false_"
"useTangents_false_capEnds_true";
visualGeomComponent.scale(
Mn::Vector3(visual.m_geometry.m_capsuleRadius,
visual.m_geometry.m_capsuleHeight / 2.0,
visual.m_geometry.m_capsuleRadius));
scale = Mn::Vector3(visual.m_geometry.m_capsuleRadius,
visual.m_geometry.m_capsuleHeight / 2.0,
visual.m_geometry.m_capsuleRadius);
// Magnum cylinder is Y up, URDF is Z up
visualGeomComponent.setTransformation(
visualGeomComponent.transformation() *
Expand All @@ -328,17 +327,16 @@ bool BulletPhysicsManager::attachLinkGeometry(
case metadata::URDF::GEOM_BOX:
visualMeshInfo.type = esp::assets::AssetType::PRIMITIVE;
visualMeshInfo.filepath = "cubeSolid";
visualGeomComponent.scale(visual.m_geometry.m_boxSize * 0.5);
scale = visual.m_geometry.m_boxSize * 0.5;
break;
case metadata::URDF::GEOM_SPHERE: {
visualMeshInfo.type = esp::assets::AssetType::PRIMITIVE;
// default sphere prim is already constructed w/ radius 1
visualMeshInfo.filepath = "icosphereSolid_subdivs_1";
visualGeomComponent.scale(
Mn::Vector3(visual.m_geometry.m_sphereRadius));
scale = Mn::Vector3(visual.m_geometry.m_sphereRadius);
} break;
case metadata::URDF::GEOM_MESH: {
visualGeomComponent.scale(visual.m_geometry.m_meshScale);
scale = visual.m_geometry.m_meshScale;
visualMeshInfo.filepath = visual.m_geometry.m_meshFileName;
} break;
case metadata::URDF::GEOM_PLANE:
Expand All @@ -358,7 +356,7 @@ bool BulletPhysicsManager::attachLinkGeometry(
flags |= assets::RenderAssetInstanceCreationInfo::Flag::IsRGBD;
flags |= assets::RenderAssetInstanceCreationInfo::Flag::IsSemantic;
assets::RenderAssetInstanceCreationInfo creation(
visualMeshInfo.filepath, Mn::Vector3{1}, flags, lightSetup);
visualMeshInfo.filepath, scale, flags, lightSetup);

auto* geomNode = resourceManager_.loadAndCreateRenderAssetInstance(
visualMeshInfo, creation, &visualGeomComponent, drawables,
Expand Down
111 changes: 111 additions & 0 deletions src/tests/BatchReplayRendererTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
#include "esp/gfx/replay/Recorder.h"
#include "esp/gfx/replay/ReplayManager.h"
#include "esp/metadata/managers/ObjectAttributesManager.h"
#include "esp/physics/objectManagers/ArticulatedObjectManager.h"
#include "esp/physics/objectManagers/RigidObjectManager.h"
#include "esp/sensor/CameraSensor.h"
#include "esp/sensor/Sensor.h"
Expand Down Expand Up @@ -50,6 +51,7 @@ struct BatchReplayRendererTest : Cr::TestSuite::Tester {
void testIntegration();
void testUnproject();
void testBatchPlayerDeletion();
void testArticulatedObject();
void testClose();

const Magnum::Float maxThreshold = 255.f;
Expand Down Expand Up @@ -157,6 +159,18 @@ const struct {
}},
};

const struct {
const char* name;
Cr::Containers::Pointer<esp::sim::AbstractReplayRenderer> (*create)(
const ReplayRendererConfiguration& configuration);
} TestArticulatedObjectData[]{
{"batch",
[](const ReplayRendererConfiguration& configuration) {
return Cr::Containers::Pointer<esp::sim::AbstractReplayRenderer>{
new esp::sim::BatchReplayRenderer{configuration}};
}},
};

const struct {
const char* name;
Cr::Containers::Pointer<esp::sim::AbstractReplayRenderer> (*create)(
Expand All @@ -183,6 +197,11 @@ BatchReplayRendererTest::BatchReplayRendererTest() {

addTests({&BatchReplayRendererTest::testBatchPlayerDeletion});

#ifdef ESP_BUILD_WITH_BULLET
addInstancedTests({&BatchReplayRendererTest::testArticulatedObject},
Cr::Containers::arraySize(TestArticulatedObjectData));
#endif

addInstancedTests({&BatchReplayRendererTest::testClose},
Cr::Containers::arraySize(TestCloseData));
} // ctor
Expand Down Expand Up @@ -470,6 +489,98 @@ void BatchReplayRendererTest::testBatchPlayerDeletion() {
}
}

void BatchReplayRendererTest::testArticulatedObject() {
auto&& data = TestArticulatedObjectData[testCaseInstanceId()];
setTestCaseDescription(data.name);

const auto sensorSpecs = getDefaultSensorSpecs(TestFlag::Color);
constexpr int numEnvs = 1;
const std::string userPrefix = "sensor_";
const std::string screenshotPrefix =
"ReplayBatchRendererTest_AO_" + std::string(data.name);

const std::string assetPath =
Cr::Utility::Path::join(TEST_ASSETS, "urdf/fridge/fridge.urdf");
const std::string assetPathScaled =
Cr::Utility::Path::join(TEST_ASSETS, "urdf/fridge/fridge_scaled.urdf");
std::vector<std::string> serKeyframes;

// Record sequence
{
SimulatorConfiguration simConfig{};
simConfig.enableGfxReplaySave = true;
simConfig.createRenderer = false;
simConfig.enablePhysics = true;
auto sim = Simulator::create_unique(simConfig);
auto aoManager = sim->getArticulatedObjectManager();
auto& sceneRoot = sim->getActiveSceneGraph().getRootNode();
auto& recorder = *sim->getGfxReplayManager()->getRecorder();

{
auto fridgeA = aoManager->addArticulatedObjectFromURDF(assetPath);
CORRADE_COMPARE(aoManager->getNumObjects(), 1);
CORRADE_VERIFY(fridgeA);
fridgeA->setTranslation(Mn::Vector3(-5.0, -2.5, -2.5));

auto fridgeB = aoManager->addArticulatedObjectFromURDF(assetPathScaled);
CORRADE_COMPARE(aoManager->getNumObjects(), 2);
CORRADE_VERIFY(fridgeB);
fridgeB->setTranslation(Mn::Vector3(-5.0, -2.5, 2.5));

auto fridgeC = aoManager->addArticulatedObjectFromURDF(
assetPath, false, 0.5f /* Global scale */);
CORRADE_COMPARE(aoManager->getNumObjects(), 3);
CORRADE_VERIFY(fridgeC);
fridgeC->setTranslation(Mn::Vector3(-5.0, 2.5, -2.5));

auto fridgeD = aoManager->addArticulatedObjectFromURDF(
assetPathScaled, false, 0.5f /* Global scale */);
CORRADE_COMPARE(aoManager->getNumObjects(), 4);
CORRADE_VERIFY(fridgeD);
fridgeD->setTranslation(Mn::Vector3(-5.0, 2.5, 2.5));

auto& recorder = *sim->getGfxReplayManager()->getRecorder();
for (const auto& sensor : sensorSpecs) {
recorder.addUserTransformToKeyframe(
userPrefix + sensor->uuid, Mn::Vector3(0.0f, 0.0f, 0.0f),
Mn::Quaternion::rotation(Mn::Deg(90.f),
Mn::Vector3(0.0f, 1.0f, 0.0f)));
}

std::string serKeyframe = esp::gfx::replay::Recorder::keyframeToString(
recorder.extractKeyframe());
serKeyframes.emplace_back(std::move(serKeyframe));
}
}
CORRADE_COMPARE(serKeyframes.size(), 1);

// Play sequence
{
ReplayRendererConfiguration batchRendererConfig;
batchRendererConfig.sensorSpecifications = sensorSpecs;
batchRendererConfig.numEnvironments = numEnvs;
batchRendererConfig.standalone = true;
Cr::Containers::Pointer<esp::sim::AbstractReplayRenderer> renderer =
data.create(batchRendererConfig);

std::vector<char> colorBuffer;
std::vector<Mn::MutableImageView2D> colorImageView;
colorImageView.emplace_back(getRGBView(
renderer->sensorSize(0).x(), renderer->sensorSize(0).y(), colorBuffer));

renderer->setEnvironmentKeyframe(0, serKeyframes[0]);
renderer->setSensorTransformsFromKeyframe(0, userPrefix);
renderer->render(colorImageView, nullptr);

// Test color output
std::string groundTruthImageFile = screenshotPrefix + ".png";
CORRADE_COMPARE_WITH(
Mn::ImageView2D{colorImageView[0]},
Cr::Utility::Path::join(screenshotDir, groundTruthImageFile),
(Mn::DebugTools::CompareImageToFile{maxThreshold, meanThreshold}));
}
}

void BatchReplayRendererTest::testClose() {
auto&& data = TestIntegrationData[testCaseInstanceId()];
setTestCaseDescription(data.name);
Expand Down
6 changes: 3 additions & 3 deletions src/tests/MetadataMediatorTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -493,13 +493,13 @@ void MetadataMediatorTest::testDataset1() {
ESP_WARNING() << "Starting test LoadArticulatedObjects";
const auto& aoAttributedsMgr = MM_->getAOAttributesManager();
int numAOHandles = aoAttributedsMgr->getNumObjects();
// verify # of urdf filepaths loaded - should be 7;
CORRADE_COMPARE(numAOHandles, 7);
// verify # of urdf filepaths loaded - should be 8;
CORRADE_COMPARE(numAOHandles, 8);
namespace Dir = Cr::Utility::Path;

std::map<std::string, std::string> urdfTestFilenames =
aoAttributedsMgr->getArticulatedObjectModelFilenames();
CORRADE_COMPARE(urdfTestFilenames.size(), 7);
CORRADE_COMPARE(urdfTestFilenames.size(), 8);
// test that each stub name key corresponds to the actual file name passed
// through the key making process
for (std::map<std::string, std::string>::const_iterator iter =
Expand Down

0 comments on commit 8f5d58d

Please sign in to comment.