15 #include <Eigen/Dense>
27 m_accum_x = m_accum_y = .0;
31 append(orientation_rad, 1.0);
36 m_accum_x += cos(orientation_rad) * weight;
37 m_accum_y += sin(orientation_rad) * weight;
42 const double x = m_accum_x / m_count;
43 const double y = m_accum_y / m_count;
45 double ang = atan2(y, x);
48 if (enable_exception_on_undeterminate)
49 throw std::runtime_error(
50 "[SO_average<2>::get_average()] Undetermined average value");
62 m_accum_rot.setZero();
72 m_accum_rot.asEigen() += weight * M.
asEigen();
79 const Eigen::Matrix3d MtM = m_accum_rot.transpose() * m_accum_rot.asEigen();
81 Eigen::JacobiSVD<Eigen::Matrix3d> svd(MtM, Eigen::ComputeFullU);
82 const Eigen::Vector3d vs = svd.singularValues();
85 const double d1 = 1.0 / sqrt(vs[0]);
86 const double d2 = 1.0 / sqrt(vs[1]);
87 const double d3 =
mrpt::sign(m_accum_rot.det()) / sqrt(vs[2]);
90 if (enable_exception_on_undeterminate)
91 throw std::runtime_error(
92 "[SO_average<3>::get_average()] Undetermined average value");
102 m_accum_rot.asEigen() * svd.matrixU() * D.
asEigen() *
103 svd.matrixU().transpose());
111 m_accum_x = m_accum_y = .0;
118 m_accum_x += weight * p.
x();
119 m_accum_y += weight * p.
y();
120 m_rot_part.append(p.
phi(), weight);
125 m_accum_x += weight * p.
x;
126 m_accum_y += weight * p.
y;
127 m_rot_part.append(p.
phi, weight);
132 ret_mean.
x(m_accum_x / m_count);
133 ret_mean.
y(m_accum_y / m_count);
134 const_cast<SO_average<2>*
>(&m_rot_part)->enable_exception_on_undeterminate =
135 this->enable_exception_on_undeterminate;
136 ret_mean.
phi(m_rot_part.get_average());
144 m_accum_x = m_accum_y = m_accum_z = .0;
151 m_accum_x += weight * p.
x();
152 m_accum_y += weight * p.
y();
153 m_accum_z += weight * p.z();
163 ret_mean.
x(m_accum_x / m_count);
164 ret_mean.
y(m_accum_y / m_count);
165 ret_mean.z(m_accum_z / m_count);
166 const_cast<SO_average<3>*
>(&m_rot_part)->enable_exception_on_undeterminate =
167 this->enable_exception_on_undeterminate;