[C++] Grammer

4 minute read

추상 클래스

실제 구현을 포함하지 않고 인터페이스만을 제공한다. 추상 클래스의 메소드는 모두 순수 가상 함수로 선언되어 있으며, 해당 클래스는 직접 인스턴스화될 수 없고, 이러한 메소드를 실제로 구현한 서브클래스가 필요하다.

예시

mppi_controller/include/mppi_controller/mpc_template.hpp

#pragma once

#include <Eigen/Dense>
#include <grid_map_core/GridMap.hpp>
#include <utility>
#include "mppi_controller/common.hpp"

namespace mppi {
namespace cpu {
    /*
     * @brief virtual class for MPC
     */
    class MPCTemplate {
    public:
        virtual ~MPCTemplate(){};

        virtual std::pair<ControlSeq, double> solve(const State& initial_state) = 0;

        virtual void set_obstacle_map(const grid_map::GridMap& obstacle_map) = 0;

        virtual void set_reference_map(const grid_map::GridMap& reference_map) = 0;

        virtual ControlSeq get_control_seq() const = 0;

        virtual std::pair<std::vector<StateSeq>, std::vector<double>> get_state_seq_candidates(const int& num_samples) const = 0;

        virtual std::tuple<StateSeq, double, double, double> get_predictive_seq(const State& initial_state,
                                                                                const ControlSeq& control_input_seq) const = 0;

        virtual ControlSeqCovMatrices get_cov_matrices() const = 0;

        virtual std::pair<StateSeq, XYCovMatrices> get_proposed_state_distribution() const = 0;

        std::vector<double> softmax(const std::vector<double>& costs, const double& lambda, const int thread_num) const {
            const double min_cost = *std::min_element(costs.begin(), costs.end());
            double normalization_term = 1e-10;
            for (const auto& cost : costs) {
                normalization_term += std::exp(-(cost - min_cost) / lambda);
            }

            std::vector<double> softmax_costs(costs.size());
#pragma omp parallel for num_threads(thread_num)
            for (size_t i = 0; i < costs.size(); i++) {
                softmax_costs[i] = std::exp(-(costs[i] - min_cost) / lambda) / normalization_term;
            }

            return softmax_costs;
        };
    };

}  // namespace cpu
}  // namespace mppi

상속

// Kohei Honda, 2023

#pragma once
#include <algorithm>
#include <iostream>
#include <limits>
#include <mutex>
#include <string>
#include <vector>

#include <Eigen/Dense>
#include <array>
#include <grid_map_core/GridMap.hpp>
#include <memory>
#include <utility>

#include "mppi_controller/common.hpp"
#include "mppi_controller/mpc_base.hpp"
#include "mppi_controller/mpc_template.hpp"
#include "mppi_controller/prior_samples_with_costs.hpp"

namespace mppi {
namespace cpu {
    /**
     * @brief Stein Variational MPC, by A. Lambert, https://arxiv.org/abs/2011.07641
     */
    class SteinVariationalMPC : public MPCTemplate {
    public:
        SteinVariationalMPC(const Params::Common& common_params, const Params::SteinVariationalMPC& forward_mppi_params);
        ~SteinVariationalMPC(){};

        /**
         * @brief solve mppi problem and return optimal control sequence
         * @param initial_state initial state
         * @return optimal control sequence and collision rate
         */
        std::pair<ControlSeq, double> solve(const State& initial_state) override;
        /**
         * @brief set obstacle map and reference map
         */
        void set_obstacle_map(const grid_map::GridMap& obstacle_map) override;

        void set_reference_map(const grid_map::GridMap& reference_map) override;

        /**
         * @brief get state sequence candidates and their weights, top num_samples
         * @return std::pair<std::vector<StateSeq>, std::vector<double>> state sequence candidates and their weights
         */
        std::pair<std::vector<StateSeq>, std::vector<double>> get_state_seq_candidates(const int& num_samples) const override;

