cost.h

// Copyright 2026 Julien Michot.
// SPDX-License-Identifier: Apache-2.0
 
#pragma once
 
#include <sstream>
 
#include <tinyopt/log.h>
#include <tinyopt/math.h>
#include <tinyopt/traits.h>
 
namespace tinyopt {
 
/***
 *  @brief Struct containing a cost and related informations
 *
 ***/
struct Cost {
  using Scalar = double;
 
  Cost() = default;
  Cost(Scalar cost_) : cost{cost_}, num_resisuals{1} {}
  Cost(Scalar cost_, int num_resisuals_, float inlier_ratio_ = 1.0f, const std::string &log_ = "")
      : cost{cost_}, num_resisuals{num_resisuals_}, inlier_ratio{inlier_ratio_}, log_str{log_} {}
 
  // Constructor receiving a 'residuals' Vector/Matrix. The cost will be the L2/Frobenius norm.
  template <typename Derived>
  Cost(const MatrixBase<Derived> &residuals, float inlier_ratio_ = 1.0f,
       const std::string &log_ = "")
      : Cost(residuals.squaredNorm(), (int)residuals.size(), inlier_ratio_, log_) {}
 
  template <typename Derived>
  Cost &operator=(const MatrixBase<Derived> &residuals) {
    cost = residuals.squaredNorm();
    num_resisuals = (int)residuals.size();
    return *this;
  }
 
  // Comparisons
  bool operator<(const Cost &other) const { return cost < other.cost; }
  bool operator<=(const Cost &other) const { return cost <= other.cost; }
  bool operator<(Scalar other_cost) const { return cost < other_cost; }
  bool operator<(float other_cost) const { return cost < other_cost; }
  bool operator<=(Scalar other_cost) const { return cost <= other_cost; }
  bool operator<=(float other_cost) const { return cost <= other_cost; }
 
  // Casting operators
  operator Scalar() const { return cost; }

  Cost &operator+=(const Cost &other) {
    cost += other.cost;
    AddResiduals(other.num_resisuals, other.NumInliers());
    if (!other.log_str.empty()) log_str += " " + other.log_str;
    return *this;
  }

  template <typename Derived>
  Cost &operator+=(const MatrixBase<Derived> &residuals) {
    cost += residuals.squaredNorm();
    AddResiduals(residuals.size(), residuals.size());
    return *this;
  }
 
  friend std::ostream &operator<<(std::ostream &os, const Cost &cost) {
    os << cost.toString();
    return os;
  }
 
  std::string toString(const std::string &cost_label = "ε", bool print_inliers = false) const {
    std::ostringstream oss;
    oss << TINYOPT_FORMAT_NS::format("{}:{:.4e}, n:{}", cost_label, cost, num_resisuals);
    if (num_resisuals > 1)
      oss << TINYOPT_FORMAT_NS::format(", √{}/n:{:.2e}", cost_label,
                                       std::sqrt(cost / num_resisuals));
    if (print_inliers)
      oss << TINYOPT_FORMAT_NS::format(", in:{:.2f}% ({})", inlier_ratio * 100.0f, NumInliers());
    if (!log_str.empty()) oss << ", " << log_str;
    return oss.str();
  }
 
  bool isValid() const { return num_resisuals > 0 && cost != std::numeric_limits<Scalar>::max(); }
  int NumInliers() const { return (int)(num_resisuals * inlier_ratio); }
  int NumOutliers() const { return (int)(num_resisuals * (1.0f - inlier_ratio)); }
 
  void AddResiduals(int new_residuals, int new_inliers) {
    if (num_resisuals + new_residuals > 0)
      inlier_ratio = (NumInliers() + new_inliers) / (float)(num_resisuals + new_residuals);
    num_resisuals += new_residuals;
  }
 
  Scalar cost = Scalar(0);    
  int num_resisuals = 0;      
  float inlier_ratio = 1.0f;  
  std::string log_str;  
};
 
}  // namespace tinyopt