Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added non-default const. for AHRS pre-int. and params #93

Merged
merged 5 commits into from
Jul 31, 2019
Merged

Added non-default const. for AHRS pre-int. and params #93

merged 5 commits into from
Jul 31, 2019

Conversation

sandro-berchier
Copy link
Contributor

@sandro-berchier sandro-berchier commented Jul 28, 2019

Added non-default constructors for AHRS pre-integration from already pre-integrated measurements. This is helpful for using AHRS factors with already pre-integrated IMU measurements (manifold or tangent pre-integration).

Also added a non-default constructor for the parameters in the AHRS pre-integration (instead of having to use the trivial constructor and parsing the gyro covariance and coriolis term afterwards).

Also added a unit test for the two non-default constructors.


This change is Reviewable

Copy link
Member

@dellaert dellaert left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Awesome. Just a few small comments

@@ -35,6 +35,13 @@ struct GTSAM_EXPORT PreintegratedRotationParams {

PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {}

PreintegratedRotationParams(Matrix3 gyroscope_covariance,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

const Matrix3&

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done

biasHat_(bias_hat),
PreintegratedRotation(p, deltaTij, deltaRij, delRdelBiasOmega),
preintMeasCov_(preint_meas_cov) {
p_->gyroscopeCovariance = p->getGyroscopeCovariance();
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why ?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Redundant indeed, base class already copies params as p_(p)

double deltaTij = 0.02;
Matrix3 delRdelBiasOmega = Matrix3::Ones()*0.5;
Matrix3 preintMeasCov = Matrix3::Ones()*0.2;
gtsam::PreintegratedAhrsMeasurements test_pim(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

using gtsam and std in test files.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done

@@ -119,6 +119,35 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
}

//******************************************************************************
TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) {
// Linearization point
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

obsolete comment?

Copy link
Contributor

@ToniRV ToniRV Jul 30, 2019

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Removed, got copied from above test I think.

double deltaTij = 0.02;
Matrix3 delRdelBiasOmega = Matrix3::Ones()*0.5;
Matrix3 preintMeasCov = Matrix3::Ones()*0.2;
gtsam::PreintegratedAhrsMeasurements test_pim(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

test_pim -> actualPim, or just pim

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

changed to actualPim

@ToniRV
Copy link
Contributor

ToniRV commented Jul 30, 2019

Hi Frank! I'll take over this PR, Sandro has to take a flight soon 🚀
I have addressed the changes suggested.

@sandro-berchier
Copy link
Contributor Author

Thanks Toni ;)

Copy link
Member

@dellaert dellaert left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Excellent, will merge...

@dellaert dellaert merged commit 6cdb69a into borglab:develop Jul 31, 2019
varunagrawal added a commit that referenced this pull request Apr 20, 2021
b2144a712 Merge pull request #95 from borglab/feature/empty-str-default-arg
9f1e727d8 Merge pull request #96 from borglab/fix/cmake
97ee2ff0c fix CMake typo
64a599827 support empty strings as default args
7b14ed542 Merge pull request #94 from borglab/fix/cmake-messages
0978641fe clean up
5b9272557 Merge pull request #91 from borglab/feature/enums
56e6f48b3 Merge pull request #93 from borglab/feature/better-template
27cc7cebf better cmake messages
a6318b567 fix tests
b7f60463f remove export_values()
38304fe0a support for class nested enums
348160740 minor fixes
5b6d66a97 use cpp_class and correct module name
2f7ae0676 add newlines and formatting
6e7cecc50 remove support for enum value assignment
c1dc925a6 formatting
798732598 better pybind template
f6dad2959 pybind_wrapper fixes with formatting
7b4a06560 Merge branch 'master' into feature/enums
1982b7131 more comprehensive tests for enums
3a0eafd66 code for wrapping enums
398780982 tests for enum support

git-subtree-dir: wrap
git-subtree-split: b2144a712953dcc3e001c97c2ace791149c97278
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

3 participants