ParamFindingRoutine.h

Go to the documentation of this file.

// Copyright 2016 Sensics, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef INCLUDED_ParamFindingRoutine_h_GUID_C2088279_D54B_4D8B_562E_5748C748DAD0
#define INCLUDED_ParamFindingRoutine_h_GUID_C2088279_D54B_4D8B_562E_5748C748DAD0

// Internal Includes
#include "OptimizationBase.h"
#include "UtilityFunctions.h"
#include "newuoa.h"

// Library/third-party includes
// - none

// Standard includes
#include <functional>
#include <iomanip>

namespace osvr {
namespace vbtracker {

    template <typename TrackingReferenceType, typename ParamSet>
    void runOptimizer(MeasurementsRows const &data, bool costOnly,
                      OptimCommonData const &commonData, std::size_t maxRuns) {

        std::cout << "Max runs: " << maxRuns << std::endl;

        using ParamVec = Vec<ParamSet::Dimension>;
        ParamVec x = ParamSet::getInitialVec(commonData);

        std::cout << "Optimizing, respectively: "
                  << ParamSet::getVecElementNames() << "\n";
        std::cout << "Initial vector:\n"
                  << x.format(getFullFormat()) << std::endl;
        auto functor = [&](ParamVec const &paramVec) -> double {
            ConfigParams params = commonData.initialParams;

            ParamSet::updateParamsFromVec(params, paramVec);

            auto optim = OptimData::make(params, commonData);

            MainAlgoUnderStudy mainAlgo;
            TrackingReferenceType ref;
            std::size_t samples = 0;
            double accum = 0;

            for (auto const &rowPtr : data) {
                mainAlgo(optim, *rowPtr);
                ref(optim, *rowPtr);
                if (ref.havePose() && mainAlgo.havePose()) {
                    auto cost =
                        costMeasurement(ref.getPose(), mainAlgo.getPose());
                    accum += cost;
                    samples++;
                }
            }

            if (samples > 0) {
                auto avgCost = (accum / static_cast<double>(samples));
                auto numResets = mainAlgo.getNumResets(optim);
                auto effectiveCost =
                    avgCost * (numResets + 1) * (numResets + 1) / samples;
                if (std::isnan(effectiveCost)) {
                    effectiveCost = getReallyBigCost();
                }
                std::cout << std::setw(15) << std::to_string(effectiveCost)
                          << " effective cost (average cost of " << std::setw(9)
                          << avgCost << " over " << std::setw(4) << samples
                          << " eligible frames with " << std::setw(2)
                          << numResets << " resets)\n";
                return effectiveCost;
            }
            std::cout << "No samples with pose for both algorithms?"
                      << std::endl;
            return getReallyBigCost();
        };

        if (costOnly) {
            auto cost = functor(x);
            std::cout
                << "The computed cost of these initial parameter values is "
                << cost << std::endl;
            return;
        }
        auto ret = ei_newuoa_wrapped(x, ParamSet::getRho(),
                                     static_cast<long>(maxRuns), functor);
        std::cout << "Optimizer returned " << ret
                  << " and these parameter values:" << std::endl;
        std::cout << x.format(getFullFormat()) << std::endl;
        std::cout << "for parameters described as, respectively,\n"
                  << ParamSet::getVecElementNames() << std::endl;
    }
    using ParamOptimizerFunc = std::function<void(
        MeasurementsRows const &, bool, OptimCommonData const &, std::size_t)>;
} // namespace vbtracker
} // namespace osvr
#endif // INCLUDED_ParamFindingRoutine_h_GUID_C2088279_D54B_4D8B_562E_5748C748DAD0