        std::tuple<StateSeq, double, double, double> get_predictive_seq(const State& initial_state,
                                                                        const ControlSeq& control_input_seq) const override;

        ControlSeqCovMatrices get_cov_matrices() const override;

        ControlSeq get_control_seq() const override;

        std::pair<StateSeq, XYCovMatrices> get_proposed_state_distribution() const override;

    private:
        const size_t prediction_step_size_;  //!< @brief prediction step size
        const int thread_num_;               //!< @brief number of thread for parallel computation
        // MPPI parameters
        const double lambda_;  //!< @brief temperature parameter [0, inf) of free energy.
        // temperature parameter is a balancing term between control cost and state cost.
        const double alpha_;  //!< @brief weighting parameter [0, 1], which balances control penalties from previous control sequence and nominal
                              //!< control sequence.
        // If lambda_ is small, weighted state cost is dominant, thus control sequence is more aggressive.
        // If lambda_ is large, weighted control cost is dominant, thus control sequence is more smooth.
        const double non_biased_sampling_rate_;  //!< @brief non-previous-control-input-biased sampling rate [0, 1], Add random noise to control
                                                 //!< sequence with this rate.
        const double steer_cov_;                 //!< @brief covariance of steering angle noise
        // double accel_cov_;                      //!< @brief covariance of acceleration noise

        // Stein Variational MPC parameters
        const int num_svgd_iteration_;
        const double steer_cov_for_grad_estimation_;
        const double svgd_step_size_;
        const bool is_max_posterior_estimation_;

        // Internal vars
        ControlSeq prev_control_seq_;
        std::vector<double> weights_ = {};  // for visualization

        // Libraries
        std::unique_ptr<MPCBase> mpc_base_ptr_;
        std::unique_ptr<PriorSamplesWithCosts> prior_samples_ptr_;
        std::vector<std::unique_ptr<PriorSamplesWithCosts>> grad_sampler_ptrs_;

    private:
        ControlSeq approx_grad_log_likelihood(const ControlSeq& mean_seq,
                                              const ControlSeq& noised_seq,
                                              const ControlSeqCovMatrices& inv_covs,
                                              const std::function<std::vector<double>(const PriorSamplesWithCosts&)>& calc_costs,
                                              PriorSamplesWithCosts* sampler) const;

        ControlSeq grad_log_normal_dist(const ControlSeq& sample, const ControlSeq& prior_mean, const ControlSeqCovMatrices& prior_covs) const;

        ControlSeqBatch approx_grad_posterior_batch(const PriorSamplesWithCosts& samples,
                                                    const std::function<std::vector<double>(const PriorSamplesWithCosts&)>& calc_costs) const;

        double RBF_kernel(const ControlSeq& seq1, const ControlSeq& seq2, const double& h) const;

        ControlSeq grad_RBF_kernel(const ControlSeq& seq, const ControlSeq& seq_const, const double& h) const;

        ControlSeqBatch phi_batch(const PriorSamplesWithCosts& samples, const ControlSeqBatch& grad_posterior_batch) const;

        std::pair<ControlSeq, ControlSeqCovMatrices> weighted_mean_and_sigma(const PriorSamplesWithCosts& samples,
                                                                             const std::vector<double>& weights) const;

        std::pair<ControlSeq, ControlSeqCovMatrices> estimate_mu_and_sigma(const PriorSamplesWithCosts& samples) const;

        std::vector<double> calc_weights(const PriorSamplesWithCosts& prior_samples_with_costs) const;
    };

}  // namespace cpu

}  // namespace mppi

람다 함수

람다 함수 : C++에서 익명 함수를 정의할 수 있는 방법. 람다 함수는 일반적인 함수와 같이 동작하지만, 코드 블록 안에서 즉석에서 정의되고 사용할 수 있다. 이는 특히 함수 객체를 인라인으로 정의하거나, 간단한 콜백 함수가 필요할 때 유용하다.

