merged from ign-math6

Signed-off-by: Louise Poubel <[email protected]>

examples/CMakeLists.txt

@@ -31,5 +31,8 @@ target_link_libraries(rand_example ignition-math${IGN_MATH_VER}::ignition-math${
 add_executable(graph_example graph_example.cc)
 target_link_libraries(graph_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER})
 
+add_executable(kmeans kmeans.cc)
+target_link_libraries(kmeans ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER})
+
 add_executable(color_example color_example.cc)
 target_link_libraries(color_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER})

examples/kmeans.cc

@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <iostream>
+#include <vector>
+
+#include <ignition/math/Kmeans.hh>
+
+int main(int argc, char **argv)
+{
+  // Create some observations.
+  std::vector<ignition::math::Vector3d> obs;
+  obs.push_back(ignition::math::Vector3d(1.0, 1.0, 0.0));
+  obs.push_back(ignition::math::Vector3d(1.1, 1.0, 0.0));
+  obs.push_back(ignition::math::Vector3d(1.2, 1.0, 0.0));
+  obs.push_back(ignition::math::Vector3d(1.3, 1.0, 0.0));
+  obs.push_back(ignition::math::Vector3d(1.4, 1.0, 0.0));
+  obs.push_back(ignition::math::Vector3d(5.0, 1.0, 0.0));
+  obs.push_back(ignition::math::Vector3d(5.1, 1.0, 0.0));
+  obs.push_back(ignition::math::Vector3d(5.2, 1.0, 0.0));
+  obs.push_back(ignition::math::Vector3d(5.3, 1.0, 0.0));
+  obs.push_back(ignition::math::Vector3d(5.4, 1.0, 0.0));
+
+  // Initialize Kmeans with two partitions.
+  ignition::math::Kmeans kmeans(obs);
+
+  std::vector<ignition::math::Vector3d> obsCopy;
+  obsCopy = kmeans.Observations();
+
+  for (auto &elem : obsCopy)
+    elem += ignition::math::Vector3d(0.1, 0.2, 0.0);
+
+  // append more observations
+  kmeans.AppendObservations(obsCopy);
+
+  // cluster
+  std::vector<ignition::math::Vector3d> centroids;
+  std::vector<unsigned int> labels;
+  auto result = kmeans.Cluster(2, centroids, labels);
+
+  // Check that there are two centroids.
+  std::cout << "Is there a solution ? " << result << std::endl;
+  std::cout << "There are " << centroids.size() << " centroids" << std::endl;
+  std::cout << "labels: [";
+  for (auto &label: labels)
+  {
+    std::cout << label << ", ";
+  }
+  std::cout << "]" << '\n';
+}

examples/kmeans.py

@@ -0,0 +1,51 @@
+# Copyright (C) 2021 Open Source Robotics Foundation
+#
+# Licensed under the Apache License, Version 2.0 (the "License")
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#       http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+from ignition.math import Kmeans, Vector3d
+
+# Create some observations.
+obs = list([])
+obs.append(Vector3d(1.0, 1.0, 0.0))
+obs.append(Vector3d(1.1, 1.0, 0.0))
+obs.append(Vector3d(1.2, 1.0, 0.0))
+obs.append(Vector3d(1.3, 1.0, 0.0))
+obs.append(Vector3d(1.4, 1.0, 0.0))
+obs.append(Vector3d(5.0, 1.0, 0.0))
+obs.append(Vector3d(5.1, 1.0, 0.0))
+obs.append(Vector3d(5.2, 1.0, 0.0))
+obs.append(Vector3d(5.3, 1.0, 0.0))
+obs.append(Vector3d(5.4, 1.0, 0.0))
+
+# Initialize Kmeans with two partitions.
+kmeans = Kmeans(obs)
+
+# copy original Vector to add more data
+obs_copy = list(kmeans.observations()).copy()
+obs2 = list([])
+
+for idx, a in enumerate(obs_copy):
+    obs_copy[idx] = a + Vector3d(0.1, 0.2, 0.0)
+    obs2.append(obs_copy[idx])
+
+# Append more observations
+kmeans.append_observations(obs2)
+
+# Calling cluster
+result, centroids, labels = kmeans.cluster(2)
+
+# Check that there are two centroids.
+print("Is there a solution ? {}".format(result));
+print("There are {} centroids".format(len(centroids)))
+print("labels:", labels)

include/ignition/math/PID.hh

@@ -100,7 +100,7 @@ namespace ignition
       /// \param[in] _i integral gain value
       public: void SetIGain(const double _i);
 
