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_XYZ_PRIOR_H_ 28 #define G2O_EDGE_XYZ_PRIOR_H_ 29 30 #include "g2o/core/base_unary_edge.h" 31 #include "g2o_types_slam3d_api.h" 32 #include "vertex_pointxyz.h" 33 34 namespace g2o { 35 /** 36 * \brief prior for an XYZ vertex (VertexPointXYZ) 37 * 38 * Provides a prior for a 3d point vertex. The measurement is represented by a 39 * Vector3 with a corresponding 3x3 upper triangle covariance matrix (upper triangle only). 40 */ 41 class G2O_TYPES_SLAM3D_API EdgeXYZPrior : public BaseUnaryEdge<3, Vector3, VertexPointXYZ> { 42 public: 43 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 44 EdgeXYZPrior(); 45 virtual bool read(std::istream& is); 46 virtual bool write(std::ostream& os) const; 47 48 void computeError(); 49 50 // jacobian 51 virtual void linearizeOplus(); 52 setMeasurementData(const number_t * d)53 virtual bool setMeasurementData(const number_t* d){ 54 Eigen::Map<const Vector3> v(d); 55 _measurement = v; 56 return true; 57 } 58 getMeasurementData(number_t * d)59 virtual bool getMeasurementData(number_t* d) const{ 60 Eigen::Map<Vector3> v(d); 61 v = _measurement; 62 return true; 63 } 64 measurementDimension()65 virtual int measurementDimension() const { return 3; } 66 67 virtual bool setMeasurementFromState() ; 68 initialEstimatePossible(const OptimizableGraph::VertexSet &,OptimizableGraph::Vertex *)69 virtual number_t initialEstimatePossible(const OptimizableGraph::VertexSet& /*from*/, 70 OptimizableGraph::Vertex* /*to*/) { 71 return 0; 72 } 73 }; 74 75 } 76 #endif 77