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