scaleAndPadd does not keep the object centered

[felixvd]

I tried loading an object and scaling down the mesh with scaleAndPadd (the file is in mm instead of m), but the vertices move to a location that's not the center.

Here is the resulting location of the object after scaling by a factor of 0.1 (the grey slate in the middle of the image is the robot and the map grid):

It looks like the function doesn't calculate the center of the mesh, but rather the center of the vertices. I don't know if that's the cause of this issue, but it seems like a bounding box would be the more appropriate operation here.

I could also imagine a center() function that moves the object to the center according to this measure. That might solve my problem at least.

[peci1]

In bodies, there is an explicit mesh center read from the mesh file. Did you try scaling the body instead of the shape?

[felixvd]

I'm not sure what you mean by body. In the section of code I am editing I only see a mesh and a ShapeConstPtr to it. The collision_world object does not have a scale method from what I can see. Am I missing something?

[peci1]

Hmm... geometric_shapes differentiate between shapes and bodies. Shapes describe just the shape. Bodies describe items in a world which have their pose attached, allow point containment tests, raycasting etc. But as I quickly skimmed through the code you're referencing, it only uses shapes and never makes a body out of them.

The definition of padding for shapes and bodies is different as of now (see #127). Also, shapes support general meshes, while bodies support only convex meshes.

See #76 which might also be related - you're not the first one who wonders about the results of applying scaling/padding on meshes.

However, I always had the idea that scaling is more or less what everyone expects, but padding is problematic. Could you share the mesh file?

[felixvd]

Sure, should have done it right away.

03-PANEL2.zip

[felixvd]

Do you have an idea about how to solve this or work around it?

[peci1]

Felix, I'm now convinced that the scaling works as it should.

The mesh you provided has vertices from (0, 0, 0) "up to" (116, 90, 25). So if you scale the mesh towards its center(oid), it should stay positioned around the original centroid (roughly (49, 42, 5)). That would exactly match what you observe.

I guess you'd get what you expect if the mesh had its centroid in (0, 0, 0).

If you agree, I think this issue can be closed and it should be treated as an issue in the rviz plugin - you could e.g. say that after adding the mesh, rviz should translate its centroid towards zero.

Here's a test that succeeds and tests the behavior:

void computeMeshStats(Mesh* mesh, Eigen::Vector3d& minVertex, Eigen::Vector3d& maxVertex, Eigen::Vector3d& centroid)
{
  minVertex = { 1e12, 1e12, 1e12 };
  maxVertex = { -1e12, -1e12, -1e12 };
  centroid = { 0, 0, 0 };

  for (size_t i = 0; i < 3 * mesh->vertex_count; i += 3)
  {
    const auto x = mesh->vertices[i];
    const auto y = mesh->vertices[i+1];
    const auto z = mesh->vertices[i+2];

    minVertex.x() = (std::min)(minVertex.x(), x);
    minVertex.y() = (std::min)(minVertex.y(), y);
    minVertex.z() = (std::min)(minVertex.z(), z);

    maxVertex.x() = (std::max)(maxVertex.x(), x);
    maxVertex.y() = (std::max)(maxVertex.y(), y);
    maxVertex.z() = (std::max)(maxVertex.z(), z);

    centroid.x() += x;
    centroid.y() += y;
    centroid.z() += z;
  }

  centroid = centroid / mesh->vertex_count;
}

TEST(Mesh, ComplexMeshScale)
{
  std::string path = "file:///home/peci1/Download/03-PANEL2.stl";
  const auto& mesh = shapes::createMeshFromResource(path);

  ASSERT_EQ(mesh->vertex_count, 542u);

  Eigen::Vector3d minVertex, maxVertex, centroid;

  computeMeshStats(mesh, minVertex, maxVertex, centroid);

  const auto minx = 0.0;
  const auto miny = 0.0;
  const auto minz = 0.0;

  const auto maxx = 116.0;
  const auto maxy = 90.0;
  const auto maxz = 25.0;

  const auto cx = 49.3427;
  const auto cy = 41.9660;
  const auto cz = 5.06458;

  EXPECT_NEAR(minVertex.x(), minx, 1e-6);
  EXPECT_NEAR(minVertex.y(), miny, 1e-6);
  EXPECT_NEAR(minVertex.z(), minz, 1e-6);

  EXPECT_NEAR(maxVertex.x(), maxx, 1e-6);
  EXPECT_NEAR(maxVertex.y(), maxy, 1e-6);
  EXPECT_NEAR(maxVertex.z(), maxz, 1e-6);

  EXPECT_NEAR(centroid.x(), cx, 1e-3);
  EXPECT_NEAR(centroid.y(), cy, 1e-3);
  EXPECT_NEAR(centroid.z(), cz, 1e-3);

  const auto scale = 0.1;

  mesh->scale(scale);

  computeMeshStats(mesh, minVertex, maxVertex, centroid);

  EXPECT_NEAR(minVertex.x(), cx - (cx - minx) * scale, 1e-3);
  EXPECT_NEAR(minVertex.y(), cy - (cy - miny) * scale, 1e-3);
  EXPECT_NEAR(minVertex.z(), cz - (cz - minz) * scale, 1e-3);

  EXPECT_NEAR(maxVertex.x(), cx + (maxx - cx) * scale, 1e-3);
  EXPECT_NEAR(maxVertex.y(), cy + (maxy - cy) * scale, 1e-3);
  EXPECT_NEAR(maxVertex.z(), cz + (maxz - cz) * scale, 1e-3);

  EXPECT_NEAR(centroid.x(), cx, 1e-3);
  EXPECT_NEAR(centroid.y(), cy, 1e-3);
  EXPECT_NEAR(centroid.z(), cz, 1e-3);

  auto mesh2 = shapes::createMeshFromResource(path);
  for (size_t i = 0; i < 3 * mesh->vertex_count; i += 3) {
    mesh2->vertices[i] -= cx;
    mesh2->vertices[i + 1] -= cy;
    mesh2->vertices[i + 2] -= cz;
  }

  computeMeshStats(mesh2, minVertex, maxVertex, centroid);

  EXPECT_NEAR(minVertex.x(), -cx, 1e-6);
  EXPECT_NEAR(minVertex.y(), -cy, 1e-6);
  EXPECT_NEAR(minVertex.z(), -cz, 1e-6);

  EXPECT_NEAR(maxVertex.x(), maxx - cx, 1e-6);
  EXPECT_NEAR(maxVertex.y(), maxy - cy, 1e-6);
  EXPECT_NEAR(maxVertex.z(), maxz - cz, 1e-6);

  EXPECT_NEAR(centroid.x(), 0.0, 1e-3);
  EXPECT_NEAR(centroid.y(), 0.0, 1e-3);
  EXPECT_NEAR(centroid.z(), 0.0, 1e-3);

  mesh2->scale(scale);

  computeMeshStats(mesh2, minVertex, maxVertex, centroid);

  EXPECT_NEAR(minVertex.x(), -cx * scale, 1e-3);
  EXPECT_NEAR(minVertex.y(), -cy * scale, 1e-3);
  EXPECT_NEAR(minVertex.z(), -cz * scale, 1e-3);

  EXPECT_NEAR(maxVertex.x(), (maxx - cx) * scale, 1e-3);
  EXPECT_NEAR(maxVertex.y(), (maxy - cy) * scale, 1e-3);
  EXPECT_NEAR(maxVertex.z(), (maxz - cz) * scale, 1e-3);

  EXPECT_NEAR(centroid.x(), 0.0, 1e-3);
  EXPECT_NEAR(centroid.y(), 0.0, 1e-3);
  EXPECT_NEAR(centroid.z(), 0.0, 1e-3);
}

[felixvd]

Thanks for looking into it! I understood the problem better. shapes.cpp scales the mesh by keeping its centroid stationary, but the origin of a mesh is generally not at its origin. The mismatch between coordinate system origin and centroid is what is causing the problem, and the interactive marker to remain so large.

I see two ways to solve this:

1. As you suggested, center the mesh when the CollisionObject is inserted in the Rviz plugin (= move the vertices of the mesh such that (0, 0, 0) is the centroid)
2. Perform the scaling in the Rviz plugin around the origin (since meshes added by the user are not guaranteed to be centered)

One could add either or both of them to the Mesh class (For option 1, a method center() and a flag centered, so that the calculation of the centroid in scaleAndPadd can be skipped. For option 2, a , bool scale_around_centroid=true) flag in the scale methods), but I am not sure it is worth the effort and extra lines of code, even though it would be great to make sure that this UI bug does not occur again. Maybe someone else has an opinion, otherwise I am good with closing this.

