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

Fixed #944

Browse Source
This commit is contained in:
matlabbe
2022-12-14 19:42:56 -08:00
parent a4761ad397
commit 4a6a765cd1
1 changed files with 8 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -303,8 +303,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity(); Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal
mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam); gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
graph.add(gtsam::PriorFactor<gtsam::Pose3>(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model)); graph.add(gtsam::PriorFactor<gtsam::Pose3>(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
@@ -383,8 +383,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity(); Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal
mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam); gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(t.toEigen4d()), model)); graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(t.toEigen4d()), model));
} }
@@ -471,8 +471,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity(); Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal mgtsam.block(0,3,3,3) = information.block(3,0,3,3); // off diagonal
mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal mgtsam.block(3,0,3,3) = information.block(0,3,3,3); // off diagonal
gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam); gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
#ifdef RTABMAP_VERTIGO #ifdef RTABMAP_VERTIGO
@@ -708,8 +708,8 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity(); Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
mgtsam.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation mgtsam.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation
mgtsam.block(0,0,3,3) = info.block(3,3,3,3); // cov translation mgtsam.block(0,0,3,3) = info.block(3,3,3,3); // cov translation
mgtsam.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal mgtsam.block(0,3,3,3) = info.block(3,0,3,3); // off diagonal
mgtsam.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal mgtsam.block(3,0,3,3) = info.block(0,3,3,3); // off diagonal
memcpy(outputCovariance.data, mgtsam.data(), outputCovariance.total()*sizeof(double)); memcpy(outputCovariance.data, mgtsam.data(), outputCovariance.total()*sizeof(double));
} }
else else