i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Correct the Jacobian calculation of GravityFactor (#1692)

Browse Source 
* correct the Jacobian calculation of GravityFactor

* reimplement GravityFactor's analytical Jacobian

* adopt some minor changes from the GTSAM mainline

* use GTSAM's native AttitudeFactor instead of GravityFactor

* add missing header file and namespace prefix

* Removed GravityFactor. Fixed build with latest GTSAM version from source. Require c++17 when building with GTSAM>=4.3.0.

* Fixed cmake error when gtsam is not installed

---------

Co-authored-by: matlabbe <matlabbe@gmail.com>
This commit is contained in:
Borong Yuan
2026-04-27 06:33:05 +08:00
committed by GitHub
parent aa6d20775f
commit ebba7e4878
10 changed files with 36 additions and 393 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/CMakeLists.txt
View File

@@ -804,10 +804,6 @@ IF(CUVSLAM_FOUND)
ENDIF(CUVSLAM_FOUND)
IF(GTSAM_FOUND)
SET(SRC_FILES
${SRC_FILES}
optimizer/gtsam/GravityFactor.cpp
)
SET(LIBRARIES
${LIBRARIES}
gtsam

29
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -51,7 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include "gtsam/GravityFactor.h"
#include <gtsam/navigation/AttitudeFactor.h>
#include <optimizer/gtsam/XYFactor.h>
#include <optimizer/gtsam/XYZFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
@@ -121,7 +121,7 @@ void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
params.relinearizeThreshold = threshold;
params.relinearizeSkip = skip;
params.evaluateNonlinearError = true;
isam2_ = new ISAM2(params);
isam2_ = new gtsam::ISAM2(params);
addedPoses_.clear();
lastAddedConstraints_.clear();
@@ -392,7 +392,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
if(id1 < 0 && !isLandmarkWithRotation.at(id1))
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector2(
1/iter->second.infMatrix().at<double>(0,0),
1/iter->second.infMatrix().at<double>(1,1)));
graph.add(XYFactor<gtsam::Point2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
@@ -400,7 +400,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
else if (1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(Vector2(
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector2(
1/iter->second.infMatrix().at<double>(0,0),
1/iter->second.infMatrix().at<double>(1,1)));
graph.add(XYFactor<gtsam::Pose2>(id1, gtsam::Point2(iter->second.transform().x(), iter->second.transform().y()), model));
@@ -431,7 +431,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
if(id1 < 0 && !isLandmarkWithRotation.at(id1))
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Precisions(gtsam::Vector3(
iter->second.infMatrix().at<double>(0,0),
iter->second.infMatrix().at<double>(1,1),
iter->second.infMatrix().at<double>(2,2)));
@@ -442,7 +442,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
1 / static_cast<double>(iter->second.infMatrix().at<double>(4,4)) >= 9999.0 ||
1 / static_cast<double>(iter->second.infMatrix().at<double>(5,5)) >= 9999.0)
{
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions(Vector3(
gtsam::noiseModel::Diagonal::shared_ptr model = gtsam::noiseModel::Diagonal::Precisions(gtsam::Vector3(
iter->second.infMatrix().at<double>(0,0),
iter->second.infMatrix().at<double>(1,1),
iter->second.infMatrix().at<double>(2,2)));
@@ -471,10 +471,17 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
else if(!isSlam2d() && gravitySigma() > 0 && iter->second.type() == Link::kGravity && newPoses.find(iter->first) != newPoses.end())
{
Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz();
gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1));
gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), gravitySigma()));
graph.add(Pose3GravityFactor(iter->first, nG, model, Unit3(0,0,1)));
gtsam::Rot3 nRbMeas = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation();
gtsam::Unit3 nZ(0,0,1);
gtsam::Unit3 bGMeas = nRbMeas.unrotate(nZ);
gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigma(2, gravitySigma());
#if GTSAM_VERSION_NUMERIC <= 40300
// Note: till 40301 is officially released, version 40300 with "4.3a1" would fail here.
// Just replace "<=" above by "<" to use AttitudeFactor<Pose3> below.
graph.add(gtsam::Pose3AttitudeFactor(iter->first, nZ, model, bGMeas));
#else
graph.add(gtsam::AttitudeFactor<gtsam::Pose3>(iter->first, nZ, model, bGMeas));
#endif
lastAddedConstraints_.push_back(ConstraintToFactor(iter->first, iter->first, -1));
}
}
@@ -783,7 +790,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
float x,y,z,roll,pitch,yaw;
std::map<int, Transform> tmpPoses;
const Values values = isam2_?isam2_->calculateEstimate():optimizer->values();
const gtsam::Values values = isam2_?isam2_->calculateEstimate():optimizer->values();
#if GTSAM_VERSION_NUMERIC >= 40200
for(gtsam::Values::deref_iterator iter=values.begin(); iter!=values.end(); ++iter)
#else