[v4hn]

I only read your comments here, but why isn't it an option to simply "scale" the origin as well w.r.t. the centroid? That doesn't sound too complicated and would not break intuitive behavior.

[peci1]

@v4hn Do you mean first figuring out the centroid, "scaling it", and then scaling all vertices and moving them to match the computed centroid?

[v4hn]

@v4hn Do you mean first figuring out the centroid, "scaling it", and then scaling all vertices and moving them to match the computed centroid?

Either that, or extend the interactive marker side to allow for an offset position that represents the scaling. Moving the Marker might not be a nice move when the user deliberately put the mesh origin somewhere else...

[peci1]

And isn't the method you propose, @v4hn, equivalent to scaling towards zero? I mean, I think (hope) there was a reason for implementing scaling towards the centroid rather than towards zero. Honestly, I don't think it's possible to change the default scaling behavior without silently breaking lots of downstream code. But we could add methods like scaleAndPaddTowardsZero() or similar for shapes::Mesh. Other shapes wouldn't need it because they are transform-less.

[v4hn]

isn't the method you propose, @v4hn, equivalent to scaling towards zero?

Probably true. I proposed to do it like that in the context of the RViz GUI in order not to break user interaction, while not changing anything in the general geometric_shapes classes or the datastructures behind the GUI.

Honestly, I don't think it's possible to change the default scaling behavior without silently breaking lots of downstream code.

No, that's not an option.

But we could add methods like `scaleAndPaddTowardsZero()` or similar for `shapes::Mesh`.

Reasonable auxiliary methods. I'm not sure that would help with the current problem, because scale and padding are represented by two numbers in MoveIt. If we add a conditional mode for meshes, we would also have to add the information throughout the representation. I don't think that's worth it.

[peci1]

I agree. Solving this issue on the rviz side seems like the best option to me.

[felixvd]

As is, scaling CollisionObjects in Rviz won't cause issues unless multiple shapes are defined and/or a mesh is not centered on the object. The current scaling behavior is safe for padding and probably mainly meant for that, so it should remain the default. I will close this.