void DrawableCloud::draw() {
    if(_parameter->show() && _cloud) {
      glPushMatrix();
      glMultMatrixf(_transformation.data());
      if(_drawablePoints)
	_drawablePoints->draw();
      if(_drawableNormals)
	_drawableNormals->draw();
      if(_drawableCovariances)
	_drawableCovariances->draw();
      if(_drawableCorrespondences)
	_drawableCorrespondences->draw();

      glPushMatrix();
      Eigen::Isometry3f sensorOffset;
      sensorOffset.translation() = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
      Eigen::Quaternionf quaternion = Eigen::Quaternionf(-.5f, -0.5f, 0.5f, 0.5f);
      sensorOffset.linear() = quaternion.toRotationMatrix();
      sensorOffset.matrix().row(3) << 0.0f, 0.0f, 0.0f, 1.0f;
      glMultMatrixf(sensorOffset.data());
      glColor4f(1,0,0,0.5);
      glPopMatrix();

      glPopMatrix();
    }
  }
  void DrawableTransformCovariance::updateCovarianceDrawList() {
    GLParameterTransformCovariance *covarianceParameter = dynamic_cast<GLParameterTransformCovariance*>(_parameter);
    glNewList(_covarianceDrawList, GL_COMPILE); 
    if(_covariance != Eigen::Matrix3f::Zero() && 
       covarianceParameter && 
       covarianceParameter->show() && 
       covarianceParameter->scale() > 0.0f) {
      float scale = covarianceParameter->scale();
      Eigen::Vector4f color = covarianceParameter->color();
      
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver;
      eigenSolver.computeDirect(_covariance, Eigen::ComputeEigenvectors);

      Eigen::Vector3f lambda = eigenSolver.eigenvalues();      
      Eigen::Isometry3f I = Eigen::Isometry3f::Identity();
      I.linear() = eigenSolver.eigenvectors();
      I.translation() = Eigen::Vector3f(_mean.x(), _mean.y(), _mean.z());
      
      float sx = sqrt(lambda[0]) * scale;
      float sy = sqrt(lambda[1]) * scale;
      float sz = sqrt(lambda[2]) * scale;
      
      glPushMatrix();
      glMultMatrixf(I.data());
      glColor4f(color[0], color[1], color[2], color[3]);
      glScalef(sx, sy, sz);
      glCallList(_sphereDrawList);
      glPopMatrix();	    
    }
    glEndList();
  }
Exemple #3
0
 void drawBounds() {
   ViewBase::Spec spec;
   if (!mRenderer->mViewClient.getSpec(mViewId, spec)) return;
   if (spec.mClipPlanes.size() == 0) return;
   std::vector<Eigen::Vector3f> vertices;
   std::vector<std::vector<int> > faces;
   maps::Utils::polyhedronFromPlanes(spec.mClipPlanes, vertices, faces);
   glMatrixMode(GL_MODELVIEW);
   glPushMatrix();
   if (spec.mRelativeLocation) {
     glMultMatrixf(mLatestTransform.data());
   }
   glColor3f(mAttributes.mColor[0], mAttributes.mColor[1],
             mAttributes.mColor[2]);
   glLineWidth(3);
   for (size_t i = 0; i < faces.size(); ++i) {
     glBegin(GL_LINE_LOOP);
     for (size_t j = 0; j < faces[i].size(); ++j) {
       Eigen::Vector3f pt = vertices[faces[i][j]];
       glVertex3f(pt[0], pt[1], pt[2]);
     }
     glEnd();
   }
   glPopMatrix();
 }
  void DrawableUncertainty::updateCovarianceDrawList() {
    GLParameterUncertainty *uncertaintyParameter = dynamic_cast<GLParameterUncertainty*>(_parameter);
    glNewList(_covarianceDrawList, GL_COMPILE); 
    if(_covarianceDrawList &&
       _covariances && 
       uncertaintyParameter && 
       uncertaintyParameter->ellipsoidScale() > 0.0f) {
      uncertaintyParameter->applyGLParameter();
      Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver;
      float ellipsoidScale = uncertaintyParameter->ellipsoidScale();
      for(size_t i = 0; i < _covariances->size(); i += uncertaintyParameter->step()) {
	Gaussian3f &gaussian3f = _covariances->at(i);
	Eigen::Matrix3f covariance = gaussian3f.covarianceMatrix();
	Eigen::Vector3f mean = gaussian3f.mean();
	eigenSolver.computeDirect(covariance, Eigen::ComputeEigenvectors);
	Eigen::Vector3f eigenValues = eigenSolver.eigenvalues();      
	Eigen::Isometry3f I = Eigen::Isometry3f::Identity();
	I.linear() = eigenSolver.eigenvectors();
	I.translation() = mean;
	float sx = sqrt(eigenValues[0]) * ellipsoidScale;
	float sy = sqrt(eigenValues[1]) * ellipsoidScale;
	float sz = sqrt(eigenValues[2]) * ellipsoidScale;
	glPushMatrix();
	glMultMatrixf(I.data());	
	sx = sx;
	sy = sy;
	sz = sz;
	glScalef(sx, sy, sz);
	glCallList(_sphereDrawList);
	glPopMatrix();	    
      }   
    }
    glEndList();
  }
  void DrawableCovariances::updateCovarianceDrawList() {
    GLParameterCovariances *covariancesParameter = dynamic_cast<GLParameterCovariances*>(_parameter);
    glNewList(_covarianceDrawList, GL_COMPILE); 
    if(_covariances && 
       covariancesParameter && 
       covariancesParameter->show() && 
       covariancesParameter->ellipsoidScale() > 0.0f) {
      float ellipsoidScale = covariancesParameter->ellipsoidScale();
      Eigen::Vector4f colorLowCurvature = covariancesParameter->colorLowCurvature();
      Eigen::Vector4f colorHighCurvature = covariancesParameter->colorHighCurvature();
      float curvatureThreshold = covariancesParameter->curvatureThreshold();
      for(size_t i = 0; i < _covariances->size(); i += covariancesParameter->step()) {
	Stats cov = _covariances->at(i);
	Eigen::Vector3f lambda = cov.eigenValues();
	Eigen::Isometry3f I = Eigen::Isometry3f::Identity();
	I.linear() = cov.eigenVectors();
	if(cov.n() == 0 )
	  continue;
	I.translation() = Eigen::Vector3f(cov.mean()[0], cov.mean()[1], cov.mean()[2]);
	float sx = sqrt(lambda[0]) * ellipsoidScale;
	float sy = sqrt(lambda[1]) * ellipsoidScale;
	float sz = sqrt(lambda[2]) * ellipsoidScale;
	float curvature = cov.curvature();
	glPushMatrix();
	glMultMatrixf(I.data());
	if(curvature > curvatureThreshold) {
	  glColor4f(colorHighCurvature[0] - curvature, colorHighCurvature[1], colorHighCurvature[2], colorHighCurvature[3]);
	  sx = ellipsoidScale;
	  sy = ellipsoidScale;
	  sz = ellipsoidScale;
	}
	else {
	  glColor4f(colorLowCurvature[0], colorLowCurvature[1] - curvature, colorLowCurvature[2], colorLowCurvature[3]);
	  sx = 1e-03;
	  sy = ellipsoidScale;
	  sz = ellipsoidScale;
	}

	glScalef(sx, sy, sz);
	glCallList(_sphereDrawList);
	glPopMatrix();	    
      }   
    }
    glEndList();
  }