diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5fdf8bf..17165f8 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -70,6 +70,9 @@ if(BUILD_ROS_SUPPORT)
   set(Torch_DIR "/home/ros/libtorch/share/cmake/Torch")
   find_package(Torch REQUIRED)
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${TORCH_CXX_FLAGS}")
+  
+  find_package(Ceres REQUIRED)
+  
   if(BUILD_DEBUG)
     add_definitions(-DPUB_EXPLORED_NODES)
   endif()
@@ -127,6 +130,7 @@ include_directories(
   include
   ${catkin_INCLUDE_DIRS}
   ${TORCH_INCLUDE_DIRS}
+  ${CERES_INCLUDE_DIRS}
   ${CMAKE_CURRENT_SOURCE_DIR}/include/voro++-0.4.6
 )
 if( BUILD_VOROCPP )
@@ -155,7 +159,8 @@ list(APPEND ${PROJECT_NAME}_UTILS_SOURCES src/utils/geometry_utils.cpp
                                           src/utils/LineOfSight.cpp
                                           src/utils/utils.cpp       
                                           src/utils/metrics.cpp
-                                          src/utils/FCNet.cpp       
+                                          src/utils/FCNet.cpp
+                                          src/utils/CeresOpt.cpp       
                                           )
 if(BUILD_ROS_SUPPORT)
   list(APPEND ${PROJECT_NAME}_UTILS_SOURCES src/utils/ros/ROSInterfaces.cpp)
@@ -185,7 +190,7 @@ add_library(LazyThetaStarSIREN src/Planners/LazyThetaStarSIREN.cpp )
 
 
 list(APPEND ${PROJECT_NAME}_LIBRARIES AlgorithmBase AStar AStarM1 AStarM2 AStarSIREN ThetaStar ThetaStarM1 ThetaStarM2 ThetaStarSIREN LazyThetaStar LazyThetaStarM1 LazyThetaStarM1Mod LazyThetaStarM2 LazyThetaStarSIREN)  
