<commit_before>/*M///////////////////////////////////////////////////////////////////////////////////////
 //
 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
 //
 //  By downloading, copying, installing or using the software you agree to this license.
 //  If you do not agree to this license, do not download, install,
 //  copy or use the software.
 //
 //
 //                           License Agreement
 //                For Open Source Computer Vision Library
 //
 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
 // Copyright (C) 2008-2013, Willow Garage Inc., all rights reserved.
 // Third party copyrights are property of their respective owners.
 //
 // Redistribution and use in source and binary forms, with or without modification,
 // are permitted provided that the following conditions are met:
 //
 //   * Redistribution's of source code must retain the above copyright notice,
 //     this list of conditions and the following disclaimer.
 //
 //   * Redistribution's 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.
 //
 //   * The name of the copyright holders may not 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 Intel Corporation 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.
 //
 //M*/

#include "test_precomp.hpp"
#include "opencv2/core/affine.hpp"
#include "opencv2/calib3d.hpp"
#include <iostream>

TEST(Calib3d_Affine3f, accuracy)
{
    cv::Vec3d rvec(0.2, 0.5, 0.3);
    cv::Affine3d affine(rvec);

    cv::Mat expected;
    cv::Rodrigues(rvec, expected);

    ASSERT_EQ(0, cvtest::norm(cv::Mat(affine.matrix, false).colRange(0, 3).rowRange(0, 3) != expected, cv::NORM_L2));
    ASSERT_EQ(0, cvtest::norm(cv::Mat(affine.linear()) != expected, cv::NORM_L2));

    cv::Matx33d R = cv::Matx33d::eye();

    double angle = 50;
    R.val[0] = R.val[4] = std::cos(CV_PI*angle/180.0);
    R.val[3] = std::sin(CV_PI*angle/180.0);
    R.val[1] = -R.val[3];

    cv::Affine3d affine1(cv::Mat(cv::Vec3d(0.2, 0.5, 0.3)).reshape(1, 1), cv::Vec3d(4, 5, 6));
    cv::Affine3d affine2(R, cv::Vec3d(1, 1, 0.4));

    cv::Affine3d result = affine1.inv() * affine2;

    expected = cv::Mat(affine1.matrix.inv(cv::DECOMP_SVD)) * cv::Mat(affine2.matrix, false);

    cv::Mat diff;
    cv::absdiff(expected, result.matrix, diff);

    ASSERT_LT(cvtest::norm(diff, cv::NORM_INF), 1e-15);
}

TEST(Calib3d_Affine3f, accuracy_rvec)
{
    cv::RNG rng;
    typedef float T;

    cv::Affine3<T>::Vec3 w;
    cv::Affine3<T>::Mat3 u, vt, R;

    for(int i = 0; i < 100; ++i)
    {
        rng.fill(R, cv::RNG::UNIFORM, -10, 10, true);
        cv::SVD::compute(R, w, u, vt, cv::SVD::FULL_UV + cv::SVD::MODIFY_A);
        R = u * vt;

        //double s = (double)cv::getTickCount();
        cv::Affine3<T>::Vec3 va = cv::Affine3<T>(R).rvec();
        //std::cout << "M:" <<(cv::getTickCount() - s)*1000/cv::getTickFrequency() << std::endl;

        cv::Affine3<T>::Vec3 vo;
        //s = (double)cv::getTickCount();
        cv::Rodrigues(R, vo);
        //std::cout << "O:" <<(cv::getTickCount() - s)*1000/cv::getTickFrequency() << std::endl;

        ASSERT_LT(cvtest::norm(va, vo, cv::NORM_L2), 1e-9);
    }
}
<commit_msg>fixing spaces for conformance<commit_after>/*M///////////////////////////////////////////////////////////////////////////////////////
 //
 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
 //
 //  By downloading, copying, installing or using the software you agree to this license.
 //  If you do not agree to this license, do not download, install,
 //  copy or use the software.
 //
 //
 //                           License Agreement
 //                For Open Source Computer Vision Library
 //
 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
 // Copyright (C) 2008-2013, Willow Garage Inc., all rights reserved.
 // Third party copyrights are property of their respective owners.
 //
 // Redistribution and use in source and binary forms, with or without modification,
 // are permitted provided that the following conditions are met:
 //
 //   * Redistribution's of source code must retain the above copyright notice,
 //     this list of conditions and the following disclaimer.
 //
 //   * Redistribution's 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.
 //
 //   * The name of the copyright holders may not 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 Intel Corporation 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.
 //
 //M*/

#include "test_precomp.hpp"
#include "opencv2/core/affine.hpp"
#include "opencv2/calib3d.hpp"
#include <iostream>

TEST(Calib3d_Affine3f, accuracy)
{
    cv::Vec3d rvec(0.2, 0.5, 0.3);
    cv::Affine3d affine(rvec);

    cv::Mat expected;
    cv::Rodrigues(rvec, expected);

    ASSERT_EQ(0, cvtest::norm(cv::Mat(affine.matrix, false).colRange(0, 3).rowRange(0, 3) != expected, cv::NORM_L2));
    ASSERT_EQ(0, cvtest::norm(cv::Mat(affine.linear()) != expected, cv::NORM_L2));

    cv::Matx33d R = cv::Matx33d::eye();

    double angle = 50;
    R.val[0] = R.val[4] = std::cos(CV_PI*angle/180.0);
    R.val[3] = std::sin(CV_PI*angle/180.0);
    R.val[1] = -R.val[3];

    cv::Affine3d affine1(cv::Mat(cv::Vec3d(0.2, 0.5, 0.3)).reshape(1, 1), cv::Vec3d(4, 5, 6));
    cv::Affine3d affine2(R, cv::Vec3d(1, 1, 0.4));

    cv::Affine3d result = affine1.inv() * affine2;

    expected = cv::Mat(affine1.matrix.inv(cv::DECOMP_SVD)) * cv::Mat(affine2.matrix, false);

    cv::Mat diff;
    cv::absdiff(expected, result.matrix, diff);

    ASSERT_LT(cvtest::norm(diff, cv::NORM_INF), 1e-15);
}

TEST(Calib3d_Affine3f, accuracy_rvec)
{
    cv::RNG rng;
    typedef float T;

    cv::Affine3<T>::Vec3 w;
    cv::Affine3<T>::Mat3 u, vt, R;

    for(int i = 0; i < 100; ++i)
    {
        rng.fill(R, cv::RNG::UNIFORM, -10, 10, true);
        cv::SVD::compute(R, w, u, vt, cv::SVD::FULL_UV + cv::SVD::MODIFY_A);
        R = u * vt;

        //double s = (double)cv::getTickCount();
        cv::Affine3<T>::Vec3 va = cv::Affine3<T>(R).rvec();
        //std::cout << "M:" <<(cv::getTickCount() - s)*1000/cv::getTickFrequency() << std::endl;

        cv::Affine3<T>::Vec3 vo;
        //s = (double)cv::getTickCount();
        cv::Rodrigues(R, vo);
        //std::cout << "O:" <<(cv::getTickCount() - s)*1000/cv::getTickFrequency() << std::endl;

        ASSERT_LT(cvtest::norm(va, vo, cv::NORM_L2), 1e-9);
    }
}
<|endoftext|>


<commit_before>/**
 * File: main.cpp
 * Project: tsc - TinyStarChart
 * Creator: jbangelo
 *
 * Description: The entry point to the program
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2014 jbangelo
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include <iostream>
#include <iomanip>
#include "sqlite3.h"
#include "Math/Degree.h"
#include "Time/Stardate.h"
#include "SkyObject/Planet.h"

using tsc::Math::Degree;
using tsc::Time::Stardate;
using tsc::Time::ChristianDate;
using tsc::SkyObject::Planet;
using namespace std;

int main(int argc, char** argv)
{
    Degree d1(90.0), d2(-90.0);

    cout.setf(ostream::fixed);
    cout << std::setprecision(10);

    cout << "d1 = " << d1.degStr() << endl;
    cout << "d2 = " << d2.hmsStr() << endl;
    cout << "d1 + d2 = " << (d1+d2).degStr() << endl;
    cout << "d1 - d2 = " << (d1-d2).degStr() << endl;
    cout << "d1 * d2 = " << (d1*d2).degStr() << endl;
    cout << "d1 / d2 = " << (d1/d2).degStr() << endl;

    Stardate sd1 = Stardate::fromDate(ChristianDate(2000, 1, 1, 12, 0, 0));
    Stardate sd2(2451179.5);
    Stardate sd3 = Stardate::fromDate(ChristianDate(1987, 1, 27));
    Stardate sd4(2446966.0);
    Stardate sd5 = Stardate::fromDate(ChristianDate(1988, 1, 27));
    Stardate sd6(2447332.0);
    Stardate sd7 = Stardate::fromDate(ChristianDate(1900, 1, 1));
    Stardate sd8(2305447.5);
    Stardate sd9 = Stardate::fromDate(ChristianDate(1600, 12, 31));
    Stardate sd10(2026871.8);
    Stardate sd11 = Stardate::fromDate(ChristianDate(-123, 12, 31));
    Stardate sd12(1676497.5);
    Stardate sd13 = Stardate::fromDate(ChristianDate(-1000, 7, 12, 12, 0, 0));
    Stardate sd14(1355866.5);
    Stardate sd15(1355671.4);
    Stardate sd16 = Stardate::fromDate(ChristianDate(-4712, 1, 1, 12, 0, 0));
    Stardate sd17(2436116.31);

    cout << "sd1 = " << sd1.toGregorianDateStr() << ", " << sd1.toJD() << endl;
    cout << "sd2 = " << sd2.toGregorianDateStr() << ", " << sd2.toJD() << endl;
    cout << "sd3 = " << sd3.toGregorianDateStr() << ", " << sd3.toJD() << endl;
    cout << "sd4 = " << sd4.toGregorianDateStr() << ", " << sd4.toJD() << endl;
    cout << "sd5 = " << sd5.toGregorianDateStr() << ", " << sd5.toJD() << endl;
    cout << "sd6 = " << sd6.toGregorianDateStr() << ", " << sd6.toJD() << endl;
    cout << "sd7 = " << sd7.toGregorianDateStr() << ", " << sd7.toJD() << endl;
    cout << "sd8 = " << sd8.toGregorianDateStr() << ", " << sd8.toJD() << endl;
    cout << "sd9 = " << sd9.toGregorianDateStr() << ", " << sd9.toJD() << endl;
    cout << "sd10 = " << sd10.toGregorianDateStr() << ", " << sd10.toJD() << endl;
    cout << "sd11 = " << sd11.toGregorianDateStr() << ", " << sd11.toJD() << endl;
    cout << "sd12 = " << sd12.toGregorianDateStr() << ", " << sd12.toJD() << endl;
    cout << "sd13 = " << sd13.toGregorianDateStr() << ", " << sd13.toJD() << endl;
    cout << "sd14 = " << sd14.toGregorianDateStr() << ", " << sd14.toJD() << endl;
    cout << "sd15 = " << sd15.toGregorianDateStr() << ", " << sd15.toJD() << endl;
    cout << "sd16 = " << sd16.toGregorianDateStr() << ", " << sd16.toJD() << endl;
    cout << "sd16 = " << sd17.toGregorianDateStr() << ", " << sd17.toJD() << endl;

    sqlite3* db;

    if (sqlite3_open_v2("../resources/tsc.db", &db, SQLITE_OPEN_READONLY, NULL) != SQLITE_OK)
    {
        cout << "Error opening up the database file!" << endl;
        return 1;
    }

    cout << "Loading planetary data..." << endl;
    Planet earth(Planet::EARTH, db, NULL);
    Planet mercury(Planet::MERCURY, db, &earth);
    Planet venus(Planet::VENUS, db, &earth);
    Planet mars(Planet::MARS, db, &earth);
    Planet jupiter(Planet::JUPITER, db, &earth);
    Planet saturn(Planet::SATURN, db, &earth);
    Planet uranus(Planet::URANUS, db, &earth);
    Planet neptune(Planet::NEPTUNE, db, &earth);

    Stardate mercDate(2305445.0);
    cout << "Calculateing the position of Mercury at " << mercDate.toGregorianDateStr() << endl;
    mercury.calculatePosition(mercDate);
    EclipticCoords mercCoords = mercury.getHeliocentricEclipticCoords();
    cout << "L = " << mercCoords.lambda.rad() << endl;
    cout << "B = " << mercCoords.beta.rad() << endl;
    cout << "R = " << mercCoords.delta << endl << endl;

    Stardate venusDate(2378495.0);
    cout << "Calculateing the position of Venus at " << venusDate.toGregorianDateStr() << endl;
    venus.calculatePosition(venusDate);
    EclipticCoords venusCoords = venus.getHeliocentricEclipticCoords();
    cout << "L = " << venusCoords.lambda.rad() << endl;
    cout << "B = " << venusCoords.beta.rad() << endl;
    cout << "R = " << venusCoords.delta << endl;

    Stardate earthDate(2341970.0);
    cout << "Calculateing the position of Earth at " << earthDate.toGregorianDateStr() << endl;
    earth.calculatePosition(earthDate);
    EclipticCoords earthCoords = earth.getHeliocentricEclipticCoords();
    cout << "L = " << earthCoords.lambda.rad() << endl;
    cout << "B = " << earthCoords.beta.rad() << endl;
    cout << "R = " << earthCoords.delta << endl;

    sqlite3_close(db);

    return 0;
}
<commit_msg>Started changing the main function to parse arguments<commit_after>/**
 * File: main.cpp
 * Project: tsc - TinyStarChart
 * Creator: jbangelo
 *
 * Description: The entry point to the program
 *
 * The MIT License (MIT)
 *
 * Copyright (c) 2014 jbangelo
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include <iostream>
#include <iomanip>
#include <stdlib.h>

#include "sqlite3.h"
#include "Math/Degree.h"
#include "Time/Stardate.h"
#include "SkyObject/Planet.h"

using tsc::Math::Degree;
using tsc::Time::Stardate;
using tsc::Time::ChristianDate;
using tsc::SkyObject::Planet;
using namespace std;

int main(int argc, char** argv)
{
    char *cvalue = NULL;
    int c;
    Stardate *date = NULL;
    opterr = 0;

    while ((c = getopt(argc, argv, "j:")) != -1)
    {
        switch (c)
        {
            case 'j':
                cvalue = optarg;
                date = new Stardate(::atof(cvalue));
                break;
            case '?':
                if (isprint(optopt))
                {
                    cerr << "Unkown option '-" << optopt << "'\n";
                }
                else
                {
                    cerr << "Unkown option character '0x" << optopt << "'\n";
                }
                return 1;
            default:
                return 1;
        }
    }

    if (date != NULL)
    {
        cout << "Using date " << *date << endl;
    }

    return 0;
}
<|endoftext|>


<commit_before>/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's 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.
//
//   * The name of the copyright holders may not 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 Intel Corporation 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.
//
//M*/

