#include "opencv2/calib3d.hpp"
#include <cloud_point/rectify.h>

namespace score {

Rectify::Rectify()  = default;
Rectify::~Rectify() = default;
/**
 * @brief perform rectification operation on providen image
 * @param cameraMatrix matrix of intrinsic params of size 3x3
 * @param distCoeffs distortion matrix
 * @param image reference to image object (changed in-place)
 */
void Rectify::perform(Image &image, cv::Mat cameraMatrix, cv::Mat distCoeffs) {
    cv::Mat newmatrix = cv::getOptimalNewCameraMatrix(
        cameraMatrix, distCoeffs, {image.get().size[0], image.get().size[1]},
        1);
    cv::Mat output;
    cv::undistort(image.get(), output, cameraMatrix, distCoeffs, newmatrix);
    output.copyTo(image.get());
}

} // namespace score