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(); }
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(); }