<commit_before>#include "perf_precomp.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/opencv_modules.hpp"
#include "opencv2/flann.hpp"

using namespace std;
using namespace cv;
using namespace perf;
using std::tr1::make_tuple;
using std::tr1::get;

typedef TestBaseWithParam<size_t> FeaturesFinderVec;
typedef TestBaseWithParam<string> match;
typedef std::tr1::tuple<string, int> matchVector_t;
typedef TestBaseWithParam<matchVector_t> matchVector;

#define NUMBER_IMAGES testing::Values(1, 5, 20)
#define SURF_MATCH_CONFIDENCE 0.65f
#define ORB_MATCH_CONFIDENCE  0.3f
#define WORK_MEGAPIX 0.6

#ifdef HAVE_OPENCV_XFEATURES2D
#define TEST_DETECTORS testing::Values("surf", "orb")
#else
#define TEST_DETECTORS testing::Values<string>("orb")
#endif

PERF_TEST_P(FeaturesFinderVec, ParallelFeaturesFinder, NUMBER_IMAGES)
{
    Mat img = imread( getDataPath("stitching/a1.png") );
    vector<Mat> imgs(GetParam(), img);
    vector<detail::ImageFeatures> features(imgs.size());

    Ptr<detail::FeaturesFinder> featuresFinder = makePtr<detail::OrbFeaturesFinder>();

    TEST_CYCLE()
    {
        (*featuresFinder)(imgs, features);
    }

    SANITY_CHECK_NOTHING();
}

PERF_TEST_P(FeaturesFinderVec, SerialFeaturesFinder, NUMBER_IMAGES)
{
    Mat img = imread( getDataPath("stitching/a1.png") );
    vector<Mat> imgs(GetParam(), img);
    vector<detail::ImageFeatures> features(imgs.size());

    Ptr<detail::FeaturesFinder> featuresFinder = makePtr<detail::OrbFeaturesFinder>();

    TEST_CYCLE()
    {
        for (size_t i = 0; i < imgs.size(); ++i)
            (*featuresFinder)(imgs[i], features[i]);
    }

    SANITY_CHECK_NOTHING();
}

PERF_TEST_P( match, bestOf2Nearest, TEST_DETECTORS)
{
    Mat img1, img1_full = imread( getDataPath("stitching/boat1.jpg") );
    Mat img2, img2_full = imread( getDataPath("stitching/boat2.jpg") );
    float scale1 = (float)std::min(1.0, sqrt(WORK_MEGAPIX * 1e6 / img1_full.total()));
    float scale2 = (float)std::min(1.0, sqrt(WORK_MEGAPIX * 1e6 / img2_full.total()));
    resize(img1_full, img1, Size(), scale1, scale1);
    resize(img2_full, img2, Size(), scale2, scale2);

    Ptr<detail::FeaturesFinder> finder;
    Ptr<detail::FeaturesMatcher> matcher;
    if (GetParam() == "surf")
    {
        finder = makePtr<detail::SurfFeaturesFinder>();
        matcher = makePtr<detail::BestOf2NearestMatcher>(false, SURF_MATCH_CONFIDENCE);
    }
    else if (GetParam() == "orb")
    {
        finder = makePtr<detail::OrbFeaturesFinder>();
        matcher = makePtr<detail::BestOf2NearestMatcher>(false, ORB_MATCH_CONFIDENCE);
    }
    else
    {
        FAIL() << "Unknown 2D features type: " << GetParam();
    }

    detail::ImageFeatures features1, features2;
    (*finder)(img1, features1);
    (*finder)(img2, features2);

    detail::MatchesInfo pairwise_matches;

    declare.in(features1.descriptors, features2.descriptors);

    while(next())
    {
        cvflann::seed_random(42);//for predictive FlannBasedMatcher
        startTimer();
        (*matcher)(features1, features2, pairwise_matches);
        stopTimer();
        matcher->collectGarbage();
    }

    Mat dist (pairwise_matches.H, Range::all(), Range(2, 3));
    Mat R (pairwise_matches.H, Range::all(), Range(0, 2));
    // separate transform matrix, use lower error on rotations
    SANITY_CHECK(dist, 1., ERROR_ABSOLUTE);
    SANITY_CHECK(R, .015, ERROR_ABSOLUTE);
}

PERF_TEST_P( matchVector, bestOf2NearestVectorFeatures, testing::Combine(
                 TEST_DETECTORS,
                 testing::Values(2, 4, 8))
             )
{
    Mat img1, img1_full = imread( getDataPath("stitching/boat1.jpg") );
    Mat img2, img2_full = imread( getDataPath("stitching/boat2.jpg") );
    float scale1 = (float)std::min(1.0, sqrt(WORK_MEGAPIX * 1e6 / img1_full.total()));
    float scale2 = (float)std::min(1.0, sqrt(WORK_MEGAPIX * 1e6 / img2_full.total()));
    resize(img1_full, img1, Size(), scale1, scale1);
    resize(img2_full, img2, Size(), scale2, scale2);

    Ptr<detail::FeaturesFinder> finder;
    Ptr<detail::FeaturesMatcher> matcher;
    string detectorName = get<0>(GetParam());
    int featuresVectorSize = get<1>(GetParam());
    if (detectorName == "surf")
    {
        finder = makePtr<detail::SurfFeaturesFinder>();
        matcher = makePtr<detail::BestOf2NearestMatcher>(false, SURF_MATCH_CONFIDENCE);
    }
    else if (detectorName == "orb")
    {
        finder = makePtr<detail::OrbFeaturesFinder>();
        matcher = makePtr<detail::BestOf2NearestMatcher>(false, ORB_MATCH_CONFIDENCE);
    }
    else
    {
        FAIL() << "Unknown 2D features type: " << get<0>(GetParam());
    }

    detail::ImageFeatures features1, features2;
    (*finder)(img1, features1);
    (*finder)(img2, features2);
    vector<detail::ImageFeatures> features;
    vector<detail::MatchesInfo> pairwise_matches;
    for(int i = 0; i < featuresVectorSize/2; i++)
    {
        features.push_back(features1);
        features.push_back(features2);
    }

    declare.time(200);
    while(next())
    {
        cvflann::seed_random(42);//for predictive FlannBasedMatcher
        startTimer();
        (*matcher)(features, pairwise_matches);
        stopTimer();
        matcher->collectGarbage();
    }

    size_t matches_count = 0;
    for (size_t i = 0; i < pairwise_matches.size(); ++i)
    {
        if (pairwise_matches[i].src_img_idx < 0)
            continue;

        EXPECT_TRUE(pairwise_matches[i].matches.size() > 100);
        EXPECT_FALSE(pairwise_matches[i].H.empty());
        ++matches_count;
    }

    EXPECT_TRUE(matches_count > 0);

    SANITY_CHECK_NOTHING();
}

PERF_TEST_P( match, affineBestOf2Nearest, TEST_DETECTORS)
{
    Mat img1, img1_full = imread( getDataPath("stitching/s1.jpg") );
    Mat img2, img2_full = imread( getDataPath("stitching/s2.jpg") );
    float scale1 = (float)std::min(1.0, sqrt(WORK_MEGAPIX * 1e6 / img1_full.total()));
    float scale2 = (float)std::min(1.0, sqrt(WORK_MEGAPIX * 1e6 / img2_full.total()));
    resize(img1_full, img1, Size(), scale1, scale1);
    resize(img2_full, img2, Size(), scale2, scale2);

    Ptr<detail::FeaturesFinder> finder;
    Ptr<detail::FeaturesMatcher> matcher;
    if (GetParam() == "surf")
    {
        finder = makePtr<detail::SurfFeaturesFinder>();
        matcher = makePtr<detail::AffineBestOf2NearestMatcher>(false, false, SURF_MATCH_CONFIDENCE);
    }
    else if (GetParam() == "orb")
    {
        finder = makePtr<detail::OrbFeaturesFinder>();
        matcher = makePtr<detail::AffineBestOf2NearestMatcher>(false, false, ORB_MATCH_CONFIDENCE);
    }
    else
    {
        FAIL() << "Unknown 2D features type: " << GetParam();
    }

    detail::ImageFeatures features1, features2;
    (*finder)(img1, features1);
    (*finder)(img2, features2);

    detail::MatchesInfo pairwise_matches;

    declare.in(features1.descriptors, features2.descriptors);

    while(next())
    {
        cvflann::seed_random(42);//for predictive FlannBasedMatcher
        startTimer();
        (*matcher)(features1, features2, pairwise_matches);
        stopTimer();
        matcher->collectGarbage();
    }

    // separate rotation and translation in transform matrix
    Mat T (pairwise_matches.H, Range(0, 2), Range(2, 3));
    Mat R (pairwise_matches.H, Range(0, 2), Range(0, 2));
    Mat h (pairwise_matches.H, Range(2, 3), Range::all());
    SANITY_CHECK(T, 5, ERROR_ABSOLUTE); // allow 5 pixels diff in translations
    SANITY_CHECK(R, .01, ERROR_ABSOLUTE); // rotations must be more precise
    // last row should be precisely (0, 0, 1) as it is just added for representation in homogeneous
    // coordinates
    EXPECT_DOUBLE_EQ(h.at<double>(0), 0.);
    EXPECT_DOUBLE_EQ(h.at<double>(1), 0.);
    EXPECT_DOUBLE_EQ(h.at<double>(2), 1.);
}

PERF_TEST_P( matchVector, affineBestOf2NearestVectorFeatures, testing::Combine(
                 TEST_DETECTORS,
                 testing::Values(2, 4, 8))
             )
{
    Mat img1, img1_full = imread( getDataPath("stitching/s1.jpg") );
    Mat img2, img2_full = imread( getDataPath("stitching/s2.jpg") );
    float scale1 = (float)std::min(1.0, sqrt(WORK_MEGAPIX * 1e6 / img1_full.total()));
    float scale2 = (float)std::min(1.0, sqrt(WORK_MEGAPIX * 1e6 / img2_full.total()));
    resize(img1_full, img1, Size(), scale1, scale1);
    resize(img2_full, img2, Size(), scale2, scale2);

    Ptr<detail::FeaturesFinder> finder;
    Ptr<detail::FeaturesMatcher> matcher;
    string detectorName = get<0>(GetParam());
    int featuresVectorSize = get<1>(GetParam());
    if (detectorName == "surf")
    {
        finder = makePtr<detail::SurfFeaturesFinder>();
        matcher = makePtr<detail::AffineBestOf2NearestMatcher>(false, false, SURF_MATCH_CONFIDENCE);
    }
    else if (detectorName == "orb")
    {
        finder = makePtr<detail::OrbFeaturesFinder>();
        matcher = makePtr<detail::AffineBestOf2NearestMatcher>(false, false, ORB_MATCH_CONFIDENCE);
    }
    else
    {
        FAIL() << "Unknown 2D features type: " << get<0>(GetParam());
    }

    detail::ImageFeatures features1, features2;
    (*finder)(img1, features1);
    (*finder)(img2, features2);
    vector<detail::ImageFeatures> features;
    vector<detail::MatchesInfo> pairwise_matches;
    for(int i = 0; i < featuresVectorSize/2; i++)
    {
        features.push_back(features1);
        features.push_back(features2);
    }

    declare.time(200);
    while(next())
    {
        cvflann::seed_random(42);//for predictive FlannBasedMatcher
        startTimer();
        (*matcher)(features, pairwise_matches);
        stopTimer();
        matcher->collectGarbage();
    }

    size_t matches_count = 0;
    for (size_t i = 0; i < pairwise_matches.size(); ++i)
    {
        if (pairwise_matches[i].src_img_idx < 0)
            continue;

        EXPECT_TRUE(pairwise_matches[i].matches.size() > 400);
        EXPECT_FALSE(pairwise_matches[i].H.empty());
        ++matches_count;
    }

    EXPECT_TRUE(matches_count > 0);

    SANITY_CHECK_NOTHING();
}
<commit_msg>test: clarify fail messages<commit_after>#include "perf_precomp.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/opencv_modules.hpp"
#include "opencv2/flann.hpp"

using namespace std;
using namespace cv;
using namespace perf;
using std::tr1::make_tuple;
using std::tr1::get;

typedef TestBaseWithParam<size_t> FeaturesFinderVec;
typedef TestBaseWithParam<string> match;
typedef std::tr1::tuple<string, int> matchVector_t;
typedef TestBaseWithParam<matchVector_t> matchVector;

#define NUMBER_IMAGES testing::Values(1, 5, 20)
#define SURF_MATCH_CONFIDENCE 0.65f
#define ORB_MATCH_CONFIDENCE  0.3f
#define WORK_MEGAPIX 0.6

#ifdef HAVE_OPENCV_XFEATURES2D
#define TEST_DETECTORS testing::Values("surf", "orb")
#else
#define TEST_DETECTORS testing::Values<string>("orb")
#endif

