diff --git a/line_refinement/CMakeLists.txt b/line_refinement/CMakeLists.txt
index 574ef00..5ad1a10 100644
--- a/line_refinement/CMakeLists.txt
+++ b/line_refinement/CMakeLists.txt
@@ -12,6 +12,13 @@ if(${CERES_VERSION} VERSION_LESS "2.2.0")
     add_definitions("-DCERES_PARAMETERIZATION_ENABLED")
 endif()
 
+FIND_PACKAGE(OpenMP)
+if (OPENMP_FOUND)
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+  set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_CXX_FLAGS}")
+  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+endif (OPENMP_FOUND)
+
 add_subdirectory(pybind11)
 include_directories(../third_party/progressive-x/graph-cut-ransac/src/pygcransac/include
 					../third_party/progressive-x/src/pyprogressivex/include
@@ -39,3 +46,7 @@ pybind11_add_module(line_refinement line_vp_optim.cpp cost_functions.h vp_det.h)
 target_link_libraries(line_refinement PUBLIC ceres stdc++fs
 					  ${OpenCV_LIBS} "${GraphCutRANSAC_LIB}"
 					  "${ProgressiveX_LIB}")
+
+if(OpenMP_CXX_FOUND)
+    target_link_libraries(line_refinement PUBLIC OpenMP::OpenMP_CXX)
+endif()
\ No newline at end of file
diff --git a/line_refinement/line_vp_optim.cpp b/line_refinement/line_vp_optim.cpp
index ac7bd27..6ee5604 100644
--- a/line_refinement/line_vp_optim.cpp
+++ b/line_refinement/line_vp_optim.cpp
@@ -106,7 +106,6 @@ std::tuple<std::vector<std::array<double, 4>>, std::vector<int>, std::vector<std
     std::vector<std::array<double, 4>> x(num_lines);
     for(int i = 0; i < num_lines; i++)
         x[i] = {lines[i][0], lines[i][1], lines[i][2], lines[i][3]};
-    double c_x, c_y, ori, len, perp_dist;
 
     // Extract VPs
     std::vector<int> vp_labels;
@@ -121,19 +120,29 @@ std::tuple<std::vector<std::array<double, 4>>, std::vector<int>, std::vector<std
     Solver::Options options;
     options.linear_solver_type = ceres::DENSE_QR;
     options.minimizer_progress_to_stdout = verbose;
-    options.num_threads = 4;
+
+    #ifdef _OPENMP
+        options.num_threads = 1;
+    #else
+        options.num_threads = 4;
+    #endif
+
     options.max_num_iterations = 20;
     if(!optimize_vps)
     {
         num_iter = 1;
         options.max_num_iterations = 100;
     }
-    Solver::Summary summary;
+
     for(int it=0; it < num_iter; it++)
     {
         // Optimize the lines independently using the current VPs
+        #pragma omp parallel for
         for(int i=0; i < num_lines; i++)
-        {   
+        {
+            double c_x, c_y, ori, len, perp_dist;
+
+            Solver::Summary summary;
             /* Optimization with fixed line length and no tangential translation (2 DOFs) */
             c_x = (x[i][0] + x[i][2]) / 2.;
             c_y = (x[i][1] + x[i][3]) / 2.;
@@ -188,8 +197,12 @@ std::tuple<std::vector<std::array<double, 4>>, std::vector<int>, std::vector<std
             assign_vps(x, vps, vp_labels, threshold);
 
             // Optimize VPs based on the lines
+            #pragma omp parallel for
             for(int i=0; i < vps.size(); i++)
             {
+                double c_x, c_y, len;
+
+                Solver::Summary summary;
                 // Define the problem
                 Problem problem;
 
@@ -201,7 +214,7 @@ std::tuple<std::vector<std::array<double, 4>>, std::vector<int>, std::vector<std
                         // Get the middle point
                         c_x = (x[j][0] + x[j][2]) / 2;
                         c_y = (x[j][1] + x[j][3]) / 2;
-                        len = std::sqrt(pow(x[i][2] - x[i][0], 2) + pow(x[i][3] - x[i][1], 2));
+                        len = std::sqrt(pow(x[j][2] - x[j][0], 2) + pow(x[j][3] - x[j][1], 2));
                         LossFunction* vp_loss = new ScaledLoss(
                             new CauchyLoss(0.5), len, ceres::TAKE_OWNERSHIP);
                         ceres::CostFunction* vp_cost_function = VpCostFunctor::Create(