diff options
Diffstat (limited to 'host/lib/cal')
| -rw-r--r-- | host/lib/cal/CMakeLists.txt | 32 | ||||
| -rw-r--r-- | host/lib/cal/interpolation.hpp | 71 | ||||
| -rw-r--r-- | host/lib/cal/interpolation.ipp | 199 | ||||
| -rw-r--r-- | host/lib/cal/power_container_impl.cpp | 80 | ||||
| -rw-r--r-- | host/lib/cal/power_container_impl.hpp | 77 |
5 files changed, 459 insertions, 0 deletions
diff --git a/host/lib/cal/CMakeLists.txt b/host/lib/cal/CMakeLists.txt new file mode 100644 index 000000000..5c616a9da --- /dev/null +++ b/host/lib/cal/CMakeLists.txt @@ -0,0 +1,32 @@ +# +# Copyright 2016 Ettus Research +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program 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/>. +# + +######################################################################## +# This file included, use CMake directory variables +######################################################################## +INCLUDE(CheckIncludeFileCXX) +MESSAGE(STATUS "") + +######################################################################## +# Convert types generation +######################################################################## +INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}) +INCLUDE_DIRECTORIES(${CMAKE_CURRENT_BINARY_DIR}) + +LIBUHD_APPEND_SOURCES( + ${CMAKE_CURRENT_SOURCE_DIR}/power_container_impl.cpp +) diff --git a/host/lib/cal/interpolation.hpp b/host/lib/cal/interpolation.hpp new file mode 100644 index 000000000..34f084fbd --- /dev/null +++ b/host/lib/cal/interpolation.hpp @@ -0,0 +1,71 @@ +// +// Copyright 2016 Ettus Research +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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/>. +// + +#ifndef INCLUDED_UHD_INTERPOLATION_HPP +#define INCLUDED_UHD_INTERPOLATION_HPP + +#include <uhd/exception.hpp> +#include <boost/format.hpp> +#include <map> +#include <cmath> + +namespace uhd { +namespace cal { + +template<typename in_type, typename out_type> +struct interp +{ +public: + typedef std::vector<in_type> args_t; + typedef std::map<args_t, out_type> container_t; + + /*! + * Nearest neighbor interpolation given a mapping: R^n -> R + * + * 1) search for the nearest point in R^n + * 2) find the nearest output scalars in R + * + * \param data input data container + * \param args input data point + * \returns interpolated output value + */ + const out_type nn_interp(container_t &data, const args_t &args); + + /*! + * Bilinear interpolation given a mapping: R^2 -> R + * + * 1) search for 4 surrounding points in R^2 + * 2) find the output scalars in R + * 3) solve the system of equations given our input mappings + * + * \param data input data container + * \param args input data point + * \returns interpolated output value + */ + const out_type bl_interp(container_t &data, const args_t &args); + +private: + /*! + * Calculate the distance between two points + */ + static in_type calc_dist(const args_t &a, const args_t &b); +}; + +} // namespace cal +} // namespace uhd + +#endif /* INCLUDED_UHD_INTERPOLATION_HPP */ diff --git a/host/lib/cal/interpolation.ipp b/host/lib/cal/interpolation.ipp new file mode 100644 index 000000000..f58b70b68 --- /dev/null +++ b/host/lib/cal/interpolation.ipp @@ -0,0 +1,199 @@ +// +// Copyright 2016 Ettus Research +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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/>. +// + +#ifndef INCLUDED_UHD_INTERPOLATION_IPP +#define INCLUDED_UHD_INTERPOLATION_IPP + +#include "interpolation.hpp" +#include <uhd/utils/msg.hpp> +#include <boost/numeric/ublas/io.hpp> +#include <boost/numeric/ublas/matrix.hpp> +#include <boost/numeric/ublas/lu.hpp> + +using namespace boost::numeric; + +namespace uhd { +namespace cal { + +#define CAL_INTERP_METHOD(return_type, method, args, ...) \ + template<typename in_type, typename out_type> \ + return_type interp<in_type, out_type>::\ + method(args, __VA_ARGS__) + +#define ARGS_T typename interp<in_type, out_type>::args_t +#define CONTAINER_T typename interp<in_type, out_type>::container_t + +CAL_INTERP_METHOD(in_type, calc_dist, const ARGS_T &a, const ARGS_T &b) +{ + in_type dist = 0; + for (size_t i = 0; i < std::min(a.size(), b.size()); i++) + { + dist += std::abs(a[i] - b[i]); + } + return dist; +} + +CAL_INTERP_METHOD(const out_type, nn_interp, CONTAINER_T &data, const ARGS_T &args) +{ + // Check the cache for the output + if (data.find(args) != data.end()) { + return data[args]; + } + + out_type output = 0; + in_type min_dist = 0; + typename container_t::const_iterator citer; + for (citer = data.begin(); citer != data.end(); citer++) + { + in_type dist = calc_dist(citer->first, args); + if (citer == data.begin() || dist < min_dist) { + min_dist = dist; + output = data[citer->first]; + } + } + + return output; +} + +CAL_INTERP_METHOD(const out_type, bl_interp, CONTAINER_T &data, const ARGS_T &args) +{ + if (args.size() != 2) { + throw uhd::assertion_error(str(boost::format( + "Bilinear interpolation expects 2D values. Received %d.") + % args.size() + )); + } + + if (data.size() < 4) { + throw uhd::assertion_error(str(boost::format( + "Bilinear interpolation requires at least 4 input points. Found %d.") + % data.size() + )); + } + + // Locate the nearest 4 points + typedef std::pair<interp<in_type, out_type>::args_t, out_type> cal_pair_t; + typename std::vector<cal_pair_t> nearest; + + // Initialize the resulting pair to something + cal_pair_t pair = *data.begin(); + + for (size_t i = 0; i < 4; i++) { + bool init = true; + in_type min_dist = 0; + typename container_t::const_iterator citer; + for (citer = data.begin(); citer != data.end(); citer++) + { + cal_pair_t temp = *citer; + if (std::find(nearest.begin(), nearest.end(), temp) == nearest.end()) + { + in_type dist = calc_dist(citer->first, args); + if (dist < min_dist || init) + { + min_dist = dist; + pair = temp; + init = false; + } + } + } + // Push back the nearest pair + nearest.push_back(pair); + } + + // + // Since these points are not grid aligned, + // we perform irregular bilinear interpolation. + // This math involves finding our interpolation + // function using lagrange multipliers: + // + // f(x, y) = ax^2 + bxy + cy^2 + dx + ey + f + // + // The solution is to solve the following system: + // + // A x b + // | E X' | | s | - | 0 | + // | X 0 | | l | - | z | + // + // where s is a vector of the above coefficients. + // + typename ublas::matrix<in_type> A(10, 10, 0.0); + + // E + A(0, 0) = 1.0; A(1, 1) = 1.0; A(2, 2) = 1.0; + + in_type x1, x2, x3, x4; + in_type y1, y2, y3, y4; + + x1 = nearest[0].first[0]; y1 = nearest[0].first[1]; + x2 = nearest[1].first[0]; y2 = nearest[1].first[1]; + x3 = nearest[2].first[0]; y3 = nearest[2].first[1]; + x4 = nearest[3].first[0]; y4 = nearest[3].first[1]; + + // X + A(0, 6) = x1*x1; A(1, 6) = x1*y1; A(2, 6) = y1*y1; A(3, 6) = x1; A(4, 6) = y1; A(5, 6) = 1.0; + A(0, 7) = x2*x2; A(1, 7) = x2*y2; A(2, 7) = y2*y2; A(3, 7) = x2; A(4, 7) = y2; A(5, 7) = 1.0; + A(0, 8) = x3*x3; A(1, 8) = x3*y3; A(2, 8) = y3*y3; A(3, 8) = x3; A(4, 8) = y3; A(5, 8) = 1.0; + A(0, 9) = x4*x4; A(1, 9) = x4*y4; A(2, 9) = y4*y4; A(3, 9) = x4; A(4, 9) = y4; A(5, 9) = 1.0; + + // X' + A(6, 0) = x1*x1; A(6, 1) = x1*y1; A(6, 2) = y1*y1; A(6, 3) = x1; A(6, 4) = y1; A(6, 5) = 1.0; + A(7, 0) = x2*x2; A(7, 1) = x2*y2; A(7, 2) = y2*y2; A(7, 3) = x2; A(7, 4) = y2; A(7, 5) = 1.0; + A(8, 0) = x3*x3; A(8, 1) = x3*y3; A(8, 2) = y3*y3; A(8, 3) = x3; A(8, 4) = y3; A(8, 5) = 1.0; + A(9, 0) = x4*x4; A(9, 1) = x4*y4; A(9, 2) = y4*y4; A(9, 3) = x4; A(9, 4) = y4; A(9, 5) = 1.0; + + // z + typename ublas::vector<in_type> b(10, 0.0); + b(6) = nearest[0].second; + b(7) = nearest[1].second; + b(8) = nearest[2].second; + b(9) = nearest[3].second; + + typename ublas::matrix<in_type> A_t = A; + typename ublas::vector<in_type> s = b; + typename ublas::permutation_matrix<in_type> P(A_t.size1()); + + // Use LUP factorization to solve for the coefficients + // We're solving the problem in the form of Ax = b + bool is_singular = ublas::lu_factorize(A_t, P); + + out_type output = 0; + + // Fall back to 1D interpolation if the matrix is singular + if (is_singular) { + // Warn the user that the A matrix is singular + UHD_MSG(warning) << "Bilinear interpolation: singular matrix detected." << std::endl + << "Performing 1D linear interpolation against the nearest measurements." << std::endl + << "Provide calibration data with more measurements" << std::endl; + + output = (b[7] - b[6]) / 2.0; + output += b[6]; + return output; + } + ublas::lu_substitute(A_t, P, s); + + in_type x = args[0]; + in_type y = args[1]; + + // Utilize the solution to calculate the interpolation function + output = s[0]*x*x + s[1]*x*y + s[2]*y*y + s[3]*x + s[4]*y + s[5]; + return output; +} + +} // namespace cal +} // namespace uhd + +#endif /* INCLUDED_UHD_INTERPOLATION_IPP */ diff --git a/host/lib/cal/power_container_impl.cpp b/host/lib/cal/power_container_impl.cpp new file mode 100644 index 000000000..5d07c3b7e --- /dev/null +++ b/host/lib/cal/power_container_impl.cpp @@ -0,0 +1,80 @@ +// +// Copyright 2016 Ettus Research +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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/>. +// + +#include "power_container_impl.hpp" +#include <uhd/exception.hpp> +#include <boost/format.hpp> + +using namespace uhd; +using namespace uhd::cal; + +power_container::sptr power_container::make() +{ + return power_container::sptr(new power_container_impl()); +} + +power_container_impl::power_container_impl() : + _nargs(0), _mode(interp_mode_t::NEAREST) +{ + /* NOP */ +} + +double power_container_impl::get(const std::vector<double> &args) +{ + this->verify_nargs(args); + switch (_mode) + { + case interp_mode_t::BILINEAR : + return _interpolator.bl_interp(_data, args); + case interp_mode_t::NEAREST : + return _interpolator.nn_interp(_data, args); + default: + return _interpolator.nn_interp(_data, args); + } +} + +void power_container_impl::add(const double output, const std::vector<double> &args) +{ + if (_nargs == 0) + { + _nargs = args.size(); + _mode = _nargs == 2 ? interp_mode_t::BILINEAR : interp_mode_t::NEAREST; + } + this->verify_nargs(args); + _data[args] = output; +} + +void power_container_impl::add_metadata(const power_container::metadata_t &data) +{ + _metadata = data; +} + +const power_container_impl::metadata_t &power_container_impl::get_metadata() +{ + return _metadata; +} + +void power_container_impl::verify_nargs(const std::vector<double> &args) +{ + // Check that the number of arguments expected are correct + if (args.size() != _nargs) { + throw uhd::assertion_error(str(boost::format( + "power_container_impl: Expected %d number of arguments/values instead of %d") + % _nargs % args.size() + )); + } +} diff --git a/host/lib/cal/power_container_impl.hpp b/host/lib/cal/power_container_impl.hpp new file mode 100644 index 000000000..4c2bcab79 --- /dev/null +++ b/host/lib/cal/power_container_impl.hpp @@ -0,0 +1,77 @@ +// +// Copyright 2016 Ettus Research +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program 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/>. +// + +#ifndef INCLUDED_UHD_CAL_POWER_CONTAINER_IMPL_HPP +#define INCLUDED_UHD_CAL_POWER_CONTAINER_IMPL_HPP + +#include "interpolation.ipp" +#include <uhd/cal/power_container.hpp> + +namespace uhd { +namespace cal { + +enum interp_mode_t +{ + BILINEAR, //! linear interpolation + NEAREST //! nearest neighbor interpolation +}; + +class power_container_impl : public power_container { +public: + typedef std::map<std::vector<double>, double> container_t; + + power_container_impl(); + + double get(const std::vector<double> &args); + const metadata_t &get_metadata(); + + void add(const double output, const std::vector<double> &args); + void add_metadata(const metadata_t &data); + +private: + // Container data to be serialized + size_t _nargs; + metadata_t _metadata; + interp_mode_t _mode; + container_t _data; + + interp<double, double> _interpolator; + + void verify_nargs(const std::vector<double> &args); + +protected: + friend class boost::serialization::access; + + void serialize(iarchive_type & ar, const unsigned int) { + ar & _nargs; + ar & _metadata; + ar & _mode; + ar & _data; + } + + void serialize(oarchive_type & ar, const unsigned int) { + ar & _nargs; + ar & _metadata; + ar & _mode; + ar & _data; + } +}; + +} // namespace cal +} // namespace uhd + +#endif /* INCLUDED_UHD_CAL_POWER_CONTAINER_IMPL_HPP */ |