|
| 1 | +/* |
| 2 | + * Software License Agreement (BSD License) |
| 3 | + * |
| 4 | + * Copyright (c) 2018, Locus Robotics |
| 5 | + * All rights reserved. |
| 6 | + * |
| 7 | + * Redistribution and use in source and binary forms, with or without |
| 8 | + * modification, are permitted provided that the following conditions |
| 9 | + * are met: |
| 10 | + * |
| 11 | + * * Redistributions of source code must retain the above copyright |
| 12 | + * notice, this list of conditions and the following disclaimer. |
| 13 | + * * Redistributions in binary form must reproduce the above |
| 14 | + * copyright notice, this list of conditions and the following |
| 15 | + * disclaimer in the documentation and/or other materials provided |
| 16 | + * with the distribution. |
| 17 | + * * Neither the name of the copyright holder nor the names of its |
| 18 | + * contributors may be used to endorse or promote products derived |
| 19 | + * from this software without specific prior written permission. |
| 20 | + * |
| 21 | + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
| 22 | + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
| 23 | + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
| 24 | + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
| 25 | + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
| 26 | + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
| 27 | + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
| 28 | + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
| 29 | + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
| 30 | + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
| 31 | + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 32 | + * POSSIBILITY OF SUCH DAMAGE. |
| 33 | + */ |
| 34 | +#ifndef FUSE_CONSTRAINTS_ABSOLUTE_CONSTRAINT_H |
| 35 | +#define FUSE_CONSTRAINTS_ABSOLUTE_CONSTRAINT_H |
| 36 | + |
| 37 | +#include <fuse_core/constraint.h> |
| 38 | +#include <fuse_core/macros.h> |
| 39 | +#include <fuse_core/uuid.h> |
| 40 | +#include <fuse_variables/acceleration_angular_2d_stamped.h> |
| 41 | +#include <fuse_variables/acceleration_linear_2d_stamped.h> |
| 42 | +#include <fuse_variables/orientation_2d_stamped.h> |
| 43 | +#include <fuse_variables/position_2d_stamped.h> |
| 44 | +#include <fuse_variables/position_3d_stamped.h> |
| 45 | +#include <fuse_variables/velocity_angular_2d_stamped.h> |
| 46 | +#include <fuse_variables/velocity_linear_2d_stamped.h> |
| 47 | + |
| 48 | +#include <ceres/cost_function.h> |
| 49 | +#include <Eigen/Core> |
| 50 | + |
| 51 | +#include <ostream> |
| 52 | +#include <vector> |
| 53 | + |
| 54 | + |
| 55 | +namespace fuse_constraints |
| 56 | +{ |
| 57 | + |
| 58 | +/** |
| 59 | + * @brief A constraint that represents prior information about a variable or a direct measurement of the variable. |
| 60 | + * |
| 61 | + * This type of constraint arises in many situations. In mapping it is common to define the very first pose as the |
| 62 | + * origin. Some sensors, such as an IMU, provide direct measurements of certain state variables (e.g. linear |
| 63 | + * acceleration). And localization systems often match laserscans to a prior map (scan-to-map measurements). |
| 64 | + * This constraint holds the measured variable value and the measurement uncertainty/covariance. |
| 65 | + */ |
| 66 | +template<class Variable> |
| 67 | +class AbsoluteConstraint final : public fuse_core::Constraint |
| 68 | +{ |
| 69 | +public: |
| 70 | + SMART_PTR_DEFINITIONS(AbsoluteConstraint<Variable>); |
| 71 | + |
| 72 | + /** |
| 73 | + * @brief Create a constraint using a measurement/prior of all dimensions of the target variable |
| 74 | + * |
| 75 | + * @param[in] variable An object derived from fuse_core::Variable. |
| 76 | + * @param[in] mean The measured/prior value of all variable dimensions |
| 77 | + * @param[in] covariance The measurement/prior uncertainty of all variable dimensions |
| 78 | + */ |
| 79 | + AbsoluteConstraint( |
| 80 | + const Variable& variable, |
| 81 | + const Eigen::VectorXd& mean, |
| 82 | + const Eigen::MatrixXd& covariance); |
| 83 | + |
| 84 | + /** |
| 85 | + * @brief Create a constraint using a measurement/prior of only a partial set of dimensions of the target variable |
| 86 | + * |
| 87 | + * @param[in] variable An object derived from fuse_core::Variable. |
| 88 | + * @param[in] partial_mean The measured value of the subset of dimensions in the order defined by \p indices |
| 89 | + * @param[in] partial_covariance The uncertainty of the subset of dimensions in the order defined by \p indices. |
| 90 | + * @param[in] indices The set of indices corresponding to the measured dimensions |
| 91 | + */ |
| 92 | + AbsoluteConstraint( |
| 93 | + const Variable& variable, |
| 94 | + const Eigen::VectorXd& partial_mean, |
| 95 | + const Eigen::MatrixXd& partial_covariance, |
| 96 | + const std::vector<size_t>& indices); |
| 97 | + |
| 98 | + /** |
| 99 | + * @brief Destructor |
| 100 | + */ |
| 101 | + virtual ~AbsoluteConstraint() = default; |
| 102 | + |
| 103 | + /** |
| 104 | + * @brief Read-only access to the measured/prior vector of mean values. |
| 105 | + * |
| 106 | + * All dimensions are present, even if only a partial set of dimensions were measured. Dimensions are in the order |
| 107 | + * defined by the variable, not the order defined by the \p indices parameter. All unmeasured variable dimensions |
| 108 | + * are set to zero. |
| 109 | + */ |
| 110 | + const Eigen::VectorXd& mean() const { return mean_; } |
| 111 | + |
| 112 | + /** |
| 113 | + * @brief Read-only access to the square root information matrix. |
| 114 | + * |
| 115 | + * Dimensions are in the order defined by the variable, not the order defined by the \p indices parameter. The |
| 116 | + * square root information matrix will have size measured_dimensions X variable_dimensions. If only a partial set |
| 117 | + * of dimensions are measured, then this matrix will not be square. |
| 118 | + */ |
| 119 | + const Eigen::MatrixXd& sqrtInformation() const { return sqrt_information_; } |
| 120 | + |
| 121 | + /** |
| 122 | + * @brief Compute the measurement covariance matrix. |
| 123 | + * |
| 124 | + * Dimensions are in the order defined by the variable, not the order defined by the \p indices parameter. The |
| 125 | + * covariance matrix will always be square, with size variable_dimensions X variable_dimensions. If only a |
| 126 | + * subset of dimensions are measured, then some rows/columns will be zero. This will result in a rank-deficient |
| 127 | + * covariance matrix. You have been warned. |
| 128 | + */ |
| 129 | + Eigen::MatrixXd covariance() const; |
| 130 | + |
| 131 | + /** |
| 132 | + * @brief Print a human-readable description of the constraint to the provided stream. |
| 133 | + * |
| 134 | + * @param[out] stream The stream to write to. Defaults to stdout. |
| 135 | + */ |
| 136 | + void print(std::ostream& stream = std::cout) const override; |
| 137 | + |
| 138 | + /** |
| 139 | + * @brief Perform a deep copy of the constraint and return a unique pointer to the copy |
| 140 | + * |
| 141 | + * Unique pointers can be implicitly upgraded to shared pointers if needed. |
| 142 | + * |
| 143 | + * @return A unique pointer to a new instance of the most-derived constraint |
| 144 | + */ |
| 145 | + fuse_core::Constraint::UniquePtr clone() const override; |
| 146 | + |
| 147 | + /** |
| 148 | + * @brief Construct an instance of this constraint's cost function |
| 149 | + * |
| 150 | + * The function caller will own the new cost function instance. It is the responsibility of the caller to delete |
| 151 | + * the cost function object when it is no longer needed. If the pointer is provided to a Ceres::Problem object, the |
| 152 | + * Ceres::Problem object will takes ownership of the pointer and delete it during destruction. |
| 153 | + * |
| 154 | + * @return A base pointer to an instance of a derived CostFunction. |
| 155 | + */ |
| 156 | + ceres::CostFunction* costFunction() const override; |
| 157 | + |
| 158 | +protected: |
| 159 | + Eigen::VectorXd mean_; //!< The measured/prior mean vector for this variable |
| 160 | + Eigen::MatrixXd sqrt_information_; //!< The square root information matrix |
| 161 | +}; |
| 162 | + |
| 163 | +// Define unique names for the different variations of the absolute constraint |
| 164 | +using AbsoluteAccelerationAngular2DStampedConstraint = AbsoluteConstraint<fuse_variables::AccelerationAngular2DStamped>; |
| 165 | +using AbsoluteAccelerationLinear2DStampedConstraint = AbsoluteConstraint<fuse_variables::AccelerationLinear2DStamped>; |
| 166 | +using AbsoluteOrientation2DStampedConstraint = AbsoluteConstraint<fuse_variables::Orientation2DStamped>; |
| 167 | +using AbsolutePosition2DStampedConstraint = AbsoluteConstraint<fuse_variables::Position2DStamped>; |
| 168 | +using AbsolutePosition3DStampedConstraint = AbsoluteConstraint<fuse_variables::Position3DStamped>; |
| 169 | +using AbsoluteVelocityAngular2DStampedConstraint = AbsoluteConstraint<fuse_variables::VelocityAngular2DStamped>; |
| 170 | +using AbsoluteVelocityLinear2DStampedConstraint = AbsoluteConstraint<fuse_variables::VelocityLinear2DStamped>; |
| 171 | +} // namespace fuse_constraints |
| 172 | + |
| 173 | +// Include the template implementation |
| 174 | +#include <fuse_constraints/absolute_constraint_impl.h> |
| 175 | + |
| 176 | +#endif // FUSE_CONSTRAINTS_ABSOLUTE_CONSTRAINT_H |
0 commit comments