JacobianFactorQ.h gtsam/linear/RegularJacobianFactor.h gtsam/linear/RegularJacobianFactor.h tests/doc-test-files/JacobianFactorQ.h gtsam::JacobianFactorQ gtsam::traits< JacobianFactorQ< D, ZDim > > gtsam /*---------------------------------------------------------------------------- *GTSAMCopyright2010,GeorgiaTechResearchCorporation, *Atlanta,Georgia30332-0415 *AllRightsReserved *Authors:FrankDellaert,etal.(seeTHANKSforthefullauthorlist) *SeeLICENSEforthelicenseinformation *--------------------------------------------------------------------------*/ /* *@fileJacobianFactorQ.h *@dateOct27,2013 *@uthorFrankDellaert */ #pragmaonce #include<gtsam/linear/RegularJacobianFactor.h> namespacegtsam{ template<size_tD,size_tZDim> classJacobianFactorQ:publicRegularJacobianFactor<D>{ typedefRegularJacobianFactor<D>Base; typedefEigen::Matrix<double,ZDim,D>MatrixZD; typedefstd::pair<Key,Matrix>KeyMatrix; public: JacobianFactorQ(){ } JacobianFactorQ(constKeyVector&keys,// constSharedDiagonal&model=SharedDiagonal()): Base(){ MatrixzeroMatrix=Matrix::Zero(0,D); VectorzeroVector=Vector::Zero(0); std::vector<KeyMatrix>QF; QF.reserve(keys.size()); for(constKey&key:keys) QF.push_back(KeyMatrix(key,zeroMatrix)); JacobianFactor::fillTerms(QF,zeroVector,model); } JacobianFactorQ(constKeyVector&keys, conststd::vector<MatrixZD,Eigen::aligned_allocator<MatrixZD>>&FBlocks,constMatrix&E,constMatrix3&P, constVector&b,constSharedDiagonal&model=SharedDiagonal()): Base(){ size_tj=0,m2=E.rows(),m=m2/ZDim; //CalculateprojectorQ MatrixQ=Matrix::Identity(m2,m2)-E*P*E.transpose(); //Calculatepre-computedJacobianmatrices //TODO:canwedobetter? std::vector<KeyMatrix>QF; QF.reserve(m); //Below,wecomputeeachmZDim*DblockA_j=Q_j*F_j=(mZDim*ZDim)*(Zdim*D) for(size_tk=0;k<FBlocks.size();++k){ Keykey=keys[k]; QF.push_back( KeyMatrix(key,-Q.block(0,ZDim*j++,m2,ZDim)*FBlocks[k])); } //WhichisthenpassedtothenormalJacobianFactorconstructor JacobianFactor::fillTerms(QF,-Q*b,model); } }; //endclassJacobianFactorQ //traits template<size_tD,size_tZDim>structtraits<JacobianFactorQ<D,ZDim>>:publicTestable< JacobianFactorQ<D,ZDim>>{ }; }