Skip to content

Commit 7aecefe

Browse files
jakemclaughlin6jakemclaughlin6
committed
Format
1 parent 995244d commit 7aecefe

16 files changed

Lines changed: 116 additions & 126 deletions

‎vesta_constraints/include/vesta_constraints/common/qr_marginalizer.h‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
#pragma once
22

3-
#include <vesta_constraints/common/marginalizer.h>
43
#include <vesta_constraints/common/marginalize_variables.h>
4+
#include <vesta_constraints/common/marginalizer.h>
55
#include <vesta_constraints/common/uuid_ordering.h>
66
#include <vesta_core/graph.h>
77
#include <vesta_core/transaction.h>
@@ -48,8 +48,7 @@ class QRMarginalizer : public Marginalizer
4848
*/
4949
vesta_core::Transaction marginalize(const std::string& source,
5050
const std::vector<vesta_core::UUID>& marginalized_variables,
51-
const vesta_core::Graph& graph,
52-
const UuidOrdering& elimination_order);
51+
const vesta_core::Graph& graph, const UuidOrdering& elimination_order);
5352

5453
private:
5554
bool use_fej_;

‎vesta_constraints/include/vesta_constraints/vision/nullspace_projection_constraint.h‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -125,9 +125,9 @@ class NullspaceProjectionConstraint : public vesta_core::Constraint
125125
void serialize(Archive& archive, const unsigned int /* version */)
126126
{
127127
archive& boost::serialization::base_object<vesta_core::Constraint>(*this);
128-
archive& observations_;
129-
archive& sqrt_information_;
130-
archive& calibration_;
128+
archive & observations_;
129+
archive & sqrt_information_;
130+
archive & calibration_;
131131
}
132132
};
133133

‎vesta_constraints/include/vesta_constraints/vision/nullspace_projection_cost_function.h‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -110,9 +110,9 @@ class NullspaceProjectionCostFunction : public ceres::CostFunction
110110
}
111111

112112
// Compute per-observation reprojection errors and Jacobians
113-
Eigen::VectorXd b(2 * n); // stacked weighted residuals
114-
Eigen::MatrixXd E(2 * n, 3); // stacked landmark Jacobians
115-
std::vector<Eigen::Matrix<double, 2, 3>> F_pos(n); // pose position Jacobians
113+
Eigen::VectorXd b(2 * n); // stacked weighted residuals
114+
Eigen::MatrixXd E(2 * n, 3); // stacked landmark Jacobians
115+
std::vector<Eigen::Matrix<double, 2, 3>> F_pos(n); // pose position Jacobians
116116
std::vector<Eigen::Matrix<double, 2, 4, Eigen::RowMajor>> F_ori(n); // pose orientation Jacobians (ambient)
117117

118118
for (int i = 0; i < n; ++i)

‎vesta_constraints/include/vesta_constraints/vision/stereo_nullspace_projection_constraint.h‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -127,9 +127,9 @@ class StereoNullspaceProjectionConstraint : public vesta_core::Constraint
127127
void serialize(Archive& archive, const unsigned int /* version */)
128128
{
129129
archive& boost::serialization::base_object<vesta_core::Constraint>(*this);
130-
archive& observations_;
131-
archive& sqrt_information_;
132-
archive& calibration_;
130+
archive & observations_;
131+
archive & sqrt_information_;
132+
archive & calibration_;
133133
}
134134
};
135135

‎vesta_constraints/include/vesta_constraints/vision/stereo_nullspace_projection_cost_function.h‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -133,9 +133,9 @@ class StereoNullspaceProjectionCostFunction : public ceres::CostFunction
133133
}
134134

135135
// Compute per-observation stereo reprojection errors and Jacobians
136-
Eigen::VectorXd b(4 * n); // stacked weighted residuals
137-
Eigen::MatrixXd E(4 * n, 3); // stacked landmark Jacobians
138-
std::vector<Eigen::Matrix<double, 4, 3>> F_pos(n); // pose position Jacobians
136+
Eigen::VectorXd b(4 * n); // stacked weighted residuals
137+
Eigen::MatrixXd E(4 * n, 3); // stacked landmark Jacobians
138+
std::vector<Eigen::Matrix<double, 4, 3>> F_pos(n); // pose position Jacobians
139139
std::vector<Eigen::Matrix<double, 4, 4, Eigen::RowMajor>> F_ori(n); // pose orientation Jacobians
140140

