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

Fix unstable c++ examples

release/4.3a0
Frank Dellaert 2022-01-02 14:59:28 -05:00
parent 8dbe5ee179
commit 9518346161
4 changed files with 27 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

@ -372,15 +372,15 @@ public:
Matrix Z_3x3 = Z_3x3;
Matrix I_3x3 = I_3x3;
Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
Matrix H_pos_pos = numericalDerivative11<Vector, Vector>(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
Matrix H_pos_vel = numericalDerivative11<Vector, Vector>(std::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
Matrix H_pos_angles = Z_3x3;
Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(std::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);
Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(std::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);
Matrix H_vel_vel = numericalDerivative11<Vector, Vector>(std::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);
Matrix H_vel_angles = numericalDerivative11<Vector, Vector>(std::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);
Matrix H_vel_pos = Z_3x3;
Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles);
Matrix H_angles_angles = numericalDerivative11<Vector, Vector>(std::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles);
Matrix H_angles_pos = Z_3x3;
Matrix H_angles_vel = Z_3x3;

16
gtsam_unstable/slam/serialization.cpp
View File

@ -5,8 +5,6 @@
* @author Alex Cunningham
*/
#include <gtsam/base/deprecated/LieMatrix_Deprecated.h>
#include <gtsam/base/deprecated/LieVector_Deprecated.h>
#include <gtsam/slam/serialization.h>
#include <gtsam/base/serialization.h>
@ -31,8 +29,6 @@
using namespace gtsam;
// Creating as many permutations of factors as possible
typedef PriorFactor<LieVector> PriorFactorLieVector;
typedef PriorFactor<LieMatrix> PriorFactorLieMatrix;
typedef PriorFactor<Point2> PriorFactorPoint2;
typedef PriorFactor<StereoPoint2> PriorFactorStereoPoint2;
typedef PriorFactor<Point3> PriorFactorPoint3;
@ -46,8 +42,6 @@ typedef PriorFactor<CalibratedCamera> PriorFactorCalibratedCamera;
typedef PriorFactor<PinholeCameraCal3_S2> PriorFactorPinholeCameraCal3_S2;
typedef PriorFactor<StereoCamera> PriorFactorStereoCamera;
typedef BetweenFactor<LieVector> BetweenFactorLieVector;
typedef BetweenFactor<LieMatrix> BetweenFactorLieMatrix;
typedef BetweenFactor<Point2> BetweenFactorPoint2;
typedef BetweenFactor<Point3> BetweenFactorPoint3;
typedef BetweenFactor<Rot2> BetweenFactorRot2;
@ -55,8 +49,6 @@ typedef BetweenFactor<Rot3> BetweenFactorRot3;
typedef BetweenFactor<Pose2> BetweenFactorPose2;
typedef BetweenFactor<Pose3> BetweenFactorPose3;
typedef NonlinearEquality<LieVector> NonlinearEqualityLieVector;
typedef NonlinearEquality<LieMatrix> NonlinearEqualityLieMatrix;
typedef NonlinearEquality<Point2> NonlinearEqualityPoint2;
typedef NonlinearEquality<StereoPoint2> NonlinearEqualityStereoPoint2;
typedef NonlinearEquality<Point3> NonlinearEqualityPoint3;
@ -112,8 +104,6 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
/* Create GUIDs for geometry */
/* ************************************************************************* */
GTSAM_VALUE_EXPORT(gtsam::LieVector);
GTSAM_VALUE_EXPORT(gtsam::LieMatrix);
GTSAM_VALUE_EXPORT(gtsam::Point2);
GTSAM_VALUE_EXPORT(gtsam::StereoPoint2);
GTSAM_VALUE_EXPORT(gtsam::Point3);
@ -133,8 +123,6 @@ GTSAM_VALUE_EXPORT(gtsam::StereoCamera);
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
BOOST_CLASS_EXPORT_GUID(PriorFactorLieVector, "gtsam::PriorFactorLieVector");
BOOST_CLASS_EXPORT_GUID(PriorFactorLieMatrix, "gtsam::PriorFactorLieMatrix");
BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2");
BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2");
BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3");
@ -147,8 +135,6 @@ BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2");
BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera");
BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera");
BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector");
BOOST_CLASS_EXPORT_GUID(BetweenFactorLieMatrix, "gtsam::BetweenFactorLieMatrix");
BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2");
BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3");
BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2");
@ -156,8 +142,6 @@ BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3");
BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2");
BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieVector, "gtsam::NonlinearEqualityLieVector");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityLieMatrix, "gtsam::NonlinearEqualityLieMatrix");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2");
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3");

41
gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp
View File

@ -16,22 +16,23 @@
* @date Jan 17, 2012
*/
#include <gtsam_unstable/slam/GaussMarkov1stOrderFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/deprecated/LieVector.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam_unstable/slam/GaussMarkov1stOrderFactor.h>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
//! Factors
typedef GaussMarkov1stOrderFactor<LieVector> GaussMarkovFactor;
typedef GaussMarkov1stOrderFactor<Vector3> GaussMarkovFactor;
/* ************************************************************************* */
LieVector predictionError(const LieVector& v1, const LieVector& v2, const GaussMarkovFactor factor) {
Vector predictionError(const Vector& v1, const Vector& v2,
const GaussMarkovFactor factor) {
return factor.evaluateError(v1, v2);
}
@ -58,29 +59,29 @@ TEST( GaussMarkovFactor, error )
Key x1(1);
Key x2(2);
double delta_t = 0.10;
Vector tau = Vector3(100.0, 150.0, 10.0);
Vector3 tau(100.0, 150.0, 10.0);
SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0);
LieVector v1 = LieVector(Vector3(10.0, 12.0, 13.0));
LieVector v2 = LieVector(Vector3(10.0, 15.0, 14.0));
Vector3 v1(10.0, 12.0, 13.0);
Vector3 v2(10.0, 15.0, 14.0);
// Create two nodes
linPoint.insert(x1, v1);
linPoint.insert(x2, v2);
GaussMarkovFactor factor(x1, x2, delta_t, tau, model);
Vector Err1( factor.evaluateError(v1, v2) );
Vector3 error1 = factor.evaluateError(v1, v2);
// Manually calculate the error
Vector alpha(tau.size());
Vector alpha_v1(tau.size());
Vector3 alpha(tau.size());
Vector3 alpha_v1(tau.size());
for(int i=0; i<tau.size(); i++){
alpha(i) = exp(- 1/tau(i)*delta_t );
alpha_v1(i) = alpha(i) * v1(i);
}
Vector Err2( v2 - alpha_v1 );
Vector3 error2 = v2 - alpha_v1;
CHECK(assert_equal(Err1, Err2, 1e-9));
CHECK(assert_equal(error1, error2, 1e-8));
}
/* ************************************************************************* */
@ -90,14 +91,14 @@ TEST (GaussMarkovFactor, jacobian ) {
Key x1(1);
Key x2(2);
double delta_t = 0.10;
Vector tau = Vector3(100.0, 150.0, 10.0);
Vector3 tau(100.0, 150.0, 10.0);
SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0);
GaussMarkovFactor factor(x1, x2, delta_t, tau, model);
// Update the linearization point
LieVector v1_upd = LieVector(Vector3(0.5, -0.7, 0.3));
LieVector v2_upd = LieVector(Vector3(-0.7, 0.4, 0.9));
Vector3 v1_upd(0.5, -0.7, 0.3);
Vector3 v2_upd(-0.7, 0.4, 0.9);
// Calculate the Jacobian matrix using the factor
Matrix computed_H1, computed_H2;
@ -115,8 +116,8 @@ TEST (GaussMarkovFactor, jacobian ) {
v1_upd, v2_upd);
// Verify they are equal for this choice of state
CHECK( assert_equal(numerical_H1, computed_H1, 1e-9));
CHECK( assert_equal(numerical_H2, computed_H2, 1e-9));
CHECK(assert_equal(numerical_H1, computed_H1, 1e-9));
CHECK(assert_equal(numerical_H2, computed_H2, 1e-9));
}
/* ************************************************************************* */

2
gtsam_unstable/slam/tests/testSerialization.cpp
View File

@ -10,7 +10,7 @@
#include <CppUnitLite/TestHarness.h>
#include <iostream>
#include <gtsam/slam/serialization.h>
#include <gtsam_unstable/slam/serialization.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>