1 // g2o - General Graph Optimization 2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard 3 // All rights reserved. 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are 7 // met: 8 // 9 // * Redistributions of source code must retain the above copyright notice, 10 // this list of conditions and the following disclaimer. 11 // * Redistributions in binary form must reproduce the above copyright 12 // notice, this list of conditions and the following disclaimer in the 13 // documentation and/or other materials provided with the distribution. 14 // 15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 27 #ifndef G2O_BASE_MULTI_EDGE_H 28 #define G2O_BASE_MULTI_EDGE_H 29 30 #include <iostream> 31 #include <iomanip> 32 #include <limits> 33 34 #include "base_edge.h" 35 #include "robust_kernel.h" 36 #include "g2o/config.h" 37 #include "g2o/stuff/misc.h" 38 #include "dynamic_aligned_buffer.hpp" 39 40 namespace g2o { 41 42 /** 43 * \brief base class to represent an edge connecting an arbitrary number of nodes 44 * 45 * D - Dimension of the measurement 46 * E - type to represent the measurement 47 */ 48 template <int D, typename E> 49 class BaseMultiEdge : public BaseEdge<D,E> 50 { 51 public: 52 /** 53 * \brief helper for mapping the Hessian memory of the upper triangular block 54 */ 55 struct HessianHelper { 56 Eigen::Map<MatrixX> matrix; ///< the mapped memory 57 bool transposed; ///< the block has to be transposed HessianHelperHessianHelper58 HessianHelper() : matrix(0, 0, 0), transposed(false) {} 59 }; 60 61 public: 62 static const int Dimension = BaseEdge<D,E>::Dimension; 63 typedef typename BaseEdge<D,E>::Measurement Measurement; 64 typedef MatrixX::MapType JacobianType; 65 typedef typename BaseEdge<D,E>::ErrorVector ErrorVector; 66 typedef typename BaseEdge<D,E>::InformationType InformationType; 67 typedef Eigen::Map<MatrixX, MatrixX::Flags & Eigen::PacketAccessBit ? Eigen::Aligned : Eigen::Unaligned > HessianBlockType; 68 BaseMultiEdge()69 BaseMultiEdge() : BaseEdge<D,E>() 70 { 71 } 72 73 virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace); 74 75 /** 76 * Linearizes the oplus operator in the vertex, and stores 77 * the result in temporary variable vector _jacobianOplus 78 */ 79 virtual void linearizeOplus(); 80 81 virtual void resize(size_t size); 82 83 virtual bool allVerticesFixed() const; 84 85 virtual void constructQuadraticForm() ; 86 87 virtual void mapHessianMemory(number_t* d, int i, int j, bool rowMajor); 88 89 using BaseEdge<D,E>::computeError; 90 91 protected: 92 using BaseEdge<D,E>::_measurement; 93 using BaseEdge<D,E>::_information; 94 using BaseEdge<D,E>::_error; 95 using BaseEdge<D,E>::_vertices; 96 using BaseEdge<D,E>::_dimension; 97 98 std::vector<HessianHelper> _hessian; 99 std::vector<JacobianType, Eigen::aligned_allocator<JacobianType> > _jacobianOplus; ///< jacobians of the edge (w.r.t. oplus) 100 101 void computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError); 102 103 public: 104 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 105 }; 106 107 108 109 // PARTIAL TEMPLATE SPECIALIZATION 110 template <typename E> 111 class BaseMultiEdge<-1,E> : public BaseEdge<-1,E> 112 { 113 public: 114 /** 115 * \brief helper for mapping the Hessian memory of the upper triangular block 116 */ 117 struct HessianHelper { 118 Eigen::Map<MatrixX> matrix; ///< the mapped memory 119 bool transposed; ///< the block has to be transposed HessianHelperHessianHelper120 HessianHelper() : matrix(0, 0, 0), transposed(false) {} 121 }; 122 123 public: 124 static const int Dimension = BaseEdge<-1,E>::Dimension; 125 typedef typename BaseEdge<-1,E>::Measurement Measurement; 126 typedef MatrixX::MapType JacobianType; 127 typedef typename BaseEdge<-1,E>::ErrorVector ErrorVector; 128 typedef typename BaseEdge<-1,E>::InformationType InformationType; 129 typedef Eigen::Map<MatrixX, MatrixX::Flags & Eigen::PacketAccessBit ? Eigen::Aligned : Eigen::Unaligned > HessianBlockType; 130 BaseMultiEdge()131 BaseMultiEdge() : BaseEdge<-1,E>() 132 { 133 // this->_variableSize = true; 134 } 135 136 virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace); 137 138 /** 139 * Linearizes the oplus operator in the vertex, and stores 140 * the result in temporary variable vector _jacobianOplus 141 */ 142 virtual void linearizeOplus(); 143 144 virtual void resize(size_t size); 145 146 virtual bool allVerticesFixed() const; 147 148 virtual void constructQuadraticForm() ; 149 150 virtual void mapHessianMemory(number_t* d, int i, int j, bool rowMajor); 151 152 using BaseEdge<-1,E>::computeError; 153 154 protected: 155 using BaseEdge<-1,E>::_measurement; 156 using BaseEdge<-1,E>::_information; 157 using BaseEdge<-1,E>::_error; 158 using BaseEdge<-1,E>::_vertices; 159 using BaseEdge<-1,E>::_dimension; 160 161 std::vector<HessianHelper> _hessian; 162 std::vector<JacobianType, Eigen::aligned_allocator<JacobianType> > _jacobianOplus; ///< jacobians of the edge (w.r.t. oplus) 163 164 void computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError); 165 166 public: 167 EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 168 }; 169 170 171 #include "base_multi_edge.hpp" 172 173 } // end namespace g2o 174 175 #endif 176