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

Merging factors

release/4.3a0
Frank Dellaert 2016-01-31 02:01:17 -08:00
parent e9f6b52620
commit 15e3b2ea34
3 changed files with 32 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/navigation/ImuFactor.cpp
View File

@ -113,9 +113,9 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
}
//------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
NonlinearFactor::shared_ptr ImuFactor::clone() const {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this)));
}
//------------------------------------------------------------------------------
@ -178,6 +178,29 @@ PreintegratedImuMeasurements ImuFactor::Merge(
return pim02;
}
//------------------------------------------------------------------------------
ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01,
const shared_ptr& f12) {
// IMU bias keys must be the same.
if (f01->key5() != f12->key5())
throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same");
// expect intermediate pose, velocity keys to matchup.
if (f01->key3() != f12->key1() || f01->key4() != f12->key2())
throw std::domain_error(
"ImuFactor::Merge: intermediate pose, velocity keys need to match up");
// return new factor
auto pim02 =
Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
return boost::make_shared<ImuFactor>(f01->key1(), // P0
f01->key2(), // V0
f12->key3(), // P2
f12->key4(), // V2
f01->key5(), // B
pim02);
}
//------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,

5
gtsam/navigation/ImuFactor.h
View File

@ -225,11 +225,14 @@ public:
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
boost::none, boost::optional<Matrix&> H5 = boost::none) const;
// Merge two pre-integrated measurement classes
/// Merge two pre-integrated measurement classes
static PreintegratedImuMeasurements Merge(
const PreintegratedImuMeasurements& pim01,
const PreintegratedImuMeasurements& pim12);
/// Merge two factors
static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @deprecated typename
typedef PreintegratedImuMeasurements PreintegratedMeasurements;

4
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -829,8 +829,8 @@ struct ImuFactorMergeTest {
ImuFactor::shared_ptr factor02_expected = boost::make_shared<ImuFactor>(
X(0), V(0), X(2), V(2), B(0), pim02_expected);
// ImuFactor::shared_ptr factor02_merged = factor01.mergeWith(factor12);
// EXPECT(assert_equal(*factor02_expected, *factor02_merged, tol));
ImuFactor::shared_ptr factor02_merged = ImuFactor::Merge(factor01, factor12);
EXPECT(assert_equal(*factor02_expected, *factor02_merged, tol));
return result_.getFailureCount();
}