diff --git a/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h b/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h index e95e0b5..f40d4b2 100644 --- a/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h +++ b/corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h @@ -110,6 +110,10 @@ namespace gtsam { // Define Key to be Testable by specializing gtsam::traits template struct traits; template<> struct traits { + typedef gtsam::manifold_tag structure_category; + enum { dimension = 1 }; + typedef vertigo::SwitchVariableLinear ManifoldType; + static void Print(const vertigo::SwitchVariableLinear& key, const std::string& str = "") { key.print(str); } @@ -118,8 +122,8 @@ template<> struct traits { } static int GetDimension(const vertigo::SwitchVariableLinear & key) {return key.Dim();} - typedef OptionalJacobian<3, 3> ChartJacobian; - typedef gtsam::Vector TangentVector; + typedef OptionalJacobian<1, 1> ChartJacobian; + typedef Eigen::Matrix TangentVector; static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other, #if GTSAM_VERSION_NUMERIC >= 40300 ChartJacobian Horigin = {}, ChartJacobian Hother = {}) { diff --git a/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h b/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h index 79e1fca..237c7cd 100644 --- a/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h +++ b/corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h @@ -111,6 +111,10 @@ namespace gtsam { // Define Key to be Testable by specializing gtsam::traits template struct traits; template<> struct traits { + typedef gtsam::manifold_tag structure_category; + enum { dimension = 1 }; + typedef vertigo::SwitchVariableSigmoid ManifoldType; + static void Print(const vertigo::SwitchVariableSigmoid& key, const std::string& str = "") { key.print(str); } @@ -119,8 +123,8 @@ template<> struct traits { } static int GetDimension(const vertigo::SwitchVariableSigmoid & key) {return key.Dim();} - typedef OptionalJacobian<3, 3> ChartJacobian; - typedef gtsam::Vector TangentVector; + typedef OptionalJacobian<1, 1> ChartJacobian; + typedef Eigen::Matrix TangentVector; static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other, #if GTSAM_VERSION_NUMERIC >= 40300 ChartJacobian Horigin = {}, ChartJacobian Hother = {}) {