Eigen and Pose Transform fixes

README.md

@@ -12,6 +12,7 @@ Prerequisites
 - [Boost](http://www.boost.org/) >= 1.50 (Ubuntu: `sudo apt-get install libboost-all-dev`), portable C++ source libraries.
 - [GTSAM](https://github.com/borglab/gtsam) >= 4.0 alpha, a C++ library that implement smoothing and mapping (SAM) framework in robotics and vision.
 Here we use factor graph implementations and inference/optimization tools provided by GTSAM.
+Do not forget to install GTSAM MATLAB wrappers!
 
 Compilation & Installation
 ------
@@ -32,20 +33,24 @@ Matlab Toolbox
 An optional Matlab toolbox is provided to use our library in Matlab. To enable Matlab toolbox during compilation:
 
 ```
-$ cmake -DGPMP2_BUILD_MATLAB_TOOLBOX:OPTION=ON -DGTSAM_TOOLBOX_INSTALL_PATH:PATH=/path/install/toolbox ..
+$ cmake -DGPMP2_BUILD_MATLAB_TOOLBOX:OPTION=ON -DGTSAM_TOOLBOX_INSTALL_PATH:PATH=/path/install/gtsam_toolbox ..
 $ make install
 ```
 
-After you install the Matlab toolbox, don't forget to add `/path/install/toolbox` to your Matlab path.
+where `/path/install/gtsam_toolbox` is the path where the GTSAM matlab toolbox is installed (usually at `/usr/local/gtsam_toolbox`).
+After you install the Matlab toolbox, don't forget to add `/path/install/gtsam_toolbox` to your Matlab path.
 
 Tested Compatibility
 -----
 
 The gpmp2 library is designed to be cross-platform. It has been tested on Ubuntu Linux and Windows for now.
 
-- Ubuntu: GCC 4.8 - 4.9, 5.3 - 5.4
+- Ubuntu: GCC 4.8 - 4.9, 5.3 - 5.4, 7.5.0
 - Windows: Visual C++ 2015 (Matlab toolbox not tested)
-- Boost: 1.50 - 1.61
+- Boost: 1.50 - 1.61, 1.65.1
+- CMake: 3.18.4
+- Matlab: R2020b
+- GTSAM: 4.0.3
 
 
 Questions & Bug reporting

gpmp2/kinematics/Arm.cpp

@@ -50,13 +50,13 @@ void Arm::forwardKinematics(
   if (J_jvx_jv) J_jvx_jv->assign(dof(), Matrix::Zero(3, dof()));
 
   // variables
-  vector<Matrix4> H(dof());
-  vector<Matrix4> Ho(dof()+1); // start from 1
+  vector<Matrix4, Eigen::aligned_allocator<Eigen::Matrix4f>> H(dof());
+  vector<Matrix4, Eigen::aligned_allocator<Eigen::Matrix4f>> Ho(dof()+1); // start from 1
   vector<Matrix> J;
   if (jv) J.assign(dof(), Matrix::Zero(3, dof()));
   // vars cached for calculate output Jacobians
-  vector<Matrix4> dH(dof());
-  vector<Matrix4> Hoinv(dof()+1); // start from 1
+  vector<Matrix4, Eigen::aligned_allocator<Eigen::Matrix4f>> dH(dof());
+  vector<Matrix4, Eigen::aligned_allocator<Eigen::Matrix4f>> Hoinv(dof()+1); // start from 1
 
 
   // first iteration
@@ -87,7 +87,7 @@ void Arm::forwardKinematics(
   }
 
   // cache dHoi_dqj (DOF^2 memory), only fill in i >= j since others are all zeros
-  vector<vector<Matrix4> > dHo_dq(dof(), vector<Matrix4>(dof()));
+  vector<vector<Matrix4, Eigen::aligned_allocator<Eigen::Matrix4f>> > dHo_dq(dof(), vector<Matrix4, Eigen::aligned_allocator<Eigen::Matrix4f>>(dof()));
   if (J_jpx_jp || J_jvx_jp)
     for (size_t i = 0; i < dof(); i++)
       for (size_t j = 0; j <= i; j++)

matlab/+gpmp2/plotPlanarMobile2Arms.m

@@ -7,10 +7,10 @@
 
 pose = p.pose();
 % vehicle corners
-corner1 = pose.transform_from(Point2(vehsize(1)/2, vehsize(2)/2));
-corner2 = pose.transform_from(Point2(-vehsize(1)/2, vehsize(2)/2));
-corner3 = pose.transform_from(Point2(-vehsize(1)/2, -vehsize(2)/2));
-corner4 = pose.transform_from(Point2(vehsize(1)/2, -vehsize(2)/2));
+corner1 = pose.transformFrom(Point2(vehsize(1)/2, vehsize(2)/2));
+corner2 = pose.transformFrom(Point2(-vehsize(1)/2, vehsize(2)/2));
+corner3 = pose.transformFrom(Point2(-vehsize(1)/2, -vehsize(2)/2));
+corner4 = pose.transformFrom(Point2(vehsize(1)/2, -vehsize(2)/2));
 
 % vehicle base black lines
 h(1) = plot([corner1.x() corner2.x() corner3.x() corner4.x() corner1.x()], ...

matlab/+gpmp2/plotPlanarMobileArm.m

@@ -7,10 +7,10 @@
 
 pose = p.pose();
 % vehicle corners
-corner1 = pose.transform_from(Point2(vehsize(1)/2, vehsize(2)/2));
-corner2 = pose.transform_from(Point2(-vehsize(1)/2, vehsize(2)/2));
-corner3 = pose.transform_from(Point2(-vehsize(1)/2, -vehsize(2)/2));
-corner4 = pose.transform_from(Point2(vehsize(1)/2, -vehsize(2)/2));
+corner1 = pose.transformFrom(Point2(vehsize(1)/2, vehsize(2)/2));
+corner2 = pose.transformFrom(Point2(-vehsize(1)/2, vehsize(2)/2));
+corner3 = pose.transformFrom(Point2(-vehsize(1)/2, -vehsize(2)/2));
+corner4 = pose.transformFrom(Point2(vehsize(1)/2, -vehsize(2)/2));
 
 % vehicle base black lines
 h(1) = plot([corner1.x() corner2.x() corner3.x() corner4.x() corner1.x()], ...

matlab/+gpmp2/plotPlanarMobileBase.m

@@ -6,10 +6,10 @@
 import gpmp2.*
 
 % vehicle corners
-corner1 = pose.transform_from(Point2(vehsize(1)/2, vehsize(2)/2));
-corner2 = pose.transform_from(Point2(-vehsize(1)/2, vehsize(2)/2));
-corner3 = pose.transform_from(Point2(-vehsize(1)/2, -vehsize(2)/2));
-corner4 = pose.transform_from(Point2(vehsize(1)/2, -vehsize(2)/2));
+corner1 = pose.transformFrom(Point2(vehsize(1)/2, vehsize(2)/2));
+corner2 = pose.transformFrom(Point2(-vehsize(1)/2, vehsize(2)/2));
+corner3 = pose.transformFrom(Point2(-vehsize(1)/2, -vehsize(2)/2));
+corner4 = pose.transformFrom(Point2(vehsize(1)/2, -vehsize(2)/2));
 
 % vehicle base black lines
 h(1) = plot([corner1.x() corner2.x() corner3.x() corner4.x() corner1.x()], ...