91
corelib/src/optimizer/gtsam/GravityFactor.cpp
View File

@@ -1,91 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* Author: Mathieu Labbe
* This file is a copy of AttitudeFactor.cpp of gtsam library but
* with attitudeError() function overridden to ignore yaw errors.
* For the noise model, use Sigmas(Vector2(0.1, 10)) (with second sigma high!)
*/
/**
* @file GravityFactor.cpp
* @author Frank Dellaert
* @brief Implementation file for Attitude factor
* @date January 28, 2014
**/
#include "GravityFactor.h"
using namespace std;
namespace rtabmap {
//***************************************************************************
Vector GravityFactor::attitudeError(const Rot3& nRb,
OptionalJacobian<2, 3> H) const {
if (H) {
Matrix23 D_nRef_R;
Matrix22 D_e_nRef;
Vector3 r = nRb.xyz();
Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0).rotate(bRef_, D_nRef_R);
Vector e = nZ_.error(nRef, D_e_nRef);
(*H) = D_e_nRef * D_nRef_R;
//printf("ref=%f %f %f grav=%f %f %f e= %f %f H=%f %f %f, %f %f %f\n",
// nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1),
// (*H)(0,0), (*H)(0,1), (*H)(0,2), (*H)(1,0), (*H)(1,1), (*H)(1,2));
return e;
} else {
Vector3 r = nRb.xyz();
Unit3 nRef = Rot3::RzRyRx(r.x(), r.y(), 0) * bRef_;
Vector e = nZ_.error(nRef);
//printf("ref=%f %f %f grav=%f %f %f e= %f %f\n", nRef.point3().x(), nRef.point3().y(), nRef.point3().z(), nZ_.point3().x(), nZ_.point3().y(), nZ_.point3().z(), e(0), e(1));
return e;
}
}
//***************************************************************************
void Rot3GravityFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << "Rot3GravityFactor on " << keyFormatter(this->key()) << "\n";
nZ_.print(" measured direction in nav frame: ");
bRef_.print(" reference direction in body frame: ");
this->noiseModel_->print(" noise model: ");
}
//***************************************************************************
bool Rot3GravityFactor::equals(const NonlinearFactor& expected,
double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
&& this->bRef_.equals(e->bRef_, tol);
}
//***************************************************************************
void Pose3GravityFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << "Pose3GravityFactor on " << keyFormatter(this->key()) << "\n";
nZ_.print(" measured direction in nav frame: ");
bRef_.print(" reference direction in body frame: ");
this->noiseModel_->print(" noise model: ");
}
//***************************************************************************
bool Pose3GravityFactor::equals(const NonlinearFactor& expected,
double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
&& this->bRef_.equals(e->bRef_, tol);
}
//***************************************************************************
}/// namespace gtsam

271
corelib/src/optimizer/gtsam/GravityFactor.h
View File

