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