Skip to content

Commit 1263313

Browse files
Gold856Gold856
committed
Remove Matrix and Vector constants
1 parent f559a2e commit 1263313

File tree

185 files changed

+967
-976
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

185 files changed

+967
-976
lines changed

‎examples/AbcEquivariantFilterExample.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -313,7 +313,7 @@ void processDataWithEqF(AbcFilter& filter, const std::vector<Data>& data_list,
313313
// Calculate errors
314314
Vector3 att_error = Rot3::Logmap(data.xi.R.between(att_est));
315315
Vector3 bias_error = bias_est - data.xi.b;
316-
Vector3 cal_error = Z_3x1;
316+
Vector3 cal_error = Vector3::Zero();
317317
if (!data.xi.S.empty()) {
318318
cal_error = Rot3::Logmap(data.xi.S[0].between(cal_est));
319319
}
@@ -358,7 +358,7 @@ void processDataWithEqF(AbcFilter& filter, const std::vector<Data>& data_list,
358358
Vector3 final_att_error =
359359
Rot3::Logmap(final_data.xi.R.between(final_att_est));
360360
Vector3 final_bias_error = final_bias_est - final_data.xi.b;
361-
Vector3 final_cal_error = Z_3x1;
361+
Vector3 final_cal_error = Vector3::Zero();
362362
if (!final_data.xi.S.empty()) {
363363
final_cal_error = Rot3::Logmap(final_data.xi.S[0].between(final_cal_est));
364364
}

‎examples/CombinedImuFactorsExample.cpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -102,12 +102,12 @@ std::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
102102
double gyro_noise_sigma = 0.000205689024915;
103103
double accel_bias_rw_sigma = 0.004905;
104104
double gyro_bias_rw_sigma = 0.000001454441043;
105-
Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2);
106-
Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2);
105+
Matrix33 measured_acc_cov = Matrix3::Identity() * pow(accel_noise_sigma, 2);
106+
Matrix33 measured_omega_cov = Matrix3::Identity() * pow(gyro_noise_sigma, 2);
107107
Matrix33 integration_error_cov =
108-
I_3x3 * 1e-8; // error committed in integrating position from velocities
109-
Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
110-
Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
108+
Matrix3::Identity() * 1e-8; // error committed in integrating position from velocities
109+
Matrix33 bias_acc_cov = Matrix3::Identity() * pow(accel_bias_rw_sigma, 2);
110+
Matrix33 bias_omega_cov = Matrix3::Identity() * pow(gyro_bias_rw_sigma, 2);
111111

112112
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
113113
// PreintegrationBase params:
@@ -125,7 +125,7 @@ std::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
125125

126126
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
127127
Matrix66 bias_acc_omega_init =
128-
I_6x6 * 1e-5; // error in the bias used for preintegration
128+
Matrix6::Identity() * 1e-5; // error in the bias used for preintegration
129129
p->biasAccOmegaInt = bias_acc_omega_init;
130130
#endif
131131

‎examples/IEKF_SE2Example.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -59,7 +59,7 @@ int main() {
5959

6060
// Define the process covariance and measurement covariance matrices Q and R.
6161
Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal();
62-
Matrix2 R = I_2x2 * 0.01;
62+
Matrix2 R = Matrix2::Identity() * 0.01;
6363

6464
// Define odometry movements.
6565
// U1: Move 1 unit in X, 1 unit in Y, 0.5 radians in theta.

‎examples/IMUKittiExampleGPS.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -201,12 +201,12 @@ int main(int argc, char* argv[]) {
201201

202202
// Set IMU preintegration parameters
203203
Matrix33 measured_acc_cov =
204-
I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2);
204+
Matrix3::Identity() * pow(kitti_calibration.accelerometer_sigma, 2);
205205
Matrix33 measured_omega_cov =
206-
I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2);
206+
Matrix3::Identity() * pow(kitti_calibration.gyroscope_sigma, 2);
207207
// error committed in integrating position from velocities
208208
Matrix33 integration_error_cov =
209-
I_3x3 * pow(kitti_calibration.integration_sigma, 2);
209+
Matrix3::Identity() * pow(kitti_calibration.integration_sigma, 2);
210210

211211
auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g);
212212
imu_params->accelerometerCovariance =

‎examples/ImuFactorsExample.cpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -89,12 +89,12 @@ std::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
8989
double gyro_noise_sigma = 0.000205689024915;
9090
double accel_bias_rw_sigma = 0.004905;
9191
double gyro_bias_rw_sigma = 0.000001454441043;
92-
Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2);
93-
Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2);
92+
Matrix33 measured_acc_cov = Matrix3::Identity() * pow(accel_noise_sigma, 2);
93+
Matrix33 measured_omega_cov = Matrix3::Identity() * pow(gyro_noise_sigma, 2);
9494
Matrix33 integration_error_cov =
95-
I_3x3 * 1e-8; // error committed in integrating position from velocities
96-
Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
97-
Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
95+
Matrix3::Identity() * 1e-8; // error committed in integrating position from velocities
96+
Matrix33 bias_acc_cov = Matrix3::Identity() * pow(accel_bias_rw_sigma, 2);
97+
Matrix33 bias_omega_cov = Matrix3::Identity() * pow(gyro_bias_rw_sigma, 2);
9898

9999
auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
100100
// PreintegrationBase params:
@@ -111,7 +111,7 @@ std::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
111111
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
112112
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
113113
Matrix66 bias_acc_omega_init =
114-
I_6x6 * 1e-5; // error in the bias used for preintegration
114+
Matrix6::Identity() * 1e-5; // error in the bias used for preintegration
115115
p->biasAccOmegaInt = bias_acc_omega_init;
116116
#endif
117117

‎examples/ImuFactorsExample2.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -38,9 +38,9 @@ const double kGravity = 9.81;
3838
/* ************************************************************************* */
3939
int main(int argc, char* argv[]) {
4040
auto params = PreintegrationParams::MakeSharedU(kGravity);
41-
params->setAccelerometerCovariance(I_3x3 * 0.1);
42-
params->setGyroscopeCovariance(I_3x3 * 0.1);
43-
params->setIntegrationCovariance(I_3x3 * 0.1);
41+
params->setAccelerometerCovariance(Matrix3::Identity() * 0.1);
42+
params->setGyroscopeCovariance(Matrix3::Identity() * 0.1);
43+
params->setIntegrationCovariance(Matrix3::Identity() * 0.1);
4444
params->setUse2ndOrderCoriolis(false);
4545
params->setOmegaCoriolis(Vector3(0, 0, 0));
4646

‎examples/InverseKinematicsExampleExpressions.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@ inline Vector3 scalarMultiply(const double& s, const Vector3& v,
3434
OptionalJacobian<3, 1> Hs,
3535
OptionalJacobian<3, 3> Hv) {
3636
if (Hs) *Hs = v;
37-
if (Hv) *Hv = s * I_3x3;
37+
if (Hv) *Hv = s * Matrix3::Identity();
3838
return s * v;
3939
}
4040

‎examples/NavStateImuExample.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -49,9 +49,9 @@ int main(int argc, char* argv[]) {
4949

5050
// --- Preintegration/IMU parameters ---
5151
auto params = PreintegrationParams::MakeSharedU(kGravity);
52-
params->setAccelerometerCovariance(I_3x3 * 0.1);
53-
params->setGyroscopeCovariance(I_3x3 * 0.1);
54-
params->setIntegrationCovariance(I_3x3 * 0.1);
52+
params->setAccelerometerCovariance(Matrix3::Identity() * 0.1);
53+
params->setGyroscopeCovariance(Matrix3::Identity() * 0.1);
54+
params->setIntegrationCovariance(Matrix3::Identity() * 0.1);
5555
params->setUse2ndOrderCoriolis(false);
5656
params->setOmegaCoriolis(Vector3::Zero());
5757

@@ -66,7 +66,7 @@ int main(int argc, char* argv[]) {
6666
// --- Initialize EKF with ground truth
6767
const double t0 = 0.0;
6868
NavState X0 = scenario.navState(t0);
69-
Matrix9 P0 = I_9x9 * 1e-2; // modest initial uncertainty
69+
Matrix9 P0 = Matrix9::Identity() * 1e-2; // modest initial uncertainty
7070
NavStateImuEKF ekf(X0, P0, params);
7171

7272
// --- Run for N steps and compare predictions ---

‎gtsam/base/Matrix.h‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -52,8 +52,6 @@ using Matrix6##N = Eigen::Matrix<double, 6, N>; \
5252
using Matrix7##N = Eigen::Matrix<double, 7, N>; \
5353
using Matrix8##N = Eigen::Matrix<double, 8, N>; \
5454
using Matrix9##N = Eigen::Matrix<double, 9, N>; \
55-
static const Eigen::MatrixBase<Matrix##N>::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \
56-
static const Eigen::MatrixBase<Matrix##N>::ConstantReturnType Z_##N##x##N = Matrix##N::Constant(0.0);
5755

5856
GTSAM_MAKE_MATRIX_DEFS(1)
5957
GTSAM_MAKE_MATRIX_DEFS(2)

‎gtsam/base/Vector.h‎

Lines changed: 5 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -42,22 +42,12 @@ typedef Eigen::VectorXd Vector;
4242
typedef Eigen::Matrix<double, 1, 1> Vector1;
4343
typedef Eigen::Vector2d Vector2;
4444
typedef Eigen::Vector3d Vector3;
45-
46-
static const Eigen::MatrixBase<Vector2>::ConstantReturnType Z_2x1 = Vector2::Constant(0.0);
47-
static const Eigen::MatrixBase<Vector3>::ConstantReturnType Z_3x1 = Vector3::Constant(0.0);
48-
49-
// Create handy typedefs and constants for vectors with N>3
50-
// VectorN and Z_Nx1, for N=1..9
51-
#define GTSAM_MAKE_VECTOR_DEFS(N) \
52-
using Vector##N = Eigen::Matrix<double, N, 1>; \
53-
static const Eigen::MatrixBase<Vector##N>::ConstantReturnType Z_##N##x1 = Vector##N::Constant(0.0);
54-
55-
GTSAM_MAKE_VECTOR_DEFS(4)
56-
GTSAM_MAKE_VECTOR_DEFS(5)
57-
GTSAM_MAKE_VECTOR_DEFS(6)
58-
GTSAM_MAKE_VECTOR_DEFS(7)
45+
using Vector4 = Eigen::Matrix<double, 4, 1>; \
46+
using Vector5 = Eigen::Matrix<double, 5, 1>; \
47+
using Vector6 = Eigen::Matrix<double, 6, 1>; \
48+
using Vector7 = Eigen::Matrix<double, 7, 1>; \
5949
using Vector8 = Eigen::Matrix<double, 8, 1>; \
60-
GTSAM_MAKE_VECTOR_DEFS(9)
50+
using Vector9 = Eigen::Matrix<double, 9, 1>; \
6151
using Vector10 = Eigen::Matrix<double, 10, 1>; \
6252
using Vector11 = Eigen::Matrix<double, 11, 1>; \
6353
using Vector12 = Eigen::Matrix<double, 12, 1>; \

0 commit comments

Comments
 (0)