From 8e6dc747ea15b91e2b633fb9a5970bc164b1030c Mon Sep 17 00:00:00 2001 From: Fernando Barbosa Date: Fri, 22 Jan 2021 10:55:37 +0100 Subject: [PATCH 1/6] Fixed eigen matrix at Arm.cpp --- gpmp2/kinematics/Arm.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gpmp2/kinematics/Arm.cpp b/gpmp2/kinematics/Arm.cpp index bdd4177..b18e152 100644 --- a/gpmp2/kinematics/Arm.cpp +++ b/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 H(dof()); - vector Ho(dof()+1); // start from 1 + vector> H(dof()); + vector> Ho(dof()+1); // start from 1 vector J; if (jv) J.assign(dof(), Matrix::Zero(3, dof())); // vars cached for calculate output Jacobians - vector dH(dof()); - vector Hoinv(dof()+1); // start from 1 + vector> dH(dof()); + vector> 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 > dHo_dq(dof(), vector(dof())); + vector> > dHo_dq(dof(), vector>(dof())); if (J_jpx_jp || J_jvx_jp) for (size_t i = 0; i < dof(); i++) for (size_t j = 0; j <= i; j++) From f9c9161757d8be2a3831c7bf38ec85f743c44e80 Mon Sep 17 00:00:00 2001 From: Fernando Barbosa Date: Fri, 22 Jan 2021 10:57:27 +0100 Subject: [PATCH 2/6] Updated gtsam method transformFrom --- matlab/+gpmp2/plotPlanarMobile2Arms.m | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/matlab/+gpmp2/plotPlanarMobile2Arms.m b/matlab/+gpmp2/plotPlanarMobile2Arms.m index 0f5d132..843f5fb 100644 --- a/matlab/+gpmp2/plotPlanarMobile2Arms.m +++ b/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()], ... From 732dd2167d36e37fbae99d07b36fae161439a86c Mon Sep 17 00:00:00 2001 From: Fernando Barbosa Date: Fri, 22 Jan 2021 10:57:51 +0100 Subject: [PATCH 3/6] Updated gtsam method transformFrom --- matlab/+gpmp2/plotPlanarMobileArm.m | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/matlab/+gpmp2/plotPlanarMobileArm.m b/matlab/+gpmp2/plotPlanarMobileArm.m index 7f5c422..ada04cc 100644 --- a/matlab/+gpmp2/plotPlanarMobileArm.m +++ b/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()], ... From e45acacb4982e7c3bdbadbb027659c676ac883cc Mon Sep 17 00:00:00 2001 From: Fernando Barbosa Date: Fri, 22 Jan 2021 10:58:18 +0100 Subject: [PATCH 4/6] Updated gtsam method transformFrom --- matlab/+gpmp2/plotPlanarMobileBase.m | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/matlab/+gpmp2/plotPlanarMobileBase.m b/matlab/+gpmp2/plotPlanarMobileBase.m index 7bdad22..3bd5af9 100644 --- a/matlab/+gpmp2/plotPlanarMobileBase.m +++ b/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()], ... From 257138fdf49cc260abb7acf828efa065c7b5bbea Mon Sep 17 00:00:00 2001 From: Fernando Barbosa Date: Fri, 22 Jan 2021 11:03:47 +0100 Subject: [PATCH 5/6] Installation clarifications --- README.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index bf61beb..90b20d8 100644 --- a/README.md +++ b/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,11 +33,12 @@ 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 ----- From b96c972a52a97d6b80d4369fbf52f3576a958608 Mon Sep 17 00:00:00 2001 From: Fernando Barbosa Date: Fri, 22 Jan 2021 11:07:58 +0100 Subject: [PATCH 6/6] Added info on tested compatibility --- README.md | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 90b20d8..bad8d84 100644 --- a/README.md +++ b/README.md @@ -45,9 +45,12 @@ 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