#include "test_precomp.hpp"

#include <string>
#include <iostream>

using namespace std;
using namespace cv;

class CV_GrabcutTest : public cvtest::BaseTest
{
public:
    CV_GrabcutTest();
    ~CV_GrabcutTest();
protected:
    bool verify(const Mat& mask, const Mat& exp);
    void run(int);
};

CV_GrabcutTest::CV_GrabcutTest() {}
CV_GrabcutTest::~CV_GrabcutTest() {}

bool CV_GrabcutTest::verify(const Mat& mask, const Mat& exp)
{
    const float maxDiffRatio = 0.005f;
    int expArea = countNonZero( exp );
    int nonIntersectArea = countNonZero( mask != exp );

    float curRatio = (float)nonIntersectArea / (float)expArea;
    ts->printf( cvtest::TS::LOG, "nonIntersectArea/expArea = %f\n", curRatio );
    return curRatio < maxDiffRatio;
}

void CV_GrabcutTest::run( int /* start_from */)
{
    cvtest::DefaultRngAuto defRng;

    Mat img = imread(string(ts->get_data_path()) + "shared/airplane.jpg");
    Mat mask_prob = imread(string(ts->get_data_path()) + "grabcut/mask_prob.png", 0);
    Mat exp_mask1 = imread(string(ts->get_data_path()) + "grabcut/exp_mask1.png", 0);
    Mat exp_mask2 = imread(string(ts->get_data_path()) + "grabcut/exp_mask2.png", 0);

    if (img.empty() || (!mask_prob.empty() && img.size() != mask_prob.size()) ||
                       (!exp_mask1.empty() && img.size() != exp_mask1.size()) ||
                       (!exp_mask2.empty() && img.size() != exp_mask2.size()) )
    {
         ts->set_failed_test_info(cvtest::TS::FAIL_MISSING_TEST_DATA);
         return;
    }

    Rect rect(Point(24, 126), Point(483, 294));
    Mat exp_bgdModel, exp_fgdModel;

    Mat mask;
    mask = Scalar(0);
    Mat bgdModel, fgdModel;
    grabCut( img, mask, rect, bgdModel, fgdModel, 0, GC_INIT_WITH_RECT );
    grabCut( img, mask, rect, bgdModel, fgdModel, 2, GC_EVAL );

    // Multiply images by 255 for more visuality of test data.
    if( mask_prob.empty() )
    {
        mask.copyTo( mask_prob );
        imwrite(string(ts->get_data_path()) + "grabcut/mask_prob.png", mask_prob);
    }
    if( exp_mask1.empty() )
    {
        exp_mask1 = (mask & 1) * 255;
        imwrite(string(ts->get_data_path()) + "grabcut/exp_mask1.png", exp_mask1);
    }

    if (!verify((mask & 1) * 255, exp_mask1))
    {
        ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
        return;
    }

    mask = mask_prob;
    bgdModel.release();
    fgdModel.release();
    rect = Rect();
    grabCut( img, mask, rect, bgdModel, fgdModel, 0, GC_INIT_WITH_MASK );
    grabCut( img, mask, rect, bgdModel, fgdModel, 1, GC_EVAL );

    if( exp_mask2.empty() )
    {
        exp_mask2 = (mask & 1) * 255;
        imwrite(string(ts->get_data_path()) + "grabcut/exp_mask2.png", exp_mask2);
    }

    if (!verify((mask & 1) * 255, exp_mask2))
    {
        ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
        return;
    }
    ts->set_failed_test_info(cvtest::TS::OK);
}

TEST(Imgproc_GrabCut, regression) { CV_GrabcutTest test; test.safe_run(); }

<commit_msg>Added regression test for #1652<commit_after>/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's 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.
//
//   * The name of the copyright holders may not 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 Intel Corporation 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.
//
//M*/

#include "test_precomp.hpp"

#include <string>
#include <iostream>

using namespace std;
using namespace cv;

class CV_GrabcutTest : public cvtest::BaseTest
{
public:
    CV_GrabcutTest();
    ~CV_GrabcutTest();
protected:
    bool verify(const Mat& mask, const Mat& exp);
    void run(int);
};

CV_GrabcutTest::CV_GrabcutTest() {}
CV_GrabcutTest::~CV_GrabcutTest() {}

bool CV_GrabcutTest::verify(const Mat& mask, const Mat& exp)
{
    const float maxDiffRatio = 0.005f;
    int expArea = countNonZero( exp );
    int nonIntersectArea = countNonZero( mask != exp );

    float curRatio = (float)nonIntersectArea / (float)expArea;
    ts->printf( cvtest::TS::LOG, "nonIntersectArea/expArea = %f\n", curRatio );
    return curRatio < maxDiffRatio;
}

void CV_GrabcutTest::run( int /* start_from */)
{
    cvtest::DefaultRngAuto defRng;

    Mat img = imread(string(ts->get_data_path()) + "shared/airplane.jpg");
    Mat mask_prob = imread(string(ts->get_data_path()) + "grabcut/mask_prob.png", 0);
    Mat exp_mask1 = imread(string(ts->get_data_path()) + "grabcut/exp_mask1.png", 0);
    Mat exp_mask2 = imread(string(ts->get_data_path()) + "grabcut/exp_mask2.png", 0);

    if (img.empty() || (!mask_prob.empty() && img.size() != mask_prob.size()) ||
                       (!exp_mask1.empty() && img.size() != exp_mask1.size()) ||
                       (!exp_mask2.empty() && img.size() != exp_mask2.size()) )
    {
         ts->set_failed_test_info(cvtest::TS::FAIL_MISSING_TEST_DATA);
         return;
    }

    Rect rect(Point(24, 126), Point(483, 294));
    Mat exp_bgdModel, exp_fgdModel;

    Mat mask;
    mask = Scalar(0);
    Mat bgdModel, fgdModel;
    grabCut( img, mask, rect, bgdModel, fgdModel, 0, GC_INIT_WITH_RECT );
    grabCut( img, mask, rect, bgdModel, fgdModel, 2, GC_EVAL );

    // Multiply images by 255 for more visuality of test data.
    if( mask_prob.empty() )
    {
        mask.copyTo( mask_prob );
        imwrite(string(ts->get_data_path()) + "grabcut/mask_prob.png", mask_prob);
    }
    if( exp_mask1.empty() )
    {
        exp_mask1 = (mask & 1) * 255;
        imwrite(string(ts->get_data_path()) + "grabcut/exp_mask1.png", exp_mask1);
    }

    if (!verify((mask & 1) * 255, exp_mask1))
    {
        ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
        return;
    }

    mask = mask_prob;
    bgdModel.release();
    fgdModel.release();
    rect = Rect();
    grabCut( img, mask, rect, bgdModel, fgdModel, 0, GC_INIT_WITH_MASK );
    grabCut( img, mask, rect, bgdModel, fgdModel, 1, GC_EVAL );

    if( exp_mask2.empty() )
    {
        exp_mask2 = (mask & 1) * 255;
        imwrite(string(ts->get_data_path()) + "grabcut/exp_mask2.png", exp_mask2);
    }

    if (!verify((mask & 1) * 255, exp_mask2))
    {
        ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
        return;
    }
    ts->set_failed_test_info(cvtest::TS::OK);
}

TEST(Imgproc_GrabCut, regression) { CV_GrabcutTest test; test.safe_run(); }

TEST(Imgproc_GrabCut, repeatability)
{
    cvtest::TS& ts = *cvtest::TS::ptr();

    Mat image_1 = imread(ts.get_data_path() + "grabcut/image1652.ppm", CV_LOAD_IMAGE_COLOR);
    Mat mask_1 = imread(ts.get_data_path() + "grabcut/mask1652.ppm", CV_LOAD_IMAGE_GRAYSCALE);
    Rect roi_1(0, 0, 150, 150);

    Mat image_2 = image_1.clone();
    Mat mask_2 = mask_1.clone();
    Rect roi_2 = roi_1;

    Mat image_3 = image_1.clone();
    Mat mask_3 = mask_1.clone();
    Rect roi_3 = roi_1;

    Mat bgdModel_1, fgdModel_1;
    Mat bgdModel_2, fgdModel_2;
    Mat bgdModel_3, fgdModel_3;

    theRNG().state = 12378213;
    grabCut(image_1, mask_1, roi_1, bgdModel_1, fgdModel_1, 1, GC_INIT_WITH_MASK);
    theRNG().state = 12378213;
    grabCut(image_2, mask_2, roi_2, bgdModel_2, fgdModel_2, 1, GC_INIT_WITH_MASK);
    theRNG().state = 12378213;
    grabCut(image_3, mask_3, roi_3, bgdModel_3, fgdModel_3, 1, GC_INIT_WITH_MASK);

    EXPECT_EQ(0, countNonZero(mask_1 != mask_2));
    EXPECT_EQ(0, countNonZero(mask_1 != mask_3));
    EXPECT_EQ(0, countNonZero(mask_2 != mask_3));
}
<|endoftext|>


<commit_before>
/*************** <auto-copyright.pl BEGIN do not edit this line> **************
 *
 * VR Juggler is (C) Copyright 1998, 1999, 2000, 2001 by Iowa State University
 *
 * Original Authors:
 *   Allen Bierbaum, Christopher Just,
 *   Patrick Hartling, Kevin Meinert,
 *   Carolina Cruz-Neira, Albert Baker
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, 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
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 *
 *************** <auto-copyright.pl END do not edit this line> ***************/

