/* * 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())); }