//////////////////////////////////////////////////////////////////////////////
// Name: drawShadedBorder
// Purpose:
// Draw a special shaded border.
// Parameters: None
// Returns: None
///
void drawShadedBorder()
{
(void) drawBoundingBox(BOUNDING_BOX_ONE);
(void) drawBoundingBox(BOUNDING_BOX_TWO);
(void) drawBoundingBox(BOUNDING_BOX_THREE);
(void) drawBoundingBox(BOUNDING_BOX_FOUR);
return;
}
static void drawTree(const TransformState& transState, const OctreeNode* node)
{
Imath::Box3f bbox(node->center - Imath::V3f(node->radius),
node->center + Imath::V3f(node->radius));
drawBoundingBox(transState, bbox, Imath::C3f(1));
drawBoundingBox(transState, node->bbox, Imath::C3f(1,0,0));
for (int i = 0; i < 8; ++i)
{
OctreeNode* n = node->children[i];
if (n)
drawTree(transState, n);
}
}
void SimplePopup::Render()
{
mClosed = false;
float modX = (SCREEN_WIDTH_F / 2)-5;
JRenderer *r = JRenderer::GetInstance();
string detailedInformation = getDetailedInformation(mDeckInformation->getFilename());
#if !defined (PSP)
JQuadPtr statsholder = WResourceManager::Instance()->RetrieveTempQuad("statsholder.png");//new graphics statsholder
//const float textHeight = mTextFont->GetHeight() * mMaxLines;
//r->FillRect(0,0,SCREEN_WIDTH_F,SCREEN_HEIGHT_F,ARGB(220,15,15,15));
if(statsholder.get())
r->RenderQuad(statsholder.get(),0,0,0,SCREEN_WIDTH_F/statsholder->mWidth,SCREEN_HEIGHT_F/statsholder->mHeight);
#endif
r->FillRoundRect(mX+modX+3, mY + 7, 190.f, 148.f, 0, ARGB( 240, 15, 15, 15 ) );
// currently causes a crash on the PSP when drawing the corners.
// TODO: clean up the image ot make it loook cleaner. Find solution to load gfx to not crash PSP
#if 0
r->DrawRoundRect(mX, mY + 2, mWidth + 11, textHeight - 12, 2.0f, ARGB( 255, 125, 255, 0) );
drawBoundingBox( mX-3, mY, mWidth + 3, textHeight );
#endif
mTextFont->DrawString(detailedInformation.c_str(), modX+mX + 9 , mY + 15);
}
//**********************************************************************************
// draw
//
// draws all children of this node
//**********************************************************************************
void Node::draw(VECTOR3D camera_position)
{
//draw all of the children of this Node
std::vector<Node *>::iterator iter = m_children.begin();
while(iter != m_children.end())
{
(*iter)->m_num_lights = m_num_lights;
(*iter)->draw(camera_position);
//(*iter)->computeBoundingBox();
//if(m_bounding_boxes)
//(*iter)->drawBoundingBox();
iter++;
}
//compute the BB of this node
//computeBoundingBox();
//if BB's are enabled, then show the BB of this node too
if(m_bounding_boxes)
drawBoundingBox();
return;
}
void Projectile::draw()
{
cg::Vector3d min = _position - _size;
cg::Vector3d max = _position + _size;
GLfloat mat_emission[] = {0.0f,0.0f,0.0f,1.0f};
GLfloat mat_ambient[] = {0.25f,0.25f,0.25f,1.0f};
GLfloat mat_diffuse[] = {0.4f,0.4f,0.4f,1.0f};
GLfloat mat_specular[] = {0.774f,0.774f,0.774f,1.0f};
GLfloat mat_shininess[] = {76.8f};
glPushMatrix();
{
glMaterialfv(GL_FRONT,GL_EMISSION,mat_emission);
glMaterialfv(GL_FRONT,GL_AMBIENT,mat_ambient);
glMaterialfv(GL_FRONT,GL_DIFFUSE,mat_diffuse);
glMaterialfv(GL_FRONT,GL_SPECULAR,mat_specular);
glMaterialfv(GL_FRONT,GL_SHININESS,mat_shininess);
if (_start && _debug)
drawBoundingBox();
glTranslated(_position[0], _position[1], _position[2]);
if (_start) {
if(_debug)
debugDrawAxis();
glutSolidSphere(0.7, 16, 16);
}
}
glPopMatrix();
}
void allBoxes(boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer,
std::vector<hypothesis> final_hypothesis)
{
//cloud_mutex.lock();
viewer->removeAllShapes();
for(int z=0; z<final_hypothesis.size();++z)
{
if(final_hypothesis.at(z).distance < 100)
{
std::stringstream ddd;
ddd << final_hypothesis.at(z).object_name;
ddd << "\n" << "(";
ddd << final_hypothesis.at(z).distance;
ddd << ")";
viewer->addText3D(ddd.str().c_str(),
final_hypothesis.at(z).cluster->points[0],
0.02,1,1,1,
boost::lexical_cast<std::string>(z));
drawBoundingBox(final_hypothesis.at(z).cluster,viewer,z);
}
}
final_hypothesis.clear();
viewer->spinOnce();
//cloud_mutex.unlock();
}
/**
* CABT::renderTree
* @date Modified Apr 17, 2006
*/
void CABT::renderTree(CFrustum& oFrustum, SABTNode* pNode, bool bContained)
{
ACEASSERT(pNode);
// Process subnodes
CFrustum::EFrustumTest eTest = CFrustum::TEST_IN;
eTest = oFrustum.containsAABB(pNode->m_oAABB);
if(eTest == CFrustum::TEST_OUT)
return;
// Draw geometry, if the node is a leaf.
if(pNode->m_bIsLeaf)
{
for(size_t v = 0; v < pNode->m_nVertCount/3; ++v)
m_pRenderQueue->addRenderable(&pNode->m_pTris[v]);
m_nNumTrianglesRendered += pNode->m_nVertCount / 3;
#ifdef _DEBUG
static int i = 0;
if(true)
{
drawBoundingBox(pNode->m_oAABB, D3DCOLOR_XRGB(255, 0, 0));
}
#endif
}
if(pNode->m_pLeft) renderTree(oFrustum, pNode->m_pLeft, eTest == CFrustum::TEST_IN);
if(pNode->m_pRight) renderTree(oFrustum, pNode->m_pRight, eTest == CFrustum::TEST_IN);
}
void GLWidget::paintGL()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// glClearColor(1.0,1.0,1.0,1.0);
Vec3f volumeScale = Vec3f(volumeDim);
volumeScale = volumeScale / volumeScale.norm();
modelViewMatrix = Mat4f(rotation, translation);
Mat4f transform = Mat4f::ortho(0, 1, 0, 1, -1, 1);
Mat4f invModelView = modelViewMatrix.inverse();
volumeRayCastingProgram->setUniform("renderMode", int(mode));
volumeRayCastingProgram->setUniform("volumeDim", Vec3f(volumeDim));
volumeRayCastingProgram->setUniform("volumeScale", volumeScale);
volumeRayCastingProgram->setUniform("transform", transform);
volumeRayCastingProgram->setUniform("invModelView", invModelView);
volumeRayCastingProgram->setUniform("aspect", float(width()) / float(height()));
volumeRayCastingProgram->setUniform("cotFOV", projectionMatrix.elem[1][1]);
volumeRayCastingProgram->setUniform("azimuth", azimuth);
volumeRayCastingProgram->setUniform("elevation", elevation);
volumeRayCastingProgram->setUniform("clipPlaneDepth", clipPlaneDepth);
volumeRayCastingProgram->setUniform("clipFlag", gb_Clipflag);
volumeRayCastingProgram->setUniform("deltaStep", deltaStep);
volumeRayCastingProgram->setUniform("ambient", gb_ambient);
volumeRayCastingProgram->setUniform("ambientCoE", gb_AmbientCoE);
volumeRayCastingProgram->setUniform("specularC", gb_SpecularC);
volumeRayCastingProgram->setUniform("diffuseC", gb_DiffuseC);
volumeRayCastingProgram->setUniform("lightcolor", gb_lightcolor);
volumeRayCastingProgram->setUniform("posdir", gb_PosOrDirect);
volumeRayCastingProgram->setUniform("fshiness", gb_Shiness);
volumeRayCastingProgram->setUniform("fstrength", gb_LightStrength);
volumeRayCastingProgram->setUniform("fcosattent", gb_CosAttent);
volumeRayCastingProgram->setUniform("flineattent", gb_LineAttent);
volumeRayCastingProgram->setUniform("fquadattent", gb_QuadAttent);
volumeRayCastingProgram->setUniform("lighttype", lighttype);
volumeRayCastingProgram->setUniform("preintFlag", gb_PreIntFlag);
volumeRayCastingProgram->setUniform("LAOFlag", gb_AOFlag);
volumeRayCastingProgram->setUniform("OCSize", opacityCorrection);
volumeRayCastingProgram->setTexture("volumeTex", volumeTex);
// volumeRayCastingProgram->setTexture("volumeTexCubic", volumeTexCubic);
volumeRayCastingProgram->setTexture("transferFuncTex", transferFuncTex);
volumeRayCastingProgram->setTexture("preInt", preIntTex);
volumeRayCastingProgram->setTexture("LAOTex", LAOTex);
volumeRayCastingProgram->begin();
rectVertexArray->drawArrays(GL_TRIANGLE_FAN, 4);
volumeRayCastingProgram->end();
// preIntProgram->setTexture("TF", transferFuncTex);
// preIntProgram->setUniform("deltastep",deltaStep);
// preIntProgram->begin();
// preIntVertexArray->drawArrays(GL_TRIANGLE_FAN, 4);
// preIntProgram->end();
// boundBoxProgram->setUniform("transform", transform);
// boundBoxProgram->begin();
// rectVertexArray->drawArrays(GL_LINES, 4);
// boundBoxProgram->end();
drawBoundingBox();
}
void
createBBoxList(BBox *bbox, GLuint *bboxList, int wire)
{
*bboxList = glGenLists(1);
glNewList(*bboxList, GL_COMPILE);
drawBoundingBox(bbox, wire);
glEndList();
}
void Octree::drawBoundingBox(OctreeNode* root)
{
if(root)
root->drawBoundingBox();
if(root->End)
return;
for(int i=0;i<8;i++)
drawBoundingBox(root->Descendant[i]);
}
/**
* @function grabber_callback
*/
void grabber_callback( const CloudConstPtr& _cloud ) {
mutex.lock();
double dt; clock_t ts, tf;
// Segment the new input
gTts.process( _cloud, gShowSegmentation );
// Show it
gRgbImg = gTts.getRgbImg();
gXyzImg = gTts.getXyzImg();
// Draw bounding box
drawBoundingBox();
mutex.unlock();
}
void BitObject::draw(BitObjectDrawMode mode, Image<T_or_RGB>& img,
const T_or_RGB& color, float opacity)
{
switch(mode)
{
case BODMnone: break;
case BODMshape: drawShape(img, color, opacity); break;
case BODMoutline: drawOutline(img, color, opacity); break;
case BODMbbox: drawBoundingBox(img, color, opacity); break;
default: LERROR("Unknown BitObjectDrawMode: %i - ignoring!",mode);
}
}
void GridSystem::drawMesh(bool walkaround)
{
glBegin(GL_QUADS);
GLfloat Lt1diff[] = { 1.0, 1.0, 1.0, 1.0 };
GLfloat Lt1pos[] = { 4.0, 4.0, 4.0, 1.0 };
glLightfv(GL_LIGHT0 + 1, GL_DIFFUSE, Lt1diff);
glLightfv(GL_LIGHT0 + 1, GL_POSITION, Lt1pos);
for (int i = 0; i < grids.size(); i++)
{
for (int j = 0; j < grids[0].size(); j++)
{
if (i > 0 && j > 0)
{
grids[i - 1][j - 1]->getTexture().chooseTexture();
//grids[i - 1][j - 1].getMaterial().bindMat();
glVertex(grids[i - 1][j - 1]->getXYZ());
grids[i][j - 1]->getTexture().chooseTexture();
//grids[i][j - 1].getMaterial().bindMat();
glVertex(grids[i][j - 1]->getXYZ());
grids[i][j]->getTexture().chooseTexture();
//grids[i][j]->getMaterial()->bindMat();
glVertex(grids[i][j]->getXYZ());
grids[i - 1][j]->getTexture().chooseTexture();
//grids[i - 1][j]->getMaterial()->bindMat();
glVertex(grids[i - 1][j]->getXYZ());
}
}
}
glEnd();
if (walkaround){
drawBoundingBox(8.0);
}
else {
drawBoundingBox(2.0);
}
}
void Moon::draw()
{
if (!visible) return;
glPushMatrix();
draw_triangle2d(0.4f, polygonFill);
glPopMatrix();
if (drawBBox)
{
drawBoundingBox();
}
}
void ofxBoxObj::draw(int _level){
ofSetColor(255, 255);
box->draw(_level);
ofPushMatrix();
ofTranslate(x-getScaledWidth()*0.5,y-getScaledHeight()*0.5);
ofScale(scale, scale);
if (*bDebug)
drawBoundingBox();
ofPopMatrix();
}
void Octree::drawBoundingBox(OctreeNode* root, int depth)
{
if(root)
root->drawBoundingBox();
if(root->End)
return;
if(depth==0)
return;
else
{
depth--;
for(int i=0;i<8;i++)
drawBoundingBox(root->Descendant[i], depth);
}
}
void King::draw()
{
glPushMatrix();
{
if (_debug)
drawBoundingBox();
glTranslatef(_position[0], _position[1], _position[2]);
glRotated(_rotation, 0, 1, 0);
glScaled(2, 2, 2);
glEnable(GL_NORMALIZE);
_model->drawModel(_type);
glDisable(GL_NORMALIZE);
}
glPopMatrix();
}
void EditPointTool::drawImpl(QPainter* painter, MapWidget* widget)
{
auto num_selected_objects = map()->selectedObjects().size();
if (num_selected_objects > 0)
{
drawSelectionOrPreviewObjects(painter, widget, text_editor != nullptr);
if (!text_editor)
{
Object* object = *map()->selectedObjectsBegin();
if (num_selected_objects == 1 &&
object->getType() == Object::Text &&
!object->asText()->hasSingleAnchor())
{
drawBoundingPath(painter, widget, object->asText()->controlPoints(), hover_state.testFlag(OverFrame) ? active_color : selection_color);
}
else if (selection_extent.isValid())
{
auto active = hover_state.testFlag(OverFrame) && !hover_state.testFlag(OverObjectNode);
drawBoundingBox(painter, widget, selection_extent, active ? active_color : selection_color);
}
if (num_selected_objects <= max_objects_for_handle_display)
{
for (const auto object: map()->selectedObjects())
{
auto active = hover_state.testFlag(OverObjectNode) && hover_object == object;
auto hover_point = active ? this->hover_point : no_point;
pointHandles().draw(painter, widget, object, hover_point, true, PointHandles::NormalHandleState);
}
}
}
}
// Text editor
if (text_editor)
{
painter->save();
widget->applyMapTransform(painter);
text_editor->draw(painter, widget);
painter->restore();
}
// Box selection
if (isDragging() && box_selection)
drawSelectionBox(painter, widget, click_pos_map, cur_pos_map);
}
void draw() {
// The camera will modify the view of everything between camera.being()
// and camera.end().
camera.begin();
// Draw the bounding box.
drawBoundingBox();
// Draw the particles.
drawParticles();
// Draw lines connecting the particles.
drawParticleLines();
// End the camera.
camera.end();
}
void SimplePopup::Render()
{
mClosed = false;
JRenderer *r = JRenderer::GetInstance();
string detailedInformation = getDetailedInformation(mDeckInformation->getFilename());
const float textHeight = mTextFont->GetHeight() * mMaxLines;
r->FillRoundRect(mX, mY + 2, mWidth + 11, textHeight - 12, 2.0f, ARGB( 255, 0, 0, 0 ) );
// currently causes a crash on the PSP when drawing the corners.
// TODO: clean up the image ot make it loook cleaner. Find solution to load gfx to not crash PSP
#if 0
r->DrawRoundRect(mX, mY + 2, mWidth + 11, textHeight - 12, 2.0f, ARGB( 255, 125, 255, 0) );
drawBoundingBox( mX-3, mY, mWidth + 3, textHeight );
#endif
mTextFont->DrawString(detailedInformation.c_str(), mX + 9 , mY + 10);
}
void Bird::draw()
{
GLfloat mat_emission[] = {0.0f,0.0f,0.0f,1.0f};
GLfloat mat_ambient[] = {0.2f,0.2f,0.7f,1.0f};
GLfloat mat_diffuse[] = {0.2f,0.2f,0.7f,1.0f};
GLfloat mat_specular[] = {0.2f,0.2f,0.7f,1.0f};
GLfloat mat_shininess[] = {0.1f};
glPushMatrix();
{
glMaterialfv(GL_FRONT,GL_EMISSION,mat_emission);
glMaterialfv(GL_FRONT,GL_AMBIENT,mat_ambient);
glMaterialfv(GL_FRONT,GL_DIFFUSE,mat_diffuse);
glMaterialfv(GL_FRONT,GL_SPECULAR,mat_specular);
glMaterialfv(GL_FRONT,GL_SHININESS,mat_shininess);
glScaled(0.7, 0.7, 0.7);
if(_debug)
drawBoundingBox();
glTranslated(_position[0], _position[1], _position[2]);
glRotated(_bodyRot, 0, 1, 0);
glEnable(GL_NORMALIZE);
_model->drawModel(_bodyName);
glPushMatrix();
{
glRotated(_wingRot, 0, 0, 1);
_model->drawModel(_wingsName);
}
glPopMatrix();
glPushMatrix();
{
glRotated(-_wingRot, 0, 0, 1);
glRotated(180, 0, 1, 0);
_model->drawModel(_wingsName);
}
glPopMatrix();
glDisable(GL_NORMALIZE);
}
glPopMatrix();
}
void Tracker::setFirstFrame(const Mat frame, vector<Point2f> bb, string title, Stats& stats)
{
cv::Point *ptMask = new cv::Point[bb.size()];
const Point* ptContain = { &ptMask[0] };
int iSize = static_cast<int>(bb.size());
for (size_t i=0; i<bb.size(); i++) {
ptMask[i].x = static_cast<int>(bb[i].x);
ptMask[i].y = static_cast<int>(bb[i].y);
}
first_frame = frame.clone();
cv::Mat matMask = cv::Mat::zeros(frame.size(), CV_8UC1);
cv::fillPoly(matMask, &ptContain, &iSize, 1, cv::Scalar::all(255));
detector->detectAndCompute(first_frame, matMask, first_kp, first_desc);
stats.keypoints = (int)first_kp.size();
drawBoundingBox(first_frame, bb);
putText(first_frame, title, Point(0, 60), FONT_HERSHEY_PLAIN, 5, Scalar::all(0), 4);
object_bb = bb;
delete[] ptMask;
}
unsigned int drawHardcodedModelRaw(unsigned int modelType)
{
switch (modelType)
{
case OBJ_FOG : drawFog(); break;
case OBJ_PLANE : drawObjPlane(0,0,0, 0.5); break;
case OBJ_GRIDPLANE : drawGridPlane( 0.0 , 0.0 , 0.0, 1.0); break;
case OBJ_AXIS : drawAxis(0,0,0,1.0); break;
case OBJ_CUBE : drawCube(); break;
case OBJ_PYRAMID : drawPyramid(); break;
case OBJ_SPHERE : drawSphere( SPHERE_QUALITY ); break;
case OBJ_INVISIBLE : /*DONT DRAW ANYTHING*/ break;
case OBJ_QUESTION : drawQuestion(); break;
case OBJ_BBOX : drawBoundingBox(0,0,0,-1.0,-1.0,-1.0,1.0,1.0,1.0); break;
default :
return 0;
break;
}
return 1;
}
void MD3Node::draw(VECTOR3D camera_postion)
{
if(m_visible == false)
{
//cerr << "not drawing lara" << endl;
return;
}
//cerr << "Drawing lara" << endl;
m_model.DrawModel();
//computeBoundingBox(); //tws 27.4 -- this is called by the parent?
//draw the bounding box if necessary //tws 27.4 -- this is called by the parent?
if(m_bounding_boxes)
{
drawBoundingBox();
}
return;
}
void AxisAlignedCutPlane::process() {
if (imageInport_.isConnected()) {
utilgl::activateTargetAndCopySource(outport_, imageInport_, ImageType::ColorDepth);
} else {
utilgl::activateAndClearTarget(outport_, ImageType::ColorDepth);
}
if (!boundingBoxDrawer_) {
boundingBoxDrawer_ =
getNetwork()->getApplication()->getMeshDrawerFactory()->create(boundingBoxMesh_.get());
}
utilgl::GlBoolState depthTest(GL_DEPTH_TEST, true);
drawBoundingBox();
TextureUnitContainer cont;
sliceShader_.activate();
utilgl::setShaderUniforms(sliceShader_, camera_, "camera_");
utilgl::bindAndSetUniforms(sliceShader_, cont, volume_);
if (nearestInterpolation_.get()) {
glTexParameteri(GL_TEXTURE_3D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_3D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
} else {
glTexParameteri(GL_TEXTURE_3D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_3D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
}
if (!disableTF_.get()) {
utilgl::bindAndSetUniforms(sliceShader_, cont, tf_);
}
xSlide_.draw(sliceShader_);
ySlide_.draw(sliceShader_);
zSlide_.draw(sliceShader_);
sliceShader_.deactivate();
utilgl::deactivateCurrentTarget();
}
void drawBoxCorner(float l, float w, float h)
{
drawBoundingBox(Vector3(0,0,0),Vector3(l,w,h));
}
void drawBox(float l, float w, float h)
{
float L = l*0.5f, W = w*0.5f, H = h*0.5f;
drawBoundingBox(Vector3(-L,-W,-H),Vector3(L,W,H));
}
void View3D::paintGL()
{
if (m_badOpenGL)
return;
QTime frameTimer;
frameTimer.start();
// Get window size
double dPR = getDevicePixelRatio();
int w = width() * dPR;
int h = height() * dPR;
// detecting a change in the device pixel ratio, since only the new QWindow (Qt5) would
// provide a signal for screen changes and this is the easiest solution
if (dPR != m_devicePixelRatio)
{
m_devicePixelRatio = dPR;
resizeGL(w, h);
}
glBindFramebuffer(GL_DRAW_FRAMEBUFFER, m_incrementalFramebuffer);
//--------------------------------------------------
// Draw main scene
TransformState transState(Imath::V2i(w, h),
m_camera.projectionMatrix(),
m_camera.viewMatrix());
glClearDepth(1.0f);
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LEQUAL);
glClearColor(m_backgroundColor.redF(), m_backgroundColor.greenF(),
m_backgroundColor.blueF(), 1.0f);
if (!m_incrementalDraw)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
std::vector<const Geometry*> geoms = selectedGeometry();
// Draw bounding boxes
if(m_drawBoundingBoxes && !m_incrementalDraw)
{
if (m_boundingBoxShader->isValid())
{
QGLShaderProgram &boundingBoxShader = m_boundingBoxShader->shaderProgram();
// shader
boundingBoxShader.bind();
// matrix stack
transState.setUniforms(boundingBoxShader.programId());
for (size_t i = 0; i < geoms.size(); ++i)
{
drawBoundingBox(transState, geoms[i]->getVAO("boundingbox"), geoms[i]->boundingBox().min,
Imath::C3f(1), geoms[i]->shaderId("boundingbox")); //boundingBoxShader.programId()
}
}
}
// Draw meshes and lines
if (!m_incrementalDraw)
{
drawMeshes(transState, geoms);
// Generic draw for any other geometry
// (TODO: make all geometries use this interface, or something similar)
// FIXME - Do generic quality scaling
const double quality = 1;
for (size_t i = 0; i < geoms.size(); ++i)
geoms[i]->draw(transState, quality);
}
// Aim for 40ms frame time - an ok tradeoff for desktop usage
const double targetMillisecs = 40;
double quality = m_drawCostModel.quality(targetMillisecs, geoms, transState,
m_incrementalDraw);
// Render points
DrawCount drawCount = drawPoints(transState, geoms, quality, m_incrementalDraw);
// Measure frame time to update estimate for how much geometry we can draw
// with a reasonable frame rate
glFinish();
int frameTime = frameTimer.elapsed();
if (!geoms.empty())
m_drawCostModel.addSample(drawCount, frameTime);
// Debug: print bar showing how well we're sticking to the frame time
// int barSize = 40;
// std::string s = std::string(barSize*frameTime/targetMillisecs, '=');
// if ((int)s.size() > barSize)
// s[barSize] = '|';
// tfm::printfln("%12f %4d %s", quality, frameTime, s);
// TODO: this should really render a texture onto a quad and not use glBlitFramebuffer
glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0);
glBindFramebuffer(GL_READ_FRAMEBUFFER, m_incrementalFramebuffer);
glBlitFramebuffer(0,0,w,h, 0,0,w,h,
GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT, GL_NEAREST); // has to be GL_NEAREST to work with DEPTH
// Draw a grid for orientation purposes
if (m_drawGrid)
drawGrid();
// Draw overlay stuff, including cursor position.
if (m_drawCursor)
drawCursor(transState, m_cursorPos, 10);
//drawCursor(transState, m_camera.center(), 10);
// Draw overlay axes
if (m_drawAxes)
drawAxes();
// Set up timer to draw a high quality frame if necessary
if (!drawCount.moreToDraw)
m_incrementalFrameTimer->stop();
else
m_incrementalFrameTimer->start(10);
m_incrementalDraw = true;
glFrameBufferStatus(m_incrementalFramebuffer);
glCheckError();
}
int main (int argc, char** argv)
{
PointCloudT::Ptr cloud (new PointCloudT);
PointCloudT::Ptr new_cloud (new PointCloudT);
bool new_cloud_available_flag = false;
//pcl::Grabber* grab = new pcl::OpenNIGrabber ();
PointCloudT::Ptr ddd;
boost::function<void (const PointCloudT::ConstPtr&)> f =
boost::bind(&grabberCallback, _1, cloud, &new_cloud_available_flag);
grab->registerCallback (f);
viewer->registerKeyboardCallback(keyboardEventOccurred);
grab->start ();
bool first_time = true;
FILE* objects;
objects = fopen ("objects.txt","a");
while(!new_cloud_available_flag)
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
new_cloud_available_flag=false;
////////////////////
// invert correction
////////////////////
Eigen::Matrix4f transMat = Eigen::Matrix4f::Identity();
transMat (1,1) = -1;
////////////////////
// pass filter
////////////////////
PointCloudT::Ptr passed_cloud;
pcl::PassThrough<PointT> pass;
passed_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
////////////////////
// voxel grid
////////////////////
PointCloudT::Ptr voxelized_cloud;
voxelized_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
pcl::VoxelGrid<PointT> vg;
vg.setLeafSize (0.001, 0.001, 0.001);
////////////////////
// sac segmentation
////////////////////
PointCloudT::Ptr cloud_f;
PointCloudT::Ptr cloud_plane;
PointCloudT::Ptr cloud_filtered;
cloud_f = boost::shared_ptr<PointCloudT>(new PointCloudT);
cloud_plane = boost::shared_ptr<PointCloudT> (new PointCloudT);
cloud_filtered = boost::shared_ptr<PointCloudT> (new PointCloudT);
pcl::SACSegmentation<PointT> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
////////////////////
// euclidean clustering
////////////////////
std::vector<pcl::PointIndices> cluster_indices;
std::vector<PointCloudT::Ptr> extracted_clusters;
pcl::search::KdTree<PointT>::Ptr eucl_tree (new pcl::search::KdTree<PointT>);
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (0.04);
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (eucl_tree);
PointCloudT::Ptr cloud_cluster;
////////////////////
// vfh estimate
////////////////////
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::search::KdTree<PointT>::Ptr vfh_tree1 (new pcl::search::KdTree<PointT> ());
pcl::VFHEstimation<PointT, pcl::Normal, pcl::VFHSignature308> vfh;
pcl::search::KdTree<PointT>::Ptr vfh_tree2 (new pcl::search::KdTree<PointT> ());
std::vector<pcl::PointCloud<pcl::VFHSignature308>::Ptr> computed_vfhs;
ne.setSearchMethod (vfh_tree1);
ne.setRadiusSearch (0.05);
vfh.setSearchMethod (vfh_tree2);
vfh.setRadiusSearch (0.05);
pcl::PointCloud<pcl::Normal>::Ptr normals;
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs;
////////////////////
// nearest neighbour
////////////////////
int k = 6;
std::string kdtree_idx_file_name = "kdtree.idx";
std::string training_data_h5_file_name = "training_data.h5";
std::string training_data_list_file_name = "training_data.list";
// std::vector<vfh_model> models;
// flann::Matrix<float> data;
// loadFileList (models, training_data_list_file_name);
// flann::load_from_file (data,
// training_data_h5_file_name,
// "training_data");
//
// flann::Index<flann::ChiSquareDistance<float> > index (data,
// flann::SavedIndexParams
// ("kdtree.idx"));
////////////////////
// process nearest neighbour
////////////////////
std::vector<hypothesis> final_hypothesis;
final_hypothesis.clear();
double last = pcl::getTime();
while (! viewer->wasStopped())
{
if (new_cloud_available_flag)
{
new_cloud_available_flag = false;
double now = pcl::getTime();
////////////////////
// pass filter
////////////////////
//passed_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
////////////////////
// voxel grid
////////////////////
//voxelized_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
////////////////////
// sac segmentation
////////////////////
//cloud_f = boost::shared_ptr<PointCloudT>(new PointCloudT);
//cloud_plane = boost::shared_ptr<PointCloudT> (new PointCloudT);
//cloud_filtered = boost::shared_ptr<PointCloudT> (new PointCloudT);
////////////////////
// euclidean clustering
////////////////////
extracted_clusters.clear();
cluster_indices.clear();
////////////////////
// vfh estimate
////////////////////
computed_vfhs.clear();
////////////////////
// nearest neighbour
////////////////////
cloud_mutex.lock();
//displayCloud(viewer,cloud);
boost::thread displayCloud_(displayCloud,viewer,cloud);
if(now-last > 13 || first_time)
{
first_time = false;
last=now;
////////////////////
// invert correction
////////////////////
pcl::transformPointCloud(*cloud,*new_cloud,transMat);
////////////////////
// pass filter
////////////////////
pass.setInputCloud (new_cloud);
pass.setFilterFieldName ("x");
pass.setFilterLimits (-0.5, 0.5);
//pass.setFilterLimitsNegative (true);
pass.filter (*passed_cloud);
////////////////////
// voxel grid
////////////////////
vg.setInputCloud (passed_cloud);
vg.filter (*voxelized_cloud);
////////////////////
// sac segmentation
////////////////////
*cloud_filtered = *voxelized_cloud;
int i=0, nr_points = (int) voxelized_cloud->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Couldnt estimate a planar model for the dataset.\n";
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
////////////////////
// euclidean clustering
////////////////////
// Creating the KdTree object for the search method of the extraction
eucl_tree->setInputCloud (cloud_filtered);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
//PointCloudT::Ptr cloud_cluster (new PointCloudT);
cloud_cluster = boost::shared_ptr<PointCloudT>(new PointCloudT);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_filtered->points [*pit]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
extracted_clusters.push_back(cloud_cluster);
}
cloud_cluster.reset();
////////////////////
// vfh estimate
////////////////////
for (int z=0; z<extracted_clusters.size(); ++z)
{
vfhs = boost::shared_ptr<pcl::PointCloud<pcl::VFHSignature308> > (new pcl::PointCloud<pcl::VFHSignature308>);
normals = boost::shared_ptr<pcl::PointCloud<pcl::Normal> > (new pcl::PointCloud<pcl::Normal>);
ne.setInputCloud (extracted_clusters.at(z));
ne.compute (*normals);
vfh.setInputCloud (extracted_clusters.at(z));
vfh.setInputNormals (normals);
vfh.compute (*vfhs);
computed_vfhs.push_back(vfhs);
}
vfhs.reset();
normals.reset();
////////////////////
// nearest neighbour
////////////////////
std::vector<vfh_model> models;
flann::Matrix<int> k_indices;
flann::Matrix<float> k_distances;
flann::Matrix<float> data;
loadFileList (models, training_data_list_file_name);
flann::load_from_file (data,
training_data_h5_file_name,
"training_data");
flann::Index<flann::ChiSquareDistance<float> > index
(data,
flann::SavedIndexParams
("kdtree.idx"));
for(int z=0; z<computed_vfhs.size(); ++z)
{
vfh_model histogram;
histogram.second.resize(308);
for (size_t i = 0; i < 308; ++i)
{
histogram.second[i] = computed_vfhs.at(z)->points[0].histogram[i];
}
index.buildIndex ();
nearestKSearch (index, histogram, k, k_indices, k_distances);
hypothesis x;
x.distance = k_distances[0][0];
size_t index = models.at(k_indices[0][0]).first.find("_v",5);
x.object_name = models.at(k_indices[0][0]).first.substr(5,index-5);
ddd = boost::shared_ptr<PointCloudT>(new PointCloudT);
pcl::transformPointCloud(*extracted_clusters.at(z),*ddd,transMat);
x.cluster = ddd;
ddd.reset();
std::string filename ="";
filename += "inputcloud_" + boost::lexical_cast<std::string>(j+1);
filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd";
x.cluster_name = filename.c_str();
final_hypothesis.push_back(x);
x.cluster.reset();
//delete x;
// std::string filename ="";
// filename += "inputcloud_" + boost::lexical_cast<std::string>(j+1);
// filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd";
// const char* filen = filename.c_str();
// fprintf(objects,"%s",filen);
// fprintf(objects,"::");
// fprintf(objects,models.at (k_indices[0][0]).first.c_str());
// fprintf(objects,"::");
// fprintf(objects,"%f",k_distances[0][0]);
// fprintf(objects,"\n");
}
delete[] k_indices.ptr ();
delete[] k_distances.ptr ();
delete[] data.ptr ();
std::cout << final_hypothesis.size() << std::endl;
viewer->removeAllShapes();
for(int z=0; z<final_hypothesis.size();++z)
{
if(final_hypothesis.at(z).distance < 100)
{
fprintf(objects,"%s",final_hypothesis.at(z).cluster_name.c_str());
fprintf(objects,"::");
fprintf(objects,"%s",final_hypothesis.at(z).object_name.c_str());
fprintf(objects,"::");
fprintf(objects,"%f",final_hypothesis.at(z).distance);
fprintf(objects,"\n");
std::stringstream ddd;
ddd << final_hypothesis.at(z).object_name;
ddd << "\n" << "(";
ddd << final_hypothesis.at(z).distance;
ddd << ")";
viewer->addText3D(ddd.str().c_str(),
final_hypothesis.at(z).cluster->points[0],
0.02,1,1,1,
boost::lexical_cast<std::string>(z));
drawBoundingBox(final_hypothesis.at(z).cluster,viewer,z);
}
}
//boost::thread allBoxes_(allBoxes,viewer,final_hypothesis);
//allBoxes_.join();
viewer->spinOnce();
final_hypothesis.clear();
j++;
}
// for(int z=0; z<extracted_clusters.size(); ++z)
// {
// //viewer->addPointCloud<PointT>(extracted_clusters.at(z),
// // boost::lexical_cast<std::string>(z));
//
// std::string filename ="";
// filename += "inputcloud_" + boost::lexical_cast<std::string>(j);
// filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd";
// pcl::io::savePCDFile(filename,*extracted_clusters.at(z),false);
// }
// for(int z=0; z<computed_vfhs.size(); ++z)
// {
// //viewer->addPointCloud<PointT>(extracted_clusters.at(z),
// // boost::lexical_cast<std::string>(z));
//
// std::string filename ="";
// filename += "inputcloud_" + boost::lexical_cast<std::string>(j);
// filename += "_" + boost::lexical_cast<std::string>(z) + "_vfhs.pcd";
// pcl::io::savePCDFileASCII<pcl::VFHSignature308> (filename, *computed_vfhs.at(z));
// }
//viewer->removeAllShapes();
// viewer->removeAllPointClouds();
// viewer->setCameraPosition(0, 0, 0, 0, 0, 1, 0, -1, 0);
// pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
// viewer->addPointCloud<PointT>(cloud,rgb,"input_cloud");
// viewer->spinOnce();
//std::cout << final_hypothesis.at(0).cluster->points[0];
//boost::this_thread::sleep(boost::posix_time::milliseconds(10));
displayCloud_.join();
cloud_mutex.unlock();
}
}
grab->stop();
return 0;
}
void display()
{
void* image = 0;
void* depth = 0;
float aux[16];
uint32_t timestamp;
freenect_sync_get_video(&image, ×tamp, 0, FREENECT_VIDEO_RGB);
freenect_sync_get_depth(&depth, ×tamp, 0, FREENECT_DEPTH_11BIT);
mFfusion->update(depth);
glDisable(GL_TEXTURE_2D);
glPointSize(1);
glMatrixMode(GL_MODELVIEW);
glEnableClientState(GL_VERTEX_ARRAY);
glEnableClientState(GL_COLOR_ARRAY);
glRotated(180, 0, 0, 1);
if(mDrawFlags[0])
{
glPushMatrix();
const double* t = getCamera()->getTransform();
float t2[16];
std::copy(t, t+16, t2);
mRenderer->measure(*mFfusion->getVolume(), t2);
glBindBuffer(GL_ARRAY_BUFFER, mRenderer->getGLVertexBuffer());
glVertexPointer(3, GL_FLOAT, 3*sizeof(float), 0);
glBindBuffer(GL_ARRAY_BUFFER, mRenderer->getGLNormalBuffer());
glColorPointer(3, GL_FLOAT, 3*sizeof(float), 0);
glDrawArrays(GL_POINTS, 0, mRenderer->getNumVertices());
glPopMatrix();
}
if(mDrawFlags[1])
{
glBindBuffer(GL_ARRAY_BUFFER, mFfusion->getVolumeMeasurement()->getGLVertexBuffer());
glVertexPointer(3, GL_FLOAT, 12, 0);
glBindBuffer(GL_ARRAY_BUFFER, mFfusion->getVolumeMeasurement()->getGLNormalBuffer());
glColorPointer(3, GL_FLOAT, 12, 0);
glDrawArrays(GL_POINTS, 0, 640*480);
}
if(mDrawFlags[2])
{
glPushMatrix();
transposeTransform(aux, mFfusion->getLocation());
glMultMatrixf(aux);
glBindBuffer(GL_ARRAY_BUFFER, mFfusion->getMeasurement()->getGLVertexBuffer(1));
glVertexPointer(3, GL_FLOAT, 12, 0);
glBindBuffer(GL_ARRAY_BUFFER, mFfusion->getMeasurement()->getGLNormalBuffer(1));
glColorPointer(3, GL_FLOAT, 12, 0);
glDrawArrays(GL_POINTS, 0, 640*480);
glPopMatrix();
}
drawBoundingBox();
drawSensor();
/*if(mDrawFlags[0])
{
MarchingCubes* mc = mFfusion->getMarchingCubes();
glBindBuffer(GL_ARRAY_BUFFER, mc->getGLVertexBuffer());
glVertexPointer(4, GL_FLOAT, 4*sizeof(float), 0);
glBindBuffer(GL_ARRAY_BUFFER, mc->getGLNormalBuffer());
glColorPointer(4, GL_FLOAT, 4*sizeof(float), 0);
glDrawArrays(GL_TRIANGLES, 0, mc->getActiveVertices());
}*/
}
