<commit_before>/** @file
    @brief Implementation

    @date 2016

    @author
    Sensics, Inc.
    <http://sensics.com/osvr>
*/

// 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.

// Internal Includes
#include "TestIMU_Common.h"

#include "FlexibleUnscentedCorrect.h"

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

// Standard includes
// - none

#ifdef DEMAND_NONNEGATIVE_WEIGHTS
template <typename GeneratorType>
inline void nonNegativeWeights(GeneratorType const &gen) {
    CAPTURE(gen.getWeightsForMean().transpose());
    CHECK((gen.getWeightsForMean().array() >= 0.).all());

    CAPTURE(gen.getWeightsForCov().transpose());
    CHECK((gen.getWeightsForCov().array() >= 0.).all());
}

template <typename GeneratorType>
inline void thenRequireNonNegativeWeights(GeneratorType const &gen) {
    THEN("Weights should be non-negative") { nonNegativeWeights(gen); }
}
template <typename GeneratorType>
inline void andThenRequireNonNegativeWeights(GeneratorType const &gen) {
    AND_THEN("Weights should be non-negative") { nonNegativeWeights(gen); }
}
#else
template <typename GeneratorType>
inline void thenRequireNonNegativeWeights(GeneratorType const &gen) {
    // not actually a requirement, or even common, it turns out.
}
template <typename GeneratorType>
inline void andThenRequireNonNegativeWeights(GeneratorType const &gen) {
    // not actually a requirement, or even common, it turns out.
}
#endif

inline void checkReconstruction(Vector3d const &mean, Matrix3d const &cov) {
    static const auto DIM = 3;
    using namespace osvr::kalman;
    using Generator = SigmaPointGenerator<DIM>;
    using Reconstructor =
        ReconstructedDistributionFromSigmaPoints<DIM, Generator>;
    const auto params = SigmaPointParameters();
    CAPTURE(mean);
    CAPTURE(cov);
    auto gen = Generator(mean, cov, params);
    thenRequireNonNegativeWeights(gen);
#if 0
	static const auto numSigmaPoints = Generator::NumSigmaPoints;
	for (std::size_t i = 0; i < numSigmaPoints; ++i) {
		CAPTURE(gen.getSigmaPoint(i));
		CHECK(false);
	}
#endif
    AND_WHEN("reconstructed from the sigma points") {
        auto recon = Reconstructor(gen, gen.getSigmaPoints());
        THEN("the reconstructed distribution should be approximately equal "
             "to the input") {
            CAPTURE(gen.getSigmaPoints());
            CAPTURE(recon.getMean());
            for (std::size_t i = 0; i < DIM; ++i) {
                CAPTURE(i);
                REQUIRE(recon.getMean()[i] == Approx(mean[i]));
            }
            CAPTURE(recon.getCov());
            REQUIRE(recon.getCov().isApprox(cov));
        }
    }
}

TEST_CASE("Sigma point reconstruction validity") {
    using namespace osvr::kalman;
    Matrix3d cov(Vector3d::Constant(10).asDiagonal());
    WHEN("Starting with a zero mean") {
        checkReconstruction(Vector3d::Zero(), cov);
    }

    WHEN("Starting with a non-zero mean") {
        checkReconstruction(Vector3d(1, 2, 3), cov);
    }
    WHEN("Starting with a small non-zero mean") {
        checkReconstruction(Vector3d(0.1, 0.2, 0.3), cov);
    }
}

template <typename MeasurementType>
inline void unscentedSmallPositiveYChecks(TestData *data,
                                          MeasurementType &kalmanMeas) {
    const auto params = kalman::SigmaPointParameters();
    auto inProgress =
        kalman::beginUnscentedCorrection(data->state, kalmanMeas, params);

    AND_THEN("transformed sigma points should not all equal 0") {
        CAPTURE(inProgress.transformedPoints.transpose());
        REQUIRE_FALSE(inProgress.transformedPoints.isApproxToConstant(0.));
    }
    andThenRequireNonNegativeWeights(inProgress.sigmaPoints);

    commonSmallPositiveYChecks(data, kalmanMeas, inProgress);
}

template <typename MeasurementType>
inline void checkEffectiveIdentityMeasurement(TestData *data,
                                              MeasurementType &kalmanMeas) {
    AND_THEN("residual should be zero") {
        CAPTURE(kalmanMeas.getResidual(data->state));
        REQUIRE(kalmanMeas.getResidual(data->state).isApproxToConstant(0.));
    }
    auto inProgress = kalman::beginUnscentedCorrection(data->state, kalmanMeas);

    andThenRequireNonNegativeWeights(inProgress.sigmaPoints);
    AND_THEN("transformed sigma points should not all equal 0") {
        REQUIRE_FALSE(inProgress.transformedPoints.isApproxToConstant(0.));
    }
    auto &recon = inProgress.reconstruction;
    AND_THEN("propagated predicted measurement mean should be zero") {
        CAPTURE(recon.getMean().transpose());
        REQUIRE(recon.getMean().isApproxToConstant(0.));
    }
    AND_THEN("state correction should be finite - specifically, zero") {
        CAPTURE(inProgress.stateCorrection.transpose());
        REQUIRE(inProgress.stateCorrection.array().allFinite());
        REQUIRE(inProgress.stateCorrection.isApproxToConstant(0));
    }

    const Quaterniond origQuat = data->state.getQuaternion();
    AND_WHEN("the correction is applied") {
        inProgress.finishCorrection();
        THEN("state should be unchanged") {
            CAPTURE(data->state.stateVector().transpose());
            REQUIRE(data->state.stateVector().isApproxToConstant(0));
            CAPTURE(origQuat);
            CAPTURE(data->state.getQuaternion());
            REQUIRE(data->state.getQuaternion().coeffs().isApprox(
                origQuat.coeffs()));
        }
        THEN("State error should have decreased") {
            REQUIRE((data->state.errorCovariance().array() <=
                     data->originalStateError.array())
                        .all());
        }
    }
}

