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

iOS: fixed origin when we do New Scan while we were already mapping, improved FPV feedback in point cloud mode, showing camera overlay in visualization mode (pinch out to disable). Localization mode: Updated how marker detection transform are modified by gravity constraints.

Browse Source
This commit is contained in:
matlabbe
2021-06-12 12:42:56 -04:00
parent 3f633081cb
commit 6342658ec5
11 changed files with 92 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
corelib/src/Rtabmap.cpp
View File

@@ -531,7 +531,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDProximityOdomGuess(), _proximityOdomGuess);
bool optimizeFromGraphEndPrevious = _optimizeFromGraphEnd;
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
if(optimizeFromGraphEndPrevious != _optimizeFromGraphEnd)
if(optimizeFromGraphEndPrevious != _optimizeFromGraphEnd && !_optimizedPoses.empty())
{
_optimizeFromGraphEndChanged = true;
}
@@ -2879,7 +2879,7 @@ bool Rtabmap::process(
UASSERT(!landmarkDetectedNodesRef.empty());
loopId = *landmarkDetectedNodesRef.begin();
const Signature * loopS = _memory->getSignature(loopId);
transform = transform * loopS->getLandmarks().at(landmarkId).transform().inverse();
transform = transform * _optimizedPoses.at(landmarkId).inverse()*_optimizedPoses.at(loopS->id());
UASSERT(_optimizedPoses.find(loopId) != _optimizedPoses.end());
oldPose = _optimizedPoses.at(loopId);
}
@@ -2897,7 +2897,6 @@ bool Rtabmap::process(
targetRotation = Transform(0,0,0,roll,pitch,targetRotation.theta());
Transform error = transform.rotation().inverse() * iterGravitySign->second.transform().rotation().inverse() * targetRotation;
transform *= error;
u = signature->getPose() * transform;
}
else if(iterGravityLoop!=loopS->getLinks().end() ||
@@ -2936,7 +2935,7 @@ bool Rtabmap::process(
UASSERT(!landmarkDetectedNodesRef.empty());
loopId = *landmarkDetectedNodesRef.begin();
const Signature * loopS = _memory->getSignature(loopId);
transform = transform * loopS->getLandmarks().at(landmarkId).transform().inverse();
transform = transform * _optimizedPoses.at(landmarkId).inverse()*_optimizedPoses.at(loopS->id());
}
const Signature * loopS = _memory->getSignature(loopId);

5
corelib/src/optimizer/OptimizerGTSAM.cpp
View File

@@ -221,6 +221,10 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
int id1 = iter->second.from();
int id2 = iter->second.to();
UASSERT_MSG(initialEstimate.find(id1)!=initialEstimate.end(), uFormat("id1=%d", id1).c_str());
UASSERT_MSG(initialEstimate.find(id2)!=initialEstimate.end(), uFormat("id2=%d", id2).c_str());
UASSERT(!iter->second.transform().isNull());
if(id1 == id2)
{
@@ -404,7 +408,6 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel));
}
#endif
if(isSlam2d())
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();