ViewSelection is stricter about the field of view when objects are very close

source/RobotComponents/components/ViewSelection/ViewSelection.cpp

@@ -174,17 +174,16 @@ void ViewSelection::process()
 {
     try
     {
-        float maxObjectDistance = getProperty<float>("MaxObjectDistance").getValue();
-        auto syncRobot = robotStateComponent->getSynchronizedRobot();
 
         if (manualViewTargets.size() > 0)
         {
             // look to the manually set target
+            SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
             FramedPositionPtr target;
             {
                 ScopedLock lock(manualViewTargetsMutex);
 
-                target = new FramedPosition(FramedPositionPtr::dynamicCast(manualViewTargets.at(0))->toEigen(), manualViewTargets.at(0)->frame, syncRobot->getName());
+                target = new FramedPosition(FramedPositionPtr::dynamicCast(manualViewTargets.at(0))->toEigen(), manualViewTargets.at(0)->frame, robotPrx->getName());
 
                 for (size_t i = 0; i < manualViewTargets.size() - 1; i++)
                 {
@@ -214,10 +213,11 @@ void ViewSelection::process()
                 setRandomNoise();
             }
 
-            auto robotPrx = robotStateComponent->getSynchronizedRobot();
             // collect localizable objects
             std::vector<memoryx::ObjectInstancePtr> localizableObjects;
             std::vector<FramedPositionPtr> localizableObjectPositions;
+            float maxObjectDistance = getProperty<float>("MaxObjectDistance").getValue();
+            SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
             int numberOfLostObjects = 0;
             memoryx::EntityIdList objectIds = objectInstancesProxy->getAllEntityIds();
 
@@ -279,10 +279,19 @@ void ViewSelection::process()
 
                     if (saliency > 0)
                     {
+                        // close objects get a smaller visibility angle to avoid problems due to the head being positioned unfortunately
+                        float modifiedHalfCameraOpeingAngle = halfCameraOpeningAngle;
+                        float objDist = position->toEigen().norm();
+                        float shortDistance = 0.33f * maxObjectDistance;
+                        if (objDist < shortDistance)
+                        {
+                            modifiedHalfCameraOpeingAngle = objDist / (shortDistance) * halfCameraOpeningAngle;
+                        }
+
                         TSphereCoord positionInSphereCoordinates;
                         MathTools::convert(position->toEigen() - offsetToHeadCenter, positionInSphereCoordinates);
                         int closestNodeIndex = graphLookupTable->getClosestNode(positionInSphereCoordinates);
-                        addSaliencyRecursive(closestNodeIndex, saliency, positionInSphereCoordinates, i);
+                        addSaliencyRecursive(closestNodeIndex, saliency, positionInSphereCoordinates, i, modifiedHalfCameraOpeingAngle);
                     }
                 }
             }
@@ -341,6 +350,10 @@ void ViewSelection::process()
                 boost::this_thread::sleep(boost::posix_time::milliseconds(sleepingTimeBetweenViewDirectionChanges));
             }
         }
+        else
+        {
+            boost::this_thread::sleep(boost::posix_time::milliseconds(5));
+        }
     }
     catch (...)
     {
@@ -351,11 +364,11 @@ void ViewSelection::process()
 
 
 
-void ViewSelection::addSaliencyRecursive(int currentNodeIndex, float saliency, TSphereCoord objectSphereCoord, int objectIndex)
+void ViewSelection::addSaliencyRecursive(int currentNodeIndex, float saliency, TSphereCoord objectSphereCoord, int objectIndex, float maxDistanceOnArc)
 {
     // distance on arc between object projection center and node,
     // normalized by the maximal viewing angle of the camera (=> in [0,1])
-    float normalizedDistance = MathTools::getDistanceOnArc(objectSphereCoord, saliencyEgosphereGraph->getNodes()->at(currentNodeIndex)->getPosition()) / halfCameraOpeningAngle;
+    float normalizedDistance = MathTools::getDistanceOnArc(objectSphereCoord, saliencyEgosphereGraph->getNodes()->at(currentNodeIndex)->getPosition()) / maxDistanceOnArc;
 
     // increase value of node
     float newValue = ((CIntensityNode*)saliencyEgosphereGraph->getNodes()->at(currentNodeIndex))->getIntensity()
@@ -374,9 +387,9 @@ void ViewSelection::addSaliencyRecursive(int currentNodeIndex, float saliency, T
 
         if (nodeVisitedForObject.at(neighbourIndex) != objectIndex)
         {
-            if (MathTools::getDistanceOnArc(objectSphereCoord, saliencyEgosphereGraph->getNodes()->at(neighbourIndex)->getPosition()) <= halfCameraOpeningAngle)
+            if (MathTools::getDistanceOnArc(objectSphereCoord, saliencyEgosphereGraph->getNodes()->at(neighbourIndex)->getPosition()) <= maxDistanceOnArc)
             {
-                addSaliencyRecursive(neighbourIndex, saliency, objectSphereCoord, objectIndex);
+                addSaliencyRecursive(neighbourIndex, saliency, objectSphereCoord, objectIndex, maxDistanceOnArc);
             }
             else
             {
@@ -413,8 +426,8 @@ void ViewSelection::setRandomNoise()
         CIntensityNode* node = (CIntensityNode*) nodes->at(i);
         TSphereCoord nodeCoord = node->getPosition();
         float distanceOnSphere = MathTools::getDistanceOnArc(currentViewDirection, nodeCoord);
-        float distanceFactor = 1.0f - 0.5f * distanceOnSphere / M_PI; // prefer directions close to current direction
-        float heightFactor = 1.0f - 0.2f * fabs(nodeCoord.fPhi - centralAngle) / 90.0f; // prefer directions that look down 20°
+        float distanceFactor = 1.0f - 0.4f * distanceOnSphere / M_PI; // prefer directions close to current direction
+        float heightFactor = 1.0f - 0.15f * fabs(nodeCoord.fPhi - centralAngle) / 90.0f; // prefer directions that look down 20°
         node->setIntensity(heightFactor * distanceFactor * noiseFactor * (rand() % randomFactor));
     }
 }

source/RobotComponents/components/ViewSelection/ViewSelection.h

@@ -156,7 +156,7 @@ namespace armarx
     private:
 
         void process();
-        void addSaliencyRecursive(int currentNodeIndex, float saliency, TSphereCoord objectSphereCoord, int objectIndex);
+        void addSaliencyRecursive(int currentNodeIndex, float saliency, TSphereCoord objectSphereCoord, int objectIndex, float maxDistanceOnArc);
         void setRandomNoise();
 
         RobotStateComponentInterfacePrx robotStateComponent;