@@ -1,271 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* Author: Mathieu Labbe
* This file is a copy of AttitudeFactor.h of gtsam library but
* with attitudeError() function overridden to ignore yaw errors.
* For the noise model, use Sigmas(Vector2(0.1, 10)) (with second sigma high!)
*/
/**
* @file Pose3GravityFactor.h
* @author Frank Dellaert
* @brief Header file for Attitude factor
* @date January 28, 2014
**/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#if GTSAM_VERSION_NUMERIC >= 40300 && defined(GTSAM_WITH_NOISE_MODEL_FACTOR_N)
#include <gtsam/nonlinear/NoiseModelFactorN.h>
#endif
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
using namespace gtsam;
namespace rtabmap {
/**
* Base class for prior on gravity
* Example:
* - measurement is direction of gravity in navigation frame nG
* - reference is direction of z axis in body frame bF
* This factor will give zero error if nG is opposite direction of bF
* @addtogroup Navigation
*/
class GravityFactor {
protected:
const Unit3 nZ_, bRef_; ///< Position measurement in
public:
/** default constructor - only use for serialization */
GravityFactor() {
}
/**
* @brief Constructor
* @param nZ measured direction in navigation frame
* @param bRef reference direction in body frame (default Z-axis in NED frame, i.e., [0; 0; 1])
*/
GravityFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) :
nZ_(nZ), bRef_(bRef) {
}
/** vector of errors */
Vector attitudeError(const Rot3& p,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalJacobian<2,3> H = {}) const;
#else
OptionalJacobian<2,3> H = boost::none) const;
#endif
/** Serialization function */
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
/*ar & boost::serialization::make_nvp("nZ_", const_cast<Unit3&>(nZ_));
ar & boost::serialization::make_nvp("bRef_", const_cast<Unit3&>(bRef_));*/
}
#endif
};
/**
* Version of GravityFactor for Rot3
* @addtogroup Navigation
*/
class Rot3GravityFactor: public NoiseModelFactor1<Rot3>, public GravityFactor {
typedef NoiseModelFactor1<Rot3> Base;
public:
/// shorthand for a smart pointer to a factor
#if GTSAM_VERSION_NUMERIC >= 40300
typedef std::shared_ptr<Rot3GravityFactor> shared_ptr;
#else
typedef boost::shared_ptr<Rot3GravityFactor> shared_ptr;
#endif
/// Typedef to this class
typedef Rot3GravityFactor This;
/** default constructor - only use for serialization */
Rot3GravityFactor() {
}
virtual ~Rot3GravityFactor() {
}
/**
* @brief Constructor
* @param key of the Rot3 variable that will be constrained
* @param nZ measured direction in navigation frame (remove yaw before rotating the gravity vector)
* @param model Gaussian noise model
* @param bRef reference direction in body frame (default Z-axis)
*/
Rot3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
const Unit3& bRef = Unit3(0, 0, 1)) :
Base(model, key), GravityFactor(nZ, bRef) {
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
#if GTSAM_VERSION_NUMERIC >= 40300
return std::static_pointer_cast<gtsam::NonlinearFactor>(
#else
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
#endif
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
/** vector of errors */
virtual Vector evaluateError(const Rot3& nRb, //
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H = OptionalNone) const {
#else
boost::optional<Matrix&> H = boost::none) const {
#endif
return attitudeError(nRb, H);
}
Unit3 nZ() const {
return nZ_;
}
Unit3 bRef() const {
return bRef_;
}
private:
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
/*ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & boost::serialization::make_nvp("GravityFactor",
boost::serialization::base_object<GravityFactor>(*this));*/
}
#endif
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
/**
* Version of GravityFactor for Pose3
* @addtogroup Navigation
*/
class Pose3GravityFactor: public NoiseModelFactor1<Pose3>,
public GravityFactor {
typedef NoiseModelFactor1<Pose3> Base;
public:
/// shorthand for a smart pointer to a factor
#if GTSAM_VERSION_NUMERIC >= 40300
typedef std::shared_ptr<Pose3GravityFactor> shared_ptr;
#else
typedef boost::shared_ptr<Pose3GravityFactor> shared_ptr;
#endif
/// Typedef to this class
typedef Pose3GravityFactor This;
/** default constructor - only use for serialization */
Pose3GravityFactor() {
}
virtual ~Pose3GravityFactor() {
}
/**
* @brief Constructor
* @param key of the Pose3 variable that will be constrained
* @param nZ measured direction in navigation frame (remove yaw before rotating the gravity vector)
* @param model Gaussian noise model
* @param bRef reference direction in body frame (default Z-axis)
*/
Pose3GravityFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
const Unit3& bRef = Unit3(0, 0, 1)) :
Base(model, key), GravityFactor(nZ, bRef) {
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
#if GTSAM_VERSION_NUMERIC >= 40300
return std::static_pointer_cast<gtsam::NonlinearFactor>(
#else
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
#endif
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
/** vector of errors */
virtual Vector evaluateError(const Pose3& nTb, //
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H = OptionalNone) const {
#else
boost::optional<Matrix&> H = boost::none) const {
#endif
Vector e = attitudeError(nTb.rotation(), H);
if (H) {
Matrix H23 = *H;
*H = Matrix::Zero(2,6);
H->block<2,3>(0,0) = H23;
}
return e;
}
Unit3 nZ() const {
return nZ_;
}
Unit3 bRef() const {
return bRef_;
}
private:
#if defined(GTSAM_ENABLE_BOOST_SERIALIZATION) || GTSAM_VERSION_NUMERIC < 40300
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
/*ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & boost::serialization::make_nvp("GravityFactor",
boost::serialization::base_object<GravityFactor>(*this));*/
}
#endif
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
} /// namespace gtsam

4
corelib/src/optimizer/gtsam/XYFactor.h
View File

@@ -43,7 +43,7 @@ public:
// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
gtsam::Vector evaluateError(const VALUE& p,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H = OptionalNone) const {
gtsam::OptionalMatrixType H = OptionalNone) const {
#else
boost::optional<gtsam::Matrix&> H = boost::none) const {
#endif
@@ -59,5 +59,5 @@ public:
};
} // namespace gtsamexamples
} // namespace rtabmap