PERF_TEST_P(FeaturesFinderVec, ParallelFeaturesFinder, NUMBER_IMAGES)
{
    Mat img = imread( getDataPath("stitching/a1.png") );
    vector<Mat> imgs(GetParam(), img);
    vector<detail::ImageFeatures> features(imgs.size());

    Ptr<detail::FeaturesFinder> featuresFinder = makePtr<detail::OrbFeaturesFinder>();

    TEST_CYCLE()
    {
        (*featuresFinder)(imgs, features);
    }

    SANITY_CHECK_NOTHING();
}

PERF_TEST_P(FeaturesFinderVec, SerialFeaturesFinder, NUMBER_IMAGES)
{
    Mat img = imread( getDataPath("stitching/a1.png") );
    vector<Mat> imgs(GetParam(), img);
    vector<detail::ImageFeatures> features(imgs.size());

    Ptr<detail::FeaturesFinder> featuresFinder = makePtr<detail::OrbFeaturesFinder>();

    TEST_CYCLE()
    {
        for (size_t i = 0; i < imgs.size(); ++i)
            (*featuresFinder)(imgs[i], features[i]);
    }

    SANITY_CHECK_NOTHING();
}

PERF_TEST_P( match, bestOf2Nearest, TEST_DETECTORS)
{
    Mat img1, img1_full = imread( getDataPath("stitching/boat1.jpg") );
    Mat img2, img2_full = imread( getDataPath("stitching/boat2.jpg") );
    float scale1 = (float)std::min(1.0, sqrt(WORK_MEGAPIX * 1e6 / img1_full.total()));
    float scale2 = (float)std::min(1.0, sqrt(WORK_MEGAPIX * 1e6 / img2_full.total()));
    resize(img1_full, img1, Size(), scale1, scale1);
    resize(img2_full, img2, Size(), scale2, scale2);

    Ptr<detail::FeaturesFinder> finder;
    Ptr<detail::FeaturesMatcher> matcher;
    if (GetParam() == "surf")
    {
        finder = makePtr<detail::SurfFeaturesFinder>();
        matcher = makePtr<detail::BestOf2NearestMatcher>(false, SURF_MATCH_CONFIDENCE);
    }
    else if (GetParam() == "orb")
    {
        finder = makePtr<detail::OrbFeaturesFinder>();
        matcher = makePtr<detail::BestOf2NearestMatcher>(false, ORB_MATCH_CONFIDENCE);
    }
    else
    {
        FAIL() << "Unknown 2D features type: " << GetParam();
    }

    detail::ImageFeatures features1, features2;
    (*finder)(img1, features1);
    (*finder)(img2, features2);

    detail::MatchesInfo pairwise_matches;

    declare.in(features1.descriptors, features2.descriptors);

    while(next())
    {
        cvflann::seed_random(42);//for predictive FlannBasedMatcher
        startTimer();
        (*matcher)(features1, features2, pairwise_matches);
        stopTimer();
        matcher->collectGarbage();
    }

    Mat dist (pairwise_matches.H, Range::all(), Range(2, 3));
    Mat R (pairwise_matches.H, Range::all(), Range(0, 2));
    // separate transform matrix, use lower error on rotations
    SANITY_CHECK(dist, 1., ERROR_ABSOLUTE);
    SANITY_CHECK(R, .015, ERROR_ABSOLUTE);
}

PERF_TEST_P( matchVector, bestOf2NearestVectorFeatures, testing::Combine(
                 TEST_DETECTORS,
                 testing::Values(2, 4, 8))
             )
{
    Mat img1, img1_full = imread( getDataPath("stitching/boat1.jpg") );
    Mat img2, img2_full = imread( getDataPath("stitching/boat2.jpg") );
    float scale1 = (float)std::min(1.0, sqrt(WORK_MEGAPIX * 1e6 / img1_full.total()));
    float scale2 = (float)std::min(1.0, sqrt(WORK_MEGAPIX * 1e6 / img2_full.total()));
    resize(img1_full, img1, Size(), scale1, scale1);
    resize(img2_full, img2, Size(), scale2, scale2);

    Ptr<detail::FeaturesFinder> finder;
    Ptr<detail::FeaturesMatcher> matcher;
    string detectorName = get<0>(GetParam());
    int featuresVectorSize = get<1>(GetParam());
    if (detectorName == "surf")
    {
        finder = makePtr<detail::SurfFeaturesFinder>();
        matcher = makePtr<detail::BestOf2NearestMatcher>(false, SURF_MATCH_CONFIDENCE);
    }
    else if (detectorName == "orb")
    {
        finder = makePtr<detail::OrbFeaturesFinder>();
        matcher = makePtr<detail::BestOf2NearestMatcher>(false, ORB_MATCH_CONFIDENCE);
    }
    else
    {
        FAIL() << "Unknown 2D features type: " << get<0>(GetParam());
    }

    detail::ImageFeatures features1, features2;
    (*finder)(img1, features1);
    (*finder)(img2, features2);
    vector<detail::ImageFeatures> features;
    vector<detail::MatchesInfo> pairwise_matches;
    for(int i = 0; i < featuresVectorSize/2; i++)
    {
        features.push_back(features1);
        features.push_back(features2);
    }

    declare.time(200);
    while(next())
    {
        cvflann::seed_random(42);//for predictive FlannBasedMatcher
        startTimer();
        (*matcher)(features, pairwise_matches);
        stopTimer();
        matcher->collectGarbage();
    }

    size_t matches_count = 0;
    for (size_t i = 0; i < pairwise_matches.size(); ++i)
    {
        if (pairwise_matches[i].src_img_idx < 0)
            continue;

        EXPECT_GT(pairwise_matches[i].matches.size(), 100);
        EXPECT_FALSE(pairwise_matches[i].H.empty());
        ++matches_count;
    }

    EXPECT_GT(matches_count, 0u);

    SANITY_CHECK_NOTHING();
}

PERF_TEST_P( match, affineBestOf2Nearest, TEST_DETECTORS)
{
    Mat img1, img1_full = imread( getDataPath("stitching/s1.jpg") );
    Mat img2, img2_full = imread( getDataPath("stitching/s2.jpg") );
    float scale1 = (float)std::min(1.0, sqrt(WORK_MEGAPIX * 1e6 / img1_full.total()));
    float scale2 = (float)std::min(1.0, sqrt(WORK_MEGAPIX * 1e6 / img2_full.total()));
    resize(img1_full, img1, Size(), scale1, scale1);
    resize(img2_full, img2, Size(), scale2, scale2);

    Ptr<detail::FeaturesFinder> finder;
    Ptr<detail::FeaturesMatcher> matcher;
    if (GetParam() == "surf")
    {
        finder = makePtr<detail::SurfFeaturesFinder>();
        matcher = makePtr<detail::AffineBestOf2NearestMatcher>(false, false, SURF_MATCH_CONFIDENCE);
    }
    else if (GetParam() == "orb")
    {
        finder = makePtr<detail::OrbFeaturesFinder>();
        matcher = makePtr<detail::AffineBestOf2NearestMatcher>(false, false, ORB_MATCH_CONFIDENCE);
    }
    else
    {
        FAIL() << "Unknown 2D features type: " << GetParam();
    }

    detail::ImageFeatures features1, features2;
    (*finder)(img1, features1);
    (*finder)(img2, features2);

    detail::MatchesInfo pairwise_matches;

    declare.in(features1.descriptors, features2.descriptors);

    while(next())
    {
        cvflann::seed_random(42);//for predictive FlannBasedMatcher
        startTimer();
        (*matcher)(features1, features2, pairwise_matches);
        stopTimer();
        matcher->collectGarbage();
    }

    // separate rotation and translation in transform matrix
    Mat T (pairwise_matches.H, Range(0, 2), Range(2, 3));
    Mat R (pairwise_matches.H, Range(0, 2), Range(0, 2));
    Mat h (pairwise_matches.H, Range(2, 3), Range::all());
    SANITY_CHECK(T, 5, ERROR_ABSOLUTE); // allow 5 pixels diff in translations
    SANITY_CHECK(R, .01, ERROR_ABSOLUTE); // rotations must be more precise
    // last row should be precisely (0, 0, 1) as it is just added for representation in homogeneous
    // coordinates
    EXPECT_DOUBLE_EQ(h.at<double>(0), 0.);
    EXPECT_DOUBLE_EQ(h.at<double>(1), 0.);
    EXPECT_DOUBLE_EQ(h.at<double>(2), 1.);
}

PERF_TEST_P( matchVector, affineBestOf2NearestVectorFeatures, testing::Combine(
                 TEST_DETECTORS,
                 testing::Values(2, 4, 8))
             )
{
    Mat img1, img1_full = imread( getDataPath("stitching/s1.jpg") );
    Mat img2, img2_full = imread( getDataPath("stitching/s2.jpg") );
    float scale1 = (float)std::min(1.0, sqrt(WORK_MEGAPIX * 1e6 / img1_full.total()));
    float scale2 = (float)std::min(1.0, sqrt(WORK_MEGAPIX * 1e6 / img2_full.total()));
    resize(img1_full, img1, Size(), scale1, scale1);
    resize(img2_full, img2, Size(), scale2, scale2);

    Ptr<detail::FeaturesFinder> finder;
    Ptr<detail::FeaturesMatcher> matcher;
    string detectorName = get<0>(GetParam());
    int featuresVectorSize = get<1>(GetParam());
    if (detectorName == "surf")
    {
        finder = makePtr<detail::SurfFeaturesFinder>();
        matcher = makePtr<detail::AffineBestOf2NearestMatcher>(false, false, SURF_MATCH_CONFIDENCE);
    }
    else if (detectorName == "orb")
    {
        finder = makePtr<detail::OrbFeaturesFinder>();
        matcher = makePtr<detail::AffineBestOf2NearestMatcher>(false, false, ORB_MATCH_CONFIDENCE);
    }
    else
    {
        FAIL() << "Unknown 2D features type: " << get<0>(GetParam());
    }

    detail::ImageFeatures features1, features2;
    (*finder)(img1, features1);
    (*finder)(img2, features2);
    vector<detail::ImageFeatures> features;
    vector<detail::MatchesInfo> pairwise_matches;
    for(int i = 0; i < featuresVectorSize/2; i++)
    {
        features.push_back(features1);
        features.push_back(features2);
    }

    declare.time(200);
    while(next())
    {
        cvflann::seed_random(42);//for predictive FlannBasedMatcher
        startTimer();
        (*matcher)(features, pairwise_matches);
        stopTimer();
        matcher->collectGarbage();
    }

    size_t matches_count = 0;
    for (size_t i = 0; i < pairwise_matches.size(); ++i)
    {
        if (pairwise_matches[i].src_img_idx < 0)
            continue;

        EXPECT_TRUE(pairwise_matches[i].matches.size() > 400);
        EXPECT_FALSE(pairwise_matches[i].H.empty());
        ++matches_count;
    }

    EXPECT_TRUE(matches_count > 0);

    SANITY_CHECK_NOTHING();
}
<|endoftext|>


<commit_before>#include "command.h"
#include "image.h"
#include "algo/loop.h"
#include "math/math.h"

#include "transform.h"
#include <Eigen/Geometry>
#include <Eigen/SVD>

// #include "registration/transform/compose.h"

using namespace MR;
using namespace App;

const char* space_choices[] = { "voxel", "image1", "image2", "average", NULL };

void usage ()
{
  AUTHOR = "Maximilian Pietsch (maximilian.pietsch@kcl.ac.uk)";

  DESCRIPTION
  + "computes the dissimiliarty metric between two transformations. Currently only affine transformations and the mean absolute displacement metric are implemented";

  ARGUMENTS
  + Argument ("image",
        "the image that defines the space over which the "
        "dissimilarity is measured.").type_image_in ();

  OPTIONS
  + Option ("linear",
        "specify a 4x4 linear transform to apply, in the form "
        "of a 4x4 ascii file. Note the standard 'reverse' convention "
        "is used, where the transform maps points in the template image "
        "to the moving image.")
    +   Argument ("transform").type_file_in ()

  + Option ("linear_inverse",
         "invert linear transformation")

  + Option ("linear2",
        "TODO: compare transformations")
    +   Argument ("transform").type_file_in ()

  + Option ("template",
        "use the voxel to scanner transformation of the template image instead "
        "of the linear transformation. ")
    +   Argument ("transformation").type_image_in()

  + Option ("voxel",
         "normalise dissimiliarity to voxel size of image")

  + Option ("norm",
         "display the norm of the displacement.");

}

typedef double value_type;

