<commit_before>#include "s3d/cv/image_operation/filter_alignment.h"

#include "s3d/cv/disparity/disparity_analyzer_stan.h"

#include <opencv2/core/mat.hpp>

namespace s3d {
namespace image_operation {

FilterAlignment::FilterAlignment(gsl::not_null<s3d::DisparityAnalyzerSTAN*> disparityAnalyzer)
  : disparityAnalyzer_{disparityAnalyzer}
  , kalmanFilter_{4}
{
  Eigen::VectorXd variances(4);
  variances << 3.6816E-4, 5.9837E-7, 7.2080E-6, 0.7405;
  kalmanFilter_.setVariances(variances);
}

bool FilterAlignment::applyOnImage(cv::Mat* leftImage, cv::Mat* rightImage, StanResults* results) {
  results->alignment;

  Eigen::VectorXd alignment(4);
  alignment << results->alignment.ch_y,
          results->alignment.a_z,
          results->alignment.a_f,
          results->alignment.f_a_x;

  kalmanFilter_.filter(alignment);

  Eigen::VectorXd filteredAligment = kalmanFilter_.getEstimation();

  results->alignment.ch_y = filteredAligment(0);
  results->alignment.a_z = filteredAligment(1);
  results->alignment.a_f = filteredAligment(2);
  results->alignment.f_a_x = filteredAligment(3);

  return true;
}

void FilterAlignment::resetFilter() {
  kalmanFilter_.setInitialValues(Eigen::VectorXd::Zero(4));
}


} // namespace s3d
} // namespace image_operation<commit_msg>Add 5th parameter to kalman filter<commit_after>#include "s3d/cv/image_operation/filter_alignment.h"

#include "s3d/cv/disparity/disparity_analyzer_stan.h"

#include <opencv2/core/mat.hpp>

namespace s3d {
namespace image_operation {

FilterAlignment::FilterAlignment(gsl::not_null<s3d::DisparityAnalyzerSTAN*> disparityAnalyzer)
  : disparityAnalyzer_{disparityAnalyzer}
  , kalmanFilter_{5}
{
  Eigen::VectorXd variances(5);
  variances << 3.6816E-4, 5.9837E-7, 7.2080E-6, 0.7405, 3.6816E-4;
  kalmanFilter_.setVariances(variances);
}

bool FilterAlignment::applyOnImage(cv::Mat* leftImage, cv::Mat* rightImage, StanResults* results) {
  results->alignment;

  Eigen::VectorXd alignment(5);
  alignment << results->alignment.ch_y,
          results->alignment.a_z,
          results->alignment.a_f,
          results->alignment.f_a_x,
          results->alignment.a_y_f;

  kalmanFilter_.filter(alignment);

  Eigen::VectorXd filteredAlignment = kalmanFilter_.getEstimation();

  results->alignment.ch_y = filteredAlignment(0);
  results->alignment.a_z = filteredAlignment(1);
  results->alignment.a_f = filteredAlignment(2);
  results->alignment.f_a_x = filteredAlignment(3);
  results->alignment.a_y_f = filteredAlignment(4);

  return true;
}

void FilterAlignment::resetFilter() {
  kalmanFilter_.setInitialValues(Eigen::VectorXd::Zero(5));
}


} // namespace s3d
} // namespace image_operation
{'fraction_non_alphanumeric': 0.13153961136023917, 'fraction_numerical': 0.030642750373692077, 'mean_word_length': 5.358669833729216, 'pattern_counts': {'":': 0, '<': 15, '<?xml version=': 0, '>': 27, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3104825', 'n_tokens_mistral': 1008, 'n_tokens_neox': 933, 'n_words': 124}