Skip to content

Commit

Permalink
Satisfy cpplint
Browse files Browse the repository at this point in the history
Signed-off-by: methylDragon <methylDragon@gmail.com>
  • Loading branch information
methylDragon committed Dec 8, 2022
1 parent dd92d05 commit ba0d7ae
Show file tree
Hide file tree
Showing 9 changed files with 63 additions and 61 deletions.
22 changes: 10 additions & 12 deletions fuse_graphs/benchmark/benchmark_create_problem.cpp
Expand Up @@ -31,24 +31,22 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <fuse_core/constraint.hpp>
#include <fuse_core/serialization.hpp>
#include <fuse_core/uuid.hpp>
#include <fuse_graphs/hash_graph.hpp>

#include "example_variable.hpp"

#include <benchmark/benchmark.h>

#include <boost/serialization/access.hpp>
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/export.hpp>
#include <ceres/dynamic_autodiff_cost_function.h>

#include <algorithm>
#include <iterator>
#include <vector>
#include <string>
#include <vector>

#include <boost/serialization/access.hpp>
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/export.hpp>
#include "example_variable.hpp"
#include <fuse_core/constraint.hpp>
#include <fuse_core/serialization.hpp>
#include <fuse_core/uuid.hpp>
#include <fuse_graphs/hash_graph.hpp>

