Update geometry utilities (RobotLocomotion#10344)

* Update geometry utilities

1. Unify "detail" and "internal" namespaces to just "internal".
  - affects invocations of CanonicalizeStringName().
2. Move the Isometry3 conversion methods into the common utilities.

geometry/BUILD.bazel

@@ -47,6 +47,7 @@ drake_cc_library(
         ":geometry_ids",
         ":geometry_index",
         ":shape_specification",
+        ":utilities",
         "//common",
         "//common:default_scalars",
         "//geometry/query_results:penetration_as_point_pair",
@@ -375,7 +376,10 @@ drake_cc_googletest(
 
 drake_cc_googletest(
     name = "utilities_test",
-    deps = ["utilities"],
+    deps = [
+        "utilities",
+        "//common/test_utilities",
+    ],
 )
 
 add_lint_tests()

geometry/dev/geometry_state.cc

@@ -426,7 +426,8 @@ const std::string& GeometryState<T>::get_name(GeometryId geometry_id) const {
 template <typename T>
 GeometryId GeometryState<T>::GetGeometryFromName(
     FrameId frame_id, Role role, const std::string& name) const {
-  const std::string canonical_name = detail::CanonicalizeStringName(name);
+  const std::string canonical_name =
+      geometry::internal::CanonicalizeStringName(name);
 
   GeometryId result;
   int count = 0;
@@ -694,7 +695,8 @@ bool GeometryState<T>::IsValidGeometryName(
   FindOrThrow(frame_id, frames_, [frame_id]() {
     return "Given frame id is not valid: " + to_string(frame_id);
   });
-  const std::string name = detail::CanonicalizeStringName(candidate_name);
+  const std::string name =
+      geometry::internal::CanonicalizeStringName(candidate_name);
   if (name.empty()) return false;
   return NameIsUnique(frame_id, role, name);
 }

geometry/geometry_instance.cc

@@ -11,7 +11,7 @@ GeometryInstance::GeometryInstance(const Isometry3<double>& X_PG,
     : id_(GeometryId::get_new_id()),
       X_PG_(X_PG),
       shape_(std::move(shape)),
-      name_(detail::CanonicalizeStringName(name)) {
+      name_(internal::CanonicalizeStringName(name)) {
   if (name_.empty()) {
     throw std::logic_error("GeometryInstance given the name '" + name +
                            "' which is an empty canonical string");

geometry/geometry_state.cc

@@ -231,7 +231,7 @@ int GeometryState<T>::NumGeometriesWithRole(FrameId frame_id, Role role) const {
 template <typename T>
 GeometryId GeometryState<T>::GetGeometryFromName(
     FrameId frame_id, Role role, const std::string& name) const {
-  const std::string canonical_name = detail::CanonicalizeStringName(name);
+  const std::string canonical_name = internal::CanonicalizeStringName(name);
 
   GeometryId result;
   int count = 0;
@@ -588,7 +588,7 @@ bool GeometryState<T>::IsValidGeometryName(
   FindOrThrow(frame_id, frames_, [frame_id]() {
     return "Given frame id is not valid: " + to_string(frame_id);
   });
-  const std::string name = detail::CanonicalizeStringName(candidate_name);
+  const std::string name = internal::CanonicalizeStringName(candidate_name);
   if (name.empty()) return false;
   return NameIsUnique(frame_id, role, name);
 }

geometry/proximity_engine.cc

@@ -21,6 +21,7 @@
 #include "drake/common/default_scalars.h"
 #include "drake/common/drake_variant.h"
 #include "drake/common/sorted_vectors_have_intersection.h"
+#include "drake/geometry/utilities.h"
 
 namespace drake {
 namespace geometry {
@@ -35,24 +36,7 @@ using std::unique_ptr;
 
 namespace {
 
-// TODO(SeanCurtis-TRI): Swap all Isometry3 for Transforms.
-
-// ADL-reliant helper functions for converting Isometry<T> to Isometry<double>.
-const Isometry3<double>& convert(const Isometry3<double>& transform) {
-  return transform;
-}
-
-template <class VectorType>
-Isometry3<double> convert(
-    const Isometry3<Eigen::AutoDiffScalar<VectorType>>& transform) {
-  Isometry3<double> result;
-  for (int r = 0; r < 4; ++r) {
-    for (int c = 0; c < 4; ++c) {
-      result.matrix()(r, c) = ExtractDoubleOrThrow(transform.matrix()(r, c));
-    }
-  }
-  return result;
-}
+// TODO(SeanCurtis-TRI): Swap all Isometry3 for RigidTransforms.
 
 // Utilities/functions for working with the encoding of collision object index
 // and mobility type in the fcl::CollisionObject user data field. The encoded

geometry/test/utilities_test.cc

@@ -4,9 +4,11 @@
 
 #include <gtest/gtest.h>
 
+#include "drake/common/test_utilities/eigen_matrix_compare.h"
+
 namespace drake {
 namespace geometry {
-namespace detail {
+namespace internal {
 namespace {
 
 GTEST_TEST(GeometryUtilities, CanonicalizeGeometryName) {
@@ -52,7 +54,22 @@ GTEST_TEST(GeometryUtilities, CanonicalizeGeometryName) {
   }
 }
 
+GTEST_TEST(GeometryUtilities, IsometryConversion) {
+  Isometry3<double> X_AB = Isometry3<double>::Identity();
+  X_AB.translation() << 1, 2, 3;
+  // NOTE: Not a valid transform; we're just looking for unique values.
+  X_AB.linear() << 10, 20, 30, 40, 50, 60, 70, 80, 90;
+  X_AB.makeAffine();
+
+  Isometry3<double> X_AB_converted = convert(X_AB);
+  EXPECT_TRUE(CompareMatrices(X_AB.matrix(), X_AB_converted.matrix()));
+
+  Isometry3<AutoDiffXd> X_AB_ad(X_AB);
+  Isometry3<double> X_AB_ad_converted = convert(X_AB_ad);
+  EXPECT_TRUE(CompareMatrices(X_AB.matrix(), X_AB_ad_converted.matrix()));
+}
+
 }  // namespace
-}  // namespace detail
+}  // namespace internal
 }  // namespace geometry
 }  // namespace drake

geometry/utilities.cc

@@ -6,7 +6,7 @@
 
 namespace drake {
 namespace geometry {
-namespace detail {
+namespace internal {
 
 std::string CanonicalizeStringName(const std::string& name) {
   // The definition of "canonical" is based on SDF and the functionality in
@@ -29,6 +29,6 @@ std::string CanonicalizeStringName(const std::string& name) {
   return matches[1].str();
 }
 
-}  // namespace detail
+}  // namespace internal
 }  // namespace geometry
 }  // namespace drake

geometry/utilities.h

@@ -3,24 +3,21 @@
 #include <string>
 #include <unordered_map>
 
+#include "drake/common/autodiff.h"
+#include "drake/common/autodiff_overloads.h"
 #include "drake/common/drake_copyable.h"
+#include "drake/common/eigen_types.h"
 
 namespace drake {
 namespace geometry {
-
-// TODO(SeanCurtis-TRI): Get rid of the "detail" namespace.
-namespace detail {
+namespace internal {
 
 /** Canonicalizes the given geometry *candidate* name. A canonicalized name may
  still not be valid (as it may duplicate a previously used name). See
  @ref canonicalized_geometry_names "documentation in GeometryInstance" for
  details. */
 std::string CanonicalizeStringName(const std::string& name);
 
-}  // namespace detail
-
-namespace internal {
-
 /// A const range iterator through the keys of an unordered map.
 template <typename K, typename V>
 class MapKeyRange {
@@ -58,7 +55,35 @@ class MapKeyRange {
   const std::unordered_map<K, V>* map_;
 };
 
-}  // namespace internal
+/** @name Isometry scalar conversion
+
+ Some of SceneGraph's inner-workings are _not_ templated on scalar type and
+ always require Isometry3<double>. These functions work in an ADL-compatible
+ manner to allow SceneGraph to mindlessly convert templated Isometry3 to
+ double-valued transforms.  */
+//@{
 
+// TODO(SeanCurtis-TRI): Get rid of these when I finally swap for
+// RigidTransforms.
+
+inline const Isometry3<double>& convert(const Isometry3<double>& transform) {
+  return transform;
+}
+
+template <class VectorType>
+Isometry3<double> convert(
+    const Isometry3<Eigen::AutoDiffScalar<VectorType>>& transform) {
+  Isometry3<double> result;
+  for (int r = 0; r < 4; ++r) {
+    for (int c = 0; c < 4; ++c) {
+      result.matrix()(r, c) = ExtractDoubleOrThrow(transform.matrix()(r, c));
+    }
+  }
+  return result;
+}
+
+//@}
+
+}  // namespace internal
 }  // namespace geometry
 }  // namespace drake