12 Eigen::Transform<double, 3, 1> poseinv = pose->
estimate().inverse();
19 unsigned int index = 3*i;
34 unsigned int index = 3*i;
36 _error[index+1] = m[1] - _measurement[index+1];
37 _error[index+2] = m[2] - _measurement[index+2];
46 unsigned int rows = 3*(
_vertices.size()-1);
52 for(
unsigned int i=1; i<
_vertices.size(); i++){
56 unsigned int index=3*(i-1);
59 Ji.block<3,3>(index,0) = -Matrix3D::Identity();
62 Ji(index, 4) = -2*Zcam(2);
63 Ji(index, 5) = 2*Zcam(1);
65 Ji(index+1, 3) = 2*Zcam(2);
66 Ji(index+1, 4) = -0.0;
67 Ji(index+1, 5) = -2*Zcam(0);
69 Ji(index+2, 3) = -2*Zcam(1);
70 Ji(index+2, 4) = 2*Zcam(0);
71 Ji(index+2, 5) = -0.0;
76 Jj.block<3,3>(index,0) = poseRot;
92 unsigned int index = 3*i;
97 for(
unsigned int i=0; i<_observedPoints*3; i++){
99 for(
unsigned int j=i; j<_observedPoints*3; j++){
104 for(
unsigned int j=0; j<i; j++){
120 unsigned int index = 3*i;
125 for(
unsigned int i=0; i<_observedPoints*3; i++){
126 for(
unsigned int j=i; j<_observedPoints*3; j++){
147 estimate_this[i] =
true;
151 for(std::set<HyperGraph::Vertex*>::iterator it=fixed.begin(); it!=fixed.end(); it++){
152 for(
unsigned int i=1; i<
_vertices.size(); i++){
154 if(vert->
id() == (*it)->id()) estimate_this[i-1] =
false;
158 for(
unsigned int i=1; i<
_vertices.size(); i++){
159 if(estimate_this[i-1]){
160 unsigned int index = 3*(i-1);
173 for(std::set<HyperGraph::Vertex *>::iterator it=fixed.begin(); it!=fixed.end(); it++){
int id() const
returns the id
virtual void computeError()
Eigen::Matrix< double, 3, 1, Eigen::ColMajor > Vector3D
std::set< Vertex * > VertexSet
unsigned int _observedPoints
virtual void initialEstimate(const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *)
virtual bool setMeasurementFromState()
virtual double initialEstimatePossible(const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *)
virtual bool write(std::ostream &os) const
write the vertex to a stream
void setSize(int vertices)
Eigen::Matrix< double, 3, 3, Eigen::ColMajor > Matrix3D
Vertex for a tracked point in space.
virtual bool read(std::istream &is)
read the vertex from a stream, i.e., the internal state of the vertex
void setEstimate(const EstimateType &et)
set the estimate for the vertex also calls updateCache()
A general case Vertex for optimization.
virtual void resize(size_t size)
EIGEN_STRONG_INLINE const InformationType & information() const
information matrix of the constraint
const EstimateType & estimate() const
return the current estimate of the vertex
3D pose Vertex, represented as an Isometry3D
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor > MatrixXD
virtual void linearizeOplus()
std::vector< JacobianType, Eigen::aligned_allocator< JacobianType > > _jacobianOplus
jacobians of the edge (w.r.t. oplus)
VertexContainer _vertices