141141
for (int i = 0; i < n; ++i)

‎vesta_constraints/src/common/qr_marginalizer.cpp‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
1-
#include <vesta_constraints/common/qr_marginalizer.h>
21
#include <vesta_constraints/common/marginal_constraint.h>
32
#include <vesta_constraints/common/marginalize_variables.h>
3+
#include <vesta_constraints/common/qr_marginalizer.h>
44
#include <vesta_constraints/common/uuid_ordering.h>
55
#include <vesta_core/graph.h>
66
#include <vesta_core/uuid.h>
@@ -96,5 +96,4 @@ vesta_core::Transaction QRMarginalizer::marginalize(const std::string& source,
9696
return transaction;
9797
}
9898

99-
10099
} // namespace vesta_constraints

‎vesta_constraints/src/vision/convert_to_nullspace.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -93,7 +93,7 @@ vesta_core::Transaction convertToNullspaceConstraints(const std::string& source,
9393

9494
// Create the nullspace projection constraint
9595
auto nullspace_constraint = NullspaceProjectionConstraint::make_shared(source, positions, orientations,
96-
*calibration_ptr, observations, covariance);
96+
*calibration_ptr, observations, covariance);
9797

9898
transaction.addConstraint(nullspace_constraint);
9999

‎vesta_constraints/src/vision/nullspace_projection_constraint.cpp‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -10,9 +10,8 @@
1010
namespace
1111
{
1212

13-
std::vector<vesta_core::UUID> buildVariableUuids(
14-
const std::vector<vesta_variables::Position3DStamped>& positions,
15-
const std::vector<vesta_variables::Orientation3DStamped>& orientations)
13+
std::vector<vesta_core::UUID> buildVariableUuids(const std::vector<vesta_variables::Position3DStamped>& positions,
14+
const std::vector<vesta_variables::Orientation3DStamped>& orientations)
1615
{
1716
std::vector<vesta_core::UUID> uuids;
1817
uuids.reserve(2 * positions.size());

‎vesta_constraints/src/vision/stereo_nullspace_projection_constraint.cpp‎

Lines changed: 6 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -10,9 +10,8 @@
1010
namespace
1111
{
1212

13-
std::vector<vesta_core::UUID> buildVariableUuids(
14-
const std::vector<vesta_variables::Position3DStamped>& positions,
15-
const std::vector<vesta_variables::Orientation3DStamped>& orientations)
13+
std::vector<vesta_core::UUID> buildVariableUuids(const std::vector<vesta_variables::Position3DStamped>& positions,
14+
const std::vector<vesta_variables::Orientation3DStamped>& orientations)
1615
{
1716
std::vector<vesta_core::UUID> uuids;
1817
uuids.reserve(2 * positions.size());
@@ -47,11 +46,10 @@ StereoNullspaceProjectionConstraint::StereoNullspaceProjectionConstraint(
4746
const std::vector<vesta_variables::Orientation3DStamped>& orientations,
4847
const vesta_variables::StereoCamera& calibration, const std::vector<Eigen::Vector4d>& observations,
4948
const vesta_core::Matrix4d& covariance)
50-
: StereoNullspaceProjectionConstraint(
51-
source, buildVariableUuids(positions, orientations), observations, covariance,
52-
(Eigen::Matrix<double, 5, 1>() << calibration.data()[0], calibration.data()[1], calibration.data()[2],
53-
calibration.data()[3], calibration.data()[4])
54-
.finished())
49+
: StereoNullspaceProjectionConstraint(source, buildVariableUuids(positions, orientations), observations, covariance,
50+
(Eigen::Matrix<double, 5, 1>() << calibration.data()[0], calibration.data()[1],
51+
calibration.data()[2], calibration.data()[3], calibration.data()[4])
52+
.finished())
5553
{
5654
assert(positions.size() == orientations.size());
5755
assert(positions.size() == observations.size());

‎vesta_constraints/test/test_marginalizer.cpp‎

Lines changed: 10 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -97,12 +97,11 @@ TEST(QRMarginalizer, MatchesFreeFunctions)
9797
tg2.graph.optimize();
9898

9999
// Marginalize x1 using free function
100-
auto transaction_free =
101-
vesta_constraints::marginalizeVariables("test", {tg1.x1->uuid()}, tg1.graph);
100+
auto transaction_free = vesta_constraints::marginalizeVariables("test", { tg1.x1->uuid() }, tg1.graph);
102101

103102
// Marginalize x1 using QRMarginalizer
104103
vesta_constraints::QRMarginalizer marginalizer(false);
105-
auto transaction_class = marginalizer.marginalize("test", {tg2.x1->uuid()}, tg2.graph);
104+
auto transaction_class = marginalizer.marginalize("test", { tg2.x1->uuid() }, tg2.graph);
106105

107106
// Both should produce the same structure
108107
auto free_removed_vars_range = transaction_free.removedVariables();
@@ -119,8 +118,7 @@ TEST(QRMarginalizer, MatchesFreeFunctions)
119118

120119
auto free_added = transaction_free.addedConstraints();
121120
auto class_added = transaction_class.addedConstraints();
122-
EXPECT_EQ(std::distance(free_added.begin(), free_added.end()),
123-
std::distance(class_added.begin(), class_added.end()));
121+
EXPECT_EQ(std::distance(free_added.begin(), free_added.end()), std::distance(class_added.begin(), class_added.end()));
124122

125123
// Apply both and re-optimize, results should be equivalent
126124
tg1.graph.update(transaction_free);
@@ -152,8 +150,8 @@ TEST(QRMarginalizer, FejFallbackMatchesNonFej)
152150
vesta_constraints::QRMarginalizer non_fej(false);
153151
vesta_constraints::QRMarginalizer fej(true);
154152

155-
auto transaction1 = non_fej.marginalize("test", {tg1.x1->uuid()}, tg1.graph);
156-
auto transaction2 = fej.marginalize("test", {tg2.x1->uuid()}, tg2.graph);
153+
auto transaction1 = non_fej.marginalize("test", { tg1.x1->uuid() }, tg1.graph);
154+
auto transaction2 = fej.marginalize("test", { tg2.x1->uuid() }, tg2.graph);
157155

158156
// Apply and re-optimize
159157
tg1.graph.update(transaction1);
@@ -207,8 +205,8 @@ TEST(QRMarginalizer, FejProducesDifferentJacobians)
207205
vesta_constraints::QRMarginalizer non_fej(false);
208206
vesta_constraints::QRMarginalizer fej(true);
209207

210-
auto transaction1 = non_fej.marginalize("test", {tg1.x1->uuid()}, tg1.graph);
211-
auto transaction2 = fej.marginalize("test", {tg2.x1->uuid()}, tg2.graph);
208+
auto transaction1 = non_fej.marginalize("test", { tg1.x1->uuid() }, tg1.graph);
209+
auto transaction2 = fej.marginalize("test", { tg2.x1->uuid() }, tg2.graph);
212210

213211
// The marginal constraints should differ because Jacobians are evaluated at different points
214212
// We verify this by applying both and checking that the optimized results diverge
@@ -258,7 +256,7 @@ TEST(QRMarginalizer, FejConsistency)
258256

259257
// Marginalize x1 with FEJ
260258
vesta_constraints::QRMarginalizer fej(true);
261-
auto transaction = fej.marginalize("test", {tg.x1->uuid()}, tg.graph);
259+
auto transaction = fej.marginalize("test", { tg.x1->uuid() }, tg.graph);
262260
tg.graph.update(transaction);
263261
tg.graph.optimize();
264262

@@ -316,7 +314,7 @@ TEST(QRMarginalizer, RuntimeMeasurement)
316314
TestGraph warmup;
317315
warmup.graph.optimize();
318316
vesta_constraints::QRMarginalizer m(false);
319-
m.marginalize("test", {warmup.x1->uuid()}, warmup.graph);
317+
m.marginalize("test", { warmup.x1->uuid() }, warmup.graph);
320318
}
321319

322320
constexpr int NUM_ITERATIONS = 100;
@@ -327,7 +325,7 @@ TEST(QRMarginalizer, RuntimeMeasurement)
327325
TestGraph t;
328326
t.graph.optimize();
329327
vesta_constraints::QRMarginalizer m(false);
330-
m.marginalize("test", {t.x1->uuid()}, t.graph);
328+
m.marginalize("test", { t.x1->uuid() }, t.graph);
331329
}
332330

333331
auto end = std::chrono::high_resolution_clock::now();

0 commit comments

Comments
 (0)