30 #ifndef MCL_3DL_STATE_6DOF_H 31 #define MCL_3DL_STATE_6DOF_H 72 RPYVec(
const float r,
const float p,
const float y)
99 return odom_err_integ_lin_.
x_;
101 return odom_err_integ_lin_.
y_;
103 return odom_err_integ_lin_.
z_;
105 return odom_err_integ_ang_.
x_;
107 return odom_err_integ_ang_.
y_;
109 return odom_err_integ_ang_.
z_;
134 return odom_err_integ_lin_.
x_;
136 return odom_err_integ_lin_.
y_;
138 return odom_err_integ_lin_.
z_;
140 return odom_err_integ_ang_.
x_;
142 return odom_err_integ_ang_.
y_;
144 return odom_err_integ_ang_.
z_;
161 noise_ll_ = noise_la_ = noise_aa_ = noise_al_ = 0.0;
169 noise_ll_ = noise_la_ = noise_aa_ = noise_al_ = 0.0;
178 noise_ll_ = noise_la_ = noise_aa_ = noise_al_ = 0.0;
187 template <
typename Po
intType>
191 for (
auto& p : pc.points)
199 template <
typename T,
typename RANDOM_ENGINE,
typename NOISE_GEN>
202 if (gen.getDimension() != 6)
204 ROS_ERROR(
"Dimension of noise must be 6. Passed: %lu", gen.getDimension());
207 const std::vector<float> org_noise = gen(engine);
208 const std::vector<float>& mean = gen.getMean();
209 for (
size_t i = 0; i < 3; i++)
211 noise[i] = noise[i + 7] = org_noise[i];
214 for (
size_t i = 0; i < 3; i++)
216 rpy_noise[i] = org_noise[i + 3];
217 noise[i + 10] = org_noise[i + 3] - mean[i + 3];
226 for (
size_t i = 0; i <
size(); i++)
228 if (3 <= i && i <= 6)
230 ret[i] = (*this)[i] + in[i];
239 for (
size_t i = 0; i <
size(); i++)
241 if (3 <= i && i <= 6)
243 ret[i] = (*this)[i] - in[i];
257 ROS_ERROR(
"Failed to generate noise. mean must be mcl_3dl::Quat.");
259 for (
size_t i = 0; i < 3; i++)
264 for (
size_t i = 0; i < 3; i++)
266 mean_[i + 3] = rpy[i];
277 ROS_ERROR(
"Failed to generate noise. sigma must be rpy vec.");
279 for (
size_t i = 0; i < 3; i++)
281 sigma_[i] = sigma[i];
283 for (
size_t i = 0; i < 3; i++)
285 sigma_[i + 3] = sigma.
rpy_.
v_[i];
297 : ParticleWeightedMean()
298 , front_sum_(0.0, 0.0, 0.0)
299 , up_sum_(0.0, 0.0, 0.0)
319 assert(p_sum_ > 0.0);
331 #endif // MCL_3DL_STATE_6DOF_H
void transform(pcl::PointCloud< PointType > &pc) const
float & operator[](const size_t i) override
static State6DOF generateNoise(RANDOM_ENGINE &engine, const NOISE_GEN &gen)
void setSigma(const T &sigma)
State6DOF operator-(const State6DOF &a) const
float getTotalProbability()
mcl_3dl::Vec3 odom_err_integ_ang_
mcl_3dl::Vec3 odom_err_integ_lin_
State6DOF operator+(const State6DOF &a) const
State6DOF(const mcl_3dl::Vec3 pos, const mcl_3dl::Vec3 rpy)
geometry_msgs::TransformStamped t
void add(const State6DOF &s, const float &prob)
size_t size() const override
float operator[](const size_t i) const
RPYVec(const mcl_3dl::Vec3 &v)
constexpr Vec3 getRPY() const
constexpr Quat inv() const
ParticleWeightedMeanQuat()
void normalize() override
void setMean(const T &mean)
RPYVec(const float r, const float p, const float y)
State6DOF(const mcl_3dl::Vec3 pos, const mcl_3dl::Quat rot)