1 // g2o - General Graph Optimization 2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, 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_EDGE_SE3_OFFSET_H_ 28 #define G2O_EDGE_SE3_OFFSET_H_ 29 30 31 #include "edge_se3.h" 32 #include "g2o_types_slam3d_api.h" 33 34 namespace g2o { 35 class ParameterSE3Offset; 36 class CacheSE3Offset; 37 38 /** 39 * \brief Offset edge 40 */ 41 // first two args are the measurement type, second two the connection classes 42 class G2O_TYPES_SLAM3D_API EdgeSE3Offset : public EdgeSE3 { 43 public: 44 EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 45 EdgeSE3Offset(); 46 virtual bool read(std::istream& is); 47 virtual bool write(std::ostream& os) const; 48 49 void computeError(); 50 51 52 void linearizeOplus(); 53 54 virtual bool setMeasurementFromState() ; 55 initialEstimatePossible(const OptimizableGraph::VertexSet &,OptimizableGraph::Vertex *)56 virtual number_t initialEstimatePossible(const OptimizableGraph::VertexSet& /*from*/, 57 OptimizableGraph::Vertex* /*to*/) { 58 return 1.; 59 } 60 61 virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to); 62 63 protected: 64 virtual bool resolveCaches(); 65 ParameterSE3Offset *_offsetFrom, *_offsetTo; 66 CacheSE3Offset *_cacheFrom, *_cacheTo; 67 }; 68 69 } // end namespace 70 #endif 71