-      /// \brief Set the derivtive Gain.
+      /// \brief Set the derivative Gain.
       /// \param[in] _d derivative gain value
       public: void SetDGain(const double _d);
 
@@ -149,7 +149,7 @@ namespace ignition
       /// \return The maximum value
       public: double CmdMax() const;
 
-      /// \brief Get the maximum value for the command.
+      /// \brief Get the minimun value for the command.
       /// \return The maximum value
       public: double CmdMin() const;
 

include/ignition/math/Quaternion.hh

@@ -449,8 +449,8 @@ namespace ignition
       }
 
       /// \brief Return rotation as axis and angle
-      /// \param[in] _axis rotation axis
-      /// \param[in] _angle ccw angle in radians
+      /// \param[out] _axis rotation axis
+      /// \param[out] _angle ccw angle in radians
       public: void ToAxis(Vector3<T> &_axis, T &_angle) const
       {
         T len = this->qx*this->qx + this->qy*this->qy + this->qz*this->qz;
@@ -590,7 +590,7 @@ namespace ignition
         }
       }
 
-      /// \brief Scale a Quaternion<T>ion
+      /// \brief Scale a Quaternion<T>
       /// \param[in] _scale Amount to scale this rotation
       public: void Scale(T _scale)
       {

include/ignition/math/Spline.hh

@@ -58,13 +58,13 @@ namespace ignition
       /// \return arc length or INF on error.
       public: double ArcLength() const;
 
-      /// \brief Gets spline arc length up to
+      /// \brief Sets spline arc length up to
       /// a given parameter value \p _t.
       /// \param[in] _t parameter value (range 0 to 1).
       /// \return arc length up to \p _t or INF on error.
       public: double ArcLength(const double _t) const;
 
-      /// \brief Gets a spline segment arc length.
+      /// \brief Sets a spline segment arc length.
       /// \param[in] _index of the spline segment.
       /// \param[in] _t parameter value (range 0 to 1).
       /// \return arc length of a given segment up to

src/python_pybind11/CMakeLists.txt

@@ -29,6 +29,7 @@ if (${pybind11_FOUND})
     src/RotationSpline.cc
     src/SemanticVersion.cc
     src/SignalStats.cc
+    src/SphericalCoordinates.cc
     src/Spline.cc
     src/StopWatch.cc
     src/Temperature.cc
@@ -115,6 +116,7 @@ if (${pybind11_FOUND})
       SemanticVersion_TEST
       SignalStats_TEST
       Sphere_TEST
+      SphericalCoordinates_TEST
       Spline_TEST
       StopWatch_TEST
       Temperature_TEST

src/python_pybind11/src/SphericalCoordinates.cc

@@ -0,0 +1,132 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#include <pybind11/operators.h>
+
+#include "SphericalCoordinates.hh"
+#include <ignition/math/Angle.hh>
+#include <ignition/math/SphericalCoordinates.hh>
+
+namespace ignition
+{
+namespace math
+{
+namespace python
+{
+void defineMathSphericalCoordinates(py::module &m, const std::string &typestr)
+{
+  using Class = ignition::math::SphericalCoordinates;
+  std::string pyclass_name = typestr;
+
+  py::class_<Class> sphericalCoordinates(m,
+                    pyclass_name.c_str(),
+                    py::buffer_protocol(),
+                    py::dynamic_attr());
+  sphericalCoordinates
+    .def(py::init<>())
+    .def(py::init<const Class&>())
+    .def(py::init<const Class::SurfaceType>())
+    .def(py::init<const Class::SurfaceType, const ignition::math::Angle &,
+                  const ignition::math::Angle &, const double,
+                  const ignition::math::Angle &>())
+    .def(py::self != py::self)
+    .def(py::self == py::self)
+    .def("spherical_from_local_position",
+         &Class::SphericalFromLocalPosition,
+         "Convert a Cartesian position vector to geodetic coordinates.")
+    .def("global_from_local_velocity",
+         &Class::GlobalFromLocalVelocity,
+         "Convert a Cartesian velocity vector in the local frame "
+         " to a global Cartesian frame with components East, North, Up")
+    .def("convert",
+         py::overload_cast<const std::string &>(&Class::Convert),
+         "Convert a string to a SurfaceType.")
+    .def("convert",
+         py::overload_cast<Class::SurfaceType>(&Class::Convert),
+         "Convert a SurfaceType to a string.")
+    .def("distance",
+         &Class::Distance,
+         "Get the distance between two points expressed in geographic "
+         "latitude and longitude. It assumes that both points are at sea level."
+         " Example: _latA = 38.0016667 and _lonA = -123.0016667) represents "
+         "the point with latitude 38d 0'6.00\"N and longitude 123d 0'6.00\"W.")
+    .def("surface",
+         &Class::Surface,
+         "Get SurfaceType currently in use.")
+    .def("latitude_reference",
+         &Class::LatitudeReference,
+         "Get reference geodetic latitude.")
+    .def("longitude_reference",
+         &Class::LongitudeReference,
+         "Get reference longitude.")
+    .def("elevation_reference",
+         &Class::ElevationReference,
+         "Get reference elevation in meters.")
+    .def("heading_offset",
+         &Class::HeadingOffset,
+         "Get heading offset for the reference frame, expressed as "
+         "angle from East to x-axis, or equivalently "
+         "from North to y-axis.")
+    .def("set_surface",
+         &Class::SetSurface,
+         "Set SurfaceType for planetary surface model.")
+    .def("set_latitude_reference",
+         &Class::SetLatitudeReference,
+         "Set reference geodetic latitude.")
+    .def("set_longitude_reference",
+         &Class::SetLongitudeReference,
+         "Set reference longitude.")
+    .def("set_elevation_reference",
+         &Class::SetElevationReference,
+         "Set reference elevation above sea level in meters.")
+    .def("set_heading_offset",
+         &Class::SetHeadingOffset,
+         "Set heading angle offset for the frame.")
+    .def("local_from_spherical_position",
+         &Class::LocalFromSphericalPosition,
+         "Convert a geodetic position vector to Cartesian coordinates.")
+    .def("local_from_global_velocity",
+         &Class::LocalFromGlobalVelocity,
+         "Convert a Cartesian velocity vector with components East, "
+         "North, Up to a local cartesian frame vector XYZ.")
+    .def("update_transformation_matrix",
+         &Class::UpdateTransformationMatrix,
+         "Update coordinate transformation matrix with reference location")
+    .def("position_transform",
+         &Class::PositionTransform,
+         "Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame "
+         "Spherical coordinates use radians, while the other frames use "
+         "meters.")
+    .def("velocity_transform",
+         &Class::VelocityTransform,
+         "Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame "
+         "Spherical coordinates use radians, while the other frames use "
+         "meters.");
+
+   py::enum_<Class::CoordinateType>(sphericalCoordinates, "CoordinateType")
+       .value("SPHERICAL", Class::CoordinateType::SPHERICAL)
+       .value("ECEF", Class::CoordinateType::ECEF)
+       .value("GLOBAL", Class::CoordinateType::GLOBAL)
+       .value("LOCAL", Class::CoordinateType::LOCAL)
+       .value("LOCAL2", Class::CoordinateType::LOCAL2)
+       .export_values();
+   py::enum_<Class::SurfaceType>(sphericalCoordinates, "SurfaceType")
+       .value("EARTH_WGS84", Class::SurfaceType::EARTH_WGS84)
+       .export_values();
+}
+}  // namespace python
+}  // namespace math
+}  // namespace ignition

src/python_pybind11/src/SphericalCoordinates.hh

@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef IGNITION_MATH_PYTHON__SPHERICALCOORDINATES_HH_
+#define IGNITION_MATH_PYTHON__SPHERICALCOORDINATES_HH_
+
+#include <pybind11/pybind11.h>
+#include <string>
+
+namespace py = pybind11;
+
+namespace ignition
+{
+namespace math
+{
+namespace python
+{
+/// Define a pybind11 wrapper for an ignition::math::SphericalCoordinates
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ * \param[in] typestr name of the type used by Python
+ */
+void defineMathSphericalCoordinates(py::module &m, const std::string &typestr);
+}  // namespace python
+}  // namespace math
+}  // namespace ignition
+
+#endif  // IGNITION_MATH_PYTHON__SPHERICALCOORDINATES_HH_

src/python_pybind11/src/_ignition_math_pybind11.cc

@@ -44,6 +44,7 @@
 #include "SemanticVersion.hh"
 #include "SignalStats.hh"
 #include "Sphere.hh"
+#include "SphericalCoordinates.hh"
 #include "Spline.hh"
 #include "StopWatch.hh"
 #include "Temperature.hh"
@@ -108,6 +109,9 @@ PYBIND11_MODULE(math, m)
 
   ignition::math::python::defineMathSemanticVersion(m, "SemanticVersion");
 
+  ignition::math::python::defineMathSphericalCoordinates(
+    m, "SphericalCoordinates");
+
   ignition::math::python::defineMathSpline(m, "Spline");
 
   ignition::math::python::defineMathStopwatch(m, "Stopwatch");