/****************** <SNX heading BEGIN do not edit this line> *****************
 *
 * Juggler Juggler
 *
 * Original Authors:
 *   Kevin Meinert, Carolina Cruz-Neira
 *
 * -----------------------------------------------------------------
 * File:          $RCSfile$
 * Date modified: $Date$
 * Version:       $Revision$
 * -----------------------------------------------------------------
 *
 ****************** <SNX heading END do not edit this line> ******************/

// Kevin Meinert
// simple glut skeleton application: look for the !!!TODO!!!s,
//  and fill in your code
//                          there as needed
//

#ifdef WIN32
   #include <windows.h>  // make the app win32 friendly. :)
#endif
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>

#include <snx/sonix.h>    // interface
#include <snx/SoundHandle.h>
#include "StopWatch.h"

#include <iostream.h>
#include <stdlib.h>

// a place to store application data...
class AppWindow
{
public:
   static int width, height;
   static int mainWin_contextID;
};
int AppWindow::width = 0, AppWindow::height = 0;
int AppWindow::mainWin_contextID = -1;

StopWatch stopWatch;

int soundpos = 0;
float pitchbend = 1.0f;
float cutoff = 0.2f;
float volume = 1.0f;

// our sound object...
snx::SoundHandle kevinSound;

void drawGrid()
{
   glBegin( GL_LINES );
      for ( int x = -1000; x < 1000; ++x)
      {
         glVertex3f( -1000, 0, x );
         glVertex3f(  1000, 0, x );
         glVertex3f( x, 0, -1000 );
         glVertex3f( x, 0,  1000 );
      }
   glEnd();
}

//////////////////////////////////////////////////
// This is called on a glutPostRedisplay
//////////////////////////////////////////////////
static void OnRedisplay()
{
   glClearColor( 0.0f, 0.0f, 0.0f, 1.0f );
   glClear( GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT );
   glEnable( GL_DEPTH_TEST );
   glEnable( GL_BLEND );
   glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );

   // set up the projection matrix
   glMatrixMode( GL_PROJECTION );
      glLoadIdentity();
      gluPerspective( 80.0f, AppWindow::width / AppWindow::height, 0.01f, 1000.0f );

   // initialize your matrix stack used for transforming your models
    glMatrixMode( GL_MODELVIEW );
      glLoadIdentity();

   // !!!TODO!!!: replace the following with your own opengl commands!
   glTranslatef( 0, -10, 0 );
   drawGrid();
   // !!!TODO!!!: ////////////////////////////////////////

   // swaps the front and back frame buffers.
   // hint: you've been drawing on the back, offscreen, buffer.
   // This command then brings that framebuffer onscreen.
   glutSwapBuffers();
}

//////////////////////////////////////////////////
// This is called repeatedly, as fast as possible
//////////////////////////////////////////////////
#include "unistd.h"
static void OnIdle()
{
   // According to the GLUT specification, the current window is
   // undefined during an idle callback.  So we need to explicitly change
   // it if necessary
   if ( glutGetWindow() != AppWindow::mainWin_contextID )
      glutSetWindow( AppWindow::mainWin_contextID );

   // tell glut to call redisplay (which then calls OnRedisplay)
   glutPostRedisplay();

   sonix::instance()->step( stopWatch.timeInstant() );
   stopWatch.pulse();
}

/////////////////////////////////////////////
// This is called on a Resize of the glut window
/////////////////////////////////////////////
static void OnReshape( int width, int height )
{
   // save these params in case your app needs them
   AppWindow::width = width;
   AppWindow::height = height;

   // set your viewport to the extents of the window
   glViewport( 0, 0, width, height );

   // let the app run idle, while resizing,
   // glut does not do this for us automatically, so call OnIdle explicitly.
   OnIdle();
}

////////////////////////////////
// This is called on a Down Keypress
////////////////////////////////
static void OnKeyboardDown( unsigned char k, int x, int y )
{
   switch (k)
   {
   // If user pressed 'q' or 'ESC', then exit the app.
   // this is really ungraceful, but necessary since GLUT does a while(1)
   // as it's control loop.  There is no GLUT method to exit, unfortunately.
   case 'q':
   case 27:
      exit( 0 );
      break;
   case '1':
   {
      sonix::instance()->changeAPI( "Stub" );
      snx::SoundInfo si;
      si.filename = "../../../data/sol.wav";
      si.ambient = false;
      si.datasource = snx::SoundInfo::FILESYSTEM;
      kevinSound.configure( si );

      sonix::instance()->changeAPI( "OpenAL" );
   }
   break;
   case '2':
   {
      sonix::instance()->changeAPI( "Stub" );
      snx::SoundInfo si;
      si.filename = "../../../data/sep.wav";
      si.ambient = false;
      si.datasource = snx::SoundInfo::FILESYSTEM;
      kevinSound.configure( si );

      sonix::instance()->changeAPI( "AudioWorks" );
   }
   break;
      case '3':
   {
      sonix::instance()->changeAPI( "Stub" );
   }
   break;
      case 'a':
   {
      snx::SoundInfo si;
      si.ambient = false;
      si.filename = "../../../data/sol.wav";
      si.datasource = snx::SoundInfo::FILESYSTEM;
      kevinSound.configure( si );
      std::cout<<"positional"<<std::endl;
   }
   break;
      case 'b':
   {
      snx::SoundInfo si;
      si.ambient = true;
      si.filename = "../../../data/sep.wav";
      si.datasource = snx::SoundInfo::FILESYSTEM;
      kevinSound.configure( si );
      kevinSound.setAmbient( true );
      std::cout<<"ambient"<<std::endl;
   }
   break;
      case 'c':
   {
      snx::SoundInfo si;
      si.ambient = false;
      si.filename = "../../../data/suck.wav";
      si.datasource = snx::SoundInfo::FILESYSTEM;
      kevinSound.configure( si );
      std::cout<<"positional"<<std::endl;
   }
   break;
   case 't':
   {
      kevinSound.trigger();
   }
   break;
   case 'p':
   {
      kevinSound.pause();
   }
   break;
   case 's':
   {
      kevinSound.stop();
   }
   break;

   case ',':
   {
      soundpos -= 1;
      kevinSound.setPosition( soundpos, 0, 0 );
      std::cout<<"soundpos: "<<soundpos<<std::endl;
   }
   break;

   case '.':
   {
      soundpos += 1;
      kevinSound.setPosition( soundpos, 0, 0 );
      std::cout<<"soundpos: "<<soundpos<<std::endl;
   }
   break;

   case '[':
   {
      pitchbend -= 0.1;
      kevinSound.setPitchBend( pitchbend );
      std::cout<<"pitchbend: "<<pitchbend<<std::endl;
   }
   break;

   case ']':
   {
      pitchbend += 0.1;
      kevinSound.setPitchBend( pitchbend );
      std::cout<<"pitchbend: "<<pitchbend<<std::endl;
   }
   break;

   case ';':
   {
      cutoff -= 0.01;
      kevinSound.setCutoff( cutoff );
      std::cout<<"Cutoff: "<<cutoff<<std::endl;
   }
   break;

   case '\'':
   {
      cutoff += 0.01;
      kevinSound.setCutoff( cutoff );
      std::cout<<"Cutoff: "<<cutoff<<std::endl;
   }
   break;

   case '-':
   {
      volume -= 0.1;
      kevinSound.setVolume( volume );
      std::cout<<"volume: "<<volume<<std::endl;
   }
   break;

   case '=':
   {
      volume += 0.1;
      kevinSound.setVolume( volume );
      std::cout<<"volume: "<<volume<<std::endl;
   }
   break;

   default:
      // do nothing if no key is pressed
      break;
   }
}
////////////////////////////////
// This is called on a Up Keypress
////////////////////////////////
static void OnKeyboardUp( unsigned char k, int x, int y )
{
   switch (k)
   {
   case 'a':
      // !!!TODO!!!: add handler for when UP is released
      break;

   case 'z':
      // !!!TODO!!!: add handler for when DOWN is released
      break;

   default:
      // do nothing if no key is pressed
      break;
   }
}

////////////////////////////////
// This is called on a Down Keypress
// of a "special" key such as the grey arrows.
////////////////////////////////
static void OnSpecialKeyboardDown(int k, int x, int y)
{
   switch (k)
   {
   case GLUT_KEY_UP:
      // !!!TODO!!!: add handler for when UP is pressed
      break;
   case GLUT_KEY_DOWN:
      // !!!TODO!!!: add handler for when DOWN is pressed
      break;
   default:
      // do nothing if no special key pressed
      break;
   }
}

////////////////////////////////
// This is called on a Up Keypress
////////////////////////////////
static void OnSpecialKeyboardUp( int k, int x, int y )
{
   switch (k)
   {
   case GLUT_KEY_UP:
      // !!!TODO!!!: add handler for when UP is released
      break;
   case GLUT_KEY_DOWN:
      // !!!TODO!!!: add handler for when DOWN is released
      break;
   default:
      // do nothing if no special key pressed
      break;
   }
}

////////////////////////////////
// This is called when mouse changes position
// x and y are the screen position
// in your 2D window's coordinate frame
////////////////////////////////
static void OnMousePos( int x, int y )
{
   // !!!TODO!!!: do something based on mouse position
}

////////////////////////////////
// This is called when mouse clicks
////////////////////////////////
static void OnMouseClick( int a, int b, int c, int d )
{
   // !!!TODO!!!: Need mouse interaction?
   //             read the glut docs/manpage to find out how to query
   //             which button was pressed...
   //             you may have to get this from the glut website
   //             (use www.google.com to search for it)
}

// Initialize the application
// initialize the state of your app here if needed...
static void OnApplicationInit()
{
   // Don't put open GL code here, this func may be called at anytime
   // even before the API is initialized
   // (like before a graphics context is obtained)

   kevinSound.init( "kevin" );
   stopWatch.pulse();
}

int main(int argc, char* argv[])
{
    // Initialize the application
    // initialize the state of your app here if needed...
    OnApplicationInit();

    // Set the window's initial size
    ::glutInitWindowSize( 640, 480 );
    ::glutInit( &argc, argv );

    // Set to double buffer to reduce flicker
    ::glutInitDisplayMode( GLUT_RGBA | GLUT_DEPTH | GLUT_DOUBLE );

    // Set the window title
    AppWindow::mainWin_contextID = ::glutCreateWindow( "GLUT application" );

    cout<<"\n"<<flush;
    cout<<"sonix sample OpenGL+sound app - by kevin - kevn@vrjuggler.org\n"<<flush;
    cout<<"       usage:  t - trigger\n"<<flush;
    cout<<"               p - pause\n"<<flush;
    cout<<"               s - stop\n"<<flush;
    cout<<"               , - put 3d sound source to the left\n"<<flush;
    cout<<"               . - put 3d sound source to the right\n"<<flush;
    cout<<"               1 - Change subsystem to OpenAL\n"<<flush;
    cout<<"               2 - Change subsystem to AudioWorks\n"<<flush;
    cout<<"               3 - Change subsystem to None\n"<<flush;
    cout<<"               a - configure sound object to sample.wav (OpenAL only)\n"<<flush;
    cout<<"               b - configure sound object to drumsolo.wav (OpenAL only)\n"<<flush;
    cout<<"\n"<<flush;

   // display callbacks.
   ::glutReshapeFunc( OnReshape );
   ::glutIdleFunc( OnIdle );
   ::glutDisplayFunc( OnRedisplay );

   // tell glut to not call the keyboard callback repeatedly
   // when holding down a key. (uses edge triggering, like the mouse)
   ::glutIgnoreKeyRepeat( 1 );

   // keyboard callback functions.
   ::glutKeyboardFunc( OnKeyboardDown );
   ::glutKeyboardUpFunc( OnKeyboardUp );
   ::glutSpecialFunc( OnSpecialKeyboardDown );
   ::glutSpecialUpFunc( OnSpecialKeyboardUp );

   // mouse callback functions...
   ::glutMouseFunc( OnMouseClick );
   ::glutMotionFunc( OnMousePos );
   ::glutPassiveMotionFunc( OnMousePos );

    // start the application loop, your callbacks will now be called
    // time for glut to sit and spin.
    ::glutMainLoop();

    return 1;
}

