text:
/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_TEST_LAWS_BASE_HPP
#define BOOST_HANA_TEST_LAWS_BASE_HPP

#include <boost/hana/assert.hpp>
#include <boost/hana/bool.hpp>
#include <boost/hana/comparable.hpp>
#include <boost/hana/constant.hpp>
#include <boost/hana/detail/std/forward.hpp>
#include <boost/hana/foldable.hpp>
#include <boost/hana/functional/infix.hpp>
#include <boost/hana/logical.hpp>
#include <boost/hana/orderable.hpp>
#include <boost/hana/tuple.hpp>

#include <iostream>
#include <type_traits>
#include <utility>

namespace boost { namespace hana {
    //////////////////////////////////////////////////////////////////////////
    // Misc
    //////////////////////////////////////////////////////////////////////////
    namespace test {
        struct laws;

        auto foreach = hana::for_each;
        auto foreach3 = [](auto xs, auto pred) {
            hana::for_each(xs, [=](auto a) {
            hana::for_each(xs, [=](auto b) {
            hana::for_each(xs, [=](auto c) {
                pred(a, b, c);
            });});});
        };

        auto foreach2 = [](auto xs, auto pred) {
            hana::for_each(xs, [=](auto a) {
            hana::for_each(xs, [=](auto b) {
                pred(a, b);
            });});
        };

        struct _implies {
            template <typename P, typename Q>
            constexpr decltype(auto) operator()(P&& p, Q&& q) const {
                return hana::or_(hana::not_(detail::std::forward<P>(p)),
                                 detail::std::forward<Q>(q));
            }
        };
        constexpr auto implies = hana::infix(_implies{});

        struct _iff {
            template <typename P, typename Q>
            constexpr decltype(auto) operator()(P&& p, Q&& q) const {
                return hana::and_(implies(p, q), implies(q, p));
            }
        };
        constexpr auto iff = hana::infix(_iff{});

        template <typename Cond, typename F>
        constexpr decltype(auto) only_when_(Cond cond, F f) {
            return hana::eval_if(cond, f, [](auto){ });
        }

        //////////////////////////////////////////////////////////////////////
        // Tracked
        //////////////////////////////////////////////////////////////////////
        struct Tracked {
            enum class State { CONSTRUCTED, MOVED_FROM, DESTROYED };

            int value;
            State state;

            explicit Tracked(int k) : value{k}, state{State::CONSTRUCTED} {
                // std::cerr << "constructing " << *this << '\n';
            }

            Tracked(Tracked const& t) : value{t.value}, state{State::CONSTRUCTED} {
                BOOST_HANA_RUNTIME_CHECK(t.state != State::MOVED_FROM &&
                    "copying a moved-from object");

                BOOST_HANA_RUNTIME_CHECK(t.state != State::DESTROYED &&
                    "copying a destroyed object");

                // std::cerr << "copying " << *this << '\n';
            }

            Tracked(Tracked&& t) : value{t.value}, state{State::CONSTRUCTED} {
                BOOST_HANA_RUNTIME_CHECK(t.state != State::MOVED_FROM &&
                    "double moving from an object");

                BOOST_HANA_RUNTIME_CHECK(t.state != State::DESTROYED &&
                    "moving from a destroyed object");

                // std::cerr << "moving " << t << '\n';
                t.state = State::MOVED_FROM;
            }

            Tracked& operator=(Tracked const& other) {
                BOOST_HANA_RUNTIME_CHECK(this->state != State::DESTROYED &&
                    "assigning to a destroyed object");

                BOOST_HANA_RUNTIME_CHECK(other.state != State::MOVED_FROM &&
                    "assigning a moved-from object");

                BOOST_HANA_RUNTIME_CHECK(other.state != State::DESTROYED &&
                    "assigning a destroyed object");

                // std::cerr << "assigning " << other << " to " << *this << '\n';
                this->value = other.value;
                return *this;
            }

            Tracked& operator=(Tracked&& other) {
                BOOST_HANA_RUNTIME_CHECK(this->state != State::DESTROYED &&
                    "assigning to a destroyed object");

                BOOST_HANA_RUNTIME_CHECK(other.state != State::MOVED_FROM &&
                    "double-moving from an object");

                BOOST_HANA_RUNTIME_CHECK(other.state != State::DESTROYED &&
                    "assigning a destroyed object");

                // std::cerr << "assigning " << other << " to " << *this << '\n';
                this->value = other.value;
                other.state = State::MOVED_FROM;
                return *this;
            }