void run ()
{
  auto image = Image<value_type>::open (argument[0]);
  auto trafo = Transform(image.original_header()); // #.transform();
  // auto voxel2scanner = trafo.voxel2scanner();
  auto cost = Eigen::Matrix<value_type, 3, 1>(0,0,0) ;
  size_t n_voxels = image.size(0) * image.size(1) * image.size(2);

  bool do_linear = false;
  transform_type affine_trafo;
  auto opt = get_options ("linear");
  if (opt.size()){
    do_linear = true;
    affine_trafo = load_transform<value_type> (opt[0][0]);
  }
  opt = get_options ("template");
  if (opt.size()){
    if (do_linear) throw Exception ("linear transformation and template image provided ");
    do_linear = true;
    auto H = Header::open(opt[0][0]);
    // check_dimensions (image, H);
    // affine_trafo = Transform(H).voxel2scanner;
    throw Exception ("template image not yet implemented");
  }

  transform_type reference_trafo;
  reference_trafo.setIdentity();
  opt = get_options ("linear2");
  if (opt.size())
    reference_trafo = load_transform<value_type> (opt[0][0]);

  bool normalise = get_options ("normalise").size();
  bool norm = get_options ("norm").size();

  if (!do_linear)
    throw Exception ("TODO: only linear transformation implemented but no linear transformation provided.");

  Eigen::Matrix<value_type, 3, 1> pos;
  Eigen::Matrix<value_type, 3, 1> displacement;

  reference_trafo = reference_trafo * trafo.voxel2scanner;
  if (get_options ("linear_inverse").size())
    affine_trafo = affine_trafo.inverse();
  auto trafo2 = affine_trafo * trafo.voxel2scanner;
  DEBUG("reference voxel2scanner transformation:\n" + str (reference_trafo.matrix()));
  DEBUG("voxel2scanner transformation:\n" + str (trafo2.matrix()));

  for (auto i = Loop() (image); i ;++i){
    pos << image.index(0), image.index(1), image.index(2);
    displacement = trafo2 * pos - reference_trafo * pos;
    cost += displacement.cwiseAbs();
  }

  cost /= static_cast<value_type>(n_voxels);
  if (normalise){
    cost[0] /= static_cast<value_type>(image.spacing(0));
    cost[1] /= static_cast<value_type>(image.spacing(1));
    cost[2] /= static_cast<value_type>(image.spacing(2));
  }
  if (norm)
    std::cout << cost.norm() << std::endl;
  else
    std::cout << cost[0] << " " << cost[1] << " " << cost[2] << std::endl;
}

<commit_msg>transformmetric: added option: cumulative_absolute<commit_after>#include "command.h"
#include "image.h"
#include "algo/loop.h"
#include "math/math.h"

#include "transform.h"
#include <Eigen/Geometry>
#include <Eigen/SVD>

// #include "registration/transform/compose.h"

using namespace MR;
using namespace App;

const char* space_choices[] = { "voxel", "image1", "image2", "average", NULL };

void usage ()
{
  AUTHOR = "Maximilian Pietsch (maximilian.pietsch@kcl.ac.uk)";

  DESCRIPTION
  + "computes the dissimiliarty metric between two transformations. Currently only affine transformations and the mean absolute displacement metric are implemented";

  ARGUMENTS
  + Argument ("image",
        "the image that defines the space over which the "
        "dissimilarity is measured.").type_image_in ();

  OPTIONS
  + Option ("linear",
        "specify a 4x4 linear transform to apply, in the form "
        "of a 4x4 ascii file. Note the standard 'reverse' convention "
        "is used, where the transform maps points in the template image "
        "to the moving image.")
    +   Argument ("transform").type_file_in ()

  + Option ("linear_inverse",
         "invert linear transformation")

  + Option ("linear2",
        "TODO: compare transformations")
    +   Argument ("transform").type_file_in ()

  + Option ("template",
        "use the voxel to scanner transformation of the template image instead "
        "of the linear transformation. ")
    +   Argument ("transformation").type_image_in()

  + Option ("voxel",
         "normalise dissimiliarity to voxel size of image")

  + Option ("cumulative_absolute",
          "sum the absolute of each voxel position displacement")

  + Option ("norm",
         "display the norm of the displacement.");

}

typedef double value_type;

void run ()
{
  auto image = Image<value_type>::open (argument[0]);
  auto trafo = Transform(image.original_header()); // #.transform();
  // auto voxel2scanner = trafo.voxel2scanner();
  auto cost = Eigen::Matrix<value_type, 3, 1>(0,0,0) ;
  size_t n_voxels = image.size(0) * image.size(1) * image.size(2);

  bool do_linear = false;
  transform_type affine_trafo;
  auto opt = get_options ("linear");
  if (opt.size()){
    do_linear = true;
    affine_trafo = load_transform<value_type> (opt[0][0]);
  }
  opt = get_options ("template");
  if (opt.size()){
    if (do_linear) throw Exception ("linear transformation and template image provided ");
    do_linear = true;
    auto H = Header::open(opt[0][0]);
    // check_dimensions (image, H);
    // affine_trafo = Transform(H).voxel2scanner;
    throw Exception ("template image not yet implemented");
  }

  transform_type reference_trafo;
  reference_trafo.setIdentity();
  opt = get_options ("linear2");
  if (opt.size())
    reference_trafo = load_transform<value_type> (opt[0][0]);

  bool cumulative_absolute = get_options ("cumulative_absolute").size();
  bool normalise = get_options ("normalise").size();
  bool norm = get_options ("norm").size();

  if (!do_linear)
    throw Exception ("TODO: only linear transformation implemented but no linear transformation provided.");

  Eigen::Matrix<value_type, 3, 1> pos;
  Eigen::Matrix<value_type, 3, 1> displacement;

  reference_trafo = reference_trafo * trafo.voxel2scanner;
  if (get_options ("linear_inverse").size())
    affine_trafo = affine_trafo.inverse();
  auto trafo2 = affine_trafo * trafo.voxel2scanner;
  DEBUG("reference voxel2scanner transformation:\n" + str (reference_trafo.matrix()));
  DEBUG("voxel2scanner transformation:\n" + str (trafo2.matrix()));

  for (auto i = Loop() (image); i ;++i){
    pos << image.index(0), image.index(1), image.index(2);
    displacement = trafo2 * pos - reference_trafo * pos;
    if (cumulative_absolute)
      cost += displacement.cwiseAbs();
    else
      cost += displacement;
  }

  cost /= static_cast<value_type>(n_voxels);
  if (normalise){
    cost[0] /= static_cast<value_type>(image.spacing(0));
    cost[1] /= static_cast<value_type>(image.spacing(1));
    cost[2] /= static_cast<value_type>(image.spacing(2));
  }
  if (norm)
    std::cout << cost.norm() << std::endl;
  else
    std::cout << cost[0] << " " << cost[1] << " " << cost[2] << std::endl;
}

<|endoftext|>


<commit_before>/*
 *  yosys -- Yosys Open SYnthesis Suite
 *
 *  Copyright (C) 2012  Clifford Wolf <clifford@clifford.at>
 *
 *  Permission to use, copy, modify, and/or distribute this software for any
 *  purpose with or without fee is hereby granted, provided that the above
 *  copyright notice and this permission notice appear in all copies.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 */

#include "kernel/register.h"
#include "kernel/celltypes.h"
#include "kernel/sigtools.h"
#include "kernel/rtlil.h"
#include "kernel/log.h"

USING_YOSYS_NAMESPACE
PRIVATE_NAMESPACE_BEGIN

struct SetundefWorker
{
	int next_bit_mode;
	uint32_t next_bit_state;
	vector<SigSpec*> siglist;

	RTLIL::State next_bit()
	{
		if (next_bit_mode == 0)
			return RTLIL::State::S0;

		if (next_bit_mode == 1)
			return RTLIL::State::S1;

		if (next_bit_mode == 4)
			return RTLIL::State::Sx;

		// xorshift32
		next_bit_state ^= next_bit_state << 13;
		next_bit_state ^= next_bit_state >> 17;
		next_bit_state ^= next_bit_state << 5;
		log_assert(next_bit_state != 0);

		return ((next_bit_state >> (next_bit_state & 15)) & 16) ? RTLIL::State::S0 : RTLIL::State::S1;
	}

	void operator()(RTLIL::SigSpec &sig)
	{
		if (next_bit_mode == 2) {
			siglist.push_back(&sig);
			return;
		}

		for (auto &bit : sig)
			if (bit.wire == NULL && bit.data > RTLIL::State::S1)
				bit = next_bit();
	}
};

struct SetundefPass : public Pass {
	SetundefPass() : Pass("setundef", "replace undef values with defined constants") { }
	virtual void help()
	{
		//   |---v---|---v---|---v---|---v---|---v---|---v---|---v---|---v---|---v---|---v---|
		log("\n");
		log("    setundef [options] [selection]\n");
		log("\n");
		log("This command replaces undef (x) constants with defined (0/1) constants.\n");
		log("\n");
		log("    -undriven\n");
		log("        also set undriven nets to constant values\n");
		log("\n");
		log("    -zero\n");
		log("        replace with bits cleared (0)\n");
		log("\n");
		log("    -one\n");
		log("        replace with bits set (1)\n");
		log("\n");
		log("    -undef\n");
		log("        replace with undef (x) bits, may be used with -undriven\n");
		log("\n");
		log("    -anyseq\n");
		log("        replace with $anyseq drivers (for formal)\n");
		log("\n");
		log("    -random <seed>\n");
		log("        replace with random bits using the specified integer als seed\n");
		log("        value for the random number generator.\n");
		log("\n");
		log("    -init\n");
		log("        also create/update init values for flip-flops\n");
		log("\n");
	}
	virtual void execute(std::vector<std::string> args, RTLIL::Design *design)
	{
		bool got_value = false;
		bool undriven_mode = false;
		bool init_mode = false;
		SetundefWorker worker;

		log_header(design, "Executing SETUNDEF pass (replace undef values with defined constants).\n");

		size_t argidx;
		for (argidx = 1; argidx < args.size(); argidx++)
		{
			if (args[argidx] == "-undriven") {
				undriven_mode = true;
				continue;
			}
			if (args[argidx] == "-zero") {
				got_value = true;
				worker.next_bit_mode = 0;
				continue;
			}
			if (args[argidx] == "-one") {
				got_value = true;
				worker.next_bit_mode = 1;
				continue;
			}
			if (args[argidx] == "-anyseq") {
				got_value = true;
				worker.next_bit_mode = 2;
				continue;
			}
			if (args[argidx] == "-undef") {
				got_value = true;
				worker.next_bit_mode = 4;
				continue;
			}
			if (args[argidx] == "-init") {
				init_mode = true;
				continue;
			}
			if (args[argidx] == "-random" && !got_value && argidx+1 < args.size()) {
				got_value = true;
				worker.next_bit_mode = 3;
				worker.next_bit_state = atoi(args[++argidx].c_str()) + 1;
				for (int i = 0; i < 10; i++)
					worker.next_bit();
				continue;
			}
			break;
		}
		extra_args(args, argidx, design);

		if (!got_value)
			log_cmd_error("One of the options -zero, -one, -anyseq, or -random <seed> must be specified.\n");

		for (auto module : design->selected_modules())
		{
			if (undriven_mode)
			{
				if (!module->processes.empty())
					log_error("The 'setundef' command can't operate in -undriven mode on modules with processes. Run 'proc' first.\n");

				SigMap sigmap(module);
				SigPool undriven_signals;

				for (auto &it : module->wires_)
					undriven_signals.add(sigmap(it.second));

				for (auto &it : module->wires_)
					if (it.second->port_input)
						undriven_signals.del(sigmap(it.second));

				CellTypes ct(design);
				for (auto &it : module->cells_)
				for (auto &conn : it.second->connections())
					if (!ct.cell_known(it.second->type) || ct.cell_output(it.second->type, conn.first))
						undriven_signals.del(sigmap(conn.second));

				RTLIL::SigSpec sig = undriven_signals.export_all();
				for (auto &c : sig.chunks()) {
					RTLIL::SigSpec bits;
					for (int i = 0; i < c.width; i++)
						bits.append(worker.next_bit());
					module->connect(RTLIL::SigSig(c, bits));
				}
			}

			if (init_mode)
			{
				SigMap sigmap(module);
				pool<SigBit> ffbits;
				pool<Wire*> initwires;

				pool<IdString> fftypes;
				fftypes.insert("$dff");
				fftypes.insert("$dffe");
				fftypes.insert("$dffsr");
				fftypes.insert("$adff");

				std::vector<char> list_np = {'N', 'P'}, list_01 = {'0', '1'};

				for (auto c1 : list_np)
					fftypes.insert(stringf("$_DFF_%c_", c1));

				for (auto c1 : list_np)
				for (auto c2 : list_np)
					fftypes.insert(stringf("$_DFFE_%c%c_", c1, c2));

				for (auto c1 : list_np)
				for (auto c2 : list_np)
				for (auto c3 : list_01)
					fftypes.insert(stringf("$_DFF_%c%c%c_", c1, c2, c3));

				for (auto c1 : list_np)
				for (auto c2 : list_np)
				for (auto c3 : list_np)
					fftypes.insert(stringf("$_DFFSR_%c%c%c_", c1, c2, c3));

				for (auto cell : module->cells())
				{
					if (!fftypes.count(cell->type))
						continue;

					for (auto bit : sigmap(cell->getPort("\\Q")))
						ffbits.insert(bit);
				}

				for (auto wire : module->wires())
				{
					if (!wire->attributes.count("\\init"))
						continue;

					for (auto bit : sigmap(wire))
						ffbits.erase(bit);

					initwires.insert(wire);
				}

				for (int wire_types = 0; wire_types < 2; wire_types++)
					for (auto wire : module->wires())
					{
						if (wire->name[0] == (wire_types ? '\\' : '$'))
					next_wire:
							continue;

						for (auto bit : sigmap(wire))
							if (!ffbits.count(bit))
								goto next_wire;

						for (auto bit : sigmap(wire))
							ffbits.erase(bit);

						initwires.insert(wire);
					}

				for (auto wire : initwires)
				{
					Const &initval = wire->attributes["\\init"];

					for (int i = 0; i < GetSize(wire); i++)
						if (GetSize(initval) <= i)
							initval.bits.push_back(worker.next_bit());
						else if (initval.bits[i] == State::Sx)
							initval.bits[i] = worker.next_bit();
				}
			}

			module->rewrite_sigspecs(worker);

			if (worker.next_bit_mode == 2)
			{
				vector<SigSpec*> siglist;
				siglist.swap(worker.siglist);

				for (auto sigptr : siglist)
				{
					SigSpec &sig = *sigptr;
					int cursor = 0;

					while (cursor < GetSize(sig))
					{
						int width = 0;
						while (cursor+width < GetSize(sig) && sig[cursor+width] == State::Sx)
							width++;

						if (width > 0) {
							sig.replace(cursor, module->Anyseq(NEW_ID, width));
							cursor += width;
						} else {
							cursor++;
						}
					}
				}
			}
		}
	}
} SetundefPass;