<commit_msg>look for wav files in current dir.  this means you'll have to ln -s them here ... :(<commit_after>
/*************** <auto-copyright.pl BEGIN do not edit this line> **************
 *
 * VR Juggler is (C) Copyright 1998, 1999, 2000, 2001 by Iowa State University
 *
 * Original Authors:
 *   Allen Bierbaum, Christopher Just,
 *   Patrick Hartling, Kevin Meinert,
 *   Carolina Cruz-Neira, Albert Baker
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, 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
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 *
 *************** <auto-copyright.pl END do not edit this line> ***************/

/****************** <SNX heading BEGIN do not edit this line> *****************
 *
 * Juggler Juggler
 *
 * Original Authors:
 *   Kevin Meinert, Carolina Cruz-Neira
 *
 * -----------------------------------------------------------------
 * File:          $RCSfile$
 * Date modified: $Date$
 * Version:       $Revision$
 * -----------------------------------------------------------------
 *
 ****************** <SNX heading END do not edit this line> ******************/

// Kevin Meinert
// simple glut skeleton application: look for the !!!TODO!!!s,
//  and fill in your code
//                          there as needed
//

#ifdef WIN32
   #include <windows.h>  // make the app win32 friendly. :)
#endif
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>

#include <snx/sonix.h>    // interface
#include <snx/SoundHandle.h>
#include "StopWatch.h"

#include <iostream.h>
#include <stdlib.h>

// a place to store application data...
class AppWindow
{
public:
   static int width, height;
   static int mainWin_contextID;
};
int AppWindow::width = 0, AppWindow::height = 0;
int AppWindow::mainWin_contextID = -1;

StopWatch stopWatch;

int soundpos = 0;
float pitchbend = 1.0f;
float cutoff = 0.2f;
float volume = 1.0f;

// our sound object...
snx::SoundHandle kevinSound;

void drawGrid()
{
   glBegin( GL_LINES );
      for ( int x = -1000; x < 1000; ++x)
      {
         glVertex3f( -1000, 0, x );
         glVertex3f(  1000, 0, x );
         glVertex3f( x, 0, -1000 );
         glVertex3f( x, 0,  1000 );
      }
   glEnd();
}

//////////////////////////////////////////////////
// This is called on a glutPostRedisplay
//////////////////////////////////////////////////
static void OnRedisplay()
{
   glClearColor( 0.0f, 0.0f, 0.0f, 1.0f );
   glClear( GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT );
   glEnable( GL_DEPTH_TEST );
   glEnable( GL_BLEND );
   glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );

   // set up the projection matrix
   glMatrixMode( GL_PROJECTION );
      glLoadIdentity();
      gluPerspective( 80.0f, AppWindow::width / AppWindow::height, 0.01f, 1000.0f );

   // initialize your matrix stack used for transforming your models
    glMatrixMode( GL_MODELVIEW );
      glLoadIdentity();

   // !!!TODO!!!: replace the following with your own opengl commands!
   glTranslatef( 0, -10, 0 );
   drawGrid();
   // !!!TODO!!!: ////////////////////////////////////////

   // swaps the front and back frame buffers.
   // hint: you've been drawing on the back, offscreen, buffer.
   // This command then brings that framebuffer onscreen.
   glutSwapBuffers();
}

//////////////////////////////////////////////////
// This is called repeatedly, as fast as possible
//////////////////////////////////////////////////
#include "unistd.h"
static void OnIdle()
{
   // According to the GLUT specification, the current window is
   // undefined during an idle callback.  So we need to explicitly change
   // it if necessary
   if ( glutGetWindow() != AppWindow::mainWin_contextID )
      glutSetWindow( AppWindow::mainWin_contextID );

   // tell glut to call redisplay (which then calls OnRedisplay)
   glutPostRedisplay();

   sonix::instance()->step( stopWatch.timeInstant() );
   stopWatch.pulse();
}

/////////////////////////////////////////////
// This is called on a Resize of the glut window
/////////////////////////////////////////////
static void OnReshape( int width, int height )
{
   // save these params in case your app needs them
   AppWindow::width = width;
   AppWindow::height = height;

   // set your viewport to the extents of the window
   glViewport( 0, 0, width, height );

   // let the app run idle, while resizing,
   // glut does not do this for us automatically, so call OnIdle explicitly.
   OnIdle();
}

////////////////////////////////
// This is called on a Down Keypress
////////////////////////////////
static void OnKeyboardDown( unsigned char k, int x, int y )
{
   switch (k)
   {
   // If user pressed 'q' or 'ESC', then exit the app.
   // this is really ungraceful, but necessary since GLUT does a while(1)
   // as it's control loop.  There is no GLUT method to exit, unfortunately.
   case 'q':
   case 27:
      exit( 0 );
      break;
   case '1':
   {
      sonix::instance()->changeAPI( "Stub" );
      snx::SoundInfo si;
      si.filename = "sol.wav";
      si.ambient = false;
      si.datasource = snx::SoundInfo::FILESYSTEM;
      kevinSound.configure( si );

      sonix::instance()->changeAPI( "OpenAL" );
   }
   break;
   case '2':
   {
      sonix::instance()->changeAPI( "Stub" );
      snx::SoundInfo si;
      si.filename = "sep.wav";
      si.ambient = false;
      si.datasource = snx::SoundInfo::FILESYSTEM;
      kevinSound.configure( si );

      sonix::instance()->changeAPI( "AudioWorks" );
   }
   break;
      case '3':
   {
      sonix::instance()->changeAPI( "Stub" );
   }
   break;
      case 'a':
   {
      snx::SoundInfo si;
      si.ambient = false;
      si.filename = "sol.wav";
      si.datasource = snx::SoundInfo::FILESYSTEM;
      kevinSound.configure( si );
      std::cout<<"positional"<<std::endl;
   }
   break;
      case 'b':
   {
      snx::SoundInfo si;
      si.ambient = true;
      si.filename = "sep.wav";
      si.datasource = snx::SoundInfo::FILESYSTEM;
      kevinSound.configure( si );
      kevinSound.setAmbient( true );
      std::cout<<"ambient"<<std::endl;
   }
   break;
      case 'c':
   {
      snx::SoundInfo si;
      si.ambient = false;
      si.filename = "suck.wav";
      si.datasource = snx::SoundInfo::FILESYSTEM;
      kevinSound.configure( si );
      std::cout<<"positional"<<std::endl;
   }
   break;
   case 't':
   {
      kevinSound.trigger();
   }
   break;
   case 'p':
   {
      kevinSound.pause();
   }
   break;
   case 's':
   {
      kevinSound.stop();
   }
   break;

   case ',':
   {
      soundpos -= 1;
      kevinSound.setPosition( soundpos, 0, 0 );
      std::cout<<"soundpos: "<<soundpos<<std::endl;
   }
   break;

   case '.':
   {
      soundpos += 1;
      kevinSound.setPosition( soundpos, 0, 0 );
      std::cout<<"soundpos: "<<soundpos<<std::endl;
   }
   break;

   case '[':
   {
      pitchbend -= 0.1;
      kevinSound.setPitchBend( pitchbend );
      std::cout<<"pitchbend: "<<pitchbend<<std::endl;
   }
   break;

   case ']':
   {
      pitchbend += 0.1;
      kevinSound.setPitchBend( pitchbend );
      std::cout<<"pitchbend: "<<pitchbend<<std::endl;
   }
   break;

   case ';':
   {
      cutoff -= 0.01;
      kevinSound.setCutoff( cutoff );
      std::cout<<"Cutoff: "<<cutoff<<std::endl;
   }
   break;

   case '\'':
   {
      cutoff += 0.01;
      kevinSound.setCutoff( cutoff );
      std::cout<<"Cutoff: "<<cutoff<<std::endl;
   }
   break;

   case '-':
   {
      volume -= 0.1;
      kevinSound.setVolume( volume );
      std::cout<<"volume: "<<volume<<std::endl;
   }
   break;

   case '=':
   {
      volume += 0.1;
      kevinSound.setVolume( volume );
      std::cout<<"volume: "<<volume<<std::endl;
   }
   break;

   default:
      // do nothing if no key is pressed
      break;
   }
}
////////////////////////////////
// This is called on a Up Keypress
////////////////////////////////
static void OnKeyboardUp( unsigned char k, int x, int y )
{
   switch (k)
   {
   case 'a':
      // !!!TODO!!!: add handler for when UP is released
      break;

   case 'z':
      // !!!TODO!!!: add handler for when DOWN is released
      break;

   default:
      // do nothing if no key is pressed
      break;
   }
}

////////////////////////////////
// This is called on a Down Keypress
// of a "special" key such as the grey arrows.
////////////////////////////////
static void OnSpecialKeyboardDown(int k, int x, int y)
{
   switch (k)
   {
   case GLUT_KEY_UP:
      // !!!TODO!!!: add handler for when UP is pressed
      break;
   case GLUT_KEY_DOWN:
      // !!!TODO!!!: add handler for when DOWN is pressed
      break;
   default:
      // do nothing if no special key pressed
      break;
   }
}

////////////////////////////////
// This is called on a Up Keypress
////////////////////////////////
static void OnSpecialKeyboardUp( int k, int x, int y )
{
   switch (k)
   {
   case GLUT_KEY_UP:
      // !!!TODO!!!: add handler for when UP is released
      break;
   case GLUT_KEY_DOWN:
      // !!!TODO!!!: add handler for when DOWN is released
      break;
   default:
      // do nothing if no special key pressed
      break;
   }
}

////////////////////////////////
// This is called when mouse changes position
// x and y are the screen position
// in your 2D window's coordinate frame
////////////////////////////////
static void OnMousePos( int x, int y )
{
   // !!!TODO!!!: do something based on mouse position
}

////////////////////////////////
// This is called when mouse clicks
////////////////////////////////
static void OnMouseClick( int a, int b, int c, int d )
{
   // !!!TODO!!!: Need mouse interaction?
   //             read the glut docs/manpage to find out how to query
   //             which button was pressed...
   //             you may have to get this from the glut website
   //             (use www.google.com to search for it)
}

// Initialize the application
// initialize the state of your app here if needed...
static void OnApplicationInit()
{
   // Don't put open GL code here, this func may be called at anytime
   // even before the API is initialized
   // (like before a graphics context is obtained)

   kevinSound.init( "kevin" );
   stopWatch.pulse();
}

int main(int argc, char* argv[])
{
    // Initialize the application
    // initialize the state of your app here if needed...
    OnApplicationInit();

    // Set the window's initial size
    ::glutInitWindowSize( 640, 480 );
    ::glutInit( &argc, argv );

    // Set to double buffer to reduce flicker
    ::glutInitDisplayMode( GLUT_RGBA | GLUT_DEPTH | GLUT_DOUBLE );

    // Set the window title
    AppWindow::mainWin_contextID = ::glutCreateWindow( "GLUT application" );

    cout<<"\n"<<flush;
    cout<<"sonix sample OpenGL+sound app - by kevin - kevn@vrjuggler.org\n"<<flush;
    cout<<"       usage:  t - trigger\n"<<flush;
    cout<<"               p - pause\n"<<flush;
    cout<<"               s - stop\n"<<flush;
    cout<<"               , - put 3d sound source to the left\n"<<flush;
    cout<<"               . - put 3d sound source to the right\n"<<flush;
    cout<<"               1 - Change subsystem to OpenAL\n"<<flush;
    cout<<"               2 - Change subsystem to AudioWorks\n"<<flush;
    cout<<"               3 - Change subsystem to None\n"<<flush;
    cout<<"               a - configure sound object to sample.wav (OpenAL only)\n"<<flush;
    cout<<"               b - configure sound object to drumsolo.wav (OpenAL only)\n"<<flush;
    cout<<"\n"<<flush;

   // display callbacks.
   ::glutReshapeFunc( OnReshape );
   ::glutIdleFunc( OnIdle );
   ::glutDisplayFunc( OnRedisplay );

   // tell glut to not call the keyboard callback repeatedly
   // when holding down a key. (uses edge triggering, like the mouse)
   ::glutIgnoreKeyRepeat( 1 );

   // keyboard callback functions.
   ::glutKeyboardFunc( OnKeyboardDown );
   ::glutKeyboardUpFunc( OnKeyboardUp );
   ::glutSpecialFunc( OnSpecialKeyboardDown );
   ::glutSpecialUpFunc( OnSpecialKeyboardUp );

   // mouse callback functions...
   ::glutMouseFunc( OnMouseClick );
   ::glutMotionFunc( OnMousePos );
   ::glutPassiveMotionFunc( OnMousePos );

    // start the application loop, your callbacks will now be called
    // time for glut to sit and spin.
    ::glutMainLoop();

    return 1;
}

