perf: use eigen affine transform to achieve 3x fk

CMakeLists.txt

@@ -7,13 +7,14 @@ endif(NOT CMAKE_BUILD_TYPE)
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_FLAGS " ${CMAKE_CXX_FLAGS_INIT} -Wall -fPIC")
 
-add_subdirectory("urdf_parser")
-include_directories("third_party" "urdf_parser/include")
-
 find_package(Eigen3 REQUIRED)
 ADD_DEFINITIONS(-DEIGEN_NO_DEBUG)
 include_directories(${EIGEN3_INCLUDE_DIR})
 
+add_subdirectory("urdf_parser")
+include_directories("third_party" "urdf_parser/include")
+
+
 set(TINYFK_SRC src/tinyfk.cpp src/kinematics.cpp)
 include_directories(include/)
 add_library(tinyfk STATIC ${TINYFK_SRC})
@@ -61,6 +62,10 @@ endif(BENCH_KDL)
 add_executable(bench_tinyfk bench/bench_tinyfk.cpp)
 target_link_libraries(bench_tinyfk tinyfk)
 
+# benchmark (tinyfk)
+add_executable(bench_eigen bench/eigen_transform.cpp)
+target_link_libraries(bench_eigen tinyfk)
+
 option(BUILD_PYTHON_INTERFACE "Set when you want to build python interface" ON)
 if(BUILD_PYTHON_INTERFACE)
     option(INSTALL_VIA_PIP "if you install via pip install . " ON)

bench/bench_tinyfk.cpp

@@ -1,3 +1,4 @@
+#include <Eigen/Geometry>
 #include <fstream>
 #include <iostream>
 #include <memory>
