Program Listing for File hash_graph.hpp
↰ Return to documentation for file (/tmp/ws/src/fuse/fuse_graphs/include/fuse_graphs/hash_graph.hpp
)
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Locus Robotics
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#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>
#include <fuse_core/serialization.hpp>
#include <fuse_core/uuid.hpp>
#include <fuse_core/variable.hpp>
#include <fuse_graphs/hash_graph_params.hpp>
#include <boost/serialization/access.hpp>
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/export.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/unordered_map.hpp>
#include <boost/serialization/unordered_set.hpp>
namespace fuse_graphs
{
class HashGraph : public fuse_core::Graph
{
public:
FUSE_GRAPH_DEFINITIONS(HashGraph)
explicit HashGraph(const HashGraphParams & params = HashGraphParams());
HashGraph(const HashGraph & other);
virtual ~HashGraph() = default;
HashGraph & operator=(const HashGraph & other);
void clear() override;
fuse_core::Graph::UniquePtr clone() const override;
bool constraintExists(const fuse_core::UUID & constraint_uuid) const noexcept override;
bool addConstraint(fuse_core::Constraint::SharedPtr constraint) override;
bool removeConstraint(const fuse_core::UUID & constraint_uuid) override;
const fuse_core::Constraint & getConstraint(const fuse_core::UUID & constraint_uuid) const
override;
fuse_core::Graph::const_constraint_range getConstraints() const noexcept override;
fuse_core::Graph::const_constraint_range getConnectedConstraints(
const fuse_core::UUID & variable_uuid) const override;
bool variableExists(const fuse_core::UUID & variable_uuid) const noexcept override;
bool addVariable(fuse_core::Variable::SharedPtr variable) override;
bool removeVariable(const fuse_core::UUID & variable_uuid) override;
const fuse_core::Variable & getVariable(const fuse_core::UUID & variable_uuid) const override;
fuse_core::Graph::const_variable_range getVariables() const noexcept override;
void holdVariable(const fuse_core::UUID & variable_uuid, bool hold_constant = true) override;
bool isVariableOnHold(const fuse_core::UUID & variable_uuid) const override;
void getCovariance(
const std::vector<std::pair<fuse_core::UUID, fuse_core::UUID>> & covariance_requests,
std::vector<std::vector<double>> & covariance_matrices,
const ceres::Covariance::Options & options = ceres::Covariance::Options(),
const bool use_tangent_space = true) const override;
ceres::Solver::Summary optimize(const ceres::Solver::Options & options = ceres::Solver::Options())
override;
ceres::Solver::Summary optimizeFor(
const rclcpp::Duration & max_optimization_time,
const ceres::Solver::Options & options = ceres::Solver::Options(),
rclcpp::Clock clock = rclcpp::Clock(RCL_STEADY_TIME)) override;
bool evaluate(
double * cost, std::vector<double> * residuals = nullptr,
std::vector<double> * gradient = nullptr,
const ceres::Problem::EvaluateOptions & options = ceres::Problem::EvaluateOptions()) const
override;
void print(std::ostream & stream = std::cout) const override;
protected:
// Define some helpful typedefs
using Constraints = std::unordered_map<fuse_core::UUID, fuse_core::Constraint::SharedPtr,
fuse_core::uuid::hash>;
using Variables = std::unordered_map<fuse_core::UUID, fuse_core::Variable::SharedPtr,
fuse_core::uuid::hash>;
using VariableSet = std::unordered_set<fuse_core::UUID, fuse_core::uuid::hash>;
using CrossReference = std::unordered_map<fuse_core::UUID, std::vector<fuse_core::UUID>,
fuse_core::uuid::hash>;
Constraints constraints_;
CrossReference constraints_by_variable_uuid_;
ceres::Problem::Options problem_options_;
Variables variables_;
VariableSet variables_on_hold_;
void createProblem(ceres::Problem & problem) const;
private:
// Allow Boost Serialization access to private methods
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & archive, const unsigned int /* version */)
{
archive & boost::serialization::base_object<fuse_core::Graph>(*this);
archive & constraints_;
archive & constraints_by_variable_uuid_;
archive & problem_options_;
archive & variables_;
archive & variables_on_hold_;
}
};
} // namespace fuse_graphs
namespace boost
{
namespace serialization
{
template<class Archive>
void serialize(
Archive & archive, ceres::Problem::Options & options,
const unsigned int /* version */)
{
archive & options.cost_function_ownership;
archive & options.disable_all_safety_checks;
archive & options.enable_fast_removal;
archive & options.local_parameterization_ownership;
archive & options.loss_function_ownership;
}
} // namespace serialization
} // namespace boost
BOOST_CLASS_EXPORT_KEY(fuse_graphs::HashGraph);
#endif // FUSE_GRAPHS__HASH_GRAPH_HPP_