6
corelib/src/optimizer/gtsam/XYZFactor.h
View File

@@ -43,7 +43,7 @@ public:
// @param H the optional Jacobian matrix, which use boost optional and has default null pointer
gtsam::Vector evaluateError(const gtsam::Pose3& p,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H = OptionalNone) const {
gtsam::OptionalMatrixType H = OptionalNone) const {
#else
boost::optional<gtsam::Matrix&> H = boost::none) const {
#endif
@@ -55,7 +55,7 @@ public:
}
gtsam::Vector evaluateError(const gtsam::Point3& p,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H = OptionalNone) const {
gtsam::OptionalMatrixType H = OptionalNone) const {
#else
boost::optional<gtsam::Matrix&> H = boost::none) const {
#endif
@@ -63,5 +63,5 @@ public:
}
};
} // namespace gtsamexamples
} // namespace rtabmap

12
corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h
View File

@@ -31,9 +31,9 @@ namespace vertigo {
gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone) const
gtsam::OptionalMatrixType H1 = OptionalNone,
gtsam::OptionalMatrixType H2 = OptionalNone,
gtsam::OptionalMatrixType H3 = OptionalNone) const
#else
boost::optional<gtsam::Matrix&> H1 = boost::none,
boost::optional<gtsam::Matrix&> H2 = boost::none,
@@ -71,9 +71,9 @@ namespace vertigo {
gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H1 = OptionalNone,
OptionalMatrixType H2 = OptionalNone,
OptionalMatrixType H3 = OptionalNone) const
gtsam::OptionalMatrixType H1 = OptionalNone,
gtsam::OptionalMatrixType H2 = OptionalNone,
gtsam::OptionalMatrixType H3 = OptionalNone) const
#else
boost::optional<gtsam::Matrix&> H1 = boost::none,
boost::optional<gtsam::Matrix&> H2 = boost::none,

5
corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h
View File

@@ -13,6 +13,7 @@
// DerivedValue2.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac
#include "DerivedValue.h"
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace vertigo {
@@ -77,8 +78,8 @@ namespace vertigo {
/** between operation */
inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H1=OptionalNone,
OptionalMatrixType H2=OptionalNone) const {
gtsam::OptionalMatrixType H1=OptionalNone,
gtsam::OptionalMatrixType H2=OptionalNone) const {
#else
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {

5
corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h
View File

@@ -13,6 +13,7 @@
// DerivedValue.h removed from gtsam repo (Dec 2018): https://github.com/borglab/gtsam/commit/e550f4f2aec423cb3f2791b81cb5858b8826ebac
#include "DerivedValue.h"
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace vertigo {
@@ -77,8 +78,8 @@ namespace vertigo {
/** between operation */
inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2,
#if GTSAM_VERSION_NUMERIC >= 40300
OptionalMatrixType H1=OptionalNone,
OptionalMatrixType H2=OptionalNone) const {
gtsam::OptionalMatrixType H1=OptionalNone,
gtsam::OptionalMatrixType H2=OptionalNone) const {
#else
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {