g2o
sensor_pointxyz_depth.cpp
Go to the documentation of this file.
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 #include "sensor_pointxyz_depth.h"
28 
29 namespace g2o {
30  using namespace std;
31  using namespace Eigen;
32 
33  // SensorPointXYZDepth
35  _offsetParam = 0;
36  _information.setIdentity();
37  _information*=1000;
39  }
40 
42  if (! _robotPoseObject)
43  return false;
44  assert(to && to->vertex());
45  VertexType* v=to->vertex();
46  VertexType::EstimateType pose=v->estimate();
47  VertexType::EstimateType delta = _sensorPose.inverse()*pose;
48  Vector3d translation=delta;
49  double range2=translation.squaredNorm();
50  if (range2>_maxRange2)
51  return false;
52  if (range2<_minRange2)
53  return false;
54  translation.normalize();
55  // the cameras have the z in front
56  double bearing=acos(translation.z());
57  if (fabs(bearing)>_fov)
58  return false;
59  return true;
60  }
61 
63  if (!_offsetParam)
65  assert(world());
67  }
68 
71  e->setMeasurement(e->measurement()+n);
73  }
74 
76  if (! _offsetParam){
77  return;
78  }
80  RobotType* r= dynamic_cast<RobotType*>(robot());
81  std::list<PoseObject*>::reverse_iterator it=r->trajectory().rbegin();
82  int count = 0;
83  while (it!=r->trajectory().rend() && count < 1){
84  if (!_robotPoseObject)
85  _robotPoseObject = *it;
86  it++;
87  count++;
88  }
89  if (!_robotPoseObject)
90  return;
91  _sensorPose = _robotPoseObject->vertex()->estimate()*_offsetParam->offset();
92  for (std::set<BaseWorldObject*>::iterator it=world()->objects().begin();
93  it!=world()->objects().end(); it++){
94  WorldObjectType* o=dynamic_cast<WorldObjectType*>(*it);
95  if (o && isVisible(o)){
96  EdgeType* e=mkEdge(o);
98  if (e && graph()) {
99  graph()->addEdge(e);
101  addNoise(e);
102  }
103  }
104  }
105  }
106 
107 }
virtual void setMeasurement(const Vector3D &m)
bool addParameter(Parameter *p)
Definition: simulator.cpp:102
SensorPointXYZDepth(const std::string &name_)
ParameterSE3Offset * _offsetParam
parameters for a camera
const Isometry3D & offset() const
rotation of the offset as 3x3 rotation matrix
GaussianSampler< typename EdgeType::ErrorVector, InformationType > _sampler
Definition: simulator.h:271
SampleType generateSample()
Definition: sampler.h:67
bool setParameterId(int argNum, int paramId)
World * world()
Definition: simulator.cpp:69
EIGEN_STRONG_INLINE void setInformation(const InformationType &information)
Definition: base_edge.h:69
TrajectoryType & trajectory()
Definition: simulator.h:119
int id() const
Definition: parameter.h:45
OptimizableGraph * graph()
Definition: simulator.cpp:75
BaseRobot * robot()
Definition: simulator.h:129
std::set< BaseWorldObject * > & objects()
Definition: simulator.h:284
bool isVisible(WorldObjectType *to)
virtual bool addEdge(HyperGraph::Edge *e)
VertexType * vertex()
Definition: simulator.h:72
void setInformation(const InformationType &information_)
Definition: simulator.h:221
EIGEN_STRONG_INLINE const Measurement & measurement() const
accessor functions for the measurement represented by the edge
Definition: base_edge.h:75