diff --git a/LICENSE b/LICENSE new file mode 100644 index 000000000..3ff0a6145 --- /dev/null +++ b/LICENSE @@ -0,0 +1,31 @@ +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. diff --git a/README.md b/README.md index c2a04229c..386eb256f 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,3 @@ # fuse -The fuse stack provides a general architecture for performing sensor fusion live on a robot. Some possible applications include state estimation, localization, mapping, and calibration. +The fuse stack provides a general architecture for performing sensor fusion live on a robot. Some possible applications +include state estimation, localization, mapping, and calibration. diff --git a/fuse/CMakeLists.txt b/fuse/CMakeLists.txt new file mode 100644 index 000000000..69d9f1683 --- /dev/null +++ b/fuse/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(fuse) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/fuse/LICENSE b/fuse/LICENSE new file mode 100644 index 000000000..3ff0a6145 --- /dev/null +++ b/fuse/LICENSE @@ -0,0 +1,31 @@ +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. diff --git a/fuse/README.md b/fuse/README.md new file mode 100644 index 000000000..b3c5d812d --- /dev/null +++ b/fuse/README.md @@ -0,0 +1,2 @@ +# fuse +This is the metapackage for the entire fuse stack. \ No newline at end of file diff --git a/fuse/package.xml b/fuse/package.xml new file mode 100644 index 000000000..750d6cabd --- /dev/null +++ b/fuse/package.xml @@ -0,0 +1,19 @@ + + + fuse + 0.0.0 + + The fuse metapackage + + + Stephen Williams + Stephen Williams + BSD + + catkin + fuse_core + + + + + diff --git a/fuse_core/CMakeLists.txt b/fuse_core/CMakeLists.txt new file mode 100644 index 000000000..51abac16a --- /dev/null +++ b/fuse_core/CMakeLists.txt @@ -0,0 +1,74 @@ +cmake_minimum_required(VERSION 2.8.3) +project(fuse_core) + +find_package(catkin REQUIRED COMPONENTS + roscpp +) +find_package(Boost REQUIRED) +find_package(Ceres REQUIRED) + +catkin_package( + INCLUDE_DIRS + include + ${CERES_INCLUDE_DIRS} + LIBRARIES + ${PROJECT_NAME} + ${CERES_LIBRARIES} + CATKIN_DEPENDS + roscpp + DEPENDS + Boost +) + +########### +## Build ## +########### + +set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++14;-Wall;-Werror") + +include_directories( + include + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${CERES_INCLUDE_DIRS} +) + +## fuse_core library +add_library(${PROJECT_NAME} + src/variable.cpp +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${CERES_LIBRARIES} +) + +############# +## Install ## +############# + +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +############# +## Testing ## +############# + +if(CATKIN_ENABLE_TESTING) + # Lint tests + find_package(roslint REQUIRED) + set(ROSLINT_CPP_OPTS "--filter=-build/c++11,-runtime/references") + roslint_cpp() + roslint_add_test() +endif() diff --git a/fuse_core/LICENSE b/fuse_core/LICENSE new file mode 100644 index 000000000..3ff0a6145 --- /dev/null +++ b/fuse_core/LICENSE @@ -0,0 +1,31 @@ +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. diff --git a/fuse_core/README.md b/fuse_core/README.md new file mode 100644 index 000000000..39df5d7f2 --- /dev/null +++ b/fuse_core/README.md @@ -0,0 +1,25 @@ +# fuse_core +This package provides the base class interfaces for the various fuse components. Concrete implementations of these +interfaces are provided in other packages. + +## Variable +Within the fuse system, a "variable" is a convenient group of one or more related scalar values. These scalars are +treated as a block within the optimizer. If the variable is modeling a time-varying quatity, the an instance of the +variable represents value of the state at one specific point in time. The fuse system maintains a history of variable +instances, allowing measurements to involve previous states as well as the current state. + +When defining a new variable type, there is a balance that must be struct between reusability and convenience. If you +define a complex composite state, it is unlikely that any other available components will use that same state +definition. If you make the state too granular, then more book-keeping and value lookups will be required to piece +together a useful concept from many smallar scalar components. + +As an example, let's consider a 3D pose consisting of a 3D position (x, y, z) and a quaternion orientation +(qx, qy, qx, qw). We could define a single state for the 3D pose consisting of all 7 scalar components. Alternatively, +we could define two variables types, a 3 dimension position vector and a 4 dimension quaternion. Or we could even +define seven variable types, one for each dimension. + +Within the `fuse_core` package, no concrete variables are actually created. We only define the common interface to which +all types must adhere. A set of common variable types are provided in the [`fuse_variables`](../fuse_variables) +package. And new variable types can be created outside the fuse stack completely. However, similar to how using common +ROS messages across nodes allow for code reuse, using common variable types will allow sensor models and motion models +to be shared across the community. diff --git a/fuse_core/include/fuse_core/macros.h b/fuse_core/include/fuse_core/macros.h new file mode 100644 index 000000000..7c0ab0d57 --- /dev/null +++ b/fuse_core/include/fuse_core/macros.h @@ -0,0 +1,135 @@ +// Copyright 2014 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/*************************************************************************** + * This file has been modified from its original published source. + * https://raw.githubusercontent.com/ros2/rclcpp/master/rclcpp/include/rclcpp/macros.hpp + * Modifications are provided under the BSD license. + ***************************************************************************/ + +/* + * 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_CORE_MACROS_H +#define FUSE_CORE_MACROS_H + +#include + +/** + * Defines aliases and static functions for using the Class with smart pointers. + * + * Use in the public section of the class. + */ +#define SMART_PTR_DEFINITIONS(...) \ + SHARED_PTR_DEFINITIONS(__VA_ARGS__) \ + WEAK_PTR_DEFINITIONS(__VA_ARGS__) \ + UNIQUE_PTR_DEFINITIONS(__VA_ARGS__) + +/** + * Defines aliases only for using the Class with smart pointers. + * + * Same as SMART_PTR_DEFINITIONS except it excludes the static + * method definitions which do not work on pure virtual classes and classes + * which are not CopyConstructable. + * + * Use in the public section of the class. + */ +#define SMART_PTR_ALIASES_ONLY(...) \ + __SHARED_PTR_ALIAS(__VA_ARGS__) \ + __WEAK_PTR_ALIAS(__VA_ARGS__) \ + __UNIQUE_PTR_ALIAS(__VA_ARGS__) + +/// Defines aliases and static functions for using the Class with shared_ptrs. +#define SHARED_PTR_DEFINITIONS(...) \ + __SHARED_PTR_ALIAS(__VA_ARGS__) \ + __MAKE_SHARED_DEFINITION(__VA_ARGS__) + +#define __SHARED_PTR_ALIAS(...) \ + using SharedPtr = std::shared_ptr<__VA_ARGS__>; \ + using ConstSharedPtr = std::shared_ptr; + +#define __MAKE_SHARED_DEFINITION(...) \ + template \ + static std::shared_ptr<__VA_ARGS__> \ + make_shared(Args && ... args) \ + { \ + return std::make_shared<__VA_ARGS__>(std::forward(args) ...); \ + } + +/// Defines aliases and static functions for using the Class with weak_ptrs. +#define WEAK_PTR_DEFINITIONS(...) \ + __WEAK_PTR_ALIAS(__VA_ARGS__) + +#define __WEAK_PTR_ALIAS(...) \ + using WeakPtr = std::weak_ptr<__VA_ARGS__>; \ + using ConstWeakPtr = std::weak_ptr; + +/// Defines aliases and static functions for using the Class with unique_ptrs. +#define UNIQUE_PTR_DEFINITIONS(...) \ + __UNIQUE_PTR_ALIAS(__VA_ARGS__) \ + __MAKE_UNIQUE_DEFINITION(__VA_ARGS__) + +#define __UNIQUE_PTR_ALIAS(...) \ + using UniquePtr = std::unique_ptr<__VA_ARGS__>; + +#if __cplusplus >= 201402L + #define __MAKE_UNIQUE_DEFINITION(...) \ + template \ + static std::unique_ptr<__VA_ARGS__> \ + make_unique(Args && ... args) \ + { \ + return std::make_unique<__VA_ARGS__>(std::forward(args) ...); \ + } +#else + #define __MAKE_UNIQUE_DEFINITION(...) \ + template \ + static std::unique_ptr<__VA_ARGS__> \ + make_unique(Args && ... args) \ + { \ + return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward(args) ...)); \ + } +#endif + +#endif // FUSE_CORE_MACROS_H diff --git a/fuse_core/include/fuse_core/uuid.h b/fuse_core/include/fuse_core/uuid.h new file mode 100644 index 000000000..ebc7829ef --- /dev/null +++ b/fuse_core/include/fuse_core/uuid.h @@ -0,0 +1,192 @@ +/* + * 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_CORE_UUID_H +#define FUSE_CORE_UUID_H + +#include + +#include +#include +#include +#include + +#include +#include +#include + + +namespace fuse_core +{ + +using UUID = boost::uuids::uuid; + +namespace uuid +{ + using boost::uuids::to_string; + using hash = boost::hash; + constexpr UUID NIL = {{0}}; + + /** + * @brief Generate a random UUID + */ + inline UUID generate() + { + return boost::uuids::random_generator()(); + } + + /** + * @brief Generate a UUID from a raw data buffer + * + * @param[in] data A data buffer containing information that makes this item unique + * @param[in] byte_count The number of bytes in the data buffer + * @return A repeatable UUID specific to the provided data + */ + inline UUID generate(const void* data, size_t byte_count) + { + return boost::uuids::name_generator(NIL)(data, byte_count); + } + + /** + * @brief Generate a UUID from a C-style string + * + * @param[in] data A data buffer held in a C-style string + * @return A repeatable UUID specific to the provided data + */ + inline UUID generate(const char* data) + { + return generate(data, std::strlen(data)); + } + + /** + * @brief Generate a UUID from a C++ string + * + * @param[in] data A data buffer held in a C++-style string + * @return A repeatable UUID specific to the provided namespace and data + */ + inline UUID generate(const std::string& data) + { + return generate(data.c_str(), data.length()); + } + + /** + * @brief Generate a UUID from a namespace string and a raw data buffer + * + * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs + * @param[in] data A data buffer containing information that makes this item unique + * @param[in] byte_count The number of bytes in the data buffer + * @return A repeatable UUID specific to the provided namespace and data + */ + inline UUID generate(const std::string& namespace_string, const void* data, size_t byte_count) + { + return boost::uuids::name_generator(generate(namespace_string))(data, byte_count); + } + + /** + * @brief Generate a UUID from a namespace string and C-style string + * + * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs + * @param[in] data A data buffer held in a C-style string + * @return A repeatable UUID specific to the provided namespace and data + */ + inline UUID generate(const std::string& namespace_string, const char* data) + { + return generate(namespace_string, data, std::strlen(data)); + } + + /** + * @brief Generate a UUID from a namespace string and a C++ string + * + * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs + * @param[in] data A data buffer held in a C++-style string + * @return A repeatable UUID specific to the provided namespace and data + */ + inline UUID generate(const std::string& namespace_string, const std::string& data) + { + return generate(namespace_string, data.c_str(), data.length()); + } + + /** + * @brief Generate a UUID from a namespace string and a ros timestamp + * + * Every unique timestamp will generate a unique UUID + * + * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs + * @param[in] stamp A ROS::Time timestamp + * @return A repeatable UUID specific to the provided namespace and timestamp + */ + inline UUID generate(const std::string& namespace_string, const ros::Time& stamp) + { + constexpr size_t buffer_size = sizeof(stamp.sec) + sizeof(stamp.nsec); + std::array buffer; + auto iter = buffer.begin(); + iter = std::copy(reinterpret_cast(&stamp.sec), + reinterpret_cast(&stamp.sec) + sizeof(stamp.sec), + iter); + iter = std::copy(reinterpret_cast(&stamp.nsec), + reinterpret_cast(&stamp.nsec) + sizeof(stamp.nsec), + iter); + return generate(namespace_string, buffer.data(), buffer.size()); + } + + /** + * @brief Generate a UUID from a namespace string, a ros timestamp, and an additional id + * + * Every unique timestamp and id pair will generate a unique UUID + * + * @param[in] namespace_string A namespace or parent string used to generate non-overlapping UUIDs + * @param[in] stamp A ROS::Time timestamp + * @param[in] id A UUID + * @return A repeatable UUID specific to the provided namespace and timestamp + */ + inline UUID generate(const std::string& namespace_string, const ros::Time& stamp, const UUID& id) + { + constexpr size_t buffer_size = sizeof(stamp.sec) + sizeof(stamp.nsec) + UUID::static_size(); + std::array buffer; + auto iter = buffer.begin(); + iter = std::copy(reinterpret_cast(&stamp.sec), + reinterpret_cast(&stamp.sec) + sizeof(stamp.sec), + iter); + iter = std::copy(reinterpret_cast(&stamp.nsec), + reinterpret_cast(&stamp.nsec) + sizeof(stamp.nsec), + iter); + iter = std::copy(id.begin(), + id.end(), + iter); + return generate(namespace_string, buffer.data(), buffer.size()); + } +} // namespace uuid + +} // namespace fuse_core + +#endif // FUSE_CORE_UUID_H diff --git a/fuse_core/include/fuse_core/variable.h b/fuse_core/include/fuse_core/variable.h new file mode 100644 index 000000000..96ab42130 --- /dev/null +++ b/fuse_core/include/fuse_core/variable.h @@ -0,0 +1,180 @@ +/* + * 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_CORE_VARIABLE_H +#define FUSE_CORE_VARIABLE_H + +#include +#include + +#include +#include + +#include +#include +#include + + +namespace fuse_core +{ + +/** + * @brief The Variable interface definition. + * + * A Variable defines some semantically meaningful group of one or more individual scale values. Each variable is + * treated as a block by the optimization engine, as the values of all of its dimensions are likely to be involved + * in the same constraints. Some common examples of variable groupings are a 2D point (x, y), 3D point (x, y, z), or + * camera calibration parameters (fx, fy, cx, cy). + * + * To support the Ceres optimization engine, the Variable must hold the scalar values of each dimension in a + * _contiguous_ memory space, and must provide access to that memory location via the Variable::data() methods. + * + * Some Variables may require special update rules, either because they are over-parameterized, as is the case with + * 3D rotations represented as quaternions, or because the update of the individual dimensions exhibit some nonlinear + * properties, as is the case with rotations in general (e.g. 2D rotations have a discontinuity around π). To + * support these situations, Ceres uses an optional "local parameterization". See the Ceres documentation for more + * details. http://ceres-solver.org/nnls_modeling.html#localparameterization + */ +class Variable +{ +public: + SMART_PTR_ALIASES_ONLY(Variable); + + /** + * @brief Constructor + */ + Variable() = default; + + /** + * @brief Destructor + */ + virtual ~Variable() = default; + + /** + * @brief Returns a unique name for this variable type. + * + * The variable type string must be unique for each class. As such, the fully-qualified class name is an excellent + * choice for the type string. + */ + virtual std::string type() const { return boost::core::demangle(typeid(*this).name()); } + + /** + * @brief Returns a UUID for this variable. + * + * The implemented UUID generation should be deterministic such that a variable with the same metadata will always + * return the same UUID. Identical UUIDs produced by sensors will be treated as the same variable by the optimizer, + * and different UUIDs will be treated as different variables. So, two derived variables representing robot poses with + * the same timestamp but different UUIDs will incorrectly be treated as different variables, and two robot poses with + * different timestamps but the same UUID will be incorrectly treated as the same variable. + * + * One method of producing UUIDs that adhere to this requirement is to use the boost::uuid::name_generator() function. + * The type() string can be used to generate a UUID namespace for all variables of a given derived type, and the + * variable metadata of consequence can be converted into a carefully-formatted string or byte array and provided to + * the generator to create the UUID for a specific variable instance. + */ + virtual UUID uuid() const = 0; + + /** + * @brief Returns the number of elements of this variable. + * + * In most cases, this will be the number of degrees of freedom this variable represents. For example, a 2D pose has + * an x, y, and theta value, so the size will be 3. A notable exception is a 3D rotation represented as a quaternion. + * It only has 3 degrees of freedom, but it is represented as four elements, (w, x, y, z), so it's size will be 4. + */ + virtual size_t size() const = 0; + + /** + * @brief Read-only access to the variable data + * + * The data elements must be contiguous (such as a C-style array double[3] or std::vector), and it must + * contain at least Variable::size() elements. Only Variable::size() elements will be accessed externally. This + * interface is provided for integration with Ceres, which uses raw pointers. + */ + virtual const double* data() const = 0; + + /** + * @brief Read-write access to the variable data + * + * The data elements must be contiguous (such as a C-style array double[3] or std::vector), and it must + * contain at least Variable::size() elements. Only Variable::size() elements will be accessed externally. This + * interface is provided for integration with Ceres, which uses raw pointers. + */ + virtual double* data() = 0; + + /** + * @brief Print a human-readable description of the variable to the provided stream. + * + * @param[out] stream The stream to write to. Defaults to stdout. + */ + virtual void print(std::ostream& stream = std::cout) const = 0; + + /** + * @brief Perform a deep copy of the Variable and return a unique pointer to the copy + * + * Unique pointers can be implicitly upgraded to shared pointers if needed. + * + * This can/should be implemented as follows in all derived classes: + * @code{.cpp} + * return Derived::make_unique(*this); + * @endcode + * + * @return A unique pointer to a new instance of the most-derived Variable + */ + virtual Variable::UniquePtr clone() const = 0; + + /** + * @brief Create a new Ceres local parameterization object to apply to updates of this variable + * + * If a local parameterization is not needed, a null pointer should be returned. + * + * The Ceres interface requires a raw pointer. Ceres will take ownership of the pointer and promises to properly + * delete the local parameterization when it is done. Additionally, fuse promises that the Variable object will + * outlive any generated local parameterization (i.e. the Ceres objects will be destroyed before the Variable + * objects). This guarantee may allow optimizations for the creation of the local parameterization objects. + * + * @return A base pointer to an instance of a derived LocalParameterization + */ + virtual ceres::LocalParameterization* localParameterization() const + { + return nullptr; + } +}; + +/** + * Stream operator implementation used for all derived Constraint classes. + */ +std::ostream& operator <<(std::ostream& stream, const Variable& variable); + +} // namespace fuse_core + +#endif // FUSE_CORE_VARIABLE_H diff --git a/fuse_core/package.xml b/fuse_core/package.xml new file mode 100644 index 000000000..dacfe6208 --- /dev/null +++ b/fuse_core/package.xml @@ -0,0 +1,18 @@ + + + fuse_core + 0.0.0 + + The fuse_core package provides the base class interfaces for the various fuse components. Concrete implementations of these + interfaces are provided in other packages. + + + Stephen Williams + Stephen Williams + BSD + + catkin + ceres-solver + roscpp + roslint + diff --git a/fuse_core/src/variable.cpp b/fuse_core/src/variable.cpp new file mode 100644 index 000000000..b77b96afe --- /dev/null +++ b/fuse_core/src/variable.cpp @@ -0,0 +1,46 @@ +/* + * 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. + */ +#include + + +namespace fuse_core +{ + +std::ostream& operator <<(std::ostream& stream, const Variable& variable) +{ + variable.print(stream); + return stream; +} + +} // namespace fuse_core