PRIVATE_NAMESPACE_END
<commit_msg>Some cleanups in setundef.cc<commit_after>/*
 *  yosys -- Yosys Open SYnthesis Suite
 *
 *  Copyright (C) 2012  Clifford Wolf <clifford@clifford.at>
 *
 *  Permission to use, copy, modify, and/or distribute this software for any
 *  purpose with or without fee is hereby granted, provided that the above
 *  copyright notice and this permission notice appear in all copies.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 */

#include "kernel/register.h"
#include "kernel/celltypes.h"
#include "kernel/sigtools.h"
#include "kernel/rtlil.h"
#include "kernel/log.h"

USING_YOSYS_NAMESPACE
PRIVATE_NAMESPACE_BEGIN

struct SetundefWorker
{
	int next_bit_mode;
	uint32_t next_bit_state;
	vector<SigSpec*> siglist;

	RTLIL::State next_bit()
	{
		if (next_bit_mode == 0)
			return RTLIL::State::S0;

		if (next_bit_mode == 1)
			return RTLIL::State::S1;

		if (next_bit_mode == 2)
			log_abort();

		if (next_bit_mode == 4)
			return RTLIL::State::Sx;

		// xorshift32
		next_bit_state ^= next_bit_state << 13;
		next_bit_state ^= next_bit_state >> 17;
		next_bit_state ^= next_bit_state << 5;
		log_assert(next_bit_state != 0);

		return ((next_bit_state >> (next_bit_state & 15)) & 16) ? RTLIL::State::S0 : RTLIL::State::S1;
	}

	void operator()(RTLIL::SigSpec &sig)
	{
		if (next_bit_mode == 2) {
			siglist.push_back(&sig);
			return;
		}

		for (auto &bit : sig)
			if (bit.wire == NULL && bit.data > RTLIL::State::S1)
				bit = next_bit();
	}
};

struct SetundefPass : public Pass {
	SetundefPass() : Pass("setundef", "replace undef values with defined constants") { }
	virtual void help()
	{
		//   |---v---|---v---|---v---|---v---|---v---|---v---|---v---|---v---|---v---|---v---|
		log("\n");
		log("    setundef [options] [selection]\n");
		log("\n");
		log("This command replaces undef (x) constants with defined (0/1) constants.\n");
		log("\n");
		log("    -undriven\n");
		log("        also set undriven nets to constant values\n");
		log("\n");
		log("    -zero\n");
		log("        replace with bits cleared (0)\n");
		log("\n");
		log("    -one\n");
		log("        replace with bits set (1)\n");
		log("\n");
		log("    -undef\n");
		log("        replace with undef (x) bits, may be used with -undriven\n");
		log("\n");
		log("    -anyseq\n");
		log("        replace with $anyseq drivers (for formal)\n");
		log("\n");
		log("    -random <seed>\n");
		log("        replace with random bits using the specified integer als seed\n");
		log("        value for the random number generator.\n");
		log("\n");
		log("    -init\n");
		log("        also create/update init values for flip-flops\n");
		log("\n");
	}
	virtual void execute(std::vector<std::string> args, RTLIL::Design *design)
	{
		bool got_value = false;
		bool undriven_mode = false;
		bool init_mode = false;
		SetundefWorker worker;

		log_header(design, "Executing SETUNDEF pass (replace undef values with defined constants).\n");

		size_t argidx;
		for (argidx = 1; argidx < args.size(); argidx++)
		{
			if (args[argidx] == "-undriven") {
				undriven_mode = true;
				continue;
			}
			if (args[argidx] == "-zero") {
				got_value = true;
				worker.next_bit_mode = 0;
				worker.next_bit_state = 0;
				continue;
			}
			if (args[argidx] == "-one") {
				got_value = true;
				worker.next_bit_mode = 1;
				worker.next_bit_state = 0;
				continue;
			}
			if (args[argidx] == "-anyseq") {
				got_value = true;
				worker.next_bit_mode = 2;
				worker.next_bit_state = 0;
				continue;
			}
			if (args[argidx] == "-undef") {
				got_value = true;
				worker.next_bit_mode = 4;
				worker.next_bit_state = 0;
				continue;
			}
			if (args[argidx] == "-init") {
				init_mode = true;
				continue;
			}
			if (args[argidx] == "-random" && !got_value && argidx+1 < args.size()) {
				got_value = true;
				worker.next_bit_mode = 3;
				worker.next_bit_state = atoi(args[++argidx].c_str()) + 1;
				for (int i = 0; i < 10; i++)
					worker.next_bit();
				continue;
			}
			break;
		}
		extra_args(args, argidx, design);

		if (!got_value)
			log_cmd_error("One of the options -zero, -one, -anyseq, or -random <seed> must be specified.\n");

		for (auto module : design->selected_modules())
		{
			if (undriven_mode)
			{
				if (!module->processes.empty())
					log_error("The 'setundef' command can't operate in -undriven mode on modules with processes. Run 'proc' first.\n");

				SigMap sigmap(module);
				SigPool undriven_signals;

				for (auto &it : module->wires_)
					undriven_signals.add(sigmap(it.second));

				for (auto &it : module->wires_)
					if (it.second->port_input)
						undriven_signals.del(sigmap(it.second));

				CellTypes ct(design);
				for (auto &it : module->cells_)
				for (auto &conn : it.second->connections())
					if (!ct.cell_known(it.second->type) || ct.cell_output(it.second->type, conn.first))
						undriven_signals.del(sigmap(conn.second));

				RTLIL::SigSpec sig = undriven_signals.export_all();
				for (auto &c : sig.chunks()) {
					RTLIL::SigSpec bits;
					for (int i = 0; i < c.width; i++)
						bits.append(worker.next_bit());
					module->connect(RTLIL::SigSig(c, bits));
				}
			}

			if (init_mode)
			{
				SigMap sigmap(module);
				pool<SigBit> ffbits;
				pool<Wire*> initwires;

				pool<IdString> fftypes;
				fftypes.insert("$dff");
				fftypes.insert("$dffe");
				fftypes.insert("$dffsr");
				fftypes.insert("$adff");

				std::vector<char> list_np = {'N', 'P'}, list_01 = {'0', '1'};

				for (auto c1 : list_np)
					fftypes.insert(stringf("$_DFF_%c_", c1));

				for (auto c1 : list_np)
				for (auto c2 : list_np)
					fftypes.insert(stringf("$_DFFE_%c%c_", c1, c2));

				for (auto c1 : list_np)
				for (auto c2 : list_np)
				for (auto c3 : list_01)
					fftypes.insert(stringf("$_DFF_%c%c%c_", c1, c2, c3));

				for (auto c1 : list_np)
				for (auto c2 : list_np)
				for (auto c3 : list_np)
					fftypes.insert(stringf("$_DFFSR_%c%c%c_", c1, c2, c3));

				for (auto cell : module->cells())
				{
					if (!fftypes.count(cell->type))
						continue;

					for (auto bit : sigmap(cell->getPort("\\Q")))
						ffbits.insert(bit);
				}

				for (auto wire : module->wires())
				{
					if (!wire->attributes.count("\\init"))
						continue;

					for (auto bit : sigmap(wire))
						ffbits.erase(bit);

					initwires.insert(wire);
				}

				for (int wire_types = 0; wire_types < 2; wire_types++)
					for (auto wire : module->wires())
					{
						if (wire->name[0] == (wire_types ? '\\' : '$'))
					next_wire:
							continue;

						for (auto bit : sigmap(wire))
							if (!ffbits.count(bit))
								goto next_wire;

						for (auto bit : sigmap(wire))
							ffbits.erase(bit);

						initwires.insert(wire);
					}

				for (auto wire : initwires)
				{
					Const &initval = wire->attributes["\\init"];

					for (int i = 0; i < GetSize(wire); i++)
						if (GetSize(initval) <= i)
							initval.bits.push_back(worker.next_bit());
						else if (initval.bits[i] == State::Sx)
							initval.bits[i] = worker.next_bit();
				}
			}

			module->rewrite_sigspecs(worker);

			if (worker.next_bit_mode == 2)
			{
				vector<SigSpec*> siglist;
				siglist.swap(worker.siglist);

				for (auto sigptr : siglist)
				{
					SigSpec &sig = *sigptr;
					int cursor = 0;

					while (cursor < GetSize(sig))
					{
						int width = 0;
						while (cursor+width < GetSize(sig) && sig[cursor+width] == State::Sx)
							width++;

						if (width > 0) {
							sig.replace(cursor, module->Anyseq(NEW_ID, width));
							cursor += width;
						} else {
							cursor++;
						}
					}
				}
			}
		}
	}
} SetundefPass;

PRIVATE_NAMESPACE_END
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2007 The Hewlett-Packard Development Company
 * All rights reserved.
 *
 * Redistribution and use of this software in source and binary forms,
 * with or without modification, are permitted provided that the
 * following conditions are met:
 *
 * The software must be used only for Non-Commercial Use which means any
 * use which is NOT directed to receiving any direct monetary
 * compensation for, or commercial advantage from such use.  Illustrative
 * examples of non-commercial use are academic research, personal study,
 * teaching, education and corporate research & development.
 * Illustrative examples of commercial use are distributing products for
 * commercial advantage and providing services using the software for
 * commercial advantage.
 *
 * If you wish to use this software or functionality therein that may be
 * covered by patents for commercial use, please contact:
 *     Director of Intellectual Property Licensing
 *     Office of Strategy and Technology
 *     Hewlett-Packard Company
 *     1501 Page Mill Road
 *     Palo Alto, California  94304
 *
 * Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.  Redistributions
 * in binary form must reproduce the above copyright notice, this list of
 * conditions and the following disclaimer in the documentation and/or
 * other materials provided with the distribution.  Neither the name of
 * the COPYRIGHT HOLDER(s), HEWLETT-PACKARD COMPANY, nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.  No right of
 * sublicense is granted herewith.  Derivatives of the software and
 * output created using the software may be prepared, but only for
 * Non-Commercial Uses.  Derivatives of the software may be shared with
 * others provided: (i) the others agree to abide by the list of
 * conditions herein which includes the Non-Commercial Use restrictions;
 * and (ii) such Derivatives of the software include the above copyright
 * notice to acknowledge the contribution from this software where
 * applicable, this list of conditions and the disclaimer below.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER 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.
 *
 * Authors: Gabe Black
 */

#include "arch/x86/insts/microregop.hh"
#include "arch/x86/miscregs.hh"
#include "base/condcodes.hh"
#include <string>

