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

Supporting GTSAM bearing/range factors for landmarks (#1346)

Browse Source 
* Supporting GTSAM bearing/range factors for landmarks

* refreshing initial covariance after being modified in same sessions
This commit is contained in:
matlabbe
2024-09-21 18:14:52 -07:00
committed by GitHub
parent 0e85bd849c
commit 379e11bd66
6 changed files with 528 additions and 324 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@@ -551,7 +552,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
}
}
else
else if(1 / static_cast<double>(iter->second.infMatrix().at<double>(1,1)) < 9999)
{
Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
if(!isCovarianceIgnored())
@@ -566,6 +567,21 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
graph.add(gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2>(id1, id2, p.bearing(landmark), p.range(landmark), model));
lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
}
else
{
Eigen::Matrix<double, 1, 1> information = Eigen::Matrix<double, 1, 1>::Identity();
if(!isCovarianceIgnored())
{
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,1), cv::Range(0,1)).clone();;
memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
}
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information);
gtsam::Point2 landmark(t.x(), t.y());
gtsam::Pose2 p;
graph.add(gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2>(id1, id2, p.bearing(landmark), model));
lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
}
}
else
{
@@ -599,14 +615,15 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
}
}
else
else if(1 / static_cast<double>(iter->second.infMatrix().at<double>(2,2)) < 9999)
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,3), cv::Range(0,3)).clone();;
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,3), cv::Range(0,3)).clone();
memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
}
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information);
gtsam::Point3 landmark(t.x(), t.y(), t.z());
@@ -614,6 +631,22 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
graph.add(gtsam::BearingRangeFactor<gtsam::Pose3, gtsam::Point3>(id1, id2, p.bearing(landmark), p.range(landmark), model));
lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
}
else
{
Eigen::Matrix<double, 2, 2> information = Eigen::Matrix<double, 2, 2>::Identity();
if(!isCovarianceIgnored())
{
cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,2), cv::Range(0,2)).clone();
memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double));
}
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information);
gtsam::Point3 landmark(t.x(), t.y(), t.z());
gtsam::Pose3 p;
graph.add(gtsam::BearingFactor<gtsam::Pose3, gtsam::Point3>(id1, id2, p.bearing(landmark), model));
lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1));
}
}
}
}