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;
Computes weighted and un-weighted averages of SO(2) orientations.
static Derived Identity()
Computes weighted and un-weighted averages of SE(2) or SE(3) poses.
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
Sets the 3x3 rotation matrix.
CMatrixFixed< double, 3, 3 > CMatrixDouble33
This base provides a set of functions for maths stuff.
double phi() const
Get the phi angle of the 2D pose (in radians)
int sign(T x)
Returns the sign of X as "1" or "-1".
double x() const
Common members of all points & poses classes.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
#define ASSERT_ABOVE_(__A, __B)
Computes weighted and un-weighted averages of SO(3) orientations.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Computes weighted and un-weighted averages of SO(2) or SO(3) orientations.
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
void clear()
Clear the contents of this container.
double phi
Orientation (rads)