Skip to content

Commit 0b7b33a

Browse files
dellaertdellaert
authored
Merge pull request borglab#2217 from borglab/fix/nd_issue
Fix all compile warnings on ubuntu_latest
2 parents eb2425e + 10be8f0 commit 0b7b33a

File tree

10 files changed

+69
-77
lines changed

10 files changed

+69
-77
lines changed

‎.github/workflows/vcpkg.yml‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -16,15 +16,15 @@ jobs:
1616
fail-fast: false
1717
matrix:
1818
include:
19-
# - os: windows-latest
20-
# triplet: x64-windows-release
21-
# build_type: Release
22-
# test_target: RUN_TESTS
23-
# binary_cache: C:\Users\runneradmin\AppData\Local\vcpkg\archives
24-
# python: python
25-
# # check_constraint_program fail on windows. should fix it for windows.
26-
# # Remove this excluded test when you fix it for windows.
27-
# ctest_extra_flags: -E check_constraint_program
19+
# - os: windows-latest
20+
# triplet: x64-windows-release
21+
# build_type: Release
22+
# test_target: RUN_TESTS
23+
# binary_cache: C:\Users\runneradmin\AppData\Local\vcpkg\archives
24+
# python: python
25+
# # check_constraint_program fail on windows. should fix it for windows.
26+
# # Remove this excluded test when you fix it for windows.
27+
# ctest_extra_flags: -E check_constraint_program
2828
- os: ubuntu-latest
2929
triplet: x64-linux-release
3030
build_type: Release

‎gtsam/basis/tests/testFourier.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -169,7 +169,7 @@ TEST(Basis, Derivative7) {
169169

170170
// Calculate expected values by numerical derivative of proxy.
171171
const double x = 0.2;
172-
Matrix numeric_dTdx = numericalDerivative11<double, double>(proxy, x);
172+
Matrix1 numeric_dTdx = numericalDerivative11<double, double>(proxy, x);
173173

174174
// Calculate derivatives at Chebyshev points using D7, interpolate
175175
Matrix D7 = FourierBasis::DifferentiationMatrix(7);

‎gtsam/geometry/Unit3.cpp‎

Lines changed: 4 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -195,16 +195,6 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1, 2> H_p,
195195
return d;
196196
}
197197

198-
/* ************************************************************************* */
199-
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2, 2> H_q) const {
200-
// 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
201-
const Vector2 xi = basis().transpose() * q.p_;
202-
if (H_q) {
203-
*H_q = basis().transpose() * q.basis();
204-
}
205-
return xi;
206-
}
207-
208198
/* ************************************************************************* */
209199
Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p,
210200
OptionalJacobian<2, 2> H_q) const {
@@ -222,20 +212,14 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p,
222212
const Matrix32& H_b1_p = H_B_p.block<3, 2>(0, 0);
223213
const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0);
224214

225-
// Derivatives of the two entries of xi wrt the basis vectors.
226-
const Matrix13 H_xi1_b1 = qn.transpose();
227-
const Matrix13 H_xi2_b2 = qn.transpose();
228-
215+
// dxi/dB = qn.transpose();
229216
// Assemble dxi/dp = dxi/dB * dB/dp.
230-
const Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p;
231-
const Matrix12 H_xi2_p = H_xi2_b2 * H_b2_p;
232-
*H_p << H_xi1_p, H_xi2_p;
217+
*H_p << qn.transpose() * H_b1_p, qn.transpose() * H_b2_p;
233218
}
234219

235220
if (H_q) {
236221
// dxi/dq is given by dxi/dqu * dqu/dq, where qu is the unit vector of q.
237-
const Matrix23 H_xi_qu = Bt;
238-
*H_q = H_xi_qu * H_qn_q;
222+
*H_q = Bt * H_qn_q;
239223
}
240224

241225
return xi;
@@ -244,7 +228,7 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p,
244228
/* ************************************************************************* */
245229
double Unit3::distance(const Unit3& q, OptionalJacobian<1, 2> H) const {
246230
Matrix2 H_xi_q;
247-
const Vector2 xi = error(q, H ? &H_xi_q : nullptr);
231+
const Vector2 xi = errorVector(q, {}, H ? &H_xi_q : nullptr);
248232
const double theta = xi.norm();
249233
if (H)
250234
*H = (xi.transpose() / theta) * H_xi_q;

‎gtsam/geometry/Unit3.h‎

Lines changed: 16 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -73,16 +73,17 @@ class GTSAM_EXPORT Unit3 {
7373
/// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point
7474
explicit Unit3(const Point2& p, double f);
7575

76-
/// Copy constructor
77-
Unit3(const Unit3& u) {
78-
p_ = u.p_;
79-
}
76+
/// Copy constructor: copies essential data and discards caches.
77+
Unit3(const Unit3& u) : p_(u.p_) {}
8078

81-
/// Copy assignment
82-
Unit3& operator=(const Unit3 & u) {
79+
/// Copy assignment: copies essential data and invalidates local caches.
80+
Unit3& operator=(const Unit3& u) {
81+
if (this == &u) return *this;
8382
p_ = u.p_;
84-
B_ = u.B_;
85-
H_B_ = u.H_B_;
83+
84+
// Since p_ has changed, the old cached basis is no longer valid.
85+
B_.reset();
86+
H_B_.reset();
8687
return *this;
8788
}
8889

@@ -144,10 +145,6 @@ class GTSAM_EXPORT Unit3 {
144145
double dot(const Unit3& q, OptionalJacobian<1,2> H1 = {}, //
145146
OptionalJacobian<1,2> H2 = {}) const;
146147

147-
/// Signed, vector-valued error between two directions
148-
/// @deprecated, errorVector has the proper derivatives, this confusingly has only the second.
149-
Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = {}) const;
150-
151148
/// Signed, vector-valued error between two directions
152149
/// NOTE(hayk): This method has zero derivatives if this (p) and q are orthogonal.
153150
Vector2 errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p = {}, //
@@ -167,7 +164,6 @@ class GTSAM_EXPORT Unit3 {
167164
}
168165

169166
/// @}
170-
171167
/// @name Manifold
172168
/// @{
173169

@@ -194,6 +190,13 @@ class GTSAM_EXPORT Unit3 {
194190

195191
/// @}
196192

193+
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
194+
/// @deprecated, errorVector has the proper derivatives, this confusingly has only the second.
195+
Vector2 error(const Unit3& q, OptionalJacobian<2, 2> H_q = {}) const {
196+
return errorVector(q, {}, H_q);
197+
}
198+
#endif
199+
197200
private:
198201

199202
/// @name Advanced Interface

‎gtsam/geometry/tests/testUnit3.cpp‎

Lines changed: 21 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -64,7 +64,7 @@ static Unit3 rotate_(const Rot3& R, const Unit3& p) {
6464
return R * p;
6565
}
6666

67-
TEST(Unit3, rotate) {
67+
TEST(Unit3, Rotate) {
6868
Rot3 R = Rot3::Yaw(0.5);
6969
Unit3 p(1, 0, 0);
7070
Unit3 expected = Unit3(R.matrix().col(0));
@@ -89,7 +89,7 @@ static Unit3 unrotate_(const Rot3& R, const Unit3& p) {
8989
return R.unrotate(p);
9090
}
9191

92-
TEST(Unit3, unrotate) {
92+
TEST(Unit3, Unrotate) {
9393
Rot3 R = Rot3::Yaw(-M_PI / 4.0);
9494
Unit3 p(1, 0, 0);
9595
Unit3 expected = Unit3(1, 1, 0);
@@ -110,7 +110,7 @@ TEST(Unit3, unrotate) {
110110
}
111111
}
112112

113-
TEST(Unit3, dot) {
113+
TEST(Unit3, Dot) {
114114
Unit3 p(1, 0.2, 0.3);
115115
Unit3 q = p.retract(Vector2(0.5, 0));
116116
Unit3 r = p.retract(Vector2(0.8, 0));
@@ -143,31 +143,31 @@ TEST(Unit3, dot) {
143143
}
144144

145145
//*******************************************************************************
146-
TEST(Unit3, error) {
146+
TEST(Unit3, ErrorVector) {
147147
Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
148148
r = p.retract(Vector2(0.8, 0));
149-
EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-5));
150-
EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5));
151-
EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5));
149+
EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.errorVector(p), 1e-5));
150+
EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.errorVector(q), 1e-5));
151+
EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.errorVector(r), 1e-5));
152152

