diff --git a/include/cloud_point/image.h b/include/cloud_point/image.h
index c54812e..5c425e3 100644
--- a/include/cloud_point/image.h
+++ b/include/cloud_point/image.h
@@ -6,15 +6,16 @@
 namespace score {
 
 class Image {
-public:
+  public:
     Image();
-    explicit Image(const cv::Mat& image);
+    explicit Image(const cv::Mat &image);
     ~Image();
 
-    [[nodiscard]] cv::Mat get() const;
+    /// @note data_ could be changed through this
+    [[nodiscard]] cv::Mat get();
 
-protected:
+  protected:
     cv::Mat data_;
 };
 
-}
\ No newline at end of file
+} // namespace score
\ No newline at end of file
diff --git a/include/cloud_point/matrixFactory.h b/include/cloud_point/matrixFactory.h
new file mode 100644
index 0000000..1c53c12
--- /dev/null
+++ b/include/cloud_point/matrixFactory.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include "opencv2/core/hal/interface.h"
+#include "opencv2/core/mat.hpp"
+#include <stdexcept>
+namespace score {
+
+class CameraMatrixFactory {
+  public:
+    /**
+     * @param rpc < vector of size Width*Height
+     * @throw runtime_error if size is not Width*Height
+     */
+    template <size_t Width, size_t Height>
+    static cv::Mat create(std::vector<double> rpc) {
+        if (rpc.size() != Width * Height)
+            throw std::runtime_error("Vector size is not Width*Height");
+        return {Width, Height, CV_64F, rpc.data()};
+    }
+};
+
+} // namespace score
\ No newline at end of file
diff --git a/include/cloud_point/rectify.h b/include/cloud_point/rectify.h
new file mode 100644
index 0000000..0e81b87
--- /dev/null
+++ b/include/cloud_point/rectify.h
@@ -0,0 +1,16 @@
+#pragma once
+#include "cloud_point/image.h"
+#include <opencv2/calib3d.hpp>
+
+namespace score {
+
+class Rectify {
+  public:
+    Rectify();
+    ~Rectify();
+
+    void perform(Image &image, cv::Mat cameraMatrix,
+                 cv::Mat distCoeffs = cv::Mat::zeros(1, 5, CV_64F));
+};
+
+} // namespace score
\ No newline at end of file
diff --git a/src/cloud_point/image.cpp b/src/cloud_point/image.cpp
index 7a36c44..728ad4b 100644
--- a/src/cloud_point/image.cpp
+++ b/src/cloud_point/image.cpp
@@ -5,18 +5,14 @@
 
 namespace score {
 
- Image::Image() {
-     //no work
- }
+Image::Image() {
+    // no work
+}
 
- Image::Image(const cv::Mat& image) {
-     this->data_ = image;
- }
+Image::Image(const cv::Mat &image) { this->data_ = image; }
 
 Image::~Image() = default;
 
- cv::Mat Image::get() const {
-    return this->data_;
-}
+cv::Mat Image::get() { return this->data_; }
 
-}
\ No newline at end of file
+} // namespace score
\ No newline at end of file
diff --git a/src/cloud_point/meson.build b/src/cloud_point/meson.build
index d8caf55..361112f 100644
--- a/src/cloud_point/meson.build
+++ b/src/cloud_point/meson.build
@@ -9,7 +9,8 @@ if not opencv_dep.found()
 endif
 
 cloud_point_sources = files(
-    'image.cpp'
+    'image.cpp',
+    'rectify.cpp'
 )
 
 cpc_deps = [ cloud_point_rpc_dep, opencv_dep ]
diff --git a/src/cloud_point/rectify.cpp b/src/cloud_point/rectify.cpp
new file mode 100644
index 0000000..f443252
--- /dev/null
+++ b/src/cloud_point/rectify.cpp
@@ -0,0 +1,23 @@
+#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
\ No newline at end of file
diff --git a/tests/test_image.cpp b/tests/test_image.cpp
index aa3f4e7..c7e3bad 100644
--- a/tests/test_image.cpp
+++ b/tests/test_image.cpp
@@ -1,6 +1,7 @@
 //
 // Created by vptyp on 12.03.2026.
 //
+#include "cloud_point/rectify.h"
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
 
@@ -89,3 +90,18 @@ TEST_F(ImageTest, CreateDepth) {
     EXPECT_DOUBLE_EQ(mat.at<double>(0, 0), 0.0);
     EXPECT_DOUBLE_EQ(mat.at<double>(4, 9), 49.0);
 }
+
+TEST_F(ImageTest, RectificationNoThrow) {
+    score::Rectify rectify;
+    score::ImageRPC rpc;
+    rpc.width  = 10;
+    rpc.height = 20;
+    rpc.type   = score::ImageRPC::Type::BGR;
+    rpc.data.resize(rpc.width * rpc.height * 3, 128);
+    score::Image image = score::ImageFactory::create(rpc);
+    double fx = 10.1, fy = 20.2, cx = 30.3, cy = 40.4;
+    cv::Mat cameraMatrix =
+        (cv::Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
+
+    EXPECT_NO_THROW(rectify.perform(image, cameraMatrix));
+}
\ No newline at end of file