namespace X86ISA
{
    uint64_t RegOpBase::genFlags(uint64_t oldFlags, uint64_t flagMask,
            uint64_t _dest, uint64_t _src1, uint64_t _src2,
            bool subtract) const
    {
        DPRINTF(X86, "flagMask = %#x\n", flagMask);
        uint64_t flags = oldFlags & ~flagMask;
        if(flagMask & (ECFBit | CFBit))
        {
            if(findCarry(dataSize*8, _dest, _src1, _src2))
                flags |= (flagMask & (ECFBit | CFBit));
            if(subtract)
                flags ^= (flagMask & (ECFBit | CFBit));
        }
        if(flagMask & PFBit && findParity(dataSize*8, _dest))
            flags |= PFBit;
        if(flagMask & AFBit)
        {
            if(findCarry(4, _dest, _src1, _src2))
                flags |= AFBit;
            if(subtract)
                flags ^= AFBit;
        }
        if(flagMask & (EZFBit | ZFBit) && findZero(dataSize*8, _dest))
            flags |= (flagMask & (EZFBit | ZFBit));
        if(flagMask & SFBit && findNegative(dataSize*8, _dest))
            flags |= SFBit;
        if(flagMask & OFBit && findOverflow(dataSize*8, _dest, _src1, _src2))
            flags |= OFBit;
        return flags;
    }

    std::string RegOp::generateDisassembly(Addr pc,
            const SymbolTable *symtab) const
    {
        std::stringstream response;

        printMnemonic(response, instMnem, mnemonic);
        printDestReg(response, 0, dataSize);
        response << ", ";
        printSrcReg(response, 0, dataSize);
        response << ", ";
        printSrcReg(response, 1, dataSize);
        return response.str();
    }

    std::string RegOpImm::generateDisassembly(Addr pc,
            const SymbolTable *symtab) const
    {
        std::stringstream response;

        printMnemonic(response, instMnem, mnemonic);
        printDestReg(response, 0, dataSize);
        response << ", ";
        printSrcReg(response, 0, dataSize);
        ccprintf(response, ", %#x", imm8);
        return response.str();
    }
}
<commit_msg>X86: Calculate flags based on the actual result.<commit_after>/*
 * Copyright (c) 2007 The Hewlett-Packard Development Company
 * All rights reserved.
 *
 * Redistribution and use of this software in source and binary forms,
 * with or without modification, are permitted provided that the
 * following conditions are met:
 *
 * The software must be used only for Non-Commercial Use which means any
 * use which is NOT directed to receiving any direct monetary
 * compensation for, or commercial advantage from such use.  Illustrative
 * examples of non-commercial use are academic research, personal study,
 * teaching, education and corporate research & development.
 * Illustrative examples of commercial use are distributing products for
 * commercial advantage and providing services using the software for
 * commercial advantage.
 *
 * If you wish to use this software or functionality therein that may be
 * covered by patents for commercial use, please contact:
 *     Director of Intellectual Property Licensing
 *     Office of Strategy and Technology
 *     Hewlett-Packard Company
 *     1501 Page Mill Road
 *     Palo Alto, California  94304
 *
 * Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.  Redistributions
 * in binary form must reproduce the above copyright notice, this list of
 * conditions and the following disclaimer in the documentation and/or
 * other materials provided with the distribution.  Neither the name of
 * the COPYRIGHT HOLDER(s), HEWLETT-PACKARD COMPANY, nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.  No right of
 * sublicense is granted herewith.  Derivatives of the software and
 * output created using the software may be prepared, but only for
 * Non-Commercial Uses.  Derivatives of the software may be shared with
 * others provided: (i) the others agree to abide by the list of
 * conditions herein which includes the Non-Commercial Use restrictions;
 * and (ii) such Derivatives of the software include the above copyright
 * notice to acknowledge the contribution from this software where
 * applicable, this list of conditions and the disclaimer below.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER 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.
 *
 * Authors: Gabe Black
 */

#include "arch/x86/insts/microregop.hh"
#include "arch/x86/miscregs.hh"
#include "base/condcodes.hh"
#include <string>

namespace X86ISA
{
    uint64_t RegOpBase::genFlags(uint64_t oldFlags, uint64_t flagMask,
            uint64_t _dest, uint64_t _src1, uint64_t _src2,
            bool subtract) const
    {
        DPRINTF(X86, "flagMask = %#x\n", flagMask);
        if (_destRegIdx[0] & (1 << 6)) {
            _dest >>= 8;
        }
        uint64_t flags = oldFlags & ~flagMask;
        if(flagMask & (ECFBit | CFBit))
        {
            if(findCarry(dataSize*8, _dest, _src1, _src2))
                flags |= (flagMask & (ECFBit | CFBit));
            if(subtract)
                flags ^= (flagMask & (ECFBit | CFBit));
        }
        if(flagMask & PFBit && findParity(dataSize*8, _dest))
            flags |= PFBit;
        if(flagMask & AFBit)
        {
            if(findCarry(4, _dest, _src1, _src2))
                flags |= AFBit;
            if(subtract)
                flags ^= AFBit;
        }
        if(flagMask & (EZFBit | ZFBit) && findZero(dataSize*8, _dest))
            flags |= (flagMask & (EZFBit | ZFBit));
        if(flagMask & SFBit && findNegative(dataSize*8, _dest))
            flags |= SFBit;
        if(flagMask & OFBit && findOverflow(dataSize*8, _dest, _src1, _src2))
            flags |= OFBit;
        return flags;
    }

    std::string RegOp::generateDisassembly(Addr pc,
            const SymbolTable *symtab) const
    {
        std::stringstream response;

        printMnemonic(response, instMnem, mnemonic);
        printDestReg(response, 0, dataSize);
        response << ", ";
        printSrcReg(response, 0, dataSize);
        response << ", ";
        printSrcReg(response, 1, dataSize);
        return response.str();
    }

    std::string RegOpImm::generateDisassembly(Addr pc,
            const SymbolTable *symtab) const
    {
        std::stringstream response;

        printMnemonic(response, instMnem, mnemonic);
        printDestReg(response, 0, dataSize);
        response << ", ";
        printSrcReg(response, 0, dataSize);
        ccprintf(response, ", %#x", imm8);
        return response.str();
    }
}
<|endoftext|>


<commit_before>#ifdef __linux__

#include <codecvt>
#include <locale>
#include <string>
#include <sys/ioctl.h>
#include <unistd.h>

#include "linux_fsnotifier.h"

#define EVENT_BUFFER_SIZE (16 * 1024)

#define EVENT_MASK (IN_CREATE | IN_DELETE | IN_DELETE_SELF | IN_EXCL_UNLINK | IN_MODIFY | IN_MOVE_SELF | IN_MOVED_FROM | IN_MOVED_TO | IN_ONLYDIR)

WatchPoint::WatchPoint(const u16string& path, shared_ptr<Inotify> inotify, int watchDescriptor)
    : status(WatchPointStatus::LISTENING)
    , watchDescriptor(watchDescriptor)
    , inotify(inotify)
    , path(path) {
}

CancelResult WatchPoint::cancel() {
    if (status == WatchPointStatus::CANCELLED) {
        return CancelResult::ALREADY_CANCELLED;
    }
    status = WatchPointStatus::CANCELLED;
    if (inotify_rm_watch(inotify->fd, watchDescriptor) != 0) {
        switch (errno) {
            case EINVAL:
                logToJava(LogLevel::INFO, "Couldn't stop watching %s (probably because the directory was removed)", utf16ToUtf8String(path).c_str());
                return CancelResult::NOT_CANCELLED;
                break;
            default:
                throw FileWatcherException("Couldn't stop watching", path, errno);
        }
    }
    return CancelResult::CANCELLED;
}

Inotify::Inotify()
    : fd(inotify_init1(IN_CLOEXEC | IN_NONBLOCK)) {
    if (fd == -1) {
        throw FileWatcherException("Couldn't register inotify handle", errno);
    }
}

Inotify::~Inotify() {
    close(fd);
}

ShutdownEvent::ShutdownEvent()
    : fd(eventfd(0, 0)) {
    if (fd == -1) {
        throw FileWatcherException("Couldn't register event source", errno);
    }
}
ShutdownEvent::~ShutdownEvent() {
    close(fd);
}

void ShutdownEvent::trigger() const {
    const uint64_t increment = 1;
    write(fd, &increment, sizeof(increment));
}

void ShutdownEvent::consume() const {
    uint64_t counter;
    ssize_t bytesRead = read(fd, &counter, sizeof(counter));
    if (bytesRead == -1) {
        throw FileWatcherException("Couldn't read from termination event notifier", errno);
    }
}

Server::Server(JNIEnv* env, jobject watcherCallback)
    : AbstractServer(env, watcherCallback)
    , inotify(new Inotify()) {
    buffer.reserve(EVENT_BUFFER_SIZE);
}

void Server::initializeRunLoop() {
}

void Server::shutdownRunLoop() {
    shutdownEvent.trigger();
}

void Server::runLoop() {
    int forever = numeric_limits<int>::max();

    while (!shouldTerminate) {
        processQueues(forever);
    }

    // No need to clean up watch points, they will be cancelled
    // and closed when the Inotify destructs
}

void Server::processQueues(int timeout) {
    struct pollfd fds[2];
    fds[0].fd = shutdownEvent.fd;
    fds[1].fd = inotify->fd;
    fds[0].events = POLLIN;
    fds[1].events = POLLIN;

    int ret = poll(fds, 2, timeout);
    if (ret == -1) {
        throw FileWatcherException("Couldn't poll for events", errno);
    }

    if (IS_SET(fds[0].revents, POLLIN)) {
        shutdownEvent.consume();
        // Ignore counter, we only care about the notification itself
        shouldTerminate = true;
        return;
    }

    if (IS_SET(fds[1].revents, POLLIN)) {
        try {
            handleEvents();
        } catch (const exception& ex) {
            reportFailure(getThreadEnv(), ex);
        }
    }
}

void Server::handleEvents() {
    unsigned int available;
    ioctl(inotify->fd, FIONREAD, &available);

    while (available > 0) {
        ssize_t bytesRead = read(inotify->fd, &buffer[0], buffer.capacity());

        switch (bytesRead) {
            case -1:
                if (errno == EAGAIN) {
                    // For a non-blocking read, we receive EAGAIN here if there is nothing to read.
                    // This may happen when the inotify is already closed.
                    return;
                } else {
                    throw FileWatcherException("Couldn't read from inotify", errno);
                }
                break;
            case 0:
                throw FileWatcherException("EOF reading from inotify", errno);
                break;
            default:
                // Handle events
                unique_lock<mutex> lock(mutationMutex);
                JNIEnv* env = getThreadEnv();
                logToJava(LogLevel::FINE, "Processing %d bytes worth of events", bytesRead);
                int index = 0;
                int count = 0;
                while (index < bytesRead) {
                    const struct inotify_event* event = (struct inotify_event*) &buffer[index];
                    handleEvent(env, event);
                    index += sizeof(struct inotify_event) + event->len;
                    count++;
                }
                logToJava(LogLevel::FINE, "Processed %d events", count);
                break;
        }
        available -= bytesRead;
    }
}

