Skip to content

Commit

Permalink
further pre-commit fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
@henrygerardmoore
henrygerardmoore committed Sep 23, 2024
1 parent 7a43bf7 commit 04c3dc9
Show file tree
Hide file tree
Showing 369 changed files with 10,059 additions and 12,837 deletions.
2 changes: 1 addition & 1 deletion .github/workflows/pre-commit.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ jobs:
- uses: actions/setup-python@v5
with:
python-version: '3.10'
- name: Install clang-format-14
- name: Install clang-format-18
run: sudo apt-get install clang-format-18
- uses: pre-commit/[email protected]
id: precommit
Expand Down
72 changes: 29 additions & 43 deletions fuse_constraints/benchmark/benchmark_normal_delta_pose_2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,11 +43,10 @@
class NormalDeltaPose2DBenchmarkFixture : public benchmark::Fixture
{
public:
NormalDeltaPose2DBenchmarkFixture()
: jacobians(num_parameter_blocks)
, J(num_parameter_blocks)
NormalDeltaPose2DBenchmarkFixture() : jacobians(num_parameter_blocks), J(num_parameter_blocks)
{
for (size_t i = 0; i < num_parameter_blocks; ++i) {
for (size_t i = 0; i < num_parameter_blocks; ++i)
{
J[i].resize(num_residuals, block_sizes[i]);
jacobians[i] = J[i].data();
}
Expand All @@ -58,18 +57,18 @@ class NormalDeltaPose2DBenchmarkFixture : public benchmark::Fixture
static const fuse_core::Matrix3d sqrt_information;

// Parameters
static const double * parameters[];
static const double* parameters[];

// Residuals
fuse_core::Vector3d residuals;

static const std::vector<int32_t> & block_sizes;
static const std::vector<int32_t>& block_sizes;
static const size_t num_parameter_blocks;

static const size_t num_residuals;

// Jacobians
std::vector<double *> jacobians;
std::vector<double*> jacobians;

private:
// Cost function covariance
Expand All @@ -88,69 +87,56 @@ class NormalDeltaPose2DBenchmarkFixture : public benchmark::Fixture
};

// Cost function covariance
const double NormalDeltaPose2DBenchmarkFixture::covariance_diagonal[] = {2e-3, 1e-3, 1e-2};
const double NormalDeltaPose2DBenchmarkFixture::covariance_diagonal[] = { 2e-3, 1e-3, 1e-2 };

const fuse_core::Matrix3d NormalDeltaPose2DBenchmarkFixture::covariance =
fuse_core::Vector3d(covariance_diagonal).asDiagonal();
fuse_core::Vector3d(covariance_diagonal).asDiagonal();

// Parameter blocks
const double NormalDeltaPose2DBenchmarkFixture::position1[] = {0.0, 1.0};
const double NormalDeltaPose2DBenchmarkFixture::orientation1[] = {0.5};
const double NormalDeltaPose2DBenchmarkFixture::position2[] = {2.0, 3.0};
const double NormalDeltaPose2DBenchmarkFixture::orientation2[] = {1.5};
const double NormalDeltaPose2DBenchmarkFixture::position1[] = { 0.0, 1.0 };
const double NormalDeltaPose2DBenchmarkFixture::orientation1[] = { 0.5 };
const double NormalDeltaPose2DBenchmarkFixture::position2[] = { 2.0, 3.0 };
const double NormalDeltaPose2DBenchmarkFixture::orientation2[] = { 1.5 };

// Delta and sqrt information matrix
const fuse_core::Vector3d NormalDeltaPose2DBenchmarkFixture::delta{1.0, 2.0, 3.0};
const fuse_core::Matrix3d NormalDeltaPose2DBenchmarkFixture::sqrt_information(covariance.inverse().
llt().matrixU());
const fuse_core::Vector3d NormalDeltaPose2DBenchmarkFixture::delta{ 1.0, 2.0, 3.0 };
const fuse_core::Matrix3d NormalDeltaPose2DBenchmarkFixture::sqrt_information(covariance.inverse().llt().matrixU());

// Parameters
const double * NormalDeltaPose2DBenchmarkFixture::parameters[] =
{position1, orientation1, position2, orientation2};
const double* NormalDeltaPose2DBenchmarkFixture::parameters[] = { position1, orientation1, position2, orientation2 };

const std::vector<int32_t> & NormalDeltaPose2DBenchmarkFixture::block_sizes = {2, 1, 2, 1};
const std::vector<int32_t>& NormalDeltaPose2DBenchmarkFixture::block_sizes = { 2, 1, 2, 1 };
const size_t NormalDeltaPose2DBenchmarkFixture::num_parameter_blocks = block_sizes.size();

const size_t NormalDeltaPose2DBenchmarkFixture::num_residuals = 3;

BENCHMARK_DEFINE_F(
NormalDeltaPose2DBenchmarkFixture,
AnalyticNormalDeltaPose2D)(benchmark::State & state)
BENCHMARK_DEFINE_F(NormalDeltaPose2DBenchmarkFixture, AnalyticNormalDeltaPose2D)(benchmark::State& state)
{
// Create analytic cost function
const fuse_constraints::NormalDeltaPose2D cost_function{sqrt_information.topRows(state.range(0)),
delta};
const fuse_constraints::NormalDeltaPose2D cost_function{ sqrt_information.topRows(state.range(0)), delta };

for (auto _ : state) {
for (auto _ : state)
{
cost_function.Evaluate(parameters, residuals.data(), jacobians.data());
}
}

BENCHMARK_REGISTER_F(NormalDeltaPose2DBenchmarkFixture, AnalyticNormalDeltaPose2D)->DenseRange(
1, 3);
BENCHMARK_REGISTER_F(NormalDeltaPose2DBenchmarkFixture, AnalyticNormalDeltaPose2D)->DenseRange(1, 3);

BENCHMARK_DEFINE_F(
NormalDeltaPose2DBenchmarkFixture,
AutoDiffNormalDeltaPose2D)(benchmark::State & state)
BENCHMARK_DEFINE_F(NormalDeltaPose2DBenchmarkFixture, AutoDiffNormalDeltaPose2D)(benchmark::State& state)
{
// Create cost function using automatic differentiation on the cost functor
const auto partial_sqrt_information = sqrt_information.topRows(state.range(0));
const ceres::AutoDiffCostFunction<
fuse_constraints::NormalDeltaPose2DCostFunctor,
ceres::DYNAMIC,
2, 1, 2, 1
>
cost_function_autodiff(new fuse_constraints::NormalDeltaPose2DCostFunctor(
partial_sqrt_information,
delta),
partial_sqrt_information.rows());

for (auto _ : state) {
const ceres::AutoDiffCostFunction<fuse_constraints::NormalDeltaPose2DCostFunctor, ceres::DYNAMIC, 2, 1, 2, 1>
cost_function_autodiff(new fuse_constraints::NormalDeltaPose2DCostFunctor(partial_sqrt_information, delta),
partial_sqrt_information.rows());

for (auto _ : state)
{
cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians.data());
}
}

BENCHMARK_REGISTER_F(NormalDeltaPose2DBenchmarkFixture, AutoDiffNormalDeltaPose2D)->DenseRange(
1, 3);
BENCHMARK_REGISTER_F(NormalDeltaPose2DBenchmarkFixture, AutoDiffNormalDeltaPose2D)->DenseRange(1, 3);

BENCHMARK_MAIN();
67 changes: 27 additions & 40 deletions fuse_constraints/benchmark/benchmark_normal_prior_pose_2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,11 +43,10 @@
class NormalPriorPose2DBenchmarkFixture : public benchmark::Fixture
{
public:
NormalPriorPose2DBenchmarkFixture()
: jacobians(num_parameter_blocks)
, J(num_parameter_blocks)
NormalPriorPose2DBenchmarkFixture() : jacobians(num_parameter_blocks), J(num_parameter_blocks)
{
for (size_t i = 0; i < num_parameter_blocks; ++i) {
for (size_t i = 0; i < num_parameter_blocks; ++i)
{
J[i].resize(num_residuals, block_sizes[i]);
jacobians[i] = J[i].data();
}
Expand All @@ -58,18 +57,18 @@ class NormalPriorPose2DBenchmarkFixture : public benchmark::Fixture
static const fuse_core::Matrix3d sqrt_information;

// Parameters
static const double * parameters[];
static const double* parameters[];

// Residuals
fuse_core::Vector3d residuals;

static const std::vector<int32_t> & block_sizes;
static const std::vector<int32_t>& block_sizes;
static const size_t num_parameter_blocks;

static const size_t num_residuals;

// Jacobians
std::vector<double *> jacobians;
std::vector<double*> jacobians;

private:
// Cost function covariance
Expand All @@ -86,66 +85,54 @@ class NormalPriorPose2DBenchmarkFixture : public benchmark::Fixture
};

// Cost function covariance
const double NormalPriorPose2DBenchmarkFixture::covariance_diagonal[] = {2e-3, 1e-3, 1e-2};
const double NormalPriorPose2DBenchmarkFixture::covariance_diagonal[] = { 2e-3, 1e-3, 1e-2 };

const fuse_core::Matrix3d NormalPriorPose2DBenchmarkFixture::covariance =
fuse_core::Vector3d(covariance_diagonal).asDiagonal();
fuse_core::Vector3d(covariance_diagonal).asDiagonal();

// Parameter blocks
const double NormalPriorPose2DBenchmarkFixture::position[] = {0.0, 0.0};
const double NormalPriorPose2DBenchmarkFixture::orientation[] = {0.0};
const double NormalPriorPose2DBenchmarkFixture::position[] = { 0.0, 0.0 };
const double NormalPriorPose2DBenchmarkFixture::orientation[] = { 0.0 };

// Mean and sqrt information matrix
const fuse_core::Vector3d NormalPriorPose2DBenchmarkFixture::mean{1.0, 2.0, 3.0};
const fuse_core::Matrix3d NormalPriorPose2DBenchmarkFixture::sqrt_information(covariance.inverse().
llt().matrixU());
const fuse_core::Vector3d NormalPriorPose2DBenchmarkFixture::mean{ 1.0, 2.0, 3.0 };
const fuse_core::Matrix3d NormalPriorPose2DBenchmarkFixture::sqrt_information(covariance.inverse().llt().matrixU());

// Parameters
const double * NormalPriorPose2DBenchmarkFixture::parameters[] = {position, orientation};
const double* NormalPriorPose2DBenchmarkFixture::parameters[] = { position, orientation };

const std::vector<int32_t> & NormalPriorPose2DBenchmarkFixture::block_sizes = {2, 1};
const std::vector<int32_t>& NormalPriorPose2DBenchmarkFixture::block_sizes = { 2, 1 };
const size_t NormalPriorPose2DBenchmarkFixture::num_parameter_blocks = block_sizes.size();

const size_t NormalPriorPose2DBenchmarkFixture::num_residuals = 3;

BENCHMARK_DEFINE_F(
NormalPriorPose2DBenchmarkFixture,
AnalyticNormalPriorPose2D)(benchmark::State & state)
BENCHMARK_DEFINE_F(NormalPriorPose2DBenchmarkFixture, AnalyticNormalPriorPose2D)(benchmark::State& state)
{
// Create analytic cost function
const fuse_constraints::NormalPriorPose2D cost_function{sqrt_information.topRows(state.range(0)),
mean};
const fuse_constraints::NormalPriorPose2D cost_function{ sqrt_information.topRows(state.range(0)), mean };

for (auto _ : state) {
for (auto _ : state)
{
cost_function.Evaluate(parameters, residuals.data(), jacobians.data());
}
}

BENCHMARK_REGISTER_F(NormalPriorPose2DBenchmarkFixture, AnalyticNormalPriorPose2D)->DenseRange(
1, 3);
BENCHMARK_REGISTER_F(NormalPriorPose2DBenchmarkFixture, AnalyticNormalPriorPose2D)->DenseRange(1, 3);

BENCHMARK_DEFINE_F(
NormalPriorPose2DBenchmarkFixture,
AutoDiffNormalPriorPose2D)(benchmark::State & state)
BENCHMARK_DEFINE_F(NormalPriorPose2DBenchmarkFixture, AutoDiffNormalPriorPose2D)(benchmark::State& state)
{
// Create cost function using automatic differentiation on the cost functor
const auto partial_sqrt_information = sqrt_information.topRows(state.range(0));
const ceres::AutoDiffCostFunction<
fuse_constraints::NormalPriorPose2DCostFunctor,
ceres::DYNAMIC,
2, 1
>
cost_function_autodiff(new fuse_constraints::NormalPriorPose2DCostFunctor(
partial_sqrt_information,
mean),
partial_sqrt_information.rows());

for (auto _ : state) {
const ceres::AutoDiffCostFunction<fuse_constraints::NormalPriorPose2DCostFunctor, ceres::DYNAMIC, 2, 1>
cost_function_autodiff(new fuse_constraints::NormalPriorPose2DCostFunctor(partial_sqrt_information, mean),
partial_sqrt_information.rows());

for (auto _ : state)
{
cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians.data());
}
}

BENCHMARK_REGISTER_F(NormalPriorPose2DBenchmarkFixture, AutoDiffNormalPriorPose2D)->DenseRange(
1, 3);
BENCHMARK_REGISTER_F(NormalPriorPose2DBenchmarkFixture, AutoDiffNormalPriorPose2D)->DenseRange(1, 3);

BENCHMARK_MAIN();
59 changes: 26 additions & 33 deletions fuse_constraints/include/fuse_constraints/absolute_constraint.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,6 @@
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/export.hpp>


namespace fuse_constraints
{

Expand All @@ -71,7 +70,7 @@ namespace fuse_constraints
* prior map (scan-to-map measurements). This constraint holds the measured variable value and the
* measurement uncertainty/covariance.
*/
template<class Variable>
template <class Variable>
class AbsoluteConstraint : public fuse_core::Constraint
{
public:
Expand All @@ -90,11 +89,8 @@ class AbsoluteConstraint : public fuse_core::Constraint
* @param[in] mean The measured/prior value of all variable dimensions
* @param[in] covariance The measurement/prior uncertainty of all variable dimensions
*/
AbsoluteConstraint(
const std::string & source,
const Variable & variable,
const fuse_core::VectorXd & mean,
const fuse_core::MatrixXd & covariance);
AbsoluteConstraint(const std::string& source, const Variable& variable, const fuse_core::VectorXd& mean,
const fuse_core::MatrixXd& covariance);

/**
* @brief Create a constraint using a measurement/prior of only a partial set of dimensions of the
Expand All @@ -109,12 +105,8 @@ class AbsoluteConstraint : public fuse_core::Constraint
* by \p indices.
* @param[in] indices The set of indices corresponding to the measured dimensions
*/
AbsoluteConstraint(
const std::string & source,
const Variable & variable,
const fuse_core::VectorXd & partial_mean,
const fuse_core::MatrixXd & partial_covariance,
const std::vector<size_t> & indices);
AbsoluteConstraint(const std::string& source, const Variable& variable, const fuse_core::VectorXd& partial_mean,
const fuse_core::MatrixXd& partial_covariance, const std::vector<size_t>& indices);

/**
* @brief Destructor
Expand All @@ -128,7 +120,10 @@ class AbsoluteConstraint : public fuse_core::Constraint
* are in the order defined by the variable, not the order defined by the \p indices parameter.
* All unmeasured variable dimensions are set to zero.
*/
const fuse_core::VectorXd & mean() const {return mean_;}
const fuse_core::VectorXd& mean() const
{
return mean_;
}

/**
* @brief Read-only access to the square root information matrix.
Expand All @@ -138,7 +133,10 @@ class AbsoluteConstraint : public fuse_core::Constraint
* variable_dimensions. If only a partial set of dimensions are measured, then this matrix will
* not be square.
*/
const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;}
const fuse_core::MatrixXd& sqrtInformation() const
{
return sqrt_information_;
}

/**
* @brief Compute the measurement covariance matrix.
Expand All @@ -155,7 +153,7 @@ class AbsoluteConstraint : public fuse_core::Constraint
*
* @param[out] stream The stream to write to. Defaults to stdout.
*/
void print(std::ostream & stream = std::cout) const override;
void print(std::ostream& stream = std::cout) const override;

/**
* @brief Construct an instance of this constraint's cost function
Expand All @@ -167,10 +165,10 @@ class AbsoluteConstraint : public fuse_core::Constraint
*
* @return A base pointer to an instance of a derived CostFunction.
*/
ceres::CostFunction * costFunction() const override;
ceres::CostFunction* costFunction() const override;

protected:
fuse_core::VectorXd mean_; //!< The measured/prior mean vector for this variable
fuse_core::VectorXd mean_; //!< The measured/prior mean vector for this variable
fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix

private:
Expand All @@ -184,28 +182,23 @@ class AbsoluteConstraint : public fuse_core::Constraint
* @param[in/out] archive - The archive object that holds the serialized class members
* @param[in] version - The version of the archive being read/written. Generally unused.
*/
template<class Archive>
void serialize(Archive & archive, const unsigned int /* version */)
template <class Archive>
void serialize(Archive& archive, const unsigned int /* version */)
{
archive & boost::serialization::base_object<fuse_core::Constraint>(*this);
archive & mean_;
archive & sqrt_information_;
archive& boost::serialization::base_object<fuse_core::Constraint>(*this);
archive& mean_;
archive& sqrt_information_;
}
};

// Define unique names for the different variations of the absolute constraint
using AbsoluteAccelerationAngular2DStampedConstraint =
AbsoluteConstraint<fuse_variables::AccelerationAngular2DStamped>;
using AbsoluteAccelerationLinear2DStampedConstraint =
AbsoluteConstraint<fuse_variables::AccelerationLinear2DStamped>;
using AbsoluteOrientation2DStampedConstraint =
AbsoluteConstraint<fuse_variables::Orientation2DStamped>;
using AbsoluteAccelerationAngular2DStampedConstraint = AbsoluteConstraint<fuse_variables::AccelerationAngular2DStamped>;
using AbsoluteAccelerationLinear2DStampedConstraint = AbsoluteConstraint<fuse_variables::AccelerationLinear2DStamped>;
using AbsoluteOrientation2DStampedConstraint = AbsoluteConstraint<fuse_variables::Orientation2DStamped>;
using AbsolutePosition2DStampedConstraint = AbsoluteConstraint<fuse_variables::Position2DStamped>;
using AbsolutePosition3DStampedConstraint = AbsoluteConstraint<fuse_variables::Position3DStamped>;
using AbsoluteVelocityAngular2DStampedConstraint =
AbsoluteConstraint<fuse_variables::VelocityAngular2DStamped>;
using AbsoluteVelocityLinear2DStampedConstraint =
AbsoluteConstraint<fuse_variables::VelocityLinear2DStamped>;
using AbsoluteVelocityAngular2DStampedConstraint = AbsoluteConstraint<fuse_variables::VelocityAngular2DStamped>;
using AbsoluteVelocityLinear2DStampedConstraint = AbsoluteConstraint<fuse_variables::VelocityLinear2DStamped>;
} // namespace fuse_constraints

// Include the template implementation
Expand Down
Loading

0 comments on commit 04c3dc9

Please sign in to comment.