@@ -23,7 +24,7 @@ void benchmark_fk(KinematicModel &kin, size_t n_iter,
   const std::vector<double> q = {0, 0, 0, 0, 0, 0, 0, 0};
   {
     clock_t start = clock();
-    urdf::Pose out;
+    Eigen::Affine3d out;
     for (size_t i = 0; i < n_iter; i++) {
       kin.set_joint_angles(joint_ids, q); // this clear cached TFs
       for (int lid : link_ids) {
@@ -36,7 +37,7 @@ void benchmark_fk(KinematicModel &kin, size_t n_iter,
 
   {
     clock_t start = clock();
-    urdf::Pose out;
+    Eigen::Affine3d out;
     for (size_t i = 0; i < n_iter; i++) {
       kin.set_joint_angles(joint_ids, q); // this clear cached TFs
       for (size_t j = 0; j < link_ids.size(); j++) {

bench/eigen_transform.cpp

@@ -0,0 +1,121 @@
+#include "urdf_model/pose.h"
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <chrono>
+#include <iostream>
+#include <tinyfk.hpp>
+
+struct EigenTransform {
+  Eigen::Vector3d translation;
+  Eigen::Quaterniond rotation;
+
+  EigenTransform operator*(const EigenTransform &other) const {
+    Eigen::Quaterniond &&rot = this->rotation * other.rotation;
+    Eigen::Vector3d &&trans =
+        this->rotation * other.translation + this->translation;
+    return EigenTransform{trans, rot};
+  }
+
+  Eigen::Vector3d operator*(const Eigen::Vector3d &other) const {
+    return this->rotation * other + this->translation;
+  }
+
+  EigenTransform inverse() const {
+    EigenTransform result;
+    result.rotation = this->rotation.inverse();
+    result.translation = -(result.rotation * this->translation);
+    return result;
+  }
+
+  EigenTransform inverse_in_place() {
+    this->rotation = this->rotation.inverse();
+    this->translation = -(this->rotation * this->translation);
+    return *this;
+  }
+
+  tinyfk::Transform toUrdf() const {
+    tinyfk::Transform pose;
+    tinyfk::Rotation rot{this->rotation.x(), this->rotation.y(),
+                         this->rotation.z(), this->rotation.w()};
+    tinyfk::Vector3 vec{this->translation.x(), this->translation.y(),
+                        this->translation.z()};
+    pose.rotation = rot;
+    pose.position = vec;
+    return pose;
+  }
+
+  friend std::ostream &operator<<(std::ostream &os, const EigenTransform &t) {
+    os << "Translation: " << t.translation.transpose() << std::endl;
+    os << "Rotation: " << t.rotation.coeffs().transpose() << std::endl;
+    return os;
+  }
+};
+
+EigenTransform urdf_to_eigen(const tinyfk::Transform &pose) {
+  Eigen::Vector3d translation{pose.position.x, pose.position.y,
+                              pose.position.z};
+  Eigen::Quaterniond rotation{pose.rotation.w, pose.rotation.x, pose.rotation.y,
+                              pose.rotation.z};
+  return EigenTransform{translation, rotation};
+}
+
+int main() {
+  auto rot1 =
+      Eigen::Quaterniond{Eigen::AngleAxisd{0.3, Eigen::Vector3d::UnitX()}};
+  auto rot2 =
+      Eigen::Quaterniond{Eigen::AngleAxisd{0.6, Eigen::Vector3d::UnitY()}};
+  auto t1 = EigenTransform{Eigen::Vector3d{0.1, 0, 0}, rot1};
+  auto t2 = EigenTransform{Eigen::Vector3d{0, 0.2, 0.3}, rot2};
+  auto tt1 = t1.toUrdf();
+  auto tt2 = t2.toUrdf();
+
+  // use eigen affine transform
+  Eigen::Affine3d ttt1;
+  ttt1.translation() = t1.translation;
+  ttt1.linear() = t1.rotation.toRotationMatrix();
+  Eigen::Affine3d ttt2;
+  ttt2.translation() = t2.translation;
+  ttt2.linear() = t2.rotation.toRotationMatrix();
+
+  // apply transform t2 to t1 10000 times
+  auto start = std::chrono::high_resolution_clock::now();
+  for (int i = 0; i < 100000000; i++) {
+    t1 = std::move(t1 * t2);
+  }
+  auto end = std::chrono::high_resolution_clock::now();
+  std::cout << "Eigen Transform: "
+            << std::chrono::duration_cast<std::chrono::milliseconds>(end -
+                                                                     start)
+                   .count()
+            << "ms" << std::endl;
+  std::cout << t1 << std::endl;
+
+  // apply transform tt2 to tt1 10000 times
+  start = std::chrono::high_resolution_clock::now();
+  for (int i = 0; i < 100000000; i++) {
+    tt1 = std::move(urdf::pose_transform(tt1, tt2));
+  }
+  end = std::chrono::high_resolution_clock::now();
+  std::cout << "tinyfk Transform: "
+            << std::chrono::duration_cast<std::chrono::milliseconds>(end -
+                                                                     start)
+                   .count()
+            << "ms" << std::endl;
+  std::cout << urdf_to_eigen(tt1) << std::endl;
+
+  // apply transform ttt2 to ttt1 10000 times
+  start = std::chrono::high_resolution_clock::now();
+  for (int i = 0; i < 100000000; i++) {
+    ttt1 = std::move(ttt1 * ttt2);
+  }
+  end = std::chrono::high_resolution_clock::now();
+  std::cout << "Affine Transform: "
+            << std::chrono::duration_cast<std::chrono::milliseconds>(end -
+                                                                     start)
+                   .count()
+            << "ms" << std::endl;
+  std::cout << "Translation: " << ttt1.translation().transpose() << std::endl;
+  std::cout << "Rotation: "
+            << Eigen::Quaterniond{ttt1.linear()}.coeffs().transpose()
+            << std::endl;
+}

include/tinyfk.hpp

@@ -10,7 +10,8 @@ tinyfk: https://github.com/HiroIshida/tinyfk
 #include "urdf_model/joint.h"
 #include "urdf_model/pose.h"
 #include "urdf_parser/urdf_parser.h"
-#include <Eigen/Core> // slow compile...
+#include <Eigen/Core>     // slow compile...
+#include <Eigen/Geometry> // slow compile...
 #include <array>
 #include <assert.h>
 #include <fstream>
@@ -47,7 +48,7 @@ struct RelevancePredicateTable {
 
 struct LinkIdAndTransform {
   size_t id;
-  Transform pose;
+  Eigen::Affine3d pose;
 };
 
 enum class RotationType { IGNORE, RPY, XYZW };
@@ -65,13 +66,13 @@ class KinematicModel {
   std::vector<urdf::JointSharedPtr> joints_;
   std::unordered_map<std::string, int> joint_ids_;
   std::vector<double> joint_angles_;
-  Transform base_pose_;
+  Eigen::Affine3d base_pose_;
 
   RelevancePredicateTable rptable_;
   int num_dof_;
 
   mutable SizedStack<LinkIdAndTransform> transform_stack_;
-  mutable SizedCache<Transform> transform_cache_;
+  mutable SizedCache<Eigen::Affine3d> transform_cache_;
 
 public: // functions
   KinematicModel(const std::string &xml_string);
@@ -122,7 +123,7 @@ class KinematicModel {
     return link_names;
   }
 
-  void get_link_pose(size_t link_id, Transform &out_tf_root_to_ef) const;
+  void get_link_pose(size_t link_id, Eigen::Affine3d &out_tf_root_to_ef) const;
 
   Eigen::MatrixXd get_jacobian(size_t elink_id,
                                const std::vector<size_t> &joint_ids,
@@ -149,11 +150,17 @@ class KinematicModel {
                                    size_t parent_id, const Transform &pose);
 
 private:
-  void get_link_pose_inner(size_t link_id, Transform &out_tf_root_to_ef) const;
+  void get_link_pose_inner(size_t link_id,
+                           Eigen::Affine3d &out_tf_root_to_ef) const;
   void update_rptable();
 };
 
 std::string load_urdf(const std::string &urdf_path);
+
+Eigen::Affine3d urdf_pose_to_eigen_affine3d(const urdf::Pose &pose);
+
+urdf::Pose eigen_affine3d_to_urdf_pose(const Eigen::Affine3d &affine);
+
 }; // namespace tinyfk
 
 #endif // include guard