12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add conversion function for Pose2 -> BinaryMeasurement<Rot2>

release/4.3a0
John Lambert 2021-07-09 17:48:26 -04:00 committed by GitHub
parent 3c8cdb4eee
commit 4bf2308ec5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 17 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/sfm/ShonanAveraging.cpp
View File

@ -944,11 +944,27 @@ ShonanAveraging2::ShonanAveraging2(string g2oFile, const Parameters &parameters)
parameters.getUseHuber()),
parameters) {}
// Extract Rot2 measurement from Pose2 betweenfactors
// Modeled after similar function in dataset.cpp
static BinaryMeasurement<Rot2> convertPose2ToBinaryMeasurementRot2(
const BetweenFactor<Pose2>::shared_ptr &f) {
auto gaussian =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
if (!gaussian)
throw std::invalid_argument(
"parseMeasurements<Rot2> can only convert Pose2 measurements "
"with Gaussian noise models.");
const Matrix6 M = gaussian->covariance();
auto model = noiseModel::Gaussian::Covariance(M.block<2, 2>(2, 2));
return BinaryMeasurement<Rot2>(f->key1(), f->key2(), f->measured().rotation(),
model);
}
static ShonanAveraging2::Measurements extractRot2Measurements(
const BetweenFactorPose2s &factors) {
ShonanAveraging2::Measurements result;
result.reserve(factors.size());
for (auto f : factors) result.push_back(convert(f));
for (auto f : factors) result.push_back(convertPose2ToBinaryMeasurementRot2(f));
return result;
}