Simplified examples, display now shows cone edges as opposed to top d…

…own view

controllers/example/example.py

@@ -31,21 +31,25 @@ class Intrinsic:
 
 intrinsic = Intrinsic(depth)
 
+# we are going to limit the region we look at to ignore the sky/car etc
+regionTop = int( depth.getHeight()*0.51 )
+regionBase = int( depth.getHeight()*0.68 )     
+
 prevTime = 0
+
+#fill the whole background with black
+display.setColor( 0x000000 )
+display.fillRectangle( 0, 0, display.getWidth(), display.getHeight() )
 
 while driver.step() != -1:
     # get a range image
     image = depth.getRangeImageArray()
 
-    # clear the display    
-    display.setColor( 0x000000 )
-    display.fillRectangle( 0, 0, display.getWidth(), display.getHeight() )
-    display.setColor( 0xFFFFFF )
-
-    # we are going to limit the region we look at to ignore the sky/car etc
-    regionTop = 370
-    regionBase = 496
-
+    # highlight the region of the image we are actually checking.
+    display.setColor( 0x333333 )
+    display.fillRectangle( 0, regionTop, display.getWidth(), regionBase-regionTop )
+    display.setColor( 0xFFFFFF );
+
     cones = [[0,0,0],[0,0,0]]
 
     # calculate intrinsic camera parameters
@@ -60,12 +64,7 @@ while driver.step() != -1:
                 cones[onRight][0] += rx
                 cones[onRight][1] += 1
 
-                # display a top down view of the cones
-                scaleFactor = display.getHeight() / depth.getMaxRange() 
-
-                px = int(rx * scaleFactor)
-                py = int(rz * scaleFactor)
-                display.drawPixel( display.getWidth()//2+px, display.getHeight()-py )
+                display.drawPixel( x, y )
 
     # calc the averages, if no points found then skip the rest of this iteration
     try:         

controllers/examplecpp/examplecpp.cpp

@@ -3,6 +3,7 @@
 #include <webots/Display.hpp>
 using namespace webots;
 
+#include <cassert>
 #include <iostream>
 #include <memory>
 #include <math.h>
@@ -47,33 +48,41 @@ int main(int argc, char **argv)
     std::unique_ptr<Display> display = 
         std::unique_ptr<Display>( driver.getDisplay( "display" ) );
     if( !display ) return 2;
-
+
+    assert( depth->getWidth() != display->getWidth() );
+    assert( depth->getHeight() != display->getHeight() );
+
     Intrinsic intrinsic( *depth );
 
     // we are going to limit the region we look at to ignore the sky/car etc
-    const int regionTop = 370;
-    const int regionBase = 496;
-    static_assert( regionTop < regionBase );
+    const int regionTop = depth->getHeight()*0.51f;
+    const int regionBase = depth->getHeight()*0.68f;
+    assert( regionTop < regionBase );
+
+    // fill the whole background with black
+    display->setColor( 0x000000 );
+    display->fillRectangle( 0, 0, display->getWidth(), display->getHeight() );
 
     while( driver.step() != -1 )
-    {
+    {  
         std::array<std::array<float,3>,2> cones;
         cones[0] = {0,0,0};
         cones[1] = {0,0,0};
 
-        display->setColor( 0x000000 );
-        display->fillRectangle( 0, 0, display->getWidth(), display->getHeight() );
+        // highlight the region of the image we are actually checking.
+        display->setColor( 0x333333 );
+        display->fillRectangle( 0, regionTop, display->getWidth(), regionBase-regionTop );
         display->setColor( 0xFFFFFF );
 
         for( int x=0; x<depth->getWidth()-1; ++x )
         {
             for( int y=regionTop; y<regionBase; ++y )
-            {
-                if( depth->getRangeImage() == nullptr ) continue;
+            {                   
+                if( depth->getRangeImage() == nullptr ) continue;         
 
                 float distance1 = RangeFinder::rangeImageGetDepth( depth->getRangeImage(), depth->getWidth(), x, y );
                 float distance2 = RangeFinder::rangeImageGetDepth( depth->getRangeImage(), depth->getWidth(), x, y+1 );
-                    
+
                 if( distance1 - 0.5 > distance2 )
                 {
                     // calculate position of point relative to camera position
@@ -84,12 +93,7 @@ int main(int argc, char **argv)
                     cones[onRight][0] += coords[0];
                     cones[onRight][1] += 1;
 
-                    // display a top down view of the cones
-                    float scaleFactor = display->getHeight() / depth->getMaxRange(); 
-
-                    int px = coords[0] * scaleFactor;
-                    int py = coords[2] * scaleFactor;
-                    display->drawPixel( display->getWidth()/2+px, display->getHeight()-py );
+                    display->drawPixel( x, y );
                 }
             }
         }