void Server::handleEvent(JNIEnv* env, const inotify_event* event) {
    uint32_t mask = event->mask;
    const char* eventName = (event->len == 0)
        ? ""
        : event->name;
    logToJava(LogLevel::FINE, "Event mask: 0x%x for %s (wd = %d, cookie = 0x%x, len = %d)", mask, eventName, event->wd, event->cookie, event->len);
    if (IS_SET(mask, IN_UNMOUNT)) {
        return;
    }

    // Overflow received, handle gracefully
    if (IS_SET(mask, IN_Q_OVERFLOW)) {
        for (auto it : watchPoints) {
            auto path = it.first;
            reportOverflow(env, path);
        }
        return;
    }

    auto iWatchRoot = watchRoots.find(event->wd);
    if (iWatchRoot == watchRoots.end()) {
        auto iRecentlyUnregisteredWatchPoint = recentlyUnregisteredWatchRoots.find(event->wd);
        if (iRecentlyUnregisteredWatchPoint == recentlyUnregisteredWatchRoots.end()) {
            logToJava(LogLevel::INFO, "Received event for unknown watch descriptor %d", event->wd);
        } else {
            // We've removed this via unregisterPath() not long ago
            auto& path = iRecentlyUnregisteredWatchPoint->second;
            if (IS_SET(mask, IN_IGNORED)) {
                recentlyUnregisteredWatchRoots.erase(iRecentlyUnregisteredWatchPoint);
                logToJava(LogLevel::FINE, "Finished watching recently unregistered watch point '%s' (wd = %d)",
                    utf16ToUtf8String(path).c_str(), event->wd);
            } else {
                logToJava(LogLevel::FINE, "Ignoring incoming events for recently removed watch descriptor for '%s' (wd = %d)",
                    utf16ToUtf8String(path).c_str(), event->wd);
            }
        }
        return;
    }

    auto path = iWatchRoot->second;
    auto& watchPoint = watchPoints.at(path);

    if (IS_SET(mask, IN_IGNORED)) {
        // Finished with watch point
        logToJava(LogLevel::FINE, "Finished watching still registered '%s' (wd = %d)",
            utf16ToUtf8String(path).c_str(), event->wd);
        watchRoots.erase(event->wd);
        watchPoints.erase(path);
        return;
    }

    if (watchPoint.status != WatchPointStatus::LISTENING) {
        logToJava(LogLevel::FINE, "Ignoring incoming events for %s as watch-point is not listening (status = %d)",
            utf16ToUtf8String(path).c_str(), watchPoint.status);
        return;
    }

    if (shouldTerminate) {
        logToJava(LogLevel::FINE, "Ignoring incoming events for %s because server is terminating (status = %d)",
            utf16ToUtf8String(path).c_str(), watchPoint.status);
        return;
    }

    ChangeType type;
    const u16string name = utf8ToUtf16String(eventName);

    if (!name.empty()) {
        path.append(u"/");
        path.append(name);
    }

    if (IS_SET(mask, IN_CREATE | IN_MOVED_TO)) {
        type = ChangeType::CREATED;
    } else if (IS_SET(mask, IN_DELETE | IN_DELETE_SELF | IN_MOVED_FROM)) {
        type = ChangeType::REMOVED;
    } else if (IS_SET(mask, IN_MODIFY)) {
        type = ChangeType::MODIFIED;
    } else {
        logToJava(LogLevel::WARNING, "Unknown event 0x%x for %s", mask, utf16ToUtf8String(path).c_str());
        reportUnknownEvent(env, path);
        return;
    }

    reportChangeEvent(env, type, path);
}

static int addInotifyWatch(const u16string& path, shared_ptr<Inotify> inotify) {
    string pathNarrow = utf16ToUtf8String(path);
    int fdWatch = inotify_add_watch(inotify->fd, pathNarrow.c_str(), EVENT_MASK);
    if (fdWatch == -1) {
        throw FileWatcherException("Couldn't add watch", path, errno);
    }
    return fdWatch;
}

void Server::registerPath(const u16string& path) {
    auto it = watchPoints.find(path);
    if (it != watchPoints.end()) {
        throw FileWatcherException("Already watching path", path);
    }
    int watchDescriptor = addInotifyWatch(path, inotify);
    if (watchRoots.find(watchDescriptor) != watchRoots.end()) {
        throw FileWatcherException("Already watching path", path);
    }
    auto result = watchPoints.emplace(piecewise_construct,
        forward_as_tuple(path),
        forward_as_tuple(path, inotify, watchDescriptor));
    auto& watchPoint = result.first->second;
    watchRoots[watchPoint.watchDescriptor] = path;
}

bool Server::unregisterPath(const u16string& path) {
    auto it = watchPoints.find(path);
    if (it == watchPoints.end()) {
        logToJava(LogLevel::INFO, "Path is not watched: %s", utf16ToUtf8String(path).c_str());
        return false;
    }
    auto& watchPoint = it->second;
    CancelResult ret = watchPoint.cancel();
    if (ret == CancelResult::ALREADY_CANCELLED) {
        return false;
    }
    recentlyUnregisteredWatchRoots.emplace(watchPoint.watchDescriptor, path);
    watchRoots.erase(watchPoint.watchDescriptor);
    watchPoints.erase(it);
    return ret == CancelResult::CANCELLED;
}

JNIEXPORT jobject JNICALL
Java_net_rubygrapefruit_platform_internal_jni_LinuxFileEventFunctions_startWatcher0(JNIEnv* env, jclass, jobject javaCallback) {
    return wrapServer(env, new Server(env, javaCallback));
}

#endif
<commit_msg>Do not release memory for path too early<commit_after>#ifdef __linux__

#include <codecvt>
#include <locale>
#include <string>
#include <sys/ioctl.h>
#include <unistd.h>

#include "linux_fsnotifier.h"

#define EVENT_BUFFER_SIZE (16 * 1024)

#define EVENT_MASK (IN_CREATE | IN_DELETE | IN_DELETE_SELF | IN_EXCL_UNLINK | IN_MODIFY | IN_MOVE_SELF | IN_MOVED_FROM | IN_MOVED_TO | IN_ONLYDIR)

WatchPoint::WatchPoint(const u16string& path, shared_ptr<Inotify> inotify, int watchDescriptor)
    : status(WatchPointStatus::LISTENING)
    , watchDescriptor(watchDescriptor)
    , inotify(inotify)
    , path(path) {
}

CancelResult WatchPoint::cancel() {
    if (status == WatchPointStatus::CANCELLED) {
        return CancelResult::ALREADY_CANCELLED;
    }
    status = WatchPointStatus::CANCELLED;
    if (inotify_rm_watch(inotify->fd, watchDescriptor) != 0) {
        switch (errno) {
            case EINVAL:
                logToJava(LogLevel::INFO, "Couldn't stop watching %s (probably because the directory was removed)", utf16ToUtf8String(path).c_str());
                return CancelResult::NOT_CANCELLED;
                break;
            default:
                throw FileWatcherException("Couldn't stop watching", path, errno);
        }
    }
    return CancelResult::CANCELLED;
}

Inotify::Inotify()
    : fd(inotify_init1(IN_CLOEXEC | IN_NONBLOCK)) {
    if (fd == -1) {
        throw FileWatcherException("Couldn't register inotify handle", errno);
    }
}

Inotify::~Inotify() {
    close(fd);
}

ShutdownEvent::ShutdownEvent()
    : fd(eventfd(0, 0)) {
    if (fd == -1) {
        throw FileWatcherException("Couldn't register event source", errno);
    }
}
ShutdownEvent::~ShutdownEvent() {
    close(fd);
}

void ShutdownEvent::trigger() const {
    const uint64_t increment = 1;
    write(fd, &increment, sizeof(increment));
}

void ShutdownEvent::consume() const {
    uint64_t counter;
    ssize_t bytesRead = read(fd, &counter, sizeof(counter));
    if (bytesRead == -1) {
        throw FileWatcherException("Couldn't read from termination event notifier", errno);
    }
}

Server::Server(JNIEnv* env, jobject watcherCallback)
    : AbstractServer(env, watcherCallback)
    , inotify(new Inotify()) {
    buffer.reserve(EVENT_BUFFER_SIZE);
}

void Server::initializeRunLoop() {
}

void Server::shutdownRunLoop() {
    shutdownEvent.trigger();
}

void Server::runLoop() {
    int forever = numeric_limits<int>::max();

    while (!shouldTerminate) {
        processQueues(forever);
    }

    // No need to clean up watch points, they will be cancelled
    // and closed when the Inotify destructs
}

void Server::processQueues(int timeout) {
    struct pollfd fds[2];
    fds[0].fd = shutdownEvent.fd;
    fds[1].fd = inotify->fd;
    fds[0].events = POLLIN;
    fds[1].events = POLLIN;

    int ret = poll(fds, 2, timeout);
    if (ret == -1) {
        throw FileWatcherException("Couldn't poll for events", errno);
    }

    if (IS_SET(fds[0].revents, POLLIN)) {
        shutdownEvent.consume();
        // Ignore counter, we only care about the notification itself
        shouldTerminate = true;
        return;
    }

    if (IS_SET(fds[1].revents, POLLIN)) {
        try {
            handleEvents();
        } catch (const exception& ex) {
            reportFailure(getThreadEnv(), ex);
        }
    }
}

void Server::handleEvents() {
    unsigned int available;
    ioctl(inotify->fd, FIONREAD, &available);

    while (available > 0) {
        ssize_t bytesRead = read(inotify->fd, &buffer[0], buffer.capacity());

        switch (bytesRead) {
            case -1:
                if (errno == EAGAIN) {
                    // For a non-blocking read, we receive EAGAIN here if there is nothing to read.
                    // This may happen when the inotify is already closed.
                    return;
                } else {
                    throw FileWatcherException("Couldn't read from inotify", errno);
                }
                break;
            case 0:
                throw FileWatcherException("EOF reading from inotify", errno);
                break;
            default:
                // Handle events
                unique_lock<mutex> lock(mutationMutex);
                JNIEnv* env = getThreadEnv();
                logToJava(LogLevel::FINE, "Processing %d bytes worth of events", bytesRead);
                int index = 0;
                int count = 0;
                while (index < bytesRead) {
                    const struct inotify_event* event = (struct inotify_event*) &buffer[index];
                    handleEvent(env, event);
                    index += sizeof(struct inotify_event) + event->len;
                    count++;
                }
                logToJava(LogLevel::FINE, "Processed %d events", count);
                break;
        }
        available -= bytesRead;
    }
}

void Server::handleEvent(JNIEnv* env, const inotify_event* event) {
    uint32_t mask = event->mask;
    const char* eventName = (event->len == 0)
        ? ""
        : event->name;
    logToJava(LogLevel::FINE, "Event mask: 0x%x for %s (wd = %d, cookie = 0x%x, len = %d)", mask, eventName, event->wd, event->cookie, event->len);
    if (IS_SET(mask, IN_UNMOUNT)) {
        return;
    }

    // Overflow received, handle gracefully
    if (IS_SET(mask, IN_Q_OVERFLOW)) {
        for (auto it : watchPoints) {
            auto path = it.first;
            reportOverflow(env, path);
        }
        return;
    }

    auto iWatchRoot = watchRoots.find(event->wd);
    if (iWatchRoot == watchRoots.end()) {
        auto iRecentlyUnregisteredWatchPoint = recentlyUnregisteredWatchRoots.find(event->wd);
        if (iRecentlyUnregisteredWatchPoint == recentlyUnregisteredWatchRoots.end()) {
            logToJava(LogLevel::INFO, "Received event for unknown watch descriptor %d", event->wd);
        } else {
            // We've removed this via unregisterPath() not long ago
            auto& path = iRecentlyUnregisteredWatchPoint->second;
            if (IS_SET(mask, IN_IGNORED)) {
                logToJava(LogLevel::FINE, "Finished watching recently unregistered watch point '%s' (wd = %d)",
                    utf16ToUtf8String(path).c_str(), event->wd);
                recentlyUnregisteredWatchRoots.erase(iRecentlyUnregisteredWatchPoint);
            } else {
                logToJava(LogLevel::FINE, "Ignoring incoming events for recently removed watch descriptor for '%s' (wd = %d)",
                    utf16ToUtf8String(path).c_str(), event->wd);
            }
        }
        return;
    }

    auto path = iWatchRoot->second;
    auto& watchPoint = watchPoints.at(path);

    if (IS_SET(mask, IN_IGNORED)) {
        // Finished with watch point
        logToJava(LogLevel::FINE, "Finished watching still registered '%s' (wd = %d)",
            utf16ToUtf8String(path).c_str(), event->wd);
        watchRoots.erase(event->wd);
        watchPoints.erase(path);
        return;
    }

    if (watchPoint.status != WatchPointStatus::LISTENING) {
        logToJava(LogLevel::FINE, "Ignoring incoming events for %s as watch-point is not listening (status = %d)",
            utf16ToUtf8String(path).c_str(), watchPoint.status);
        return;
    }

    if (shouldTerminate) {
        logToJava(LogLevel::FINE, "Ignoring incoming events for %s because server is terminating (status = %d)",
            utf16ToUtf8String(path).c_str(), watchPoint.status);
        return;
    }

    ChangeType type;
    const u16string name = utf8ToUtf16String(eventName);

    if (!name.empty()) {
        path.append(u"/");
        path.append(name);
    }

    if (IS_SET(mask, IN_CREATE | IN_MOVED_TO)) {
        type = ChangeType::CREATED;
    } else if (IS_SET(mask, IN_DELETE | IN_DELETE_SELF | IN_MOVED_FROM)) {
        type = ChangeType::REMOVED;
    } else if (IS_SET(mask, IN_MODIFY)) {
        type = ChangeType::MODIFIED;
    } else {
        logToJava(LogLevel::WARNING, "Unknown event 0x%x for %s", mask, utf16ToUtf8String(path).c_str());
        reportUnknownEvent(env, path);
        return;
    }

    reportChangeEvent(env, type, path);
}

static int addInotifyWatch(const u16string& path, shared_ptr<Inotify> inotify) {
    string pathNarrow = utf16ToUtf8String(path);
    int fdWatch = inotify_add_watch(inotify->fd, pathNarrow.c_str(), EVENT_MASK);
    if (fdWatch == -1) {
        throw FileWatcherException("Couldn't add watch", path, errno);
    }
    return fdWatch;
}

