Maximum Likelihood (#69)

* Added a cost function for maximum likelihood

* Added naive unit test for maximum likelihood

* Improved unit test

rct_optimizations/include/rct_optimizations/maximum_likelihood.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <rct_optimizations/types.h>
+
+namespace rct_optimizations
+{
+/**
+ * @brief Cost function for a parameter block based on the expected means and standard deviations for the values in the block.
+ * The inputs are an array of mean values and an array of standard deviations corresponding to the optimization variables in the parameter block
+ *
+ * Ceres provides an example of a maximum likelihood constraint here:
+ * https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/robot_pose_mle.cc#165
+ * blob: 83c0903d11f2558a9eb48884addc44853ffb903b
+ */
+class MaximumLikelihood
+{
+  public:
+  MaximumLikelihood(const Eigen::ArrayXXd &mean_, const Eigen::ArrayXXd &stdev_)
+    : mean(mean_)
+    , stdev(stdev_)
+  {
+    if (mean.size() != stdev.size())
+      throw OptimizationException("Mean and standard deviation matrix are not the same size");
+  }
+
+  template<typename T>
+  bool operator()(T const *const *parameter, T *residual) const
+  {
+    using ArrayXX = Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic>;
+    Eigen::Map<const ArrayXX> param_array(parameter[0], mean.rows(), mean.cols());
+
+    Eigen::Map<ArrayXX> residual_array(residual, mean.rows(), mean.cols());
+    residual_array = (param_array - mean.cast<T>()) / stdev.cast<T>();
+
+    return true;
+  }
+
+  Eigen::ArrayXXd mean;
+  Eigen::ArrayXXd stdev;
+};
+
+} // namespace rct_optimizations

rct_optimizations/test/CMakeLists.txt

@@ -81,6 +81,13 @@ rct_gtest_discover_tests(${PROJECT_NAME}_noise_tests)
 add_dependencies(${PROJECT_NAME}_noise_tests ${PROJECT_NAME})
 add_dependencies(run_tests ${PROJECT_NAME}_noise_tests)
 
+# Maximum Likelihood
+add_executable(${PROJECT_NAME}_maximum_likelihood_tests maximum_likelihood_utest.cpp)
+target_link_libraries(${PROJECT_NAME}_maximum_likelihood_tests PRIVATE ${PROJECT_NAME}_test_support GTest::GTest GTest::Main)
+rct_gtest_discover_tests(${PROJECT_NAME}_maximum_likelihood_tests)
+add_dependencies(${PROJECT_NAME}_maximum_likelihood_tests ${PROJECT_NAME})
+add_dependencies(run_tests ${PROJECT_NAME}_maximum_likelihood_tests)
+
 # Install the test executables so they can be run independently later if needed
 install(
   TARGETS
@@ -94,7 +101,7 @@ install(
     ${PROJECT_NAME}_pnp_tests
     ${PROJECT_NAME}_camera_intrinsic_validation_tests
     ${PROJECT_NAME}_noise_tests
-
+    ${PROJECT_NAME}_maximum_likelihood_tests
   RUNTIME DESTINATION bin/tests
   LIBRARY DESTINATION lib/tests
   ARCHIVE DESTINATION lib/tests

rct_optimizations/test/maximum_likelihood_utest.cpp

@@ -0,0 +1,74 @@
+#include <rct_optimizations/maximum_likelihood.h>
+
+#include <ceres/problem.h>
+#include <ceres/dynamic_autodiff_cost_function.h>
+#include <ceres/solver.h>
+#include <gtest/gtest.h>
+
+using namespace rct_optimizations;
+
+TEST(MaximumLikelihoodTests, NaiveTest)
+{
+  // Create matrices of mean and standard deviation values for each parameter
+  Eigen::ArrayXXd mean(Eigen::ArrayXXd::Random(6, 4));
+  const double stdev_val = 0.1;
+  Eigen::ArrayXXd stdev(Eigen::ArrayXXd::Constant(6, 4, stdev_val));
+
+  // Create a matrix of random values scaled up from a range of [0, 1]
+  const double scale = 10.0;
+  Eigen::ArrayXXd rand(Eigen::ArrayXXd::Random(6, 4) * scale);
+
+  Eigen::IOFormat fmt(4, 0, "|", "\n", "|", "|");
+  std::cout << "Mean\n" << mean.format(fmt) << std::endl;
+  std::cout << "Random:\n" << rand.format(fmt) << std::endl;
+
+  auto *ml = new MaximumLikelihood(mean, stdev);
+
+  // Check the cost function
+  {
+    Eigen::ArrayXXd residual(Eigen::ArrayXXd::Zero(6, 4));
+    std::vector<double *> params = {rand.data()};
+
+    bool success = ml->operator()(params.data(), residual.data());
+    EXPECT_TRUE(success);
+    EXPECT_TRUE(residual.isApprox((rand - mean) / stdev));
+  }
+
+  // Check that inputs of different sizes to the cost function results in an exception
+  {
+    EXPECT_THROW(MaximumLikelihood(mean, stdev.block<2, 2>(0, 0)), OptimizationException);
+  }
+
+  // Run an optimization to see if we can drive the random numbers to the mean
+  ceres::Problem problem;
+  auto *cost_block = new ceres::DynamicAutoDiffCostFunction<MaximumLikelihood>(ml);
+  cost_block->AddParameterBlock(rand.size());
+  cost_block->SetNumResiduals(rand.size());
+  problem.AddResidualBlock(cost_block, nullptr, rand.data());
+
+  // Solve the optimization
+  ceres::Solver::Options options;
+  ceres::Solver::Summary summary;
+  ceres::Solve(options, &problem, &summary);
+
+  EXPECT_TRUE(summary.termination_type == ceres::CONVERGENCE);
+
+  // Calculate the difference between the parameters
+  Eigen::ArrayXXd diff = (rand - mean).abs();
+  std::cout << "Difference:\n" << diff.format(fmt) << std::endl;
+
+  // Check element-wise that absolute difference between the mean matrix and optimized random matrix is almost zero
+  for (auto row = 0; row < diff.rows(); ++row)
+  {
+    for (auto col = 0; col < diff.cols(); ++col)
+    {
+      EXPECT_LT(diff(row, col), 1.0e-12);
+    }
+  }
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}