Added main optimization loop for local path with Ceres. No tunning pe…

…rformed

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})
 

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)
 	{

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

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

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

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

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 
          *