-target_link_libraries(${${PROJECT_NAME}_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${TORCH_LIBRARIES})
+target_link_libraries(${${PROJECT_NAME}_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${TORCH_LIBRARIES} ${CERES_LIBRARIES})
 add_dependencies( ${${PROJECT_NAME}_LIBRARIES} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 list(APPEND ${PROJECT_NAME}_TARGETS ${${PROJECT_NAME}_LIBRARIES})
 
diff --git a/include/Grid3D/local_grid3d.hpp b/include/Grid3D/local_grid3d.hpp
index 550b3fa..587e74c 100755
--- a/include/Grid3D/local_grid3d.hpp
+++ b/include/Grid3D/local_grid3d.hpp
@@ -46,6 +46,21 @@
 // #include "voro++-0.4.6/src/voro++.hh"
 // #endif
 
+struct TrilinearParams
+{
+	float a0, a1, a2, a3, a4, a5, a6, a7;
+
+	TrilinearParams(void)
+	{
+		a0 = a1 = a2 = a3 = a4 = a5 = a6 = a7 = 0.0;
+	}
+
+	double interpolate(double x, double y, double z)
+	{
+		return a0 + a1*x + a2*y + a3*z + a4*x*y + a5*x*z + a6*y*z + a7*x*y*z;
+	}
+};
+
 class Local_Grid3d
 {
 private:
@@ -60,7 +75,7 @@ class Local_Grid3d
 	
 	// Octomap parameters
 	float m_maxX, m_maxY, m_maxZ;
-	float m_resolution, m_oneDivRes;
+	float m_oneDivRes;
 	octomap::OcTree *m_octomap;
 	
 	
@@ -87,6 +102,7 @@ class Local_Grid3d
 	Planners::utils::gridCell *m_grid;
 	int m_gridSize, m_gridSizeX, m_gridSizeY, m_gridSizeZ;
 	int m_gridStepY, m_gridStepZ;
+	float m_resolution;
 
 	// Local_Grid3d(): m_cloud(new pcl::PointCloud<pcl::PointXYZI>)
 	Local_Grid3d(): m_cloud(new pcl::PointCloud<pcl::PointXYZ>)
@@ -182,6 +198,57 @@ class Local_Grid3d
 		m_gridSliceMsg.header.stamp = ros::Time::now();
 		m_gridSlicePub.publish(m_gridSliceMsg);
 	}
+
+	TrilinearParams computeDistInterpolation(const double x, const double y, const double z)
+	{
+		TrilinearParams r;
+
+		if(isIntoMap(x, y, z) && isIntoMap(x+m_resolution, y+m_resolution, z+m_resolution))
+		{
+			// Get 3D point index
+			uint64_t i = point2grid(x, y, z); 
+
+			// Get neightbour values to compute trilinear interpolation
+			float c000, c001, c010, c011, c100, c101, c110, c111;
+			c000 = m_grid[i].dist; 
+			c001 = m_grid[i+m_gridStepZ].dist; 
+			c010 = m_grid[i+m_gridStepY].dist; 
+			c011 = m_grid[i+m_gridStepY+m_gridStepZ].dist; 
+			c100 = m_grid[i+1].dist; 
+			c101 = m_grid[i+1+m_gridStepZ].dist; 
+			c110 = m_grid[i+1+m_gridStepY].dist; 
+			c111 = m_grid[i+1+m_gridStepY+m_gridStepZ].dist; 
+
+			// Compute trilinear parameters
+			const float div = -m_oneDivRes*m_oneDivRes*m_oneDivRes;
+			float x0, y0, z0, x1, y1, z1;
+			x0 = ((int)(x*m_oneDivRes))*m_resolution;
+			x1 = x0+m_resolution;
+			y0 = ((int)(y*m_oneDivRes))*m_resolution;
+			y1 = y0+m_resolution;
+			z0 = ((int)(z*m_oneDivRes))*m_resolution;
+			z1 = z0+m_resolution;
+			r.a0 = (-c000*x1*y1*z1 + c001*x1*y1*z0 + c010*x1*y0*z1 - c011*x1*y0*z0 
+			+ c100*x0*y1*z1 - c101*x0*y1*z0 - c110*x0*y0*z1 + c111*x0*y0*z0)*div;
+			r.a1 = (c000*y1*z1 - c001*y1*z0 - c010*y0*z1 + c011*y0*z0
+			- c100*y1*z1 + c101*y1*z0 + c110*y0*z1 - c111*y0*z0)*div;
+			r.a2 = (c000*x1*z1 - c001*x1*z0 - c010*x1*z1 + c011*x1*z0 
+			- c100*x0*z1 + c101*x0*z0 + c110*x0*z1 - c111*x0*z0)*div;
+			r.a3 = (c000*x1*y1 - c001*x1*y1 - c010*x1*y0 + c011*x1*y0 
+			- c100*x0*y1 + c101*x0*y1 + c110*x0*y0 - c111*x0*y0)*div;
+			r.a4 = (-c000*z1 + c001*z0 + c010*z1 - c011*z0 + c100*z1 
+			- c101*z0 - c110*z1 + c111*z0)*div;
+			r.a5 = (-c000*y1 + c001*y1 + c010*y0 - c011*y0 + c100*y1 
+			- c101*y1 - c110*y0 + c111*y0)*div;
+			r.a6 = (-c000*x1 + c001*x1 + c010*x1 - c011*x1 + c100*x0 
+			- c101*x0 - c110*x0 + c111*x0)*div;
+			r.a7 = (c000 - c001 - c010 + c011 - c100
+			+ c101 + c110 - c111)*div;
+		}
+
+		return r;
+	}
+
 	
 	bool isIntoMap(float x, float y, float z)
 	{
diff --git a/include/local_planner_optimizer/ceres_constraint_dist_to_obstacle.hpp b/include/local_planner_optimizer/ceres_constraint_dist_to_obstacle.hpp
new file mode 100644
index 0000000..ac194ed
--- /dev/null
+++ b/include/local_planner_optimizer/ceres_constraint_dist_to_obstacle.hpp
@@ -0,0 +1,78 @@
+#ifndef CERES_CONSTRAINTS_DIST_TO_OBSTACLE
+#define CERES_CONSTRAINTS_DIST_TO_OBSTACLE
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include "utils/ros/ROSInterfaces.hpp"
+#include "utils/SaveDataVariantToFile.hpp"
+#include "utils/misc.hpp"
+#include "utils/geometry_utils.hpp"
+#include "utils/metrics.hpp"
+#include <ros/ros.h>
+
+#include <heuristic_planners/Vec3i.h>
+#include <heuristic_planners/CoordinateList.h>
+
+#include "Grid3D/local_grid3d.hpp"
+
+#include <ceres/ceres.h>
+
+using ceres::SizedCostFunction;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solve;
+using ceres::Solver;
+
+class ObstacleDistanceCostFunctor : public SizedCostFunction<1, 3>
+    {
+    public:
+        ObstacleDistanceCostFunctor(Local_Grid3d* grid_, double weight): g_3D(grid_), weight_(weight)
+        {}
+
+        virtual ~ObstacleDistanceCostFunctor(void)
+        {}
+
+        virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
+        {
+            residuals[0] = 0;
+            float x = parameters[0][0] * g_3D->m_resolution;
+            float y = parameters[0][1] * g_3D->m_resolution;
+            float z = parameters[0][2] * g_3D->m_resolution;
+            float dist_;
+            if(g_3D->isIntoMap(x, y, z))
+            {
+                TrilinearParams p = g_3D->computeDistInterpolation(x, y, z);
+                dist_ = p.a0 + p.a1*x + p.a2*y + p.a3*z + p.a4*x*y + p.a5*x*z + p.a6*y*z + p.a7*x*y*z;
+                if(fabsf(dist_) > 0.01)
+                    residuals[0] = 1/dist_;
+                else
+                    residuals[0] = 100.0;
+                if (jacobians != nullptr && jacobians[0] != nullptr)
+                {
+                    int cte = -1/(dist_*dist_);
+                    jacobians[0][0] = cte*(p.a1 + p.a4*y + p.a5*z + p.a7*y*z);
+                    jacobians[0][1] = cte*(p.a2 + p.a4*x + p.a6*z + p.a7*x*z);
+                    jacobians[0][2] = cte*(p.a3 + p.a5*x + p.a6*y + p.a7*x*y); 
+                }			
+            }
+            else // This might give an error
+            {
+                std::cout << "Ceres found point outside map" << std::endl;
+                if (jacobians != nullptr && jacobians[0] != nullptr)
+                {
+                    jacobians[0][0] = 0.0;
+                    jacobians[0][1] = 0.0;
+                    jacobians[0][2] = 0.0;
+                }
+            }
+            return true;
+        }
+
+        double weight_;
+        Local_Grid3d* g_3D;
+
+    private:
+    };
+
+#endif
\ No newline at end of file
diff --git a/include/local_planner_optimizer/ceres_constraint_path_length.hpp b/include/local_planner_optimizer/ceres_constraint_path_length.hpp
new file mode 100644
index 0000000..9554297
--- /dev/null
+++ b/include/local_planner_optimizer/ceres_constraint_path_length.hpp
@@ -0,0 +1,51 @@
+#ifndef CERES_CONSTRAINTS_PATH_LEGTH
+#define CERES_CONSTRAINTS_PATH_LEGTH
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include "utils/ros/ROSInterfaces.hpp"
+#include "utils/SaveDataVariantToFile.hpp"
+#include "utils/misc.hpp"
+#include "utils/geometry_utils.hpp"
+#include "utils/metrics.hpp"
+#include <ros/ros.h>
+
+#include <heuristic_planners/Vec3i.h>
+#include <heuristic_planners/CoordinateList.h>
+
+#include "Grid3D/local_grid3d.hpp"
+
+#include <ceres/ceres.h>
+
+using ceres::AutoDiffCostFunction;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solve;
+using ceres::Solver;
+
+class PathLengthFunctor {
+
+public:
+    PathLengthFunctor(double weight): weight_(weight) {}
+
+    template <typename T>
+    bool operator()(const T* const stateWP1, const T* const stateWP2, T* residual) const {
+
+        T dx = stateWP2[0] - stateWP1[0];
+        T dy = stateWP2[1] - stateWP1[1];
+        T dz = stateWP2[2] - stateWP1[2];
+
+        residual[0] = weight_* (dx * dx + dy * dy + dz * dz);
+
+        return true;
+    }
+
+    double weight_;
+    
+private:
+
+
+};
+
+#endif
\ No newline at end of file
diff --git a/include/local_planner_optimizer/ceres_constraint_wp_equidistance.hpp b/include/local_planner_optimizer/ceres_constraint_wp_equidistance.hpp
new file mode 100644
index 0000000..9d57301
--- /dev/null
+++ b/include/local_planner_optimizer/ceres_constraint_wp_equidistance.hpp
@@ -0,0 +1,51 @@
+#ifndef CERES_CONSTRAINTS_WP_EQUIDISTANCE
+#define CERES_CONSTRAINTS_WP_EQUIDISTANCE
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include "utils/ros/ROSInterfaces.hpp"
+#include "utils/SaveDataVariantToFile.hpp"
+#include "utils/misc.hpp"
+#include "utils/geometry_utils.hpp"
+#include "utils/metrics.hpp"
+#include <ros/ros.h>
+
+#include <heuristic_planners/Vec3i.h>
+#include <heuristic_planners/CoordinateList.h>
+
+#include "Grid3D/local_grid3d.hpp"
+
+#include <ceres/ceres.h>
+
+using ceres::AutoDiffCostFunction;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solve;
+using ceres::Solver;
+
+class EquidistanceFunctor {
+
+public:
+    EquidistanceFunctor(double d_target, double weight): d_target_(d_target), weight_(weight) {}
+
+    template <typename T>
+    bool operator()(const T* const stateWP1, const T* const stateWP2, T* residual) const {
+
+        T dx = stateWP2[0] - stateWP1[0];
+        T dy = stateWP2[1] - stateWP1[1];
+        T dz = stateWP2[2] - stateWP1[2];
+
+        residual[0] = weight_* (dx * dx + dy * dy + dz * dz - T(d_target_));
+
+        return true;
+    }
+
+    double d_target_, weight_;
+    
+private:
+
+
+};
+
+#endif
\ No newline at end of file
diff --git a/include/utils/CeresOpt.hpp b/include/utils/CeresOpt.hpp
new file mode 100644
index 0000000..755a72a
--- /dev/null
+++ b/include/utils/CeresOpt.hpp
@@ -0,0 +1,44 @@
+#ifndef CERESOPT_HPP
+#define CERESOPT_HPP
+
+#include <iostream>
+#include "utils/ros/ROSInterfaces.hpp"
+#include "utils/SaveDataVariantToFile.hpp"
+#include "utils/misc.hpp"
+#include "utils/geometry_utils.hpp"
+#include "utils/metrics.hpp"
+#include <ros/ros.h>
+
+#include <heuristic_planners/Vec3i.h>
+#include <heuristic_planners/CoordinateList.h>
+
+#include "Grid3D/local_grid3d.hpp"
+
+#include <ceres/ceres.h>
+
+#include "local_planner_optimizer/ceres_constraint_dist_to_obstacle.hpp"
+#include "local_planner_optimizer/ceres_constraint_wp_equidistance.hpp"
+#include "local_planner_optimizer/ceres_constraint_path_length.hpp"
+
+using ceres::AutoDiffCostFunction;
+using ceres::NumericDiffCostFunction;
+using ceres::CENTRAL;
+using ceres::FORWARD;
+using ceres::RIDDERS;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solve;
+using ceres::Solver;
+using ceres::LossFunction;
+using ceres::CauchyLoss;
+
+struct parameterBlockWP{
+	double parameter[3];
+};
+
+namespace Ceresopt{
+    Planners::utils::CoordinateList ceresOptimizer(Planners::utils::CoordinateList initial_path, Local_Grid3d &_grid);
+}
+
+
+#endif // CERESOPT_HPP
\ No newline at end of file
diff --git a/include/utils/utils.hpp b/include/utils/utils.hpp
index f521b25..7853ef2 100755
--- a/include/utils/utils.hpp
+++ b/include/utils/utils.hpp
@@ -203,6 +203,13 @@ namespace Planners
                     
             return { _vec.x * _mult, _vec.y * _mult, _vec.z * _mult};
         }
+        
+        struct Vec3d 
+        {
+            double x, y, z;
+            Vec3d(double x_, double y_, double z_) : x(x_), y(y_), z(z_) {}
+        };
+
         /**
          * @brief 
          * 
diff --git a/src/ROS/local_planner_ros_node.cpp b/src/ROS/local_planner_ros_node.cpp
index 3d15576..a08cbd0 100755
--- a/src/ROS/local_planner_ros_node.cpp
+++ b/src/ROS/local_planner_ros_node.cpp
@@ -15,6 +15,7 @@
 #include "utils/misc.hpp"
 #include "utils/geometry_utils.hpp"
 #include "utils/metrics.hpp"
+#include "utils/CeresOpt.hpp"
 
 #include "Grid3D/local_grid3d.hpp"
 
@@ -447,12 +448,29 @@ class HeuristicLocalPlannerROS
             Planners::utils::CoordinateList local_path;
             local_path = std::get<Planners::utils::CoordinateList>(local_path_data["path"]);
 
+            // Trilinear params test
+            for(const auto& point: local_path){
+                double x_test = static_cast<double>(point.x) * m_local_grid3d_->m_resolution;
+                double y_test = static_cast<double>(point.y) * m_local_grid3d_->m_resolution;
+                double z_test = static_cast<double>(point.z) * m_local_grid3d_->m_resolution;
+                TrilinearParams p_test = m_local_grid3d_->computeDistInterpolation(x_test, y_test, z_test);
+                std::cout << "Params for x = " << x_test/m_local_grid3d_->m_resolution << ", y = " << y_test/m_local_grid3d_->m_resolution << ", z = " << z_test/m_local_grid3d_->m_resolution << ": a0=" << p_test.a0 << ", a1=" << p_test.a1 << ", a2=" << p_test.a2 << ", a3=" << p_test.a3 << ", a4=" << p_test.a4 << ", a5=" << p_test.a5 << ", a6=" << p_test.a6 << ", a7=" << p_test.a7 << std::endl;
+
+                float test_dist = p_test.a0 + p_test.a1*x_test + p_test.a2*y_test + p_test.a3*z_test + p_test.a4*x_test*y_test + p_test.a5*x_test*z_test + p_test.a6*y_test*z_test + p_test.a7*x_test*y_test*z_test;
+                uint64_t test_index = point.x + point.y*m_local_grid3d_->m_gridStepY + point.z*m_local_grid3d_->m_gridStepZ;
+                std::cout << "Interpolated distance : " << test_dist << "Real distance : " << m_local_grid3d_->m_grid[test_index].dist << std::endl;
+            }
+
+            // -----------------CERES OPTIMIZATION OF THE PATH-----------------
+            Planners::utils::CoordinateList opt_local_path = Ceresopt::ceresOptimizer(local_path, *m_local_grid3d_);
+
+
             //Convert the local path to GLOBAL COORDINATES and push them into the markers
 
             Planners::utils::Vec3i local_origin = {origen_local_x, origen_local_y, origen_local_z};
             std::cout << "Local origin: " << local_origin;
 
-            for(const auto &it: std::get<Planners::utils::CoordinateList>(local_path_data["path"])){
+            for(const auto &it: opt_local_path){
                 Planners::utils::Vec3i global_wp_point = it + local_origin;
                 local_path_line_markers_.points.push_back(Planners::utils::continousPoint(global_wp_point, resolution_));
                 local_path_points_markers_.points.push_back(Planners::utils::continousPoint(global_wp_point, resolution_));
diff --git a/src/utils/CeresOpt.cpp b/src/utils/CeresOpt.cpp
new file mode 100644
index 0000000..f84b6e9
--- /dev/null
+++ b/src/utils/CeresOpt.cpp
@@ -0,0 +1,113 @@
+#include "utils/CeresOpt.hpp"
+
+namespace Ceresopt
+{
+
+    Planners::utils::CoordinateList ceresOptimizer(Planners::utils::CoordinateList initial_path, Local_Grid3d &_grid)
+    {
+        // Convert trajectory to ceres state wp vector
+        std::vector<parameterBlockWP> wp_state_vector;
+        int num_wp = initial_path.size();
+        wp_state_vector.reserve(num_wp);
+
+        for (const auto& point: initial_path){
+            parameterBlockWP newWP;
+            newWP.parameter[0]=static_cast<double>(point.x);
+            newWP.parameter[1]=static_cast<double>(point.y);
+            newWP.parameter[2]=static_cast<double>(point.z);
+            wp_state_vector.push_back(newWP);
+        }
+
+        // Print the entire wp_state_vector
+        for (size_t i = 0; i < wp_state_vector.size(); ++i) {
+            const auto& wp = wp_state_vector[i];
+            std::cout << "Waypoint " << i + 1 << ": "
+                    << "x = " << wp.parameter[0] << ", "
+                    << "y = " << wp.parameter[1] << ", "
+                    << "z = " << wp.parameter[2] << std::endl;
+        }
+
+
+        // Declare Ceres optimization problem
+        ceres::Problem problem;
+
+        // Equidistance function target distance squared
+
+        double dist_target = 0;
+        for (int i=0; i < (num_wp-1); i++){
+            double tdx = wp_state_vector[i+1].parameter[0] - wp_state_vector[i].parameter[0];
+            double tdy = wp_state_vector[i+1].parameter[1] - wp_state_vector[i].parameter[1];
+            double tdz = wp_state_vector[i+1].parameter[2] - wp_state_vector[i].parameter[2];
+
+            dist_target += tdx * tdx + tdy * tdy + tdz * tdz;
+        }
+        dist_target = dist_target/(num_wp-1);
+
+        // Cost function weights
+
+        double weight_equidistance = 1.0;
+        double weight_path_length = 1.0;
+        double weight_esdf = 1.0;
+
+        // Define cost functions
+
+        for (int i = 0; i < wp_state_vector.size() - 1; i++){
+
+            // 1 - Equidistance cost function
+            ceres::CostFunction* equidistance_function = new AutoDiffCostFunction<EquidistanceFunctor, 1, 3, 3>
+                                                        (new EquidistanceFunctor(dist_target, weight_equidistance));
+            problem.AddResidualBlock(equidistance_function, nullptr, wp_state_vector[i].parameter, wp_state_vector[i+1].parameter);
+
+            // 2 - Path length cost function
+            ceres::CostFunction* path_length_function = new AutoDiffCostFunction<PathLengthFunctor, 1, 3, 3>
+                                                        (new PathLengthFunctor(weight_path_length));;
+            problem.AddResidualBlock(path_length_function, nullptr, wp_state_vector[i].parameter, wp_state_vector[i+1].parameter);
+        }
+
+
+        // 3 - Distance to obstacles cost function
+        for (int i = 0; i < wp_state_vector.size(); i++)
+        {
+            ceres::CostFunction* esdf_function = new ObstacleDistanceCostFunctor(&_grid, weight_esdf);
+            problem.AddResidualBlock(esdf_function, nullptr, wp_state_vector[i].parameter);
+
+        }
+
+        // Freeze first and last points
+        problem.SetParameterBlockConstant(wp_state_vector[0].parameter);   // Freeze first wp
+        problem.SetParameterBlockConstant(wp_state_vector[wp_state_vector.size()-1].parameter);   // Freeze last wp
+
+        // Solve problem
+        ceres::Solver::Options options;
+        options.linear_solver_type = ceres::DENSE_QR;
+        options.minimizer_progress_to_stdout = true;
+        options.max_num_iterations = 100;
+
+        ceres::Solver::Summary summary;
+        ceres::Solve(options, &problem, &summary);
+
+        // std::cout << summary.FullReport() << "\n";
+
+        // Print the entire wp_state_vector
+        for (size_t i = 0; i < wp_state_vector.size(); ++i) {
+            const auto& wp = wp_state_vector[i];
+            std::cout << "New Waypoint " << i + 1 << ": "
+                    << "x = " << wp.parameter[0] << ", "
+                    << "y = " << wp.parameter[1] << ", "
+                    << "z = " << wp.parameter[2] << std::endl;
+        }
+
+        // Update the path with optimized values
+        Planners::utils::CoordinateList optimized_path;
+        for (int i=0; i<num_wp; i++){
+            Planners::utils::Vec3i newpoint;
+            newpoint.x = wp_state_vector[i].parameter[0];
+            newpoint.y = wp_state_vector[i].parameter[1];
+            newpoint.z = wp_state_vector[i].parameter[2];
+            optimized_path.push_back(newpoint);
+        }
+
+        return optimized_path;
+    }
+}
+