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

4
corelib/include/rtabmap/core/Link.h
View File

@@ -79,6 +79,7 @@ public:
void setTo(int to) {to_ = to;}
void setTransform(const Transform & transform) {transform_ = transform;}
void setType(Type type) {type_ = type;}
void setInfMatrix(const cv::Mat & infMatrix);
const cv::Mat & userDataRaw() const {return _userDataRaw;}
const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
@@ -88,9 +89,6 @@ public:
Link merge(const Link & link, Type outputType) const;
Link inverse() const;
private:
void setInfMatrix(const cv::Mat & infMatrix);
private:
int from_;
int to_;

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();