CATCH_TYPELIST_DESCRIBED_TESTCASE(
    "unscented with identity calibration output", "[ukf]",
    /*kalman::QFirst, kalman::QLast,*/ kalman::SplitQ) {

#if 0
TEST_CASE("unscented with identity calibration output", "[ukf]") {
    using TypeParam = kalman::IMUOrientationMeasForUnscented;
#endif

    const auto params = kalman::SigmaPointParameters();
    using MeasurementType = OrientationMeasurementUsingPolicy<TypeParam>;
    // using MeasurementType = TypeParam;
    unique_ptr<TestData> data(new TestData);
    GIVEN("an identity state") {
        WHEN("filtering in an identity measurement") {
            Quaterniond xformedMeas = data->xform(Quaterniond::Identity());

            THEN("the transformed measurement should equal the measurement") {
                CAPTURE(xformedMeas);
                REQUIRE(xformedMeas.isApprox(Quaterniond::Identity()));

                MeasurementType kalmanMeas{xformedMeas, data->imuVariance};
                checkEffectiveIdentityMeasurement(data.get(), kalmanMeas);
            }
        }

        WHEN("filtering in a small positive rotation about y") {
            Quaterniond smallPositiveRotationAboutY(
                AngleAxisd(SMALL_VALUE, Vector3d::UnitY()));
            CAPTURE(SMALL_VALUE);
            CAPTURE(smallPositiveRotationAboutY);
            Quaterniond xformedMeas = data->xform(smallPositiveRotationAboutY);
            CAPTURE(xformedMeas);

            THEN("the transformed measurement should equal the measurement") {
                REQUIRE(xformedMeas.isApprox(smallPositiveRotationAboutY));
                /// Do the rest of the checks for a small rotation about y
                MeasurementType kalmanMeas{xformedMeas, data->imuVariance};
                unscentedSmallPositiveYChecks(data.get(), kalmanMeas);
            }
        }
    }
    GIVEN("a state rotated about y") {
        Quaterniond stateRotation(AngleAxisd(M_PI / 4., Vector3d::UnitY()));
        data->state.setQuaternion(stateRotation);
        WHEN("filtering in a small positive rotation about y") {
            Quaterniond smallPositiveRotationAboutY =
                Quaterniond(AngleAxisd(SMALL_VALUE, Vector3d::UnitY())) *
                stateRotation;
            CAPTURE(SMALL_VALUE);
            CAPTURE(smallPositiveRotationAboutY);
            Quaterniond xformedMeas = data->xform(smallPositiveRotationAboutY);
            CAPTURE(xformedMeas);

            THEN("the transformed measurement should equal the measurement") {
                REQUIRE(xformedMeas.isApprox(smallPositiveRotationAboutY));

                /// Do the rest of the checks for a small rotation about y
                MeasurementType kalmanMeas{xformedMeas, data->imuVariance};
                unscentedSmallPositiveYChecks(data.get(), kalmanMeas);
            }
        }
    }
    GIVEN("a state rotated about x") {
        Quaterniond stateRotation(AngleAxisd(M_PI / 4., Vector3d::UnitX()));
        data->state.setQuaternion(stateRotation);
        WHEN("filtering in a small positive rotation about y") {
            Quaterniond smallPositiveRotationAboutY =
                Quaterniond(AngleAxisd(SMALL_VALUE, Vector3d::UnitY())) *
                stateRotation;
            CAPTURE(SMALL_VALUE);
            CAPTURE(smallPositiveRotationAboutY);
            Quaterniond xformedMeas = data->xform(smallPositiveRotationAboutY);
            CAPTURE(xformedMeas);

            THEN("the transformed measurement should equal the measurement") {
                REQUIRE(xformedMeas.isApprox(smallPositiveRotationAboutY));

                /// Do the rest of the checks for a small rotation about y
                MeasurementType kalmanMeas{xformedMeas, data->imuVariance};
                CAPTURE(kalmanMeas.getResidual(data->state));
                unscentedSmallPositiveYChecks(data.get(), kalmanMeas);
            }
        }
    }
}

TEST_CASE("unscented with small x rotation calibration output", "[ukf]") {
    using MeasurementType = OrientationMeasurementUsingPolicy<kalman::SplitQ>;
    unique_ptr<TestData> data(new TestData);
    CAPTURE(SMALL_VALUE);
    data->roomToCameraRotation =
        Quaterniond(AngleAxisd(SMALL_VALUE, Vector3d::UnitX()));
    CAPTURE(data->roomToCameraRotation);
    GIVEN("an identity state in camera space") {
        CAPTURE(data->state.getQuaternion());
        WHEN("filtering in a measurement that's the inverse of the room to "
             "camera rotation (a measurement matching state)") {
            Quaterniond xformedMeas =
                data->xform(data->roomToCameraRotation.inverse());

            THEN("the transformed measurement should be approximately the "
                 "identity") {
                CAPTURE(xformedMeas);
                REQUIRE(xformedMeas.isApprox(Quaterniond::Identity()));

                MeasurementType kalmanMeas{xformedMeas, data->imuVariance};
                checkEffectiveIdentityMeasurement(data.get(), kalmanMeas);
            }
        }
    }
    GIVEN("an identity state in room space") {
        data->state.setQuaternion(data->roomToCameraRotation);
        CAPTURE(data->state.getQuaternion());
        WHEN("filtering in an identity measurement (a measurement matching "
             "state)") {
            Quaterniond xformedMeas = data->xform(Quaterniond::Identity());

            THEN("the transformed measurement should be approximately the "
                 "room to camera rotation") {
                CAPTURE(xformedMeas);
                REQUIRE(xformedMeas.isApprox(data->roomToCameraRotation));

                MeasurementType kalmanMeas{xformedMeas, data->imuVariance};
                checkEffectiveIdentityMeasurement(data.get(), kalmanMeas);
            }
        }

        WHEN("filtering in a small positive rotation about y") {
            Quaterniond smallPositiveRotationAboutY =
                Quaterniond(AngleAxisd(SMALL_VALUE, Vector3d::UnitY()));
            Quaterniond expectedXformedSmallPositiveRotationAboutY(
                (Isometry3d(smallPositiveRotationAboutY) *
                 Isometry3d(data->state.getQuaternion()))
                    .rotation());
            CAPTURE(smallPositiveRotationAboutY);
            Quaterniond xformedMeas = data->xform(smallPositiveRotationAboutY);
            CAPTURE(xformedMeas);

            THEN("the transformed measurement should equal a manually "
                 "transformed "
                 "measurement") {
                REQUIRE(xformedMeas.isApprox(
                    expectedXformedSmallPositiveRotationAboutY));
                /// Do the rest of the checks for a small rotation about y
                MeasurementType kalmanMeas{xformedMeas, data->imuVariance};
                CAPTURE(kalmanMeas.getResidual(data->state));
                unscentedSmallPositiveYChecks(data.get(), kalmanMeas);
            }
        }
    }
}
<commit_msg>Add a large number of test assertions checking all axes<commit_after>/** @file
    @brief Implementation

    @date 2016

    @author
    Sensics, Inc.
    <http://sensics.com/osvr>
*/

// 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.

// Internal Includes
#include "TestIMU_Common.h"

#include "FlexibleUnscentedCorrect.h"

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

// Standard includes
// - none

#ifdef DEMAND_NONNEGATIVE_WEIGHTS
template <typename GeneratorType>
inline void nonNegativeWeights(GeneratorType const &gen) {
    CAPTURE(gen.getWeightsForMean().transpose());
    CHECK((gen.getWeightsForMean().array() >= 0.).all());

    CAPTURE(gen.getWeightsForCov().transpose());
    CHECK((gen.getWeightsForCov().array() >= 0.).all());
}

template <typename GeneratorType>
inline void thenRequireNonNegativeWeights(GeneratorType const &gen) {
    THEN("Weights should be non-negative") { nonNegativeWeights(gen); }
}
template <typename GeneratorType>
inline void andThenRequireNonNegativeWeights(GeneratorType const &gen) {
    AND_THEN("Weights should be non-negative") { nonNegativeWeights(gen); }
}
#else
template <typename GeneratorType>
inline void thenRequireNonNegativeWeights(GeneratorType const &gen) {
    // not actually a requirement, or even common, it turns out.
}
template <typename GeneratorType>
inline void andThenRequireNonNegativeWeights(GeneratorType const &gen) {
    // not actually a requirement, or even common, it turns out.
}
#endif

TEST_CASE("Sigma point reconstruction validity") {

    static const auto DIM = 3;
    using namespace osvr::kalman;
    Matrix3d cov(Vector3d::Constant(10).asDiagonal());

    auto checkReconstruction = [&](Eigen::Vector3d const &mean) {
        using namespace osvr::kalman;
        using Generator = SigmaPointGenerator<DIM>;
        using Reconstructor =
            ReconstructedDistributionFromSigmaPoints<DIM, Generator>;
        const auto params = SigmaPointParameters();
        CAPTURE(mean);
        CAPTURE(cov);
        auto gen = Generator(mean, cov, params);
        thenRequireNonNegativeWeights(gen);
#if 0
        static const auto numSigmaPoints = Generator::NumSigmaPoints;
        for (std::size_t i = 0; i < numSigmaPoints; ++i) {
            CAPTURE(gen.getSigmaPoint(i));
            CHECK(false);
        }
#endif
        AND_WHEN("reconstructed from the sigma points") {
            auto recon = Reconstructor(gen, gen.getSigmaPoints());
            THEN("the reconstructed distribution should be approximately equal "
                 "to the input") {
                CAPTURE(gen.getSigmaPoints());
                CAPTURE(recon.getMean());
                for (std::size_t i = 0; i < DIM; ++i) {
                    CAPTURE(i);
                    REQUIRE(recon.getMean()[i] == Approx(mean[i]));
                }
                CAPTURE(recon.getCov());
                REQUIRE(recon.getCov().isApprox(cov));
            }
        }
    };

    WHEN("Starting with a zero mean") { checkReconstruction(Vector3d::Zero()); }

    WHEN("Starting with a non-zero mean") {
        checkReconstruction(Vector3d(1, 2, 3));
    }
    WHEN("Starting with a small non-zero mean") {
        checkReconstruction(Vector3d(0.1, 0.2, 0.3));
    }
}

enum class Axis : std::size_t { X = 0, Y = 1, Z = 2 };

inline double zeroOrValueForAxis(Axis rotationAxis, Axis currentAxis,
                                 double value) {
    return ((rotationAxis == currentAxis) ? value : 0.);
}

inline Vector3d ZeroVec3dExceptAtAxis(Axis rotationAxis, double value) {
    return Vector3d(zeroOrValueForAxis(rotationAxis, Axis::X, value),
                    zeroOrValueForAxis(rotationAxis, Axis::Y, value),
                    zeroOrValueForAxis(rotationAxis, Axis::Z, value));
}

inline Vector3d Vec3dSmallValueAt(Axis rotationAxis) {
    return ZeroVec3dExceptAtAxis(rotationAxis, SMALL_VALUE);
}

inline Vector3d Vec3dUnit(Axis rotationAxis) {
    return ZeroVec3dExceptAtAxis(rotationAxis, 1.);
}

inline double getSignCorrect(bool positive) { return (positive ? 1. : -1.); }

template <typename MeasurementType, typename InProgressType>
inline void
commonSmallSingleAxisChecks(TestData *data, MeasurementType &kalmanMeas,
                            InProgressType &inProgress, Axis const rotationAxis,
                            bool const positive = true) {
    const double signCorrect = getSignCorrect(positive);
    Vector3d rotationVector = Vec3dSmallValueAt(rotationAxis) * signCorrect;
    CAPTURE(rotationVector.transpose());

    Vector3d residual = kalmanMeas.getResidual(data->state);
    CAPTURE(residual.transpose());

    AND_THEN("residual directly computed by measurement should be zero, except "
             "for the single axis of rotation, which should be of magnitude "
             "SMALL_VALUE") {
        CHECK(residual.isApprox(rotationVector));
    }

    CAPTURE(inProgress.deltaz);
    AND_THEN("computed deltaz should equal residual") {
        CHECK(residual.isApprox(inProgress.deltaz));
    }

    AND_THEN("delta z (residual/propagated mean residual) should be "
             "approximately 0, except for the single axis of rotation, which "
             "should be magnitude SMALL_VALUE") {
        REQUIRE(inProgress.deltaz[0] == Approx(rotationVector[0]));
        REQUIRE(inProgress.deltaz[1] == Approx(rotationVector[1]));
        REQUIRE(inProgress.deltaz[2] == Approx(rotationVector[2]));
    }

    CAPTURE(inProgress.stateCorrection.transpose());
    REQUIRE(inProgress.stateCorrectionFinite);
    AND_THEN("state correction should have a rotation component with small "
             "absolute element (sign matching rotation vector) corresponding "
             "to rotation axis") {
        for (std::size_t i = 0; i < 3; ++i) {
            const auto stateIndex = i + 3;
            CAPTURE(stateIndex);
            if (static_cast<std::size_t>(rotationAxis) == i) {
                /// This is our rotation axis - correction should be in (0,
                /// SMALL_VALUE)
                REQUIRE((signCorrect * inProgress.stateCorrection[stateIndex]) >
                        0);
                REQUIRE((signCorrect * inProgress.stateCorrection[stateIndex]) <
                        SMALL_VALUE);
            } else {
                /// Not our rotation axis, correction should be approx 0.
                REQUIRE(inProgress.stateCorrection[stateIndex] == Approx(0.));
            }
        }
    }

    AND_THEN("state correction should have an angular velocity component "
             "with zero or small abs (sign matching rotation) corresponding to "
             "rotation axis") {

        for (std::size_t i = 0; i < 3; ++i) {
            const auto stateIndex = i + 9;
            CAPTURE(stateIndex);
            if (static_cast<std::size_t>(rotationAxis) == i) {
                /// This is our rotation axis - correction should be >= 0 if
                /// positive rotation.
                REQUIRE(signCorrect * inProgress.stateCorrection[stateIndex] >=
                        0.);
            } else {
                /// Not our rotation axis, correction should be approx 0.
                REQUIRE(inProgress.stateCorrection[stateIndex] == Approx(0.));
            }
        }
    }
    AND_THEN("state correction should not contain any translational/linear "
             "velocity components") {
        REQUIRE(inProgress.stateCorrection.head<3>().isApproxToConstant(0.));
        REQUIRE(
            inProgress.stateCorrection.segment<3>(6).isApproxToConstant(0.));
    }
    AND_WHEN("the correction is applied") {
        auto errorCovarianceCorrectionWasFinite = inProgress.finishCorrection();
        THEN("the new error covariance should be finite") {
            REQUIRE(errorCovarianceCorrectionWasFinite);
            AND_THEN("State error should have decreased") {
                REQUIRE((data->state.errorCovariance().array() <=
                         data->originalStateError.array())
                            .all());
            }
        }
    }
}

template <typename MeasurementType>
inline void
unscentedSmallSingleAxisChecks(TestData *data, MeasurementType &kalmanMeas,
                               Axis rotationAxis, bool positive = true) {
    const auto params = kalman::SigmaPointParameters();
    auto inProgress =
        kalman::beginUnscentedCorrection(data->state, kalmanMeas, params);

    AND_THEN("transformed sigma points should not all equal 0") {
        CAPTURE(inProgress.transformedPoints.transpose());
        REQUIRE_FALSE(inProgress.transformedPoints.isApproxToConstant(0.));
    }
    andThenRequireNonNegativeWeights(inProgress.sigmaPoints);

    commonSmallSingleAxisChecks(data, kalmanMeas, inProgress, rotationAxis,
                                positive);
}

template <typename MeasurementType>
inline void checkEffectiveIdentityMeasurement(TestData *data,
                                              MeasurementType &kalmanMeas) {
    AND_THEN("residual should be zero") {
        CAPTURE(kalmanMeas.getResidual(data->state));
        REQUIRE(kalmanMeas.getResidual(data->state).isApproxToConstant(0.));
    }
    auto inProgress = kalman::beginUnscentedCorrection(data->state, kalmanMeas);

    andThenRequireNonNegativeWeights(inProgress.sigmaPoints);
    AND_THEN("transformed sigma points should not all equal 0") {
        REQUIRE_FALSE(inProgress.transformedPoints.isApproxToConstant(0.));
    }
    auto &recon = inProgress.reconstruction;
    AND_THEN("propagated predicted measurement mean should be zero") {
        CAPTURE(recon.getMean().transpose());
        REQUIRE(recon.getMean().isApproxToConstant(0.));
    }
    AND_THEN("state correction should be finite - specifically, zero") {
        CAPTURE(inProgress.stateCorrection.transpose());
        REQUIRE(inProgress.stateCorrection.array().allFinite());
        REQUIRE(inProgress.stateCorrection.isApproxToConstant(0));
    }

    const Quaterniond origQuat = data->state.getQuaternion();
    AND_WHEN("the correction is applied") {
        inProgress.finishCorrection();
        THEN("state should be unchanged") {
            CAPTURE(data->state.stateVector().transpose());
            REQUIRE(data->state.stateVector().isApproxToConstant(0));
            CAPTURE(origQuat);
            CAPTURE(data->state.getQuaternion());
            REQUIRE(data->state.getQuaternion().coeffs().isApprox(
                origQuat.coeffs()));
        }
        THEN("State error should have decreased") {
            REQUIRE((data->state.errorCovariance().array() <=
                     data->originalStateError.array())
                        .all());
        }
    }
}

template <typename F, typename... Args>
inline void allSmallSingleAxisRotations(F &&f, Args &&... args) {
    WHEN("filtering in a small positive rotation about x") {
        std::forward<F>(f)(std::forward<Args>(args)..., Axis::X, true);
    }

    WHEN("filtering in a small positive rotation about y") {
        std::forward<F>(f)(std::forward<Args>(args)..., Axis::Y, true);
    }

    WHEN("filtering in a small positive rotation about z") {
        std::forward<F>(f)(std::forward<Args>(args)..., Axis::Z, true);
    }

    WHEN("filtering in a small negative rotation about x") {
        std::forward<F>(f)(std::forward<Args>(args)..., Axis::X, false);
    }
    WHEN("filtering in a small negative rotation about y") {
        std::forward<F>(f)(std::forward<Args>(args)..., Axis::Y, false);
    }

    WHEN("filtering in a small negative rotation about z") {
        std::forward<F>(f)(std::forward<Args>(args)..., Axis::Z, false);
    }
}

#if 0
CATCH_TYPELIST_DESCRIBED_TESTCASE(
    "unscented with identity calibration output", "[ukf]",
    /*kalman::QFirst, kalman::QLast,*/ kalman::SplitQ) {

#endif
TEST_CASE("unscented with identity calibration output", "[ukf]") {
    using TypeParam = kalman::SplitQ;

    const auto params = kalman::SigmaPointParameters();
    using MeasurementType = OrientationMeasurementUsingPolicy<TypeParam>;
    // using MeasurementType = TypeParam;
    unique_ptr<TestData> data(new TestData);
    GIVEN("an identity state") {
        WHEN("filtering in an identity measurement") {
            Quaterniond xformedMeas = data->xform(Quaterniond::Identity());

            THEN("the transformed measurement should equal the "
                 "measurement") {
                CAPTURE(xformedMeas);
                REQUIRE(xformedMeas.isApprox(Quaterniond::Identity()));

                MeasurementType kalmanMeas{xformedMeas, data->imuVariance};
                checkEffectiveIdentityMeasurement(data.get(), kalmanMeas);
            }
        }
        allSmallSingleAxisRotations([&](Axis rotationAxis, bool positive) {
            const double signCorrect = getSignCorrect(positive);
            auto radians = signCorrect * SMALL_VALUE;
            CAPTURE(radians);
            auto axisVec = Vec3dUnit(rotationAxis);
            CAPTURE(axisVec.transpose());
            Quaterniond smallRotation =
                Quaterniond(AngleAxisd(radians, axisVec));

            CAPTURE(smallRotation);
            Quaterniond xformedMeas = data->xform(smallRotation);
            CAPTURE(xformedMeas);

            THEN("the transformed measurement should equal the "
                 "measurement") {
                REQUIRE(xformedMeas.isApprox(smallRotation));
                /// Do the rest of the checks for a small rotation about y
                MeasurementType kalmanMeas{xformedMeas, data->imuVariance};
                unscentedSmallSingleAxisChecks(data.get(), kalmanMeas,
                                               rotationAxis, positive);
            }
        });
    }

    auto runIncrementalSmallRotChecksNonIdentityState = [&](Axis rotationAxis,
                                                            bool positive) {
        const double signCorrect = getSignCorrect(positive);
        auto radians = signCorrect * SMALL_VALUE;
        CAPTURE(radians);
        auto axisVec = Vec3dUnit(rotationAxis);
        CAPTURE(axisVec.transpose());
        Quaterniond smallRotation = Quaterniond(AngleAxisd(radians, axisVec)) *
                                    data->state.getQuaternion();

        CAPTURE(smallRotation);
        Quaterniond xformedMeas = data->xform(smallRotation);
        CAPTURE(xformedMeas);

        THEN("the transformed measurement should equal the "
             "measurement") {
            REQUIRE(xformedMeas.isApprox(smallRotation));
            /// Do the rest of the checks for a small rotation about y
            MeasurementType kalmanMeas{xformedMeas, data->imuVariance};
            unscentedSmallSingleAxisChecks(data.get(), kalmanMeas, rotationAxis,
                                           positive);
        }
    };
    GIVEN("a state rotated about y") {
        Quaterniond stateRotation(AngleAxisd(M_PI / 4., Vector3d::UnitY()));
        data->state.setQuaternion(stateRotation);
        allSmallSingleAxisRotations(
            runIncrementalSmallRotChecksNonIdentityState);
    }
    GIVEN("a state rotated about x") {
        Quaterniond stateRotation(AngleAxisd(M_PI / 4., Vector3d::UnitX()));
        data->state.setQuaternion(stateRotation);
        allSmallSingleAxisRotations(
            runIncrementalSmallRotChecksNonIdentityState);
    }
}

TEST_CASE("unscented with small x rotation calibration output", "[ukf]") {
    using MeasurementType = OrientationMeasurementUsingPolicy<kalman::SplitQ>;
    unique_ptr<TestData> data(new TestData);
    CAPTURE(SMALL_VALUE);
    data->roomToCameraRotation =
        Quaterniond(AngleAxisd(SMALL_VALUE, Vector3d::UnitX()));
    CAPTURE(data->roomToCameraRotation);
    INFO("This calibration value means that the camera is rotated negative "
         "about its x wrt. the IMU/room");
    INFO("Equivalently, that quaternion calibration value takes "
         "points/orientations in room space and moves them to camera space");
    GIVEN("an identity state in camera space") {
        CAPTURE(data->state.getQuaternion());
        WHEN("filtering in a measurement that's the inverse of the room to "
             "camera rotation (a measurement matching state)") {
            Quaterniond meas = data->roomToCameraRotation.inverse();
            CAPTURE(meas);
            Quaterniond xformedMeas = data->xform(meas);

            THEN("the transformed measurement should be approximately the "
                 "identity") {
                CAPTURE(xformedMeas);
                REQUIRE(xformedMeas.isApprox(Quaterniond::Identity()));

                MeasurementType kalmanMeas{xformedMeas, data->imuVariance};
                checkEffectiveIdentityMeasurement(data.get(), kalmanMeas);
            }
        }
    }
    GIVEN("an identity state in room space") {
        data->state.setQuaternion(data->roomToCameraRotation);
        CAPTURE(data->state.getQuaternion());
        WHEN("filtering in an identity measurement (a measurement matching "
             "state)") {
            Quaterniond xformedMeas = data->xform(Quaterniond::Identity());

            THEN("the transformed measurement should be approximately the "
                 "room to camera rotation") {
                CAPTURE(xformedMeas);
                REQUIRE(xformedMeas.isApprox(data->roomToCameraRotation));

                MeasurementType kalmanMeas{xformedMeas, data->imuVariance};
                checkEffectiveIdentityMeasurement(data.get(), kalmanMeas);
            }
        }
        allSmallSingleAxisRotations([&](Axis rotationAxis, bool positive) {
            const double signCorrect = getSignCorrect(positive);
            auto radians = signCorrect * SMALL_VALUE;
            CAPTURE(radians);
            auto axisVec = Vec3dUnit(rotationAxis);
            CAPTURE(axisVec.transpose());
            Quaterniond smallRelRotationInCameraSpace =
                Quaterniond(AngleAxisd(radians, axisVec)) *
                data->state.getQuaternion();
            CAPTURE(smallRelRotationInCameraSpace);
            Quaterniond smallRotation = data->roomToCameraRotation.inverse() *
                                        smallRelRotationInCameraSpace;
            CAPTURE(smallRotation);
            THEN("the transformed measurement should equal the original "
                 "measurement as computed in camera space") {
                Quaterniond xformedMeas = data->xform(smallRotation);
                CAPTURE(xformedMeas);
                REQUIRE(xformedMeas.isApprox(smallRelRotationInCameraSpace));

                /// Do the rest of the checks for a small rotation about a
                /// single axis
                MeasurementType kalmanMeas{xformedMeas, data->imuVariance};
                CAPTURE(kalmanMeas.getResidual(data->state));
                unscentedSmallSingleAxisChecks(data.get(), kalmanMeas,
                                               rotationAxis, positive);
            }
        });
    }
}

TEST_CASE("conceptual transformation orders") {
    Quaterniond positiveX(AngleAxisd(0.5, Vector3d::UnitX()));
    Quaterniond positiveY(AngleAxisd(0.5, Vector3d::UnitY()));
    Vector3d yVec = Vector3d::UnitY();
    SECTION(
        "Quaternion composition/multiplication order matches "
        "transformation order (transformation and quat mult are associative)") {
        CAPTURE((positiveX * (positiveY * yVec)).transpose());
        CAPTURE(((positiveX * positiveY) * yVec).transpose());
        REQUIRE((positiveX * (positiveY * yVec))
                    .isApprox((positiveX * positiveY) * yVec));
    }
    SECTION("Behavior of rotations") {
        INFO("positive X is a transformation taking points from a coordinate "
             "system pitched up, to a level coordinate system.");
        CAPTURE((positiveX * yVec).transpose());
        REQUIRE((positiveX * yVec).z() > 0.);
    }
}
<|endoftext|>


<commit_before>#ifndef STAN_MATH_PRIM_CORE_INIT_THREADPOOL_TBB_HPP
#define STAN_MATH_PRIM_CORE_INIT_THREADPOOL_TBB_HPP

#include <stan/math/prim/scal/err/invalid_argument.hpp>
#include <boost/lexical_cast.hpp>

#include <cstdlib>
#include <thread>

#include <tbb/task_scheduler_init.h>

namespace stan {
namespace math {
namespace internal {

/**
 * Get number of threads to use. The function uses the environment
 * variable STAN_NUM_THREADS and follows these conventions:
 *
 * - STAN_NUM_THREADS is not defined => num_threads=1
 * - STAN_NUM_THREADS is positive => num_threads is set to the
 *   specified number
 * - STAN_NUM_THREADS is set to -1 => num_threads is the number of
 *   available cores on the machine
 * - STAN_NUM_THREADS < -1, STAN_NUM_THREADS = 0 or STAN_NUM_THREADS is
 *   not numeric => throws an exception
 *
 * @return number of threads to use
 * @throws std::invalid_argument if the value of STAN_NUM_THREADS env. variable
 * is invalid
 */
inline int get_num_threads() {
  int num_threads = 1;
  const char* env_stan_num_threads = std::getenv("STAN_NUM_THREADS");
  if (env_stan_num_threads != nullptr) {
    try {
      const int env_num_threads
          = boost::lexical_cast<int>(env_stan_num_threads);
      if (env_num_threads > 0) {
        num_threads = env_num_threads;
      } else if (env_num_threads == -1) {
        num_threads = tbb::task_scheduler_init::default_num_threads();
      } else {
        invalid_argument("get_num_threads(int)", "STAN_NUM_THREADS",
                         env_stan_num_threads,
                         "The STAN_NUM_THREADS environment variable is '",
                         "' but it must be positive or -1");
      }
    } catch (boost::bad_lexical_cast) {
      invalid_argument("get_num_threads(int)", "STAN_NUM_THREADS",
                       env_stan_num_threads,
                       "The STAN_NUM_THREADS environment variable is '",
                       "' but it must be a positive number or -1");
    }
  }
  return num_threads;
}

}  // namespace internal

/**
 * Initialize the Intel TBB threadpool and global scheduler through
 * the tbb::task_scheduler_init object. In case an instance of the
 * tbb::task_scheduler_object has been instantiated prior to calling
 * this function, then any subsequent initialization is ignored, which
 * is the default behavior of the Intel TBB library.
 *
 * The maximal number of threads is read from the environment variable
 * STAN_NUM_THREADS using internal::get_num_threads. See conventions
 * of get_num_threads.
 *
 * The function returns as a boolean if the tbb::task_scheduler_init
 * instance has become active once constructed. The instance may not
 * become active if there is already another instance created before
 * calling this function.
 *
 * @param stack_size sets the stack size of each thread; the default 0
 * let's the TBB choose the stack size
 * @return active status of static tbb::task_scheduler_init
 * @throws std::runtime_error if the value of STAN_NUM_THREADS env. variable
 * is invalid
 */
inline bool init_threadpool_tbb(tbb::stack_size_type stack_size = 0) {
  int tbb_max_threads = internal::get_num_threads();

  static tbb::task_scheduler_init tbb_scheduler(tbb_max_threads, stack_size);

  return tbb_scheduler.is_active();
}

}  // namespace math
}  // namespace stan

#endif
<commit_msg>cpplint<commit_after>#ifndef STAN_MATH_PRIM_CORE_INIT_THREADPOOL_TBB_HPP
#define STAN_MATH_PRIM_CORE_INIT_THREADPOOL_TBB_HPP

#include <stan/math/prim/scal/err/invalid_argument.hpp>

#include <boost/lexical_cast.hpp>

#include <tbb/task_scheduler_init.h>

#include <cstdlib>
#include <thread>

namespace stan {
namespace math {
namespace internal {

/**
 * Get number of threads to use. The function uses the environment
 * variable STAN_NUM_THREADS and follows these conventions:
 *
 * - STAN_NUM_THREADS is not defined => num_threads=1
 * - STAN_NUM_THREADS is positive => num_threads is set to the
 *   specified number
 * - STAN_NUM_THREADS is set to -1 => num_threads is the number of
 *   available cores on the machine
 * - STAN_NUM_THREADS < -1, STAN_NUM_THREADS = 0 or STAN_NUM_THREADS is
 *   not numeric => throws an exception
 *
 * @return number of threads to use
 * @throws std::invalid_argument if the value of STAN_NUM_THREADS env. variable
 * is invalid
 */
inline int get_num_threads() {
  int num_threads = 1;
  const char* env_stan_num_threads = std::getenv("STAN_NUM_THREADS");
  if (env_stan_num_threads != nullptr) {
    try {
      const int env_num_threads
          = boost::lexical_cast<int>(env_stan_num_threads);
      if (env_num_threads > 0) {
        num_threads = env_num_threads;
      } else if (env_num_threads == -1) {
        num_threads = tbb::task_scheduler_init::default_num_threads();
      } else {
        invalid_argument("get_num_threads(int)", "STAN_NUM_THREADS",
                         env_stan_num_threads,
                         "The STAN_NUM_THREADS environment variable is '",
                         "' but it must be positive or -1");
      }
    } catch (boost::bad_lexical_cast) {
      invalid_argument("get_num_threads(int)", "STAN_NUM_THREADS",
                       env_stan_num_threads,
                       "The STAN_NUM_THREADS environment variable is '",
                       "' but it must be a positive number or -1");
    }
  }
  return num_threads;
}

}  // namespace internal

/**
 * Initialize the Intel TBB threadpool and global scheduler through
 * the tbb::task_scheduler_init object. In case an instance of the
 * tbb::task_scheduler_object has been instantiated prior to calling
 * this function, then any subsequent initialization is ignored, which
 * is the default behavior of the Intel TBB library.
 *
 * The maximal number of threads is read from the environment variable
 * STAN_NUM_THREADS using internal::get_num_threads. See conventions
 * of get_num_threads.
 *
 * The function returns as a boolean if the tbb::task_scheduler_init
 * instance has become active once constructed. The instance may not
 * become active if there is already another instance created before
 * calling this function.
 *
 * @param stack_size sets the stack size of each thread; the default 0
 * let's the TBB choose the stack size
 * @return active status of static tbb::task_scheduler_init
 * @throws std::runtime_error if the value of STAN_NUM_THREADS env. variable
 * is invalid
 */
inline bool init_threadpool_tbb(tbb::stack_size_type stack_size = 0) {
  int tbb_max_threads = internal::get_num_threads();

  static tbb::task_scheduler_init tbb_scheduler(tbb_max_threads, stack_size);

  return tbb_scheduler.is_active();
}

}  // namespace math
}  // namespace stan

#endif
<|endoftext|>


<commit_before>#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <cblas.h>
#include <math.h>
#include <omp.h>

#include "mvnormal.h"
#include "macau.h"
#include "chol.h"
#include "linop.h"
extern "C" {
  #include <sparse.h>
}

using namespace std;
using namespace Eigen;

/** BPMFPrior */
void BPMFPrior::sample_latents(Eigen::MatrixXd &U, const Eigen::SparseMatrix<double> &mat, double mean_value,
                    const Eigen::MatrixXd &samples, double alpha, const int num_latent) {
  const int N = U.cols();

#pragma omp parallel for
  for(int n = 0; n < N; n++) {
    sample_latent_blas(U, n, mat, mean_value, samples, alpha, mu, Lambda, num_latent);
  }
}

void BPMFPrior::update_prior(const Eigen::MatrixXd &U) {
  tie(mu, Lambda) = CondNormalWishart(U, mu0, b0, WI, df);
}

void BPMFPrior::init(const int num_latent) {
  mu.resize(num_latent);
  mu.setZero();

  Lambda.resize(num_latent, num_latent);
  Lambda.setIdentity();
  Lambda *= 10;

  // parameters of Inv-Whishart distribution
  WI.resize(num_latent, num_latent);
  WI.setIdentity();
  mu0.resize(num_latent);
  mu0.setZero();
  b0 = 2;
  df = num_latent;
}

/** MacauPrior */
template<class FType>
void MacauPrior<FType>::init(const int num_latent, FType & Fmat, bool comp_FtF) {
  mu.resize(num_latent);
  mu.setZero();

  Lambda.resize(num_latent, num_latent);
  Lambda.setIdentity();
  Lambda *= 10;

  // parameters of Inv-Whishart distribution
  WI.resize(num_latent, num_latent);
  WI.setIdentity();
  mu0.resize(num_latent);
  mu0.setZero();
  b0 = 2;
  df = num_latent;

  // side information
  F = Fmat;
  use_FtF = comp_FtF;
  if (use_FtF) {
    At_mul_A(FtF, F);
  }

  Uhat.resize(num_latent, F.rows());
  Uhat.setZero();

  beta.resize(num_latent, F.cols());
  beta.setZero();
}

template<class FType>
void MacauPrior<FType>::sample_latents(Eigen::MatrixXd &U, const Eigen::SparseMatrix<double> &mat, double mean_value,
                    const Eigen::MatrixXd &samples, double alpha, const int num_latent) {
  const int N = U.cols();
#pragma omp parallel for
  for(int n = 0; n < N; n++) {
    // TODO: try moving mu + Uhat.col(n) inside sample_latent for speed
    sample_latent_blas(U, n, mat, mean_value, samples, alpha, mu + Uhat.col(n), Lambda, num_latent);
  }
}

template<class FType>
void MacauPrior<FType>::update_prior(const Eigen::MatrixXd &U) {
  // residual:
  Uhat.noalias() = U - Uhat;
  tie(mu, Lambda) = CondNormalWishart(Uhat, mu0, b0, WI + lambda_beta * (beta * beta.transpose()), df + beta.cols());
  // update beta
  sample_beta(U);
  // update Uhat
  compute_uhat(Uhat, F, beta);
}

/** Update beta and Uhat */
template<class FType>
void MacauPrior<FType>::sample_beta(const Eigen::MatrixXd &U) {
  const int num_feat = beta.cols();
  // Ft_y = (U .- mu + Normal(0, Lambda^-1)) * F + sqrt(lambda_beta) * Normal(0, Lambda^-1)
  // Ft_y is [ D x F ] matrix
  MatrixXd Ft_y = A_mul_B( (U + MvNormal_prec_omp(Lambda, U.cols())).colwise() - mu, F) + sqrt(lambda_beta) * MvNormal_prec_omp(Lambda, num_feat);

  if (use_FtF) {
    MatrixXd K(FtF.rows(), FtF.cols());
    K.triangularView<Eigen::Lower>() = FtF;
    K.diagonal() += lambda_beta;
    chol_decomp(K);
    chol_solve_t(K, Ft_y);
    beta = Ft_y;
  } else {
    // BlockCG
    solve_blockcg(beta, F, lambda_beta, Ft_y, tol, 32, 8);
  }
}

std::pair<double,double> posterior_lambda_beta(Eigen::MatrixXd & beta, Eigen::MatrixXd & Lambda_u, double nu, double mu) {
  const int D = beta.rows();
  MatrixXd BB(D, D);
  A_mul_At_combo(BB, beta);
  double nux = nu + beta.rows() * beta.cols();
  double mux = mu * nux / (nu + mu * (BB.selfadjointView<Eigen::Lower>() * Lambda_u).trace() );
  double b   = nux / 2;
  double c   = 2 * mux / nux;
  return std::make_pair(b, c);
}

double sample_lambda_beta(Eigen::MatrixXd & beta, Eigen::MatrixXd & Lambda_u, double nu, double mu) {
  auto gamma_post = posterior_lambda_beta(beta, Lambda_u, nu, mu);
  return rgamma(gamma_post.first, gamma_post.second);
}

/** global function */
void sample_latent(MatrixXd &s, int mm, const SparseMatrix<double> &mat, double mean_rating,
    const MatrixXd &samples, double alpha, const VectorXd &mu_u, const MatrixXd &Lambda_u,
    const int num_latent)
{
  // TODO: add cholesky update version
  MatrixXd MM = MatrixXd::Zero(num_latent, num_latent);
  VectorXd rr = VectorXd::Zero(num_latent);
  for (SparseMatrix<double>::InnerIterator it(mat, mm); it; ++it) {
    auto col = samples.col(it.row());
    MM.noalias() += col * col.transpose();
    rr.noalias() += col * ((it.value() - mean_rating) * alpha);
  }

  Eigen::LLT<MatrixXd> chol = (Lambda_u + alpha * MM).llt();
  if(chol.info() != Eigen::Success) {
    throw std::runtime_error("Cholesky Decomposition failed!");
  }

  rr.noalias() += Lambda_u * mu_u;
  chol.matrixL().solveInPlace(rr);
  for (int i = 0; i < num_latent; i++) {
    rr[i] += randn0();
  }
  chol.matrixU().solveInPlace(rr);
  s.col(mm).noalias() = rr;
}

void sample_latent_blas(MatrixXd &s, int mm, const SparseMatrix<double> &mat, double mean_rating,
    const MatrixXd &samples, double alpha, const VectorXd &mu_u, const MatrixXd &Lambda_u,
    const int num_latent)
{
  MatrixXd MM = Lambda_u;
  VectorXd rr = VectorXd::Zero(num_latent);
  for (SparseMatrix<double>::InnerIterator it(mat, mm); it; ++it) {
    auto col = samples.col(it.row());
    MM.triangularView<Eigen::Lower>() += alpha * col * col.transpose();
    rr.noalias() += col * ((it.value() - mean_rating) * alpha);
  }

  Eigen::LLT<MatrixXd> chol = MM.llt();
  if(chol.info() != Eigen::Success) {
    throw std::runtime_error("Cholesky Decomposition failed!");
  }

  rr.noalias() += Lambda_u * mu_u;
  chol.matrixL().solveInPlace(rr);
  for (int i = 0; i < num_latent; i++) {
    rr[i] += randn0();
  }
  chol.matrixU().solveInPlace(rr);
  s.col(mm).noalias() = rr;
}

Eigen::MatrixXd A_mul_B(const Eigen::MatrixXd & A, const Eigen::MatrixXd & B) {
  // TODO: use blas
  return A * B;
}

Eigen::MatrixXd A_mul_B(const Eigen::MatrixXd & A, const SparseFeat & B) {
  // TODO: use blas
  //return A * B;
  return MatrixXd(1,1);
}

<commit_msg>using OpenMP schedule(dynamic,1) for latents<commit_after>#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <cblas.h>
#include <math.h>
#include <omp.h>

#include "mvnormal.h"
#include "macau.h"
#include "chol.h"
#include "linop.h"
extern "C" {
  #include <sparse.h>
}

using namespace std;
using namespace Eigen;

/** BPMFPrior */
void BPMFPrior::sample_latents(Eigen::MatrixXd &U, const Eigen::SparseMatrix<double> &mat, double mean_value,
                    const Eigen::MatrixXd &samples, double alpha, const int num_latent) {
  const int N = U.cols();

#pragma omp parallel for schedule(dynamic, 1)
  for(int n = 0; n < N; n++) {
    sample_latent_blas(U, n, mat, mean_value, samples, alpha, mu, Lambda, num_latent);
  }
}

void BPMFPrior::update_prior(const Eigen::MatrixXd &U) {
  tie(mu, Lambda) = CondNormalWishart(U, mu0, b0, WI, df);
}

void BPMFPrior::init(const int num_latent) {
  mu.resize(num_latent);
  mu.setZero();

  Lambda.resize(num_latent, num_latent);
  Lambda.setIdentity();
  Lambda *= 10;

  // parameters of Inv-Whishart distribution
  WI.resize(num_latent, num_latent);
  WI.setIdentity();
  mu0.resize(num_latent);
  mu0.setZero();
  b0 = 2;
  df = num_latent;
}

/** MacauPrior */
template<class FType>
void MacauPrior<FType>::init(const int num_latent, FType & Fmat, bool comp_FtF) {
  mu.resize(num_latent);
  mu.setZero();

  Lambda.resize(num_latent, num_latent);
  Lambda.setIdentity();
  Lambda *= 10;

  // parameters of Inv-Whishart distribution
  WI.resize(num_latent, num_latent);
  WI.setIdentity();
  mu0.resize(num_latent);
  mu0.setZero();
  b0 = 2;
  df = num_latent;

  // side information
  F = Fmat;
  use_FtF = comp_FtF;
  if (use_FtF) {
    At_mul_A(FtF, F);
  }

  Uhat.resize(num_latent, F.rows());
  Uhat.setZero();

  beta.resize(num_latent, F.cols());
  beta.setZero();
}

template<class FType>
void MacauPrior<FType>::sample_latents(Eigen::MatrixXd &U, const Eigen::SparseMatrix<double> &mat, double mean_value,
                    const Eigen::MatrixXd &samples, double alpha, const int num_latent) {
  const int N = U.cols();
#pragma omp parallel for schedule(dynamic, 1)
  for(int n = 0; n < N; n++) {
    // TODO: try moving mu + Uhat.col(n) inside sample_latent for speed
    sample_latent_blas(U, n, mat, mean_value, samples, alpha, mu + Uhat.col(n), Lambda, num_latent);
  }
}

template<class FType>
void MacauPrior<FType>::update_prior(const Eigen::MatrixXd &U) {
  // residual:
  Uhat.noalias() = U - Uhat;
  tie(mu, Lambda) = CondNormalWishart(Uhat, mu0, b0, WI + lambda_beta * (beta * beta.transpose()), df + beta.cols());
  // update beta
  sample_beta(U);
  // update Uhat
  compute_uhat(Uhat, F, beta);
}

/** Update beta and Uhat */
template<class FType>
void MacauPrior<FType>::sample_beta(const Eigen::MatrixXd &U) {
  const int num_feat = beta.cols();
  // Ft_y = (U .- mu + Normal(0, Lambda^-1)) * F + sqrt(lambda_beta) * Normal(0, Lambda^-1)
  // Ft_y is [ D x F ] matrix
  MatrixXd Ft_y = A_mul_B( (U + MvNormal_prec_omp(Lambda, U.cols())).colwise() - mu, F) + sqrt(lambda_beta) * MvNormal_prec_omp(Lambda, num_feat);

  if (use_FtF) {
    MatrixXd K(FtF.rows(), FtF.cols());
    K.triangularView<Eigen::Lower>() = FtF;
    K.diagonal() += lambda_beta;
    chol_decomp(K);
    chol_solve_t(K, Ft_y);
    beta = Ft_y;
  } else {
    // BlockCG
    solve_blockcg(beta, F, lambda_beta, Ft_y, tol, 32, 8);
  }
}

std::pair<double,double> posterior_lambda_beta(Eigen::MatrixXd & beta, Eigen::MatrixXd & Lambda_u, double nu, double mu) {
  const int D = beta.rows();
  MatrixXd BB(D, D);
  A_mul_At_combo(BB, beta);
  double nux = nu + beta.rows() * beta.cols();
  double mux = mu * nux / (nu + mu * (BB.selfadjointView<Eigen::Lower>() * Lambda_u).trace() );
  double b   = nux / 2;
  double c   = 2 * mux / nux;
  return std::make_pair(b, c);
}

double sample_lambda_beta(Eigen::MatrixXd & beta, Eigen::MatrixXd & Lambda_u, double nu, double mu) {
  auto gamma_post = posterior_lambda_beta(beta, Lambda_u, nu, mu);
  return rgamma(gamma_post.first, gamma_post.second);
}

/** global function */
void sample_latent(MatrixXd &s, int mm, const SparseMatrix<double> &mat, double mean_rating,
    const MatrixXd &samples, double alpha, const VectorXd &mu_u, const MatrixXd &Lambda_u,
    const int num_latent)
{
  // TODO: add cholesky update version
  MatrixXd MM = MatrixXd::Zero(num_latent, num_latent);
  VectorXd rr = VectorXd::Zero(num_latent);
  for (SparseMatrix<double>::InnerIterator it(mat, mm); it; ++it) {
    auto col = samples.col(it.row());
    MM.noalias() += col * col.transpose();
    rr.noalias() += col * ((it.value() - mean_rating) * alpha);
  }

  Eigen::LLT<MatrixXd> chol = (Lambda_u + alpha * MM).llt();
  if(chol.info() != Eigen::Success) {
    throw std::runtime_error("Cholesky Decomposition failed!");
  }

  rr.noalias() += Lambda_u * mu_u;
  chol.matrixL().solveInPlace(rr);
  for (int i = 0; i < num_latent; i++) {
    rr[i] += randn0();
  }
  chol.matrixU().solveInPlace(rr);
  s.col(mm).noalias() = rr;
}

void sample_latent_blas(MatrixXd &s, int mm, const SparseMatrix<double> &mat, double mean_rating,
    const MatrixXd &samples, double alpha, const VectorXd &mu_u, const MatrixXd &Lambda_u,
    const int num_latent)
{
  MatrixXd MM = Lambda_u;
  VectorXd rr = VectorXd::Zero(num_latent);
  for (SparseMatrix<double>::InnerIterator it(mat, mm); it; ++it) {
    auto col = samples.col(it.row());
    MM.triangularView<Eigen::Lower>() += alpha * col * col.transpose();
    rr.noalias() += col * ((it.value() - mean_rating) * alpha);
  }

  Eigen::LLT<MatrixXd> chol = MM.llt();
  if(chol.info() != Eigen::Success) {
    throw std::runtime_error("Cholesky Decomposition failed!");
  }

  rr.noalias() += Lambda_u * mu_u;
  chol.matrixL().solveInPlace(rr);
  for (int i = 0; i < num_latent; i++) {
    rr[i] += randn0();
  }
  chol.matrixU().solveInPlace(rr);
  s.col(mm).noalias() = rr;
}

Eigen::MatrixXd A_mul_B(const Eigen::MatrixXd & A, const Eigen::MatrixXd & B) {
  // TODO: use blas
  return A * B;
}

Eigen::MatrixXd A_mul_B(const Eigen::MatrixXd & A, const SparseFeat & B) {
  // TODO: use blas
  //return A * B;
  return MatrixXd(1,1);
}

<|endoftext|>


<commit_before>
// Copyright 2014 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "config.h"

#include "core/css/invalidation/StyleInvalidator.h"

#include "core/css/invalidation/DescendantInvalidationSet.h"
#include "core/dom/Document.h"
#include "core/dom/Element.h"
#include "core/dom/ElementTraversal.h"
#include "core/dom/shadow/ElementShadow.h"
#include "core/dom/shadow/ShadowRoot.h"
#include "core/inspector/InspectorTraceEvents.h"
#include "core/rendering/RenderObject.h"

namespace blink {

static const unsigned char* s_tracingEnabled = nullptr;

#define TRACE_STYLE_INVALIDATOR_INVALIDATION_IF_ENABLED(element, reason) \
    if (UNLIKELY(*s_tracingEnabled)) \
        TRACE_STYLE_INVALIDATOR_INVALIDATION(element, reason);

void StyleInvalidator::invalidate(Document& document)
{
    RecursionData recursionData;
    if (Element* documentElement = document.documentElement())
        invalidate(*documentElement, recursionData);
    document.clearChildNeedsStyleInvalidation();
    document.clearNeedsStyleInvalidation();
    clearPendingInvalidations();
}

void StyleInvalidator::scheduleInvalidation(PassRefPtrWillBeRawPtr<DescendantInvalidationSet> invalidationSet, Element& element)
{
    ASSERT(element.inActiveDocument());
    ASSERT(element.styleChangeType() < SubtreeStyleChange);
    InvalidationList& list = ensurePendingInvalidationList(element);
    // If we're already going to invalidate the whole subtree we don't need to store any new sets.
    if (!list.isEmpty() && list.last()->wholeSubtreeInvalid())
        return;
    // If this set would invalidate the whole subtree we can discard all existing sets.
    if (invalidationSet->wholeSubtreeInvalid())
        list.clear();
    list.append(invalidationSet);
    element.setNeedsStyleInvalidation();
}

StyleInvalidator::InvalidationList& StyleInvalidator::ensurePendingInvalidationList(Element& element)
{
    PendingInvalidationMap::AddResult addResult = m_pendingInvalidationMap.add(&element, nullptr);
    if (addResult.isNewEntry)
        addResult.storedValue->value = adoptPtrWillBeNoop(new InvalidationList);
    return *addResult.storedValue->value;
}

void StyleInvalidator::clearInvalidation(Node& node)
{
    if (node.isElementNode() && node.needsStyleInvalidation())
        m_pendingInvalidationMap.remove(toElement(&node));
}

void StyleInvalidator::clearPendingInvalidations()
{
    m_pendingInvalidationMap.clear();
}

StyleInvalidator::StyleInvalidator()
{
    s_tracingEnabled = TRACE_EVENT_API_GET_CATEGORY_ENABLED(TRACE_DISABLED_BY_DEFAULT("devtools.timeline.invalidationTracking"));
    DescendantInvalidationSet::cacheTracingFlag();
}

StyleInvalidator::~StyleInvalidator()
{
}

void StyleInvalidator::RecursionData::pushInvalidationSet(const DescendantInvalidationSet& invalidationSet)
{
    ASSERT(!m_wholeSubtreeInvalid);
    if (invalidationSet.treeBoundaryCrossing())
        m_treeBoundaryCrossing = true;
    if (invalidationSet.insertionPointCrossing())
        m_insertionPointCrossing = true;
    if (invalidationSet.wholeSubtreeInvalid()) {
        m_wholeSubtreeInvalid = true;
        return;
    }
    m_invalidationSets.append(&invalidationSet);
    m_invalidateCustomPseudo = invalidationSet.customPseudoInvalid();
}

ALWAYS_INLINE bool StyleInvalidator::RecursionData::matchesCurrentInvalidationSets(Element& element)
{
    ASSERT(!m_wholeSubtreeInvalid);

    if (m_invalidateCustomPseudo && element.shadowPseudoId() != nullAtom) {
        TRACE_STYLE_INVALIDATOR_INVALIDATION_IF_ENABLED(element, InvalidateCustomPseudo);
        return true;
    }

    if (m_insertionPointCrossing && element.isInsertionPoint())
        return true;

    for (const auto& invalidationSet : m_invalidationSets) {
        if (invalidationSet->invalidatesElement(element))
            return true;
    }

    return false;
}

ALWAYS_INLINE bool StyleInvalidator::checkInvalidationSetsAgainstElement(Element& element, StyleInvalidator::RecursionData& recursionData)
{
    if (element.styleChangeType() >= SubtreeStyleChange || recursionData.wholeSubtreeInvalid()) {
        recursionData.setWholeSubtreeInvalid();
        return false;
    }
    if (element.needsStyleInvalidation()) {
        if (InvalidationList* invalidationList = m_pendingInvalidationMap.get(&element)) {
            for (const auto& invalidationSet : *invalidationList)
                recursionData.pushInvalidationSet(*invalidationSet);
            // FIXME: It's really only necessary to clone the render style for this element, not full style recalc.
            if (UNLIKELY(*s_tracingEnabled)) {
                TRACE_EVENT_INSTANT1(TRACE_DISABLED_BY_DEFAULT("devtools.timeline.invalidationTracking"),
                    "StyleInvalidatorInvalidationTracking",
                    "data", InspectorStyleInvalidatorInvalidateEvent::invalidationList(element, *invalidationList));
            }
            return true;
        }
    }

    return recursionData.matchesCurrentInvalidationSets(element);
}

bool StyleInvalidator::invalidateChildren(Element& element, StyleInvalidator::RecursionData& recursionData)
{
    bool someChildrenNeedStyleRecalc = false;
    for (ShadowRoot* root = element.youngestShadowRoot(); root; root = root->olderShadowRoot()) {
        if (!recursionData.treeBoundaryCrossing() && !root->childNeedsStyleInvalidation() && !root->needsStyleInvalidation())
            continue;
        for (Element* child = ElementTraversal::firstChild(*root); child; child = ElementTraversal::nextSibling(*child)) {
            bool childRecalced = invalidate(*child, recursionData);
            someChildrenNeedStyleRecalc = someChildrenNeedStyleRecalc || childRecalced;
        }
        root->clearChildNeedsStyleInvalidation();
        root->clearNeedsStyleInvalidation();
    }
    for (Element* child = ElementTraversal::firstChild(element); child; child = ElementTraversal::nextSibling(*child)) {
        bool childRecalced = invalidate(*child, recursionData);
        someChildrenNeedStyleRecalc = someChildrenNeedStyleRecalc || childRecalced;
    }
    return someChildrenNeedStyleRecalc;
}

bool StyleInvalidator::invalidate(Element& element, StyleInvalidator::RecursionData& recursionData)
{
    RecursionCheckpoint checkpoint(&recursionData);

    bool thisElementNeedsStyleRecalc = checkInvalidationSetsAgainstElement(element, recursionData);

    bool someChildrenNeedStyleRecalc = false;
    if (recursionData.hasInvalidationSets() || element.childNeedsStyleInvalidation())
        someChildrenNeedStyleRecalc = invalidateChildren(element, recursionData);

    if (thisElementNeedsStyleRecalc) {
        element.setNeedsStyleRecalc(recursionData.wholeSubtreeInvalid() ? SubtreeStyleChange : LocalStyleChange, StyleChangeReasonForTracing::create(StyleChangeReason::StyleInvalidator));
    } else if (recursionData.hasInvalidationSets() && someChildrenNeedStyleRecalc) {
        // Clone the RenderStyle in order to preserve correct style sharing, if possible. Otherwise recalc style.
        if (RenderObject* renderer = element.renderer()) {
            renderer->setStyleInternal(RenderStyle::clone(renderer->style()));
        } else {
            TRACE_STYLE_INVALIDATOR_INVALIDATION_IF_ENABLED(element, PreventStyleSharingForParent);
            element.setNeedsStyleRecalc(LocalStyleChange, StyleChangeReasonForTracing::create(StyleChangeReason::StyleInvalidator));
        }
    }

    if (recursionData.insertionPointCrossing() && element.isInsertionPoint())
        element.setNeedsStyleRecalc(SubtreeStyleChange, StyleChangeReasonForTracing::create(StyleChangeReason::StyleInvalidator));

    element.clearChildNeedsStyleInvalidation();
    element.clearNeedsStyleInvalidation();

    return thisElementNeedsStyleRecalc;
}

void StyleInvalidator::trace(Visitor* visitor)
{
#if ENABLE(OILPAN)
    visitor->trace(m_pendingInvalidationMap);
#endif
}

} // namespace blink
<commit_msg>Add comment on StyleInvalidator trace events<commit_after>
// Copyright 2014 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "config.h"

#include "core/css/invalidation/StyleInvalidator.h"

#include "core/css/invalidation/DescendantInvalidationSet.h"
#include "core/dom/Document.h"
#include "core/dom/Element.h"
#include "core/dom/ElementTraversal.h"
#include "core/dom/shadow/ElementShadow.h"
#include "core/dom/shadow/ShadowRoot.h"
#include "core/inspector/InspectorTraceEvents.h"
#include "core/rendering/RenderObject.h"

namespace blink {

// StyleInvalidator methods are super sensitive to performance benchmarks.
// We easily get 1% regression per additional if statement on recursive
// invalidate methods.
// To minimize performance impact, we wrap trace events with a lookup of
// cached flag. The cached flag is made "static const" and is not shared
// with DescendantInvalidationSet to avoid additional GOT lookup cost.
static const unsigned char* s_tracingEnabled = nullptr;

#define TRACE_STYLE_INVALIDATOR_INVALIDATION_IF_ENABLED(element, reason) \
    if (UNLIKELY(*s_tracingEnabled)) \
        TRACE_STYLE_INVALIDATOR_INVALIDATION(element, reason);

void StyleInvalidator::invalidate(Document& document)
{
    RecursionData recursionData;
    if (Element* documentElement = document.documentElement())
        invalidate(*documentElement, recursionData);
    document.clearChildNeedsStyleInvalidation();
    document.clearNeedsStyleInvalidation();
    clearPendingInvalidations();
}

void StyleInvalidator::scheduleInvalidation(PassRefPtrWillBeRawPtr<DescendantInvalidationSet> invalidationSet, Element& element)
{
    ASSERT(element.inActiveDocument());
    ASSERT(element.styleChangeType() < SubtreeStyleChange);
    InvalidationList& list = ensurePendingInvalidationList(element);
    // If we're already going to invalidate the whole subtree we don't need to store any new sets.
    if (!list.isEmpty() && list.last()->wholeSubtreeInvalid())
        return;
    // If this set would invalidate the whole subtree we can discard all existing sets.
    if (invalidationSet->wholeSubtreeInvalid())
        list.clear();
    list.append(invalidationSet);
    element.setNeedsStyleInvalidation();
}

StyleInvalidator::InvalidationList& StyleInvalidator::ensurePendingInvalidationList(Element& element)
{
    PendingInvalidationMap::AddResult addResult = m_pendingInvalidationMap.add(&element, nullptr);
    if (addResult.isNewEntry)
        addResult.storedValue->value = adoptPtrWillBeNoop(new InvalidationList);
    return *addResult.storedValue->value;
}

void StyleInvalidator::clearInvalidation(Node& node)
{
    if (node.isElementNode() && node.needsStyleInvalidation())
        m_pendingInvalidationMap.remove(toElement(&node));
}

void StyleInvalidator::clearPendingInvalidations()
{
    m_pendingInvalidationMap.clear();
}

StyleInvalidator::StyleInvalidator()
{
    s_tracingEnabled = TRACE_EVENT_API_GET_CATEGORY_ENABLED(TRACE_DISABLED_BY_DEFAULT("devtools.timeline.invalidationTracking"));
    DescendantInvalidationSet::cacheTracingFlag();
}

StyleInvalidator::~StyleInvalidator()
{
}

void StyleInvalidator::RecursionData::pushInvalidationSet(const DescendantInvalidationSet& invalidationSet)
{
    ASSERT(!m_wholeSubtreeInvalid);
    if (invalidationSet.treeBoundaryCrossing())
        m_treeBoundaryCrossing = true;
    if (invalidationSet.insertionPointCrossing())
        m_insertionPointCrossing = true;
    if (invalidationSet.wholeSubtreeInvalid()) {
        m_wholeSubtreeInvalid = true;
        return;
    }
    m_invalidationSets.append(&invalidationSet);
    m_invalidateCustomPseudo = invalidationSet.customPseudoInvalid();
}

ALWAYS_INLINE bool StyleInvalidator::RecursionData::matchesCurrentInvalidationSets(Element& element)
{
    ASSERT(!m_wholeSubtreeInvalid);

    if (m_invalidateCustomPseudo && element.shadowPseudoId() != nullAtom) {
        TRACE_STYLE_INVALIDATOR_INVALIDATION_IF_ENABLED(element, InvalidateCustomPseudo);
        return true;
    }

    if (m_insertionPointCrossing && element.isInsertionPoint())
        return true;

    for (const auto& invalidationSet : m_invalidationSets) {
        if (invalidationSet->invalidatesElement(element))
            return true;
    }

    return false;
}

ALWAYS_INLINE bool StyleInvalidator::checkInvalidationSetsAgainstElement(Element& element, StyleInvalidator::RecursionData& recursionData)
{
    if (element.styleChangeType() >= SubtreeStyleChange || recursionData.wholeSubtreeInvalid()) {
        recursionData.setWholeSubtreeInvalid();
        return false;
    }
    if (element.needsStyleInvalidation()) {
        if (InvalidationList* invalidationList = m_pendingInvalidationMap.get(&element)) {
            for (const auto& invalidationSet : *invalidationList)
                recursionData.pushInvalidationSet(*invalidationSet);
            // FIXME: It's really only necessary to clone the render style for this element, not full style recalc.
            if (UNLIKELY(*s_tracingEnabled)) {
                TRACE_EVENT_INSTANT1(TRACE_DISABLED_BY_DEFAULT("devtools.timeline.invalidationTracking"),
                    "StyleInvalidatorInvalidationTracking",
                    "data", InspectorStyleInvalidatorInvalidateEvent::invalidationList(element, *invalidationList));
            }
            return true;
        }
    }

    return recursionData.matchesCurrentInvalidationSets(element);
}

bool StyleInvalidator::invalidateChildren(Element& element, StyleInvalidator::RecursionData& recursionData)
{
    bool someChildrenNeedStyleRecalc = false;
    for (ShadowRoot* root = element.youngestShadowRoot(); root; root = root->olderShadowRoot()) {
        if (!recursionData.treeBoundaryCrossing() && !root->childNeedsStyleInvalidation() && !root->needsStyleInvalidation())
            continue;
        for (Element* child = ElementTraversal::firstChild(*root); child; child = ElementTraversal::nextSibling(*child)) {
            bool childRecalced = invalidate(*child, recursionData);
            someChildrenNeedStyleRecalc = someChildrenNeedStyleRecalc || childRecalced;
        }
        root->clearChildNeedsStyleInvalidation();
        root->clearNeedsStyleInvalidation();
    }
    for (Element* child = ElementTraversal::firstChild(element); child; child = ElementTraversal::nextSibling(*child)) {
        bool childRecalced = invalidate(*child, recursionData);
        someChildrenNeedStyleRecalc = someChildrenNeedStyleRecalc || childRecalced;
    }
    return someChildrenNeedStyleRecalc;
}

bool StyleInvalidator::invalidate(Element& element, StyleInvalidator::RecursionData& recursionData)
{
    RecursionCheckpoint checkpoint(&recursionData);

    bool thisElementNeedsStyleRecalc = checkInvalidationSetsAgainstElement(element, recursionData);

    bool someChildrenNeedStyleRecalc = false;
    if (recursionData.hasInvalidationSets() || element.childNeedsStyleInvalidation())
        someChildrenNeedStyleRecalc = invalidateChildren(element, recursionData);

    if (thisElementNeedsStyleRecalc) {
        element.setNeedsStyleRecalc(recursionData.wholeSubtreeInvalid() ? SubtreeStyleChange : LocalStyleChange, StyleChangeReasonForTracing::create(StyleChangeReason::StyleInvalidator));
    } else if (recursionData.hasInvalidationSets() && someChildrenNeedStyleRecalc) {
        // Clone the RenderStyle in order to preserve correct style sharing, if possible. Otherwise recalc style.
        if (RenderObject* renderer = element.renderer()) {
            renderer->setStyleInternal(RenderStyle::clone(renderer->style()));
        } else {
            TRACE_STYLE_INVALIDATOR_INVALIDATION_IF_ENABLED(element, PreventStyleSharingForParent);
            element.setNeedsStyleRecalc(LocalStyleChange, StyleChangeReasonForTracing::create(StyleChangeReason::StyleInvalidator));
        }
    }

    if (recursionData.insertionPointCrossing() && element.isInsertionPoint())
        element.setNeedsStyleRecalc(SubtreeStyleChange, StyleChangeReasonForTracing::create(StyleChangeReason::StyleInvalidator));

    element.clearChildNeedsStyleInvalidation();
    element.clearNeedsStyleInvalidation();

    return thisElementNeedsStyleRecalc;
}

void StyleInvalidator::trace(Visitor* visitor)
{
#if ENABLE(OILPAN)
    visitor->trace(m_pendingInvalidationMap);
#endif
}

} // namespace blink
<|endoftext|>


<commit_before>/*
  This file is part of the Grantlee template system.

  Copyright (c) 2009,2010 Stephen Kelly <steveire@gmail.com>

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either version
  2.1 of the Licence, or (at your option) any later version.

  This library 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
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library.  If not, see <http://www.gnu.org/licenses/>.

*/

#include "filterexpression.h"

#include "exception.h"
#include "filter.h"
#include "grantlee_latin1literal_p.h"
#include "metatype.h"
#include "parser.h"
#include "util.h"

#include <QtCore/QBuffer>

typedef QPair<Grantlee::Filter::Ptr, Grantlee::Variable > ArgFilter;

namespace Grantlee
{

class FilterExpressionPrivate
{
  FilterExpressionPrivate( FilterExpression *fe )
      : q_ptr( fe ) {
  }

  Variable m_variable;
  QList<ArgFilter> m_filters;
  QStringList m_filterNames;

  Q_DECLARE_PUBLIC( FilterExpression )
  FilterExpression * const q_ptr;
};

}

using namespace Grantlee;

static const char FILTER_SEPARATOR = '|';
static const char FILTER_ARGUMENT_SEPARATOR = ':';

static QRegExp getFilterRegexp()
{
  const QString filterSep( QRegExp::escape( QChar( FILTER_SEPARATOR ) ) );
  const QString argSep( QRegExp::escape( QChar( FILTER_ARGUMENT_SEPARATOR ) ) );

  const QLatin1Literal varChars( "abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789_." );
  const QLatin1Literal numChars( "[-+\\.]?\\d[\\d\\.e]*" );
  const QString i18nOpen( QRegExp::escape( QLatin1String( "_(" ) ) );
  const QLatin1Literal doubleQuoteStringLiteral( "\"[^\"\\\\]*(?:\\\\.[^\"\\\\]*)*\"" );
  const QLatin1Literal singleQuoteStringLiteral( "\'[^\'\\\\]*(?:\\\\.[^\'\\\\]*)*\'" );
  const QString i18nClose( QRegExp::escape( QLatin1String( ")" ) ) );

  const QString constantString = QLatin1Literal( "(?:" ) + i18nOpen + doubleQuoteStringLiteral + i18nClose + QLatin1Char( '|' )
                                                         + i18nOpen + singleQuoteStringLiteral + i18nClose + QLatin1Char( '|' )
                                                         + doubleQuoteStringLiteral + QLatin1Char( '|' )
                                                         + singleQuoteStringLiteral
                               + QLatin1Char( ')' );

  const QString filterRawString = QLatin1Char( '^' ) + constantString + QLatin1Char( '|' )
                               + QLatin1Literal( "^[" ) + varChars + QLatin1Literal( "]+|" )
                               + numChars + QLatin1Char( '|' )
                               + filterSep + QLatin1Literal( "\\w+|" )
                               + argSep
                               + QLatin1Literal( "(?:" )
                                 + constantString
                                 + QLatin1Literal( "|[" )
                                 + varChars
                                 + QLatin1Literal( "]+|" )
                                 + numChars
                                 + QLatin1Char( '|' )
                                 + filterSep
                               + QLatin1Literal( "\\w+)" );

  return QRegExp( filterRawString );
}

FilterExpression::FilterExpression( const QString &varString, Parser *parser )
  : d_ptr( new FilterExpressionPrivate( this ) )
{
  Q_D( FilterExpression );

  int pos = 0;
  int lastPos = 0;
  int len;
  QString subString;

  QString vs = varString;

  static const QRegExp sFilterRe = getFilterRegexp();

  // This is one fo the few contstructors that can throw so we make sure to delete its d->pointer.
  try {
    while ( ( pos = sFilterRe.indexIn( vs, pos ) ) != -1 ) {
      len = sFilterRe.matchedLength();
      subString = vs.mid( pos, len );
      const int ssSize = subString.size();

      if ( pos != lastPos ) {
        throw Grantlee::Exception( TagSyntaxError,
            QString::fromLatin1( "Could not parse some characters: \"%1\"" ).arg( vs.mid( lastPos, pos ) ) );
      }

      if ( subString.startsWith( FILTER_SEPARATOR ) ) {
        subString = subString.right( ssSize - 1 );
        Filter::Ptr f = parser->getFilter( subString );

        Q_ASSERT( f );

        d->m_filterNames << subString;
        d->m_filters << qMakePair<Filter::Ptr, Variable>( f, Variable() );

      } else if ( subString.startsWith( FILTER_ARGUMENT_SEPARATOR ) ) {
        subString = subString.right( ssSize - 1 );
        const int lastFilter = d->m_filters.size();
        if ( subString.isEmpty() )
          throw Grantlee::Exception( EmptyVariableError,
              QString::fromLatin1( "Missing argument to filter: %1" ).arg( d->m_filterNames[lastFilter -1] ) );

        d->m_filters[lastFilter -1].second = Variable( subString );
      } else {
        // Token is _("translated"), or "constant", or a variable;
        d->m_variable = Variable( subString );
      }

      pos += len;
      lastPos = pos;
    }

    const QString remainder = vs.right( vs.size() - lastPos );
    if ( !remainder.isEmpty() ) {
      throw Grantlee::Exception( TagSyntaxError,
          QString::fromLatin1( "Could not parse the remainder, %1 from %2" ).arg( remainder ).arg( varString ) );
    }
  } catch ( ... ) {
    delete d_ptr;
    throw;
  }
}

FilterExpression::FilterExpression( const FilterExpression &other )
    : d_ptr( new FilterExpressionPrivate( this ) )
{
  d_ptr->m_variable = other.d_ptr->m_variable;
  d_ptr->m_filters = other.d_ptr->m_filters;
  d_ptr->m_filterNames = other.d_ptr->m_filterNames;
}

FilterExpression::FilterExpression()
    : d_ptr( new FilterExpressionPrivate( this ) )
{
}

bool FilterExpression::isValid() const
{
  Q_D( const FilterExpression );
  return d->m_variable.isValid();
}

FilterExpression::~FilterExpression()
{
  delete d_ptr;
}

Variable FilterExpression::variable() const
{
  Q_D( const FilterExpression );
  return d->m_variable;
}

FilterExpression &FilterExpression::operator=( const FilterExpression & other )
{
  d_ptr->m_variable = other.d_ptr->m_variable;
  d_ptr->m_filters = other.d_ptr->m_filters;
  return *this;
}

QVariant FilterExpression::resolve( OutputStream *stream, Context *c ) const
{
  Q_D( const FilterExpression );
  QVariant var = d->m_variable.resolve( c );

  Q_FOREACH( const ArgFilter &argfilter, d->m_filters ) {
    Filter::Ptr filter = argfilter.first;
    filter->setStream( stream );
    const Variable argVar = argfilter.second;
    QVariant arg = argVar.resolve( c );

    if ( arg.isValid() ) {
      Grantlee::SafeString argString;
      if ( arg.userType() == qMetaTypeId<Grantlee::SafeString>() ) {
        argString = arg.value<Grantlee::SafeString>();
      } else if ( arg.type() == QVariant::String ) {
        argString = Grantlee::SafeString( arg.toString() );
      }
      if ( argVar.isConstant() ) {
        argString = markSafe( argString );
      }
      if ( !argString.get().isEmpty() ) {
        arg = argString;
      }
    }

    const SafeString varString = getSafeString( var );

    var = filter->doFilter( var, arg, c->autoEscape() );

    if ( var.userType() == qMetaTypeId<Grantlee::SafeString>() || var.type() == QVariant::String ) {
      if ( filter->isSafe() && varString.isSafe() ) {
        var = markSafe( getSafeString( var ) );
      } else if ( varString.needsEscape() ) {
        var = markForEscaping( getSafeString( var ) );
      } else {
        var = getSafeString( var );
      }
    }
  }
  ( *stream ) << getSafeString( var ).get();
  return var;
}

QVariant FilterExpression::resolve( Context *c ) const
{
  OutputStream _dummy;
  return resolve( &_dummy, c );
}

QVariantList FilterExpression::toList( Context *c ) const
{
  const QVariant var = resolve( c );
  return MetaType::toVariantList( var );
}

bool FilterExpression::isTrue( Context *c ) const
{
  return variantIsTrue( resolve( c ) );
}

QStringList FilterExpression::filters() const
{
  Q_D( const FilterExpression );
  return d->m_filterNames;
}

<commit_msg>Some minor cleanups.<commit_after>/*
  This file is part of the Grantlee template system.

  Copyright (c) 2009,2010 Stephen Kelly <steveire@gmail.com>

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either version
  2.1 of the Licence, or (at your option) any later version.

  This library 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
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General Public
  License along with this library.  If not, see <http://www.gnu.org/licenses/>.

*/

#include "filterexpression.h"

#include "exception.h"
#include "filter.h"
#include "grantlee_latin1literal_p.h"
#include "metatype.h"
#include "parser.h"
#include "util.h"

typedef QPair<Grantlee::Filter::Ptr, Grantlee::Variable> ArgFilter;

namespace Grantlee
{

class FilterExpressionPrivate
{
  FilterExpressionPrivate( FilterExpression *fe )
      : q_ptr( fe )
  {

  }

  Variable m_variable;
  QList<ArgFilter> m_filters;
  QStringList m_filterNames;

  Q_DECLARE_PUBLIC( FilterExpression )
  FilterExpression * const q_ptr;
};

}

using namespace Grantlee;

static const char FILTER_SEPARATOR = '|';
static const char FILTER_ARGUMENT_SEPARATOR = ':';

static QRegExp getFilterRegexp()
{
  const QString filterSep( QRegExp::escape( QChar( FILTER_SEPARATOR ) ) );
  const QString argSep( QRegExp::escape( QChar( FILTER_ARGUMENT_SEPARATOR ) ) );

  const QLatin1Literal varChars( "abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789_." );
  const QLatin1Literal numChars( "[-+\\.]?\\d[\\d\\.e]*" );
  const QString i18nOpen( QRegExp::escape( QLatin1String( "_(" ) ) );
  const QLatin1Literal doubleQuoteStringLiteral( "\"[^\"\\\\]*(?:\\\\.[^\"\\\\]*)*\"" );
  const QLatin1Literal singleQuoteStringLiteral( "\'[^\'\\\\]*(?:\\\\.[^\'\\\\]*)*\'" );
  const QString i18nClose( QRegExp::escape( QLatin1String( ")" ) ) );

  const QString constantString = QLatin1Literal( "(?:" ) + i18nOpen + doubleQuoteStringLiteral + i18nClose + QLatin1Char( '|' )
                                                         + i18nOpen + singleQuoteStringLiteral + i18nClose + QLatin1Char( '|' )
                                                         + doubleQuoteStringLiteral + QLatin1Char( '|' )
                                                         + singleQuoteStringLiteral
                               + QLatin1Char( ')' );

  const QString filterRawString = QLatin1Char( '^' ) + constantString + QLatin1Char( '|' )
                               + QLatin1Literal( "^[" ) + varChars + QLatin1Literal( "]+|" )
                               + numChars + QLatin1Char( '|' )
                               + filterSep + QLatin1Literal( "\\w+|" )
                               + argSep
                               + QLatin1Literal( "(?:" )
                                 + constantString
                                 + QLatin1Literal( "|[" )
                                 + varChars
                                 + QLatin1Literal( "]+|" )
                                 + numChars
                                 + QLatin1Char( '|' )
                                 + filterSep
                               + QLatin1Literal( "\\w+)" );

  return QRegExp( filterRawString );
}

FilterExpression::FilterExpression( const QString &varString, Parser *parser )
  : d_ptr( new FilterExpressionPrivate( this ) )
{
  Q_D( FilterExpression );

  int pos = 0;
  int lastPos = 0;
  int len;
  QString subString;

  QString vs = varString;

  static const QRegExp sFilterRe = getFilterRegexp();

  // This is one fo the few contstructors that can throw so we make sure to delete its d->pointer.
  try {
    while ( ( pos = sFilterRe.indexIn( vs, pos ) ) != -1 ) {
      len = sFilterRe.matchedLength();
      subString = vs.mid( pos, len );
      const int ssSize = subString.size();

      if ( pos != lastPos ) {
        throw Grantlee::Exception( TagSyntaxError,
            QString::fromLatin1( "Could not parse some characters: \"%1\"" ).arg( vs.mid( lastPos, pos ) ) );
      }

      if ( subString.startsWith( FILTER_SEPARATOR ) ) {
        subString = subString.right( ssSize - 1 );
        Filter::Ptr f = parser->getFilter( subString );

        Q_ASSERT( f );

        d->m_filterNames << subString;
        d->m_filters << qMakePair<Filter::Ptr, Variable>( f, Variable() );

      } else if ( subString.startsWith( FILTER_ARGUMENT_SEPARATOR ) ) {
        subString = subString.right( ssSize - 1 );
        const int lastFilter = d->m_filters.size();
        if ( subString.isEmpty() )
          throw Grantlee::Exception( EmptyVariableError,
              QString::fromLatin1( "Missing argument to filter: %1" ).arg( d->m_filterNames[lastFilter -1] ) );

        d->m_filters[lastFilter -1].second = Variable( subString );
      } else {
        // Token is _("translated"), or "constant", or a variable;
        d->m_variable = Variable( subString );
      }

      pos += len;
      lastPos = pos;
    }

    const QString remainder = vs.right( vs.size() - lastPos );
    if ( !remainder.isEmpty() ) {
      throw Grantlee::Exception( TagSyntaxError,
          QString::fromLatin1( "Could not parse the remainder, %1 from %2" ).arg( remainder ).arg( varString ) );
    }
  } catch ( ... ) {
    delete d_ptr;
    throw;
  }
}

FilterExpression::FilterExpression( const FilterExpression &other )
    : d_ptr( new FilterExpressionPrivate( this ) )
{
  d_ptr->m_variable = other.d_ptr->m_variable;
  d_ptr->m_filters = other.d_ptr->m_filters;
  d_ptr->m_filterNames = other.d_ptr->m_filterNames;
}

FilterExpression::FilterExpression()
    : d_ptr( new FilterExpressionPrivate( this ) )
{
}

bool FilterExpression::isValid() const
{
  Q_D( const FilterExpression );
  return d->m_variable.isValid();
}

FilterExpression::~FilterExpression()
{
  delete d_ptr;
}

Variable FilterExpression::variable() const
{
  Q_D( const FilterExpression );
  return d->m_variable;
}

FilterExpression &FilterExpression::operator=( const FilterExpression & other )
{
  d_ptr->m_variable = other.d_ptr->m_variable;
  d_ptr->m_filters = other.d_ptr->m_filters;
  d_ptr->m_filterNames = other.d_ptr->m_filterNames;
  return *this;
}

QVariant FilterExpression::resolve( OutputStream *stream, Context *c ) const
{
  Q_D( const FilterExpression );
  QVariant var = d->m_variable.resolve( c );

  Q_FOREACH( const ArgFilter &argfilter, d->m_filters ) {
    Filter::Ptr filter = argfilter.first;
    filter->setStream( stream );
    const Variable argVar = argfilter.second;
    QVariant arg = argVar.resolve( c );

    if ( arg.isValid() ) {
      Grantlee::SafeString argString;
      if ( arg.userType() == qMetaTypeId<Grantlee::SafeString>() ) {
        argString = arg.value<Grantlee::SafeString>();
      } else if ( arg.type() == QVariant::String ) {
        argString = Grantlee::SafeString( arg.toString() );
      }
      if ( argVar.isConstant() ) {
        argString = markSafe( argString );
      }
      if ( !argString.get().isEmpty() ) {
        arg = argString;
      }
    }

    const SafeString varString = getSafeString( var );

    var = filter->doFilter( var, arg, c->autoEscape() );

    if ( var.userType() == qMetaTypeId<Grantlee::SafeString>() || var.type() == QVariant::String ) {
      if ( filter->isSafe() && varString.isSafe() ) {
        var = markSafe( getSafeString( var ) );
      } else if ( varString.needsEscape() ) {
        var = markForEscaping( getSafeString( var ) );
      } else {
        var = getSafeString( var );
      }
    }
  }
  ( *stream ) << getSafeString( var ).get();
  return var;
}

QVariant FilterExpression::resolve( Context *c ) const
{
  OutputStream _dummy;
  return resolve( &_dummy, c );
}

QVariantList FilterExpression::toList( Context *c ) const
{
  const QVariant var = resolve( c );
  return MetaType::toVariantList( var );
}

bool FilterExpression::isTrue( Context *c ) const
{
  return variantIsTrue( resolve( c ) );
}

QStringList FilterExpression::filters() const
{
  Q_D( const FilterExpression );
  return d->m_filterNames;
}

<|endoftext|>


<commit_before>
#include "Halide.h"
using namespace Halide;

enum ComplxFields { _REAL = 0, _IMAG,  _CPLX_FIELDS };

int main(int argc, char **argv) {
    if (argc < 2) return 1;

    ImageParam in_grid(type_of<double>(), 3, "in_grid");
    in_grid.set_extent(0, _CPLX_FIELDS).set_stride(1, _CPLX_FIELDS);

    Func out_grid("out_grid"); Var cmplx("c"),x("x"),y("y");
    out_grid(cmplx,x,y) = undef<double>();

    // Determine bounds
    OutputImageParam out_buf = out_grid.output_buffer();
    Expr xmin = max(out_buf.min(1), in_grid.min(1)),
         xmax = min(out_buf.min(1) + out_buf.extent(1),
                    in_grid.min(1) + in_grid.extent(1));
    Expr ymin = max(out_buf.min(2), in_grid.min(2)),
         ymax = min(out_buf.min(2) + out_buf.extent(2),
                    in_grid.min(2) + in_grid.extent(2));

    // Update with data
    RDom rdom(0,2,xmin,xmax-xmin,ymin,ymax-ymin);
    RVar rc = rdom.x, rx = rdom.y, ry = rdom.z;
    out_grid(rc,rx,ry) += in_grid(rc,rx,ry);

    RVar rcx("cx");
    out_grid.output_buffer().set_extent(0, _CPLX_FIELDS).set_stride(1, _CPLX_FIELDS);
    out_grid.bound(cmplx, 0, _CPLX_FIELDS);
    out_grid.update().fuse(rx, rc, rcx).vectorize(rcx, 2);

    std::vector<Halide::Argument> args = { in_grid };
    Target target(get_target_from_environment().os, Target::X86, 64, { Target::SSE41, Target::AVX});
    Module mod = out_grid.compile_to_module(args, "kern_detile", target);
    compile_module_to_object(mod, argv[1]);
    return 0;
}
<commit_msg>Detiling adjustments<commit_after>
#include "Halide.h"
using namespace Halide;

int main(int argc, char **argv) {
    if (argc < 2) return 1;

    const int _CPLX_FIELDS = 2;

    ImageParam in_grid(type_of<double>(), 3, "in_grid");
    in_grid.set_extent(0, _CPLX_FIELDS).set_stride(0,1).set_stride(1, _CPLX_FIELDS);

    Func out_grid("out_grid"); Var cmplx("c"),x("x"),y("y");
    out_grid(cmplx,x,y) = undef<double>();

    // Determine bounds. As tiles with margin are expected to go
    // outside the field, we have to account for *both* buffer bounds
    // here.
    OutputImageParam out_buf = out_grid.output_buffer();
    Expr xmin = max(out_buf.min(1), in_grid.min(1)),
         xmax = min(out_buf.min(1) + out_buf.extent(1),
                    in_grid.min(1) + in_grid.extent(1));
    Expr ymin = max(out_buf.min(2), in_grid.min(2)),
         ymax = min(out_buf.min(2) + out_buf.extent(2),
                    in_grid.min(2) + in_grid.extent(2));

    // Update with data
    RDom rdom(0,2,xmin,xmax-xmin,ymin,ymax-ymin);
    RVar rc = rdom.x, rx = rdom.y, ry = rdom.z;
    out_grid(rc,rx,ry) += in_grid(rc,rx,ry);

    out_grid.output_buffer().set_extent(0, _CPLX_FIELDS).set_stride(0,1).set_stride(1, _CPLX_FIELDS);
    out_grid.bound(cmplx, 0, _CPLX_FIELDS);
    out_grid.update().vectorize(rc, 2);

    std::vector<Halide::Argument> args = { in_grid };
    Target target(get_target_from_environment().os, Target::X86, 64, { Target::SSE41, Target::AVX});
    Module mod = out_grid.compile_to_module(args, "kern_detile", target);
    compile_module_to_object(mod, argv[1]);
    compile_module_to_html(mod, "detile.html");
    return 0;
}
<|endoftext|>