vertex_pose.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2016,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the institute nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Notes:
37  * The following class is derived from a class defined by the
38  * g2o-framework. g2o is licensed under the terms of the BSD License.
39  * Refer to the base class source for detailed licensing information.
40  *
41  * Author: Christoph Rösmann
42  *********************************************************************/
43 
44 #ifndef VERTEX_POSE_H_
45 #define VERTEX_POSE_H_
46 
47 #include <g2o/config.h>
48 #include <g2o/core/base_vertex.h>
49 #include <g2o/core/hyper_graph_action.h>
50 #include <g2o/stuff/misc.h>
51 
53 
54 namespace teb_local_planner
55 {
56 
63 class VertexPose : public g2o::BaseVertex<3, PoseSE2 >
64 {
65 public:
66 
71  VertexPose(bool fixed = false)
72  {
74  setFixed(fixed);
75  }
76 
82  VertexPose(const PoseSE2& pose, bool fixed = false)
83  {
84  _estimate = pose;
85  setFixed(fixed);
86  }
87 
94  VertexPose(const Eigen::Ref<const Eigen::Vector2d>& position, double theta, bool fixed = false)
95  {
96  _estimate.position() = position;
97  _estimate.theta() = theta;
98  setFixed(fixed);
99  }
100 
108  VertexPose(double x, double y, double theta, bool fixed = false)
109  {
110  _estimate.x() = x;
111  _estimate.y() = y;
112  _estimate.theta() = theta;
113  setFixed(fixed);
114  }
115 
120 
126  PoseSE2& pose() {return _estimate;}
127 
133  const PoseSE2& pose() const {return _estimate;}
134 
135 
141  Eigen::Vector2d& position() {return _estimate.position();}
142 
148  const Eigen::Vector2d& position() const {return _estimate.position();}
149 
154  double& x() {return _estimate.x();}
155 
160  const double& x() const {return _estimate.x();}
161 
166  double& y() {return _estimate.y();}
167 
172  const double& y() const {return _estimate.y();}
173 
178  double& theta() {return _estimate.theta();}
179 
184  const double& theta() const {return _estimate.theta();}
185 
189  virtual void setToOriginImpl()
190  {
191  _estimate.setZero();
192  }
193 
200  virtual void oplusImpl(const double* update)
201  {
202  _estimate.plus(update);
203  }
204 
211  virtual bool read(std::istream& is)
212  {
213  is >> _estimate.x() >> _estimate.y() >> _estimate.theta();
214  return true;
215  }
216 
223  virtual bool write(std::ostream& os) const
224  {
225  os << _estimate.x() << " " << _estimate.y() << _estimate.theta();
226  return os.good();
227  }
228 
229  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
230 };
231 
232 }
233 
234 #endif // VERTEX_POSE_H_
PoseSE2 & pose()
Access the pose.
Definition: vertex_pose.h:126
const Eigen::Vector2d & position() const
Access the 2D position part (read-only)
Definition: vertex_pose.h:148
virtual bool read(std::istream &is)
Read an estimate from an input stream. First the x-coordinate followed by y and the yaw angle...
Definition: vertex_pose.h:211
double & y()
Access the y-coordinate the pose.
Definition: vertex_pose.h:166
VertexPose(bool fixed=false)
Default constructor.
Definition: vertex_pose.h:71
const PoseSE2 & pose() const
Access the pose (read-only)
Definition: vertex_pose.h:133
virtual void oplusImpl(const double *update)
Define the update increment . A simple addition for the position. The angle is first added to the pre...
Definition: vertex_pose.h:200
const double & x() const
Access the x-coordinate the pose (read-only)
Definition: vertex_pose.h:160
double & theta()
Access the orientation part (yaw angle) of the pose.
Definition: vertex_pose.h:178
double & x()
Access the x-coordinate the pose.
Definition: vertex_pose.h:154
VertexPose(const Eigen::Ref< const Eigen::Vector2d > &position, double theta, bool fixed=false)
Construct pose using a given 2D position vector and orientation.
Definition: vertex_pose.h:94
VertexPose(double x, double y, double theta, bool fixed=false)
Construct pose using single components x, y, and the yaw angle.
Definition: vertex_pose.h:108
const double & y() const
Access the y-coordinate the pose (read-only)
Definition: vertex_pose.h:172
virtual bool write(std::ostream &os) const
Write the estimate to an output stream First the x-coordinate followed by y and the yaw angle...
Definition: vertex_pose.h:223
This class implements a pose in the domain SE2: The pose consist of the position x and y and an orie...
Definition: pose_se2.h:57
Eigen::Vector2d & position()
Access the 2D position part.
Definition: vertex_pose.h:141
VertexPose(const PoseSE2 &pose, bool fixed=false)
Construct pose using a given PoseSE2.
Definition: vertex_pose.h:82
This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized...
Definition: vertex_pose.h:63
const double & theta() const
Access the orientation part (yaw angle) of the pose (read-only)
Definition: vertex_pose.h:184
~VertexPose()
Destructs the VertexPose.
Definition: vertex_pose.h:119
virtual void setToOriginImpl()
Set the underlying estimate (2D vector) to zero.
Definition: vertex_pose.h:189


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Jun 5 2019 19:25:10