1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 G. Grisetti, R. Kuemmerle, 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_SIMULATOR_
28 #define G2O_SIMULATOR_
29 
30 #include <string>
31 #include <set>
32 #include <list>
33 #include "g2o/config.h"
34 #include "g2o/types/slam3d/types_slam3d.h"
35 #include "g2o/stuff/sampler.h"
36 #include "g2o_simulator_api.h"
37 
38 namespace g2o {
39 
40 class World;
41 class BaseSensor;
42 
43 class G2O_SIMULATOR_API BaseWorldObject{
44   public:
45     BaseWorldObject(World* world_=0) {_world = world_; _vertex=0;}
46     virtual ~BaseWorldObject();
setWorld(World * world_)47     void setWorld(World* world_) {_world = world_;}
world()48     World* world() {return _world;}
49     OptimizableGraph* graph();
vertex()50     OptimizableGraph::Vertex* vertex() {return _vertex;}
51     virtual void setVertex(OptimizableGraph::Vertex* vertex_);
52   protected:
53     OptimizableGraph* _graph;
54     OptimizableGraph::Vertex* _vertex;
55     World* _world;
56 };
57 
58 template <class VertexType_>
59 class WorldObject: public BaseWorldObject, VertexType_{
60   public:
61     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
62     typedef VertexType_ VertexType;
63     typedef typename VertexType_::EstimateType EstimateType;
BaseWorldObject(world_)64     WorldObject(World* world_=0): BaseWorldObject(world_){
65       _vertex = new VertexType();
66     }
setVertex(OptimizableGraph::Vertex * vertex_)67     virtual void setVertex(OptimizableGraph::Vertex* vertex_){
68       if(! dynamic_cast<VertexType*>(vertex_))
69         return;
70       _vertex = vertex_;
71     }
72 
vertex()73     VertexType* vertex() {
74       if (! _vertex) return 0;
75       return dynamic_cast<VertexType*>(_vertex);
76     }
77 };
78 
79 class G2O_SIMULATOR_API BaseRobot {
80   public:
BaseRobot(World * world_,const std::string & name_)81     BaseRobot(World* world_, const std::string& name_){_world = world_; _name = name_; }
setWorld(World * world_)82     void setWorld(World* world_) {_world = world_;}
world()83     World* world() {return _world;}
name()84     const std::string& name() const {return _name;}
85     OptimizableGraph* graph();
86     bool addSensor(BaseSensor* sensor);
sensors()87     const std::set<BaseSensor*> sensors() {return _sensors;}
88     virtual void sense();
89   protected:
90     World* _world;
91     std::set<BaseSensor*> _sensors;
92     std::string _name;
93 };
94 
95 class G2O_SIMULATOR_API World
96 {
97   public:
World(OptimizableGraph * graph_)98     World(OptimizableGraph* graph_) {_graph = graph_; _runningId=0; _paramId=0;}
graph()99     OptimizableGraph* graph() {return _graph;}
100     bool addRobot(BaseRobot* robot);
101     bool addWorldObject(BaseWorldObject* worldObject);
102     bool addParameter(Parameter* p);
103 
objects()104     std::set<BaseWorldObject*>& objects() {return _objects;}
robots()105     std::set<BaseRobot*>&  robots() {return _robots; }
106   protected:
107     std::set<BaseWorldObject*> _objects;
108     std::set<BaseRobot*> _robots;
109     OptimizableGraph* _graph;
110     int _runningId;
111     int _paramId;
112 };
113 
114 template <class RobotPoseObject>
115 class Robot: public BaseRobot{
116   public:
117     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
118     typedef RobotPoseObject PoseObject;
119     typedef std::list<PoseObject*> TrajectoryType;
120     typedef typename PoseObject::VertexType VertexType;
121     typedef typename PoseObject::EstimateType PoseType;
122 
Robot(World * world_,const std::string & name_)123     Robot(World* world_, const std::string& name_): BaseRobot(world_, name_){}
relativeMove(const PoseType & movement_)124     virtual void relativeMove(const PoseType& movement_) {
125       _pose=_pose*movement_;
126       move(_pose);
127     }
128 
move(const PoseType & pose_)129     virtual void move(const PoseType& pose_) {
130       _pose=pose_;
131       if (world()) {
132         PoseObject* po=new PoseObject();
133         po->vertex()->setEstimate(_pose);
134         world()->addWorldObject(po);
135         _trajectory.push_back(po);
136       }
137     }
138 
trajectory()139     TrajectoryType& trajectory() {return _trajectory;}
pose()140     const PoseType& pose() const {return _pose;}
141   protected:
142     TrajectoryType _trajectory;
143     PoseType _pose;
144 };
145 
146 class G2O_SIMULATOR_API BaseSensor{
147  public:
BaseSensor(const std::string & name_)148   BaseSensor(const std::string& name_){ _name = name_;}
robot()149   inline BaseRobot* robot() {return _robot;}
setRobot(BaseRobot * robot_)150   inline void setRobot(BaseRobot* robot_) {_robot = robot_;}
151   World* world();
152   OptimizableGraph* graph();
parameters()153   std::vector<Parameter*> parameters() {return _parameters;}
154   virtual void sense() = 0;
addParameters()155   virtual void addParameters() {}
156  protected:
157   std::string _name;
158   std::vector<Parameter*> _parameters;
159   BaseRobot* _robot;
160 };
161 
162 template <class RobotType_, class EdgeType_>
163 class UnarySensor: public BaseSensor {
164   public:
165     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
166     typedef RobotType_ RobotType;
167     typedef typename RobotType::PoseObject PoseObject;
168     typedef typename RobotType::TrajectoryType TrajectoryType;
169     typedef typename RobotType::PoseObject::VertexType PoseVertexType;
170     typedef EdgeType_ EdgeType;
171     typedef typename EdgeType::InformationType InformationType;
172 
UnarySensor(const std::string & name)173     UnarySensor(const std::string& name): BaseSensor(name) {
174       _information.setIdentity();
175     }
176 
setInformation(const InformationType & information_)177     void setInformation(const InformationType& information_ ) {
178       _information = information_ ;
179       _sampler.setDistribution(_information.inverse());
180     }
181 
information()182     const InformationType& information() {return _information; }
183 
sense()184     virtual void sense() {
185       _robotPoseObject = 0;
186       // set the robot pose
187       if (! robot())
188         return;
189 
190       RobotType* r =dynamic_cast<RobotType*>(robot());
191       if (!r)
192         return;
193 
194       if(! r->trajectory().empty())
195         _robotPoseObject = *(r->trajectory().rbegin());
196 
197       if (! world() || ! graph())
198         return;
199 
200       EdgeType* e=mkEdge();
201       if (e) {
202         e->setMeasurementFromState();
203         addNoise(e);
204         graph()->addEdge(e);
205       }
206     }
207 
208 
209   protected:
210     PoseObject* _robotPoseObject;
211     InformationType _information;
212 
mkEdge()213     EdgeType* mkEdge(){
214       PoseVertexType* robotVertex = (PoseVertexType*)_robotPoseObject->vertex();
215       EdgeType* e = new EdgeType();
216       e->vertices()[0]=robotVertex;
217       e->information().setIdentity();
218       return e;
219     }
220     GaussianSampler<typename EdgeType::ErrorVector, InformationType> _sampler;
addNoise(EdgeType *)221     virtual void addNoise(EdgeType*){};
222 };
223 
224 template <class RobotType_, class EdgeType_, class WorldObjectType_>
225 class BinarySensor: public BaseSensor {
226   public:
227     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
228     typedef RobotType_ RobotType;
229     typedef typename RobotType::PoseObject PoseObject;
230     typedef typename RobotType::TrajectoryType TrajectoryType;
231     typedef typename RobotType::PoseObject::VertexType PoseVertexType;
232     typedef EdgeType_ EdgeType;
233     typedef WorldObjectType_ WorldObjectType;
234     typedef typename WorldObjectType::VertexType VertexType;
235     typedef typename EdgeType::InformationType InformationType;
236 
BinarySensor(const std::string & name)237     BinarySensor(const std::string& name): BaseSensor(name) {
238       _information.setIdentity();
239     }
240 
setInformation(const InformationType & information_)241     void setInformation(const InformationType& information_ ) {
242       _information = information_ ;
243       _sampler.setDistribution(_information.inverse());
244     }
245 
information()246     const InformationType& information() {return _information; }
247 
sense()248     virtual void sense() {
249       _robotPoseObject = 0;
250       // set the robot pose
251       if (! robot())
252         return;
253 
254       RobotType* r =dynamic_cast<RobotType*>(robot());
255       if (!r)
256         return;
257 
258       if(! r->trajectory().empty())
259         _robotPoseObject = *(r->trajectory().rbegin());
260 
261       if (! world() || ! graph())
262         return;
263 
264       // naive search. just for initial testing
265       for(std::set<BaseWorldObject*>::iterator it=world()->objects().begin(); it!=world()->objects().end(); ++it){
266         WorldObjectType * wo = dynamic_cast<WorldObjectType*>(*it);
267         if (wo){
268           EdgeType* e=mkEdge(wo);
269           if (e) {
270             e->setMeasurementFromState();
271             addNoise(e);
272             graph()->addEdge(e);
273           }
274         }
275       }
276     }
277 
278 
279   protected:
280     PoseObject* _robotPoseObject;
281     InformationType _information;
282 
mkEdge(WorldObjectType * object)283     EdgeType* mkEdge(WorldObjectType* object){
284       PoseVertexType* robotVertex = (PoseVertexType*)_robotPoseObject->vertex();
285       EdgeType* e = new EdgeType();
286       e->vertices()[0]=robotVertex;
287       e->vertices()[1]=object->vertex();
288       e->information().setIdentity();
289       return e;
290     }
291     GaussianSampler<typename EdgeType::ErrorVector, InformationType> _sampler;
addNoise(EdgeType *)292     virtual void addNoise(EdgeType*){};
293 };
294 
295 } // end namespace
296 
297 #endif
298