Class RangeConstraint

• Defined in File range_constraint.hpp

Inheritance Relationships

Base Type

• public fuse_core::Constraint

Class Documentation

class RangeConstraint : public fuse_core::Constraint

Implements a range-only measurement constraint between the robot and a beacon.

The main purpose for this constraint is to demonstrate how to write your own cost constraint classes.

For the purposes of this tutorial, let’s imagine that you have developed some new robotic sensor that is capable of measuring the distance to some sort of beacon, but does not provide any information about the bearing/heading to that beacon. None of the fuse packages provide such a sensor model, so you need to develop one yourself.

The “sensor model” class provides an interface to ROS, allowing sensor messages to be received. The sensor model class also acts as a “factory” (in a programming sense) that creates new sensor constraints for each received sensor measurement. The constraint class does a number of things:

• Represents a single measurement, and likely holds the actual measured value

• Defines exactly which variable instances are involved in this measurement model

• Defines any robust loss function that should be applied to the computed costs

• Generates instances of a Ceres Solver cost function that implements the desired sensor measurement model

This range-only constraint will use the mathematical model defined in the RangeCostFunctor, and Ceres Solver’s automatic differentiation system to create a Ceres Solver cost function.

Public Functions

RangeConstraint() = default

Default constructor.

A default constructor is required to support the serialize/deserialize functionality.

RangeConstraint(const std::string &source, const fuse_variables::Position2DStamped &robot_position, const fuse_variables::Point2DLandmark &beacon_position, const double z, const double sigma)

Create a range-only constraint between the provided robot position and the beacon position.

This is the constructor that will be used from within the RangeSensorModel. It accepts references to the variables involved with this specific measurement — the robot position at the time the measurement was sampled, and the beacon that was measured.

Note that, when measuring subset of dimensions, empty axis vectors are permitted. This signifies, e.g., that you don’t want to measure any of the quantities in that variable.

The mean is given as a vector. The first components (if any) will be dictated, both in content and in ordering, by the value of the linear_indices. The final component (if any) is dictated by the angular_indices. The covariance matrix follows the same ordering.

Parameters:

• source – [in] - The name of the sensor that generated this constraint. This is largely information to aid in debugging or visualizing the system. If multiple sensors of the same type exist, being able to disambiguate the constraints from sensor1 versus sensor2 is useful.

• robot_position – [in] - The 2D position of the robot at the time the measurement was sampled

• beacon_position – [in] - The 2D position of the sampled beacon

• z – [in] - The distance measured between the robot and beacon by our new sensor

• sigma – [in] - The uncertainty of measured distance

void print(std::ostream &stream = std::cout) const override

Print a human-readable description of the constraint to the provided stream.

This is required by the fuse_core::Constraint base class, but is largely just for debugging and visualization.

Parameters:

stream – [out] - The stream to write to. Defaults to stdout.

ceres::CostFunction *costFunction() const override

Construct an instance of this constraint’s cost function.

This is the most important operation of a Constraint — to create a Ceres Solver CostFunction object that is then optimized by the Ceres Solver least-squares solver. This implementation uses the RangeCostFunctor and the Ceres Solver AutoDiffCostFunction class to automatically compute the cost function Jacobians. This is also where the sizes of the input variables and output cost array is defined.

Returns:

A base pointer to an instance of a derived Ceres Solver CostFunction. Ownership of the CostFunction object is transferred Ceres Solver; Ceres Solver will delete the CostFunction object when it is done. Also note that the fuse project guarantees the derived Constraint object will outlive the Ceres Solver CostFunction object.

Friends

friend class boost::serialization::access