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_