Custom Factor in MATLAB

[hsc0009]

I am currently engaged in an effort to integrate an existing factor in GTSAM, namely the BarometricFactor, into the latest MATLAB wrapper. To this end, I have had to modify both the BarometricFactor.cpp and BarometricFactor.h files, ensuring their proper compilation.

However, I've encountered a stumbling block while attempting to wrap these modifications for MATLAB integration. A parsing error arises during this phase, reminiscent of a similar issue I found documented online (Issue #1673 regarding error compiling a new class and GTSAM_EXPORT to MATLAB), which was attributed to minor syntax errors within the example.i interface file.

For your convenience, I've attached the modified .cpp and .h files, alongside the .i file and a screenshot capturing the specific error encountered in the terminal. Any insight or expertise into resolving this matter would be invaluable and greatly appreciated.

example.i

// Specify classes from gtsam that we are using
virtual class gtsam::Value;
class gtsam::Point3;
class gtsam::Pose3;
virtual class gtsam::noiseModel::Base;
virtual class gtsam::NonlinearFactor;
virtual class gtsam::NoiseModelFactor2;
class gtsam::NonlinearFactorGraph;
class gtsam::Values;
class gtsam::Key;

/// Include custom factor
#include <example/BarometricFactor.h>

namespace example {

virtual class BarometricFactor : gtsam::NonlinearFactor {
  
  /// Standard Constructor
  BarometricFactor();
  BarometricFactor(
    size_t key, 
    size_t baroKey, 
    double double& baroIn, 
    const gtsam::noiseModel::Base* model);

  /// Testable
  void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
  bool equals(const gtsam::NonlinearFactor& expected, double tol);

  /// Standard Interface
  const double& measurementIn() const;
  double heightOut(double n) const;
  double baroOut(const double& meters) const;
};

} /// namespace example

BarometricFactor.h

#pragma once

#include <iostream>
#include <memory>
#include <string>
#include <boost/optional.hpp>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>

using namespace gtsam;

namespace example {

class BarometricFactor : public NoiseModelFactorN<Pose3,double> {

private:

    /// Basis to form custom unary factor
    typedef NoiseModelFactorN<Pose3,double> Base;

    /// Input
    double nT_;

public:

    /// Provide access to the Matrix& version of evaluateError
    using Base::evaluateError;

    /// shorthand for a smart pointer to a factor
    typedef std::shared_ptr<BarometricFactor> shared_ptr;

    /// Typedef to this class
    typedef BarometricFactor This;

    /** default constructor - only use for serialization */
    BarometricFactor(): nT_(0) {}
    ~BarometricFactor() override {}

    /// Barometric Update
    BarometricFactor(Key key, Key baroKey, const double& baroIn, const SharedNoiseModel& model) :
        Base(model,key,baroKey),nT_(heightOut(baroIn)) {}

    /// @return a deep copy of this factor
    gtsam::NonlinearFactor::shared_ptr clone() const override {
        return std::static_pointer_cast<gtsam::NonlinearFactor>(
            std::shared_ptr<This>(new This(*this)));}
    
    /// print
    void print(
        const std::string& s = "", 
        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;

    /// equals
    bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

    /// error evaluation
    Vector evaluateError(const Pose3& p, const double& bias, 
             OptionalMatrixType H, OptionalMatrixType H2) const override; 

    /// Input
    inline const double& measurementIn() const { return nT_; }
    
    /// Earth Atmosphere Model
    inline double heightOut(double n) const {
        return (std::pow(n / 101.29, 1. / 5.256) * 288.08 - 273.1 - 15.04) / -0.00649;}
    
    /// Pressure Estimation
    inline double baroOut(const double& meters) const{
        double temp = 15.04 - 0.00649 * meters;
        return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256);}

};
} // namespace example

BarometricFactor.cpp

#include "BarometricFactor.h"
#include <gtsam/inference/Key.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/Vector.h> 
#include <iostream>

using namespace std;

namespace example{

 //***************************************************************************
 void BarometricFactor::print(const string& s,
                              const KeyFormatter& keyFormatter) const {
     cout << (s.empty() ? "" : s + " ") << "Barometric Factor on "
          << keyFormatter(key<1>()) << "Barometric Bias on "
          << keyFormatter(key<2>()) << "\n";
  
     cout << "  Baro measurement: " << nT_ << "\n";
        noiseModel_->print("  noise model: ");
 }
  
 //***************************************************************************
 bool BarometricFactor::equals(const NonlinearFactor& expected,
                               double tol) const {
        const This* e = dynamic_cast<const This*>(&expected);
        return e != nullptr && Base::equals(*e, tol) &&
            traits<double>::Equals(nT_, e->nT_, tol);
 }
  
 //***************************************************************************
 Vector BarometricFactor::evaluateError(
                                        const Pose3& p, 
                                        const double& bias,
                                        OptionalMatrixType H,
                                        OptionalMatrixType H2) const {
        Matrix tH;
        Vector ret = (Vector(1) << (p.translation(tH).z() + bias - nT_)).finished();
        if (H) (*H) = tH.block<1, 6>(2, 0);
        if (H2) (*H2) = (Matrix(1, 1) << 1.0).finished();
        return ret;
 }
 };

[varunagrawal]

I'll take a look at this in the morning. It should be a relatively easy fix, but I agree the parser errors are not very helpful.

[varunagrawal]

@hsc0009 instead of putting in screenshots, please copy-paste the code directly into the issue comment. You can highlight it by using triple backticks (please look at Markdown formatting examples).

[varunagrawal]

So what you've written is correct. My guess is that the issue lies somewhere else in the gtsam-matlab-example project.

[hsc0009]

@varunagrawal I've updated the issue to include the code directly in a format that's easy to copy and paste. Could you advise on the best course of action moving forward?

[varunagrawal]

@hsc0009 I've created a PR to wrap the BarometricFactor in Matlab. You should be able to call it directly now.

[varunagrawal]

@hsc0009 I found your bug. In the .i file, the include call should be with angled brackets, not double quotes.

#include <example/BarometricFactor.h>  // Correct
#include "example/BarometricFactor.h"  // Incorrect

I also recommend putting the #include outside the namespace scope similar to a .h file in C++.

[hsc0009]

@varunagrawal Thank you for creating the pull request; however, I would still like to deduce the reason why my code is failing to facilitate future creation of custom factors that aren't already present in traditional GTSAM. Unfortunately, the aforementioned bug does not alleviate the problem. I am still encountering compilation errors.

[varunagrawal]

I pushed some fixes and updates to the example project. I recommend doing a git pull to get the latest version of the main branch and trying again.

I was able to compile your example after that.