NonlinearFactor.h gtsam/nonlinear/Values.h gtsam/linear/NoiseModel.h gtsam/linear/JacobianFactor.h gtsam/inference/Factor.h gtsam/base/OptionalJacobian.h boost/serialization/base_object.hpp boost/assign/list_of.hpp gtsam/linear/JacobianFactor.h gtsam/base/OptionalJacobian.h tests/doc-test-files/NonlinearFactor.h gtsam/inference/Factor.h gtsam/linear/NoiseModel.h boost/serialization/base_object.hpp boost/assign/list_of.hpp gtsam/nonlinear/Values.h gtsam::NonlinearFactor gtsam::traits< NonlinearFactor > gtsam::NoiseModelFactor gtsam::NoiseModelFactor1 gtsam::NoiseModelFactor2 gtsam::NoiseModelFactor3 gtsam::NoiseModelFactor4 gtsam::NoiseModelFactor5 gtsam::NoiseModelFactor6 gtsam ADD_CLONE_NONLINEAR_FACTOR Derived virtual gtsam::NonlinearFactor::shared_ptr clone() const { \ return std::static_pointer_cast<gtsam::NonlinearFactor>( \ gtsam::NonlinearFactor::shared_ptr(new Derived(*this))); } Macro to add a standard clone function to a derived factor Deprecated: will go away shortly - just add the clone function directly Non-linear factor base classes. Frank Dellaert Richard Roberts /*---------------------------------------------------------------------------- *GTSAMCopyright2010,GeorgiaTechResearchCorporation, *Atlanta,Georgia30332-0415 *AllRightsReserved *Authors:FrankDellaert,etal.(seeTHANKSforthefullauthorlist) *SeeLICENSEforthelicenseinformation *--------------------------------------------------------------------------*/ //\callgraph #pragmaonce #include<gtsam/nonlinear/Values.h> #include<gtsam/linear/NoiseModel.h> #include<gtsam/linear/JacobianFactor.h> #include<gtsam/inference/Factor.h> #include<gtsam/base/OptionalJacobian.h> #include<boost/serialization/base_object.hpp> #include<boost/assign/list_of.hpp> #defineADD_CLONE_NONLINEAR_FACTOR(Derived)\ virtualgtsam::NonlinearFactor::shared_ptrclone()const{\ returnstd::static_pointer_cast<gtsam::NonlinearFactor>(\ gtsam::NonlinearFactor::shared_ptr(newDerived(*this)));} namespacegtsam{ usingboost::assign::cref_list_of; /***************************************************************************/ classGTSAM_EXPORTNonlinearFactor:publicFactor{ protected: //Somehandytypedefs typedefFactorBase; typedefNonlinearFactorThis; public: typedefstd::shared_ptr<This>shared_ptr; NonlinearFactor(){} template<typenameCONTAINER> NonlinearFactor(constCONTAINER&keys): Base(keys){} virtualvoidprint(conststd::string&s="", constKeyFormatter&keyFormatter=DefaultKeyFormatter)const; virtualboolequals(constNonlinearFactor&f,doubletol=1e-9)const; virtual~NonlinearFactor(){} virtualdoubleerror(constValues&c)const=0; virtualsize_tdim()const=0; virtualboolactive(constValues&/*c*/)const{returntrue;} virtualstd::shared_ptr<GaussianFactor> linearize(constValues&c)const=0; virtualshared_ptrclone()const{ //TODO:choosebetterexceptiontothrowhere throwstd::runtime_error("NonlinearFactor::clone():Attemptingtoclonefactorwithnoclone()implemented!"); returnshared_ptr(); } shared_ptrrekey(conststd::map<Key,Key>&rekey_mapping)const; shared_ptrrekey(constKeyVector&new_keys)const; };//\classNonlinearFactor template<>structtraits<NonlinearFactor>:publicTestable<NonlinearFactor>{ }; /***************************************************************************/ classGTSAM_EXPORTNoiseModelFactor:publicNonlinearFactor{ protected: //handytypedefs typedefNonlinearFactorBase; typedefNoiseModelFactorThis; SharedNoiseModelnoiseModel_; public: typedefstd::shared_ptr<This>shared_ptr; NoiseModelFactor(){} virtual~NoiseModelFactor(){} template<typenameCONTAINER> NoiseModelFactor(constSharedNoiseModel&noiseModel,constCONTAINER&keys): Base(keys),noiseModel_(noiseModel){} protected: NoiseModelFactor(constSharedNoiseModel&noiseModel):noiseModel_(noiseModel){} public: virtualvoidprint(conststd::string&s="", constKeyFormatter&keyFormatter=DefaultKeyFormatter)const; virtualboolequals(constNonlinearFactor&f,doubletol=1e-9)const; virtualsize_tdim()const{ returnnoiseModel_->dim(); } constSharedNoiseModel&noiseModel()const{ returnnoiseModel_; } SharedNoiseModelget_noiseModel()const{ returnnoiseModel_; } virtualVectorunwhitenedError(constValues&x, boost::optional<std::vector<Matrix>&>H=boost::none)const=0; VectorwhitenedError(constValues&c)const; virtualdoubleerror(constValues&c)const; std::shared_ptr<GaussianFactor>linearize(constValues&x)const; private: friendclassboost::serialization::access; template<classARCHIVE> voidserialize(ARCHIVE&ar,constunsignedint/*version*/){ ar&boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object<Base>(*this)); ar&BOOST_SERIALIZATION_NVP(noiseModel_); } };//\classNoiseModelFactor /***************************************************************************/ template<classVALUE> classNoiseModelFactor1:publicNoiseModelFactor{ public: //typedefsforvaluetypespulledfromkeys typedefVALUEX; protected: typedefNoiseModelFactorBase; typedefNoiseModelFactor1<VALUE>This; public: NoiseModelFactor1(){} virtual~NoiseModelFactor1(){} inlineKeykey()const{returnkeys_[0];} NoiseModelFactor1(constSharedNoiseModel&noiseModel,Keykey1): Base(noiseModel,cref_list_of<1>(key1)){} virtualVectorunwhitenedError(constValues&x,boost::optional<std::vector<Matrix>&>H=boost::none)const{ if(this->active(x)){ constX&x1=x.at<X>(keys_[0]); if(H){ returnevaluateError(x1,(*H)[0]); }else{ returnevaluateError(x1); } }else{ returnVector::Zero(this->dim()); } } virtualVectorevaluateError(constX&x,boost::optional<Matrix&>H= boost::none)const=0; private: friendclassboost::serialization::access; template<classARCHIVE> voidserialize(ARCHIVE&ar,constunsignedint/*version*/){ ar&boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object<Base>(*this)); } };//\classNoiseModelFactor1 /***************************************************************************/ template<classVALUE1,classVALUE2> classNoiseModelFactor2:publicNoiseModelFactor{ public: //typedefsforvaluetypespulledfromkeys typedefVALUE1X1; typedefVALUE2X2; protected: typedefNoiseModelFactorBase; typedefNoiseModelFactor2<VALUE1, VALUE2>This; public: NoiseModelFactor2(){} NoiseModelFactor2(constSharedNoiseModel&noiseModel,Keyj1,Keyj2): Base(noiseModel,cref_list_of<2>(j1)(j2)){} virtual~NoiseModelFactor2(){} inlineKeykey1()const{returnkeys_[0];} inlineKeykey2()const{returnkeys_[1];} virtualVectorunwhitenedError(constValues&x,boost::optional<std::vector<Matrix>&>H=boost::none)const{ if(this->active(x)){ constX1&x1=x.at<X1>(keys_[0]); constX2&x2=x.at<X2>(keys_[1]); if(H){ returnevaluateError(x1,x2,(*H)[0],(*H)[1]); }else{ returnevaluateError(x1,x2); } }else{ returnVector::Zero(this->dim()); } } virtualVector evaluateError(constX1&,constX2&,boost::optional<Matrix&>H1= boost::none,boost::optional<Matrix&>H2=boost::none)const=0; private: friendclassboost::serialization::access; template<classARCHIVE> voidserialize(ARCHIVE&ar,constunsignedint/*version*/){ ar&boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object<Base>(*this)); } };//\classNoiseModelFactor2 /***************************************************************************/ template<classVALUE1,classVALUE2,classVALUE3> classNoiseModelFactor3:publicNoiseModelFactor{ public: //typedefsforvaluetypespulledfromkeys typedefVALUE1X1; typedefVALUE2X2; typedefVALUE3X3; protected: typedefNoiseModelFactorBase; typedefNoiseModelFactor3<VALUE1, VALUE2, VALUE3>This; public: NoiseModelFactor3(){} NoiseModelFactor3(constSharedNoiseModel&noiseModel,Keyj1,Keyj2,Keyj3): Base(noiseModel,cref_list_of<3>(j1)(j2)(j3)){} virtual~NoiseModelFactor3(){} inlineKeykey1()const{returnkeys_[0];} inlineKeykey2()const{returnkeys_[1];} inlineKeykey3()const{returnkeys_[2];} virtualVectorunwhitenedError(constValues&x,boost::optional<std::vector<Matrix>&>H=boost::none)const{ if(this->active(x)){ if(H) returnevaluateError(x.at<X1>(keys_[0]),x.at<X2>(keys_[1]),x.at<X3>(keys_[2]),(*H)[0],(*H)[1],(*H)[2]); else returnevaluateError(x.at<X1>(keys_[0]),x.at<X2>(keys_[1]),x.at<X3>(keys_[2])); }else{ returnVector::Zero(this->dim()); } } virtualVector evaluateError(constX1&,constX2&,constX3&, boost::optional<Matrix&>H1=boost::none, boost::optional<Matrix&>H2=boost::none, boost::optional<Matrix&>H3=boost::none)const=0; private: friendclassboost::serialization::access; template<classARCHIVE> voidserialize(ARCHIVE&ar,constunsignedint/*version*/){ ar&boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object<Base>(*this)); } };//\classNoiseModelFactor3 /***************************************************************************/ template<classVALUE1,classVALUE2,classVALUE3,classVALUE4> classNoiseModelFactor4:publicNoiseModelFactor{ public: //typedefsforvaluetypespulledfromkeys typedefVALUE1X1; typedefVALUE2X2; typedefVALUE3X3; typedefVALUE4X4; protected: typedefNoiseModelFactorBase; typedefNoiseModelFactor4<VALUE1, VALUE2, VALUE3, VALUE4>This; public: NoiseModelFactor4(){} NoiseModelFactor4(constSharedNoiseModel&noiseModel,Keyj1,Keyj2,Keyj3,Keyj4): Base(noiseModel,cref_list_of<4>(j1)(j2)(j3)(j4)){} virtual~NoiseModelFactor4(){} inlineKeykey1()const{returnkeys_[0];} inlineKeykey2()const{returnkeys_[1];} inlineKeykey3()const{returnkeys_[2];} inlineKeykey4()const{returnkeys_[3];} virtualVectorunwhitenedError(constValues&x,boost::optional<std::vector<Matrix>&>H=boost::none)const{ if(this->active(x)){ if(H) returnevaluateError(x.at<X1>(keys_[0]),x.at<X2>(keys_[1]),x.at<X3>(keys_[2]),x.at<X4>(keys_[3]),(*H)[0],(*H)[1],(*H)[2],(*H)[3]); else returnevaluateError(x.at<X1>(keys_[0]),x.at<X2>(keys_[1]),x.at<X3>(keys_[2]),x.at<X4>(keys_[3])); }else{ returnVector::Zero(this->dim()); } } virtualVector evaluateError(constX1&,constX2&,constX3&,constX4&, boost::optional<Matrix&>H1=boost::none, boost::optional<Matrix&>H2=boost::none, boost::optional<Matrix&>H3=boost::none, boost::optional<Matrix&>H4=boost::none)const=0; private: friendclassboost::serialization::access; template<classARCHIVE> voidserialize(ARCHIVE&ar,constunsignedint/*version*/){ ar&boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object<Base>(*this)); } };//\classNoiseModelFactor4 /***************************************************************************/ template<classVALUE1,classVALUE2,classVALUE3,classVALUE4,classVALUE5> classNoiseModelFactor5:publicNoiseModelFactor{ public: //typedefsforvaluetypespulledfromkeys typedefVALUE1X1; typedefVALUE2X2; typedefVALUE3X3; typedefVALUE4X4; typedefVALUE5X5; protected: typedefNoiseModelFactorBase; typedefNoiseModelFactor5<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5>This; public: NoiseModelFactor5(){} NoiseModelFactor5(constSharedNoiseModel&noiseModel,Keyj1,Keyj2,Keyj3,Keyj4,Keyj5): Base(noiseModel,cref_list_of<5>(j1)(j2)(j3)(j4)(j5)){} virtual~NoiseModelFactor5(){} inlineKeykey1()const{returnkeys_[0];} inlineKeykey2()const{returnkeys_[1];} inlineKeykey3()const{returnkeys_[2];} inlineKeykey4()const{returnkeys_[3];} inlineKeykey5()const{returnkeys_[4];} virtualVectorunwhitenedError(constValues&x,boost::optional<std::vector<Matrix>&>H=boost::none)const{ if(this->active(x)){ if(H) returnevaluateError(x.at<X1>(keys_[0]),x.at<X2>(keys_[1]),x.at<X3>(keys_[2]),x.at<X4>(keys_[3]),x.at<X5>(keys_[4]),(*H)[0],(*H)[1],(*H)[2],(*H)[3],(*H)[4]); else returnevaluateError(x.at<X1>(keys_[0]),x.at<X2>(keys_[1]),x.at<X3>(keys_[2]),x.at<X4>(keys_[3]),x.at<X5>(keys_[4])); }else{ returnVector::Zero(this->dim()); } } virtualVector evaluateError(constX1&,constX2&,constX3&,constX4&,constX5&, boost::optional<Matrix&>H1=boost::none, boost::optional<Matrix&>H2=boost::none, boost::optional<Matrix&>H3=boost::none, boost::optional<Matrix&>H4=boost::none, boost::optional<Matrix&>H5=boost::none)const=0; private: friendclassboost::serialization::access; template<classARCHIVE> voidserialize(ARCHIVE&ar,constunsignedint/*version*/){ ar&boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object<Base>(*this)); } };//\classNoiseModelFactor5 /***************************************************************************/ template<classVALUE1,classVALUE2,classVALUE3,classVALUE4,classVALUE5,classVALUE6> classNoiseModelFactor6:publicNoiseModelFactor{ public: //typedefsforvaluetypespulledfromkeys typedefVALUE1X1; typedefVALUE2X2; typedefVALUE3X3; typedefVALUE4X4; typedefVALUE5X5; typedefVALUE6X6; protected: typedefNoiseModelFactorBase; typedefNoiseModelFactor6<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6>This; public: NoiseModelFactor6(){} NoiseModelFactor6(constSharedNoiseModel&noiseModel,Keyj1,Keyj2,Keyj3,Keyj4,Keyj5,Keyj6): Base(noiseModel,cref_list_of<6>(j1)(j2)(j3)(j4)(j5)(j6)){} virtual~NoiseModelFactor6(){} inlineKeykey1()const{returnkeys_[0];} inlineKeykey2()const{returnkeys_[1];} inlineKeykey3()const{returnkeys_[2];} inlineKeykey4()const{returnkeys_[3];} inlineKeykey5()const{returnkeys_[4];} inlineKeykey6()const{returnkeys_[5];} virtualVectorunwhitenedError(constValues&x,boost::optional<std::vector<Matrix>&>H=boost::none)const{ if(this->active(x)){ if(H) returnevaluateError(x.at<X1>(keys_[0]),x.at<X2>(keys_[1]),x.at<X3>(keys_[2]),x.at<X4>(keys_[3]),x.at<X5>(keys_[4]),x.at<X6>(keys_[5]),(*H)[0],(*H)[1],(*H)[2],(*H)[3],(*H)[4],(*H)[5]); else returnevaluateError(x.at<X1>(keys_[0]),x.at<X2>(keys_[1]),x.at<X3>(keys_[2]),x.at<X4>(keys_[3]),x.at<X5>(keys_[4]),x.at<X6>(keys_[5])); }else{ returnVector::Zero(this->dim()); } } virtualVector evaluateError(constX1&,constX2&,constX3&,constX4&,constX5&,constX6&, boost::optional<Matrix&>H1=boost::none, boost::optional<Matrix&>H2=boost::none, boost::optional<Matrix&>H3=boost::none, boost::optional<Matrix&>H4=boost::none, boost::optional<Matrix&>H5=boost::none, boost::optional<Matrix&>H6=boost::none)const=0; private: friendclassboost::serialization::access; template<classARCHIVE> voidserialize(ARCHIVE&ar,constunsignedint/*version*/){ ar&boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object<Base>(*this)); } };//\classNoiseModelFactor6 /***************************************************************************/ }//\namespacegtsam