Skip to content
Snippets Groups Projects
Commit 1090e81f authored by David Schiebener's avatar David Schiebener
Browse files

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

parent 62798891
No related branches found
No related tags found
No related merge requests found
...@@ -174,17 +174,16 @@ void ViewSelection::process() ...@@ -174,17 +174,16 @@ void ViewSelection::process()
{ {
try try
{ {
float maxObjectDistance = getProperty<float>("MaxObjectDistance").getValue();
auto syncRobot = robotStateComponent->getSynchronizedRobot();
if (manualViewTargets.size() > 0) if (manualViewTargets.size() > 0)
{ {
// look to the manually set target // look to the manually set target
SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
FramedPositionPtr target; FramedPositionPtr target;
{ {
ScopedLock lock(manualViewTargetsMutex); 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++) for (size_t i = 0; i < manualViewTargets.size() - 1; i++)
{ {
...@@ -214,10 +213,11 @@ void ViewSelection::process() ...@@ -214,10 +213,11 @@ void ViewSelection::process()
setRandomNoise(); setRandomNoise();
} }
auto robotPrx = robotStateComponent->getSynchronizedRobot();
// collect localizable objects // collect localizable objects
std::vector<memoryx::ObjectInstancePtr> localizableObjects; std::vector<memoryx::ObjectInstancePtr> localizableObjects;
std::vector<FramedPositionPtr> localizableObjectPositions; std::vector<FramedPositionPtr> localizableObjectPositions;
float maxObjectDistance = getProperty<float>("MaxObjectDistance").getValue();
SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
int numberOfLostObjects = 0; int numberOfLostObjects = 0;
memoryx::EntityIdList objectIds = objectInstancesProxy->getAllEntityIds(); memoryx::EntityIdList objectIds = objectInstancesProxy->getAllEntityIds();
...@@ -279,10 +279,19 @@ void ViewSelection::process() ...@@ -279,10 +279,19 @@ void ViewSelection::process()
if (saliency > 0) 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; TSphereCoord positionInSphereCoordinates;
MathTools::convert(position->toEigen() - offsetToHeadCenter, positionInSphereCoordinates); MathTools::convert(position->toEigen() - offsetToHeadCenter, positionInSphereCoordinates);
int closestNodeIndex = graphLookupTable->getClosestNode(positionInSphereCoordinates); int closestNodeIndex = graphLookupTable->getClosestNode(positionInSphereCoordinates);
addSaliencyRecursive(closestNodeIndex, saliency, positionInSphereCoordinates, i); addSaliencyRecursive(closestNodeIndex, saliency, positionInSphereCoordinates, i, modifiedHalfCameraOpeingAngle);
} }
} }
} }
...@@ -341,6 +350,10 @@ void ViewSelection::process() ...@@ -341,6 +350,10 @@ void ViewSelection::process()
boost::this_thread::sleep(boost::posix_time::milliseconds(sleepingTimeBetweenViewDirectionChanges)); boost::this_thread::sleep(boost::posix_time::milliseconds(sleepingTimeBetweenViewDirectionChanges));
} }
} }
else
{
boost::this_thread::sleep(boost::posix_time::milliseconds(5));
}
} }
catch (...) catch (...)
{ {
...@@ -351,11 +364,11 @@ void ViewSelection::process() ...@@ -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, // distance on arc between object projection center and node,
// normalized by the maximal viewing angle of the camera (=> in [0,1]) // 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 // increase value of node
float newValue = ((CIntensityNode*)saliencyEgosphereGraph->getNodes()->at(currentNodeIndex))->getIntensity() float newValue = ((CIntensityNode*)saliencyEgosphereGraph->getNodes()->at(currentNodeIndex))->getIntensity()
...@@ -374,9 +387,9 @@ void ViewSelection::addSaliencyRecursive(int currentNodeIndex, float saliency, T ...@@ -374,9 +387,9 @@ void ViewSelection::addSaliencyRecursive(int currentNodeIndex, float saliency, T
if (nodeVisitedForObject.at(neighbourIndex) != objectIndex) 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 else
{ {
...@@ -413,8 +426,8 @@ void ViewSelection::setRandomNoise() ...@@ -413,8 +426,8 @@ void ViewSelection::setRandomNoise()
CIntensityNode* node = (CIntensityNode*) nodes->at(i); CIntensityNode* node = (CIntensityNode*) nodes->at(i);
TSphereCoord nodeCoord = node->getPosition(); TSphereCoord nodeCoord = node->getPosition();
float distanceOnSphere = MathTools::getDistanceOnArc(currentViewDirection, nodeCoord); float distanceOnSphere = MathTools::getDistanceOnArc(currentViewDirection, nodeCoord);
float distanceFactor = 1.0f - 0.5f * distanceOnSphere / M_PI; // prefer directions close to current direction float distanceFactor = 1.0f - 0.4f * 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 heightFactor = 1.0f - 0.15f * fabs(nodeCoord.fPhi - centralAngle) / 90.0f; // prefer directions that look down 20°
node->setIntensity(heightFactor * distanceFactor * noiseFactor * (rand() % randomFactor)); node->setIntensity(heightFactor * distanceFactor * noiseFactor * (rand() % randomFactor));
} }
} }
......
...@@ -156,7 +156,7 @@ namespace armarx ...@@ -156,7 +156,7 @@ namespace armarx
private: private:
void process(); 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(); void setRandomNoise();
RobotStateComponentInterfacePrx robotStateComponent; RobotStateComponentInterfacePrx robotStateComponent;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment