Merge pull request #953 from borglab/fix/matlab-wrapper

Fix Matlab Wrapper

CMakeLists.txt

@@ -87,6 +87,13 @@ if(GTSAM_BUILD_PYTHON OR GTSAM_INSTALL_MATLAB_TOOLBOX)
         CACHE STRING "The Python version to use for wrapping")
     # Set the include directory for matlab.h
     set(GTWRAP_INCLUDE_NAME "wrap")
+
+    # Copy matlab.h to the correct folder.
+    configure_file(${PROJECT_SOURCE_DIR}/wrap/matlab.h
+               ${PROJECT_BINARY_DIR}/wrap/matlab.h COPYONLY)
+    # Add the include directories so that matlab.h can be found
+    include_directories("${PROJECT_BINARY_DIR}" "${GTSAM_EIGEN_INCLUDE_FOR_BUILD}")
+
     add_subdirectory(wrap)
     list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/wrap/cmake")
 endif()

gtsam/base/base.i

@@ -38,7 +38,7 @@ class DSFMap {
   DSFMap();
   KEY find(const KEY& key) const;
   void merge(const KEY& x, const KEY& y);
-  std::map<KEY, Set> sets();
+  std::map<KEY, This::Set> sets();
 };
 
 class IndexPairSet {

gtsam/basis/basis.i

@@ -140,7 +140,7 @@ class FitBasis {
   static gtsam::GaussianFactorGraph::shared_ptr LinearGraph(
       const std::map<double, double>& sequence,
       const gtsam::noiseModel::Base* model, size_t N);
-  Parameters parameters() const;
+  This::Parameters parameters() const;
 };
 
 }  // namespace gtsam

gtsam/slam/slam.i

@@ -168,7 +168,7 @@ template <POSE>
 virtual class PoseTranslationPrior : gtsam::NoiseModelFactor {
   PoseTranslationPrior(size_t key, const POSE& pose_z,
                        const gtsam::noiseModel::Base* noiseModel);
-  POSE measured() const;
+  POSE::Translation measured() const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -185,7 +185,7 @@ template <POSE>
 virtual class PoseRotationPrior : gtsam::NoiseModelFactor {
   PoseRotationPrior(size_t key, const POSE& pose_z,
                     const gtsam::noiseModel::Base* noiseModel);
-  POSE measured() const;
+  POSE::Rotation measured() const;
 };
 
 typedef gtsam::PoseRotationPrior<gtsam::Pose2> PoseRotationPrior2D;

matlab/+gtsam/VisualISAMInitialize.m

@@ -12,11 +12,11 @@
 isam = ISAM2(params);
 
 %% Set Noise parameters
-noiseModels.pose = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
+noiseModels.pose = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]', true);
 %noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.001 0.001 0.001 0.1 0.1 0.1]');
-noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.05 0.05 0.2 0.2 0.2]');
-noiseModels.point = noiseModel.Isotropic.Sigma(3, 0.1);
-noiseModels.measurement = noiseModel.Isotropic.Sigma(2, 1.0);
+noiseModels.odometry = noiseModel.Diagonal.Sigmas([0.05 0.05 0.05 0.2 0.2 0.2]', true);
+noiseModels.point = noiseModel.Isotropic.Sigma(3, 0.1, true);
+noiseModels.measurement = noiseModel.Isotropic.Sigma(2, 1.0, true);
 
 %% Add constraints/priors
 % TODO: should not be from ground truth!

matlab/CMakeLists.txt

@@ -64,8 +64,21 @@ set(ignore
     gtsam::Point3
     gtsam::CustomFactor)
 
+set(interface_files
+    ${GTSAM_SOURCE_DIR}/gtsam/gtsam.i
+    ${GTSAM_SOURCE_DIR}/gtsam/base/base.i
+    ${GTSAM_SOURCE_DIR}/gtsam/basis/basis.i
+    ${GTSAM_SOURCE_DIR}/gtsam/geometry/geometry.i
+    ${GTSAM_SOURCE_DIR}/gtsam/linear/linear.i
+    ${GTSAM_SOURCE_DIR}/gtsam/nonlinear/nonlinear.i
+    ${GTSAM_SOURCE_DIR}/gtsam/symbolic/symbolic.i
+    ${GTSAM_SOURCE_DIR}/gtsam/sam/sam.i
+    ${GTSAM_SOURCE_DIR}/gtsam/slam/slam.i
+    ${GTSAM_SOURCE_DIR}/gtsam/sfm/sfm.i
+    ${GTSAM_SOURCE_DIR}/gtsam/navigation/navigation.i
+)
 # Wrap
-matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam/gtsam.i "${GTSAM_ADDITIONAL_LIBRARIES}"
+matlab_wrap("${interface_files}" "gtsam" "${GTSAM_ADDITIONAL_LIBRARIES}"
             "" "${mexFlags}" "${ignore}")
 
 # Wrap version for gtsam_unstable

matlab/gtsam_tests/testJacobianFactor.m

@@ -32,7 +32,7 @@
 % the RHS
 b2=[-1;1.5;2;-1];
 sigmas = [1;1;1;1];
-model4 = noiseModel.Diagonal.Sigmas(sigmas);
+model4 = noiseModel.Diagonal.Sigmas(sigmas, true);
 combined = JacobianFactor(x2, Ax2,  l1, Al1, x1, Ax1, b2, model4);
 
 % eliminate the first variable (x2) in the combined factor, destructive !
@@ -74,7 +74,7 @@
 % the RHS
 b1= [0.0;0.894427];
 
-model2 = noiseModel.Diagonal.Sigmas([1;1]);
+model2 = noiseModel.Diagonal.Sigmas([1;1], true);
 expectedLF = JacobianFactor(l1, Bl1, x1, Bx1, b1, model2);
 
 % check if the result matches the combined (reduced) factor

matlab/gtsam_tests/testKalmanFilter.m

@@ -23,13 +23,13 @@
 F = eye(2,2);
 B = eye(2,2);
 u = [1.0; 0.0];
-modelQ = noiseModel.Diagonal.Sigmas([0.1;0.1]);
+modelQ = noiseModel.Diagonal.Sigmas([0.1;0.1], true);
 Q = 0.01*eye(2,2);
 H = eye(2,2);
 z1 = [1.0, 0.0]';
 z2 = [2.0, 0.0]';
 z3 = [3.0, 0.0]';
-modelR = noiseModel.Diagonal.Sigmas([0.1;0.1]);
+modelR = noiseModel.Diagonal.Sigmas([0.1;0.1], true);
 R = 0.01*eye(2,2);
 
 %% Create the set of expected output TestValues

matlab/gtsam_tests/testLocalizationExample.m

@@ -17,7 +17,7 @@
 
 %% Add two odometry factors
 odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true); % 20cm std on x,y, 0.1 rad on theta
 graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise));
 graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise));
 
@@ -27,7 +27,7 @@
 groundTruth.insert(1, Pose2(0.0, 0.0, 0.0));
 groundTruth.insert(2, Pose2(2.0, 0.0, 0.0));
 groundTruth.insert(3, Pose2(4.0, 0.0, 0.0));
-model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10]);
+model = noiseModel.Diagonal.Sigmas([0.1; 0.1; 10], true);
 for i=1:3
     graph.add(PriorFactorPose2(i, groundTruth.atPose2(i), model));
 end

matlab/gtsam_tests/testOdometryExample.m

@@ -17,12 +17,12 @@
 
 %% Add a Gaussian prior on pose x_1
 priorMean = Pose2(0.0, 0.0, 0.0); % prior mean is at origin
-priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]); % 30cm std on x,y, 0.1 rad on theta
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true); % 30cm std on x,y, 0.1 rad on theta
 graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph
 
 %% Add two odometry factors
 odometry = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]); % 20cm std on x,y, 0.1 rad on theta
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true); % 20cm std on x,y, 0.1 rad on theta
 graph.add(BetweenFactorPose2(1, 2, odometry, odometryNoise));
 graph.add(BetweenFactorPose2(2, 3, odometry, odometryNoise));
 

matlab/gtsam_tests/testPlanarSLAMExample.m

@@ -30,18 +30,18 @@
 
 %% Add prior
 priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true);
 graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
 
 %% Add odometry
 odometry = Pose2(2.0, 0.0, 0.0);
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true);
 graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
 graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
 
 %% Add bearing/range measurement factors
 degrees = pi/180;
-brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
+brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2], true);
 graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
 graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
 graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));

matlab/gtsam_tests/testPose2SLAMExample.m

@@ -26,19 +26,19 @@
 %% Add prior
 % gaussian for prior
 priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true);
 graph.add(PriorFactorPose2(1, priorMean, priorNoise)); % add directly to graph
 
 %% Add odometry
 % general noisemodel for odometry
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true);
 graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, 0.0 ), odometryNoise));
 graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, pi/2), odometryNoise));
 graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, pi/2), odometryNoise));
 graph.add(BetweenFactorPose2(4, 5, Pose2(2.0, 0.0, pi/2), odometryNoise));
 
 %% Add pose constraint
-model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
+model = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true);
 graph.add(BetweenFactorPose2(5, 2, Pose2(2.0, 0.0, pi/2), model));
 
 %% Initialize to noisy points

