locusrobotics · svwilliams · Feb 26, 2020 · Feb 11, 2020
diff --git a/fuse_loss/CMakeLists.txt b/fuse_loss/CMakeLists.txt
@@ -33,12 +33,18 @@ add_compile_options(-Wall -Werror)
 add_library(${PROJECT_NAME}
   src/arctan_loss.cpp
   src/cauchy_loss.cpp
+  src/dcs_loss.cpp
+  src/fair_loss.cpp
+  src/geman_mcclure_loss.cpp
   src/huber_loss.cpp
+  src/loss_function.cpp
+  src/pseudo_huber_loss.cpp
   src/scaled_loss.cpp
   src/softlone_loss.cpp
   src/tolerant_loss.cpp
   src/trivial_loss.cpp
   src/tukey_loss.cpp
+  src/welsch_loss.cpp
 )
 target_include_directories(${PROJECT_NAME}
   PUBLIC
@@ -88,6 +94,30 @@ if(CATKIN_ENABLE_TESTING)
   roslint_cpp()
   roslint_add_test()
 
+  # Loss function Tests
+  catkin_add_gtest(test_loss_function
+    test/test_loss_function.cpp
+  )
+  add_dependencies(test_loss_function
+    ${catkin_EXPORTED_TARGETS}
+  )
+  target_include_directories(test_loss_function
+    PRIVATE
+      include
+      ${catkin_INCLUDE_DIRS}
+      ${CERES_INCLUDE_DIRS}
+  )
+  target_link_libraries(test_loss_function
+    ${PROJECT_NAME}
+    ${catkin_LIBRARIES}
+    ${CERES_LIBRARIES}
+  )
+  set_target_properties(test_loss_function
+    PROPERTIES
+      CXX_STANDARD 14
+      CXX_STANDARD_REQUIRED YES
+  )
+
   # Huber Loss Tests
   catkin_add_gtest(test_huber_loss
     test/test_huber_loss.cpp

diff --git a/fuse_loss/fuse_plugins.xml b/fuse_loss/fuse_plugins.xml
@@ -29,6 +29,44 @@
     where b = a^2 and c = 1 / b.
     </description>
   </class>
+  <class type="fuse_loss::DCSLoss" base_class_type="fuse_core::Loss">
+    <description>
+    DCS (Dynamic Covariance Scaling) loss function with scaling parameter 'a', defined as follows for the residual 's':
+
+    rho(s) = a * (3 * s - a) / (a + s)  if s > a   // outlier region
+           = s                          otherwise  // inlier region
+
+    which weight function:
+
+    rho'(s) = min { 1, (2 * a / (a + s))^2 }
+
+    as described in Eq. 5.19 and 5.20 in:
+
+      http://www2.informatik.uni-freiburg.de/~agarwal/resources/agarwal-thesis.pdf (p. 89)
+
+    that is equal to the square of the scaling factor in Eq. 15 in the original paper:
+
+      http://www2.informatik.uni-freiburg.de/~spinello/agarwalICRA13.pdf (p.3)
+    </description>
+  </class>
+  <class type="fuse_loss::FairLoss" base_class_type="fuse_core::Loss">
+    <description>
+    Fair loss function with scaling parameter 'a', defined as follows for the residual 's':
+
+    rho(s) = 2 * b * (r/a - log(1 + r/a))
+
+    where b = a^2 and r = sqrt(s).
+    </description>
+  </class>
+  <class type="fuse_loss::GemanMcClureLoss" base_class_type="fuse_core::Loss">
+    <description>
+    Geman-McClure loss function with scaling parameter 'a', defined as follows for the residual 's':
+
+    rho(s) = s * b / (b + s)
+
+    where b = a^2.
+    </description>
+  </class>
   <class type="fuse_loss::HuberLoss" base_class_type="fuse_core::Loss">
     <description>
     Huber loss function with scaling parameter 'a', defined as follows for the residual 's':
@@ -39,18 +77,29 @@
     where b = a^2.
     </description>
   </class>
+  <class type="fuse_loss::PseudoHuberLoss" base_class_type="fuse_core::Loss">
+    <description>
+    Pseudo-Huber loss function with scaling parameter 'a', defined as follows for the residual 's':
+
+    rho(s) = 2 * b * (sqrt(1 + s * c) - 1)
+
+    where b = a^2 and c = 1 / b.
+
+    This is a smooth approximation of the Huber loss function.
+    </description>
+  </class>
   <class type="fuse_loss::ScaledLoss" base_class_type="fuse_core::Loss">
     <description>
-      The other basic loss function has to do with length scaling, i.e. they affect the space in which 's' is measured.
-      Sometimes you want to simply scale the output value of the robustifier.
+    The other basic loss function has to do with length scaling, i.e. they affect the space in which 's' is measured.
+    Sometimes you want to simply scale the output value of the robustifier.
 
-      If '\hat{rho}' is the wrapped robustifier, then this simply outputs:
+    If '\hat{rho}' is the wrapped robustifier, then this simply outputs:
 
-      rho(s) = a * \hat{rho(s)}
+    rho(s) = a * \hat{rho(s)}
 
-      If '\hat{rho}' is not given, it is set as NULL and is treated as the Trivial loss function, so we get:
+    If '\hat{rho}' is not given, it is set as NULL and is treated as the Trivial loss function, so we get:
 
-      rho(s) = a * s
+    rho(s) = a * s
     </description>
   </class>
   <class type="fuse_loss::SoftLOneLoss" base_class_type="fuse_core::Loss">
@@ -93,4 +142,13 @@
            = a^2 / 6 * (1 - (1 - s / a^2)^3)  otherwise   // outlier region
     </description>
   </class>
+  <class type="fuse_loss::WelschLoss" base_class_type="fuse_core::Loss">
+    <description>
+    Welsch loss function with scaling parameter 'a', defined as follows for the residual 's':
+
+    rho(s) = b * (1 - exp(-s/b))
+
+    where b = a^2.
+    </description>
+  </class>
 </library>
diff --git a/fuse_loss/include/fuse_loss/dcs_loss.h b/fuse_loss/include/fuse_loss/dcs_loss.h
@@ -0,0 +1,150 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, Clearpath Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef FUSE_LOSS_DCS_LOSS_H
+#define FUSE_LOSS_DCS_LOSS_H
+
+#include <fuse_core/loss.h>
+
+#include <boost/serialization/access.hpp>
+#include <boost/serialization/base_object.hpp>
+#include <boost/serialization/export.hpp>
+
+#include <ostream>
+#include <string>
+
+
+namespace fuse_loss
+{
+
+/**
+ * @brief The DCS (Dynamic Covariance Scaling) loss function.
+ *
+ * This class encapsulates the ceres::DCSLoss class, adding the ability to serialize it and load it
+ * dynamically.
+ *
+ * The DCS loss is not provided by the Ceres solver, so it is implemented here, based on equation #315 from:
+ * http://www2.informatik.uni-freiburg.de/~spinello/agarwalICRA13.pdf (p. 3)
+ *
+ * See the Ceres documentation for more details. http://ceres-solver.org/nnls_modeling.html#lossfunction
+ */
+class DCSLoss : public fuse_core::Loss
+{
+public:
+  FUSE_LOSS_DEFINITIONS(DCSLoss);
+
+  /**
+   * @brief Constructor
+   *
+   * @param[in] a DCSLoss parameter 'a'
+   */
+  explicit DCSLoss(const double a = 1.0);
+
+  /**
+   * @brief Destructor
+   */
+  ~DCSLoss() override = default;
+
+  /**
+   * @brief Perform any required post-construction initialization, such as reading from the parameter server.
+   *
+   * This will be called on each plugin after construction.
+   *
+   * @param[in] name A unique name to initialize this plugin instance, such as from the parameter server.
+   */
+  void initialize(const std::string& name) override;
+
+  /**
+   * @brief Print a human-readable description of the loss function to the provided stream.
+   *
+   * @param[out] stream The stream to write to. Defaults to stdout.
+   */
+  void print(std::ostream& stream = std::cout) const override;
+
+  /**
+   * @brief Return a raw pointer to a ceres::LossFunction that implements the loss function
+   *
+   * The Ceres interface requires a raw pointer. Ceres will take ownership of the pointer and promises to properly
+   * delete the loss function when it is done. Additionally, Fuse promises that the Loss object will outlive any
+   * generated loss functions (i.e. the Ceres objects will be destroyed before the Loss Function objects). This
+   * guarantee may allow optimizations for the creation of the loss function objects.
+   *
+   * @return A base pointer to an instance of a derived ceres::LossFunction.
+   */
+  ceres::LossFunction* lossFunction() const override;
+
+  /**
+   * @brief Parameter 'a' accessor.
+   *
+   * @return Parameter 'a'.
+   */
+  double a() const
+  {
+    return a_;
+  }
+
+  /**
+   * @brief Parameter 'a' mutator.
+   *
+   * @param[in] a Parameter 'a'.
+   */
+  void a(const double a)
+  {
+    a_ = a;
+  }
+
+private:
+  double a_{ 1.0 };  //<! DCSLoss parameter 'a'
+
+  // Allow Boost Serialization access to private methods
+  friend class boost::serialization::access;
+
+  /**
+   * @brief The Boost Serialize method that serializes all of the data members in to/out of the archive
+   *
+   * @param[in/out] archive - The archive object that holds the serialized class members
+   * @param[in] version - The version of the archive being read/written. Generally unused.
+   */
+  template<class Archive>
+  void serialize(Archive& archive, const unsigned int /* version */)
+  {
+    archive & boost::serialization::base_object<fuse_core::Loss>(*this);
+    archive & a_;
+  }
+};
+
+}  // namespace fuse_loss
+
+BOOST_CLASS_EXPORT_KEY(fuse_loss::DCSLoss);
+
+#endif  // FUSE_LOSS_DCS_LOSS_H