/**
* @brief Testable fuse_graphs::HashGraph that exposes the protected createProblem method as public
Expand Down
20 changes: 11 additions & 9 deletions fuse_graphs/include/fuse_graphs/hash_graph.hpp
Expand Up @@ -34,6 +34,15 @@
#ifndef FUSE_GRAPHS__HASH_GRAPH_HPP_
#define FUSE_GRAPHS__HASH_GRAPH_HPP_

#include <ceres/covariance.h>
#include <ceres/problem.h>
#include <ceres/solver.h>

#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#include <fuse_core/constraint.hpp>
#include <fuse_core/graph.hpp>
#include <fuse_core/fuse_macros.hpp>
Expand All @@ -48,14 +57,6 @@
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/unordered_map.hpp>
#include <boost/serialization/unordered_set.hpp>
#include <ceres/covariance.h>
#include <ceres/problem.h>
#include <ceres/solver.h>

#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>


namespace fuse_graphs
Expand Down Expand Up @@ -401,7 +402,8 @@ class HashGraph : public fuse_core::Graph

Constraints constraints_; //!< The set of all constraints
CrossReference constraints_by_variable_uuid_; //!< Index all of the constraints by variable uuids
ceres::Problem::Options problem_options_; //!< User-defined options to be applied to all constructed ceres::Problems
ceres::Problem::Options problem_options_; //!< User-defined options to be applied to all
//!< constructed ceres::Problems
Variables variables_; //!< The set of all variables
VariableSet variables_on_hold_; //!< The set of variables that should be held constant

Expand Down
4 changes: 2 additions & 2 deletions fuse_graphs/include/fuse_graphs/hash_graph_params.hpp
Expand Up @@ -34,11 +34,11 @@
#ifndef FUSE_GRAPHS__HASH_GRAPH_PARAMS_HPP_
#define FUSE_GRAPHS__HASH_GRAPH_PARAMS_HPP_

#include <ceres/problem.h>

#include <fuse_core/ceres_options.hpp>
#include <fuse_core/node_interfaces/node_interfaces.hpp>

#include <ceres/problem.h>


namespace fuse_graphs
{
Expand Down
15 changes: 6 additions & 9 deletions fuse_graphs/src/hash_graph.cpp
Expand Up @@ -31,15 +31,6 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <fuse_graphs/hash_graph.hpp>

#include <fuse_core/uuid.hpp>
#include <pluginlib/class_list_macros.hpp>

#include <boost/iterator/transform_iterator.hpp>
#include <boost/serialization/export.hpp>
#include <rclcpp/clock.hpp>

#include <algorithm>
#include <functional>
#include <limits>
Expand All @@ -48,6 +39,12 @@
#include <utility>
#include <vector>

#include <boost/iterator/transform_iterator.hpp>
#include <boost/serialization/export.hpp>
#include <fuse_core/uuid.hpp>
#include <fuse_graphs/hash_graph.hpp>
#include <pluginlib/class_list_macros.hpp>
#include <rclcpp/clock.hpp>

namespace fuse_graphs
{
Expand Down
9 changes: 5 additions & 4 deletions fuse_graphs/test/covariance_constraint.hpp
Expand Up @@ -34,6 +34,11 @@
#ifndef FUSE_GRAPHS__TEST_COVARIANCE_CONSTRAINT_HPP_ // NOLINT{build/header_guard}
#define FUSE_GRAPHS__TEST_COVARIANCE_CONSTRAINT_HPP_ // NOLINT{build/header_guard}

#include <ceres/cost_function.h>

#include <algorithm>
#include <string>

#include <fuse_core/constraint.hpp>
#include <fuse_core/fuse_macros.hpp>
#include <fuse_core/serialization.hpp>
Expand All @@ -42,10 +47,6 @@
#include <boost/serialization/access.hpp>
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/export.hpp>
#include <ceres/cost_function.h>

#include <algorithm>
#include <string>


/**
Expand Down
7 changes: 4 additions & 3 deletions fuse_graphs/test/example_constraint.hpp
Expand Up @@ -34,6 +34,10 @@
#ifndef FUSE_GRAPHS__TEST_EXAMPLE_CONSTRAINT_HPP_ // NOLINT{build/header_guard}
#define FUSE_GRAPHS__TEST_EXAMPLE_CONSTRAINT_HPP_ // NOLINT{build/header_guard}

#include <ceres/autodiff_cost_function.h>

#include <string>

#include <fuse_core/constraint.hpp>
#include <fuse_core/fuse_macros.hpp>
#include <fuse_core/serialization.hpp>
Expand All @@ -42,9 +46,6 @@
#include <boost/serialization/access.hpp>
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/export.hpp>
#include <ceres/autodiff_cost_function.h>

#include <string>


/**
Expand Down
6 changes: 3 additions & 3 deletions fuse_graphs/test/example_loss.hpp
Expand Up @@ -34,6 +34,9 @@
#ifndef FUSE_GRAPHS__TEST_EXAMPLE_LOSS_HPP_ // NOLINT{build/header_guard}
#define FUSE_GRAPHS__TEST_EXAMPLE_LOSS_HPP_ // NOLINT{build/header_guard}

#include <ostream>
#include <string>

#include <fuse_core/loss.hpp>
#include <fuse_core/fuse_macros.hpp>
#include <fuse_core/serialization.hpp>
Expand All @@ -42,9 +45,6 @@
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/export.hpp>

#include <ostream>
#include <string>


/**
* @brief Dummy loss implementation for testing
Expand Down
4 changes: 2 additions & 2 deletions fuse_graphs/test/example_variable.hpp
Expand Up @@ -34,6 +34,8 @@
#ifndef FUSE_GRAPHS__TEST_EXAMPLE_VARIABLE_HPP_ // NOLINT{build/header_guard}
#define FUSE_GRAPHS__TEST_EXAMPLE_VARIABLE_HPP_ // NOLINT{build/header_guard}

#include <vector>

#include <fuse_core/serialization.hpp>
#include <fuse_core/uuid.hpp>
#include <fuse_core/variable.hpp>
Expand All @@ -43,8 +45,6 @@
#include <boost/serialization/export.hpp>
#include <boost/serialization/vector.hpp>

#include <vector>


/**
* @brief Dummy variable implementation for testing
Expand Down
37 changes: 20 additions & 17 deletions fuse_graphs/test/test_hash_graph.cpp
Expand Up @@ -31,16 +31,6 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <fuse_core/constraint.hpp>
#include <fuse_core/serialization.hpp>
#include <fuse_core/uuid.hpp>
#include <fuse_core/variable.hpp>
#include <fuse_graphs/hash_graph.hpp>
#include "covariance_constraint.hpp"
#include "example_constraint.hpp"
#include "example_loss.hpp"
#include "example_variable.hpp"

#include <gtest/gtest.h>

#include <algorithm>
Expand All @@ -49,6 +39,16 @@
#include <utility>
#include <vector>

#include "covariance_constraint.hpp"
#include "example_constraint.hpp"
#include "example_loss.hpp"
#include "example_variable.hpp"
#include <fuse_core/constraint.hpp>
#include <fuse_core/serialization.hpp>
#include <fuse_core/uuid.hpp>
#include <fuse_core/variable.hpp>
#include <fuse_graphs/hash_graph.hpp>

/**
* @brief Test fixture for the HashGraph
*
Expand Down Expand Up @@ -145,7 +145,8 @@ class HashGraphTestFixture : public ::testing::Test
return constraints_equal;
}

std::string failure_description {""}; //!< The last failure description. Empty if no failure happened
std::string failure_description {""}; //!< The last failure description. Empty if no failure
//!< happened
};

TEST_F(HashGraphTestFixture, AddVariable)
Expand Down Expand Up @@ -765,8 +766,10 @@ TEST_F(HashGraphTestFixture, GetCovariance)
std::vector<std::pair<fuse_core::UUID, fuse_core::UUID>> covariance_requests;
covariance_requests.emplace_back(x->uuid(), x->uuid());
covariance_requests.emplace_back(x->uuid(), y->uuid());
covariance_requests.emplace_back(y->uuid(), x->uuid()); // Adding both versions Cov(X,Y) and Cov(Y,X)
covariance_requests.emplace_back(x->uuid(), y->uuid()); // Adding a duplicate to verify everything still works.
covariance_requests.emplace_back(y->uuid(), x->uuid()); // Adding both versions Cov(X,Y) and
// Cov(Y,X)
covariance_requests.emplace_back(x->uuid(), y->uuid()); // Adding a duplicate to verify
// everything still works.
covariance_requests.emplace_back(z->uuid(), y->uuid());
std::vector<std::vector<double>> covariance_matrices;
graph.getCovariance(covariance_requests, covariance_matrices);
Expand All @@ -791,12 +794,12 @@ TEST_F(HashGraphTestFixture, GetCovariance)
// [ ZX (1x2), ZY(1x3), ZZ (1x1)]
std::vector<double> expected0 = {7.0747e-02, -8.4923e-03, -8.4923e-03, 8.1352e-02}; // XX
std::vector<double> expected1 =
{1.6821e-02, 3.3643e-02, 5.0464e-02, 2.4758e-02, 4.9517e-02, 7.4275e-02}; // XY
{1.6821e-02, 3.3643e-02, 5.0464e-02, 2.4758e-02, 4.9517e-02, 7.4275e-02}; // XY
std::vector<double> expected2 =
{1.6821e-02, 2.4758e-02, 3.3643e-02, 4.9517e-02, 5.0464e-02, 7.4275e-02}; // YX
{1.6821e-02, 2.4758e-02, 3.3643e-02, 4.9517e-02, 5.0464e-02, 7.4275e-02}; // YX
std::vector<double> expected3 =
{1.6821e-02, 3.3643e-02, 5.0464e-02, 2.4758e-02, 4.9517e-02, 7.4275e-02}; // XY
std::vector<double> expected4 = {-6.5325e-05, -1.3065e-04, -1.9598e-04}; // ZY
{1.6821e-02, 3.3643e-02, 5.0464e-02, 2.4758e-02, 4.9517e-02, 7.4275e-02}; // XY
std::vector<double> expected4 = {-6.5325e-05, -1.3065e-04, -1.9598e-04}; // ZY

ASSERT_EQ(expected0.size(), actual0.size());
for (size_t i = 0; i < expected0.size(); ++i) {
Expand Down

0 comments on commit ba0d7ae

Please sign in to comment.