void Server::registerPath(const u16string& path) {
    auto it = watchPoints.find(path);
    if (it != watchPoints.end()) {
        throw FileWatcherException("Already watching path", path);
    }
    int watchDescriptor = addInotifyWatch(path, inotify);
    if (watchRoots.find(watchDescriptor) != watchRoots.end()) {
        throw FileWatcherException("Already watching path", path);
    }
    auto result = watchPoints.emplace(piecewise_construct,
        forward_as_tuple(path),
        forward_as_tuple(path, inotify, watchDescriptor));
    auto& watchPoint = result.first->second;
    watchRoots[watchPoint.watchDescriptor] = path;
}

bool Server::unregisterPath(const u16string& path) {
    auto it = watchPoints.find(path);
    if (it == watchPoints.end()) {
        logToJava(LogLevel::INFO, "Path is not watched: %s", utf16ToUtf8String(path).c_str());
        return false;
    }
    auto& watchPoint = it->second;
    CancelResult ret = watchPoint.cancel();
    if (ret == CancelResult::ALREADY_CANCELLED) {
        return false;
    }
    recentlyUnregisteredWatchRoots.emplace(watchPoint.watchDescriptor, path);
    watchRoots.erase(watchPoint.watchDescriptor);
    watchPoints.erase(it);
    return ret == CancelResult::CANCELLED;
}

JNIEXPORT jobject JNICALL
Java_net_rubygrapefruit_platform_internal_jni_LinuxFileEventFunctions_startWatcher0(JNIEnv* env, jclass, jobject javaCallback) {
    return wrapServer(env, new Server(env, javaCallback));
}

#endif
<|endoftext|>


<commit_before>/*
 * http://github.com/dusty-nv/jetson-inference
 */

// #include "gstCamera.h"
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <autobot/compound_img.h>
#include <autobot/detected_img.h>
#include <autobot/bounding_box.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <boost/make_shared.hpp>
#include <stdio.h>
#include <signal.h>
#include <unistd.h>
#include <chrono>
#include <jetson-inference/cudaMappedMemory.h>
#include <jetson-inference/cudaNormalize.h>
#include <jetson-inference/cudaFont.h>

#include <jetson-inference/detectNet.h>

#define DEFAULT_CAMERA -1	// -1 for onboard camera, or change to index of /dev/video V4L2 camera (>=0)

using namespace std;

bool signal_recieved = false;

//void sig_handler(int signo)
//{
	//if( signo == SIGINT )
	//{
		//printf("received SIGINT\n");
		//signal_recieved = true;
	//}
//}

static const std::string OPENCV_WINDOW = "Image post bridge conversion";
static const std::string OPENCV_WINDOW2 = "Image post bit depth conversion";
static const std::string OPENCV_WINDOW3 = "Image post color conversion";
static const std::string OPENCV_WINDOW4 = "Image window";

class ObjectDetector
{
	ros::NodeHandle nh_;
	image_transport::ImageTransport it_;
	ros::Subscriber image_sub_;
    ros::Publisher detect_img_pub;
    cv::Mat cv_im;
    cv_bridge::CvImagePtr cv_ptr;
    std::chrono::steady_clock::time_point prev;

	float confidence = 0.0f;

	float* bbCPU    = NULL;
	float* bbCUDA   = NULL;
	float* confCPU  = NULL;
	float* confCUDA = NULL;

	detectNet* net = NULL;
	uint32_t maxBoxes = 0;
	uint32_t classes = 0;

	float4* gpu_data = NULL;

	uint32_t imgWidth;
	uint32_t imgHeight;
	size_t imgSize;

public:
	ObjectDetector(int argc, char** argv ) : it_(nh_)
	{
		cout << "start constructor" << endl;
		// Subscrive to input video feed and publish output video feed
		//image_sub_ = it_.subscribe("/left/image_rect_color", 2,
		  //&ObjectDetector::imageCb, this);
        image_sub_ = nh_.subscribe("/compound_img", 2,
		  &ObjectDetector::imageCb, this);
        detect_img_pub = nh_.advertise<autobot::detected_img>("/detected_image", 2);

		cv::namedWindow(OPENCV_WINDOW);

		cout << "Named a window" << endl;

        prev = std::chrono::steady_clock::now();

		/*
		 * create detectNet
		 */
		net = detectNet::Create(argc, argv);
		cout << "Created DetectNet" << endl;

		if( !net )
		{
			printf("obj_detect:   failed to initialize imageNet\n");

		}

        maxBoxes = net->GetMaxBoundingBoxes();
		printf("maximum bounding boxes:  %u\n", maxBoxes);
		classes  = net->GetNumClasses();

		/*
		 * allocate memory for output bounding boxes and class confidence
		 */

		if( !cudaAllocMapped((void**)&bbCPU, (void**)&bbCUDA, maxBoxes * sizeof(float4)) ||
			!cudaAllocMapped((void**)&confCPU, (void**)&confCUDA, maxBoxes * classes * sizeof(float)) )
		{
			printf("detectnet-console:  failed to alloc output memory\n");

		}
		cout << "Allocated CUDA mem" << endl;

		maxBoxes = net->GetMaxBoundingBoxes();
		printf("maximum bounding boxes:  %u\n", maxBoxes);
		classes  = net->GetNumClasses();
		cout << "Constructor operations complete" << endl;

	}

	~ObjectDetector()
	{
		cv::destroyWindow(OPENCV_WINDOW);
	}

	void imageCb(const autobot::compound_img& cp_msg)
	{
        //ROS_INFO("callback called");
        //boost::shared_ptr<sensor_msgs::Image> cp_shared(&cp_msg.depthImg);
        //sensor_msgs::ImageConstPtr msg = cp_shared;

		try
		{
			cv_ptr = cv_bridge::toCvCopy(cp_msg.img, sensor_msgs::image_encodings::BGR8);
			cv_im = cv_ptr->image;

			cv_im.convertTo(cv_im,CV_32FC3);

			// convert color
			cv::cvtColor(cv_im,cv_im,CV_BGR2RGBA);

		}
		catch (cv_bridge::Exception& e)
		{
		  ROS_ERROR("cv_bridge exception: %s", e.what());
		  return;
		}

        // allocate GPU data if necessary
        if(!gpu_data){
            ROS_INFO("first allocation");
            CUDA(cudaMalloc(&gpu_data, cv_im.rows*cv_im.cols * sizeof(float4)));
        }else if(imgHeight != cv_im.rows || imgWidth != cv_im.cols){
            ROS_INFO("re allocation");
            // reallocate for a new image size if necessary
            CUDA(cudaFree(gpu_data));
            CUDA(cudaMalloc(&gpu_data, cv_im.rows*cv_im.cols * sizeof(float4)));
        }

		imgHeight = cv_im.rows;
		imgWidth = cv_im.cols;
		imgSize = cv_im.rows*cv_im.cols * sizeof(float4);
		float4* cpu_data = (float4*)(cv_im.data);

		// copy to device
		CUDA(cudaMemcpy(gpu_data, cpu_data, imgSize, cudaMemcpyHostToDevice));

		void* imgCUDA = NULL;

		// find object with detectNet
		int numBoundingBoxes = maxBoxes;

		if( net->Detect((float*)gpu_data, imgWidth, imgHeight, bbCPU, &numBoundingBoxes, confCPU))
		{
			//printf("%i bounding boxes detected\n", numBoundingBoxes);

			int lastClass = 0;
			int lastStart = 0;

			for( int n=0; n < numBoundingBoxes; n++ )
			{
				const int nc = confCPU[n*2+1];
				float* bb = bbCPU + (n * 4);

				printf("bounding box %i   (%f, %f)  (%f, %f)  w=%f  h=%f\n", n, bb[0], bb[1], bb[2], bb[3], bb[2] - bb[0], bb[3] - bb[1]);
                cv::rectangle( cv_im, cv::Point( bb[0], bb[1] ), cv::Point( bb[2], bb[3]), cv::Scalar( 255, 55, 0 ), +1, 4 );

			}

            std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now();
            float fps = 1000.0 / std::chrono::duration_cast<std::chrono::milliseconds>(now - prev).count() ;

            prev = now;

			char str[256];
			sprintf(str, "TensorRT build %x | %s | %04.1f FPS", NV_GIE_VERSION, net->HasFP16() ? "FP16" : "FP32", fps);
			cv::setWindowTitle(OPENCV_WINDOW, str);

		}

		// update image back to original

        cv_im.convertTo(cv_im,CV_8UC3);
        cv::cvtColor(cv_im,cv_im,CV_RGBA2BGR);

		// Update GUI Window
        cv::imshow(OPENCV_WINDOW, cv_im);
		cv::waitKey(1);

        for( int n=0; n < numBoundingBoxes; n++ ) {

            const int nc = confCPU[n*2+1];
            float* bb = bbCPU + (n * 4);

            sensor_msgs::ImagePtr msg;

            float crop_width = bb[2] - bb[0];
            float crop_height = bb[3] - bb[1];
            float origin_x = bb[0];
            float origin_y = bb[1];

            printf("imgWidth: %u imgHeight: %u\n",imgWidth,imgHeight);
            printf("BEFORE: origin_x: %f origin_y: %f crop_width: %f crop_height: %f\n",origin_x, origin_y, crop_width, crop_height);

            if (crop_width < crop_height) {
                float diff = crop_height - crop_width;
                printf("diff: %f\n",diff);
                origin_x = origin_x - (diff / 2.0);
                crop_width = crop_width + (diff / 2.0);

            } else if (crop_width > crop_height) {
                float diff = crop_width - crop_height;
                printf("diff: %f\n",diff);
                origin_y = origin_y - (diff / 2.0);
                crop_height = crop_height + (diff / 2.0);
            }
            printf("MIDDLE: origin_x: %f origin_y: %f crop_width: %f crop_height: %f\n",origin_x, origin_y, crop_width, crop_height);

            if (origin_x < 0.0) {
                origin_x = 0.0;
            }

            if (origin_y < 0.0) {
                origin_y = 0.0;
            }

            if (origin_x + crop_width >= (float)imgWidth ) {
                crop_width = (float)imgWidth - origin_x - 1.0;
            }

            if (origin_y + crop_height >= (float)imgHeight) {
                crop_height = (float)imgHeight - origin_y - 1.0;
            }
            printf("AFTER : origin_x: %f origin_y: %f crop_width: %f crop_height: %f\n",origin_x, origin_y, crop_width, crop_height);
            cv::Mat croppedImage = cv_im(cv::Rect(origin_x, origin_y, crop_width, crop_height));
            sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", croppedImage).toImageMsg();
            boost::shared_ptr<autobot::detected_img> detected_img = boost::make_shared<autobot::detected_img>();
            boost::shared_ptr<autobot::bounding_box> bbox = boost::make_shared<autobot::bounding_box>();

                        bbox->origin_x = origin_x;
            bbox->origin_y = origin_y;
            bbox->height = crop_height;
            bbox->width = crop_width;

            detected_img->img = *img_msg.get();
            detected_img->depthImg = cp_msg.depthImg;
            detected_img->box = *bbox.get();

            detect_img_pub.publish<autobot::detected_img>(detected_img);
            cv::imshow("crop", croppedImage);
            cv::waitKey(1);
        }

	}
};

int main( int argc, char** argv ) {
	cout << "starting node" << endl;
	printf("obj_detect\n  args (%i):  ", argc);

	ros::init(argc, argv, "obj_detector");
    ros::NodeHandle nh;

	for( int i=0; i < argc; i++ )
		printf("%i [%s]  ", i, argv[i]);

	printf("\n\n");

	ObjectDetector ic(argc, argv);

	ros::spin();

	return 0;
}

<commit_msg>add dummy subscription for depth map to avoid issue with its transmission in other messages<commit_after>/*
 * http://github.com/dusty-nv/jetson-inference
 */

// #include "gstCamera.h"
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <autobot/compound_img.h>
#include <autobot/detected_img.h>
#include <autobot/bounding_box.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <boost/make_shared.hpp>
#include <stdio.h>
#include <signal.h>
#include <unistd.h>
#include <chrono>
#include <jetson-inference/cudaMappedMemory.h>
#include <jetson-inference/cudaNormalize.h>
#include <jetson-inference/cudaFont.h>

#include <jetson-inference/detectNet.h>

#define DEFAULT_CAMERA -1	// -1 for onboard camera, or change to index of /dev/video V4L2 camera (>=0)

using namespace std;

bool signal_recieved = false;

//void sig_handler(int signo)
//{
	//if( signo == SIGINT )
	//{
		//printf("received SIGINT\n");
		//signal_recieved = true;
	//}
//}

static const std::string OPENCV_WINDOW = "Image post bridge conversion";
static const std::string OPENCV_WINDOW2 = "Image post bit depth conversion";
static const std::string OPENCV_WINDOW3 = "Image post color conversion";
static const std::string OPENCV_WINDOW4 = "Image window";

