bool extractAbsolutePrior(Eigen::Isometry3f& priorMean, Matrix6f& priorInfo, const DrawableFrame* current){ VertexSE3* currentVertex =current->_vertex; ImuData* imuData = 0; OptimizableGraph::Data* d = currentVertex->userData(); while(d) { ImuData* imuData_ = dynamic_cast<ImuData*>(d); if (imuData_){ imuData = imuData_; } d=d->next(); } if (imuData){ Eigen::Matrix3d R=imuData->getOrientation().matrix(); Eigen::Matrix3d Omega = imuData->getOrientationCovariance().inverse(); priorMean.setIdentity(); priorInfo.setZero(); for (int c = 0; c<3; c++) for (int r = 0; r<3; r++) priorMean.linear()(r,c)=R(r,c); for (int c = 0; c<3; c++) for (int r = 0; r<3; r++) priorInfo(r+3,c+3)=Omega(r,c); return true; } return false; }
HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ if (typeid(*element).name()!=_typeName) return 0; initializeDrawActionsCache(); refreshPropertyPtrs(params_); if (! _previousParams) return this; if (_show && !_show->value()) return this; VertexSE3* that = static_cast<VertexSE3*>(element); glColor3f(POSE_VERTEX_COLOR); glPushMatrix(); glMultMatrixd(that->estimate().matrix().data()); opengl::drawArrow2D(_triangleX->value(), _triangleY->value(), _triangleX->value()*.3f); drawCache(that->cacheContainer(), params_); drawUserData(that->userData(), params_); if(_showId && _showId->value()){ float scale = _idSize ? _idSize->value() : 1.f; drawId(std::to_string(that->id()), scale); } glPopMatrix(); return this; }
HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ if (typeid(*element).name()!=_typeName) return 0; if (! _cacheDrawActions){ _cacheDrawActions = HyperGraphActionLibrary::instance()->actionByName("draw"); } refreshPropertyPtrs(params_); if (! _previousParams) return this; if (_show && !_show->value()) return this; VertexSE3* that = static_cast<VertexSE3*>(element); glColor3f(0.5f,0.5f,0.8f); glPushMatrix(); glTranslatef((float)that->estimate().translation().x(),(float)that->estimate().translation().y(),(float)that->estimate().translation().z()); AngleAxisd aa(that->estimate().rotation()); glRotatef((float)RAD2DEG(aa.angle()),(float)aa.axis().x(),(float)aa.axis().y(),(float)aa.axis().z()); if (_triangleX && _triangleY){ drawTriangle(_triangleX->value(), _triangleY->value()); } CacheContainer* caches=that->cacheContainer(); if (caches){ for (CacheContainer::iterator it=caches->begin(); it!=caches->end(); it++){ Cache* c = it->second; (*_cacheDrawActions)(c, params_); } } if (that->userData() && _cacheDrawActions ) (*_cacheDrawActions)(that->userData(), params_); glPopMatrix(); return this; }
HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ if (typeid(*element).name()!=_typeName) return 0; if (! _cacheDrawActions){ _cacheDrawActions = HyperGraphActionLibrary::instance()->actionByName("draw"); } refreshPropertyPtrs(params_); if (! _previousParams) return this; if (_show && !_show->value()) return this; VertexSE3* that = static_cast<VertexSE3*>(element); glColor3f(0.5f,0.5f,0.8f); glPushMatrix(); glMultMatrixd(that->estimate().matrix().data()); if (_triangleX && _triangleY){ drawTriangle(_triangleX->value(), _triangleY->value()); } CacheContainer* caches=that->cacheContainer(); if (caches){ for (CacheContainer::iterator it=caches->begin(); it!=caches->end(); it++){ Cache* c = it->second; (*_cacheDrawActions)(c, params_); } } OptimizableGraph::Data* d=that->userData(); while (d && _cacheDrawActions ){ (*_cacheDrawActions)(d, params_); d=d->next(); } glPopMatrix(); return this; }