HyperGraphElementAction* VertexPointXYZDrawAction::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;
    VertexPointXYZ* that = static_cast<VertexPointXYZ*>(element);


    glPushMatrix();
    glPushAttrib(GL_ENABLE_BIT | GL_POINT_BIT);
    glDisable(GL_LIGHTING);
    glColor3f(LANDMARK_VERTEX_COLOR);
    float ps = _pointSize ? _pointSize->value() :  1.f;
    glTranslatef((float)that->estimate()(0),(float)that->estimate()(1),(float)that->estimate()(2));
    opengl::drawPoint(ps);
    glPopAttrib();
    drawCache(that->cacheContainer(), params);
    drawUserData(that->userData(), params);
    glPopMatrix();
    return this;
}
Пример #2
0
  HyperGraphElementAction* VertexSE3PriorDrawAction::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;

    VertexSE3Prior* that = static_cast<VertexSE3Prior*>(element);

    glColor3f(POSE_VERTEX_PRIOR_COLOR);
    glPushMatrix();
    glMultMatrixd(that->estimate().matrix().data());
    opengl::drawArrow2D(_triangleX->value(), _triangleY->value(), _triangleX->value()*.3f);
    drawCache(that->cacheContainer(), params_);
    drawUserData(that->userData(), params_);
    glPopMatrix();
    return this;
  }