i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-12 06:20:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated how locations are retrieved on a planned path or near based on space if no path is activated.

Browse Source 
Added mirroring option to OpenNI2 camera
Removed "RGBD/MaxAnticipatedNodes" and "RGBD/GoalMaxDistance" parameters
Renamed "RGBD/LocalLoopDetectionRadius" to "RGBD/LocalRadius"
Added "RGBD/MaxLocalRetrieved" parameter
GUI: fixed OpenNI 2 selection from the MainWindow, added OpenNI2 under Kinect menu
This commit is contained in:
Mathieu Labbe
2015-02-20 15:32:39 -05:00
parent 647c709595
commit 38a4e8e4ee
16 changed files with 519 additions and 265 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/CameraRGBD.h
+4 -1
View File
@@ -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;
};
corelib/include/rtabmap/core/Graph.h
+23 -1
View File
@@ -77,6 +77,14 @@ bool RTABMAP_EXP loadTOROGraph(const std::string & fileName,
std::map<int, Transform> & poses,
std::multimap<int, std::pair<int, Transform> > & 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<int, Transform> RTABMAP_EXP radiusPosesFiltering(
const std::map<int, Transform> & poses,
float radius,
@@ -87,7 +95,7 @@ std::map<int, Transform> 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<int, int> RTABMAP_EXP radiusPosesClustering(
@@ -115,12 +123,26 @@ int RTABMAP_EXP findNearestNode(
const std::map<int, rtabmap::Transform> & 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<int, float> RTABMAP_EXP getNodesInRadius(
int nodeId,
const std::map<int, Transform> & nodes,
int maxNearestNeighbors,
float radius);
float RTABMAP_EXP computePathLength(
const std::vector<std::pair<int, Transform> > & path,
unsigned int fromIndex = 0,
unsigned int toIndex = 0);
} /* namespace graph */
} /* namespace rtabmap */
corelib/include/rtabmap/core/Parameters.h
+2 -3
View File
@@ -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.")
corelib/include/rtabmap/core/Rtabmap.h
+3 -5
View File
@@ -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<int, float> getNodesInRadius(int fromId, int maxNearestNeighbors, float radius) const;
std::map<int, Transform> getWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
void adjustLikelihood(std::map<int, float> & likelihood) const;
std::pair<int, float> selectHypothesis(const std::map<int, float> & 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<int, float> _loopClosureHypothesis;
std::pair<int, float> _highestHypothesis;
corelib/src/CameraRGBD.cpp
+29 -4
View File
@@ -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;
corelib/src/Graph.cpp
+58 -16
View File
@@ -505,9 +505,13 @@ bool loadTOROGraph(const std::string & fileName,
}
std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & poses, float radius, float angle, bool keepLatest)
std::map<int, Transform> radiusPosesFiltering(
const std::map<int, Transform> & 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<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(poses.size());
@@ -528,7 +532,6 @@ std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & p
for(unsigned int i=0; i<cloud->size(); ++i)
{
// ignore scans
if(indicesChecked.find(i) == indicesChecked.end())
{
std::vector<int> kIndices;
@@ -542,11 +545,18 @@ std::map<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & 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<int, Transform> radiusPosesFiltering(const std::map<int, Transform> & p
std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & poses, float radius, float angle)
{
std::multimap<int, int> clusters;
if(poses.size() > 1 && radius > 0.0f && angle>0.0f)
if(poses.size() > 1 && radius > 0.0f)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(poses.size());
@@ -635,11 +645,18 @@ std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & 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 <id, distance>, including query
// return <id, sqrd distance>, including query
std::map<int, float> getNodesInRadius(
int nodeId,
const std::map<int, Transform> & nodes,
@@ -868,7 +885,7 @@ std::map<int, float> 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<int, float> getNodesInRadius(
return foundNodes;
}
float computePathLength(
const std::vector<std::pair<int, Transform> > & 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<toIndex-1; ++i)
{
x += fabs(path[i].second.x() - path[i+1].second.x());
y += fabs(path[i].second.y() - path[i+1].second.y());
z += fabs(path[i].second.z() - path[i+1].second.z());
}
length = sqrt(x*x + y*y + z*z);
}
return length;
}
} /* namespace graph */
} /* namespace rtabmap */
corelib/src/Rtabmap.cpp
+205 -91
View File
@@ -82,6 +82,7 @@ Rtabmap::Rtabmap() :
_loopThr(Parameters::defaultRtabmapLoopThr()),
_loopRatio(Parameters::defaultRtabmapLoopRatio()),
_maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
_maxLocalRetrieved(Parameters::defaultRGBDMaxLocalRetrieved()),
_statisticLogsBufferedInRAM(Parameters::defaultRtabmapStatisticLogsBufferedInRAM()),
_statisticLogged(Parameters::defaultRtabmapStatisticLogged()),
_statisticLoggedHeaders(Parameters::defaultRtabmapStatisticLoggedHeaders()),
@@ -94,7 +95,7 @@ Rtabmap::Rtabmap() :
_poseScanMatching(Parameters::defaultRGBDPoseScanMatching()),
_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
_localDetectRadius(Parameters::defaultRGBDLocalLoopDetectionRadius()),
_localRadius(Parameters::defaultRGBDLocalRadius()),
_localDetectMaxNeighbors(Parameters::defaultRGBDLocalLoopDetectionNeighbors()),
_localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()),
_toroIterations(Parameters::defaultRGBDToroIterations()),
@@ -108,9 +109,7 @@ Rtabmap::Rtabmap() :
_reextractMaxWords(Parameters::defaultLccReextractMaxWords()),
_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
_maxAnticipatedNodes(Parameters::defaultRGBDMaxAnticipatedNodes()),
_planWithNearNodesLinked(Parameters::defaultRGBDPlanWithNearNodesLinked()),
_goalMaxDistance(Parameters::defaultRGBDGoalMaxDistance()),
_loopClosureHypothesis(0,0.0f),
_highestHypothesis(0,0.0f),
_lastProcessTime(0.0),
@@ -351,6 +350,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio);
Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
Parameters::parse(parameters, Parameters::kRGBDMaxLocalRetrieved(), _maxLocalRetrieved);
Parameters::parse(parameters, Parameters::kRtabmapStatisticLogsBufferedInRAM(), _statisticLogsBufferedInRAM);
Parameters::parse(parameters, Parameters::kRtabmapStatisticLogged(), _statisticLogged);
Parameters::parse(parameters, Parameters::kRtabmapStatisticLoggedHeaders(), _statisticLoggedHeaders);
@@ -362,7 +362,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kLccIcpMaxDistance(), _globalLoopClosureIcpMaxDistance);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionRadius(), _localDetectRadius);
Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionNeighbors(), _localDetectMaxNeighbors);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
Parameters::parse(parameters, Parameters::kRGBDToroIterations(), _toroIterations);
@@ -375,9 +375,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kLccReextractMaxWords(), _reextractMaxWords);
Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
Parameters::parse(parameters, Parameters::kRGBDMaxAnticipatedNodes(), _maxAnticipatedNodes);
Parameters::parse(parameters, Parameters::kRGBDPlanWithNearNodesLinked(), _planWithNearNodesLinked);
Parameters::parse(parameters, Parameters::kRGBDGoalMaxDistance(), _goalMaxDistance);
// RGB-D SLAM stuff
if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
@@ -1224,34 +1222,80 @@ bool Rtabmap::process(const SensorData & data)
//============================================================
// RETRIEVAL 2/3 : Update planned path and get next nodes to retrieve
//============================================================
std::list<int> retrievalPathIds;
if(_path.size() && _rgbdSlamMode)
std::set<int> 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<int, float> nearNodes = graph::getNodesInRadius(signature->id(), _optimizedPoses, 0, _localRadius);
// sort by distance
std::multimap<float, int> nearNodesByDist;
for(std::map<int, float>::iterator iter=nearNodes.begin(); iter!=nearNodes.end(); ++iter)
{
nearNodesByDist.insert(std::make_pair(iter->second, iter->first));
}
for(std::multimap<float, int>::iterator iter=nearNodesByDist.begin();
iter!=nearNodesByDist.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
++iter)
{
const Signature * s = _memory->getSignature(iter->second);
UASSERT(s != 0);
for(std::map<int, Link>::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<int, int> ids = _memory->getNeighborsId(signature->id(), 0, 0, true);
std::map<int, Transform> poses;
std::map<int, int> mapIds;
mapIds.insert(std::make_pair(signature->id(), _memory->getMapId(signature->id())));
if(_loopClosureHypothesis.first)
std::multimap<int, Link> constraints;
_memory->getMetricConstraints(uKeys(ids), poses, constraints, false);
for(std::map<int, Transform>::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<int>::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<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, 0, true);
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end();)
{
if(!uContains(ids, iter->first))
{
_optimizedPoses.erase(iter++);
}
else
{
++iter;
}
}
for(std::multimap<int, Link>::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<int, int> 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<int, int> mapIds;
for(std::map<int, Transform>::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<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, 0, true);
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
_memory->getMetricConstraints(uKeys(ids), poses, constraints, false);
for(std::map<int, Transform>::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;
examples/RGBDMapping/main.cpp
+1 -1
View File
@@ -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)
{
guilib/include/rtabmap/gui/PreferencesDialog.h
+1
View File
@@ -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
guilib/src/MainWindow.cpp
+19 -5
View File
@@ -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());
}
guilib/src/PreferencesDialog.cpp
+27 -4
View File
@@ -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());
guilib/src/ui/mainWindow.ui
+15 -3
View File
@@ -105,18 +105,19 @@
</property>
<widget class="QMenu" name="menuKinect_for_Xbox_360">
<property name="title">
<string>Kinect for Xbox 360</string>
<string>Kinect</string>
</property>
<addaction name="actionOpenNI_PCL"/>
<addaction name="actionFreenect"/>
<addaction name="actionOpenNI2_kinect"/>
<addaction name="actionOpenNI_PCL"/>
<addaction name="actionOpenNI_CV"/>
</widget>
<widget class="QMenu" name="menuXtion_PRO_LIVE">
<property name="title">
<string>Xtion PRO LIVE</string>
</property>
<addaction name="actionOpenNI_PCL_ASUS"/>
<addaction name="actionOpenNI2"/>
<addaction name="actionOpenNI_PCL_ASUS"/>
<addaction name="actionOpenNI_CV_ASUS"/>
</widget>
<widget class="QMenu" name="menuSense_3D_scanner">
@@ -1089,6 +1090,9 @@
</property>
</action>
<action name="actionOpenNI2_Sense">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>OpenNI2</string>
</property>
@@ -1123,6 +1127,14 @@
<string>Save config</string>
</property>
</action>
<action name="actionOpenNI2_kinect">
<property name="checkable">
<bool>true</bool>
</property>
<property name="text">
<string>OpenNI2</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>
guilib/src/ui/preferencesDialog.ui
+129 -128
View File
@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>1035</width>
<height>789</height>
<height>716</height>
</rect>
</property>
<property name="sizePolicy">
@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-733</y>
<y>-522</y>
<width>744</width>
<height>1475</height>
<height>1179</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>19</number>
<number>3</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1720,7 +1720,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>OpenNI 2</string>
</property>
<layout class="QFormLayout" name="formLayout_6">
<item row="3" column="0">
<item row="6" column="0">
<widget class="QSpinBox" name="openni2_gain">
<property name="maximum">
<number>1000</number>
@@ -1730,7 +1730,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="2" column="0">
<item row="3" column="0">
<widget class="QSpinBox" name="openni2_exposure">
<property name="maximum">
<number>65535</number>
@@ -1777,7 +1777,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="2" column="1">
<item row="3" column="1">
<widget class="QLabel" name="label_219">
<property name="text">
<string>Exposure.</string>
@@ -1787,7 +1787,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="3" column="1">
<item row="6" column="1">
<widget class="QLabel" name="label_220">
<property name="text">
<string>Gain.</string>
@@ -1797,6 +1797,26 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QCheckBox" name="openni2_mirroring">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_223">
<property name="text">
<string>Mirroring.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>
@@ -2880,7 +2900,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="8" column="1">
<item row="7" column="1">
<widget class="QLabel" name="label_retrieved_6">
<property name="text">
<string>Image decimation. This feature can be used to save images in lower resolution (size/decimation).</string>
@@ -2890,7 +2910,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="8" column="0">
<item row="7" column="0">
<widget class="QSpinBox" name="spinBox_imageDecimation">
<property name="minimum">
<number>1</number>
@@ -2900,26 +2920,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_retrieved_7">
<property name="text">
<string>If local space links are kept in WM.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QCheckBox" name="checkBox_localSpaceLinksKeptInWM">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -4993,6 +4993,39 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_scanMatching_3">
<property name="text">
<string>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).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QSpinBox" name="spinBox_maxLocalLocationsRetrieved"/>
</item>
<item row="6" column="0">
<widget class="QDoubleSpinBox" name="localDetection_radius">
<property name="suffix">
<string> m</string>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_space2">
<property name="text">
<string>Local radius for nodes selection in the local map. This parameter is used in some approaches below.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>
@@ -5215,7 +5248,7 @@ Warning when set to false: when some nodes are transferred, the first referentia
<item>
<widget class="QLabel" name="label_space1_2">
<property name="text">
<string>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.</string>
<string>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.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -5224,27 +5257,17 @@ Warning when set to false: when some nodes are transferred, the first referentia
</item>
<item>
<layout class="QGridLayout" name="gridLayout_50" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="localDetection_radius">
<property name="suffix">
<string> m</string>
<item row="1" column="0">
<widget class="QSpinBox" name="localDetection_maxDiffID">
<property name="maximum">
<number>999</number>
</property>
<property name="value">
<double>1.000000000000000</double>
<number>0</number>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_space2">
<property name="text">
<string>Maximum radius for space detection.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<item row="0" column="0">
<widget class="QSpinBox" name="localDetection_maxNeighbors">
<property name="value">
<number>10</number>
@@ -5252,6 +5275,16 @@ Warning when set to false: when some nodes are transferred, the first referentia
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_space3_2">
<property name="text">
<string>Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_space3">
<property name="text">
<string>Maximum nearest neighbors for space detection.</string>
@@ -5262,19 +5295,16 @@ Warning when set to false: when some nodes are transferred, the first referentia
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="localDetection_maxDiffID">
<property name="maximum">
<number>999</number>
</property>
<property name="value">
<number>0</number>
<widget class="QCheckBox" name="checkBox_localSpaceLinksKeptInWM">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_space3_2">
<widget class="QLabel" name="label_scanMatching_2">
<property name="text">
<string>Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.</string>
<string>If local space links are kept in WM.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -5291,86 +5321,57 @@ Warning when set to false: when some nodes are transferred, the first referentia
<property name="title">
<string>Graph planning</string>
</property>
<layout class="QGridLayout" name="gridLayout_51" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="graphPlan_goalReachedRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_space2_2">
<layout class="QVBoxLayout" name="verticalLayout_61">
<item>
<widget class="QLabel" name="label_space2_3">
<property name="text">
<string>Goal reached radius.</string>
<string> If the goal is too far (&gt;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 &quot;Maximum local locations retrieved&quot;. A virtual link is added between the current location and the nearest one on the path up to local radius.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QSpinBox" name="graphPlan_maxAnticipatedNodes">
<property name="maximum">
<number>999999999</number>
</property>
<property name="value">
<number>10</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_space3_3">
<property name="text">
<string>Maximum anticipated nodes on the computed path that can be retrieved (the number of nodes actually retrieved at each iteration is limited by &quot;Maximum locations retrieved&quot; from the Memory panel).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="graphPlan_planWithNearNodesLinked">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_space3_4">
<property name="text">
<string>Before planning in the graph, near nodes are linked together (even if they don't belong to same map). Radius is defined by &quot;Goal reached radius&quot; above.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_space3_5">
<property name="text">
<string>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.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="graphPlan_goalMaxDistance">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
<item>
<layout class="QGridLayout" name="gridLayout_51" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="graphPlan_goalReachedRadius">
<property name="suffix">
<string> m</string>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_space2_2">
<property name="text">
<string>Goal reached radius.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="graphPlan_planWithNearNodesLinked">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_space3_4">
<property name="text">
<string>Before planning in the graph, near nodes are linked together (even if they don't belong to same map). Radius is defined by &quot;Goal reached radius&quot; above.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
tools/Calibration/main.cpp
+1 -1
View File
@@ -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)
{
tools/DataRecorder/main.cpp
+1 -1
View File
@@ -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)
{
tools/OdometryViewer/main.cpp
+1 -1
View File
@@ -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)
{