            ~Tracked() {
                BOOST_HANA_RUNTIME_CHECK(state != State::DESTROYED &&
                    "double-destroying an object");

                // std::cerr << "destructing " << *this << '\n';
                state = State::DESTROYED;
            }

            template <typename CharT, typename Traits>
            friend std::basic_ostream<CharT, Traits>&
            operator<<(std::basic_ostream<CharT, Traits>& os, Tracked const& t) {
                os << "Tracked{" << t.value << "}";
                switch (t.state) {
                case State::CONSTRUCTED:
                    os << "[ok]"; break;
                case State::MOVED_FROM:
                    os << "[moved from]"; break;
                case State::DESTROYED:
                    os << "[destroyed]"; break;
                default:
                    BOOST_HANA_RUNTIME_CHECK(false && "never reached");
                }
                return os;
            }
        };

        //////////////////////////////////////////////////////////////////////
        // InjectionResult
        //////////////////////////////////////////////////////////////////////
        struct InjectionResult { };

        template <int i, typename ...X>
        struct injection_result {
            struct hana { using datatype = InjectionResult; };
            static constexpr int injection_id = i;
            boost::hana::_tuple<X...> args;
            Tracked tracker;

            template <typename ...Y, typename = decltype(_tuple<X...>{std::declval<Y>()...})>
            constexpr explicit injection_result(Y&& ...y)
                : args{boost::hana::detail::std::forward<Y>(y)...}, tracker{i}
            { }
        };

        //! A monotonic injective function.
        //!
        //! This is used in the unit tests, where we often just need a function
        //! which preserves equality and order, but which also satisfies the
        //! following law for all `Injection`s `f` and `g`:
        //! @code
        //!     f(x) == g(x) if and only if f === g
        //! @endcode
        //! where `===` means _was created by the same call to `injection`_.
        //! This allows creating several such functions in the unit tests while
        //! conserving precious information such as the fact that
        //! `f(g(x)) != g(f(x))`.
        template <int i>
        struct _injection {
            template <typename ...X>
            constexpr auto operator()(X&& ...x) const {
                return injection_result<i,
                    typename hana::detail::std::decay<X>::type...
                >{hana::detail::std::forward<X>(x)...};
            }
        };
    } // end namespace test

    using test::Tracked;