<|endoftext|>


<commit_before>// This file is part of the dune-stuff project:
//   https://github.com/wwu-numerik/dune-stuff
// Copyright holders: Rene Milk, Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_STUFF_FEMEOC_HH
#define DUNE_STUFF_FEMEOC_HH

#if HAVE_DUNE_FEM

#include <cassert>
#include <sstream>
#include <fstream>
#include <vector>
#include <limits>

#include <boost/format.hpp>
#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/fvector.hh>

#include <dune/fem/io/file/iointerface.hh>

namespace Dune {
namespace Stuff {
namespace Fem {
/**
   *  @ingroup HelperClasses
   *  \brief Write a self contained tex table
   *  for eoc runs with timing information.
   *
   *  Constructor takes base name (filename) of file and
   *  generates two files:
   *  filename.tex and filename_body.tex.
   *  The file filename_body.tex hold the actual body
   *  of the eoc table which is included in filename.tex
   *  but can also be used to combine e.g. runs with different
   *  parameters or for plotting using gnuplot.
   *
   *  The class is singleton and thus new errors for eoc
   *  computations can be added in any part of the program.
   *  To add a new entry for eoc computations use one of the
   *  addEntry methods. These return a unique unsinged int
   *  which can be used to add error values to the table
   *  with the setErrors methods.
   *  The method write is used to write a single line
   *  to the eoc table.
   *  \note copy/paste from fem with certain adjustments
   */
class FemEoc
{
  std::ofstream outputFile_;
  int level_;
  std::vector< double > prevError_;
  std::vector< double > error_;
  std::vector< std::string > description_;
  double prevh_;
  bool initial_;
  std::vector< int > pos_;

  FemEoc();
  void init(const std::string& path,
            const std::string& name, const std::string& descript, const std::string& inputFile);
  void init(const std::string& name, const std::string& descript, const std::string& inputFile);

  template< class VectorType >
  void seterrors(size_t id, const VectorType& err, size_t size) {
    int pos = pos_[id];

    for (size_t i = 0; i < size; ++i)
      error_[pos + i] = err[i];
  }

  template< int SIZE >
  void seterrors(size_t id, const Dune::FieldVector< double, SIZE >& err) {
    int pos = pos_[id];

    for (int i = 0; i < SIZE; ++i)
      error_[pos + i] = err[i];
  }

  void seterrors(size_t id, const double& err);
  void writeerr(double h, double size, double time, int counter);

  template< class Writer >
  void writeerr(Writer& writer, bool last) {
    if (initial_)
    {
      writer.putHeader(outputFile_);
    }

    writer.putStaticCols(outputFile_);

    for (size_t i = 0; i < 2; ++i)
    {
      writer.putErrorCol(outputFile_, prevError_[i], error_[i], prevh_, initial_);
      prevError_[i] = error_[i];
      error_[i] = -1;    // uninitialized
    }

    writer.putLineEnd(outputFile_);

    if (last)
      writer.endTable(outputFile_);

    prevh_ = writer.get_h();
    level_++;
    initial_ = false;
  } // writeerr

  template< class StrVectorType >
  size_t addentry(const StrVectorType& descript, size_t size) {
    if (!initial_)
      DUNE_THROW(Dune::InvalidStateException, "");
    assert(error_.size() < std::numeric_limits< int >::max());
    pos_.push_back( boost::numeric_convertion< int >(error_.size()) );
    for (size_t i = 0; i < size; ++i)
    {
      error_.push_back(0);
      prevError_.push_back(0);
      description_.push_back(descript[i]);
    }
    return pos_.size() - 1;
  } // addentry

  size_t addentry(const std::string& descript);

public:
  ~FemEoc();

  static FemEoc& instance() {
    static FemEoc instance_;
    return instance_;
  }

  //! open file path/name and write a description string into tex file
  static void initialize(const std::string& path,
                         const std::string& name,
                         const std::string& descript,
                         const std::string& templateFilename) {
    instance().init(path, name, descript, templateFilename);
  }

  //! open file name and write description string into tex file
  static void initialize(const std::string& name,
                         const std::string& descript,
                         const std::string& templateFilename) {
    instance().init(name, descript, templateFilename);
  }

  /** \brief add a vector of new eoc values
     *
     *  \tparam  StrVectorType a vector type with operator[]
     *           returning a string (a C style array can be used)
     *           the size of the vector is given as parameter
     *  \return  a unique index used to add the error values
     */
  template< class StrVectorType >
  static size_t addEntry(const StrVectorType& descript, size_t size) {
    return instance().addentry(descript, size);
  }

  /** \brief add a vector of new eoc values
     *
     *  \tparam  StrVectorType a vector type with size() and operator[]
     *           returning a string
     *  \return  a unique index used to add the error values
     */

  template< class StrVectorType >
  static size_t addEntry(const StrVectorType& descript) {
    return instance().addentry( descript, descript.size() );
  }

  /** \brief add a single new eoc output
     *
     *  \return  a unique index used to add the error values
     */
  static size_t addEntry(const std::string& descript) {
    return instance().addentry(descript);
  }

  /** \brief add a single new eoc output
     *
     *  \return  a unique index used to add the error values
     */
  static size_t addEntry(const char* descript) {
    return addEntry( std::string(descript) );
  }

  /** \brief add a vector of error values for the given id (returned by
     *         addEntry)
     *  \tparam  VectorType a vector type with an operator[]
     *           returning a double (C style array can be used)
     */
  template< class VectorType >
  static void setErrors(size_t id, const VectorType& err, int size) {
    instance().seterrors(id, err, size);
  }

  /** \brief add a vector of error values for the given id (returned bywrite(GridWidth::calcGridWidth(gridPart),
     *                 grid.size(0),runTime,0);
     *
     *         addEntry)
     *  \tparam  VectorType a vector type with a size() and an operator[]
     *           returning a double
     */
  template< class VectorType >
  static void setErrors(size_t id, const VectorType& err) {
    instance().seterrors( id, err, err.size() );
  }

  /** \brief add a vector in a FieldVector of error values for the given id (returned by
     *         addEntry)
     */
  template< int SIZE >
  static void setErrors(size_t id, const Dune::FieldVector< double, SIZE >& err) {
    instance().seterrors(id, err);
  }

  /** \brief add a single error value for the given id (returned by
     *         addEntry)
     */
  static void setErrors(size_t id, const double& err) {
    instance().seterrors(id, err);
  }

  /** \brief commit a line to the eoc file
     */
  static void write(double h, double size, double time, int counter) {
    instance().writeerr(h, size, time, counter);
  }

  template< class Writer >
  static void write(Writer& writer, bool last = false) {
    instance().writeerr(writer, last);
  }
};

} // namespace Stuff
} // namespace Fem
} // namespace Dune

#endif // HAVE_DUNE_FEM

#endif // ifndef DUNE_STUFF_FEMEOC_HH
<commit_msg>[femfemeoc] remove useless code<commit_after>// This file is part of the dune-stuff project:
//   https://github.com/wwu-numerik/dune-stuff
// Copyright holders: Rene Milk, Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_STUFF_FEMEOC_HH
#define DUNE_STUFF_FEMEOC_HH

#if HAVE_DUNE_FEM

#include <cassert>
#include <sstream>
#include <fstream>
#include <vector>
#include <limits>

#include <boost/format.hpp>
#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/fvector.hh>

#include <dune/fem/io/file/iointerface.hh>

namespace Dune {
namespace Stuff {
namespace Fem {
/**
   *  @ingroup HelperClasses
   *  \brief Write a self contained tex table
   *  for eoc runs with timing information.
   *
   *  Constructor takes base name (filename) of file and
   *  generates two files:
   *  filename.tex and filename_body.tex.
   *  The file filename_body.tex hold the actual body
   *  of the eoc table which is included in filename.tex
   *  but can also be used to combine e.g. runs with different
   *  parameters or for plotting using gnuplot.
   *
   *  The class is singleton and thus new errors for eoc
   *  computations can be added in any part of the program.
   *  To add a new entry for eoc computations use one of the
   *  addEntry methods. These return a unique unsinged int
   *  which can be used to add error values to the table
   *  with the setErrors methods.
   *  The method write is used to write a single line
   *  to the eoc table.
   *  \note copy/paste from fem with certain adjustments
   */
class FemEoc
{
  std::ofstream outputFile_;
  int level_;
  std::vector< double > prevError_;
  std::vector< double > error_;
  std::vector< std::string > description_;
  double prevh_;
  bool initial_;
  std::vector< int > pos_;

  FemEoc();
  void init(const std::string& path,
            const std::string& name, const std::string& descript, const std::string& inputFile);
  void init(const std::string& name, const std::string& descript, const std::string& inputFile);

  template< class VectorType >
  void seterrors(size_t id, const VectorType& err, size_t size) {
    int pos = pos_[id];

    for (size_t i = 0; i < size; ++i)
      error_[pos + i] = err[i];
  }

  template< int SIZE >
  void seterrors(size_t id, const Dune::FieldVector< double, SIZE >& err) {
    int pos = pos_[id];

    for (int i = 0; i < SIZE; ++i)
      error_[pos + i] = err[i];
  }

  void seterrors(size_t id, const double& err);
  void writeerr(double h, double size, double time, int counter);

  template< class Writer >
  void writeerr(Writer& writer, bool last) {
    if (initial_)
    {
      writer.putHeader(outputFile_);
    }

    writer.putStaticCols(outputFile_);

    for (size_t i = 0; i < 2; ++i)
    {
      writer.putErrorCol(outputFile_, prevError_[i], error_[i], prevh_, initial_);
      prevError_[i] = error_[i];
      error_[i] = -1;    // uninitialized
    }

    writer.putLineEnd(outputFile_);

    if (last)
      writer.endTable(outputFile_);

    prevh_ = writer.get_h();
    level_++;
    initial_ = false;
  } // writeerr

  template< class StrVectorType >
  size_t addentry(const StrVectorType& descript, size_t size) {
    if (!initial_)
      DUNE_THROW(Dune::InvalidStateException, "");
    pos_.push_back( boost::numeric_convertion< int >(error_.size()) );
    for (size_t i = 0; i < size; ++i)
    {
      error_.push_back(0);
      prevError_.push_back(0);
      description_.push_back(descript[i]);
    }
    return pos_.size() - 1;
  } // addentry

  size_t addentry(const std::string& descript);

public:
  ~FemEoc();

  static FemEoc& instance() {
    static FemEoc instance_;
    return instance_;
  }

  //! open file path/name and write a description string into tex file
  static void initialize(const std::string& path,
                         const std::string& name,
                         const std::string& descript,
                         const std::string& templateFilename) {
    instance().init(path, name, descript, templateFilename);
  }

  //! open file name and write description string into tex file
  static void initialize(const std::string& name,
                         const std::string& descript,
                         const std::string& templateFilename) {
    instance().init(name, descript, templateFilename);
  }

  /** \brief add a vector of new eoc values
     *
     *  \tparam  StrVectorType a vector type with operator[]
     *           returning a string (a C style array can be used)
     *           the size of the vector is given as parameter
     *  \return  a unique index used to add the error values
     */
  template< class StrVectorType >
  static size_t addEntry(const StrVectorType& descript, size_t size) {
    return instance().addentry(descript, size);
  }

  /** \brief add a vector of new eoc values
     *
     *  \tparam  StrVectorType a vector type with size() and operator[]
     *           returning a string
     *  \return  a unique index used to add the error values
     */

