transform_with_variance.h
Go to the documentation of this file.
1 #ifndef TRANSFORM_VARIANCE_H
2 #define TRANSFORM_VARIANCE_H
3 
4 #include <geometry_msgs/PoseWithCovarianceStamped.h>
9 
11 public:
13  double variance;
14 
15  TransformWithVariance() = default;
16 
17  // Constructors that make a transform out of the different implementations
18  TransformWithVariance(const tf2::Transform& t, double var) : transform(t), variance(var){};
19  TransformWithVariance(const geometry_msgs::Transform& t, double var) : variance(var) {
20  fromMsg(t, transform);
21  };
22  TransformWithVariance(const tf2::Vector3& tvec, const tf2::Quaternion& q, double var)
23  : transform(q, tvec), variance(var){};
24 
25  // Used to combine this transform with another one increasing total variance
27  // Update this transform, increasing variance
28  transform *= rhs.transform;
29 
30  // Do simple addition of the variances
31  // In a multi-variate case RMS may be more correct
32  variance += rhs.variance;
33  return *this;
34  }
36  const TransformWithVariance& rhs) {
37  lhs *= rhs;
38  return lhs;
39  }
42  lhs *= rhs;
43  return lhs;
44  }
45 
46  // Used to combine this transform with another one keeping variance the same
48  transform *= rhs;
49  // No need to change the variance, we are assuming that rhs has variance of 0
50  return *this;
51  }
53  lhs *= rhs;
54  return lhs;
55  }
57  lhs *= rhs.transform;
58  return TransformWithVariance(lhs, rhs.variance);
59  }
60 
61  // Update this transform with a new one, with variances as weights
62  // combine variances using David method
63  void update(const TransformWithVariance& newT);
64 };
65 
66 // Weighted average of 2 transforms, with new variance
68  const TransformWithVariance& t2);
69 
70 inline geometry_msgs::PoseWithCovarianceStamped toPose(
72  geometry_msgs::PoseWithCovarianceStamped msg;
73  msg.header.stamp = in.stamp_;
74  msg.header.frame_id = in.frame_id_;
75 
76  toMsg(in.transform, msg.pose.pose);
77 
78  std::fill(msg.pose.covariance.begin(), msg.pose.covariance.end(), 0);
79 
80  for (int i = 0; i <= 5; i++) {
81  msg.pose.covariance[i * 6 + i] = in.variance; // Fill the diagonal
82  }
83 
84  return msg;
85 }
86 
87 inline geometry_msgs::TransformStamped toMsg(const tf2::Stamped<TransformWithVariance>& in) {
88  geometry_msgs::TransformStamped msg;
89  msg.header.stamp = in.stamp_;
90  msg.header.frame_id = in.frame_id_;
91  msg.transform = toMsg(in.transform);
92 
93  return msg;
94 }
95 
96 #endif
TransformWithVariance(const tf2::Vector3 &tvec, const tf2::Quaternion &q, double var)
friend tf2::Stamped< TransformWithVariance > operator*(tf2::Stamped< TransformWithVariance > lhs, const tf2::Stamped< TransformWithVariance > &rhs)
TransformWithVariance & operator*=(const tf2::Transform &rhs)
TransformWithVariance & operator*=(const TransformWithVariance &rhs)
TransformWithVariance(const geometry_msgs::Transform &t, double var)
geometry_msgs::TransformStamped toMsg(const tf2::Stamped< TransformWithVariance > &in)
TransformWithVariance(const tf2::Transform &t, double var)
ros::Time stamp_
TransformWithVariance averageTransforms(const TransformWithVariance &t1, const TransformWithVariance &t2)
void fromMsg(const A &, B &b)
friend TransformWithVariance operator*(TransformWithVariance lhs, const TransformWithVariance &rhs)
geometry_msgs::PoseWithCovarianceStamped toPose(const tf2::Stamped< TransformWithVariance > &in)
friend TransformWithVariance operator*(TransformWithVariance lhs, const tf2::Transform &rhs)
std::string frame_id_
friend TransformWithVariance operator*(tf2::Transform lhs, const TransformWithVariance &rhs)
TransformWithVariance()=default
void update(const TransformWithVariance &newT)


fiducial_slam
Author(s): Jim Vaughan
autogenerated on Sat Jun 13 2020 04:04:52