Wrapper fixes

[varunagrawal]

• Wrapped full version of ISAM2's update.
• Wrapped the Measure function in BearingRange.

[ProfFan]

This will break MATLAB, please refer to the other PRs for correct way to wrap

[varunagrawal]

Can you please point me to those PRs?

[ProfFan]

Sure, it's #511

[ProfFan]

Can't use This?

[varunagrawal]

Is that This with a capital T?

[ProfFan]

Yes

[varunagrawal]

Nvm got it. Thanks Fan!!

[varunagrawal]

@ProfFan so there is a bug in matlab_wrapper.py where if a class has more than one template parameters, then it doesn't add the templates to the return type (L213). I'm almost done fixing it.

[ProfFan]

@varunagrawal ./update_wrap.sh after merging wrap PR

[ProfFan]

LieVector is still used in unstable, can you take a look and possibly fix them?

[varunagrawal]

That's gonna be a lot of effort, for which I don't currently have the bandwidth. :/

[dellaert]

That's gonna be a lot of effort, for which I don't currently have the bandwidth. :/

I’m not sure it’s a lot of effort, as with type traits matrix and vector should just work as drop-in replacements, I think?

[varunagrawal]

Okay, update done.

[ProfFan]

unstable_examples/+imuSimulator/coriolisExample.m
178:            currentVelocityEstimate = LieVector(currentVelocityRotatingGT);
180:            currentVelocityEstimate = LieVector(currentVelocityFixedGT);
189:        newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityEstimate, sigma_init_v));

unstable_examples/+imuSimulator/IMUComparison_with_cov.m
46:initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal));
53:initialFactors.add(PriorFactorLieVector(symbol('v',0), ...
54:    LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, sigma_init_v)));
94:          initialVel = LieVector(velocity);

unstable_examples/+imuSimulator/IMUComparison.m
54:initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal));
59:initialFactors.add(PriorFactorLieVector(symbol('v',0), ...
60:    LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, 1.0)));
99:          initialVel = LieVector(velocity);

unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
85:      values.insert(currentVelKey, LieVector(currentVel));

unstable_examples/IMUKittiExampleVO.m
49:currentVelocityGlobal = LieVector([0;0;0]); % the vehicle is stationary at the beginning
91:    newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));

unstable_examples/FlightCameraTransformIMU.m
170:        % fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));

unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
30:      graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel));

[dellaert]

Awesome! So, no more LieXXX anywhere ? In .py, .m, .h, .cpp?

[ProfFan]

Some are under gtsam_unstable:

slam/tests/testBetweenFactorEM.cpp
27:LieVector predictionError(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactorEM<gtsam::Pose2>& factor){
31:  //  LieVector err = factor.whitenedError(values);
33:  return LieVector::Expmap(factor.whitenedError(values));
37:LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& key1, const gtsam::Key& key2, const BetweenFactor<gtsam::Pose2>& factor){
41:  //  LieVector err = factor.whitenedError(values);
43:  return LieVector::Expmap(factor.whitenedError(values));
193:  Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, _1, p2, key1, key2, f), p1, stepsize);
194:  Matrix H2_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, p1, _1, key1, key2, f), p2, stepsize);
198:  Matrix H1_expected_stnd = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError_standard, _1, p2, key1, key2, h), p1, stepsize);

slam/tests/testGaussMarkov1stOrderFactor.cpp
24:#include <gtsam/base/deprecated/LieVector.h>
30:typedef GaussMarkov1stOrderFactor<LieVector> GaussMarkovFactor;
33:LieVector predictionError(const LieVector& v1, const LieVector& v2, const GaussMarkovFactor factor) {
63:  LieVector v1 = LieVector(Vector3(10.0, 12.0, 13.0));
64:  LieVector v2 = LieVector(Vector3(10.0, 15.0, 14.0));
98:  LieVector v1_upd = LieVector(Vector3(0.5, -0.7, 0.3));
99:  LieVector v2_upd = LieVector(Vector3(-0.7, 0.4, 0.9));

slam/EquivInertialNavFactor_GlobalVel_NoBias.h
373:    Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
374:    Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
377:    Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0);
378:    Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles);
381:    Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles);

slam/serialization.cpp
9:#include <gtsam/base/deprecated/LieVector_Deprecated.h>
34:typedef PriorFactor<LieVector>            PriorFactorLieVector;
50:typedef BetweenFactor<LieVector>       BetweenFactorLieVector;
59:typedef NonlinearEquality<LieVector>              NonlinearEqualityLieVector;
119:GTSAM_VALUE_EXPORT(gtsam::LieVector);
141:BOOST_CLASS_EXPORT_GUID(PriorFactorLieVector, "gtsam::PriorFactorLieVector");
156:BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector");
165:BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieVector, "gtsam::NonlinearEqualityLieVector");

gtsam_unstable.i
9:class gtsam::LieVector;

docs needs to be regenerated as well, but I don't know how to do that?

[varunagrawal]

I was going to save that for a follow up PR rather than muddle stuff here.