Add files via upload

Added references

Author: tipf

include/QuaternionCalc.h

@@ -37,6 +37,23 @@
 
 namespace libRSF
 {
+
+  /** @brief Converts a quaterion to Tait–Bryan angles
+   * From:
+   * Wikipedia contributors,
+   * "Conversion between quaternions and Euler angles"
+   * Wikipedia, The Free Encyclopedia,
+   * https://en.wikipedia.org/w/index.php?title=Conversion_between_quaternions_and_Euler_angles&oldid=883423661
+   * (accessed February 15, 2019).
+   *
+   * @param Quaternion  ceres quaternion with ordering w,x,y,z
+   * @param Yaw         rotation around z
+   * @param Pitch       rotation around y
+   * @param Roll        rotation around x
+   */
+  template<typename T>
+  inline void QuaternionToRPY(const T* Quaternion, T* Roll, T* Pitch, T* Yaw);
+
   /** @brief invert a quaternion
    *
    * @param Quaternion    ceres quaternion with ordering w,x,y,z
@@ -48,6 +65,36 @@ namespace libRSF
   template<typename T>
   inline T RotationBetween(const T* Vector1, const T* Vector2, T* Quaternion);
 
+  template<typename T>
+  inline void QuaternionToRPY(const T* Quaternion, T* Roll, T* Pitch, T* Yaw)
+  {
+
+    const T& Qw = Quaternion[0];
+    const T& Qx = Quaternion[1];
+    const T& Qy = Quaternion[2];
+    const T& Qz = Quaternion[3];
+
+    /** conversion from Wikipedia */
+    // roll (x-axis rotation)
+    T sinr_cosp = +2.0 * (Qw * Qx + Qy * Qz);
+    T cosr_cosp = +1.0 - 2.0 * (Qx * Qx + Qy * Qy);
+    Roll[0] = ceres::atan2(sinr_cosp, cosr_cosp);
+
+    // pitch (y-axis rotation)
+    T sinp = +2.0 * (Qw * Qy - Qz * Qx);
+    if (sinp >= T(1))
+      Pitch[0] = T(M_PI / 2); // use 90 degrees if out of range
+    else if (sinp <= T(-1))
+      Pitch[0] = T(-M_PI / 2);
+    else
+      Pitch[0] = ceres::asin(sinp);
+
+    // yaw (z-axis rotation)
+    T siny_cosp = +2.0 * (Qw * Qz + Qx * Qy);
+    T cosy_cosp = +1.0 - 2.0 * (Qy * Qy + Qz * Qz);
+    Yaw[0] = ceres::atan2(siny_cosp, cosy_cosp);
+  }
+
   template<typename T>
   inline void InvertQuaterion(const T* Quaternion, T* QuaternionInv)
   {

include/error_models/LossFunction.h

@@ -38,6 +38,16 @@ using ceres::LossFunction;
 
 namespace libRSF
 {
+  /** \brief The robust Dynamic Covariance Scaling loss function
+   * Based on:
+   * P. Agarwal, G. D. Tipaldi, L. Spinello, C. Stachniss and W. Burgard
+   * "Robust map optimization using dynamic covariance scaling"
+   * 2013 IEEE International Conference on Robotics and Automation, Karlsruhe, 2013
+   * DOI: 10.1109/ICRA.2013.6630557
+   *
+   * \param Phi Tuning parameter of DCS
+   *
+   */
   class DCSLoss : public LossFunction
   {
     public:
@@ -50,6 +60,16 @@ namespace libRSF
       const double Phi_;
   };
 
+  /** \brief The robust closed form of Dynamic Covariance Estimation
+   * Based on:
+   * T. Pfeifer, S. Lange and P. Protzel
+   * "Dynamic Covariance Estimation — A parameter free approach to robust Sensor Fusion"
+   * 2017 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI), Daegu, 2017
+   * DOI: 10.1109/MFI.2017.8170347
+   *
+   * \param Sigma Standard deviation without outliers
+   *
+   */
   class cDCELoss : public LossFunction
   {
     public:

include/error_models/MaxMixture.h

@@ -38,6 +38,16 @@
 
 namespace libRSF
 {
+  /** \brief The robust Max-Mixture error model
+   * Based on:
+   * E. Olson and P. Agarwal
+   * “Inference on networks of mixtures for robust robot mapping”
+   * Proc. of Robotics: Science and Systems (RSS), Sydney, 2012.
+   * DOI: 10.15607/RSS.2012.VIII.040
+   *
+   * \param Mixture Underlying mixture distribution
+   *
+   */
   template <int Dimension, typename MixtureType>
   class MaxMixture : public ErrorModel <Dimension, Dimension+1>
   {

include/error_models/SumMixture.h

@@ -39,6 +39,16 @@
 
 namespace libRSF
 {
+  /** \brief The robust Sum-Mixture error model
+   * Based on:
+   * D. M. Rosen, M. Kaess, and J. J. Leonard
+   * “Robust incremental online inference over sparse factor graphs: Beyond the Gaussian case”
+   * Proc. of Intl. Conf. on Robotics and Automation (ICRA), Karlsruhe, 2013.
+   * DOI: 10.1109/ICRA.2013.6630699
+   *
+   * \param Mixture Underlying mixture distribution
+   *
+   */
   template <int Dimension, typename MixtureType>
   class SumMixture : public ErrorModel <Dimension, 1>
   {