orientation optimization using ceres

source/Navigation/libraries/global_planning/AStar.cpp

@@ -11,6 +11,7 @@
 #include <Navigation/libraries/conversions/eigen.h>
 #include <Navigation/libraries/core/Trajectory.h>
 #include <Navigation/libraries/global_planning/aron/AStarParams.aron.generated.h>
+#include <Navigation/libraries/global_planning/optimization/OrientationOptimizer.h>
 
 namespace armarx::nav::glob_plan
 {
@@ -134,6 +135,16 @@ namespace armarx::nav::glob_plan
         // - orientation diff between neighbor nodes should be minimized (smoothness)
         // - check if position could be optimized gently (this might require resampling during optimization)
 
+        ARMARX_TRACE;
+        OrientationOptimizer optimizer(resampledTrajectory);
+        const auto result = optimizer.optimize();
+
+        if (not result)
+        {
+            ARMARX_ERROR << "Optimizer failure";
+            return nullptr;
+        }
+
         ARMARX_TRACE;
         return std::make_shared<core::Trajectory>(resampledTrajectory);
     }

source/Navigation/libraries/global_planning/CMakeLists.txt

@@ -3,6 +3,8 @@ set(LIB_NAME ${PROJECT_NAME}GlobalPlanning)
 armarx_component_set_name("${LIB_NAME}")
 armarx_set_target("Library: ${LIB_NAME}")
 
+find_package(Ceres 2.0 REQUIRED)
+
 armarx_add_library(
     LIBS
         ArmarXCoreInterfaces
@@ -10,16 +12,19 @@ armarx_add_library(
         aroncommon
         Navigation::Core
         Navigation::Algorithms
+        Ceres::ceres
     SOURCES
         ./GlobalPlanner.cpp
         ./AStar.cpp
         ./Point2Point.cpp
         ./aron_conversions.cpp
+        ./optimization/OrientationOptimizer.cpp
     HEADERS
         ./GlobalPlanner.h
         ./AStar.h
         ./Point2Point.h
         ./aron_conversions.h
+        ./optimization/OrientationOptimizer.h
 )
 
 add_library(

source/Navigation/libraries/global_planning/optimization/OrientationOptimizer.cpp

+#include "OrientationOptimizer.h"
+
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/drop.hpp>
+#include <range/v3/view/drop_last.hpp>
+#include <range/v3/view/transform.hpp>
+
+#include <SimoxUtility/math/convert/mat4f_to_xyyaw.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "Navigation/libraries/core/Trajectory.h"
+#include <ceres/ceres.h>
+#include <ceres/cost_function.h>
+
+
+namespace armarx::nav::glob_plan
+{
+    OrientationOptimizer::OrientationOptimizer(const core::Trajectory& trajectory) :
+        trajectory(trajectory)
+    {
+    }
+
+    //   problem.AddResidualBlock(new MyBinaryCostFunction(...), nullptr, x2, x1);
+
+
+    struct CostFunctor
+    {
+        template <typename T>
+        bool
+        operator()(const T* const x, T* residual) const
+        {
+            residual[0] = 10.0 - x[0];
+            return true;
+        }
+    };
+
+    struct SmoothOrientationCostFunctor
+    {
+
+        SmoothOrientationCostFunctor(const double& yawPrev, const double& yawNext) :
+            yawPrev(yawPrev), yawNext(yawNext)
+        {
+        }
+
+        template <typename T>
+        bool
+        operator()(const T* const x, T* residual) const
+        {
+            // TODO periodic
+            residual[0] = yawPrev - x[0];
+            residual[1] = yawNext - x[0];
+            return true;
+        }
+
+    private:
+        const double& yawPrev;
+        const double& yawNext;
+    };
+
+    struct OrientationPriorCostFunctor
+    {
+
+        OrientationPriorCostFunctor(const double prior) : prior(prior)
+        {
+        }
+
+        template <typename T>
+        bool
+        operator()(const T* const x, T* residual) const
+        {
+            // TODO periodic
+            residual[0] = prior - x[0];
+            return true;
+        }
+
+    private:
+        const double prior;
+    };
+
+    OrientationOptimizer::OptimizationResult
+    OrientationOptimizer::optimize()
+    {
+
+        const auto toYaw = [](const core::TrajectoryPoint& pt) -> double
+        { return simox::math::mat4f_to_xyyaw(pt.waypoint.pose.matrix()).z(); };
+
+        std::vector<double> orientations =
+            trajectory.points() | ranges::views::transform(toYaw) | ranges::to_vector;
+
+        ARMARX_INFO << "orientations before: " << orientations;
+
+        // const float yawStart = orientations.front();
+        // const float yawEnd = orientations.back();
+
+        // //
+        // const auto yawsVariable =
+        //     orientations | ranges::views::drop(1) | ranges::views::drop_last(1) | ranges::to_vector;
+
+        ceres::Problem problem;
+
+        ARMARX_INFO << orientations.size() - 2 << "orientations to optimize";
+
+        // TODO https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/slam/pose_graph_2d/pose_graph_2d.cc
+        //     ceres::LocalParameterization* angle_local_parameterization =
+        //   ceres::AngleLocalParameterization::Create();
+
+        for (size_t i = 1; i < (orientations.size() - 1); i++)
+        {
+            // TODO weights
+            ceres::CostFunction* smoothCostFunction =
+                new ceres::AutoDiffCostFunction<SmoothOrientationCostFunctor, 2, 1>(
+                    new SmoothOrientationCostFunctor(orientations.at(i - 1),
+                                                     orientations.at(i + 1)));
+
+            problem.AddResidualBlock(smoothCostFunction, nullptr, &orientations.at(i));
+
+            // TODO weights
+            ceres::CostFunction* priorCostFunction =
+                new ceres::AutoDiffCostFunction<OrientationPriorCostFunctor, 1, 1>(
+                    new OrientationPriorCostFunctor(orientations.at(i)));
+
+            problem.AddResidualBlock(priorCostFunction, nullptr, &orientations.at(i));
+        }
+
+        // Run the solver!
+        ceres::Solver::Options options;
+        options.linear_solver_type = ceres::DENSE_QR;
+        options.minimizer_progress_to_stdout = true;
+        ceres::Solver::Summary summary;
+        Solve(options, &problem, &summary);
+
+        std::cout << summary.BriefReport() << "\n";
+
+        ARMARX_INFO << "orientations after: " << orientations;
+
+        return OrientationOptimizer::OptimizationResult{.trajectory = trajectory};
+    }
+
+
+} // namespace armarx::nav::glob_plan

source/Navigation/libraries/global_planning/optimization/OrientationOptimizer.h

+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <Navigation/libraries/core/Trajectory.h>
+
+namespace armarx::nav::glob_plan
+{
+
+
+    class OrientationOptimizer
+    {
+    public:
+        OrientationOptimizer(const core::Trajectory& trajectory);
+
+
+        struct OptimizationResult
+        {
+
+            core::Trajectory trajectory;
+
+            operator bool() const noexcept
+            {
+                return true; // FIXME implement
+            }
+        };
+
+
+        OptimizationResult optimize();
+
+    protected:
+    private:
+        core::Trajectory trajectory;
+    };
+} // namespace armarx::nav::glob_plan