Skip to content

Camera fixes to SDF to USD converter#787

Merged
ahcorde merged 7 commits intoadlarkin/sdf_to_usdfrom
ahcorde/sdf_to_usd/camera_fix
Jan 6, 2022
Merged

Camera fixes to SDF to USD converter#787
ahcorde merged 7 commits intoadlarkin/sdf_to_usdfrom
ahcorde/sdf_to_usd/camera_fix

Conversation

@ahcorde
Copy link
Copy Markdown
Collaborator

@ahcorde ahcorde commented Dec 14, 2021

Signed-off-by: ahcorde ahcorde@gmail.com

🦟 Bug fix

Summary

Fixes camera orientation

Checklist

  • Signed all commits for DCO
  • Added tests
  • Updated documentation (as needed)
  • Updated migration guide (as needed)
  • codecheck passed (See contributing)
  • All tests passed (See test coverage)
  • While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge

Signed-off-by: ahcorde <ahcorde@gmail.com>
@ahcorde ahcorde requested a review from adlarkin December 14, 2021 21:44
@ahcorde ahcorde self-assigned this Dec 14, 2021
Comment thread src/usd/sdf_usd_parser/sensor.cc Outdated
Comment on lines 52 to 57
usdCamera.CreateHorizontalApertureAttr().Set(
static_cast<float>(sdfCamera->HorizontalFov().Degree()));

// TODO(adlarkin) Do I need to handle the following in USD
// (I don't think there's an SDF equivalent):
// * horizontal and vertical aperture (there's also an offset for these)
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
usdCamera.CreateHorizontalApertureAttr().Set(
static_cast<float>(sdfCamera->HorizontalFov().Degree()));
// TODO(adlarkin) Do I need to handle the following in USD
// (I don't think there's an SDF equivalent):
// * horizontal and vertical aperture (there's also an offset for these)
usdCamera.CreateHorizontalApertureAttr().Set(
static_cast<float>(sdfCamera->HorizontalFov().Degree()));

I believe the TODO can be removed since you addressed the horizontal aperture attribute.

Comment thread src/usd/sdf_usd_parser/sensor.cc Outdated
// TODO(adlarkin) check units to make sure they match (no documented units for SDF)
usdCamera.CreateFocalLengthAttr().Set(
static_cast<float>(sdfCamera->LensFocalLength()));
static_cast<float>(sdfCamera->LensFocalLength() * 10));
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you add a comment that explains why you are multiplying by 10? I assume it's because SDF and USD use different units. It would be nice to have this documented in case someone else has to look at this code later.

Comment thread src/usd/sdf_usd_parser/sensor.cc Outdated
@@ -44,11 +44,14 @@ namespace usd

// TODO(adlarkin) check units to make sure they match (no documented units for SDF)
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If the conversions below seem to be okay regarding units, can we remove this comment?

Comment thread src/usd/sdf_usd_parser/sensor.cc Outdated
Comment on lines +133 to +134
ignition::math::Pose3d poseCamera(0, 0, 0, 1.57, 0, -1.57);
usd::SetPose(poseCamera * _sensor.RawPose(), _stage, sdfSensorPath);
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I just read the following note in the UsdGeomCamera docs:

Cameras in USD are always "Y up", regardless of the stage's orientation (i.e. UsdGeomGetStageUpAxis()).
This means that the inverse of 'camXform' (the VIEW half of the MODELVIEW transform in OpenGL parlance) will transform the world such that the camera is at the origin, looking down the -Z axis, with +Y as the up axis, and +X pointing to the right.
This describes a right handed coordinate system.

So, are you applying the rotations here to fix the default camera direction? If so, that makes sense, but would you mind adding a comment that explains why this has to be done? You can even reference the link I just included above in the comment if that's easiest

@ahcorde ahcorde requested a review from adlarkin January 3, 2022 16:09
Signed-off-by: ahcorde <ahcorde@gmail.com>
Signed-off-by: ahcorde <ahcorde@gmail.com>
Signed-off-by: ahcorde <ahcorde@gmail.com>
@ahcorde
Copy link
Copy Markdown
Collaborator Author

ahcorde commented Jan 5, 2022

I converted this file https://app.ignitionrobotics.org/OpenRobotics/fuel/models/SOPHISTICATED_ENGINEERING_X2_SENSOR_CONFIG_1 and cameras are in the right poses

Selection_016

Copy link
Copy Markdown
Contributor

@adlarkin adlarkin left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I converted this file https://app.ignitionrobotics.org/OpenRobotics/fuel/models/SOPHISTICATED_ENGINEERING_X2_SENSOR_CONFIG_1 and cameras are in the right poses

How are you testing this? I am using the following SDF file:

ugv.sdf
<?xml version="1.0" ?>
<sdf version="1.6">
  <world name="empty">
    <physics name="1ms" type="ignored">
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1.0</real_time_factor>
    </physics>
    <plugin
      filename="ignition-gazebo-physics-system"
      name="ignition::gazebo::systems::Physics">
    </plugin>
    <plugin
      filename="ignition-gazebo-user-commands-system"
      name="ignition::gazebo::systems::UserCommands">
    </plugin>
    <plugin
      filename="ignition-gazebo-scene-broadcaster-system"
      name="ignition::gazebo::systems::SceneBroadcaster">
    </plugin>
    <plugin
      filename="ignition-gazebo-contact-system"
      name="ignition::gazebo::systems::Contact">
    </plugin>

    <light type="directional" name="sun">
      <cast_shadows>true</cast_shadows>
      <pose>0 0 10 0 0 0</pose>
      <diffuse>0.8 0.8 0.8 1</diffuse>
      <specular>0.2 0.2 0.2 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
    </light>

    <model name="ground_plane">
      <static>true</static>
      <link name="link">
        <collision name="collision">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.8 0.8 0.8 1</specular>
          </material>
        </visual>
      </link>
    </model>

    <include>
	    <name>UGV</name>
	    <pose>0 0 0 0 0 0</pose>
	    <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/SOPHISTICATED_ENGINEERING_X2_SENSOR_CONFIG_1</uri>
    </include>
  </world>
</sdf>

I merged in #770 locally since the included model has materials, and then ran the converter:

./usdConverter ugv.sdf ugv.usda

But, there is a box surrounding the model when I run usdview.usda:

box_around_robot

Comment thread src/usd/sdf_usd_parser/sensor.cc
Comment thread src/usd/sdf_usd_parser/sensor.cc
Signed-off-by: ahcorde <ahcorde@gmail.com>
@ahcorde ahcorde marked this pull request as ready for review January 6, 2022 14:56
@ahcorde ahcorde requested review from azeey and scpeters as code owners January 6, 2022 14:56
@ahcorde ahcorde merged commit 5cfcd74 into adlarkin/sdf_to_usd Jan 6, 2022
@ahcorde ahcorde deleted the ahcorde/sdf_to_usd/camera_fix branch January 6, 2022 14:56
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

5 participants