matlab/gtsam_tests/testPose3SLAMExample.m

@@ -21,7 +21,7 @@
 fg = NonlinearFactorGraph;
 fg.add(NonlinearEqualityPose3(0, p0));
 delta = p0.between(p1);
-covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
+covariance = noiseModel.Diagonal.Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180], true);
 fg.add(BetweenFactorPose3(0,1, delta, covariance));
 fg.add(BetweenFactorPose3(1,2, delta, covariance));
 fg.add(BetweenFactorPose3(2,3, delta, covariance));

matlab/gtsam_tests/testSFMExample.m

@@ -25,17 +25,17 @@
 graph = NonlinearFactorGraph;
 
 %% Add factors for all measurements
-measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma);
+measurementNoise = noiseModel.Isotropic.Sigma(2,measurementNoiseSigma, true);
 for i=1:length(data.Z)
     for k=1:length(data.Z{i})
         j = data.J{i}{k};
         graph.add(GenericProjectionFactorCal3_S2(data.Z{i}{k}, measurementNoise, symbol('x',i), symbol('p',j), data.K));
     end
 end
 
-posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
+posePriorNoise  = noiseModel.Diagonal.Sigmas(poseNoiseSigmas, true);
 graph.add(PriorFactorPose3(symbol('x',1), truth.cameras{1}.pose, posePriorNoise));
-pointPriorNoise  = noiseModel.Isotropic.Sigma(3,pointNoiseSigma);
+pointPriorNoise  = noiseModel.Isotropic.Sigma(3,pointNoiseSigma, true);
 graph.add(PriorFactorPoint3(symbol('p',1), truth.points{1}, pointPriorNoise));
 
 %% Initial estimate

matlab/gtsam_tests/testSerialization.m

@@ -45,30 +45,30 @@
 
 % Prior factor
 priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
-priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
+priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1], true);
 graph.add(PriorFactorPose2(i1, priorMean, priorNoise)); % add directly to graph
 
 % Between Factors
 odometry = Pose2(2.0, 0.0, 0.0);
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
+odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1], true);
 graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
 graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));
 
 % Range Factors
-rNoise = noiseModel.Diagonal.Sigmas([0.2]);
+rNoise = noiseModel.Diagonal.Sigmas([0.2], true);
 graph.add(RangeFactor2D(i1, j1, sqrt(4+4), rNoise));
 graph.add(RangeFactor2D(i2, j1, 2, rNoise));
 graph.add(RangeFactor2D(i3, j2, 2, rNoise));
 
 % Bearing Factors
 degrees = pi/180;
-bNoise = noiseModel.Diagonal.Sigmas([0.1]);
+bNoise = noiseModel.Diagonal.Sigmas([0.1], true);
 graph.add(BearingFactor2D(i1, j1, Rot2(45*degrees), bNoise));
 graph.add(BearingFactor2D(i2, j1, Rot2(90*degrees), bNoise));
 graph.add(BearingFactor2D(i3, j2, Rot2(90*degrees), bNoise));
 
 % BearingRange Factors
-brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
+brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2], true);
 graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(4+4), brNoise));
 graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
 graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));

matlab/gtsam_tests/testStereoVOExample.m

@@ -33,7 +33,7 @@
 %% Create realistic calibration and measurement noise model
 % format: fx fy skew cx cy baseline
 K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2);
-stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0]);
+stereo_model = noiseModel.Diagonal.Sigmas([1.0; 1.0; 1.0], true);
 
 %% Add measurements
 % pose 1

wrap/cmake/MatlabWrap.cmake

@@ -62,10 +62,10 @@ macro(find_and_configure_matlab)
 endmacro()
 
 # Consistent and user-friendly wrap function
