Improve performance of bodies::setPose()

[peci1]

Performance of bodies::setPose() was improved by calling .linear() instead of .rotation().
For isometries, these should result to the same code, but this optimization is not in any released version of Eigen so far (3.3.7).

I also had to fix test_bounding_box.cpp test, as I found there a few usages of AngleAxis to which a denormalized axis was passed, which is illegal. And the fixes in this PR broke one of the wrong tests.

I also did additional speedups in ConvexMesh::setPose().
First, there was an unncessary dynamic memory allocation, which was not optimized out by the compiler (removing the allocation speeded up the pref. test by ~25%).
Second, the construction of the helper bounding_box_ object led to calling Box::updateInternalData() 4 times in succession. So I fixed that by accessing the protected members of Box and making ConvexMesh its friend class. If this approach is not desirable, I extracted it to a single commit which can be reverted.

For the measured performance improvement, see #125 (comment).

[peci1]

As was suggested in #125, I didn't create a performance test that would be a part of the package, because the rules on how to write them in a non-flaky way are unclear.

Anyways, if anybody wanted to replicate my experiments, here's the test code:

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2019, Open Robotics.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Open Robotics nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/** \Author Martin Pecka */

#include <geometric_shapes/bodies.h>
#include <gtest/gtest.h>
#include <ros/duration.h>
#include <ros/time.h>
#include <geometric_shapes/mesh_operations.h>
#include "resources/config.h"


TEST(BodiesPerformance, Sphere)
{
  shapes::Sphere shape(1.0);
  bodies::Sphere body(&shape);
  Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();

  const auto start = ros::WallTime::now();
  for (size_t i = 0; i < 10000000; ++i)
    body.setPose(tf);
  const auto end = ros::WallTime::now();

  EXPECT_GT(1.0, (end - start).toSec());
}

TEST(BodiesPerformance, Box)
{
  shapes::Box shape(1.0, 1.0, 1.0);
  bodies::Box body(&shape);
  Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();

  const auto start = ros::WallTime::now();
  for (size_t i = 0; i < 10000000; ++i)
    body.setPose(tf);
  const auto end = ros::WallTime::now();

  EXPECT_GT(1.0, (end - start).toSec());
}

TEST(BodiesPerformance, Cylinder)
{
  shapes::Cylinder shape(1.0, 1.0);
  bodies::Cylinder body(&shape);
  Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();

  const auto start = ros::WallTime::now();
  for (size_t i = 0; i < 10000000; ++i)
    body.setPose(tf);
  const auto end = ros::WallTime::now();

  EXPECT_GT(1.0, (end - start).toSec());
}

TEST(BodiesPerformance, Mesh)
{
  shapes::Mesh* shape = shapes::createMeshFromResource("file://" + std::string(TEST_RESOURCES_DIR) + "/box.dae");
  bodies::ConvexMesh body(shape);
  delete shape;
  Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();

  const auto start = ros::WallTime::now();
  for (size_t i = 0; i < 10000000; ++i)
    body.setPose(tf);
  const auto end = ros::WallTime::now();

  EXPECT_GT(1.0, (end - start).toSec());
}

TEST(BodiesPerformance, ScaledMesh)
{
  shapes::Mesh* shape = shapes::createMeshFromResource("file://" + std::string(TEST_RESOURCES_DIR) + "/box.dae");
  bodies::ConvexMesh body(shape);
  delete shape;
  body.setScale(1.1);
  Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();

  const auto start = ros::WallTime::now();
  for (size_t i = 0; i < 10000000; ++i)
    body.setPose(tf);
  const auto end = ros::WallTime::now();

  EXPECT_GT(1, (end - start).toSec());
}

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}

[v4hn]

Things to do on a Saturday night. 😎
Thank you so much for your analysis and patches!

The friend approach to access internal members is indeed not ideal.
What do you think about extending the interface of the Body class to include a setAttributes(pose,padding,scale) method instead?

[peci1]

You're right, the quarantine has some positive effects for open source, I'd say.

I was also thinking about adding a method like the one you suggest, maybe rather with pointer arguments to allow setting just some of them. Also setDimensions() is another one that would be good to add. But, eh, having a 4-arg function doesn't seem really nice to me.

Another approach I can think of is adding an optional parameter to setPose() etc., like updateInternalData=true, which would tell the function if it should call the updateInternalData() or not. And leaving a note in the documentation that when the user calls this function with false argument, then he is responsible for calling one of these functions with a true in the end. Or we could even make updateInternalData() public, but that doesn't look good to me. Adding these optional arguments should not affect ABI (as all the relevant functions are non-vritual). I'm just not sure how C++ handles optional arguments API-wise, so it might instead be safer to not alter the existing signatures and adding new 2-arg functions like setPose(pose, updateInternalData) (which I think is even recommended by some people over default argument values).

Yet another approach would be to add some "factory-like" object, e.g. AtrributeSetter, where you'd set any of these 4 parameters, and then you'd pass the setter object to e.g. Body::setAttributes(AttributeSetter). But I don't personally like this approach very much as it seems overcomplicated to me.

[v4hn]

