base_edge.h
Go to the documentation of this file.
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_EDGE_H
28 #define G2O_BASE_EDGE_H
29 
30 #include <iostream>
31 #include <limits>
32 
33 #include <Eigen/Core>
34 
35 #include "optimizable_graph.h"
36 
37 namespace g2o {
38 
39  using namespace Eigen;
40 
41  template <int D, typename E>
43  {
44  public:
45 
46  static const int Dimension = D;
47  typedef E Measurement;
48  typedef Matrix<double, D, 1> ErrorVector;
49  typedef Matrix<double, D, D> InformationType;
50 
52  {
53  _dimension = D;
54  }
55 
56  virtual ~BaseEdge() {}
57 
58  virtual double chi2() const
59  {
60  return _error.dot(information()*_error);
61  }
62 
63  virtual const double* errorData() const { return _error.data();}
64  virtual double* errorData() { return _error.data();}
65  const ErrorVector& error() const { return _error;}
66  ErrorVector& error() { return _error;}
67 
69  const InformationType& information() const { return _information;}
70  InformationType& information() { return _information;}
71  void setInformation(const InformationType& information) { _information = information;}
72 
73  virtual const double* informationData() const { return _information.data();}
74  virtual double* informationData() { return _information.data();}
75 
77  const Measurement& measurement() const { return _measurement;}
78  virtual void setMeasurement(const Measurement& m) { _measurement = m;}
79 
80  virtual int rank() const {return _dimension;}
81 
83  {
84  std::cerr << "inititialEstimate() is not implemented, please give implementation in your derived class" << std::endl;
85  }
86 
87  protected:
88 
89  Measurement _measurement;
90  InformationType _information;
91  ErrorVector _error;
92 
96  InformationType robustInformation(const Eigen::Vector3d& rho)
97  {
98  InformationType result = rho[1] * _information;
99  //ErrorVector weightedErrror = _information * _error;
100  //result.noalias() += 2 * rho[2] * (weightedErrror * weightedErrror.transpose());
101  return result;
102  }
103 
104  public:
105  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
106  };
107 
108 } // end namespace g2o
109 
110 #endif
const Measurement & measurement() const
accessor functions for the measurement represented by the edge
Definition: base_edge.h:77
ErrorVector _error
Definition: base_edge.h:91
Measurement _measurement
Definition: base_edge.h:89
std::set< Vertex * > VertexSet
Definition: hyper_graph.h:91
virtual double chi2() const
computes the chi2 based on the cached error value, only valid after computeError has been called...
Definition: base_edge.h:58
InformationType _information
Definition: base_edge.h:90
const InformationType & information() const
information matrix of the constraint
Definition: base_edge.h:69
virtual void setMeasurement(const Measurement &m)
Definition: base_edge.h:78
virtual double * informationData()
Definition: base_edge.h:74
InformationType & information()
Definition: base_edge.h:70
virtual ~BaseEdge()
Definition: base_edge.h:56
virtual void initialEstimate(const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *)
Definition: base_edge.h:82
const ErrorVector & error() const
Definition: base_edge.h:65
Matrix< double, D, 1 > ErrorVector
Definition: base_edge.h:48
virtual int rank() const
Definition: base_edge.h:80
A general case Vertex for optimization.
virtual const double * informationData() const
returns the memory of the information matrix, usable for example with a Eigen::Map<MatrixXd> ...
Definition: base_edge.h:73
virtual const double * errorData() const
returns the error vector cached after calling the computeError;
Definition: base_edge.h:63
ErrorVector & error()
Definition: base_edge.h:66
InformationType robustInformation(const Eigen::Vector3d &rho)
Definition: base_edge.h:96
virtual double * errorData()
Definition: base_edge.h:64
Matrix< double, D, D > InformationType
Definition: base_edge.h:49
void setInformation(const InformationType &information)
Definition: base_edge.h:71


orb_slam2_with_maps_odom
Author(s): teng zhang
autogenerated on Fri Sep 25 2020 03:24:47