edge_se3_xyzprior.h
Go to the documentation of this file.
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_PRIOR_XYZ_H
28 #define G2O_EDGE_SE3_PRIOR_XYZ_H
29 
30 #include "g2o/types/slam3d/vertex_se3.h"
31 #include "g2o/core/base_unary_edge.h"
32 #include "g2o/types/slam3d/parameter_se3_offset.h"
33 
34 namespace g2o {
35 
36 using namespace Eigen;
37 
41 class EdgeSE3XYZPrior : public BaseUnaryEdge<3, Vector3d, VertexSE3>
42 {
43 public:
46 
47  virtual void setMeasurement(const Vector3d& m) {
48  _measurement = m;
49  }
50 
51  virtual bool setMeasurementData(const double * d) {
53  _measurement = v;
54  return true;
55  }
56 
57  virtual bool getMeasurementData(double* d) const {
58  Map<Vector3d> v(d);
59  v = _measurement;
60  return true;
61  }
62 
63  virtual int measurementDimension() const {return 3;}
64 
65  virtual bool read(std::istream& is);
66  virtual bool write(std::ostream& os) const;
67  virtual void computeError();
68  virtual bool setMeasurementFromState();
69 
70  virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& /*from*/, OptimizableGraph::Vertex* /*to*/) {return 1.;}
71  virtual void initialEstimate(const OptimizableGraph::VertexSet& /*from_*/, OptimizableGraph::Vertex* /*to_*/);
72 
73  const ParameterSE3Offset* offsetParameter() { return _offsetParam; }
74 
75 protected:
76  virtual bool resolveCaches();
77  ParameterSE3Offset* _offsetParam;
78  CacheSE3Offset* _cache;
79 };
80 
81 EdgeSE3XYZPrior::EdgeSE3XYZPrior() : BaseUnaryEdge<3, Vector3d, VertexSE3>()
82 {
83  information().setIdentity();
84  setMeasurement(Vector3d::Zero());
85  _cache = 0;
86  _offsetParam = 0;
87  resizeParameters(1);
88  installParameter(_offsetParam, 0);
89 }
90 
92  assert(_offsetParam);
93  ParameterVector pv(1);
94  pv[0] = _offsetParam;
95  resolveCache(_cache, (OptimizableGraph::Vertex*)_vertices[0], "CACHE_SE3_OFFSET", pv);
96  return _cache != 0;
97 }
98 
99 bool EdgeSE3XYZPrior::read(std::istream& is)
100 {
101  int pid;
102  is >> pid;
103  if (!setParameterId(0, pid))
104  return false;
105 
106  // measured keypoint
107  Vector3d meas;
108  for (int i = 0; i < 3; i++) is >> meas[i];
109  setMeasurement(meas);
110 
111  // read covariance matrix (upper triangle)
112  if (is.good()) {
113  for (int i = 0; i < 3; i++) {
114  for (int j = i; j < 3; j++) {
115  is >> information()(i,j);
116  if (i != j)
117  information()(j,i) = information()(i,j);
118  }
119  }
120  }
121  return !is.fail();
122 }
123 
124 bool EdgeSE3XYZPrior::write(std::ostream& os) const {
125  os << _offsetParam->id() << " ";
126  for (int i = 0; i < 3; i++) os << measurement()[i] << " ";
127  for (int i = 0; i < 3; i++) {
128  for (int j = i; j < 3; j++) {
129  os << information()(i,j) << " ";
130  }
131  }
132  return os.good();
133 }
134 
136  const VertexSE3* v = static_cast<const VertexSE3*>(_vertices[0]);
137  _error = v->estimate().translation() - _measurement;
138 }
139 
141  const VertexSE3* v = static_cast<const VertexSE3*>(_vertices[0]);
142  _measurement = v->estimate().translation();
143  return true;
144 }
145 
146 void EdgeSE3XYZPrior::initialEstimate(const OptimizableGraph::VertexSet& /*from_*/, OptimizableGraph::Vertex* /*to_*/) {
147  VertexSE3 *v = static_cast<VertexSE3*>(_vertices[0]);
148  assert(v && "Vertex for the Prior edge is not set");
149 
150  Isometry3d newEstimate = _offsetParam->offset().inverse() * Translation3d(measurement());
151  if (_information.block<3,3>(0,0).array().abs().sum() == 0){ // do not set translation, as that part of the information is all zero
152  newEstimate.translation() = v->estimate().translation();
153  }
154  v->setEstimate(newEstimate);
155 }
156 
157 }
158 
159 #endif
g2o::EdgeSE3XYZPrior::initialEstimatePossible
virtual double initialEstimatePossible(const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *)
Definition: edge_se3_xyzprior.h:70
g2o::EdgeSE3XYZPrior::getMeasurementData
virtual bool getMeasurementData(double *d) const
Definition: edge_se3_xyzprior.h:57
Eigen
Eigen::Transform
g2o::EdgeSE3XYZPrior
Prior for a 3D pose with constraints only in xyz direction.
Definition: edge_se3_xyzprior.h:41
g2o::EdgeSE3XYZPrior::_offsetParam
ParameterSE3Offset * _offsetParam
Definition: edge_se3_xyzprior.h:77
g2o::EdgeSE3XYZPrior::measurementDimension
virtual int measurementDimension() const
Definition: edge_se3_xyzprior.h:63
g2o::EdgeSE3XYZPrior::setMeasurementFromState
virtual bool setMeasurementFromState()
Definition: edge_se3_xyzprior.h:140
g2o::EdgeSE3XYZPrior::EdgeSE3XYZPrior
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3XYZPrior()
Definition: edge_se3_xyzprior.h:81
os
ofstream os("timeSchurFactors.csv")
g2o::EdgeSE3XYZPrior::write
virtual bool write(std::ostream &os) const
Definition: edge_se3_xyzprior.h:124
j
std::ptrdiff_t j
g2o
Definition: edge_se3_xyzprior.h:34
g2o::EdgeSE3XYZPrior::read
virtual bool read(std::istream &is)
Definition: edge_se3_xyzprior.h:99
g2o::EdgeSE3XYZPrior::computeError
virtual void computeError()
Definition: edge_se3_xyzprior.h:135
d
d
g2o::EdgeSE3XYZPrior::initialEstimate
virtual void initialEstimate(const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *)
Definition: edge_se3_xyzprior.h:146
m
Matrix3f m
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Eigen::Map
g2o::EdgeSE3XYZPrior::setMeasurementData
virtual bool setMeasurementData(const double *d)
Definition: edge_se3_xyzprior.h:51
v
Array< int, Dynamic, 1 > v
g2o::EdgeSE3XYZPrior::setMeasurement
virtual void setMeasurement(const Vector3d &m)
Definition: edge_se3_xyzprior.h:47
Eigen::Translation
g2o::EdgeSE3XYZPrior::_cache
CacheSE3Offset * _cache
Definition: edge_se3_xyzprior.h:78
Eigen::Transform::translation
EIGEN_DEVICE_FUNC TranslationPart translation()
i
int i
g2o::EdgeSE3XYZPrior::resolveCaches
virtual bool resolveCaches()
Definition: edge_se3_xyzprior.h:91
g2o::EdgeSE3XYZPrior::offsetParameter
const ParameterSE3Offset * offsetParameter()
Definition: edge_se3_xyzprior.h:73


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:09