  template< class StrVectorType >
  static size_t addEntry(const StrVectorType& descript) {
    return instance().addentry( descript, descript.size() );
  }

  /** \brief add a single new eoc output
     *
     *  \return  a unique index used to add the error values
     */
  static size_t addEntry(const std::string& descript) {
    return instance().addentry(descript);
  }

  /** \brief add a single new eoc output
     *
     *  \return  a unique index used to add the error values
     */
  static size_t addEntry(const char* descript) {
    return addEntry( std::string(descript) );
  }

  /** \brief add a vector of error values for the given id (returned by
     *         addEntry)
     *  \tparam  VectorType a vector type with an operator[]
     *           returning a double (C style array can be used)
     */
  template< class VectorType >
  static void setErrors(size_t id, const VectorType& err, int size) {
    instance().seterrors(id, err, size);
  }

  /** \brief add a vector of error values for the given id (returned bywrite(GridWidth::calcGridWidth(gridPart),
     *                 grid.size(0),runTime,0);
     *
     *         addEntry)
     *  \tparam  VectorType a vector type with a size() and an operator[]
     *           returning a double
     */
  template< class VectorType >
  static void setErrors(size_t id, const VectorType& err) {
    instance().seterrors( id, err, err.size() );
  }

  /** \brief add a vector in a FieldVector of error values for the given id (returned by
     *         addEntry)
     */
  template< int SIZE >
  static void setErrors(size_t id, const Dune::FieldVector< double, SIZE >& err) {
    instance().seterrors(id, err);
  }

  /** \brief add a single error value for the given id (returned by
     *         addEntry)
     */
  static void setErrors(size_t id, const double& err) {
    instance().seterrors(id, err);
  }

  /** \brief commit a line to the eoc file
     */
  static void write(double h, double size, double time, int counter) {
    instance().writeerr(h, size, time, counter);
  }

  template< class Writer >
  static void write(Writer& writer, bool last = false) {
    instance().writeerr(writer, last);
  }
};

} // namespace Stuff
} // namespace Fem
} // namespace Dune

#endif // HAVE_DUNE_FEM

#endif // ifndef DUNE_STUFF_FEMEOC_HH
<|endoftext|>


<commit_before>/**
* @file CameraMatrixProvider.cpp
*
* @author Heinrich Mellmann
* Implementation of class CameraMatrixProvider
*/

#include "CameraMatrixCorrectorV2.h"

CameraMatrixCorrectorV2::CameraMatrixCorrectorV2()
{
  DEBUG_REQUEST_REGISTER("CameraMatrixV2:calibrate_camera_matrix_line_matching",
    "calculates the roll and tilt offset of the camera using field lines (it. shoult be exactely 3000mm in front of the robot)",
    false);

  DEBUG_REQUEST_REGISTER("CameraMatrixV2:reset_calibration", "set the calibration offsets of the CM to 0", false);

  DEBUG_REQUEST_REGISTER("CameraMatrixV2:collect_calibration_data", "collect the data for calibration", false);
  DEBUG_REQUEST_REGISTER("CameraMatrixV2:clear_calibration_data", "clears the data used for calibration", false);

  DEBUG_REQUEST_REGISTER("CameraMatrixV2:enable_CamMatErrorFunction_drawings", "needed to be activated for error function drawings", false);

  theCamMatErrorFunction = registerModule<CamMatErrorFunction>("CamMatErrorFunction", true);
  last_idx_yaw   = 0;
  last_idx_pitch = 0;
  damping = 0.1;

  head_state      = look_left;
  last_head_state = initial;
}

CameraMatrixCorrectorV2::~CameraMatrixCorrectorV2()
{
}

void CameraMatrixCorrectorV2::execute()
{
  DEBUG_REQUEST("CameraMatrixV2:clear_calibration_data",
    (theCamMatErrorFunction->getModuleT())->calibrationData.clear();
  );

  bool collect_data = false;
  DEBUG_REQUEST("CameraMatrixV2:collect_calibration_data",
    collect_data = true;
  );

  DEBUG_REQUEST_ON_DEACTIVE("CameraMatrixV2:collect_calibration_data",
    head_state = look_left;
    last_head_state = initial;
  );

  if(collect_data){
      // head control states
      getHeadMotionRequest().id = HeadMotionRequest::goto_angle;

      double yaw = 0;
      double pitch = 0;
      bool   target_reached = false;

      switch (head_state){
      case look_left:
          yaw = 88;
          pitch = 0;
          break;
      case look_right:
          yaw = -88;
          pitch = 0;
          break;
      case look_left_down:
          yaw = 88;
          pitch = -14;
          break;
      case look_right_down:
          yaw = -88;
          pitch = -14;
          break;
      default:
          break;
      }

      getHeadMotionRequest().id = HeadMotionRequest::goto_angle;
      getHeadMotionRequest().targetJointPosition.x = Math::fromDegrees(yaw);
      getHeadMotionRequest().targetJointPosition.y = Math::fromDegrees(pitch);
      getHeadMotionRequest().velocity = 30;

      // state transitions
      target_reached = (fabs(Math::toDegrees(getSensorJointData().position[JointData::HeadYaw]) - yaw) < 1) && (fabs(Math::toDegrees(getSensorJointData().position[JointData::HeadPitch]) - pitch) < 1);

      if(target_reached){
          if(head_state == look_left && last_head_state == initial){
              head_state = look_right;
              last_head_state = look_left;
          } else if (head_state == look_right && last_head_state == look_left){
              head_state = look_left;
              last_head_state = look_right;
          } else if (head_state == look_left && last_head_state == look_right){
              head_state = look_left_down;
              last_head_state = look_left;
          } else if (head_state == look_left_down && last_head_state == look_left){
              head_state = look_right_down;
              last_head_state = look_left_down;
          } else if (head_state == look_right_down && last_head_state == look_left_down){
              head_state = look_left_down;
              last_head_state = look_right_down;
          } else if (head_state == look_left_down && last_head_state == look_right_down){
              head_state = look_left;
              last_head_state = look_left_down;
          } else if (head_state == look_left && last_head_state == look_left_down){
              head_state = look_right;
              last_head_state = look_left;
          }
      }

      // collecting data
      if(last_head_state != initial) {
          CamMatErrorFunction::CalibrationData& c_data = (theCamMatErrorFunction->getModuleT())->calibrationData;

          int current_index_yaw   = static_cast<int>((Math::toDegrees(getSensorJointData().position[JointData::HeadYaw])/20.0) + 0.5);
          int current_index_pitch = static_cast<int>((Math::toDegrees(getSensorJointData().position[JointData::HeadPitch])/5.0) + 0.5);

          std::pair<int,int> index;
          index.first  = current_index_yaw;
          index.second = current_index_pitch;

          if(last_idx_pitch != current_index_pitch || last_idx_yaw != current_index_yaw) {
              std::vector<CamMatErrorFunction::CalibrationDataSample>& data = c_data[index];

              data.push_back((struct CamMatErrorFunction::CalibrationDataSample){getKinematicChain().theLinks[KinematicChain::Torso].M,
                                                                                getLineGraphPercept(),getInertialModel(),
                                                                                getSensorJointData().position[JointData::HeadYaw],
                                                                                getSensorJointData().position[JointData::HeadPitch]
                                                                                });

    //          c_data[index].chestPose        = getKinematicChain().theLinks[KinematicChain::Torso].M;
    //          c_data[index].lineGraphPercept = getLineGraphPercept();
    //          c_data[index].headYaw          = getSensorJointData().position[JointData::HeadYaw];
    //          c_data[index].headPitch        = getSensorJointData().position[JointData::HeadPitch];
    //          c_data[index].inertialModel    = getInertialModel();

              last_idx_pitch = current_index_pitch;
              last_idx_yaw   = current_index_yaw;
          }
      }
  }

  MODIFY("CameraMatrixV2:OffsetRollTop",getCameraMatrixOffset().correctionOffset[naoth::CameraInfo::Top].x);
  MODIFY("CameraMatrixV2:OffsetTiltTop",getCameraMatrixOffset().correctionOffset[naoth::CameraInfo::Top].y);
  MODIFY("CameraMatrixV2:OffsetRollBottom",getCameraMatrixOffset().correctionOffset[naoth::CameraInfo::Bottom].x);
  MODIFY("CameraMatrixV2:OffsetTiltBottom",getCameraMatrixOffset().correctionOffset[naoth::CameraInfo::Bottom].y);

  MODIFY("CameraMatrixV2:Body:Roll",  getCameraMatrixOffset().body_rot.x);
  MODIFY("CameraMatrixV2:Body:Pitch", getCameraMatrixOffset().body_rot.y);

  MODIFY("CameraMatrixV2:Head:Roll",  getCameraMatrixOffset().head_rot.x);
  MODIFY("CameraMatrixV2:Head:Pitch", getCameraMatrixOffset().head_rot.y);
  MODIFY("CameraMatrixV2:Head:Yaw",   getCameraMatrixOffset().head_rot.z);

  MODIFY("CameraMatrixV2:Cam:Top:Roll",  getCameraMatrixOffset().cam_rot[naoth::CameraInfo::Top].x);
  MODIFY("CameraMatrixV2:Cam:Top:Pitch", getCameraMatrixOffset().cam_rot[naoth::CameraInfo::Top].y);
  MODIFY("CameraMatrixV2:Cam:Top:Yaw",   getCameraMatrixOffset().cam_rot[naoth::CameraInfo::Top].z);

  MODIFY("CameraMatrixV2:Cam:Bottom:Roll",  getCameraMatrixOffset().cam_rot[naoth::CameraInfo::Bottom].x);
  MODIFY("CameraMatrixV2:Cam:Bottom:Pitch", getCameraMatrixOffset().cam_rot[naoth::CameraInfo::Bottom].y);
  MODIFY("CameraMatrixV2:Cam:Bottom:Yaw",   getCameraMatrixOffset().cam_rot[naoth::CameraInfo::Bottom].z);

  DEBUG_REQUEST("CameraMatrixV2:enable_CamMatErrorFunction_drawings",
      theCamMatErrorFunction->getModuleT()->plot_CalibrationData();
  );

  DEBUG_REQUEST("CameraMatrixV2:calibrate_camera_matrix_line_matching",
      calibrate();
  );

  DEBUG_REQUEST_ON_DEACTIVE("CameraMatrixV2:calibrate_camera_matrix_line_matching",
      getCameraMatrixOffset().saveToConfig();
  );

  DEBUG_REQUEST("CameraMatrixV2:reset_calibration",
      reset_calibration();
  );

  DEBUG_REQUEST_ON_DEACTIVE("CameraMatrixV2:reset_calibration",
      getCameraMatrixOffset().saveToConfig();
  );

}//end execute

void CameraMatrixCorrectorV2::reset_calibration()
{
  getCameraMatrixOffset().body_rot = Vector2d();
  getCameraMatrixOffset().head_rot = Vector3d();
  getCameraMatrixOffset().cam_rot[CameraInfo::Top]    = Vector3d();
  getCameraMatrixOffset().cam_rot[CameraInfo::Bottom] = Vector3d();
}

void CameraMatrixCorrectorV2::calibrate()
{
  // calibrate the camera matrix
  Eigen::Matrix<double, 11, 1> offset;

  offset = gn_minimizer.minimizeOneStep(*(theCamMatErrorFunction->getModuleT()),1e-4);

  MODIFY("CameraMatrixV2:damping_factor", damping);

  getCameraMatrixOffset().body_rot = getCameraMatrixOffset().body_rot * (1-damping) + (getCameraMatrixOffset().body_rot + Vector2d(offset(0),offset(1)))           * damping;
  getCameraMatrixOffset().head_rot = getCameraMatrixOffset().head_rot * (1-damping) + (getCameraMatrixOffset().head_rot + Vector3d(offset(2),offset(3),offset(4))) * damping;
  getCameraMatrixOffset().cam_rot[CameraInfo::Top]    = getCameraMatrixOffset().cam_rot[CameraInfo::Top]    * (1-damping) + (getCameraMatrixOffset().cam_rot[CameraInfo::Top]    + Vector3d(offset(5),offset(6),offset(7)))  * damping;
  getCameraMatrixOffset().cam_rot[CameraInfo::Bottom] = getCameraMatrixOffset().cam_rot[CameraInfo::Bottom] * (1-damping) + (getCameraMatrixOffset().cam_rot[CameraInfo::Bottom] + Vector3d(offset(8),offset(9),offset(10))) * damping;
}//end calibrate
<commit_msg>increase number of horizontal bins change pitch angle in collecting stuff<commit_after>/**
* @file CameraMatrixProvider.cpp
*
* @author Heinrich Mellmann
* Implementation of class CameraMatrixProvider
*/

