Fix aruco marker position error with small markers

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) {
+        try{
             destroyWindow("markers");
             destroyWindow("filled_markers");
+        }
+        catch(...) {
+
+        }
         waitKey(10);
     }
     m_debuggingMarkers=val;