diff --git a/src/lib/matrix/matrix/SquareMatrix.hpp b/src/lib/matrix/matrix/SquareMatrix.hpp
index 4a1d3d0a81..7d72710e17 100644
--- a/src/lib/matrix/matrix/SquareMatrix.hpp
+++ b/src/lib/matrix/matrix/SquareMatrix.hpp
@@ -280,6 +280,27 @@ public:
 		return self.isBlockSymmetric<Width>(first, eps);
 	}
 
+	void copyLowerToUpperTriangle()
+	{
+		SquareMatrix<Type, M> &self = *this;
+
+		for (size_t row_idx = 1; row_idx < M; row_idx++) {
+			for (size_t col_idx = 0 ; col_idx < row_idx; col_idx++) {
+				self(col_idx, row_idx) = self(row_idx, col_idx);
+			}
+		}
+	}
+
+	void copyUpperToLowerTriangle()
+	{
+		SquareMatrix<Type, M> &self = *this;
+
+		for (size_t row_idx = 1; row_idx < M; row_idx++) {
+			for (size_t col_idx = 0 ; col_idx < row_idx; col_idx++) {
+				self(row_idx, col_idx) = self(col_idx, row_idx);
+			}
+		}
+	}
 };
 
 using SquareMatrix3f = SquareMatrix<float, 3>;
diff --git a/src/lib/matrix/test/MatrixSquareTest.cpp b/src/lib/matrix/test/MatrixSquareTest.cpp
index b52e9dd665..58ceab940f 100644
--- a/src/lib/matrix/test/MatrixSquareTest.cpp
+++ b/src/lib/matrix/test/MatrixSquareTest.cpp
@@ -174,4 +174,28 @@ TEST(MatrixSquareTest, Square)
 			   };
 	SquareMatrix<float, 4> K(data_K);
 	EXPECT_FALSE(K.isRowColSymmetric<1>(2));
+
+	float data_L[16] = {1, 0, 0, 0,
+			    2, 3, 0, 0,
+			    3, 4, 11, 0,
+			    4, 11, 15, 16
+			   };
+	float data_L_check[16] = {1, 2, 3, 4,
+				  2, 3, 4, 11,
+				  3, 4, 11, 15,
+				  4, 11, 15, 16
+				 };
+	SquareMatrix<float, 4> L(data_L);
+	L.copyLowerToUpperTriangle();
+	SquareMatrix<float, 4> L_check(data_L_check);
+	EXPECT_EQ(L, L_check);
+
+	float data_M[16] = {1, 2, 3, 4,
+			    0, 3, 4, 11,
+			    0, 0, 11, 15,
+			    0, 0, 0, 16
+			   };
+	SquareMatrix<float, 4> M(data_M);
+	M.copyUpperToLowerTriangle();
+	EXPECT_EQ(M, L_check);
 }