Updating MATLAB Toolbox Wrapping + Integration

[mattking-smith]

Description

I have been working on using the current GTSAM wrap repository for integrating this toolbox into MATLAB for research purposes. I was able to successfully do this some time ago on another development computer, but the wrap repository has changed since then, and I am now facing a segmentation fault errors in the MATLAB environment, when running a few of the examples, post toolbox wrapping.

Additionally, I hope this issue provides information for new documentation on an up-to-date MATLAB + GPMP2 toolbox wrapping procedure on the README.md.

Steps to reproduce current wrapping method

Wrapping GTSAM to MATLAB Toolbox

1. Build GTSAM from source, clone or download the latest release from the GTSAM Github repo. Then follow the build & install instructions posted on GTSAM build page.
2. To flag the MATLAB toolbox for to build in the \home\user\ directory in the folder \toolbox, before the build process, in the terminal, run the command

cmake -DGTSAM_TOOLBOX_INSTALL_PATH:PATH= $HOME/toolbox ..`

3. Install GTSAM by running the command make install
4. Follow the step given in the MATLAB Wrapper documentation.

Wrapping GPMP2 to MATLAB Toolbox

1. In the command prompt run

git clone https://github.com/gtrll/gpmp2.git 

2. Download and install the current matlab.h from the GTSAM wrap repository into a wrap folder within the gpmp2 folder.

sudo mkdir wrap

3. Noting the MATLAB wrapper vdocumentation in the current GTSAM wrap repository , and after some discussion with the developers of the wrap repository I needed to make edits to the CMakeLists.txt in the main gpmp2 repository. Specifically, I changes the following lines:

   gpmp2/CMakeLists.txt

   Lines 65 to 74 in 48c41c1

   	# Wrapping to MATLAB
   	if(GPMP2_BUILD_MATLAB_TOOLBOX)
   	# wrap
   	include(GtsamMatlabWrap)
   	wrap_and_install_library(gpmp2.h ${PROJECT_NAME} "${CMAKE_CURRENT_SOURCE_DIR}" "")
   	# install matlab functions and scripts
   	add_subdirectory(matlab)
   	endif()

   
   to

# Find current wrap repository
find_package(gtwrap)

# Wrapping to MATLAB
if(GPMP2_BUILD_MATLAB_TOOLBOX)

  include(MatlabWrap)
  matlab_wrap(gpmp2.h ${PROJECT_NAME} "gtsam;${PROJECT_NAME}" "" "" "")
  
  # install matlab functions and scripts
  add_subdirectory(matlab)
endif()

4. To flag for installing the MATLAB toolbox and a path for the toolbox to be installed (I have chosen to install in the /home/user/toolbox directory), in the command prompt run

cd gpmp2 && mkdir build && cd build
sudo cmake -DGPMP2_BUILD_MATLAB_TOOLBOX:=ON -DWRAP_TOOLBOX_INSTALL_PATH=$HOME/gpmp2_toolbox ..

5. Now executing sudo make install in the command prompt resulted initially in the following errors:

In file included from /usr/local/gpmp2/gpmp2/kinematics/GaussianPriorWorkspacePositionArm.h:12,
                 from /usr/local/gpmp2/build/wrap/gpmp2/gpmp2_wrapper.cpp:20:
/usr/local/gpmp2/gpmp2/kinematics/GaussianPriorWorkspacePosition.h: In member function ‘gtsam::Vector gpmp2::GaussianPriorWorkspacePosition<ROBOT>::evaluateError(const Pose&, boost::optional<Eigen::Matrix<double, -1, -1>&>) const’:
/usr/local/gpmp2/gpmp2/kinematics/GaussianPriorWorkspacePosition.h:66:28: error: ‘gtsam::Point3’ {aka ‘class Eigen::Matrix<double, 3, 1>’} has no member named ‘vector’
   66 |       return curr_position.vector() - des_position_.vector();
      |                            ^~~~~~
/usr/local/gpmp2/gpmp2/kinematics/GaussianPriorWorkspacePosition.h:66:53: error: ‘const Point3’ {aka ‘const class Eigen::Matrix<double, 3, 1>’} has no member named ‘vector’
   66 |       return curr_position.vector() - des_position_.vector();
      |                                                     ^~~~~~
/usr/local/gpmp2/gpmp2/kinematics/GaussianPriorWorkspacePosition.h:69:46: error: ‘const Point3’ {aka ‘const class Eigen::Matrix<double, 3, 1>’} has no member named ‘vector’
   69 |       return joint_pos[joint_].translation().vector() - des_position_.vector();
      |                                              ^~~~~~
/usr/local/gpmp2/gpmp2/kinematics/GaussianPriorWorkspacePosition.h:69:71: error: ‘const Point3’ {aka ‘const class Eigen::Matrix<double, 3, 1>’} has no member named ‘vector’
   69 |       return joint_pos[joint_].translation().vector() - des_position_.vector();
      |                                                                       ^~~~~~
.
.
.

make[2]: *** [CMakeFiles/gpmp2_matlab_wrapper.dir/build.make:79: CMakeFiles/gpmp2_matlab_wrapper.dir/wrap/gpmp2/gpmp2_wrapper.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:401: CMakeFiles/gpmp2_matlab_wrapper.dir/all] Error 2
make: *** [Makefile:141: all] Error 2

which I believe is an error related to an open pull request.

Specifically, it appears that Eigen and Pose Tranform have changed in GTSAM and those changes need to be updated in a few key header files. Without specifically without naming all of the header files I changed here in this issues, as I will open a pull request after this issue submission, note that I had to remove the functions .vector() and .print() in order to successfully compile and wrap.

6. After removing .vector() and .print() from key header files, again run sudo make install in the command prompt.
7. Note that for my system I was originally having a mex linker problem as discussed on the GTSAM MATLAB section, so in order to satisfy MATLAB when using GTSAM + GPMP2 once wrapped, I run the following at the command line to open MATLAB

export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6; matlab

8. Once in MATLAB make sure to add the file paths to the toolboxes

addpath('/home/user/toolbox');
addpath('/home/user/gpmp2_toolbox');

which should be sufficient for running the toolbox with both GTSAM + GPMP2.

Outstanding Issues

Currently I am able to successfully run a handful of examples without issue, like PointRobot2DFactorGraphExample.m, but I am getting a segmentation fault in one crucial function, arm.forwardKinematicsPosition() (which is called in many plotting functions likes PlotPlanarArm.m), is possibly related to the altering of the header files necessary for building, as I was mentioning previously.

For instance, if I run >> Arm3FactorGraphExample in the MATLAB command prompt, I get the following segmentation error:

------------------------------------------------
MATLAB Log File
------------------------------------------------ 


--------------------------------------------------------------------------------
          Segmentation violation detected at 2021-12-12 11:30:43 -0500
--------------------------------------------------------------------------------

Configuration:
  Crash Decoding           : Disabled - No sandbox or build area path
  Crash Mode               : continue (default)
  Default Encoding         : UTF-8
  Deployed                 : false
  Desktop Environment      : ubuntu:GNOME
  GNU C Library            : 2.31 stable
  Graphics Driver          : Intel Mesa Intel(R) HD Graphics 5500 (BDW GT2) Version 4.6 (Compatibility Profile) Mesa 21.0.3
  Graphics card 1          : Not Started 0x10de ( 0x10de ) 0x1346 Version 0.0.0.0 (0-0-0)
  Graphics card 2          : 0x8086 ( 0x8086 ) 0x1616 Version 0.0.0.0 (0-0-0)
  Java Version             : Java 1.8.0_202-b08 with Oracle Corporation Java HotSpot(TM) 64-Bit Server VM mixed mode
  MATLAB Architecture      : glnxa64
  MATLAB Entitlement ID    : 2075860
  MATLAB Root              : /usr/local/MATLAB/R2021a
  MATLAB Version           : 9.10.0.1684407 (R2021a) Update 3
  OpenGL                   : hardware
  Operating System         : Ubuntu 20.04.3 LTS
  Process ID               : 91269
  Processor ID             : x86 Family 6 Model 61 Stepping 4, GenuineIntel
  Session Key              : f4ed3dee-2797-450d-91e9-c762785e1bfc
  Static TLS mitigation    : Enabled: Full
  Window System            : The X.Org Foundation (12011000), display :0

Fault Count: 1


Abnormal termination:
Segmentation violation

Current Thread: 'MCR 0 interpret' id 140577137805056

Register State (from fault):
  RAX = 00007fda2bb51ac0  RBX = 00007fdaaa5c6440
  RCX = 0000000000000000  RDX = 0000000000000018
  RSP = 00007fdaaa5c6078  RBP = 00007fdaaa5c6590
  RSI = 00007fda29c821d0  RDI = 00007fda2bb51ac0

   R8 = 00007fdaaa5c64a0   R9 = 00007fdaaa5c6480
  R10 = 00007fdaaa5c6020  R11 = 0000000000000000
  R12 = 0000000000000000  R13 = 00007fdaaa5c6690
  R14 = 00007fd9d7d2da70  R15 = 00007fdaaa5c65f0

  RIP = 00007fd9cf34a15a  EFL = 0000000000010206

   CS = 0033   FS = 0000   GS = 0000

Stack Trace (from fault):
[  0] 0x00007fd9cf34a15a                       /usr/local/lib/libgpmp2.so.0+00528730 _ZN5Eigen8internal21compute_inverse_size4ILi1EdNS_6MatrixIdLi4ELi4ELi0ELi4ELi4EEES3_E3runERKS3_RS3_+00001354
[  1] 0x00007fd9cf346586                       /usr/local/lib/libgpmp2.so.0+00513414
.
.
.

The forwardKinematicsPosition() is defined in the ForwardKinematics-inl.h, where we can see .vector() is used as follows:

gpmp2/gpmp2/kinematics/ForwardKinematics-inl.h

Line 25 in 48c41c1

jpx[i].translation().vector()).finished();

gpmp2/gpmp2/kinematics/ForwardKinematics-inl.h

Line 39 in 48c41c1

jpx_mat.col(i) = jpx[i].translation().vector();

So in my code, I explicitly removed .vector() from each of these lines in order to successfully compile and wrap. However now whenever MATLAB tries to execute arm.forwardKinematicsPosition() I get a segmentation faults when I get the program gets to a custom wrapper definition generated in the toolbox:

    function varargout = forwardKinematicsPosition(this, varargin)
      % FORWARDKINEMATICSPOSITION usage: forwardKinematicsPosition(Pose2Vector jp) : returns Matrix
      % Doxygen can be found at https://gtsam.org/doxygen/
      if length(varargin) == 1 && isa(varargin{1},'gpmp2.Pose2Vector')
        varargout{1} = gpmp2_wrapper(56, this, varargin{:});
        return
      end
      error('Arguments do not match any overload of function gpmp2.Pose2MobileArm.forwardKinematicsPosition');
    end

I am sure this is result of editing ForwardKinematics-inl.h](https://github.com/gtrll/gpmp2/blob/main/gpmp2/kinematics/ForwardKinematics-inl.h), but I am not sure what edits need to made to allow me to compile + wrap and work in MATLAB.

Any suggestions or edits on this matter would be greatly appreciated.

Also if someone could verify this install method, that would be beneficial too.

Environment

Linux OS: Ubuntu 20.04
MATLAB version: 2021a
Standard GTSAM + GPMP2 + MATLAB Toolbox build procedure.

Additional information

I do current have a fully working (i.e. no segmentation faults on any GPMP2 example) development computer which has successfully wrapped and working GTSAM + GPMP2 for the MATLAB environment, but that was done some time ago and that code is out of sync with GPMP2. Hence I have not pulled the current repositories onto the development computer and have been testing this MATLAB toolbox integration on a different computer.

[mattking-smith]

Segmentation Fault Update:

After looking at some previously closed issues and comments, (#29 (comment)) I ended up needing to add Eigen::aligned_allocator<Eigen::Matrix4d to keys lines in the Arm.cpp code to get over working on the current forwardKinematics segmentation fault. These changes can be see in the most recent PR push: 9f10f84.

Although these changes seemed to have fixed running commands like arm.forwardKinematicsPosition(), I am now finding segmentation faults when calling optimizer.optimize();:

MATLAB Log File: /home/matt/matlab_crash_dump.128916-1

------------------------------------------------
MATLAB Log File
------------------------------------------------ 


--------------------------------------------------------------------------------
                Assertion detected at 2021-12-13 16:38:23 -0500
--------------------------------------------------------------------------------

Configuration:
  Crash Decoding           : Disabled - No sandbox or build area path
  Crash Mode               : continue (default)
  Default Encoding         : UTF-8
  Deployed                 : false
  Desktop Environment      : ubuntu:GNOME
  GNU C Library            : 2.31 stable
  Graphics Driver          : Intel Mesa Intel(R) HD Graphics 5500 (BDW GT2) Version 4.6 (Compatibility Profile) Mesa 21.0.3
  Graphics card 1          : Not Started 0x10de ( 0x10de ) 0x1346 Version 0.0.0.0 (0-0-0)
  Graphics card 2          : 0x8086 ( 0x8086 ) 0x1616 Version 0.0.0.0 (0-0-0)
  Java Version             : Java 1.8.0_202-b08 with Oracle Corporation Java HotSpot(TM) 64-Bit Server VM mixed mode
  MATLAB Architecture      : glnxa64
  MATLAB Entitlement ID    : 2075860
  MATLAB Root              : /usr/local/MATLAB/R2021a
  MATLAB Version           : 9.10.0.1684407 (R2021a) Update 3
  OpenGL                   : hardware
  Operating System         : Ubuntu 20.04.3 LTS
  Process ID               : 128916
  Processor ID             : x86 Family 6 Model 61 Stepping 4, GenuineIntel
  Session Key              : 3e4bec99-ac1d-440c-9c41-4de0e5307aee
  Static TLS mitigation    : Enabled: Full
  Window System            : The X.Org Foundation (12011000), display :0

Fault Count: 1


Assertion in find at management.cpp line 761:
find: no active context for type '(anonymous namespace)::AlreadyReportedFailure'

Current Thread: 'MCR 0 interpret' id 140450581174016

Register State (captured):
  RAX = 00007fbd32ffac90  RBX = 00007fbda020cb68
  RCX = 000000000000000a  RDX = 00007fbd32ffb330
  RSP = 00007fbd32ffab00  RBP = 00007fbd32ffaee0
  RSI = 00007fbda01d452a  RDI = 00007fbd32ffab10

   R8 = 0000000000000000   R9 = 00007fbd32ffade0
  R10 = 000000000000000a  R11 = 00007fbd32ffb330
  R12 = 00007fbda01fcae8  R13 = 00007fbd9f452c63
  R14 = 00007fbd9f453d63  R15 = 00007fbd32ffb7f0

  RIP = 00007fbda017580e  EFL = 0000000000000002

   CS = d139   FS = a0ba   GS = 7fbd

Stack Trace (captured):
[  0] 0x00007fbda016a923    /usr/local/MATLAB/R2021a/bin/glnxa64/libmwfl.so+00387363
[  1] 0x00007fbda016ab1c    /usr/local/MATLAB/R2021a/bin/glnxa64/libmwfl.so+00387868 _ZN10foundation4core4diag15stacktrace_base7captureEm+00000028
[  2] 0x00007fbda016ec45    /usr/local/MATLAB/R2021a/bin/glnxa64/libmwfl.so+00404549
[  3] 0x00007fbda016ed80    /usr/local/MATLAB/R2021a/bin/glnxa64/libmwfl.so+00404864
[  4] 0x00007fbd9f44252e /usr/local/MATLAB/R2021a/bin/glnxa64/libmwfoundation_usm.so+00087342
[  5] 0x00007fbd9f450a48 /usr/local/MATLAB/R2021a/bin/glnxa64/libmwfoundation_usm.so+00145992 _ZN10foundation3usm6DetailINS0_5scope3MvmEE4findEmRKSt9type_info+00000136
[  6] 0x00007fbd8c0b25db /usr/local/MATLAB/R2021a/bin/glnxa64/libmwbridge.so+00275931
[  7] 0x00007fbda0281b69      /usr/local/MATLAB/R2021a/bin/glnxa64/libut.so+00473961 utVprintf+00000185
[  8] 0x00007fbd685829eb     /usr/local/MATLAB/R2021a/bin/glnxa64/libmex.so+00723435 mexPrintf+00000139
[  9] 0x00007fbc6fa5f04d            /home/matt/toolbox/gtsam_wrapper.mexa64+20148301 _ZN7mstream6xsputnEPKcl+00000029
[ 10] 0x00007fbda1704824               /lib/x86_64-linux-gnu/libstdc++.so.6+01259556 _ZSt16__ostream_insertIcSt11char_traitsIcEERSt13basic_ostreamIT_T0_ES6_PKS3_l+00000404
[ 11] 0x00007fbc728b57b3                       /usr/local/lib/libgpmp2.so.0+00518067 _ZNK5gpmp23Arm17forwardKinematicsERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEN5boost8optionalIS5_EERSt6vectorIN5gtsam5Pose3ESaISB_EENS7_IRS9_INS2_IdLi3ELi1ELi0ELi3ELi1EEESaISF_EEEENS7_IRS9_INS2_IdLin1ELin1ELi0ELin1ELin1EEESaISK_EEEESO_SO_+00000147
[ 12] 0x00007fbc72b0dd31      /home/matt/gpmp2_toolbox/gpmp2_wrapper.mexa64+01473841 _ZNK5gpmp210RobotModelINS_3ArmEE13sphereCentersERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEERSt6vectorINS4_IdLi3ELi1ELi0ELi3ELi1EEESaIS9_EEN5boost8optionalIRS8_INS4_IdLin1ELin1ELi0ELin1ELin1EEESaISF_EEEE+00000225
[ 13] 0x00007fbc72b1c11b      /home/matt/gpmp2_toolbox/gpmp2_wrapper.mexa64+01532187 _ZNK5gpmp223ObstaclePlanarSDFFactorINS_10RobotModelINS_3ArmEEEE13evaluateErrorERKN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEN5boost8optionalIRNS6_IdLin1ELin1ELi0ELin1ELin1EEEEE+00000235
[ 14] 0x00007fbc72b4342c      /home/matt/gpmp2_toolbox/gpmp2_wrapper.mexa64+01692716 _ZNK5gtsam17NoiseModelFactor1IN5Eigen6MatrixIdLin1ELi1ELi0ELin1ELi1EEEE15unwhitenedErrorERKNS_6ValuesEN5boost8optionalIRSt6vectorINS2_IdLin1ELin1ELi0ELin1ELin1EEESaISB_EEEE+00000124
[ 15] 0x00007fbc726924c3                       /usr/local/lib/libgtsam.so.4+04072643 _ZNK5gtsam16NoiseModelFactor9linearizeERKNS_6ValuesE+00000419
[ 16] 0x00007fbc72693bf4                       /usr/local/lib/libgtsam.so.4+04078580
[ 17] 0x00007fbc72694178                       /usr/local/lib/libgtsam.so.4+04079992
[ 18] 0x00007fbd9e1c55f9   /usr/local/MATLAB/R2021a/bin/glnxa64/libtbb.so.2+00173561
[ 19] 0x00007fbd9e1bf69e   /usr/local/MATLAB/R2021a/bin/glnxa64/libtbb.so.2+00149150
[ 20] 0x00007fbd9e1bd67c   /usr/local/MATLAB/R2021a/bin/glnxa64/libtbb.so.2+00140924
[ 21] 0x00007fbd9e1b8a46   /usr/local/MATLAB/R2021a/bin/glnxa64/libtbb.so.2+00121414
[ 22] 0x00007fbd9e1b89d6   /usr/local/MATLAB/R2021a/bin/glnxa64/libtbb.so.2+00121302
[ 23] 0x00007fbda17be609              /lib/x86_64-linux-gnu/libpthread.so.0+00038409
[ 24] 0x00007fbda14e6293                    /lib/x86_64-linux-gnu/libc.so.6+01188499 clone+00000067

Note that adding print statements into the forwardKinematics() shows that GPMP2 fails at different points when running optimizer.optimize(); in the function forwardKinematics().

[mattking-smith]

I have successfully wrapped GPMP2 into the MATLAB environment.

I was able achieve this by looking at the tag number of the repository which I had originally successfully wrapped the GPMP2 library into on my currently working development computer. I found that the gpmp2.h file had changed between tags 0.3.0 and 0.2.1. So by reverting to the gpmp2.h from tag 0.2.1 to tage 0.2.0 and updating some .m and .h files (which are given in the most recent PR) I am running GPMP2 segmentation free.

[mattking-smith]

I know that reverting to gpmp2.h from tag 0.2.0 is undesirable, so perhaps we could work together to figure out why gpmp2.h from 0.3.0 is causing the segmentation faults @mhmukadam @dongjing3309 @kalyanvasudev.

[wbthomason]

This may be more suitable for a new issue, but I do want to note that the Python wrapper procedure seems to be broken in a similar way, after updates on GTSAM's end.

[mattking-smith]

@wbthomason It is difficult to say if the issue is related or not without opening a new issue.