#include "CameraMatrixCorrectorV2.h"

CameraMatrixCorrectorV2::CameraMatrixCorrectorV2()
{
  DEBUG_REQUEST_REGISTER("CameraMatrixV2:calibrate_camera_matrix_line_matching",
    "calculates the roll and tilt offset of the camera using field lines (it. shoult be exactely 3000mm in front of the robot)",
    false);

  DEBUG_REQUEST_REGISTER("CameraMatrixV2:reset_calibration", "set the calibration offsets of the CM to 0", false);

  DEBUG_REQUEST_REGISTER("CameraMatrixV2:collect_calibration_data", "collect the data for calibration", false);
  DEBUG_REQUEST_REGISTER("CameraMatrixV2:clear_calibration_data", "clears the data used for calibration", false);

  DEBUG_REQUEST_REGISTER("CameraMatrixV2:enable_CamMatErrorFunction_drawings", "needed to be activated for error function drawings", false);

  theCamMatErrorFunction = registerModule<CamMatErrorFunction>("CamMatErrorFunction", true);
  last_idx_yaw   = 0;
  last_idx_pitch = 0;
  damping = 0.1;

  head_state      = look_left;
  last_head_state = initial;
}

CameraMatrixCorrectorV2::~CameraMatrixCorrectorV2()
{
}

void CameraMatrixCorrectorV2::execute()
{
  DEBUG_REQUEST("CameraMatrixV2:clear_calibration_data",
    (theCamMatErrorFunction->getModuleT())->calibrationData.clear();
  );

  bool collect_data = false;
  DEBUG_REQUEST("CameraMatrixV2:collect_calibration_data",
    collect_data = true;
  );

  DEBUG_REQUEST_ON_DEACTIVE("CameraMatrixV2:collect_calibration_data",
    head_state = look_left;
    last_head_state = initial;
    getHeadMotionRequest().id = HeadMotionRequest::hold;
  );

  if(collect_data){
      // head control states

      double yaw = 0;
      double pitch = 0;
      bool   target_reached = false;

      switch (head_state){
      case look_left:
          yaw = 88;
          pitch = 0;
          break;
      case look_right:
          yaw = -88;
          pitch = 0;
          break;
      case look_left_down:
          yaw = 88;
          pitch = 10;
          break;
      case look_right_down:
          yaw = -88;
          pitch = 10;
          break;
      default:
          break;
      }

      getHeadMotionRequest().id = HeadMotionRequest::goto_angle;
      getHeadMotionRequest().targetJointPosition.x = Math::fromDegrees(yaw);
      getHeadMotionRequest().targetJointPosition.y = Math::fromDegrees(pitch);
      getHeadMotionRequest().velocity = 10;
      MODIFY("CameraMatrixV2:collecting_velocity", getHeadMotionRequest().velocity);

      // state transitions
      target_reached = (fabs(Math::toDegrees(getSensorJointData().position[JointData::HeadYaw]) - yaw) < 2) && (fabs(Math::toDegrees(getSensorJointData().position[JointData::HeadPitch]) - pitch) < 2);

      if(target_reached){
          if(head_state == look_left && last_head_state == initial){
              head_state = look_right;
              last_head_state = look_left;
          } else if (head_state == look_right && last_head_state == look_left){
              head_state = look_left;
              last_head_state = look_right;
          } else if (head_state == look_left && last_head_state == look_right){
              head_state = look_left_down;
              last_head_state = look_left;
          } else if (head_state == look_left_down && last_head_state == look_left){
              head_state = look_right_down;
              last_head_state = look_left_down;
          } else if (head_state == look_right_down && last_head_state == look_left_down){
              head_state = look_left_down;
              last_head_state = look_right_down;
          } else if (head_state == look_left_down && last_head_state == look_right_down){
              head_state = look_left;
              last_head_state = look_left_down;
          } else if (head_state == look_left && last_head_state == look_left_down){
              head_state = look_right;
              last_head_state = look_left;
          }
      }

      // collecting data
      if(last_head_state != initial) {
          CamMatErrorFunction::CalibrationData& c_data = (theCamMatErrorFunction->getModuleT())->calibrationData;

          int current_index_yaw   = static_cast<int>((Math::toDegrees(getSensorJointData().position[JointData::HeadYaw])/15.0) + 0.5);
          int current_index_pitch = static_cast<int>((Math::toDegrees(getSensorJointData().position[JointData::HeadPitch])/5.0) + 0.5);

          std::pair<int,int> index;
          index.first  = current_index_yaw;
          index.second = current_index_pitch;

          if(last_idx_pitch != current_index_pitch || last_idx_yaw != current_index_yaw) {
              std::vector<CamMatErrorFunction::CalibrationDataSample>& data = c_data[index];

              data.push_back((struct CamMatErrorFunction::CalibrationDataSample){getKinematicChain().theLinks[KinematicChain::Torso].M,
                                                                                getLineGraphPercept(),getInertialModel(),
                                                                                getSensorJointData().position[JointData::HeadYaw],
                                                                                getSensorJointData().position[JointData::HeadPitch]
                                                                                });

    //          c_data[index].chestPose        = getKinematicChain().theLinks[KinematicChain::Torso].M;
    //          c_data[index].lineGraphPercept = getLineGraphPercept();
    //          c_data[index].headYaw          = getSensorJointData().position[JointData::HeadYaw];
    //          c_data[index].headPitch        = getSensorJointData().position[JointData::HeadPitch];
    //          c_data[index].inertialModel    = getInertialModel();

              last_idx_pitch = current_index_pitch;
              last_idx_yaw   = current_index_yaw;
          }
      }
  }

  MODIFY("CameraMatrixV2:OffsetRollTop",getCameraMatrixOffset().correctionOffset[naoth::CameraInfo::Top].x);
  MODIFY("CameraMatrixV2:OffsetTiltTop",getCameraMatrixOffset().correctionOffset[naoth::CameraInfo::Top].y);
  MODIFY("CameraMatrixV2:OffsetRollBottom",getCameraMatrixOffset().correctionOffset[naoth::CameraInfo::Bottom].x);
  MODIFY("CameraMatrixV2:OffsetTiltBottom",getCameraMatrixOffset().correctionOffset[naoth::CameraInfo::Bottom].y);

  MODIFY("CameraMatrixV2:Body:Roll",  getCameraMatrixOffset().body_rot.x);
  MODIFY("CameraMatrixV2:Body:Pitch", getCameraMatrixOffset().body_rot.y);

  MODIFY("CameraMatrixV2:Head:Roll",  getCameraMatrixOffset().head_rot.x);
  MODIFY("CameraMatrixV2:Head:Pitch", getCameraMatrixOffset().head_rot.y);
  MODIFY("CameraMatrixV2:Head:Yaw",   getCameraMatrixOffset().head_rot.z);

  MODIFY("CameraMatrixV2:Cam:Top:Roll",  getCameraMatrixOffset().cam_rot[naoth::CameraInfo::Top].x);
  MODIFY("CameraMatrixV2:Cam:Top:Pitch", getCameraMatrixOffset().cam_rot[naoth::CameraInfo::Top].y);
  MODIFY("CameraMatrixV2:Cam:Top:Yaw",   getCameraMatrixOffset().cam_rot[naoth::CameraInfo::Top].z);

  MODIFY("CameraMatrixV2:Cam:Bottom:Roll",  getCameraMatrixOffset().cam_rot[naoth::CameraInfo::Bottom].x);
  MODIFY("CameraMatrixV2:Cam:Bottom:Pitch", getCameraMatrixOffset().cam_rot[naoth::CameraInfo::Bottom].y);
  MODIFY("CameraMatrixV2:Cam:Bottom:Yaw",   getCameraMatrixOffset().cam_rot[naoth::CameraInfo::Bottom].z);

  DEBUG_REQUEST("CameraMatrixV2:enable_CamMatErrorFunction_drawings",
      theCamMatErrorFunction->getModuleT()->plot_CalibrationData();
  );

  DEBUG_REQUEST("CameraMatrixV2:calibrate_camera_matrix_line_matching",
      calibrate();
  );

  DEBUG_REQUEST_ON_DEACTIVE("CameraMatrixV2:calibrate_camera_matrix_line_matching",
      getCameraMatrixOffset().saveToConfig();
  );

  DEBUG_REQUEST("CameraMatrixV2:reset_calibration",
      reset_calibration();
  );

  DEBUG_REQUEST_ON_DEACTIVE("CameraMatrixV2:reset_calibration",
      getCameraMatrixOffset().saveToConfig();
  );

}//end execute

void CameraMatrixCorrectorV2::reset_calibration()
{
  getCameraMatrixOffset().body_rot = Vector2d();
  getCameraMatrixOffset().head_rot = Vector3d();
  getCameraMatrixOffset().cam_rot[CameraInfo::Top]    = Vector3d();
  getCameraMatrixOffset().cam_rot[CameraInfo::Bottom] = Vector3d();
}

void CameraMatrixCorrectorV2::calibrate()
{
  // calibrate the camera matrix
  Eigen::Matrix<double, 11, 1> offset;

  offset = gn_minimizer.minimizeOneStep(*(theCamMatErrorFunction->getModuleT()),1e-4);

  MODIFY("CameraMatrixV2:damping_factor", damping);

  getCameraMatrixOffset().body_rot = getCameraMatrixOffset().body_rot * (1-damping) + (getCameraMatrixOffset().body_rot + Vector2d(offset(0),offset(1)))           * damping;
  getCameraMatrixOffset().head_rot = getCameraMatrixOffset().head_rot * (1-damping) + (getCameraMatrixOffset().head_rot + Vector3d(offset(2),offset(3),offset(4))) * damping;
  getCameraMatrixOffset().cam_rot[CameraInfo::Top]    = getCameraMatrixOffset().cam_rot[CameraInfo::Top]    * (1-damping) + (getCameraMatrixOffset().cam_rot[CameraInfo::Top]    + Vector3d(offset(5),offset(6),offset(7)))  * damping;
  getCameraMatrixOffset().cam_rot[CameraInfo::Bottom] = getCameraMatrixOffset().cam_rot[CameraInfo::Bottom] * (1-damping) + (getCameraMatrixOffset().cam_rot[CameraInfo::Bottom] + Vector3d(offset(8),offset(9),offset(10))) * damping;
}//end calibrate
<|endoftext|>


<commit_before>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit (ITK)
  Module:    itkImageToImageAffineMeanSquaresRegularStepGradientDescentRegistrationTest.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

Copyright (c) 2000 National Library of Medicine
All rights reserved.

See COPYRIGHT.txt for copyright details.

=========================================================================*/
#include "itkPhysicalImage.h"
#include "itkSimpleImageRegionIterator.h"
#include "itkImageToImageAffineMeanSquaresRegularStepGradientDescentRegistration.h"

/**
 *  This test uses two 2D-Gaussians (standard deviation RegionSize/2)
 *  One is shifted by 7,3 pixels from the other.
 *  therefore the solution of the registration is |-7 -3|
 *  This test uses LevenbergMarquart Optimizer but
 *  conjugate gradient optimizer tolerances are also defined
 *  in the itkImageToImageAffineMeanSquaresRegularStepGradientDescentRegistration.txx file
 *  (you need to change the type of the optimizer in the header file
 *  ie itkImageToImageAffineMeanSquaresRegularStepGradientDescentRegistration.h)
 */

