/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package RobotComponents::ViewSelection
* @author David Schiebener (schiebener at kit dot edu)
* @date 2015
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#include "ViewSelection.h"
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <MemoryX/libraries/helpers/ObjectRecognitionHelpers/ObjectRecognitionWrapper.h>
using namespace armarx;
using namespace Eigen;
void ViewSelection::onInitComponent()
{
usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
usingProxy(getProperty<std::string>("HeadIKUnitName").getValue());
usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
usingProxy(getProperty<std::string>("PriorKnowledgeName").getValue());
offeringTopic("DebugDrawerUpdates");
headIKKinematicChainName = getProperty<std::string>("HeadIKKinematicChainName").getValue();
headFrameName = getProperty<std::string>("HeadFrameName").getValue();
cameraFrameName = getProperty<std::string>("CameraFrameName").getValue();
drawViewDirection = getProperty<bool>("VisualizeViewDirection").getValue();
randomNoiseLevel = getProperty<float>("RandomNoiseLevel").getValue();
centralHeadTiltAngle = getProperty<float>("CentralHeadTiltAngle").getValue();
std::string graphFileName;
if (ArmarXDataPath::getAbsolutePath("RobotComponents/ViewSelection/graph40k.gra", graphFileName))
{
saliencyEgosphereGraph = new CIntensityGraph(graphFileName);
ARMARX_VERBOSE << "Created egosphere graph with " << saliencyEgosphereGraph->getNodes()->size() << "nodes";
graphLookupTable = new CGraphPyramidLookupTable(9, 18);
graphLookupTable->buildLookupTable(saliencyEgosphereGraph);
nodeVisitedForObject.resize(saliencyEgosphereGraph->getNodes()->size());
// create visibility mask
visibilityMaskGraph = new CIntensityGraph(*saliencyEgosphereGraph);
TNodeList* nodes = visibilityMaskGraph->getNodes();
const float maxOverallHeadTiltAngle = getProperty<float>("MaxOverallHeadTiltAngle").getValue();
const float borderAreaAngle = 10.0f;
const float centralAngle = getProperty<float>("CentralHeadTiltAngle").getValue();
for (size_t i = 0; i < nodes->size(); i++)
{
CIntensityNode* node = (CIntensityNode*) nodes->at(i);
if (fabs(node->getPosition().fPhi - centralAngle) < maxOverallHeadTiltAngle - borderAreaAngle)
{
node->setIntensity(1.0f);
}
else if (fabs(node->getPosition().fPhi - centralAngle) < maxOverallHeadTiltAngle)
{
node->setIntensity(1.0f - (fabs(node->getPosition().fPhi - centralAngle) - (maxOverallHeadTiltAngle - borderAreaAngle)) / borderAreaAngle);
}
else
{
node->setIntensity(0.0f);
}
}
randomNoiseGraph = new CIntensityGraph(*saliencyEgosphereGraph);
randomNoiseGraph->set(0);
}
else
{
ARMARX_ERROR << "Could not find required graph file";
}
halfCameraOpeningAngle = getProperty<float>("HalfCameraOpeningAngle").getValue();
sleepingTimeBetweenViewDirectionChanges = getProperty<int>("SleepingTimeBetweenViewDirectionChanges").getValue();
doAutomaticViewSelection = getProperty<bool>("ActiveAtStartup").getValue();
deviationFromCameraCenterFactor = 0.5f;
timeOfLastViewChange = IceUtil::Time::now();
// this is robot model specific: offset from the used head coordinate system to the actual
// head center where the eyes are assumed to be located. Here it is correct for ARMAR-III
offsetToHeadCenter << 0, 0, 150;
}
void ViewSelection::onConnectComponent()
{
robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
memoryProxy = getProxy<memoryx::WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
priorKnowledge = getProxy<memoryx::PriorKnowledgeInterfacePrx>(getProperty<std::string>("PriorKnowledgeName").getValue());
objectInstancesProxy = memoryx::ObjectInstanceMemorySegmentBasePrx::uncheckedCast(memoryProxy->getSegment("objectInstances"));
objectClassesProxy = memoryx::ObjectClassMemorySegmentBasePrx::uncheckedCast(memoryProxy->getSegment("objectClasses"));
headIKUnitProxy = getProxy<HeadIKUnitInterfacePrx>(getProperty<std::string>("HeadIKUnitName").getValue());
drawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
headIKUnitProxy->request();
auto objClassSegment = priorKnowledge->getObjectClassesSegment();
auto ids = objClassSegment->getAllEntityIds();
for (auto& id : ids)
{
memoryx::ObjectClassPtr objClass = memoryx::ObjectClassPtr::dynamicCast(objClassSegment->getEntityById(id));
if (objClass)
{
memoryx::EntityWrappers::ObjectRecognitionWrapperPtr wrapper = objClass->addWrapper(new memoryx::EntityWrappers::ObjectRecognitionWrapper());
if (!wrapper->getRecognitionMethod().empty() && wrapper->getRecognitionMethod() != "<none>")
{
recognizableObjClasses[objClass->getName()] = objClass;
ARMARX_INFO << objClass->getName() << " has " << wrapper->getRecognitionMethod();
}
}
}
processorTask = new PeriodicTask<ViewSelection>(this, &ViewSelection::process, 30);
processorTask->setDelayWarningTolerance(sleepingTimeBetweenViewDirectionChanges + 100);
processorTask->start();
}
void ViewSelection::onDisconnectComponent()
{
try
{
if (drawer)
{
drawer->removeArrowDebugLayerVisu("HeadRealViewDirection");
}
processorTask->stop();
headIKUnitProxy->release();
}
catch (...)
{
ARMARX_WARNING_S << "An exception occured during onDisconnectComponent()";
handleExceptions();
}
}
void ViewSelection::onExitComponent()
{
delete visibilityMaskGraph;
delete graphLookupTable;
delete saliencyEgosphereGraph;
delete randomNoiseGraph;
}
void ViewSelection::process()
{
try
{
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, robotPrx->getName());
for (size_t i = 0; i < manualViewTargets.size() - 1; i++)
{
manualViewTargets.at(i) = manualViewTargets.at(i + 1);
}
manualViewTargets.pop_back();
}
ARMARX_INFO << "Looking to manual head target " << *target;
headIKUnitProxy->setHeadTarget(headIKKinematicChainName, target);
boost::this_thread::sleep(boost::posix_time::milliseconds(sleepingTimeBetweenViewDirectionChanges));
}
else if (doAutomaticViewSelection)
{
// reset saliency and visited nodes
saliencyEgosphereGraph->set(0);
for (size_t i = 0; i < saliencyEgosphereGraph->getNodes()->size(); i++)
{
nodeVisitedForObject.at(i) = -1;
}
// 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::EntityBaseList objectInstances = objectInstancesProxy->getAllEntities();
for (size_t i = 0; i < objectInstances.size(); i++)
{
const memoryx::ObjectInstancePtr object = memoryx::ObjectInstancePtr::dynamicCast(objectInstances.at(i));
if (object)
{
if (recognizableObjClasses.count(object->getName()) > 0 && !object->getPosition()->getFrame().empty())
{
if (objectClassesProxy->hasEntityByName(object->getName())) // should be true if and only if object has been requested
{
FramedPositionPtr position = object->getPosition();
position->changeFrame(robotPrx, headFrameName);
float objDist = position->toEigen().norm();
if (objDist <= maxObjectDistance)
{
localizableObjects.push_back(object);
localizableObjectPositions.push_back(position);
if (object->getExistenceCertainty() < 0.5)
{
numberOfLostObjects++;
}
}
else
{
ARMARX_DEBUG << "Discarding " << object->getName() << " which is at " << position->output() << " ObjectDistance " << objDist;
}
}
}
}
}
// generate new random noise
if (randomNoiseLevel > 0)
{
// if there are requested objects that were not localized yet, the random noise component will be bigger
memoryx::EntityBaseList requestedObjectClasses = objectClassesProxy->getAllEntities();
bool unlocalizedObjectExists = false;
for (size_t i = 0; i < requestedObjectClasses.size(); i++)
{
bool isInInstancesList = false;
for (size_t j = 0; j < objectInstances.size(); j++)
{
if (requestedObjectClasses.at(i)->getName() == objectInstances.at(j)->getName())
{
isInInstancesList = true;
break;
}
}
if (!isInInstancesList)
{
unlocalizedObjectExists = true;
break;
}
}
if (unlocalizedObjectExists)
{
if (localizableObjects.size() == 0)
{
ARMARX_DEBUG << "There are objects requested, and none of them has been localized yet";
setRandomNoise(centralHeadTiltAngle + 30, 3.0f);
}
else
{
ARMARX_DEBUG << "There are objects requested, and some but not all of them have been localized already";
setRandomNoise(centralHeadTiltAngle + 20, 2.0f);
}
}
else
{
ARMARX_DEBUG << "There are no requested objects that were not localized yet. requestedObjectClasses.size() = " << requestedObjectClasses.size()
<< ", objectInstances.size() = " << objectInstances.size();// << ", difference.size() = " << difference.size();
setRandomNoise(centralHeadTiltAngle, 1.0f);
}
}
const float probabilityToAddALostObject = (numberOfLostObjects > 0) ? getProperty<float>("ProbabilityToLookForALostObject").getValue() / numberOfLostObjects : 0;
// add localization necessities to sphere of possible view directions
for (size_t i = 0; i < localizableObjects.size(); i++)
{
const memoryx::ObjectInstancePtr object = localizableObjects.at(i);
if ((object->getExistenceCertainty() >= 0.5) || (rand() % 100000 < 100000 * probabilityToAddALostObject))
{
const FramedPositionPtr position = localizableObjectPositions.at(i);
memoryx::MultivariateNormalDistributionPtr uncertaintyGaussian = memoryx::MultivariateNormalDistributionPtr::dynamicCast(object->getPositionUncertainty());
if (!uncertaintyGaussian)
{
uncertaintyGaussian = memoryx::MultivariateNormalDistribution::CreateDefaultDistribution(1000000);
}
float saliency = log(1.0 + uncertaintyGaussian->getVarianceScalar());
if (saliency > 0)
{
// close objects get a smaller visibility angle to avoid problems due to the head being positioned unfortunately
float modifiedHalfCameraOpeningAngle = halfCameraOpeningAngle;
float objDist = position->toEigen().norm();
float shortDistance = 0.5f * maxObjectDistance;
if (objDist < shortDistance)
{
modifiedHalfCameraOpeningAngle = (objDist - 0.1f * shortDistance) / (0.9f * shortDistance) * halfCameraOpeningAngle;
modifiedHalfCameraOpeningAngle = std::max(0.0f, modifiedHalfCameraOpeningAngle);
}
TSphereCoord positionInSphereCoordinates;
MathTools::convert(position->toEigen() - offsetToHeadCenter, positionInSphereCoordinates);
int closestNodeIndex = graphLookupTable->getClosestNode(positionInSphereCoordinates);
addSaliencyRecursive(closestNodeIndex, saliency, positionInSphereCoordinates, i, modifiedHalfCameraOpeningAngle);
}
}
}
// find the direction with highest saliency
TNodeList* visibilityMaskGraphNodes = visibilityMaskGraph->getNodes();
TNodeList* saliencyEgosphereGraphNodes = saliencyEgosphereGraph->getNodes();
TNodeList* randomNoiseGraphNodes = randomNoiseGraph->getNodes();
int maxIndex = -1;
float maxSaliency = 0;
for (size_t i = 0; i < saliencyEgosphereGraphNodes->size(); i++)
{
CIntensityNode* visibilityNode = (CIntensityNode*) visibilityMaskGraphNodes->at(i);
CIntensityNode* saliencyNode = (CIntensityNode*) saliencyEgosphereGraphNodes->at(i);
CIntensityNode* randomNoiseNode = (CIntensityNode*) randomNoiseGraphNodes->at(i);
float saliency = visibilityNode->getIntensity() * (saliencyNode->getIntensity() + randomNoiseNode->getIntensity());
if (saliency > maxSaliency)
{
maxSaliency = saliency;
maxIndex = i;
}
}
ARMARX_DEBUG << "Highest saliency: " << maxSaliency;
// select a new view if saliency is bigger than threshold (which converges towards 0 over time)
int timeSinceLastViewChange = (IceUtil::Time::now() - timeOfLastViewChange).toMilliSeconds();
float saliencyThreshold = 0;
if (timeSinceLastViewChange > 0)
{
saliencyThreshold = 2.0f * sleepingTimeBetweenViewDirectionChanges / timeSinceLastViewChange;
}
if (maxSaliency > saliencyThreshold)
{
Eigen::Vector3f target;
MathTools::convert(saliencyEgosphereGraphNodes->at(maxIndex)->getPosition(), target);
const float distanceFactor = 1500;
target = distanceFactor * target + offsetToHeadCenter;
FramedPositionPtr viewTargetPositionPtr = new FramedPosition(target, headFrameName, robotPrx->getName());
if (drawer && robotPrx->hasRobotNode("Cameras") && drawViewDirection)
{
Vector3Ptr startPos = new Vector3(FramedPosePtr::dynamicCast(robotPrx->getRobotNode("Cameras")->getGlobalPose())->toEigen());
FramedDirectionPtr currentDirection = new FramedDirection(Eigen::Vector3f(1, 0, 0), "Cameras", robotPrx->getName());
drawer->setArrowDebugLayerVisu("CurrentHeadViewDirection", startPos, currentDirection->toGlobal(robotPrx), DrawColor {0, 0.5, 0.5, 0.1}, maxObjectDistance, 5);
Eigen::Vector3f plannedDirectionEigen = viewTargetPositionPtr->toGlobalEigen(robotPrx) - startPos->toEigen();
Vector3Ptr plannedDirection = new Vector3(plannedDirectionEigen);
drawer->setArrowDebugLayerVisu("PlannedHeadViewDirection", startPos, plannedDirection, DrawColor {0.5, 0.5, 0, 0.1}, maxObjectDistance, 5);
}
headIKUnitProxy->setHeadTarget(headIKKinematicChainName, viewTargetPositionPtr);
timeOfLastViewChange = IceUtil::Time::now();
boost::this_thread::sleep(boost::posix_time::milliseconds(sleepingTimeBetweenViewDirectionChanges));
}
}
else
{
boost::this_thread::sleep(boost::posix_time::milliseconds(5));
}
}
catch (...)
{
ARMARX_WARNING_S << "An exception occured during process()";
handleExceptions();
}
}
void ViewSelection::addSaliencyRecursive(const int currentNodeIndex, const float saliency, const TSphereCoord objectSphereCoord, const int objectIndex, const 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()) / maxDistanceOnArc;
// increase value of node
float newValue = ((CIntensityNode*)saliencyEgosphereGraph->getNodes()->at(currentNodeIndex))->getIntensity()
+ (1.0f - deviationFromCameraCenterFactor * normalizedDistance * normalizedDistance) * saliency;
((CIntensityNode*)saliencyEgosphereGraph->getNodes()->at(currentNodeIndex))->setIntensity(newValue);
// mark node as visited for this object
nodeVisitedForObject.at(currentNodeIndex) = objectIndex;
// recurse on neighbours if they were not yet visited and close enough to the object projection center
int neighbourIndex;
for (int i = 0; i < (int)saliencyEgosphereGraph->getNodeAdjacency(currentNodeIndex)->size(); i++)
{
neighbourIndex = saliencyEgosphereGraph->getNodeAdjacency(currentNodeIndex)->at(i);
if (nodeVisitedForObject.at(neighbourIndex) != objectIndex)
{
if (MathTools::getDistanceOnArc(objectSphereCoord, saliencyEgosphereGraph->getNodes()->at(neighbourIndex)->getPosition()) <= maxDistanceOnArc)
{
addSaliencyRecursive(neighbourIndex, saliency, objectSphereCoord, objectIndex, maxDistanceOnArc);
}
else
{
nodeVisitedForObject.at(neighbourIndex) = objectIndex;
}
}
}
}
void ViewSelection::setRandomNoise(const float centralAngleForVerticalDirection, const float directionVariabilityFactor)
{
if (!robotStateComponent)
{
return;
}
Eigen::Vector3f currentViewTargetEigen(1000, 0, 0); // virtual central gaze goes into x direction
FramedPosition currentViewTarget(currentViewTargetEigen, cameraFrameName, robotStateComponent->getSynchronizedRobot()->getName());
currentViewTarget.changeFrame(robotStateComponent->getSynchronizedRobot(), headFrameName);
currentViewTargetEigen = currentViewTarget.toEigen() - offsetToHeadCenter;
TSphereCoord currentViewDirection;
MathTools::convert(currentViewTargetEigen, currentViewDirection);
TNodeList* nodes = randomNoiseGraph->getNodes();
const int randomFactor = 100000;
const float noiseFactor = randomNoiseLevel / randomFactor;
const float distanceWeight = std::min(0.6f / directionVariabilityFactor, 1.0f);
const float centralityWeight = std::min(0.1f / directionVariabilityFactor, 0.2f);
for (size_t i = 0; i < nodes->size(); i++)
{
CIntensityNode* node = (CIntensityNode*) nodes->at(i);
TSphereCoord nodeCoord = node->getPosition();
float distanceOnSphere = MathTools::getDistanceOnArc(currentViewDirection, nodeCoord);
float distanceFactor = 1.0f - distanceWeight * distanceOnSphere / M_PI; // prefer directions close to current direction
float verticalCenterFactor = 1.0f - centralityWeight * fabs(nodeCoord.fPhi - centralAngleForVerticalDirection) / 90.0f; // prefer directions that look to the center
float horizontalCenterFactor = 1.0f - centralityWeight * fabs(nodeCoord.fTheta) / 120.0f;
node->setIntensity(verticalCenterFactor * horizontalCenterFactor * distanceFactor * noiseFactor * (rand() % randomFactor));
}
}
void ViewSelection::addManualViewTarget(const FramedPositionBasePtr& target, const Ice::Current& c)
{
ScopedLock lock(manualViewTargetsMutex);
manualViewTargets.push_back(target);
}
PropertyDefinitionsPtr ViewSelection::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new ViewSelectionPropertyDefinitions(getConfigIdentifier()));
}