diff --git a/corelib/include/rtabmap/core/CameraRGBD.h b/corelib/include/rtabmap/core/CameraRGBD.h index 65d93b83..b00b0df9 100644 --- a/corelib/include/rtabmap/core/CameraRGBD.h +++ b/corelib/include/rtabmap/core/CameraRGBD.h @@ -199,7 +199,8 @@ public: static bool exposureGainAvailable(); public: - CameraOpenNI2(float imageRate = 0, + CameraOpenNI2(const std::string & deviceId = "", + float imageRate = 0, const Transform & localTransform = Transform::getIdentity(), float fx = 0.0f, float fy = 0.0f, @@ -213,6 +214,7 @@ public: bool setAutoExposure(bool enabled); bool setExposure(int value); bool setGain(int value); + bool setMirroring(bool enabled); protected: virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy); @@ -223,6 +225,7 @@ private: openni::VideoStream * _depth; float _depthFx; float _depthFy; + std::string _deviceId; }; diff --git a/corelib/include/rtabmap/core/Graph.h b/corelib/include/rtabmap/core/Graph.h index 10d850c2..3649abc2 100644 --- a/corelib/include/rtabmap/core/Graph.h +++ b/corelib/include/rtabmap/core/Graph.h @@ -77,6 +77,14 @@ bool RTABMAP_EXP loadTOROGraph(const std::string & fileName, std::map & poses, std::multimap > & edgeConstraints); +/** + * Get only the the most recent or older poses in the defined radius. + * @param poses The poses + * @param radius Radius (m) of the search for near neighbors + * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle) + * @param keepLatest keep the latest node if true, otherwise the oldest node is kept + * @return A map containing only most recent or older poses in the the defined radius + */ std::map RTABMAP_EXP radiusPosesFiltering( const std::map & poses, float radius, @@ -87,7 +95,7 @@ std::map RTABMAP_EXP radiusPosesFiltering( * Get all neighbor nodes in a fixed radius around each pose. * @param poses The poses * @param radius Radius (m) of the search for near neighbors - * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius + * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle) * @return A map between each pose id and its neighbors found in the radius */ std::multimap RTABMAP_EXP radiusPosesClustering( @@ -115,12 +123,26 @@ int RTABMAP_EXP findNearestNode( const std::map & nodes, const rtabmap::Transform & targetPose); +/** + * Get nodes near the query + * @param nodeId the query id + * @param nodes the nodes to search for + * @param maxNearestNeighbors Maximum nearest neighbor to get. 0 means all. + * @param radius radius to search for (m) + * @return the nodes with squared distance to query node. + */ std::map RTABMAP_EXP getNodesInRadius( int nodeId, const std::map & nodes, int maxNearestNeighbors, float radius); +float RTABMAP_EXP computePathLength( + const std::vector > & path, + unsigned int fromIndex = 0, + unsigned int toIndex = 0); + + } /* namespace graph */ } /* namespace rtabmap */ diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 9bfde0b9..9a939406 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -290,14 +290,13 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(RGBD, ToroIgnoreVariance, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint in TORO. Otherwise, an information matrix is generated from the variance saved in the links."); RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation)."); RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m)."); - RTABMAP_PARAM(RGBD, MaxAnticipatedNodes, unsigned int, 10, "Maximum anticipated nodes on the computed path that can be retrieved (the number of nodes actually retrieved at each iteration is limited by \"Rtabmap/MaxRetrieved\")."); RTABMAP_PARAM(RGBD, PlanWithNearNodesLinked, bool, true, "Before planning in the graph, near nodes are linked together (even if they don't belong to same map). Radius is defined by \"RGBD/GoalReachedRadius\" parameter."); - RTABMAP_PARAM(RGBD, GoalMaxDistance, float, 0, "Maximum distance (m) of the target goal from the graph (0 means infinity). If the goal is too far from the graph, the plan is aborted. Also when set, the next goal in the graph can't be farther than this distance from the current position."); + RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority)."); + RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management."); // Local loop closure detection RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, false, "Detection over all locations in STM."); RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace, bool, false, "Detection over locations (in Working Memory or STM) near in space."); - RTABMAP_PARAM(RGBD, LocalLoopDetectionRadius, float, 15, "Maximum radius for space detection."); RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors, int, 20, "Maximum nearest neighbor."); RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID, int, 50, "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.") diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index a8532621..f15b7fdf 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -93,7 +93,7 @@ public: Transform getMapCorrection() const {return _mapCorrection;} const Memory * getMemory() const {return _memory;} float getGoalReachedRadius() const {return _goalReachedRadius;} - float getGoalMaxDistance() const {return _goalMaxDistance;} + float getLocalRadius() const {return _localRadius;} float getTimeThreshold() const {return _maxTimeAllowed;} // in ms void setTimeThreshold(float maxTimeAllowed); // in ms @@ -128,7 +128,6 @@ public: int getPathCurrentGoalId() const; const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;} - std::map getNodesInRadius(int fromId, int maxNearestNeighbors, float radius) const; std::map getWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const; void adjustLikelihood(std::map & likelihood) const; std::pair selectHypothesis(const std::map & posterior, @@ -156,6 +155,7 @@ private: float _loopThr; float _loopRatio; unsigned int _maxRetrieved; + unsigned int _maxLocalRetrieved; bool _statisticLogsBufferedInRAM; bool _statisticLogged; bool _statisticLoggedHeaders; @@ -168,7 +168,7 @@ private: bool _poseScanMatching; bool _localLoopClosureDetectionTime; bool _localLoopClosureDetectionSpace; - float _localDetectRadius; + float _localRadius; float _localDetectMaxNeighbors; int _localDetectMaxDiffID; int _toroIterations; @@ -182,9 +182,7 @@ private: int _reextractMaxWords; bool _startNewMapOnLoopClosure; float _goalReachedRadius; // meters - unsigned int _maxAnticipatedNodes; bool _planWithNearNodesLinked; - float _goalMaxDistance; std::pair _loopClosureHypothesis; std::pair _highestHypothesis; diff --git a/corelib/src/CameraRGBD.cpp b/corelib/src/CameraRGBD.cpp index cd23d580..2cd06181 100644 --- a/corelib/src/CameraRGBD.cpp +++ b/corelib/src/CameraRGBD.cpp @@ -346,7 +346,14 @@ bool CameraOpenNI2::exposureGainAvailable() #endif } -CameraOpenNI2::CameraOpenNI2(float imageRate, const rtabmap::Transform & localTransform, float fx, float fy, float cx, float cy) : +CameraOpenNI2::CameraOpenNI2( + const std::string & deviceId, + float imageRate, + const rtabmap::Transform & localTransform, + float fx, + float fy, + float cx, + float cy) : CameraRGBD(imageRate, localTransform, fx, fy, cx, cy), #ifdef WITH_OPENNI2 _device(new openni::Device()), @@ -358,7 +365,8 @@ CameraOpenNI2::CameraOpenNI2(float imageRate, const rtabmap::Transform & localTr _depth(0), #endif _depthFx(0.0f), - _depthFy(0.0f) + _depthFy(0.0f), + _deviceId(deviceId) { } @@ -438,14 +446,31 @@ bool CameraOpenNI2::setGain(int value) return false; } +bool CameraOpenNI2::setMirroring(bool enabled) +{ + if(_color->isValid() && _depth->isValid()) + { + return _depth->setMirroringEnabled(enabled) == openni::STATUS_OK && + _color->setMirroringEnabled(enabled) == openni::STATUS_OK; + } + return false; +} + bool CameraOpenNI2::init() { #ifdef WITH_OPENNI2 openni::OpenNI::initialize(); - if(_device->open(openni::ANY_DEVICE) != openni::STATUS_OK) + if(_device->open(_deviceId.empty()?openni::ANY_DEVICE:_deviceId.c_str()) != openni::STATUS_OK) { - UERROR("CameraOpenNI2: Cannot open device."); + if(!_deviceId.empty()) + { + UERROR("CameraOpenNI2: Cannot open device \"%s\".", _deviceId.c_str()); + } + else + { + UERROR("CameraOpenNI2: Cannot open device."); + } _device->close(); openni::OpenNI::shutdown(); return false; diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index cbc04a31..483b0765 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -505,9 +505,13 @@ bool loadTOROGraph(const std::string & fileName, } -std::map radiusPosesFiltering(const std::map & poses, float radius, float angle, bool keepLatest) +std::map radiusPosesFiltering( + const std::map & poses, + float radius, + float angle, + bool keepLatest) { - if(poses.size() > 1 && radius > 0.0f && angle>0.0f) + if(poses.size() > 1 && radius > 0.0f) { pcl::PointCloud::Ptr cloud(new pcl::PointCloud); cloud->resize(poses.size()); @@ -528,7 +532,6 @@ std::map radiusPosesFiltering(const std::map & p for(unsigned int i=0; isize(); ++i) { - // ignore scans if(indicesChecked.find(i) == indicesChecked.end()) { std::vector kIndices; @@ -542,11 +545,18 @@ std::map radiusPosesFiltering(const std::map & p { if(indicesChecked.find(kIndices[j]) == indicesChecked.end()) { - const Transform & checkT = transforms.at(kIndices[j]); - // same orientation? - Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0); - double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0)); - if(a <= angle) + if(angle > 0.0f) + { + const Transform & checkT = transforms.at(kIndices[j]); + // same orientation? + Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0); + double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0)); + if(a <= angle) + { + cloudIndices.insert(kIndices[j]); + } + } + else { cloudIndices.insert(kIndices[j]); } @@ -605,7 +615,7 @@ std::map radiusPosesFiltering(const std::map & p std::multimap radiusPosesClustering(const std::map & poses, float radius, float angle) { std::multimap clusters; - if(poses.size() > 1 && radius > 0.0f && angle>0.0f) + if(poses.size() > 1 && radius > 0.0f) { pcl::PointCloud::Ptr cloud(new pcl::PointCloud); cloud->resize(poses.size()); @@ -635,11 +645,18 @@ std::multimap radiusPosesClustering(const std::map & p { if((int)i != kIndices[j]) { - const Transform & checkT = transforms.at(kIndices[j]); - // same orientation? - Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0); - double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0)); - if(a <= angle) + if(angle > 0.0f) + { + const Transform & checkT = transforms.at(kIndices[j]); + // same orientation? + Eigen::Vector3f vB = checkT.toEigen3f().rotation()*Eigen::Vector3f(1,0,0); + double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0)); + if(a <= angle) + { + clusters.insert(std::make_pair(ids[i], ids[kIndices[j]])); + } + } + else { clusters.insert(std::make_pair(ids[i], ids[kIndices[j]])); } @@ -834,7 +851,7 @@ int findNearestNode( return id; } -// return , including query +// return , including query std::map getNodesInRadius( int nodeId, const std::map & nodes, @@ -868,7 +885,7 @@ std::map getNodesInRadius( { if(ind[i] >=0) { - UDEBUG("Inlier %d: %f", ids[ind[i]], dist[i]); + UDEBUG("Inlier %d: %f", ids[ind[i]], sqrt(dist[i])); foundNodes.insert(std::make_pair(ids[ind[i]], dist[i])); } } @@ -877,6 +894,31 @@ std::map getNodesInRadius( return foundNodes; } +float computePathLength( + const std::vector > & path, + unsigned int fromIndex, + unsigned int toIndex) +{ + float length = 0.0f; + if(path.size() > 1) + { + UASSERT(fromIndex < path.size() && toIndex < path.size() && fromIndex <= toIndex); + if(fromIndex >= toIndex) + { + toIndex = path.size()-1; + } + float x=0, y=0, z=0; + for(unsigned int i=fromIndex; i retrievalPathIds; - if(_path.size() && _rgbdSlamMode) + std::set retrievalLocalIds; + if(_rgbdSlamMode && _maxLocalRetrieved > 0) { - updateGoalIndex(); - + // Priority on locations on the planned path if(_path.size()) { - // immunize all nodes after current node - for(unsigned int i=_pathCurrentIndex; i<_path.size() && i<_pathCurrentIndex+_maxAnticipatedNodes; ++i) + updateGoalIndex(); + + if(_path.size()) { - immunizedLocations.insert(_path[i].first); - UDEBUG("Path immunization: node %d", _path[i].first); - } - // retrieve nodes after current node up to _maxPathRetrievalSize - for(unsigned int i=_pathCurrentIndex; - i<_path.size() && i<_pathCurrentIndex+_maxAnticipatedNodes && retrievalPathIds.size() < _maxRetrieved; - ++i) - { - if(_memory->getSignature(_path[i].first) == 0) + float distanceSoFar = 0.0f; + // immunize all nodes after current node and + // retrieve nodes after current node in the maximum radius from the current node + for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i) { - UINFO("retrieval of node %d on path", _path[i].first); - retrievalPathIds.push_back(_path[i].first); + if(_localRadius > 0.0f && i != _pathCurrentIndex) + { + distanceSoFar += _path[i-1].second.getDistance(_path[i].second); + } + + if(distanceSoFar <= _localRadius) + { + if(_memory->getSignature(_path[i].first) != 0) + { + immunizedLocations.insert(_path[i].first); + UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar); + } + else if(retrievalLocalIds.size() < _maxLocalRetrieved) + { + UINFO("retrieval of node %d on path (dist=%fm)", _path[i].first, distanceSoFar); + retrievalLocalIds.insert(_path[i].first); + // retrieved locations are automatically immunized + } + } + else + { + UDEBUG("Stop on node %d (dist=%fm > %fm)", + _path[i].first, distanceSoFar, _localRadius); + break; + } } } - - // insert them first to make sure they are loaded. - reactivatedIds.insert(reactivatedIds.begin(), retrievalPathIds.begin(), retrievalPathIds.end()); } + else if(retrievalLocalIds.size() < _maxLocalRetrieved) + { + // retrieval based on the nodes near the current pose + std::map nearNodes = graph::getNodesInRadius(signature->id(), _optimizedPoses, 0, _localRadius); + // sort by distance + std::multimap nearNodesByDist; + for(std::map::iterator iter=nearNodes.begin(); iter!=nearNodes.end(); ++iter) + { + nearNodesByDist.insert(std::make_pair(iter->second, iter->first)); + } + for(std::multimap::iterator iter=nearNodesByDist.begin(); + iter!=nearNodesByDist.end() && retrievalLocalIds.size() < _maxLocalRetrieved; + ++iter) + { + const Signature * s = _memory->getSignature(iter->second); + UASSERT(s != 0); + for(std::map::const_iterator jter=s->getLinks().begin(); + jter!=s->getLinks().end() && retrievalLocalIds.size() < _maxLocalRetrieved; + ++jter) + { + if(_memory->getSignature(jter->first) == 0) + { + UINFO("retrieval of node %d on local map", jter->first); + retrievalLocalIds.insert(jter->first); + } + } + } + } + + // insert them first to make sure they are loaded. + reactivatedIds.insert(reactivatedIds.begin(), retrievalLocalIds.begin(), retrievalLocalIds.end()); } //============================================================ @@ -1263,7 +1307,7 @@ bool Rtabmap::process(const SensorData & data) // only a loop closure link is added... signaturesRetrieved = _memory->reactivateSignatures( reactivatedIds, - _maxRetrieved+retrievalPathIds.size(), // add path retrieved + _maxRetrieved+retrievalLocalIds.size(), // add path retrieved timeRetrievalDbAccess); ULOGGER_INFO("retrieval of %d (db time = %fs)", (int)signaturesRetrieved.size(), timeRetrievalDbAccess); @@ -1417,7 +1461,7 @@ bool Rtabmap::process(const SensorData & data) localSpacePoses = this->getWMPosesInRadius( signature->id(), _localDetectMaxNeighbors, - _localDetectRadius, + _localRadius, _localDetectMaxDiffID, localSpaceNearestId); @@ -1519,31 +1563,11 @@ bool Rtabmap::process(const SensorData & data) uContains(_optimizedPoses, _path[_pathCurrentIndex].first)) { Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first); - if(_localDetectRadius > 0.0f && virtualLoop.getNorm() < _localDetectRadius) + if(_localRadius > 0.0f && virtualLoop.getNorm() < _localRadius) { _memory->addLink(_path[_pathCurrentIndex].first, signature->id(), virtualLoop, Link::kVirtualClosure, 99999); } } - - // Make sure the next signatures on the path are linked together - for(unsigned int i=_pathCurrentIndex; - i<_path.size() && i<_pathCurrentIndex+_maxAnticipatedNodes; - ++i) - { - if(i>0) - { - const Signature * s = _memory->getSignature(_path[i].first); - if(s) - { - if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0) - { - Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second; - _memory->addLink(_path[i-1].first, _path[i].first, virtualLoop, Link::kVirtualClosure, 99999); - UWARN("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first); - } - } - } - } } //============================================================ // Prepare statistics @@ -1603,24 +1627,32 @@ bool Rtabmap::process(const SensorData & data) UINFO("Set loop closure transform = %s", sLoop->getLinks().at(signature->id()).transform().prettyPrint().c_str()); statistics_.setLoopClosureTransform(sLoop->getLinks().at(signature->id()).transform()); } + statistics_.setMapCorrection(_mapCorrection); + UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str()); + // Set local graph if(!_rgbdSlamMode) { + // no optimization on appearance-only mode, create a local graph + std::map ids = _memory->getNeighborsId(signature->id(), 0, 0, true); + std::map poses; std::map mapIds; - mapIds.insert(std::make_pair(signature->id(), _memory->getMapId(signature->id()))); - if(_loopClosureHypothesis.first) + std::multimap constraints; + _memory->getMetricConstraints(uKeys(ids), poses, constraints, false); + for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { - mapIds.insert(std::make_pair(_loopClosureHypothesis.first, _memory->getMapId(_loopClosureHypothesis.first))); + mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first))); } + statistics_.setPoses(poses); + statistics_.setConstraints(constraints); statistics_.setMapIds(mapIds); - }//else... see finalize statistics below + } + else // RGBD-SLAM mode + { + //see after transfer below + } - - statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size()); - statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size()); - statistics_.addStatistic(Statistics::kMemorySignatures_retrieved(), (float)signaturesRetrieved.size()); - - // timing... + // timings... statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000); statistics_.addStatistic(Statistics::kTimingScan_matching(), timeScanMatching*1000); statistics_.addStatistic(Statistics::kTimingLocal_detection_TIME(), timeLocalTimeDetection*1000); @@ -1634,6 +1666,9 @@ bool Rtabmap::process(const SensorData & data) statistics_.addStatistic(Statistics::kTimingHypotheses_validation(), timeHypothesesValidation*1000); statistics_.addStatistic(Statistics::kTimingCleaning_neighbors(), timeCleaningNeighbors*1000); + // retrieval + statistics_.addStatistic(Statistics::kMemorySignatures_retrieved(), (float)signaturesRetrieved.size()); + // Surf specific parameters statistics_.addStatistic(Statistics::kKeypointDictionary_size(), dictionarySize); @@ -1683,7 +1718,7 @@ bool Rtabmap::process(const SensorData & data) _memory->deleteLocation(signature->id()); } - // Pass this point signature should not be used, since it could be transferred... + // Pass this point signature should not be used, since it could have been transferred... signature = 0; //By default, remove all signatures with a loop closure link if they are not in reactivateIds @@ -1713,11 +1748,40 @@ bool Rtabmap::process(const SensorData & data) _lastProcessTime = totalTime; //Remove optimized poses from signatures transferred - for(std::list::iterator iter = signaturesRemoved.begin(); iter!=signaturesRemoved.end(); ++iter) + if(signaturesRemoved.size() && (_optimizedPoses.size() || _constraints.size())) { - UDEBUG("removing optimized pose %d...", *iter); - _optimizedPoses.erase(*iter); - _constraints.erase(*iter); + //refresh the local map because some transferred nodes may have broken the tree + if(_memory->getLastWorkingSignature()) + { + std::map ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, 0, true); + for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end();) + { + if(!uContains(ids, iter->first)) + { + _optimizedPoses.erase(iter++); + } + else + { + ++iter; + } + } + for(std::multimap::iterator iter=_constraints.begin(); iter!=_constraints.end();) + { + if(!uContains(ids, iter->second.from()) || !uContains(ids, iter->second.to())) + { + _constraints.erase(iter++); + } + else + { + ++iter; + } + } + } + else + { + _optimizedPoses.clear(); + _constraints.clear(); + } } @@ -1740,34 +1804,22 @@ bool Rtabmap::process(const SensorData & data) statistics_.addStatistic(Statistics::kTimingMemory_cleanup(), timeMemoryCleanup*1000); statistics_.addStatistic(Statistics::kMemorySignatures_removed(), signaturesRemoved.size()); - //Poses, place this after Transfer! (_optimizedPoses may change) - std::map mapIds; + // place after transfer because the memory/local graph may have changed + statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size()); + statistics_.addStatistic(Statistics::kMemoryShort_time_memory_size(), _memory->getStMem().size()); + if(_rgbdSlamMode) { + std::map mapIds; for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) { mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first))); } statistics_.setPoses(_optimizedPoses); statistics_.setConstraints(_constraints); - statistics_.setMapCorrection(_mapCorrection); - UINFO("Set map correction = %s", _mapCorrection.prettyPrint().c_str()); + statistics_.setMapIds(mapIds); } - else if(_memory->getLastWorkingSignature()) - { - // no optimization on appearance-only mode - std::map ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, 0, true); - std::map poses; - std::multimap constraints; - _memory->getMetricConstraints(uKeys(ids), poses, constraints, false); - for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) - { - mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first))); - } - statistics_.setPoses(poses); - statistics_.setConstraints(constraints); - } - statistics_.setMapIds(mapIds); + } // Log info... @@ -2437,10 +2489,10 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global) UINFO("Nearest node found=%d ,%fs", nearestId, timer.ticks()); if(nearestId > 0) { - if(_goalMaxDistance != 0.0f && targetPose.getDistance(nodes.at(nearestId)) > _goalMaxDistance) + if(_localRadius != 0.0f && targetPose.getDistance(nodes.at(nearestId)) > _localRadius) { UWARN("Cannot plan farther than %f m from the graph! (distance=%f m from node %d)", - _goalMaxDistance, targetPose.getDistance(nodes.at(nearestId)), nearestId); + _localRadius, targetPose.getDistance(nodes.at(nearestId)), nearestId); } else { @@ -2525,7 +2577,6 @@ int Rtabmap::getPathCurrentGoalId() const void Rtabmap::updateGoalIndex() { - UDEBUG(""); if(!_rgbdSlamMode) { UWARN("This method can on be used in RGBD-SLAM mode!"); @@ -2534,6 +2585,39 @@ void Rtabmap::updateGoalIndex() if(_path.size()) { + // Make sure the next signatures on the path are linked together + float distanceSoFar = 0.0f; + for(unsigned int i=_pathCurrentIndex; + i<_path.size(); + ++i) + { + if(i>0) + { + if(_localRadius > 0.0f) + { + distanceSoFar += _path[i-1].second.getDistance(_path[i].second); + } + if(distanceSoFar <= _localRadius) + { + const Signature * s = _memory->getSignature(_path[i].first); + if(s) + { + if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0) + { + Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second; + _memory->addLink(_path[i-1].first, _path[i].first, virtualLoop, Link::kVirtualClosure, 99999); + UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first); + } + } + } + else + { + break; + } + } + } + + UDEBUG("current node = %d current goal = %d", _path[_pathCurrentIndex].first, _path[_pathGoalIndex].first); if(_memory->getLastWorkingSignature() == 0 || !uContains(_optimizedPoses, _memory->getLastWorkingSignature()->id())) { @@ -2556,13 +2640,35 @@ void Rtabmap::updateGoalIndex() if(_path.size()) { //Always check if the farthest node is accessible in local map (max to local space radius if set) - int goalIndex = _pathGoalIndex; - for(int i=(int)_path.size()-1; i>=goalIndex; --i) + int goalIndex = _pathCurrentIndex; + float distanceSoFar = 0.0f; + for(unsigned int i=_pathCurrentIndex; i<_path.size(); ++i) { - if(uContains(_optimizedPoses, _path[i].first) && - (_goalMaxDistance == 0.0f || _optimizedPoses.at(_memory->getLastWorkingSignature()->id()).getDistance(_optimizedPoses.at(_path[i].first)) < _goalMaxDistance)) + if(uContains(_optimizedPoses, _path[i].first)) + { + if(_localRadius > 0.0f) + { + if(i == _pathCurrentIndex) + { + distanceSoFar += _optimizedPoses.at(_memory->getLastWorkingSignature()->id()).getDistance(_optimizedPoses.at(_path[i].first)); + } + else + { + distanceSoFar += _optimizedPoses.at(_path[i-1].first).getDistance(_optimizedPoses.at(_path[i].first)); + } + } + + if(distanceSoFar <= _localRadius) + { + goalIndex = i; + } + else + { + break; + } + } + else { - goalIndex = i; break; } } @@ -2592,6 +2698,14 @@ void Rtabmap::updateGoalIndex() } } } + if(distance < 0) + { + UERROR("The nearest pose on the path not found!"); + } + else + { + UDEBUG("Nearest node = %d", _path[nearestNodeIndex].first); + } if(distance >= 0 && nearestNodeIndex != _pathCurrentIndex) { _pathCurrentIndex = nearestNodeIndex; diff --git a/examples/RGBDMapping/main.cpp b/examples/RGBDMapping/main.cpp index 2c6b0f4d..e543cf83 100644 --- a/examples/RGBDMapping/main.cpp +++ b/examples/RGBDMapping/main.cpp @@ -83,7 +83,7 @@ int main(int argc, char * argv[]) UERROR("Not built with OpenNI2 support..."); exit(-1); } - camera = new CameraOpenNI2(0, opticalRotation); + camera = new CameraOpenNI2("", 0, opticalRotation); } else if(driver == 2) { diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 214200f3..c9930291 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -177,6 +177,7 @@ public: bool getSourceOpenni2AutoExposure() const; //Openni group int getSourceOpenni2Exposure() const; //Openni group int getSourceOpenni2Gain() const; //Openni group + bool getSourceOpenni2Mirroring() const; //Openni group QString getSourceOpenniDevice() const; //Openni group Transform getSourceOpenniLocalTransform() const; //Openni group float getSourceOpenniFx() const; // Openni group diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index adbc3617..daf479d8 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -345,6 +345,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _ui->actionOpenNI2->setEnabled(CameraOpenNI2::available()); connect(_ui->actionOpenNI2_Sense, SIGNAL(triggered()), this, SLOT(selectOpenni2())); _ui->actionOpenNI2_Sense->setEnabled(CameraOpenNI2::available()); + connect(_ui->actionOpenNI2_kinect, SIGNAL(triggered()), this, SLOT(selectOpenni2())); + _ui->actionOpenNI2_kinect->setEnabled(CameraOpenNI2::available()); connect(_ui->actionSave_state, SIGNAL(triggered()), this, SLOT(saveFigures())); connect(_ui->actionLoad_state, SIGNAL(triggered()), this, SLOT(loadFigures())); @@ -2193,6 +2195,8 @@ void MainWindow::updateSelectSourceRGBDMenu(bool used, PreferencesDialog::Src sr _ui->actionOpenNI_CV->setChecked(used && src == PreferencesDialog::kSrcOpenNI_CV); _ui->actionOpenNI_CV_ASUS->setChecked(used && src == PreferencesDialog::kSrcOpenNI_CV_ASUS); _ui->actionOpenNI2->setChecked(used && src == PreferencesDialog::kSrcOpenNI2); + _ui->actionOpenNI2_Sense->setChecked(used && src == PreferencesDialog::kSrcOpenNI2); + _ui->actionOpenNI2_kinect->setChecked(used && src == PreferencesDialog::kSrcOpenNI2); } void MainWindow::changeImgRateSetting() @@ -2544,6 +2548,7 @@ void MainWindow::startDetection() else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2) { camera = new CameraOpenNI2( + _preferencesDialog->getSourceOpenniDevice().toStdString(), _preferencesDialog->getGeneralInputRate(), _preferencesDialog->getSourceOpenniLocalTransform(), _preferencesDialog->getSourceOpenniFx(), @@ -2599,6 +2604,7 @@ void MainWindow::startDetection() { ((CameraOpenNI2*)camera)->setAutoWhiteBalance(_preferencesDialog->getSourceOpenni2AutoWhiteBalance()); ((CameraOpenNI2*)camera)->setAutoExposure(_preferencesDialog->getSourceOpenni2AutoExposure()); + ((CameraOpenNI2*)camera)->setMirroring(_preferencesDialog->getSourceOpenni2Mirroring()); if(CameraOpenNI2::exposureGainAvailable()) { ((CameraOpenNI2*)camera)->setExposure(_preferencesDialog->getSourceOpenni2Exposure()); @@ -3473,27 +3479,35 @@ void MainWindow::selectDatabase() void MainWindow::selectOpenni() { - _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_PCL, _ui->actionOpenNI_PCL->isChecked()); + _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_PCL, + _ui->actionOpenNI_PCL->isChecked() || + _ui->actionOpenNI_PCL_ASUS->isChecked()); } void MainWindow::selectFreenect() { - _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcFreenect, _ui->actionFreenect->isChecked()); + _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcFreenect, + _ui->actionFreenect->isChecked()); } void MainWindow::selectOpenniCv() { - _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV, _ui->actionOpenNI_CV->isChecked()); + _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV, + _ui->actionOpenNI_CV->isChecked()); } void MainWindow::selectOpenniCvAsus() { - _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV_ASUS, _ui->actionOpenNI_CV_ASUS->isChecked()); + _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI_CV_ASUS, + _ui->actionOpenNI_CV_ASUS->isChecked()); } void MainWindow::selectOpenni2() { - _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI2, _ui->actionOpenNI2->isChecked()); + _preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcOpenNI2, + _ui->actionOpenNI2->isChecked() || + _ui->actionOpenNI2_Sense->isChecked() || + _ui->actionOpenNI2_kinect->isChecked()); } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 93bace44..5143867b 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -119,6 +119,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->label_map_shown->setText(_ui->label_map_shown->text() + " (Disabled, PCL >=1.7.2 required)"); #endif +#ifdef _WIN32 + _ui->radioButton_openni2->setChecked(true); + _ui->radioButton_opennipcl->setChecked(false); +#endif + if(RTABMAP_NONFREE == 0) { _ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1); @@ -278,6 +283,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->openni2_autoExposure, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->openni2_exposure, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->openni2_gain, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->openni2_mirroring, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->lineEdit_openniDevice, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->lineEdit_openniLocalTransform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->doubleSpinBox_openniFx, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); @@ -449,15 +455,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->globalDetection_toroIterations->setObjectName(Parameters::kRGBDToroIterations().c_str()); _ui->globalDetection_toroIgnoreVariance->setObjectName(Parameters::kRGBDToroIgnoreVariance().c_str()); _ui->globalDetection_optimizeFromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str()); + _ui->spinBox_maxLocalLocationsRetrieved->setObjectName(Parameters::kRGBDMaxLocalRetrieved().c_str()); _ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str()); - _ui->graphPlan_maxAnticipatedNodes->setObjectName(Parameters::kRGBDMaxAnticipatedNodes().c_str()); _ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanWithNearNodesLinked().c_str()); - _ui->graphPlan_goalMaxDistance->setObjectName(Parameters::kRGBDGoalMaxDistance().c_str()); _ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str()); _ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDLocalLoopDetectionSpace().c_str()); - _ui->localDetection_radius->setObjectName(Parameters::kRGBDLocalLoopDetectionRadius().c_str()); + _ui->localDetection_radius->setObjectName(Parameters::kRGBDLocalRadius().c_str()); _ui->localDetection_maxNeighbors->setObjectName(Parameters::kRGBDLocalLoopDetectionNeighbors().c_str()); _ui->localDetection_maxDiffID->setObjectName(Parameters::kRGBDLocalLoopDetectionMaxDiffID().c_str()); @@ -894,15 +899,21 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->source_spinBox_databaseStartPos->setValue(0); _ui->groupBox_sourceOpenni->setChecked(true); +#ifdef _WIN32 + _ui->radioButton_openni2->setChecked(true); + _ui->radioButton_opennipcl->setChecked(false); +#else _ui->radioButton_opennipcl->setChecked(true); - _ui->radioButton_freenect->setChecked(false); _ui->radioButton_openni2->setChecked(false); +#endif + _ui->radioButton_freenect->setChecked(false); _ui->radioButton_opennicv->setChecked(false); _ui->radioButton_opennicvasus->setChecked(false); _ui->openni2_autoWhiteBalance->setChecked(true); _ui->openni2_autoExposure->setChecked(true); _ui->openni2_exposure->setValue(0); _ui->openni2_gain->setValue(100); + _ui->openni2_mirroring->setChecked(false); _ui->lineEdit_openniDevice->setText(""); _ui->lineEdit_openniLocalTransform->setText("0 0 0 -PI_2 0 -PI_2"); _ui->doubleSpinBox_openniFx->setValue(0.0); @@ -1168,6 +1179,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->openni2_autoExposure->setChecked(settings.value("openni2AutoExposure", _ui->openni2_autoExposure->isChecked()).toBool()); _ui->openni2_exposure->setValue(settings.value("openni2Exposure", _ui->openni2_exposure->value()).toInt()); _ui->openni2_gain->setValue(settings.value("openni2Gain", _ui->openni2_gain->value()).toInt()); + _ui->openni2_mirroring->setChecked(settings.value("openni2Mirroring", _ui->openni2_mirroring->isChecked()).toBool()); _ui->lineEdit_openniDevice->setText(settings.value("device",_ui->lineEdit_openniDevice->text()).toString()); _ui->lineEdit_openniLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_openniLocalTransform->text()).toString()); _ui->doubleSpinBox_openniFx->setValue(settings.value("fx", _ui->doubleSpinBox_openniFx->value()).toDouble()); @@ -1416,6 +1428,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.setValue("openni2AutoExposure", _ui->openni2_autoExposure->isChecked()); settings.setValue("openni2Exposure", _ui->openni2_exposure->value()); settings.setValue("openni2Gain", _ui->openni2_gain->value()); + settings.setValue("openni2Mirroring", _ui->openni2_mirroring->isChecked()); settings.setValue("device", _ui->lineEdit_openniDevice->text()); settings.setValue("localTransform", _ui->lineEdit_openniLocalTransform->text()); settings.setValue("fx", _ui->doubleSpinBox_openniFx->value()); @@ -3147,6 +3160,10 @@ int PreferencesDialog::getSourceOpenni2Gain() const { return _ui->openni2_gain->value(); } +bool PreferencesDialog::getSourceOpenni2Mirroring() const +{ + return _ui->openni2_mirroring->isChecked(); +} QString PreferencesDialog::getSourceOpenniDevice() const { return _ui->lineEdit_openniDevice->text(); @@ -3335,6 +3352,7 @@ void PreferencesDialog::testOdometry(int type) else if(this->getSourceRGBD() == kSrcOpenNI2) { camera = new CameraOpenNI2( + this->getSourceOpenniDevice().toStdString(), this->getGeneralInputRate(), this->getSourceOpenniLocalTransform(), this->getSourceOpenniFx(), @@ -3382,6 +3400,7 @@ void PreferencesDialog::testOdometry(int type) { ((CameraOpenNI2*)camera)->setAutoWhiteBalance(getSourceOpenni2AutoWhiteBalance()); ((CameraOpenNI2*)camera)->setAutoExposure(getSourceOpenni2AutoExposure()); + ((CameraOpenNI2*)camera)->setMirroring(getSourceOpenni2Mirroring()); if(CameraOpenNI2::exposureGainAvailable()) { ((CameraOpenNI2*)camera)->setExposure(getSourceOpenni2Exposure()); @@ -3500,6 +3519,7 @@ void PreferencesDialog::testRGBDCamera() else if(this->getSourceRGBD() == kSrcOpenNI2) { camera = new CameraOpenNI2( + this->getSourceOpenniDevice().toStdString(), this->getGeneralInputRate(), this->getSourceOpenniLocalTransform(), this->getSourceOpenniFx(), @@ -3547,6 +3567,7 @@ void PreferencesDialog::testRGBDCamera() { ((CameraOpenNI2*)camera)->setAutoWhiteBalance(getSourceOpenni2AutoWhiteBalance()); ((CameraOpenNI2*)camera)->setAutoExposure(getSourceOpenni2AutoExposure()); + ((CameraOpenNI2*)camera)->setMirroring(getSourceOpenni2Mirroring()); if(CameraOpenNI2::exposureGainAvailable()) { ((CameraOpenNI2*)camera)->setExposure(getSourceOpenni2Exposure()); @@ -3596,6 +3617,7 @@ void PreferencesDialog::calibrate() else if(this->getSourceRGBD() == kSrcOpenNI2) { camera = new CameraOpenNI2( + this->getSourceOpenniDevice().toStdString(), this->getGeneralInputRate(), this->getSourceOpenniLocalTransform(), this->getSourceOpenniFx(), @@ -3643,6 +3665,7 @@ void PreferencesDialog::calibrate() { ((CameraOpenNI2*)camera)->setAutoWhiteBalance(getSourceOpenni2AutoWhiteBalance()); ((CameraOpenNI2*)camera)->setAutoExposure(getSourceOpenni2AutoExposure()); + ((CameraOpenNI2*)camera)->setMirroring(getSourceOpenni2Mirroring()); if(CameraOpenNI2::exposureGainAvailable()) { ((CameraOpenNI2*)camera)->setExposure(getSourceOpenni2Exposure()); diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index a5d04de4..a2aa66ec 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -105,18 +105,19 @@ - Kinect for Xbox 360 + Kinect - + + Xtion PRO LIVE - + @@ -1089,6 +1090,9 @@ + + true + OpenNI2 @@ -1123,6 +1127,14 @@ Save config + + + true + + + OpenNI2 + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 9e67261f..44b5e714 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -7,7 +7,7 @@ 0 0 1035 - 789 + 716 @@ -63,9 +63,9 @@ 0 - -733 + -522 744 - 1475 + 1179 @@ -86,7 +86,7 @@ QFrame::Raised - 19 + 3 @@ -1720,7 +1720,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki OpenNI 2 - + 1000 @@ -1730,7 +1730,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + 65535 @@ -1777,7 +1777,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + Exposure. @@ -1787,7 +1787,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - + Gain. @@ -1797,6 +1797,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + + + + false + + + + + + + Mirroring. + + + true + + + @@ -2880,7 +2900,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Image decimation. This feature can be used to save images in lower resolution (size/decimation). @@ -2890,7 +2910,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + 1 @@ -2900,26 +2920,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - If local space links are kept in WM. - - - true - - - - - - - - - - true - - - @@ -4993,6 +4993,39 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority). + + + true + + + + + + + + + + m + + + 1.000000000000000 + + + + + + + Local radius for nodes selection in the local map. This parameter is used in some approaches below. + + + true + + + @@ -5215,7 +5248,7 @@ Warning when set to false: when some nodes are transferred, the first referentia - Activate local detection over locations (in Working Memory) near in space. The Bayes filter is not used here, so it may results in more false detections. ICP 2D only is used here. + Activate local detection over locations (in Working Memory) near in space. The Bayes filter is not used here, so it may results in more false detections. ICP 2D only is used here. Only nodes in the local radius are used. true @@ -5224,27 +5257,17 @@ Warning when set to false: when some nodes are transferred, the first referentia - - - - m + + + + 999 - 1.000000000000000 + 0 - - - - Maximum radius for space detection. - - - true - - - - + 10 @@ -5252,6 +5275,16 @@ Warning when set to false: when some nodes are transferred, the first referentia + + + Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore. + + + true + + + + Maximum nearest neighbors for space detection. @@ -5262,19 +5295,16 @@ Warning when set to false: when some nodes are transferred, the first referentia - - - 999 - - - 0 + + + - + - Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore. + If local space links are kept in WM. true @@ -5291,86 +5321,57 @@ Warning when set to false: when some nodes are transferred, the first referentia Graph planning - - - - - m - - - 1.000000000000000 - - - - - + + + - Goal reached radius. + If the goal is too far (>local radius) from the graph, the plan is aborted. The next goal in the graph can't be farther than the local radius around the current position. Anticipated nodes on the path are retrieved in the local radius up to "Maximum local locations retrieved". A virtual link is added between the current location and the nearest one on the path up to local radius. true - - - - 999999999 - - - 10 - - - - - - - Maximum anticipated nodes on the computed path that can be retrieved (the number of nodes actually retrieved at each iteration is limited by "Maximum locations retrieved" from the Memory panel). - - - true - - - - - - - - - - - - - - Before planning in the graph, near nodes are linked together (even if they don't belong to same map). Radius is defined by "Goal reached radius" above. - - - true - - - - - - - Maximum distance (m) of the target goal from the graph (0 means infinity). If the goal is too far from the graph, the plan is aborted. Also when set, the next goal in the graph can't be farther than this distance from the current position. - - - true - - - - - - - m - - - 0 - - - 1.000000000000000 - - + + + + + + m + + + 1.000000000000000 + + + + + + + Goal reached radius. + + + true + + + + + + + + + + + + + + Before planning in the graph, near nodes are linked together (even if they don't belong to same map). Radius is defined by "Goal reached radius" above. + + + true + + + + diff --git a/tools/Calibration/main.cpp b/tools/Calibration/main.cpp index 6074f359..537afb72 100644 --- a/tools/Calibration/main.cpp +++ b/tools/Calibration/main.cpp @@ -122,7 +122,7 @@ int main(int argc, char * argv[]) UERROR("Not built with OpenNI2 support..."); exit(-1); } - camera = new rtabmap::CameraOpenNI2(imageRate); + camera = new rtabmap::CameraOpenNI2(uNumber2Str(device), imageRate); } else if(driver == 3) { diff --git a/tools/DataRecorder/main.cpp b/tools/DataRecorder/main.cpp index d203a00e..6e049437 100644 --- a/tools/DataRecorder/main.cpp +++ b/tools/DataRecorder/main.cpp @@ -182,7 +182,7 @@ int main (int argc, char * argv[]) if(openni2) { - cam = new rtabmap::CameraThread(new rtabmap::CameraOpenNI2(rate, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0))); + cam = new rtabmap::CameraThread(new rtabmap::CameraOpenNI2("", rate, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0))); } else if(openni) { diff --git a/tools/OdometryViewer/main.cpp b/tools/OdometryViewer/main.cpp index 7d4b91b4..74e84897 100644 --- a/tools/OdometryViewer/main.cpp +++ b/tools/OdometryViewer/main.cpp @@ -721,7 +721,7 @@ int main (int argc, char * argv[]) UERROR("Not built with OpenNI2 support..."); exit(-1); } - camera = new rtabmap::CameraOpenNI2(rate, t); + camera = new rtabmap::CameraOpenNI2("", rate, t); } else if(driver == 2) {