-function(matlab_wrap interfaceHeader linkLibraries
+function(matlab_wrap interfaceHeader moduleName linkLibraries
          extraIncludeDirs extraMexFlags ignore_classes)
   find_and_configure_matlab()
-  wrap_and_install_library("${interfaceHeader}" "${linkLibraries}"
+  wrap_and_install_library("${interfaceHeader}" "${moduleName}" "${linkLibraries}"
                            "${extraIncludeDirs}" "${extraMexFlags}"
                            "${ignore_classes}")
 endfunction()
@@ -77,6 +77,7 @@ endfunction()
 # Arguments:
 #
 # interfaceHeader:  The relative path to the wrapper interface definition file.
+# moduleName:       The name of the wrapped module, e.g. gtsam
 # linkLibraries:    Any *additional* libraries to link.  Your project library
 # (e.g. `lba`), libraries it depends on, and any necessary MATLAB libraries will
 # be linked automatically.  So normally, leave this empty.
@@ -85,15 +86,15 @@ endfunction()
 # extraMexFlags:    Any *additional* flags to pass to the compiler when building
 # the wrap code.  Normally, leave this empty.
 # ignore_classes:  List of classes to ignore in the wrapping.
-function(wrap_and_install_library interfaceHeader linkLibraries
+function(wrap_and_install_library interfaceHeader moduleName linkLibraries
          extraIncludeDirs extraMexFlags ignore_classes)
-  wrap_library_internal("${interfaceHeader}" "${linkLibraries}"
+  wrap_library_internal("${interfaceHeader}" "${moduleName}" "${linkLibraries}"
                         "${extraIncludeDirs}" "${mexFlags}")
-  install_wrapped_library_internal("${interfaceHeader}")
+  install_wrapped_library_internal("${moduleName}")
 endfunction()
 
 # Internal function that wraps a library and compiles the wrapper
-function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs
+function(wrap_library_internal interfaceHeader moduleName linkLibraries extraIncludeDirs
          extraMexFlags)
   if(UNIX AND NOT APPLE)
     if(CMAKE_SIZEOF_VOID_P EQUAL 8)
@@ -120,7 +121,6 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs
   # Extract module name from interface header file name
   get_filename_component(interfaceHeader "${interfaceHeader}" ABSOLUTE)
   get_filename_component(modulePath "${interfaceHeader}" PATH)
-  get_filename_component(moduleName "${interfaceHeader}" NAME_WE)
 
   # Paths for generated files
   set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
@@ -136,8 +136,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs
   # explicit link libraries list so that the next block of code can unpack any
   # static libraries
   set(automaticDependencies "")
-  foreach(lib ${moduleName} ${linkLibraries})
-    # message("MODULE NAME: ${moduleName}")
+  foreach(lib ${module} ${linkLibraries})
     if(TARGET "${lib}")
       get_target_property(dependentLibraries ${lib} INTERFACE_LINK_LIBRARIES)
       # message("DEPENDENT LIBRARIES:  ${dependentLibraries}")
@@ -176,7 +175,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs
   set(otherLibraryTargets "")
   set(otherLibraryNontargets "")
   set(otherSourcesAndObjects "")
-  foreach(lib ${moduleName} ${linkLibraries} ${automaticDependencies})
+  foreach(lib ${module} ${linkLibraries} ${automaticDependencies})
     if(TARGET "${lib}")
       if(WRAP_MEX_BUILD_STATIC_MODULE)
         get_target_property(target_sources ${lib} SOURCES)
@@ -250,7 +249,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs
     COMMAND
       ${CMAKE_COMMAND} -E env
       "PYTHONPATH=${GTWRAP_PACKAGE_DIR}${GTWRAP_PATH_SEPARATOR}$ENV{PYTHONPATH}"
-      ${PYTHON_EXECUTABLE} ${MATLAB_WRAP_SCRIPT} --src ${interfaceHeader}
+      ${PYTHON_EXECUTABLE} ${MATLAB_WRAP_SCRIPT} --src "${interfaceHeader}"
       --module_name ${moduleName} --out ${generated_files_path}
       --top_module_namespaces ${moduleName} --ignore ${ignore_classes}
     VERBATIM
@@ -324,8 +323,8 @@ endfunction()
 
 # Internal function that installs a wrap toolbox
 function(install_wrapped_library_internal interfaceHeader)
-  get_filename_component(moduleName "${interfaceHeader}" NAME_WE)
-  set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
+  get_filename_component(module "${interfaceHeader}" NAME_WE)
+  set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${module}")
 
   # NOTE: only installs .m and mex binary files (not .cpp) - the trailing slash
   # on the directory name here prevents creating the top-level module name

wrap/gtwrap/interface_parser/type.py

@@ -53,6 +53,10 @@ def __init__(self,
         self.name = t[-1]  # the name is the last element in this list
         self.namespaces = t[:-1]
 
+        # If the first namespace is empty string, just get rid of it.
+        if self.namespaces and self.namespaces[0] == '':
+            self.namespaces.pop(0)
+
         if instantiations:
             if isinstance(instantiations, Sequence):
                 self.instantiations = instantiations  # type: ignore
@@ -92,8 +96,8 @@ def to_cpp(self) -> str:
         else:
             cpp_name = self.name
         return '{}{}{}'.format(
-            "::".join(self.namespaces[idx:]),
-            "::" if self.namespaces[idx:] else "",
+            "::".join(self.namespaces),
+            "::" if self.namespaces else "",
             cpp_name,
         )