class ObjectDetector
{
	ros::NodeHandle nh_;
	image_transport::ImageTransport it_;
	ros::Subscriber image_sub_;
    ros::Publisher detect_img_pub;
    cv::Mat cv_im;
    cv_bridge::CvImagePtr cv_ptr;
    std::chrono::steady_clock::time_point prev;
    image_transport::Subscriber dummysub;
	float confidence = 0.0f;

	float* bbCPU    = NULL;
	float* bbCUDA   = NULL;
	float* confCPU  = NULL;
	float* confCUDA = NULL;

	detectNet* net = NULL;
	uint32_t maxBoxes = 0;
	uint32_t classes = 0;

	float4* gpu_data = NULL;

	uint32_t imgWidth;
	uint32_t imgHeight;
	size_t imgSize;

public:
	ObjectDetector(int argc, char** argv ) : it_(nh_)
	{
		cout << "start constructor" << endl;
		// Subscrive to input video feed and publish output video feed
		//image_sub_ = it_.subscribe("/left/image_rect_color", 2,
		  //&ObjectDetector::imageCb, this);
        image_sub_ = nh_.subscribe("/compound_img", 2,
		  &ObjectDetector::imageCb, this);
        detect_img_pub = nh_.advertise<autobot::detected_img>("/detected_image", 2);
        dummysub = it_.subscribe("depth/depth_registered", 1, &ObjectDetector::dummy, this);
		cv::namedWindow(OPENCV_WINDOW);

		cout << "Named a window" << endl;

        prev = std::chrono::steady_clock::now();

		/*
		 * create detectNet
		 */
		net = detectNet::Create(argc, argv);
		cout << "Created DetectNet" << endl;

		if( !net )
		{
			printf("obj_detect:   failed to initialize imageNet\n");

		}

        maxBoxes = net->GetMaxBoundingBoxes();
		printf("maximum bounding boxes:  %u\n", maxBoxes);
		classes  = net->GetNumClasses();

		/*
		 * allocate memory for output bounding boxes and class confidence
		 */

		if( !cudaAllocMapped((void**)&bbCPU, (void**)&bbCUDA, maxBoxes * sizeof(float4)) ||
			!cudaAllocMapped((void**)&confCPU, (void**)&confCUDA, maxBoxes * classes * sizeof(float)) )
		{
			printf("detectnet-console:  failed to alloc output memory\n");

		}
		cout << "Allocated CUDA mem" << endl;

		maxBoxes = net->GetMaxBoundingBoxes();
		printf("maximum bounding boxes:  %u\n", maxBoxes);
		classes  = net->GetNumClasses();
		cout << "Constructor operations complete" << endl;

	}

	~ObjectDetector()
	{
		cv::destroyWindow(OPENCV_WINDOW);
	}

    void dummy(const sensor_msgs::ImageConstPtr& msg) {

    }

	void imageCb(const autobot::compound_img& cp_msg)
	{
        //ROS_INFO("callback called");
        //boost::shared_ptr<sensor_msgs::Image> cp_shared(&cp_msg.depthImg);
        //sensor_msgs::ImageConstPtr msg = cp_shared;

		try
		{
			cv_ptr = cv_bridge::toCvCopy(cp_msg.img, sensor_msgs::image_encodings::BGR8);
			cv_im = cv_ptr->image;

			cv_im.convertTo(cv_im,CV_32FC3);

			// convert color
			cv::cvtColor(cv_im,cv_im,CV_BGR2RGBA);

		}
		catch (cv_bridge::Exception& e)
		{
		  ROS_ERROR("cv_bridge exception: %s", e.what());
		  return;
		}

        // allocate GPU data if necessary
        if(!gpu_data){
            ROS_INFO("first allocation");
            CUDA(cudaMalloc(&gpu_data, cv_im.rows*cv_im.cols * sizeof(float4)));
        }else if(imgHeight != cv_im.rows || imgWidth != cv_im.cols){
            ROS_INFO("re allocation");
            // reallocate for a new image size if necessary
            CUDA(cudaFree(gpu_data));
            CUDA(cudaMalloc(&gpu_data, cv_im.rows*cv_im.cols * sizeof(float4)));
        }

		imgHeight = cv_im.rows;
		imgWidth = cv_im.cols;
		imgSize = cv_im.rows*cv_im.cols * sizeof(float4);
		float4* cpu_data = (float4*)(cv_im.data);

		// copy to device
		CUDA(cudaMemcpy(gpu_data, cpu_data, imgSize, cudaMemcpyHostToDevice));

		void* imgCUDA = NULL;

		// find object with detectNet
		int numBoundingBoxes = maxBoxes;

		if( net->Detect((float*)gpu_data, imgWidth, imgHeight, bbCPU, &numBoundingBoxes, confCPU))
		{
			//printf("%i bounding boxes detected\n", numBoundingBoxes);

			int lastClass = 0;
			int lastStart = 0;

			for( int n=0; n < numBoundingBoxes; n++ )
			{
				const int nc = confCPU[n*2+1];
				float* bb = bbCPU + (n * 4);

				printf("bounding box %i   (%f, %f)  (%f, %f)  w=%f  h=%f\n", n, bb[0], bb[1], bb[2], bb[3], bb[2] - bb[0], bb[3] - bb[1]);
                cv::rectangle( cv_im, cv::Point( bb[0], bb[1] ), cv::Point( bb[2], bb[3]), cv::Scalar( 255, 55, 0 ), +1, 4 );

			}

            std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now();
            float fps = 1000.0 / std::chrono::duration_cast<std::chrono::milliseconds>(now - prev).count() ;

            prev = now;

			char str[256];
			sprintf(str, "TensorRT build %x | %s | %04.1f FPS", NV_GIE_VERSION, net->HasFP16() ? "FP16" : "FP32", fps);
			cv::setWindowTitle(OPENCV_WINDOW, str);

		}

		// update image back to original

        cv_im.convertTo(cv_im,CV_8UC3);
        cv::cvtColor(cv_im,cv_im,CV_RGBA2BGR);

		// Update GUI Window
        cv::imshow(OPENCV_WINDOW, cv_im);
		cv::waitKey(1);

        for( int n=0; n < numBoundingBoxes; n++ ) {

            const int nc = confCPU[n*2+1];
            float* bb = bbCPU + (n * 4);

            sensor_msgs::ImagePtr msg;

            float crop_width = bb[2] - bb[0];
            float crop_height = bb[3] - bb[1];
            float origin_x = bb[0];
            float origin_y = bb[1];

            printf("imgWidth: %u imgHeight: %u\n",imgWidth,imgHeight);
            printf("BEFORE: origin_x: %f origin_y: %f crop_width: %f crop_height: %f\n",origin_x, origin_y, crop_width, crop_height);

            if (crop_width < crop_height) {
                float diff = crop_height - crop_width;
                printf("diff: %f\n",diff);
                origin_x = origin_x - (diff / 2.0);
                crop_width = crop_width + (diff / 2.0);

            } else if (crop_width > crop_height) {
                float diff = crop_width - crop_height;
                printf("diff: %f\n",diff);
                origin_y = origin_y - (diff / 2.0);
                crop_height = crop_height + (diff / 2.0);
            }
            printf("MIDDLE: origin_x: %f origin_y: %f crop_width: %f crop_height: %f\n",origin_x, origin_y, crop_width, crop_height);

            if (origin_x < 0.0) {
                origin_x = 0.0;
            }

            if (origin_y < 0.0) {
                origin_y = 0.0;
            }

            if (origin_x + crop_width >= (float)imgWidth ) {
                crop_width = (float)imgWidth - origin_x - 1.0;
            }

            if (origin_y + crop_height >= (float)imgHeight) {
                crop_height = (float)imgHeight - origin_y - 1.0;
            }
            printf("AFTER : origin_x: %f origin_y: %f crop_width: %f crop_height: %f\n",origin_x, origin_y, crop_width, crop_height);
            cv::Mat croppedImage = cv_im(cv::Rect(origin_x, origin_y, crop_width, crop_height));
            sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", croppedImage).toImageMsg();
            boost::shared_ptr<autobot::detected_img> detected_img = boost::make_shared<autobot::detected_img>();
            boost::shared_ptr<autobot::bounding_box> bbox = boost::make_shared<autobot::bounding_box>();

                        bbox->origin_x = origin_x;
            bbox->origin_y = origin_y;
            bbox->height = crop_height;
            bbox->width = crop_width;

            detected_img->img = *img_msg.get();
            detected_img->depthImg = cp_msg.depthImg;
            detected_img->box = *bbox.get();

            detect_img_pub.publish<autobot::detected_img>(detected_img);
            cv::imshow("crop", croppedImage);
            cv::waitKey(1);
        }

	}
};

int main( int argc, char** argv ) {
	cout << "starting node" << endl;
	printf("obj_detect\n  args (%i):  ", argc);

	ros::init(argc, argv, "obj_detector");
    ros::NodeHandle nh;

	for( int i=0; i < argc; i++ )
		printf("%i [%s]  ", i, argv[i]);

	printf("\n\n");

	ObjectDetector ic(argc, argv);

	ros::spin();

	return 0;
}

<|endoftext|>


<commit_before>#include "common.h"

Chain * get_chain_from_perl_gen_pdb_file(string & filename) {
    ifstream filestream(filename.c_str());
    ASSERT(filestream.is_open(), "Could not open input PDB file " + STRING(filename));

    Chain * chain = NULL;
    Residue * current_residue = NULL;
    Atom * current_atom = NULL;

    string line, line_key, atom_name, resname, chain_id, element;
    int atom_id, residue_id;
    double * coords = new double [3];

    while (getline(filestream, line)) {
        element = line[13];
        istringstream iss(line);
        iss >> line_key;        // get ATOM
        iss >> atom_id;         // get atom id
        iss >> atom_name;       // get atom name (ex CA)
        iss >> resname;         // get residue name (ex HIS)
        iss >> chain_id;        // get chain id
        iss >> residue_id;      // get residue id
        iss >> coords[0];
        iss >> coords[1];
        iss >> coords[2];

        if (chain == NULL)
            chain = new Chain(chain_id);

        if (current_residue == NULL or current_residue->id != residue_id) {
            current_residue = new Residue(residue_id, resname);
            chain->add_residue(current_residue);
        }

        current_atom = new Atom(atom_id, atom_name, coords, element);
        current_residue->add_atom(current_atom);
    }

    return chain;
}

void test_get_chain_from_perl_gen_pdb_file() {
    string filename = "sample.pdb";
    Chain * chain = get_chain_from_perl_gen_pdb_file(filename);
    Residue * residue = (*chain->residues)[0];
    Atom * atom = (*residue->atoms)["CG"];
    cout << chain << endl << residue << endl << atom
            << " {" atom->coords[0] << " " << atom->coords[1] << " " << atom->coords[2] << "}"
            << "Element: " << atom->element << endl;
    return;
}
<commit_msg>perl_gen_pdb_parser complete<commit_after>#include "common.h"

Chain * get_chain_from_perl_gen_pdb_file(string & filename) {
    ifstream filestream(filename.c_str());
    ASSERT(filestream.is_open(), "Could not open input PDB file " + STRING(filename));

    Chain * chain = NULL;
    Residue * current_residue = NULL;
    Atom * current_atom = NULL;

    string line, line_key, atom_name, resname, chain_id, element;
    int atom_id, residue_id;
    double * coords = new double [3];

    while (getline(filestream, line)) {
        element = line[13];
        istringstream iss(line);
        iss >> line_key;        // get ATOM
        iss >> atom_id;         // get atom id
        iss >> atom_name;       // get atom name (ex CA)
        iss >> resname;         // get residue name (ex HIS)
        iss >> chain_id;        // get chain id
        iss >> residue_id;      // get residue id
        iss >> coords[0];
        iss >> coords[1];
        iss >> coords[2];

        if (chain == NULL)
            chain = new Chain(chain_id);

        if (current_residue == NULL or current_residue->id != residue_id) {
            current_residue = new Residue(residue_id, resname);
            chain->add_residue(current_residue);
        }

        current_atom = new Atom(atom_id, atom_name, coords, element);
        current_residue->add_atom(current_atom);
    }

    return chain;
}

void test_get_chain_from_perl_gen_pdb_file() {
    string filename = "sample.pdb";
    Chain * chain = get_chain_from_perl_gen_pdb_file(filename);
    Residue * residue = (*chain->residues)[0];
    Atom * atom = (*residue->atoms)["CG"];
    cout << chain << endl << residue << endl << atom
            << " {" << atom->coords[0] << " " << atom->coords[1] << " " << atom->coords[2] << "} "
            << "Element: " << atom->element << endl;
    return;
}
<|endoftext|>