    template <>
    struct equal_impl<test::InjectionResult, test::InjectionResult> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y) {
            return hana::and_(
                hana::bool_<X::injection_id == Y::injection_id>,
                hana::equal(x.args, y.args)
            );
        }
    };

    template <>
    struct less_impl<test::InjectionResult, test::InjectionResult> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y) {
            static_assert(X::injection_id == Y::injection_id,
            "can't order the result of two different injections");
            return hana::less(x.args, y.args);
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Integer
    //////////////////////////////////////////////////////////////////////////
    namespace test {
        enum class Policy : int {
            // One of those is mandatory
              Constant   = 1 << 0
            , Constexpr  = 1 << 1
            , Runtime    = 1 << 2

            // Those are optional
            , Tracked    = 1 << 3
            , Comparable = 1 << 4
            , Orderable  = 1 << 5
        };

        constexpr bool operator&&(Policy a, Policy b) {
            return static_cast<int>(a) && static_cast<int>(b);
        }

        constexpr bool operator&&(Policy a, bool b) {
            return static_cast<int>(a) && b;
        }

        constexpr bool operator&&(bool a, Policy b) {
            return a && static_cast<int>(b);
        }

        constexpr bool operator||(Policy a, Policy b) {
            return static_cast<int>(a) || static_cast<int>(b);
        }

        constexpr bool operator||(Policy a, bool b) {
            return static_cast<int>(a) || b;
        }

        constexpr bool operator||(bool a, Policy b) {
            return a || static_cast<int>(b);
        }

        constexpr bool operator!(Policy a) {
            return !static_cast<int>(a);
        }

        constexpr Policy operator|(Policy a, Policy b) {
            return static_cast<Policy>(static_cast<int>(a) | static_cast<int>(b));
        }

        constexpr Policy operator&(Policy a, Policy b) {
            return static_cast<Policy>(static_cast<int>(a) & static_cast<int>(b));
        }

        template <Policy policy, typename = void>
        struct Integer { };

        template <Policy policy>
        struct Integer<policy, std::enable_if_t<!!(policy & Policy::Constant)>> {
            using value_type = int;
        };

        template <int i, Policy policy>
        struct integer {
            static_assert(
            !!(policy & (Policy::Constant | Policy::Constexpr | Policy::Runtime))
            , "You must choose at least one of Constant, Constexpr and Runtime.");

            static constexpr int value = i;
            constexpr operator int() const { return value; }
            struct hana { using datatype = Integer<policy>; };

            struct notrack { constexpr notrack(int) { } };
            //! @todo Fix broken usages of `value` and then enable this.
            std::conditional_t<!!(policy & Policy::Tracked), Tracked, notrack> tracker{i};
        };

        template <int i>
        using eq = integer<i, Policy::Comparable | Policy::Runtime>;

        template <int i>
        using ct_eq = integer<i, Policy::Comparable | Policy::Constant>;

        template <int i>
        using cx_eq = integer<i, Policy::Comparable | Policy::Constexpr>;

        template <int i>
        using ord = integer<i, Policy::Orderable | Policy::Runtime>;

        template <int i>
        using ct_ord = integer<i, Policy::Orderable | Policy::Constant>;

        template <int i>
        using cx_ord = integer<i, Policy::Orderable | Policy::Constexpr>;

        template <int i>
        using _constant = integer<i,
              Policy::Constant
            | Policy::Comparable
            | Policy::Orderable
        >;
    }

    //////////////////////////////////////////////////////////////////////////
    // Model of Constant
    //////////////////////////////////////////////////////////////////////////
    template <test::Policy policy>
    struct value_impl<test::Integer<policy>, when<
        !!(policy & test::Policy::Constant)
    >> {
        template <typename C>
        static constexpr auto apply()
        { return C::value; }
    };

    template <test::Policy policy, typename C>
    struct to_impl<test::Integer<policy>, C, when<
        (policy & test::Policy::Constant) &&
        _models<Constant, C>{}() &&
        detail::std::is_integral<typename C::value_type>{}
    >>
        : embedding<is_embedded<typename C::value_type, int>{}>
    {
        template <typename X>
        static constexpr auto apply(X const&) {
            constexpr int v = hana::value<X>();
            return test::integer<v, policy>{};
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Model of Comparable
    //////////////////////////////////////////////////////////////////////////
    template <test::Policy p1, test::Policy p2>
    struct equal_impl<test::Integer<p1>, test::Integer<p2>, when<
        // both Comparable or Orderable
        (p1 & (test::Policy::Comparable | test::Policy::Orderable)) &&
        (p2 & (test::Policy::Comparable | test::Policy::Orderable)) &&

        // one Constexpr and the other Constant, or both Constexpr
        (((p1 & test::Policy::Constant) && (p2 & test::Policy::Constexpr)) ||
        ((p1 & test::Policy::Constexpr) && (p2 & test::Policy::Constant))  ||
        ((p1 & test::Policy::Constexpr) && (p2 & test::Policy::Constexpr)))
    >> {
        template <typename X, typename Y>
        static constexpr bool apply(X const&, Y const&)
        { return X::value == Y::value; }
    };

    template <test::Policy p1, test::Policy p2>
    struct equal_impl<test::Integer<p1>, test::Integer<p2>, when<
        // both Comparable or Orderable
        (p1 & (test::Policy::Comparable | test::Policy::Orderable)) &&
        (p2 & (test::Policy::Comparable | test::Policy::Orderable)) &&

        // either one is Runtime
        ((p1 & test::Policy::Runtime) || (p2 & test::Policy::Runtime))
    >> {
        template <typename X, typename Y>
        static bool apply(X const&, Y const&)
        { return X::value == Y::value; }
    };

    //////////////////////////////////////////////////////////////////////////
    // Model of Orderable
    //////////////////////////////////////////////////////////////////////////
    template <test::Policy p1, test::Policy p2>
    struct less_impl<test::Integer<p1>, test::Integer<p2>, when<
        // both Orderable
        (p1 & test::Policy::Orderable) && (p2 & test::Policy::Orderable) &&

        // one Constexpr and the other Constant, or both Constexpr
        (((p1 & test::Policy::Constant) && (p2 & test::Policy::Constexpr)) ||
        ((p1 & test::Policy::Constexpr) && (p2 & test::Policy::Constant))  ||
        ((p1 & test::Policy::Constexpr) && (p2 & test::Policy::Constexpr)))
    >> {
        template <typename X, typename Y>
        static constexpr bool apply(X const&, Y const&)
        { return X::value < Y::value; }
    };

    template <test::Policy p1, test::Policy p2>
    struct less_impl<test::Integer<p1>, test::Integer<p2>, when<
        // both Orderable
        (p1 & test::Policy::Orderable) && (p2 & test::Policy::Orderable) &&

        // either one is Runtime
        ((p1 & test::Policy::Runtime) || (p2 & test::Policy::Runtime))
    >> {
        template <typename X, typename Y>
        static bool apply(X const&, Y const&)
        { return X::value < Y::value; }
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_TEST_LAWS_BASE_HPP


text:
#include <time.h>
#include "wasm/types/time.hpp"
#include "wasm/wasm_serialize_reflect.hpp"
#include <boost/date_time/posix_time/posix_time.hpp>

namespace wasm {

    time_point time_point::from_iso_string( const std::string& s )
    {
        auto dot = s.find( '.' );
        if( dot == std::string::npos )
           return time_point( time_point_sec::from_iso_string( s ) );
        else {
           auto ms = s.substr( dot );
           ms[0] = '1';
           while( ms.size() < 4 ) ms.push_back('0');
           return time_point( time_point_sec::from_iso_string( s ) ) + milliseconds( std::strtoll(ms.c_str(), nullptr, 10) - 1000 );
        }
    }

    std::string time_point::to_iso_string() const{
        auto count = elapsed.count();
        if (count >= 0) {
           uint64_t secs = (uint64_t)count / 1000000ULL;
           uint64_t msec = ((uint64_t)count % 1000000ULL) / 1000ULL;
           string padded_ms = to_string((uint64_t)(msec + 1000ULL)).substr(1);

           return time_point_sec(secs).to_iso_string() + "." + padded_ms;
         }

         return std::string();
    }

    time_point_sec time_point_sec::from_iso_string( const std::string& s )
    {

        int year, month, day, hour, minute, second;
        sscanf((char *) s.data(), "%4d-%2d-%2dT%2d:%2d:%2d", &year, &month, &day, &hour, &minute, &second);
        std::tm time = {};
        time.tm_year = year - 1900;
        time.tm_mon  = month - 1;
        time.tm_mday = day;
        time.tm_hour = hour;
        time.tm_min  = minute;
        time.tm_sec  = second;
        time.tm_isdst  = 0;
        time_t seconds = mktime(&time);

        return time_point_sec(seconds);

    }

    std::string time_point_sec::to_iso_string() const {

       std::time_t t = time_t(sec_since_epoch() );

       char utc[128];
       std::tm* p = localtime(&t);
       sprintf(utc, "%4d-%02d-%02d %02d:%02d:%02d",
               p->tm_year + 1900,
               p->tm_mon + 1,
               p->tm_mday,
               p->tm_hour,
               p->tm_min,
               p->tm_sec);

       return string(utc);

    }

    // inline std::string to_string(const wasm::microseconds& v){
    //   return v.to_iso_string();
    // }

    // inline std::string to_string(const wasm::time_point& v){
    //     return v.to_iso_string();
    // }

    // inline std::string to_string(const wasm::time_point_sec& v){
    //   return v.to_iso_string();

    // }

} // namespace wasm

ext: cpp

max_issues_repo_name: linnbenton/WaykiChain

max_forks_repo_name: linnbenton/WaykiChain


text:
// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
//       its contributors may be used to endorse or promote products derived
//       from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: Johannes L. Schoenberger (jsch at inf.ethz.ch)

#define TEST_NAME "base/projection"
#include "util/testing.h"

#include <Eigen/Core>

#include "base/camera_models.h"
#include "base/pose.h"
#include "base/projection.h"
#include "util/math.h"

using namespace colmap;

BOOST_AUTO_TEST_CASE(TestComposeProjectionMatrix) {
  const Eigen::Matrix3d R = EulerAnglesToRotationMatrix(0, 1, 2);
  const Eigen::Vector4d qvec = RotationMatrixToQuaternion(R);
  const Eigen::Vector3d tvec = Eigen::Vector3d::Random();

  const auto proj_matrix1 = ComposeProjectionMatrix(qvec, tvec);
  const auto proj_matrix2 = ComposeProjectionMatrix(R, tvec);

  BOOST_CHECK((proj_matrix1 - proj_matrix2).norm() < 1e-6);
  BOOST_CHECK((proj_matrix1.leftCols<3>() - R).norm() < 1e-6);
  BOOST_CHECK_CLOSE((proj_matrix1.rightCols<1>() - tvec).norm(), 0, 1e-6);
  BOOST_CHECK_CLOSE((proj_matrix2.leftCols<3>() - R).norm(), 0, 1e-6);
  BOOST_CHECK_CLOSE((proj_matrix2.rightCols<1>() - tvec).norm(), 0, 1e-6);
}

BOOST_AUTO_TEST_CASE(TestInvertProjectionMatrix) {
  const Eigen::Matrix3d R = EulerAnglesToRotationMatrix(0, 1, 2);
  const Eigen::Vector3d tvec = Eigen::Vector3d::Random();

  const auto proj_matrix = ComposeProjectionMatrix(R, tvec);
  const auto inv_proj_matrix = InvertProjectionMatrix(proj_matrix);
  const auto inv_inv_proj_matrix = InvertProjectionMatrix(inv_proj_matrix);

  BOOST_CHECK((proj_matrix - inv_inv_proj_matrix).norm() < 1e-6);
}

BOOST_AUTO_TEST_CASE(TestComputeClosestRotationMatrix) {
  const Eigen::Matrix3d A = Eigen::Matrix3d::Identity();
  BOOST_CHECK_LT((ComputeClosestRotationMatrix(A) - A).norm(), 1e-6);
  BOOST_CHECK_LT((ComputeClosestRotationMatrix(2 * A) - A).norm(), 1e-6);
}

BOOST_AUTO_TEST_CASE(TestDecomposeProjectionMatrix) {
  for (int i = 1; i < 100; ++i) {
    Eigen::Matrix3d ref_K = i * Eigen::Matrix3d::Identity();
    ref_K(0, 2) = i;
    ref_K(1, 2) = 2 * i;
    const Eigen::Matrix3d ref_R = EulerAnglesToRotationMatrix(i, 2 * i, 3 * i);
    const Eigen::Vector3d ref_T = Eigen::Vector3d::Random();
    const Eigen::Matrix3x4d ref_P =
        ref_K * ComposeProjectionMatrix(ref_R, ref_T);
    Eigen::Matrix3d K;
    Eigen::Matrix3d R;
    Eigen::Vector3d T;
    DecomposeProjectionMatrix(ref_P, &K, &R, &T);
    BOOST_CHECK(ref_K.isApprox(K, 1e-6));
    BOOST_CHECK(ref_R.isApprox(R, 1e-6));
    BOOST_CHECK(ref_T.isApprox(T, 1e-6));
  }
}

BOOST_AUTO_TEST_CASE(TestCalculateReprojectionError) {
  const Eigen::Vector4d qvec = Eigen::Vector4d::Random().normalized();
  const Eigen::Vector3d tvec = Eigen::Vector3d::Random();

  const auto proj_matrix = ComposeProjectionMatrix(qvec, tvec);

  const Eigen::Vector3d point3D = Eigen::Vector3d::Random();
  const Eigen::Vector3d point2D_h = proj_matrix * point3D.homogeneous();
  const Eigen::Vector2d point2D = point2D_h.hnormalized();

  Camera camera;
  camera.InitializeWithId(SimplePinholeCameraModel::model_id, 1, 0, 0);

  const double error1 =
      CalculateReprojectionError(point2D, point3D, proj_matrix, camera);
  BOOST_CHECK_CLOSE(error1, 0, 1e-6);

  const double error2 = CalculateReprojectionError(point2D.array() + 1, point3D,
                                                   proj_matrix, camera);
  BOOST_CHECK_CLOSE(error2, std::sqrt(2), 1e-6);
}

BOOST_AUTO_TEST_CASE(TestCalculateAngularError) {
  const Eigen::Vector4d qvec = ComposeIdentityQuaternion();
  const Eigen::Vector3d tvec = Eigen::Vector3d(0, 0, 0);

  const auto proj_matrix = ComposeProjectionMatrix(qvec, tvec);
  Camera camera;
  camera.SetModelId(SimplePinholeCameraModel::model_id);
  camera.Params() = {1, 0, 0};

  const double error1 = CalculateAngularError(
      Eigen::Vector2d(0, 0), Eigen::Vector3d(0, 0, 1), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error1, 0, 1e-6);

  const double error2 = CalculateAngularError(
      Eigen::Vector2d(0, 0), Eigen::Vector3d(0, 1, 1), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error2, M_PI / 4, 1e-6);

  const double error3 = CalculateAngularError(
      Eigen::Vector2d(0, 0), Eigen::Vector3d(0, 5, 5), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error3, M_PI / 4, 1e-6);

  const double error4 = CalculateAngularError(
      Eigen::Vector2d(1, 0), Eigen::Vector3d(0, 0, 1), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error4, M_PI / 4, 1e-6);

  const double error5 = CalculateAngularError(
      Eigen::Vector2d(2, 0), Eigen::Vector3d(0, 0, 1), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error5, 1.10714872, 1e-6);

  const double error6 = CalculateAngularError(
      Eigen::Vector2d(2, 0), Eigen::Vector3d(1, 0, 1), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error6, 1.10714872 - M_PI / 4, 1e-6);

  const double error7 = CalculateAngularError(
      Eigen::Vector2d(2, 0), Eigen::Vector3d(5, 0, 5), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error7, 1.10714872 - M_PI / 4, 1e-6);

  const double error8 = CalculateAngularError(
      Eigen::Vector2d(1, 0), Eigen::Vector3d(-1, 0, 1), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error8, M_PI / 2, 1e-6);

  const double error9 = CalculateAngularError(
      Eigen::Vector2d(1, 0), Eigen::Vector3d(-1, 0, 0), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error9, M_PI * 3 / 4, 1e-6);

  const double error10 = CalculateAngularError(
      Eigen::Vector2d(1, 0), Eigen::Vector3d(-1, 0, -1), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error10, M_PI, 1e-6);

  const double error11 = CalculateAngularError(
      Eigen::Vector2d(1, 0), Eigen::Vector3d(0, 0, -1), proj_matrix, camera);
  BOOST_CHECK_CLOSE(error11, M_PI * 3 / 4, 1e-6);
}

BOOST_AUTO_TEST_CASE(TestCalculateDepth) {
  const Eigen::Vector4d qvec(1, 0, 0, 0);
  const Eigen::Vector3d tvec(0, 0, 0);
  const auto proj_matrix = ComposeProjectionMatrix(qvec, tvec);

  // In the image plane
  const double depth1 = CalculateDepth(proj_matrix, Eigen::Vector3d(0, 0, 0));
  BOOST_CHECK_CLOSE(depth1, 0, 1e-10);
  const double depth2 = CalculateDepth(proj_matrix, Eigen::Vector3d(0, 2, 0));
  BOOST_CHECK_CLOSE(depth2, 0, 1e-10);

  // Infront of camera
  const double depth3 = CalculateDepth(proj_matrix, Eigen::Vector3d(0, 0, 1));
  BOOST_CHECK_CLOSE(depth3, 1, 1e-10);

  // Behind camera
  const double depth4 = CalculateDepth(proj_matrix, Eigen::Vector3d(0, 0, -1));
  BOOST_CHECK_CLOSE(depth4, -1, 1e-10);
}

BOOST_AUTO_TEST_CASE(TestHasPointPositiveDepth) {
  const Eigen::Vector4d qvec(1, 0, 0, 0);
  const Eigen::Vector3d tvec(0, 0, 0);
  const auto proj_matrix = ComposeProjectionMatrix(qvec, tvec);

  // In the image plane
  const bool check1 =
      HasPointPositiveDepth(proj_matrix, Eigen::Vector3d(0, 0, 0));
  BOOST_CHECK(!check1);
  const bool check2 =
      HasPointPositiveDepth(proj_matrix, Eigen::Vector3d(0, 2, 0));
  BOOST_CHECK(!check2);

  // Infront of camera
  const bool check3 =
      HasPointPositiveDepth(proj_matrix, Eigen::Vector3d(0, 0, 1));
  BOOST_CHECK(check3);

  // Behind camera
  const bool check4 =
      HasPointPositiveDepth(proj_matrix, Eigen::Vector3d(0, 0, -1));
  BOOST_CHECK(!check4);
}

ext: cc

max_stars_repo_name: XieDufang/colmap

max_issues_repo_name: XieDufang/colmap

max_forks_repo_name: XieDufang/colmap


text:
#include <boost/python.hpp>

#include "compiler.hpp"
#include "wrappers.hpp"
#include "cuda_printer.hpp"
#include "type_printer.hpp"
#include "utility/up_get.hpp"
#include "utility/isinstance.hpp"

#include "python_wrap.hpp"
#include "sequence_extract.hpp"
#include "cuarray_extract.hpp"
#include "namespace_wrap.hpp"

using std::string;
using std::shared_ptr;
using std::set;
using std::static_pointer_cast;
using std::ostringstream;
using boost::python::list;

namespace backend {
template<class T>
T* get_pointer(shared_ptr<T> const &p) {
    return p.get();
}

shared_ptr<procedure> wrapper;
string hash_value;
set<string> extractions;

string compile(compiler &c,
               suite_wrap &s) {
    //Compile
    shared_ptr<suite> result = c(s);
    python_wrap python_wrapper(c.entry_point());
    shared_ptr<suite> wrapped =
        static_pointer_cast<suite>(boost::apply_visitor(python_wrapper, *result));
    wrapper = python_wrapper.wrapper();
    const string entry_point = c.entry_point();

    sequence_extract sequence_extractor(entry_point, c.reg());
    boost::apply_visitor(sequence_extractor, *wrapped);
    extractions = sequence_extractor.extractions();

    cuarray_extract cuarray_extractor(entry_point, c.reg());
    boost::apply_visitor(cuarray_extractor, *wrapped);
    for(auto i = cuarray_extractor.extractions().cbegin();
        i != cuarray_extractor.extractions().cend();
        i++) {
        extractions.insert(*i);
    }

    entry_hash entry_hasher(c.entry_point());
    boost::apply_visitor(entry_hasher, *wrapped);
    hash_value = entry_hasher.hash();
    namespace_wrap namespace_wrapper(hash_value);
    shared_ptr<suite> namespaced =
        static_pointer_cast<suite>(boost::apply_visitor(namespace_wrapper, *wrapped));

    ostringstream os;
    //Convert to string
    backend::cuda_printer p(entry_point, c.reg(), os);
    boost::apply_visitor(p, *namespaced);
    string device_code = os.str();

    return device_code;
}

string wrap_name() {
    return wrapper->id().id();
}

string wrap_result_type() {
    ostringstream os;
    backend::ctype::ctype_printer cp(os);
    const ctype::type_t& n_t = wrapper->ctype();
    assert(detail::isinstance<ctype::monotype_t>(n_t));
    const ctype::monotype_t& n_mt =
        detail::up_get<ctype::monotype_t>(n_t);
    assert(string("void") !=
           n_mt.name());
    assert(detail::isinstance<ctype::fn_t>(n_t));
    const ctype::fn_t &n_f = boost::get<const ctype::fn_t&>(n_t);
    const ctype::type_t& ret_t = n_f.result();
    boost::apply_visitor(cp, ret_t);
    return os.str();
}

list wrap_arg_types() {
    ostringstream os;
    backend::ctype::ctype_printer cp(os);
    list result;
    const backend::tuple &args = wrapper->args();
    for(auto i = args.begin();
        i != args.end();
        i++) {
        boost::apply_visitor(cp, i->ctype());
        result.append(os.str());
        os.str("");
    }
    return result;
}

list wrap_arg_names() {
    list result;
    const backend::tuple& args = wrapper->args();
    for(auto i = args.begin();
        i != args.end();
        i++) {
        assert(detail::isinstance<backend::name>(*i));
        const backend::name& arg = boost::get<const backend::name&>(*i);
        result.append(arg.id());
    }
    return result;
}

string hash() {
    return hash_value;
}

list get_extractions() {
    list result;
    for(auto i = extractions.begin();
        i != extractions.end();
        i++) {
        result.append(*i);
    }
    return result;
}

}

using namespace boost::python;
using namespace backend;

BOOST_PYTHON_MODULE(backendcompiler) {

    class_<compiler, shared_ptr<compiler> >("Compiler", init<string>())
        .def("__call__", &compile);
    def("wrap_name", &wrap_name);
    def("wrap_result_type", &wrap_result_type);
    def("wrap_arg_types", &wrap_arg_types);
    def("wrap_arg_names", &wrap_arg_names);
    def("hash", &hash);
    def("extractions", &get_extractions);
}

max_stars_repo_name: zahangircse/copperhead

max_issues_repo_name: zahangircse/copperhead

max_forks_repo_name: zahangircse/copperhead