[capture](parameters) -> return_type {
    // function body
}

capture : 외부 변수를 람다 함수 내에서 사용할 수 있게 해주는 부분
parameters : 함수의 매개변수
return_type : 함수가 반환하는 타입
function body : 함수의 실제 내용

예시

이 람다 함수는 특정 비용 계산 작업을 수행하는 데 사용된다. func_calc_costs 는 SVGD 알고리즘에서 비용을 계산하기 위한 함수를 정의하며, 이 비용은 샘플의 품질을 평가하거나 최적화 문제의 해를 찾는 데 피료하다.

람다 함수의 장점은 코드가 명확하게 구성되고 필요한 컨텍스트 (여기서는 mpc_base_ptr_와 initial_state) 를 간편하게 캡처할 수 있다는 점이다.

auto func_calc_costs = [&](const PriorSamplesWithCosts& sampler) {
    const auto [costs, _flags] = mpc_base_ptr_->calc_sample_costs(sampler, initial_state);
    return costs;
};

Capture Clause [&] : & 는 람수 함수가 외부 변수들을 참조로 캡처한다는 것을 의미한다. 여기서는 mpc_base_ptr_ 와 initial_state 같은 외부 변수들을 사용할 수 있게 된다.
mpc_base_ptr_->calc_sample_costs(sampler, initial_state)를 호출하여 sampler와 initial_state를 사용하여 비용을 계산합니다. 이 함수는 두 개의 값을 반환하는데, 여기서는 costs와 _flags를 구조 분해(decomposition)하여 costs만 사용합니다.

타입 정의와 using

C++에서는 카워드 using을 사용하여 특정 데이터 타입을 정의한다.

예시

ControlSeqBatch 라는 새로운 타입을 정의한다. std::vector의 템플릿을 사용하여 여러 개의 Eigen::MatrixX 객체를 저장하는 컨테이너를 만든다.

namespace cpu {
    // ...
    using ControlSeqBatch = std::vector<Eigen::MatrixXd, Eigen::aligned_allocator<Eigen::MatrixXd>>;
}

std::vector<Eigen::MatrixXd>
- std::vector : C++ 표준 라이브러리에서 제공하는 동적 배열로, 크기를 동적으로 조절할 수 있는 연속적인 메모리 블럭
- Eigen::MatrixXd : Eigen 라이브러리에서 제공하는 동적 크기의 행렬 클래스. 이 클래스는 실수를 요소로 갖는 행렬을 표현한다.
Eigen::aligned_allocator<Eigen::Matrixd>
- aligned_allocator : 메모리 정렬을 보장하는 할당자를 제공
- Eigen 라이브러리는 고성능을 위해 메모리 정렬을 중요시한다. 특히 SIMD 명령어를 사용하는 경우 정렬된 메모리를 필요로 하므로 aligned_allocator를 사용하여 Eigen::MatrixXd 객체들이 올바르게 정렬되도록 보장한다.

ControlSeqBatch : 여러 개의 Eigen::MatrixXd 행렬을 저장할 수 있는 동적 배열. Eigen 라이브러리의 성능 최적화를 위해 정렬된 메모리를 사용한다. 이 타입은 최적화된 행렬 연산을 수행하거나 다양한 제어 시퀀스를 효율적으로 관리하기 위해 사용된다. 이는 SteinVariationalMPC::solve 함수와 같이 복잡한 계산을 필요로 하는 상황에서 유용하게 사용된다.

이 정의를 통해 ControlSeqBatch를 사용하는 코드에서 여러 행렬을 효율적으로 처리할 수 있다. MPC 알고리즘에서 굉장히 중요하다.

[C++] Grammer

추상 클래스

람다 함수

타입 정의와 using

You may also enjoy

[AI Semiconductor] In-Memory Computing

[AI Semiconductor] GPU and TPU

[Differential Equation] One-Dimentional Spring Problem

[Differential Equation] Existence and Uniqueness of Partial Differential Equation