Another approach I can think of is adding an optional parameter to setPose() etc., like updateInternalData=true, which would tell the function if it should call the updateInternalData() or not. And leaving a note in the documentation that when the user calls this function with false argument, then he is responsible for calling one of these functions with a true in the end. Or we could even make updateInternalData() public, but that doesn't look good to me. Adding these optional arguments should not affect ABI (as all the relevant functions are non-vritual). I'm just not sure how C++ handles optional arguments API-wise, so it might instead be safer to not alter the existing signatures and adding new 2-arg functions like setPose(pose, updateInternalData) (which I think is even recommended by some people over default argument values).

You mixed up API and ABI: a new optional parameter breaks ABI, not API.
For geometric_shapes we apparently never adopted SONAMES, so an ABI-stable solution is strongly preferred, unless we merge these changes in a new branch (e.g., master) only.
I would support adding new interface methods to change the values without updating the datastructures, but adding another parameter does not make sense then.
Instead, I would propose a new family of setters, e.g., setPoseUnsafe. This makes it rather obvious people should be careful when to call it 😃

[peci1]

Okay, adding new setters sounds reasonable. I'm just thinking whether to use Unsafe of Dirty. But what about setDimensions()? This method already has the dirty counterpart, useDimensions(), which is protected. Should I create a public version setDimensionsUnsafe(), or should I just make useDimensions() public? Personally, I'd go with the former option as that would make the naming consistent with the other methods.

[v4hn]

I'm just thinking whether to use Unsafe of Dirty

Either is fine.

Should I create a public version setDimensionsUnsafe(), or should I just make useDimensions() public?

Consistent naming is important. I prefer setDimensions* too.

[peci1]

I added the setScaleDirty(), setPaddingDirty(), setPoseDirty() and setDimensionsDirty() and made updateInternalData() public in all classes. The friend specification was removed.

[v4hn]

Looks good to me and does not break API/ABI.

The only unresolved question is whether we want to merge 4f5a7be in melodic-devel still.
Once this is decided (tomorrows meeting), this can be merged as appropriate.

[peci1]

I added the isometry asserts. Once we agree on the correct approach, I'll update the moveit PR, too.

Here I added a "no-op" message as the first test in the assert so that when code fails on the assert, the user will see something more friendly than just a bunch of mathematical computations (C++ prints out the whole definition of the assert that failed).

[peci1]

The error message looks exactly like this: https://travis-ci.org/github/ros-planning/geometric_shapes/jobs/667849514#L911 :-D

Any idea why the test succeeded on melodic and failed on kinetic?

[peci1]

Ah, kinetic build was choosing the integral version of ::abs(int) instead of std::abs(double). No idea why. I added std:: to disambiguate.

I also made the assert macro a bit more verbose so that the user can see not only the failed condition, but also the value of the transformation matrix and its computed determinant. This comes at the cost of uglier macro, but provides better user experience.

[v4hn]

I very much like the current approach and clean implementation.

Ready for merge in my opinion. I'm still happy to merge this in melodic-devel, alternatively we have to split off a noetic-devel branch already.

[rhaschke]

Generally looks good. Some minor remarks though.

[peci1]

@rhaschke This should be ready to merge.

[peci1]

There was a forgotten non-isometry in test_bounding_cylinder. It did not trigger locally for me beacause this branch was not up-to-date with melodic-devel. I rebased and now let's hope the tests will pass.

[peci1]

@rhaschke Are you sure this check is done by design? Wouldn't that only be the case in the "efficient storage" which only stores a 3x4 matrix?

[rhaschke]

True. Will revert.

[peci1]

Shouldn't the test do at least something in release mode? I.e. test that calling the assert on invalid transform would not actually do anything?

[rhaschke]

Yes, I can change this.
Do you want to keep the different commits of this PR or can I squash-merge it?

[peci1]

Well, the last two or three commits do not carry any useful information, but I think the previous ones are nicely "decoupled" and explain why each change was made. I'd vouch for keeping these.

[rhaschke]

No problem. In this case, I ask you to cleanup the history along the following line:

pick 074502d Normalize the direction vector passed to Body::intersectsRay() (#115)
pick e2f4850 Improved performance of Body::setPose() by calling .linear() instead of .rotation().
pick 8dfe31c Improved performance of ConvexMesh::setPose() by getting rid of unnecessary dynamic memory allocation.
pick 13d662e Fixed test_bounding_box.cpp wrong usage of AngleAxis with denormalized axes.
squash e170bcf Fixed a non-isometry in test_bounding_cylinder
pick 5381162 Added dirty versions of setScale/setPadding/setPose/setDimensions and made updateInternalData public.
pick 9c678de Added asserts that check bodies::Body::pose_ is an actual isometry before calling .linear() on it.
fixup 8555637 Fixed ASSERT_ISOMETRY, made setDimensionsDirty() inline.
fixup 721e0b1 Cleanup of ASSERT_ISOMETRY
fixup f3a3af5 fixup! Cleanup of ASSERT_ISOMETRY

Obviously, the commit 8555637 Fixed ASSERT_ISOMETRY, made setDimensionsDirty() inline. needs to be split up onto two previous commits.

[peci1]

I cleaned up the commit history.