VertexVertexDistanceErrorFunction. (#577)

Summary:
Pull Request resolved: #577

We want to be able to pin certain vertices to be a specific distance apart.  This has some similarities to the PointTriangleVertexErrorFunction since it is a mesh-mesh distance, but should work better for long distances.

Recall that the point-triangle error function defines a local space using the triangle plane and normal, and the target point is rigidly attached in this space.  This works well when you are close to the triangle, but as you get far away from the triangle you end up with a very large moment arm torquing the triangle (basically the error function may "prefer" to rotate the triangle rather than actually changing the distance).

For this reason it makes sense to have a much simpler point-to-point error that just acts on vertices, it won't be able to constrain the location rigidly in the same way (since there is no rigid frame defined on an individual vertex) but it should be numerically well-behaved.

Reviewed By: jeongseok-meta

Differential Revision: D82848960

fbshipit-source-id: 723f84b58fa365ee269f793033ff4081bb0f8bd1

Author: cdtwigg

cmake/build_variables.bzl

@@ -227,6 +227,7 @@ character_solver_public_headers = [
     "character_solver/normal_error_function.h",
     "character_solver/orientation_error_function.h",
     "character_solver/plane_error_function.h",
+    "character_solver/point_triangle_vertex_error_function.h",
     "character_solver/pose_prior_error_function.h",
     "character_solver/position_error_function.h",
     "character_solver/projection_error_function.h",
@@ -240,7 +241,7 @@ character_solver_public_headers = [
     "character_solver/trust_region_qr.h",
     "character_solver/vertex_error_function.h",
     "character_solver/vertex_projection_error_function.h",
-    "character_solver/point_triangle_vertex_error_function.h",
+    "character_solver/vertex_vertex_distance_error_function.h",
 ]
 
 character_solver_sources = [
@@ -255,6 +256,7 @@ character_solver_sources = [
     "character_solver/normal_error_function.cpp",
     "character_solver/orientation_error_function.cpp",
     "character_solver/plane_error_function.cpp",
+    "character_solver/point_triangle_vertex_error_function.cpp",
     "character_solver/pose_prior_error_function.cpp",
     "character_solver/position_error_function.cpp",
     "character_solver/projection_error_function.cpp",
@@ -267,7 +269,7 @@ character_solver_sources = [
     "character_solver/trust_region_qr.cpp",
     "character_solver/vertex_error_function.cpp",
     "character_solver/vertex_projection_error_function.cpp",
-    "character_solver/point_triangle_vertex_error_function.cpp",
+    "character_solver/vertex_vertex_distance_error_function.cpp",
 ]
 
 character_solver_test_sources = [

momentum/character_solver/fwd.h

@@ -608,6 +608,31 @@ using PlaneErrorFunctiond_const_p = ::std::shared_ptr<const PlaneErrorFunctiond>
 using PlaneErrorFunctiond_const_u = ::std::unique_ptr<const PlaneErrorFunctiond>;
 using PlaneErrorFunctiond_const_w = ::std::weak_ptr<const PlaneErrorFunctiond>;
 
+template <typename T>
+class PointTriangleVertexErrorFunctionT;
+using PointTriangleVertexErrorFunction = PointTriangleVertexErrorFunctionT<float>;
+using PointTriangleVertexErrorFunctiond = PointTriangleVertexErrorFunctionT<double>;
+
+using PointTriangleVertexErrorFunction_p = ::std::shared_ptr<PointTriangleVertexErrorFunction>;
+using PointTriangleVertexErrorFunction_u = ::std::unique_ptr<PointTriangleVertexErrorFunction>;
+using PointTriangleVertexErrorFunction_w = ::std::weak_ptr<PointTriangleVertexErrorFunction>;
+using PointTriangleVertexErrorFunction_const_p =
+    ::std::shared_ptr<const PointTriangleVertexErrorFunction>;
+using PointTriangleVertexErrorFunction_const_u =
+    ::std::unique_ptr<const PointTriangleVertexErrorFunction>;
+using PointTriangleVertexErrorFunction_const_w =
+    ::std::weak_ptr<const PointTriangleVertexErrorFunction>;
+
+using PointTriangleVertexErrorFunctiond_p = ::std::shared_ptr<PointTriangleVertexErrorFunctiond>;
+using PointTriangleVertexErrorFunctiond_u = ::std::unique_ptr<PointTriangleVertexErrorFunctiond>;
+using PointTriangleVertexErrorFunctiond_w = ::std::weak_ptr<PointTriangleVertexErrorFunctiond>;
+using PointTriangleVertexErrorFunctiond_const_p =
+    ::std::shared_ptr<const PointTriangleVertexErrorFunctiond>;
+using PointTriangleVertexErrorFunctiond_const_u =
+    ::std::unique_ptr<const PointTriangleVertexErrorFunctiond>;
+using PointTriangleVertexErrorFunctiond_const_w =
+    ::std::weak_ptr<const PointTriangleVertexErrorFunctiond>;
+
 template <typename T>
 class PosePriorErrorFunctionT;
 using PosePriorErrorFunction = PosePriorErrorFunctionT<float>;
@@ -872,28 +897,28 @@ using VertexProjectionErrorFunctiond_const_w =
     ::std::weak_ptr<const VertexProjectionErrorFunctiond>;
 
 template <typename T>
-class PointTriangleVertexErrorFunctionT;
-using PointTriangleVertexErrorFunction = PointTriangleVertexErrorFunctionT<float>;
-using PointTriangleVertexErrorFunctiond = PointTriangleVertexErrorFunctionT<double>;
-
-using PointTriangleVertexErrorFunction_p = ::std::shared_ptr<PointTriangleVertexErrorFunction>;
-using PointTriangleVertexErrorFunction_u = ::std::unique_ptr<PointTriangleVertexErrorFunction>;
-using PointTriangleVertexErrorFunction_w = ::std::weak_ptr<PointTriangleVertexErrorFunction>;
-using PointTriangleVertexErrorFunction_const_p =
-    ::std::shared_ptr<const PointTriangleVertexErrorFunction>;
-using PointTriangleVertexErrorFunction_const_u =
-    ::std::unique_ptr<const PointTriangleVertexErrorFunction>;
-using PointTriangleVertexErrorFunction_const_w =
-    ::std::weak_ptr<const PointTriangleVertexErrorFunction>;
-
-using PointTriangleVertexErrorFunctiond_p = ::std::shared_ptr<PointTriangleVertexErrorFunctiond>;
-using PointTriangleVertexErrorFunctiond_u = ::std::unique_ptr<PointTriangleVertexErrorFunctiond>;
-using PointTriangleVertexErrorFunctiond_w = ::std::weak_ptr<PointTriangleVertexErrorFunctiond>;
-using PointTriangleVertexErrorFunctiond_const_p =
-    ::std::shared_ptr<const PointTriangleVertexErrorFunctiond>;
-using PointTriangleVertexErrorFunctiond_const_u =
-    ::std::unique_ptr<const PointTriangleVertexErrorFunctiond>;
-using PointTriangleVertexErrorFunctiond_const_w =
-    ::std::weak_ptr<const PointTriangleVertexErrorFunctiond>;
+class VertexVertexDistanceErrorFunctionT;
+using VertexVertexDistanceErrorFunction = VertexVertexDistanceErrorFunctionT<float>;
+using VertexVertexDistanceErrorFunctiond = VertexVertexDistanceErrorFunctionT<double>;
+
+using VertexVertexDistanceErrorFunction_p = ::std::shared_ptr<VertexVertexDistanceErrorFunction>;
+using VertexVertexDistanceErrorFunction_u = ::std::unique_ptr<VertexVertexDistanceErrorFunction>;
+using VertexVertexDistanceErrorFunction_w = ::std::weak_ptr<VertexVertexDistanceErrorFunction>;
+using VertexVertexDistanceErrorFunction_const_p =
+    ::std::shared_ptr<const VertexVertexDistanceErrorFunction>;
+using VertexVertexDistanceErrorFunction_const_u =
+    ::std::unique_ptr<const VertexVertexDistanceErrorFunction>;
+using VertexVertexDistanceErrorFunction_const_w =
+    ::std::weak_ptr<const VertexVertexDistanceErrorFunction>;
+
+using VertexVertexDistanceErrorFunctiond_p = ::std::shared_ptr<VertexVertexDistanceErrorFunctiond>;
+using VertexVertexDistanceErrorFunctiond_u = ::std::unique_ptr<VertexVertexDistanceErrorFunctiond>;
+using VertexVertexDistanceErrorFunctiond_w = ::std::weak_ptr<VertexVertexDistanceErrorFunctiond>;
+using VertexVertexDistanceErrorFunctiond_const_p =
+    ::std::shared_ptr<const VertexVertexDistanceErrorFunctiond>;
+using VertexVertexDistanceErrorFunctiond_const_u =
+    ::std::unique_ptr<const VertexVertexDistanceErrorFunctiond>;
+using VertexVertexDistanceErrorFunctiond_const_w =
+    ::std::weak_ptr<const VertexVertexDistanceErrorFunctiond>;
 
 } // namespace momentum