153153
Matrix actual, expected;
154154
// Use numerical derivatives to calculate the expected Jacobian
155155
{
156156
expected = numericalDerivative11<Vector2,Unit3>(
157-
std::bind(&Unit3::error, &p, std::placeholders::_1, nullptr), q);
158-
p.error(q, actual);
157+
std::bind(&Unit3::errorVector, &p, std::placeholders::_1, nullptr, nullptr), q);
158+
p.errorVector(q, {}, actual);
159159
EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
160160
}
161161
{
162162
expected = numericalDerivative11<Vector2,Unit3>(
163-
std::bind(&Unit3::error, &p, std::placeholders::_1, nullptr), r);
164-
p.error(r, actual);
163+
std::bind(&Unit3::errorVector, &p, std::placeholders::_1, nullptr, nullptr), r);
164+
p.errorVector(r, {}, actual);
165165
EXPECT(assert_equal(expected.transpose(), actual, 1e-5));
166166
}
167167
}
168168

169169
//*******************************************************************************
170-
TEST(Unit3, error2) {
170+
TEST(Unit3, ErrorVector2) {
171171
Unit3 p(0.1, -0.2, 0.8);
172172
Unit3 q = p.retract(Vector2(0.2, -0.1));
173173
Unit3 r = p.retract(Vector2(0.8, 0));
@@ -214,7 +214,7 @@ TEST(Unit3, error2) {
214214
}
215215

216216
//*******************************************************************************
217-
TEST(Unit3, distance) {
217+
TEST(Unit3, Distance) {
218218
Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
219219
r = p.retract(Vector2(0.8, 0));
220220
EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-5);
@@ -238,7 +238,7 @@ TEST(Unit3, distance) {
238238
}
239239

240240
//*******************************************************************************
241-
TEST(Unit3, localCoordinates0) {
241+
TEST(Unit3, LocalCoordinates0) {
242242
Unit3 p;
243243
Vector actual = p.localCoordinates(p);
244244
EXPECT(assert_equal(Z_2x1, actual, 1e-5));
@@ -342,7 +342,7 @@ TEST(Unit3, basis) {
342342

343343
//*******************************************************************************
344344
/// Check the basis derivatives of a bunch of random Unit3s.
345-
TEST(Unit3, basis_derivatives) {
345+
TEST(Unit3, BasisDerivatives) {
346346
int num_tests = 100;
347347
std::mt19937 rng(42);
348348
for (int i = 0; i < num_tests; i++) {
@@ -358,7 +358,7 @@ TEST(Unit3, basis_derivatives) {
358358
}
359359

360360
//*******************************************************************************
361-
TEST(Unit3, retract) {
361+
TEST(Unit3, Retract) {
362362
{
363363
Unit3 p;
364364
Vector2 v(0.5, 0);
@@ -377,7 +377,7 @@ TEST(Unit3, retract) {
377377
}
378378

379379
//*******************************************************************************
380-
TEST (Unit3, jacobian_retract) {
380+
TEST (Unit3, JacobianRetract) {
381381
Matrix22 H;
382382
Unit3 p;
383383
std::function<Unit3(const Vector2&)> f =
@@ -397,7 +397,7 @@ TEST (Unit3, jacobian_retract) {
397397
}
398398

399399
//*******************************************************************************
400-
TEST(Unit3, retract_expmap) {
400+
TEST(Unit3, RetractExpmap) {
401401
Unit3 p;
402402
Vector2 v((M_PI / 2.0), 0);
403403
Unit3 expected(Point3(0, 0, 1));
@@ -419,7 +419,7 @@ TEST(Unit3, Random) {
419419

420420
//*******************************************************************************
421421
// New test that uses Unit3::Random
422-
TEST(Unit3, localCoordinates_retract) {
422+
TEST(Unit3, LocalCoordinatesRetract) {
423423
std::mt19937 rng(42);
424424
size_t numIterations = 10000;
425425

@@ -492,10 +492,10 @@ TEST(Unit3, ErrorBetweenFactor) {
492492
TEST(Unit3, CopyAssign) {
493493
Unit3 p{1, 0.2, 0.3};
494494

495-
EXPECT(p.error(p).isZero());
495+
EXPECT(p.errorVector(p).isZero());
496496

497497
p = Unit3{-1, 2, 8};
498-
EXPECT(p.error(p).isZero());
498+
EXPECT(p.errorVector(p).isZero());
499499
}
500500

501501
/* ************************************************************************* */

‎gtsam/inference/Conditional.h‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -86,9 +86,11 @@ namespace gtsam {
8686
size_t size() const { return std::distance(range_.first, range_.second); }
8787
const auto& front() const { return *begin(); }
8888
// == operator overload for comparison with another iterator
89-
template<class OTHER>
89+
template <class OTHER>
9090
bool operator==(const OTHER& rhs) const {
91-
return std::equal(begin(), end(), rhs.begin());
91+
if (this->size() != rhs.size()) return false;
92+
if (this->size() == 0) return true;
93+
return std::equal(begin(), end(), rhs.begin(), rhs.end());
9294
}
9395
};
9496

‎gtsam/navigation/AttitudeFactor.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -29,13 +29,13 @@ Vector AttitudeFactor::attitudeError(const Rot3& nRb,
2929
Matrix23 D_nRotated_R;
3030
Matrix22 D_e_nRotated;
3131
Unit3 nRotated = nRb.rotate(bMeasured_, D_nRotated_R);
32-
Vector e = nRef_.error(nRotated, D_e_nRotated);
32+
Vector e = nRef_.errorVector(nRotated, {}, D_e_nRotated);
3333

3434
(*H) = D_e_nRotated * D_nRotated_R;
3535
return e;
3636
} else {
3737
Unit3 nRotated = nRb * bMeasured_;
38-
return nRef_.error(nRotated);
38+
return nRef_.errorVector(nRotated);
3939
}
4040
}
4141

‎gtsam/slam/EssentialMatrixFactor.h‎

Lines changed: 10 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -97,7 +97,7 @@ class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
9797
Vector evaluateError(const EssentialMatrix& E,
9898
OptionalMatrixType H) const override {
9999
Vector error(1);
100-
error << E.error(vA_, vB_, H);
100+
error(0) = E.error(vA_, vB_, H);
101101
return error;
102102
}
103103

@@ -404,12 +404,13 @@ class EssentialMatrixFactor4
404404
// H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK
405405
// where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK
406406
// and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]]
407-
*HK = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
408-
vA.transpose() * E.matrix().leftCols<2>() * cB_H_K;
407+
Matrix DynamicH_K = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
408+
vA.transpose() * E.matrix().leftCols<2>() * cB_H_K; // (1*2) * (2*DimK)
409+
*HK = DynamicH_K;
409410
}
410411

411412
Vector error(1);
412-
error << E.error(vA, vB, HE);
413+
error(0) = E.error(vA, vB, HE);
413414

414415
return error;
415416
}
@@ -504,16 +505,18 @@ class EssentialMatrixFactor5
504505

505506
if (HKa) {
506507
// Compute the jacobian of error w.r.t Ka.
507-
*HKa = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_Ka;
508+
Matrix DynamicHka = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_Ka;
509+
*HKa = DynamicHka;
508510
}
509511

510512
if (HKb) {
511513
// Compute the jacobian of error w.r.t Kb.
512-
*HKb = vA.transpose() * E.matrix().leftCols<2>() * cB_H_Kb;
514+
Matrix DynamicHkb = vA.transpose() * E.matrix().leftCols<2>() * cB_H_Kb;
515+
*HKb = DynamicHkb;
513516
}
514517

515518
Vector error(1);
516-
error << E.error(vA, vB, HE);
519+
error(0) = E.error(vA, vB, HE);
517520

518521
return error;
519522
}

‎gtsam/slam/OrientedPlane3Factor.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -68,7 +68,7 @@ Vector OrientedPlane3DirectionPrior::evaluateError(
6868
Unit3 n_hat_p = measured_p_.normal();
6969
Unit3 n_hat_q = plane.normal();
7070
Matrix2 H_p;
71-
Vector e = n_hat_p.error(n_hat_q, H ? &H_p : nullptr);
71+
Vector e = n_hat_p.errorVector(n_hat_q, {}, H ? &H_p : nullptr);
7272
if (H) {
7373
H->resize(2, 3);
7474
*H << H_p, Z_2x1;

‎gtsam/slam/RotateFactor.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -109,7 +109,7 @@ class RotateDirectionsFactor: public NoiseModelFactorN<Rot3> {
109109
/// vector of errors returns 2D vector
110110
Vector evaluateError(const Rot3& iRc, OptionalMatrixType H) const override {
111111
Unit3 i_q = iRc * c_z_;
112-
Vector error = i_p_.error(i_q, H);
112+
Vector error = i_p_.errorVector(i_q, {}, H);
113113
if (H) {
114114
Matrix DR;
115115
iRc.rotate(c_z_, DR);

0 commit comments

Comments
 (0)