diff --git a/src/modules/DepthCamModule.cpp b/src/modules/DepthCamModule.cpp index 0da5bf265ef199309020c023830f5a39d2ddd99b..3f12335bbd86a08cb191c6fde4f56e3ae131107e 100644 --- a/src/modules/DepthCamModule.cpp +++ b/src/modules/DepthCamModule.cpp @@ -457,23 +457,8 @@ void DepthCamModule::draw(const int& contextID, for(; it != m_sentDetectedMarkers.end(); it++) { mark.push_back(m_depthID+it->id); mark.push_back(1); - /* - float depth = m_rgbDepthImg.at<cv::Vec3w> - (it->y, - it->x)[2]; - glm::vec4 pos((it->x/m_width-0.5) - *depth*tan(m_horizontalFOV/2.0)*2.0, - (0.5-it->y/m_height) - *depth*tan(m_verticalFOV/2.0)*2.0, - depth, 1.0); - pos = m_modelMat * pos; - pos[0]*=-1; - mark.push_back(pos[0]); - mark.push_back(pos[1]); - mark.push_back(pos[2]); - */ - - glm::vec3 pos(0, 0, 0); + + //compute position for(int c=0; c<4; c++) { float depth = m_rgbDepthImg.at<cv::Vec3w> (it->corners[c].y, @@ -487,11 +472,18 @@ void DepthCamModule::draw(const int& contextID, cpos[0]*=-1; for(int i=0; i<3; ++i) { it->corners3D[c][i]=cpos[i]; - pos[i]+=cpos[i]/4.0; } } - //compute position + float depth = m_rgbDepthImg.at<cv::Vec3w>(it->y, it->x)[2]; + glm::vec4 pos((it->x/m_width-0.5) + *depth*tan(m_horizontalFOV/2.0)*2.0, + (0.5-it->y/m_height) + *depth*tan(m_verticalFOV/2.0)*2.0, + depth, 1.0); + pos = m_modelMat * pos; + pos[0]*=-1; + mark.push_back(pos[0]); mark.push_back(pos[1]); mark.push_back(pos[2]); @@ -526,6 +518,8 @@ void DepthCamModule::draw(const int& contextID, mark.push_back(markQ[1]); mark.push_back(markQ[2]); mark.push_back(markQ[3]); + + //set attribute m_attributesMap["marker_output"]->setFloats(mark); mark.clear(); } @@ -1618,8 +1612,13 @@ void DepthCamModule::drawDelaunay(cv::Mat& img, cv::Subdiv2D& subdiv, void DepthCamModule::debugMarkers(const bool& val) { if(!val && m_debuggingMarkers) { - destroyWindow("markers"); - destroyWindow("filled_markers"); + try{ + destroyWindow("markers"); + destroyWindow("filled_markers"); + } + catch(...) { + + } waitKey(10); } m_debuggingMarkers=val;