20 #ifndef COVARIANCE_INSERTION__ADD_COVARIANCE_HPP_
21 #define COVARIANCE_INSERTION__ADD_COVARIANCE_HPP_
31 namespace covariance_insertion
41 template<
typename MsgT,
typename ScalarT>
44 const std::vector<ScalarT> & covariance,
49 throw std::runtime_error(
"Message has covariance directly, but asked for field: " + field);
51 if (msg->covariance.size() != covariance.size()) {
52 throw std::runtime_error(
53 "Number of covariance entries does not match. The message has " +
54 std::to_string(msg->covariance.size()) +
" entries, while there are " +
55 std::to_string(covariance.size()) +
" entries in parameters of this node.");
57 for (
auto i = 0U; i < covariance.size(); ++i) {
58 msg->covariance[i] = covariance[i];
62 template<
typename MsgT,
typename ScalarT>
65 const std::vector<ScalarT> & covariance,
66 const std::string & field)
76 template<
typename MsgT,
typename ScalarT>
79 const std::vector<ScalarT> & covariance,
80 const std::enable_if_t<
87 throw std::runtime_error(
"Cannot set: " + field);
92 template<
typename MsgT,
typename ScalarT>
95 const std::vector<ScalarT> & covariance,
96 const std::enable_if_t<
103 throw std::runtime_error(
"Cannot set: " + field);
108 template<
typename MsgT,
typename ScalarT>
111 const std::vector<ScalarT> & covariance,
112 const std::enable_if_t<
123 throw std::runtime_error(
"Cannot set: " + field);
130 #endif // COVARIANCE_INSERTION__ADD_COVARIANCE_HPP_