int main()
{

  /*Allocate Images*/
  typedef itk::PhysicalImage<unsigned char,2>           ReferenceType;
  typedef itk::PhysicalImage<unsigned char,2>           TargetType;

  typedef itk::ImageToImageAffineMeanSquaresRegularStepGradientDescentRegistration<ReferenceType,TargetType> RegistrationType;

  ReferenceType::SizeType size = {{100,100}};
  ReferenceType::IndexType index = {{0,0}};
  ReferenceType::RegionType region;
  region.SetSize( size );
  region.SetIndex( index );

  ReferenceType::Pointer imgReference = ReferenceType::New();
  imgReference->SetLargestPossibleRegion( region );
  imgReference->SetBufferedRegion( region );
  imgReference->SetRequestedRegion( region );
  imgReference->Allocate();

  TargetType::Pointer imgTarget = TargetType::New();
  imgTarget->SetLargestPossibleRegion( region );
  imgTarget->SetBufferedRegion( region );
  imgTarget->SetRequestedRegion( region );
  imgTarget->Allocate();

  /* Fill images with a 2D gaussian*/
  typedef  itk::SimpleImageRegionIterator<ReferenceType> ReferenceIteratorType;

  typedef  itk::SimpleImageRegionIterator<TargetType> TargetIteratorType;

  itk::Point<double,2> center;
  center[0] = (double)region.GetSize()[0]/2.0;
  center[1] = (double)region.GetSize()[1]/2.0;

  const double s = (double)region.GetSize()[0]/2.0;

  itk::Point<double,2>  p;
  itk::Vector<double,2> d;

  /* Set the displacement */
  itk::Vector<double,2> displacement;
  displacement[0] = 7;
  displacement[1] = 3;

  ReferenceIteratorType ri(imgReference,region);
  TargetIteratorType ti(imgTarget,region);
  ri.Begin();
  while(!ri.IsAtEnd())
  {
    p[0] = ri.GetIndex()[0];
    p[1] = ri.GetIndex()[1];
    d = p-center;
    d += displacement;
    const double x = d[0];
    const double y = d[1];
    ri.Set( 200.0 * exp( - ( x*x + y*y )/(s*s) ) );
    ++ri;
  }

  ti.Begin();
  while(!ti.IsAtEnd())
  {
    p[0] = ti.GetIndex()[0];
    p[1] = ti.GetIndex()[1];
    d = p-center;
    const double x = d[0];
    const double y = d[1];
    ti.Set( 200.0 * exp( - ( x*x + y*y )/(s*s) ) );
    ++ti;
  }

  RegistrationType::Pointer registrationMethod = RegistrationType::New();

  registrationMethod->SetReference(imgReference);
  registrationMethod->SetTarget(imgTarget);
  registrationMethod->SetTranslationScale( 1e4 );

  registrationMethod->StartRegistration();

  std::cout << "The correct answer should be : " << std::endl;
  std::cout << " 1.0    0.0    0.0    1.0  ";
  std::cout << -displacement << std::endl;

  return EXIT_SUCCESS;

}
<commit_msg>ERR: Optimizer initialization missing.  Added quantitative test<commit_after>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit (ITK)
  Module:    itkImageToImageAffineMeanSquaresRegularStepGradientDescentRegistrationTest.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

Copyright (c) 2000 National Library of Medicine
All rights reserved.

See COPYRIGHT.txt for copyright details.

=========================================================================*/
#include "itkPhysicalImage.h"
#include "itkSimpleImageRegionIterator.h"
#include "itkImageToImageAffineMeanSquaresRegularStepGradientDescentRegistration.h"

/**
 *  This test uses two 2D-Gaussians (standard deviation RegionSize/2)
 *  One is shifted by 7,3 pixels from the other.
 *  therefore the solution of the registration is |-7 -3|
 *  This test uses LevenbergMarquart Optimizer but
 *  conjugate gradient optimizer tolerances are also defined
 *  in the itkImageToImageAffineMeanSquaresRegularStepGradientDescentRegistration.txx file
 *  (you need to change the type of the optimizer in the header file
 *  ie itkImageToImageAffineMeanSquaresRegularStepGradientDescentRegistration.h)
 */

int main()
{

  /*Allocate Images*/
  typedef itk::PhysicalImage<unsigned char,2>           ReferenceType;
  typedef itk::PhysicalImage<unsigned char,2>           TargetType;

  typedef itk::ImageToImageAffineMeanSquaresRegularStepGradientDescentRegistration<ReferenceType,TargetType> RegistrationType;

  ReferenceType::SizeType size = {{100,100}};
  ReferenceType::IndexType index = {{0,0}};
  ReferenceType::RegionType region;
  region.SetSize( size );
  region.SetIndex( index );

  ReferenceType::Pointer imgReference = ReferenceType::New();
  imgReference->SetLargestPossibleRegion( region );
  imgReference->SetBufferedRegion( region );
  imgReference->SetRequestedRegion( region );
  imgReference->Allocate();

  TargetType::Pointer imgTarget = TargetType::New();
  imgTarget->SetLargestPossibleRegion( region );
  imgTarget->SetBufferedRegion( region );
  imgTarget->SetRequestedRegion( region );
  imgTarget->Allocate();

  /* Fill images with a 2D gaussian*/
  typedef  itk::SimpleImageRegionIterator<ReferenceType> ReferenceIteratorType;

  typedef  itk::SimpleImageRegionIterator<TargetType> TargetIteratorType;

  itk::Point<double,2> center;
  center[0] = (double)region.GetSize()[0]/2.0;
  center[1] = (double)region.GetSize()[1]/2.0;

  const double s = (double)region.GetSize()[0]/2.0;

  itk::Point<double,2>  p;
  itk::Vector<double,2> d;

  /* Set the displacement */
  itk::Vector<double,2> displacement;
  displacement[0] = 7;
  displacement[1] = 3;

  ReferenceIteratorType ri(imgReference,region);
  TargetIteratorType ti(imgTarget,region);
  ri.Begin();
  while(!ri.IsAtEnd())
  {
    p[0] = ri.GetIndex()[0];
    p[1] = ri.GetIndex()[1];
    d = p-center;
    d += displacement;
    const double x = d[0];
    const double y = d[1];
    ri.Set( 200.0 * exp( - ( x*x + y*y )/(s*s) ) );
    ++ri;
  }

  ti.Begin();
  while(!ti.IsAtEnd())
  {
    p[0] = ti.GetIndex()[0];
    p[1] = ti.GetIndex()[1];
    d = p-center;
    const double x = d[0];
    const double y = d[1];
    ti.Set( 200.0 * exp( - ( x*x + y*y )/(s*s) ) );
    ++ti;
  }

  const double translationScale = 1e4;

  RegistrationType::Pointer registrationMethod = RegistrationType::New();

  registrationMethod->SetReference(imgReference);
  registrationMethod->SetTarget(imgTarget);
  registrationMethod->SetTranslationScale( translationScale );

  registrationMethod->GetOptimizer()->SetMaximumStepLength( 1.0  );
  registrationMethod->GetOptimizer()->SetMinimumStepLength( 1e-3 );
  registrationMethod->GetOptimizer()->SetGradientMagnitudeTolerance( 1e-8 );
  registrationMethod->GetOptimizer()->SetMaximumNumberOfIterations( 200 );

  registrationMethod->StartRegistration();

  // get the results
  RegistrationType::ParametersType solution =
    registrationMethod->GetOptimizer()->GetCurrentPosition();

  std::cout << "Solution is: " << solution << std::endl;

  //
  // check results to see if it is within range
  //
  bool pass = true;
  double trueParameters[6] = { 1, 0, 0, 1, -7, -3 };
  for( unsigned int j = 0; j < 4; j++ )
    {
    if( vnl_math_abs( solution[j] - trueParameters[j] ) > 0.02 )
      pass = false;
    }
  for( unsigned int j = 4; j < 6; j++ )
    {
    if( vnl_math_abs( solution[j] * translationScale - trueParameters[j] ) > 1.0 )
      pass = false;
    }

  if( !pass )
    {
    std::cout << "Test failed." << std::endl;
    return EXIT_FAILURE;
    }

  std::cout << "Test passed." << std::endl;
  return EXIT_SUCCESS;

}
<|endoftext|>


<commit_before>/* -*- Mode: C++; tab-width: 4; c-basic-offset: 4; indent-tabs-mode: nil -*- */
#ifndef FLUSHER_H
#define FLUSHER_H 1

#include "common.hh"
#include "ep.hh"
#include "dispatcher.hh"

enum flusher_state {
    initializing,
    running,
    pausing,
    paused,
    stopping,
    stopped
};

class Flusher;

class FlusherStepper : public DispatcherCallback {
public:
    FlusherStepper(Flusher* f) : flusher(f) { }
    bool callback(Dispatcher &d, TaskId t);
private:
    Flusher *flusher;
};

class Flusher {
public:
    Flusher(EventuallyPersistentStore *st, Dispatcher *d) :
        store(st), _state(initializing), dispatcher(d),
        flushQueue(NULL) {
    }
    ~Flusher() {
        if (_state != stopped) {
            getLogger()->log(EXTENSION_LOG_WARNING, NULL,
                             "Flusher being destroyed in state %s\n",
                             stateName(_state));

        }
    }

    bool stop();
    void wait();
    bool pause();
    bool resume();

    void initialize(TaskId);

    void start(void);
    void wake(void);
    bool step(Dispatcher&, TaskId);

    bool transition_state(enum flusher_state to);

    enum flusher_state state() const;
    const char * stateName() const;
private:
    int doFlush();
    void completeFlush();
    void schedule_UNLOCKED();

    EventuallyPersistentStore *store;
    volatile enum flusher_state _state;
    Mutex taskMutex;
    TaskId task;
    Dispatcher *dispatcher;
    const char * stateName(enum flusher_state st) const;

    // Current flush cycle state.
    int                     flushRv;
    std::queue<QueuedItem> *flushQueue;
    std::queue<QueuedItem> *rejectQueue;
    rel_time_t              flushStart;

    DISALLOW_COPY_AND_ASSIGN(Flusher);
};

#endif /* FLUSHER_H */
<commit_msg>Some flusher doc updates.<commit_after>/* -*- Mode: C++; tab-width: 4; c-basic-offset: 4; indent-tabs-mode: nil -*- */
#ifndef FLUSHER_H
#define FLUSHER_H 1

#include "common.hh"
#include "ep.hh"
#include "dispatcher.hh"

enum flusher_state {
    initializing,
    running,
    pausing,
    paused,
    stopping,
    stopped
};

class Flusher;

/**
 * A DispatcherCallback adaptor over Flusher.
 */
class FlusherStepper : public DispatcherCallback {
public:
    FlusherStepper(Flusher* f) : flusher(f) { }
    bool callback(Dispatcher &d, TaskId t);
private:
    Flusher *flusher;
};

/**
 * Manage persistence of data for an EventuallyPersistentStore.
 */
class Flusher {
public:

    Flusher(EventuallyPersistentStore *st, Dispatcher *d) :
        store(st), _state(initializing), dispatcher(d),
        flushQueue(NULL) {
    }

    ~Flusher() {
        if (_state != stopped) {
            getLogger()->log(EXTENSION_LOG_WARNING, NULL,
                             "Flusher being destroyed in state %s\n",
                             stateName(_state));

        }
    }

    bool stop();
    void wait();
    bool pause();
    bool resume();

    void initialize(TaskId);

    void start(void);
    void wake(void);
    bool step(Dispatcher&, TaskId);

    bool transition_state(enum flusher_state to);

    enum flusher_state state() const;
    const char * stateName() const;
private:
    int doFlush();
    void completeFlush();
    void schedule_UNLOCKED();

    EventuallyPersistentStore *store;
    volatile enum flusher_state _state;
    Mutex taskMutex;
    TaskId task;
    Dispatcher *dispatcher;
    const char * stateName(enum flusher_state st) const;

    // Current flush cycle state.
    int                     flushRv;
    std::queue<QueuedItem> *flushQueue;
    std::queue<QueuedItem> *rejectQueue;
    rel_time_t              flushStart;

    DISALLOW_COPY_AND_ASSIGN(Flusher);
};

#endif /* FLUSHER_H */
<|endoftext|>