<commit_before>/* Copyright 2015 The TensorFlow Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#include "tensorflow/core/framework/common_shape_fns.h"
#include "tensorflow/core/framework/op.h"
#include "tensorflow/core/framework/shape_inference.h"

namespace tensorflow {

using shape_inference::InferenceContext;

REGISTER_OP("SymbolicGradient")
    .Input("input: Tin")
    .Output("output: Tout")
    .Attr("Tin: list(type)")
    .Attr("Tout: list(type)")
    .Attr("f: func")
    .SetShapeFn([](InferenceContext* c) {
      if (c->num_inputs() < c->num_outputs()) {
        return errors::InvalidArgument("len(inputs) < len(outputs)");
      }
      std::vector<DataType> types;
      TF_RETURN_IF_ERROR(c->GetAttr("Tin", &types));
      // Say, (u, v) = f(x, y, z), _symbolic_gradient(f) is a function of
      // (x, y, z, du, dv) -> (dx, dy, dz). Therefore, shapes of its
      // outputs (dx, dy, dz) are the same as (x, y, z).
      for (int i = 0; i < c->num_outputs(); ++i) {
        if (types[i] == DT_RESOURCE) {
          const std::vector<shape_inference::ShapeAndType>* handle_type =
              c->input_handle_shapes_and_types(i);
          c->set_output(i, handle_type->at(0).shape);
        } else {
          c->set_output(i, c->input(i));
        }
      }
      return Status::OK();
    });

REGISTER_OP("RemoteCall")
    .Input("target: string")
    .Input("args: Tin")
    .Output("output: Tout")
    .Attr("Tin: list(type)")
    .Attr("Tout: list(type)")
    .Attr("f: func")
    .SetIsStateful()
    .SetShapeFn(shape_inference::UnknownShape);

// TODO(drpng): remove this.
REGISTER_OP("_If")
    .Input("cond: Tcond")
    .Input("input: Tin")
    .Output("output: Tout")
    .Attr("Tcond: type")
    .Attr("Tin: list(type)")
    .Attr("Tout: list(type)")
    .Attr("then_branch: func")
    .Attr("else_branch: func")
    .SetShapeFn(shape_inference::UnknownShape)
    .Doc(R"doc(
output = cond ? then_branch(input) : else_branch(input)

cond: A Tensor. If the tensor is a scalar of non-boolean type, the
    scalar is converted to a boolean according to the
    following rule: if the scalar is a numerical value, non-zero means
    True and zero means False; if the scalar is a string, non-empty
    means True and empty means False. If the tensor is not a scalar,
    being empty means False and being non-empty means True.
input: A list of input tensors.
then_branch: A function that takes 'inputs' and returns a list of
    tensors, whose types are the same as what else_branch returns.
else_branch: A function that takes 'inputs' and returns a list of
    tensors.  whose types are the same as what then_branch returns.
)doc");

REGISTER_OP("If")
    .Input("cond: Tcond")
    .Input("input: Tin")
    .Output("output: Tout")
    .Attr("Tcond: type")
    .Attr("Tin: list(type) >= 0")
    .Attr("Tout: list(type)")
    .Attr("then_branch: func")
    .Attr("else_branch: func")
    .SetShapeFn(shape_inference::UnknownShape);

// TODO(drpng): remove this.
REGISTER_OP("_While")
    .Input("input: T")
    .Output("output: T")
    .Attr("T: list(type) >= 0")
    .Attr("cond: func")
    .Attr("body: func")
    .SetIsStateful()
    .SetShapeFn([](shape_inference::InferenceContext* c) {
      for (int i = 0; i < c->num_outputs(); ++i) {
        c->set_output(i, c->input(i));
      }
      return Status::OK();
    })
    .Doc(R"doc(
output = input; While (Cond(output)) { output = Body(output) }

input: A list of input tensors whose types are T.
output: A list of output tensors whose types are T.
cond: A function takes 'input' and returns a tensor.  If the tensor is
    a scalar of non-boolean, the scalar is converted to a boolean
    according to the following rule: if the scalar is a numerical
    value, non-zero means True and zero means False; if the scalar is
    a string, non-empty means True and empty means False. If the
    tensor is not a scalar, non-emptiness means True and False
    otherwise.
body: A function that takes a list of tensors and returns another
      list of tensors. Both lists have the same types as specified
      by T.
)doc");

// TODO(b/37549631) setting the While Op to always be stateful is too
// conservative.
REGISTER_OP("While")
    .Input("input: T")
    .Output("output: T")
    .Attr("T: list(type) >= 0")
    .Attr("cond: func")
    .Attr("body: func")
    .SetIsStateful()
    .SetShapeFn([](shape_inference::InferenceContext* c) {
      for (int i = 0; i < c->num_outputs(); ++i) {
        c->set_output(i, c->input(i));
      }
      return Status::OK();
    });

REGISTER_OP("For")
    .Input("start: int32")
    .Input("limit: int32")
    .Input("delta: int32")
    .Input("input: T")
    .Output("output: T")
    .Attr("T: list(type) >= 0")
    .Attr("body: func")
    .SetShapeFn(shape_inference::UnknownShape);

// TODO(b/73826847, b/37549631) Mark as stateful.
REGISTER_OP("PartitionedCall")
    .Input("args: Tin")
    .Output("output: Tout")
    .Attr("Tin: list(type) >= 0")
    .Attr("Tout: list(type) >= 0")
    .Attr("f: func")
    .SetShapeFn(shape_inference::UnknownShape);

// This op is used as a placeholder in If branch functions. It doesn't provide a
// valid output when run, so must either be removed (e.g. replaced with a
// function input) or guaranteed not to be used (e.g. if mirroring an
// intermediate output needed for the gradient computation of the other branch).
REGISTER_OP("FakeParam")
    .Output("output: dtype")
    .Attr("dtype: type")
    .Attr("shape: shape")
    .SetShapeFn([](InferenceContext* c) {
      PartialTensorShape shape;
      TF_RETURN_IF_ERROR(c->GetAttr("shape", &shape));
      shape_inference::ShapeHandle out;
      TF_RETURN_IF_ERROR(c->MakeShapeFromPartialTensorShape(shape, &out));
      c->set_output(0, out);
      return Status::OK();
    });

}  // end namespace tensorflow
<commit_msg>Relaxed the functional If op to allow no outputs.<commit_after>/* Copyright 2015 The TensorFlow Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#include "tensorflow/core/framework/common_shape_fns.h"
#include "tensorflow/core/framework/op.h"
#include "tensorflow/core/framework/shape_inference.h"

namespace tensorflow {

using shape_inference::InferenceContext;

REGISTER_OP("SymbolicGradient")
    .Input("input: Tin")
    .Output("output: Tout")
    .Attr("Tin: list(type)")
    .Attr("Tout: list(type)")
    .Attr("f: func")
    .SetShapeFn([](InferenceContext* c) {
      if (c->num_inputs() < c->num_outputs()) {
        return errors::InvalidArgument("len(inputs) < len(outputs)");
      }
      std::vector<DataType> types;
      TF_RETURN_IF_ERROR(c->GetAttr("Tin", &types));
      // Say, (u, v) = f(x, y, z), _symbolic_gradient(f) is a function of
      // (x, y, z, du, dv) -> (dx, dy, dz). Therefore, shapes of its
      // outputs (dx, dy, dz) are the same as (x, y, z).
      for (int i = 0; i < c->num_outputs(); ++i) {
        if (types[i] == DT_RESOURCE) {
          const std::vector<shape_inference::ShapeAndType>* handle_type =
              c->input_handle_shapes_and_types(i);
          c->set_output(i, handle_type->at(0).shape);
        } else {
          c->set_output(i, c->input(i));
        }
      }
      return Status::OK();
    });

REGISTER_OP("RemoteCall")
    .Input("target: string")
    .Input("args: Tin")
    .Output("output: Tout")
    .Attr("Tin: list(type)")
    .Attr("Tout: list(type)")
    .Attr("f: func")
    .SetIsStateful()
    .SetShapeFn(shape_inference::UnknownShape);

// TODO(drpng): remove this.
REGISTER_OP("_If")
    .Input("cond: Tcond")
    .Input("input: Tin")
    .Output("output: Tout")
    .Attr("Tcond: type")
    .Attr("Tin: list(type)")
    .Attr("Tout: list(type)")
    .Attr("then_branch: func")
    .Attr("else_branch: func")
    .SetShapeFn(shape_inference::UnknownShape)
    .Doc(R"doc(
output = cond ? then_branch(input) : else_branch(input)

cond: A Tensor. If the tensor is a scalar of non-boolean type, the
    scalar is converted to a boolean according to the
    following rule: if the scalar is a numerical value, non-zero means
    True and zero means False; if the scalar is a string, non-empty
    means True and empty means False. If the tensor is not a scalar,
    being empty means False and being non-empty means True.
input: A list of input tensors.
then_branch: A function that takes 'inputs' and returns a list of
    tensors, whose types are the same as what else_branch returns.
else_branch: A function that takes 'inputs' and returns a list of
    tensors.  whose types are the same as what then_branch returns.
)doc");

REGISTER_OP("If")
    .Input("cond: Tcond")
    .Input("input: Tin")
    .Output("output: Tout")
    .Attr("Tcond: type")
    .Attr("Tin: list(type) >= 0")
    .Attr("Tout: list(type) >= 0")
    .Attr("then_branch: func")
    .Attr("else_branch: func")
    .SetShapeFn(shape_inference::UnknownShape);

// TODO(drpng): remove this.
REGISTER_OP("_While")
    .Input("input: T")
    .Output("output: T")
    .Attr("T: list(type) >= 0")
    .Attr("cond: func")
    .Attr("body: func")
    .SetIsStateful()
    .SetShapeFn([](shape_inference::InferenceContext* c) {
      for (int i = 0; i < c->num_outputs(); ++i) {
        c->set_output(i, c->input(i));
      }
      return Status::OK();
    })
    .Doc(R"doc(
output = input; While (Cond(output)) { output = Body(output) }

input: A list of input tensors whose types are T.
output: A list of output tensors whose types are T.
cond: A function takes 'input' and returns a tensor.  If the tensor is
    a scalar of non-boolean, the scalar is converted to a boolean
    according to the following rule: if the scalar is a numerical
    value, non-zero means True and zero means False; if the scalar is
    a string, non-empty means True and empty means False. If the
    tensor is not a scalar, non-emptiness means True and False
    otherwise.
body: A function that takes a list of tensors and returns another
      list of tensors. Both lists have the same types as specified
      by T.
)doc");

// TODO(b/37549631) setting the While Op to always be stateful is too
// conservative.
REGISTER_OP("While")
    .Input("input: T")
    .Output("output: T")
    .Attr("T: list(type) >= 0")
    .Attr("cond: func")
    .Attr("body: func")
    .SetIsStateful()
    .SetShapeFn([](shape_inference::InferenceContext* c) {
      for (int i = 0; i < c->num_outputs(); ++i) {
        c->set_output(i, c->input(i));
      }
      return Status::OK();
    });

REGISTER_OP("For")
    .Input("start: int32")
    .Input("limit: int32")
    .Input("delta: int32")
    .Input("input: T")
    .Output("output: T")
    .Attr("T: list(type) >= 0")
    .Attr("body: func")
    .SetShapeFn(shape_inference::UnknownShape);

// TODO(b/73826847, b/37549631) Mark as stateful.
REGISTER_OP("PartitionedCall")
    .Input("args: Tin")
    .Output("output: Tout")
    .Attr("Tin: list(type) >= 0")
    .Attr("Tout: list(type) >= 0")
    .Attr("f: func")
    .SetShapeFn(shape_inference::UnknownShape);

// This op is used as a placeholder in If branch functions. It doesn't provide a
// valid output when run, so must either be removed (e.g. replaced with a
// function input) or guaranteed not to be used (e.g. if mirroring an
// intermediate output needed for the gradient computation of the other branch).
REGISTER_OP("FakeParam")
    .Output("output: dtype")
    .Attr("dtype: type")
    .Attr("shape: shape")
    .SetShapeFn([](InferenceContext* c) {
      PartialTensorShape shape;
      TF_RETURN_IF_ERROR(c->GetAttr("shape", &shape));
      shape_inference::ShapeHandle out;
      TF_RETURN_IF_ERROR(c->MakeShapeFromPartialTensorShape(shape, &out));
      c->set_output(0, out);
      return Status::OK();
    });

}  // end namespace tensorflow
<|endoftext|>


<commit_before>// RUN: grep -Ev "// *[A-Z0-9]+:" %s > %t.cpp
// RUN: clang-format -style="{BasedOnStyle: Google, IndentWidth: 8}" %t.cpp | FileCheck -strict-whitespace -check-prefix=CHECK1 %s
// RUN: clang-format -style="{BasedOnStyle: LLVM, IndentWidth: 7}" %t.cpp | FileCheck -strict-whitespace -check-prefix=CHECK2 %s
// RUN: clang-format -style="{BasedOnStyle: invalid, IndentWidth: 7}" %t.cpp 2>&1 | FileCheck -strict-whitespace -check-prefix=CHECK3 %s
// RUN: clang-format -style="{lsjd}" %t.cpp 2>&1 | FileCheck -strict-whitespace -check-prefix=CHECK4 %s
// RUN: [ ! -e %T/.clang-format ] || rm %T/.clang-format
// RUN: printf "BasedOnStyle: google\nIndentWidth: 5\n" > %T/.clang-format
// RUN: clang-format -style=file %t.cpp 2>&1 | FileCheck -strict-whitespace -check-prefix=CHECK5 %s
void f() {
// CHECK1: {{^        int\* i;$}}
// CHECK2: {{^       int \*i;$}}
// CHECK3: Unknown value for BasedOnStyle: invalid
// CHECK3: Error parsing -style: Invalid argument, using LLVM style
// CHECK3: {{^  int \*i;$}}
// CHECK4: Error parsing -style: Invalid argument, using LLVM style
// CHECK4: {{^  int \*i;$}}
// CHECK5: {{^     int\* i;$}}
int*i;
int j;
}
<commit_msg>Test for empty .clang-format file.<commit_after>// RUN: grep -Ev "// *[A-Z0-9]+:" %s > %t.cpp
// RUN: clang-format -style="{BasedOnStyle: Google, IndentWidth: 8}" %t.cpp | FileCheck -strict-whitespace -check-prefix=CHECK1 %s
// RUN: clang-format -style="{BasedOnStyle: LLVM, IndentWidth: 7}" %t.cpp | FileCheck -strict-whitespace -check-prefix=CHECK2 %s
// RUN: clang-format -style="{BasedOnStyle: invalid, IndentWidth: 7}" %t.cpp 2>&1 | FileCheck -strict-whitespace -check-prefix=CHECK3 %s
// RUN: clang-format -style="{lsjd}" %t.cpp 2>&1 | FileCheck -strict-whitespace -check-prefix=CHECK4 %s
// RUN: [ ! -e %T/.clang-format ] || rm %T/.clang-format
// RUN: printf "BasedOnStyle: google\nIndentWidth: 5\n" > %T/.clang-format
// RUN: clang-format -style=file %t.cpp 2>&1 | FileCheck -strict-whitespace -check-prefix=CHECK5 %s
// RUN: printf "\n" > %T/.clang-format
// RUN: clang-format -style=file %t.cpp 2>&1 | FileCheck -strict-whitespace -check-prefix=CHECK6 %s
void f() {
// CHECK1: {{^        int\* i;$}}
// CHECK2: {{^       int \*i;$}}
// CHECK3: Unknown value for BasedOnStyle: invalid
// CHECK3: Error parsing -style: Invalid argument, using LLVM style
// CHECK3: {{^  int \*i;$}}
// CHECK4: Error parsing -style: Invalid argument, using LLVM style
// CHECK4: {{^  int \*i;$}}
// CHECK5: {{^     int\* i;$}}
// CHECK6: Can't find usable .clang-format, using LLVM style
// CHECK6: {{^  int \*i;$}}
int*i;
int j;
}
<|endoftext|>


<commit_before>#include <atomic>
#include <cassert>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <limits>
#include <string>

#include "benchmark/benchmark.h"

// Test that Setup() and Teardown() are called exactly once
// for each benchmark run (single-threaded).
namespace single {
static int setup_call = 0;
static int teardown_call = 0;
}  // namespace single
static void DoSetup1(const benchmark::State& state) {
  ++single::setup_call;

  // Setup/Teardown should never be called with any thread_idx != 0.
  assert(state.thread_index() == 0);
}

static void DoTeardown1(const benchmark::State& state) {
  ++single::teardown_call;
  assert(state.thread_index() == 0);
}

static void BM_with_setup(benchmark::State& state) {
  for (auto s : state) {
  }
}
BENCHMARK(BM_with_setup)
    ->Arg(1)
    ->Arg(3)
    ->Arg(5)
    ->Arg(7)
    ->Iterations(100)
    ->Setup(DoSetup1)
    ->Teardown(DoTeardown1);

// Test that Setup() and Teardown() are called once for each group of threads.
namespace concurrent {
static std::atomic<int> setup_call(0);
static std::atomic<int> teardown_call(0);
static std::atomic<int> func_call(0);
}  // namespace concurrent

static void DoSetup2(const benchmark::State& state) {
  concurrent::setup_call.fetch_add(1, std::memory_order_acquire);
  assert(state.thread_index() == 0);
}

static void DoTeardown2(const benchmark::State& state) {
  concurrent::teardown_call.fetch_add(1, std::memory_order_acquire);
  assert(state.thread_index() == 0);
}

static void BM_concurrent(benchmark::State& state) {
  for (auto s : state) {
  }
  concurrent::func_call.fetch_add(1, std::memory_order_acquire);
}

BENCHMARK(BM_concurrent)
    ->Setup(DoSetup2)
    ->Teardown(DoTeardown2)
    ->Iterations(100)
    ->Threads(5)
    ->Threads(10)
    ->Threads(15);

// Testing interaction with Fixture::Setup/Teardown
namespace fixture_interaction {
int setup = 0;
int fixture_setup = 0;
}  // namespace fixture_interaction

#define FIXTURE_BECHMARK_NAME MyFixture

class FIXTURE_BECHMARK_NAME : public ::benchmark::Fixture {
 public:
  void SetUp(const ::benchmark::State& state) BENCHMARK_OVERRIDE {
    fixture_interaction::fixture_setup++;
  }

  ~FIXTURE_BECHMARK_NAME() {}
};

BENCHMARK_F(FIXTURE_BECHMARK_NAME, BM_WithFixture)(benchmark::State& st) {
  for (auto _ : st) {
  }
}

static void DoSetupWithFixture(const benchmark::State& state) {
  fixture_interaction::setup++;
}

BENCHMARK_REGISTER_F(FIXTURE_BECHMARK_NAME, BM_WithFixture)
    ->Arg(1)
    ->Arg(3)
    ->Arg(5)
    ->Arg(7)
    ->Setup(DoSetupWithFixture)
    ->Repetitions(1)
    ->Iterations(100);

// Testing repetitions.
namespace repetitions {
int setup = 0;
}

static void DoSetupWithRepetitions(const benchmark::State& state) {
  repetitions::setup++;
}
static void BM_WithRep(benchmark::State& state) {
  for (auto _ : state) {
  }
}

BENCHMARK(BM_WithRep)
    ->Arg(1)
    ->Arg(3)
    ->Arg(5)
    ->Arg(7)
    ->Setup(DoSetupWithRepetitions)
    ->Iterations(100)
    ->Repetitions(4);

int main(int argc, char** argv) {
  benchmark::Initialize(&argc, argv);

  size_t ret = benchmark::RunSpecifiedBenchmarks(".");
  assert(ret > 0);

  // Setup/Teardown is called once for each arg group (1,3,5,7).
  assert(single::setup_call == 4);
  assert(single::teardown_call == 4);

  // 3 group of threads calling this function (3,5,10).
  assert(concurrent::setup_call.load(std::memory_order_relaxed) == 3);
  assert(concurrent::teardown_call.load(std::memory_order_relaxed) == 3);
  assert((5 + 10 + 15) ==
         concurrent::func_call.load(std::memory_order_relaxed));

  // Setup is called 4 times, once for each arg group (1,3,5,7)
  assert(fixture_interaction::setup == 4);
  // Fixture::Setup is called everytime the bm routine is run.
  // The exact number is indeterministic, so we just assert that
  // it's more than setup.
  assert(fixture_interaction::fixture_setup > fixture_interaction::setup);

  // Setup is call once for each repetition * num_arg =  4 * 4 = 16.
  assert(repetitions::setup == 16);

  return 0;
}
<commit_msg>lose some build warnings<commit_after>#include <atomic>
#include <cassert>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <limits>
#include <string>

#include "benchmark/benchmark.h"

// Test that Setup() and Teardown() are called exactly once
// for each benchmark run (single-threaded).
namespace single {
static int setup_call = 0;
static int teardown_call = 0;
}  // namespace single
static void DoSetup1(const benchmark::State& state) {
  ++single::setup_call;

  // Setup/Teardown should never be called with any thread_idx != 0.
  assert(state.thread_index() == 0);
}

static void DoTeardown1(const benchmark::State& state) {
  ++single::teardown_call;
  assert(state.thread_index() == 0);
}

static void BM_with_setup(benchmark::State& state) {
  for (auto s : state) {
  }
}
BENCHMARK(BM_with_setup)
    ->Arg(1)
    ->Arg(3)
    ->Arg(5)
    ->Arg(7)
    ->Iterations(100)
    ->Setup(DoSetup1)
    ->Teardown(DoTeardown1);

// Test that Setup() and Teardown() are called once for each group of threads.
namespace concurrent {
static std::atomic<int> setup_call(0);
static std::atomic<int> teardown_call(0);
static std::atomic<int> func_call(0);
}  // namespace concurrent

static void DoSetup2(const benchmark::State& state) {
  concurrent::setup_call.fetch_add(1, std::memory_order_acquire);
  assert(state.thread_index() == 0);
}

static void DoTeardown2(const benchmark::State& state) {
  concurrent::teardown_call.fetch_add(1, std::memory_order_acquire);
  assert(state.thread_index() == 0);
}

static void BM_concurrent(benchmark::State& state) {
  for (auto s : state) {
  }
  concurrent::func_call.fetch_add(1, std::memory_order_acquire);
}

BENCHMARK(BM_concurrent)
    ->Setup(DoSetup2)
    ->Teardown(DoTeardown2)
    ->Iterations(100)
    ->Threads(5)
    ->Threads(10)
    ->Threads(15);

// Testing interaction with Fixture::Setup/Teardown
namespace fixture_interaction {
int setup = 0;
int fixture_setup = 0;
}  // namespace fixture_interaction

#define FIXTURE_BECHMARK_NAME MyFixture

class FIXTURE_BECHMARK_NAME : public ::benchmark::Fixture {
 public:
  void SetUp(const ::benchmark::State&) BENCHMARK_OVERRIDE {
    fixture_interaction::fixture_setup++;
  }

  ~FIXTURE_BECHMARK_NAME() {}
};

BENCHMARK_F(FIXTURE_BECHMARK_NAME, BM_WithFixture)(benchmark::State& st) {
  for (auto _ : st) {
  }
}

static void DoSetupWithFixture(const benchmark::State&) {
  fixture_interaction::setup++;
}

BENCHMARK_REGISTER_F(FIXTURE_BECHMARK_NAME, BM_WithFixture)
    ->Arg(1)
    ->Arg(3)
    ->Arg(5)
    ->Arg(7)
    ->Setup(DoSetupWithFixture)
    ->Repetitions(1)
    ->Iterations(100);

// Testing repetitions.
namespace repetitions {
int setup = 0;
}

static void DoSetupWithRepetitions(const benchmark::State&) {
  repetitions::setup++;
}
static void BM_WithRep(benchmark::State& state) {
  for (auto _ : state) {
  }
}

BENCHMARK(BM_WithRep)
    ->Arg(1)
    ->Arg(3)
    ->Arg(5)
    ->Arg(7)
    ->Setup(DoSetupWithRepetitions)
    ->Iterations(100)
    ->Repetitions(4);

int main(int argc, char** argv) {
  benchmark::Initialize(&argc, argv);

  size_t ret = benchmark::RunSpecifiedBenchmarks(".");
  assert(ret > 0);

  // Setup/Teardown is called once for each arg group (1,3,5,7).
  assert(single::setup_call == 4);
  assert(single::teardown_call == 4);

  // 3 group of threads calling this function (3,5,10).
  assert(concurrent::setup_call.load(std::memory_order_relaxed) == 3);
  assert(concurrent::teardown_call.load(std::memory_order_relaxed) == 3);
  assert((5 + 10 + 15) ==
         concurrent::func_call.load(std::memory_order_relaxed));

  // Setup is called 4 times, once for each arg group (1,3,5,7)
  assert(fixture_interaction::setup == 4);
  // Fixture::Setup is called everytime the bm routine is run.
  // The exact number is indeterministic, so we just assert that
  // it's more than setup.
  assert(fixture_interaction::fixture_setup > fixture_interaction::setup);

  // Setup is call once for each repetition * num_arg =  4 * 4 = 16.
  assert(repetitions::setup == 16);

  return 0;
}
<|endoftext|>


<commit_before>/*
    Copyright (C) 2013 Digia Plc and/or its subsidiary(-ies).
    Copyright (C) 2013 basysKom GmbH <info@basyskom.com>
    Copyright (C) 2013 Collabora Ltd. <info@collabora.com>

    This library is free software; you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License version 2.1
    as published by the Free Software Foundation.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

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

/**
 * @file
 *   @brief Extracted from QtGstreamer to avoid overly complex dependency
 *   @author Gus Grubba <mavlink@grubba.com>
 */

#include "videonode.h"
#include "videomaterial.h"

#include <QtQuick/QSGFlatColorMaterial>

VideoNode::VideoNode()
  : QSGGeometryNode()
{
    setFlags(OwnsGeometry | OwnsMaterial, true);
    setMaterialTypeSolidBlack();
}

void VideoNode::changeFormat(const BufferFormat & format)
{
    setMaterial(VideoMaterial::create(format));
    setGeometry(0);
    m_materialType = MaterialTypeVideo;
}

void VideoNode::setMaterialTypeSolidBlack()
{
    QSGFlatColorMaterial *m = new QSGFlatColorMaterial;
    m->setColor(Qt::black);
    setMaterial(m);
    setGeometry(0);
    m_materialType = MaterialTypeSolidBlack;
}

void VideoNode::setCurrentFrame(GstBuffer* buffer)
{
    Q_ASSERT (m_materialType == MaterialTypeVideo);
    static_cast<VideoMaterial*>(material())->setCurrentFrame(buffer);
    markDirty(DirtyMaterial);
}

void VideoNode::updateColors(int brightness, int contrast, int hue, int saturation)
{
    Q_ASSERT (m_materialType == MaterialTypeVideo);
    static_cast<VideoMaterial*>(material())->updateColors(brightness, contrast, hue, saturation);
    markDirty(DirtyMaterial);
}

/* Helpers */
template <typename V>
static inline void setGeom(V *v, const QPointF &p)
{
    v->x = p.x();
    v->y = p.y();
}

static inline void setTex(QSGGeometry::TexturedPoint2D *v, const QPointF &p)
{
    v->tx = p.x();
    v->ty = p.y();
}

void VideoNode::updateGeometry(const PaintAreas & areas)
{
    QSGGeometry *g = geometry();

    if (m_materialType == MaterialTypeVideo) {
        if (!g)
            g = new QSGGeometry(QSGGeometry::defaultAttributes_TexturedPoint2D(), 4);

        QSGGeometry::TexturedPoint2D *v = g->vertexDataAsTexturedPoint2D();

        // Set geometry first
        setGeom(v + 0, areas.videoArea.topLeft());
        setGeom(v + 1, areas.videoArea.bottomLeft());
        setGeom(v + 2, areas.videoArea.topRight());
        setGeom(v + 3, areas.videoArea.bottomRight());

        // and then texture coordinates
        setTex(v + 0, areas.sourceRect.topLeft());
        setTex(v + 1, areas.sourceRect.bottomLeft());
        setTex(v + 2, areas.sourceRect.topRight());
        setTex(v + 3, areas.sourceRect.bottomRight());
    } else {
        if (!g)
            g = new QSGGeometry(QSGGeometry::defaultAttributes_Point2D(), 4);

        QSGGeometry::Point2D *v = g->vertexDataAsPoint2D();

        setGeom(v + 0, areas.videoArea.topLeft());
        setGeom(v + 1, areas.videoArea.bottomLeft());
        setGeom(v + 2, areas.videoArea.topRight());
        setGeom(v + 3, areas.videoArea.bottomRight());
    }

    if (!geometry())
        setGeometry(g);

    markDirty(DirtyGeometry);
}
<commit_msg>videonode: Do not finish if material is different<commit_after>/*
    Copyright (C) 2013 Digia Plc and/or its subsidiary(-ies).
    Copyright (C) 2013 basysKom GmbH <info@basyskom.com>
    Copyright (C) 2013 Collabora Ltd. <info@collabora.com>

    This library is free software; you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License version 2.1
    as published by the Free Software Foundation.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

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

/**
 * @file
 *   @brief Extracted from QtGstreamer to avoid overly complex dependency
 *   @author Gus Grubba <mavlink@grubba.com>
 */

#include "videonode.h"
#include "videomaterial.h"

#include <QtQuick/QSGFlatColorMaterial>

VideoNode::VideoNode()
  : QSGGeometryNode()
{
    setFlags(OwnsGeometry | OwnsMaterial, true);
    setMaterialTypeSolidBlack();
}

void VideoNode::changeFormat(const BufferFormat & format)
{
    setMaterial(VideoMaterial::create(format));
    setGeometry(0);
    m_materialType = MaterialTypeVideo;
}

void VideoNode::setMaterialTypeSolidBlack()
{
    QSGFlatColorMaterial *m = new QSGFlatColorMaterial;
    m->setColor(Qt::black);
    setMaterial(m);
    setGeometry(0);
    m_materialType = MaterialTypeSolidBlack;
}

void VideoNode::setCurrentFrame(GstBuffer* buffer)
{
    if (m_materialType != MaterialTypeVideo) {
        return;
    }
    static_cast<VideoMaterial*>(material())->setCurrentFrame(buffer);
    markDirty(DirtyMaterial);
}

void VideoNode::updateColors(int brightness, int contrast, int hue, int saturation)
{
    if (m_materialType != MaterialTypeVideo) {
        return;
    }
    static_cast<VideoMaterial*>(material())->updateColors(brightness, contrast, hue, saturation);
    markDirty(DirtyMaterial);
}

/* Helpers */
template <typename V>
static inline void setGeom(V *v, const QPointF &p)
{
    v->x = p.x();
    v->y = p.y();
}

static inline void setTex(QSGGeometry::TexturedPoint2D *v, const QPointF &p)
{
    v->tx = p.x();
    v->ty = p.y();
}

void VideoNode::updateGeometry(const PaintAreas & areas)
{
    QSGGeometry *g = geometry();

    if (m_materialType == MaterialTypeVideo) {
        if (!g)
            g = new QSGGeometry(QSGGeometry::defaultAttributes_TexturedPoint2D(), 4);

        QSGGeometry::TexturedPoint2D *v = g->vertexDataAsTexturedPoint2D();

        // Set geometry first
        setGeom(v + 0, areas.videoArea.topLeft());
        setGeom(v + 1, areas.videoArea.bottomLeft());
        setGeom(v + 2, areas.videoArea.topRight());
        setGeom(v + 3, areas.videoArea.bottomRight());

        // and then texture coordinates
        setTex(v + 0, areas.sourceRect.topLeft());
        setTex(v + 1, areas.sourceRect.bottomLeft());
        setTex(v + 2, areas.sourceRect.topRight());
        setTex(v + 3, areas.sourceRect.bottomRight());
    } else {
        if (!g)
            g = new QSGGeometry(QSGGeometry::defaultAttributes_Point2D(), 4);

        QSGGeometry::Point2D *v = g->vertexDataAsPoint2D();

        setGeom(v + 0, areas.videoArea.topLeft());
        setGeom(v + 1, areas.videoArea.bottomLeft());
        setGeom(v + 2, areas.videoArea.topRight());
        setGeom(v + 3, areas.videoArea.bottomRight());
    }

    if (!geometry())
        setGeometry(g);

    markDirty(DirtyGeometry);
}
<|endoftext|>


<commit_before>#include <rail_recognition/PointCloudRecognizer.h>

// ROS
#include <tf2/LinearMath/Transform.h>

// PCL
#include <pcl/filters/extract_indices.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/registration/icp.h>

using namespace std;
using namespace pcl;
using namespace rail::pick_and_place;

PointCloudRecognizer::PointCloudRecognizer()
{
}

bool PointCloudRecognizer::recognizeObject(rail_manipulation_msgs::SegmentedObject &object,
    const vector<graspdb::GraspModel> &candidates) const
{
  // make sure we have some candidates
  if (candidates.empty())
  {
    ROS_WARN("Candidate object list is empty. Nothing to compare segmented object to.");
    return false;
  }
  if (object.point_cloud.data.empty())
  {
    ROS_WARN("Segmented object point cloud is empty. Nothing to compare candidate objects to.");
    return false;
  }

  // convert to a PCL point cloud
  pcl::PCLPointCloud2 object_converter;
  pcl_conversions::toPCL(object.point_cloud, object_converter);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr object_point_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::fromPCLPointCloud2(object_converter, *object_point_cloud);

  // pre-process input cloud
  this->filterPointCloudOutliers(object_point_cloud);
  this->translateToOrigin(object_point_cloud, object.centroid);

  // perform recognition
  double min_score = numeric_limits<double>::infinity();
  size_t min_index;
  Eigen::Matrix4f min_icp_tf;
  bool min_swapped;
  for (size_t i = 0; i < candidates.size(); i++)
  {
    // convert the candidate point cloud to a PCL point cloud
    pcl::PCLPointCloud2 cur_converter;
    pcl_conversions::toPCL(candidates[i].getPointCloud(), cur_converter);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr candidate_point_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::fromPCLPointCloud2(cur_converter, *candidate_point_cloud);

    Eigen::Matrix4f cur_icp_tf;
    bool cur_swapped;
    double score = this->scoreRegistration(candidate_point_cloud, object_point_cloud, cur_icp_tf, cur_swapped);
    if (score < min_score)
    {
      min_score = score;
      min_index = i;
      min_icp_tf = cur_icp_tf;
      min_swapped = cur_swapped;
    }
  }

  // check if there is enough confidence
  if (min_score > SCORE_CONFIDENCE_THRESHOLD)
  {
    return false;
  }

  // fill in recognition information
  object.name = candidates[min_index].getObjectName();
  object.model_id = candidates[min_index].getID();
  object.recognized = true;
  object.grasps.clear();

  // extract possible grasps for this model
  vector<rail_pick_and_place_msgs::GraspWithSuccessRate> possible_grasps;
  this->transformGrasps(min_icp_tf, min_swapped, object.centroid,
      candidates[min_index].toROSGraspModelMessage().grasps, possible_grasps);

  // sort and remove any grasps with 0 success rates
  vector<double> success_rates;
  for (size_t i = 0; i < possible_grasps.size(); i++)
  {
    double rate = (possible_grasps[i].attempts > 0) ?
        ((double) possible_grasps[i].successes) / ((double) possible_grasps[i].attempts) : 0.0;

    // check the success rate
    if (rate > 0)
    {
      // place it in order
      bool inserted = false;
      for (size_t j = 0; j < success_rates.size(); j++)
      {
        if (rate <= success_rates[j])
        {
          object.grasps.insert(object.grasps.begin() + j, possible_grasps[i].grasp_pose);
          success_rates.insert(success_rates.begin() + j, rate);
          inserted = true;
          break;
        }
      }

      if (!inserted)
      {
        object.grasps.push_back(possible_grasps[i].grasp_pose);
        success_rates.push_back(rate);
      }
    }
  }

  return true;
}

void PointCloudRecognizer::filterPointCloudOutliers(PointCloud<PointXYZRGB>::Ptr pc) const
{
  // use a KD tree to search
  pcl::KdTreeFLANN<pcl::PointXYZRGB> search_tree;
  search_tree.setInputCloud(pc);

  // check each point
  pcl::IndicesPtr to_keep(new vector<int>);
  for (size_t i = 0; i < pc->size(); i++)
  {
    vector<int> indices;
    vector<float> distances;
    // check how many neighbors pass the test
    int neighbors = search_tree.radiusSearch(pc->at(i), FILTER_SEARCH_RADIUS, indices, distances);
    if (neighbors >= FILTER_MIN_NUM_NEIGHBORS)
    {
      to_keep->push_back(i);
    }
  }

  // extract the point we wish to keep
  pcl::ExtractIndices<pcl::PointXYZRGB> extract;
  extract.setInputCloud(pc);
  extract.setIndices(to_keep);
  extract.filter(*pc);
}

void PointCloudRecognizer::translateToOrigin(pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc,
    const geometry_msgs::Point &centroid) const
{
  // transformation matrix
  Eigen::Matrix4f tf;
  tf << 1, 0, 0, -centroid.x,
      0, 1, 0, -centroid.y,
      0, 0, 1, -centroid.z,
      0, 0, 0, 1;

  // transform the point cloud
  pcl::transformPointCloud(*pc, *pc, tf);
}

double PointCloudRecognizer::scoreRegistration(PointCloud<PointXYZRGB>::ConstPtr candidate,
    PointCloud<PointXYZRGB>::ConstPtr object, Eigen::Matrix4f &icp_tf, bool &icp_swapped) const
{
  // use the larger as the base cloud
  icp_swapped = object->size() > candidate->size();
  // use ICP to for matching
  pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
  if (icp_swapped)
  {
    icp.setInputSource(candidate);
    icp.setInputTarget(object);
  } else
  {
    icp.setInputSource(object);
    icp.setInputTarget(candidate);
  }

  // run ICP on the two point clouds
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZRGB>);
  icp.align(*aligned);
  // extract the final transform
  icp_tf = icp.getFinalTransformation();

  // calculate the distance and color error
  double distance_error = this->calculateRegistrationMetricDistanceError(candidate, aligned);
  double color_error = this->calculateRegistrationMetricOverlap(candidate, aligned);

  // calculate the final weighted result
  double result = ALPHA * (3.0 * distance_error) + (1.0 - ALPHA) * (color_error / 100.0);
  return result;
}

double PointCloudRecognizer::calculateRegistrationMetricDistanceError(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr base,
    pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target) const
{
  // search using a KD tree
  pcl::KdTreeFLANN<PointXYZRGB> search_tree;
  search_tree.setInputCloud(base);

  // search for the nearest point to each point
  double score = 0;
  for (size_t i = 0; i < target->size(); i++)
  {
    vector<int> indices;
    vector<float> distances;
    search_tree.nearestKSearch(target->at(i), 1, indices, distances);
    score += (double) distances[0];
  }

  return score;
}

double PointCloudRecognizer::calculateRegistrationMetricOverlap(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr base,
    pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target) const
{
  // search with a KD tree
  pcl::KdTreeFLANN<pcl::PointXYZRGB> search_tree;
  search_tree.setInputCloud(base);

  // search each point
  int score = 0;
  float error = 0;
  for (size_t i = 0; i < target->size(); i++)
  {
    // get the current point
    vector<int> indices;
    vector<float> distances;
    const pcl::PointXYZRGB &search_point = target->at(i);
    // perform a radius search to see how many neighbors are found
    int neighbors = search_tree.radiusSearch(search_point, OVERLAP_SEARCH_RADIUS, indices, distances);
    // check if there are enough neighbors
    if (neighbors > 0)
    {
      score++;
      // check the average RGB color distance
      double rgb_distance = 0;
      for (size_t j = 0; j < indices.size(); j++)
      {
        const pcl::PointXYZRGB &point = base->at(indices[j]);
        rgb_distance += sqrt(pow(search_point.r - point.r, 2) + pow(search_point.g - point.g, 2) +
            pow(search_point.b - point.b, 2));
      }
      // normalize the distance
      rgb_distance /= neighbors;
      error += rgb_distance;
    }
  }

  // normalize the error
  error /= ((double) score);
  return error;
}

void PointCloudRecognizer::transformGrasps(const Eigen::Matrix4f &icp_transform, const bool icp_swapped,
    const geometry_msgs::Point &centroid,
    const vector<rail_pick_and_place_msgs::GraspWithSuccessRate> &candidate_grasps,
    vector<rail_pick_and_place_msgs::GraspWithSuccessRate> &grasps) const
{
  // convert to a TF2 matrix
  tf2::Matrix3x3 rotation(icp_transform(0, 0), icp_transform(0, 1), icp_transform(0, 2),
      icp_transform(1, 0), icp_transform(1, 1), icp_transform(1, 2),
      icp_transform(2, 0), icp_transform(2, 1), icp_transform(2, 2));
  tf2::Vector3 translation(icp_transform(0, 3), icp_transform(1, 3), icp_transform(2, 3));
  tf2::Transform tf_icp(rotation, translation);

  // transform each pose
  for (size_t i = 0; i < candidate_grasps.size(); i++)
  {
    // convert to tf2 matrix
    const geometry_msgs::Pose &pose = candidate_grasps[i].grasp_pose.pose;
    tf2::Quaternion q_pose(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
    tf2::Vector3 v_pose(pose.position.x, pose.position.y, pose.position.z);
    tf2::Transform tf_pose(q_pose, v_pose);

    // push back the basic information
    grasps.push_back(candidate_grasps[i]);

    tf2::Transform result;
    if (icp_swapped)
    {
      tf2::Transform result;
      result.mult(tf_icp, tf_pose);
    } else
    {
      // use the invese for the result
      tf2::Transform inverse = tf_icp.inverse();
      result.mult(inverse, tf_pose);
    }

    // copy over the values
    grasps[i].grasp_pose.pose.position.x = result.getOrigin().x();
    grasps[i].grasp_pose.pose.position.y = result.getOrigin().y();
    grasps[i].grasp_pose.pose.position.z = result.getOrigin().z();
    grasps[i].grasp_pose.pose.orientation.x = result.getRotation().x();
    grasps[i].grasp_pose.pose.orientation.y = result.getRotation().y();
    grasps[i].grasp_pose.pose.orientation.z = result.getRotation().z();
    grasps[i].grasp_pose.pose.orientation.w = result.getRotation().w();

    // correct for the origin transform
    grasps[i].grasp_pose.pose.position.x += centroid.x;
    grasps[i].grasp_pose.pose.position.y += centroid.y;
    grasps[i].grasp_pose.pose.position.z += centroid.z;
  }
}
<commit_msg>orientation added<commit_after>#include <rail_recognition/PointCloudRecognizer.h>

// ROS
#include <tf2/LinearMath/Transform.h>

// PCL
#include <pcl/filters/extract_indices.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/registration/icp.h>

using namespace std;
using namespace pcl;
using namespace rail::pick_and_place;

PointCloudRecognizer::PointCloudRecognizer()
{
}

bool PointCloudRecognizer::recognizeObject(rail_manipulation_msgs::SegmentedObject &object,
    const vector<graspdb::GraspModel> &candidates) const
{
  // make sure we have some candidates
  if (candidates.empty())
  {
    ROS_WARN("Candidate object list is empty. Nothing to compare segmented object to.");
    return false;
  }
  if (object.point_cloud.data.empty())
  {
    ROS_WARN("Segmented object point cloud is empty. Nothing to compare candidate objects to.");
    return false;
  }

  // convert to a PCL point cloud
  pcl::PCLPointCloud2 object_converter;
  pcl_conversions::toPCL(object.point_cloud, object_converter);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr object_point_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::fromPCLPointCloud2(object_converter, *object_point_cloud);

  // pre-process input cloud
  this->filterPointCloudOutliers(object_point_cloud);
  this->translateToOrigin(object_point_cloud, object.centroid);

  // perform recognition
  double min_score = numeric_limits<double>::infinity();
  size_t min_index;
  Eigen::Matrix4f min_icp_tf;
  bool min_swapped;
  for (size_t i = 0; i < candidates.size(); i++)
  {
    // convert the candidate point cloud to a PCL point cloud
    pcl::PCLPointCloud2 cur_converter;
    pcl_conversions::toPCL(candidates[i].getPointCloud(), cur_converter);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr candidate_point_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::fromPCLPointCloud2(cur_converter, *candidate_point_cloud);

    Eigen::Matrix4f cur_icp_tf;
    bool cur_swapped;
    double score = this->scoreRegistration(candidate_point_cloud, object_point_cloud, cur_icp_tf, cur_swapped);
    if (score < min_score)
    {
      min_score = score;
      min_index = i;
      min_icp_tf = cur_icp_tf;
      min_swapped = cur_swapped;
    }
  }

  // check if there is enough confidence
  if (min_score > SCORE_CONFIDENCE_THRESHOLD)
  {
    return false;
  }

  // fill in recognition information
  object.name = candidates[min_index].getObjectName();
  object.model_id = candidates[min_index].getID();
  object.confidence = min_score;
  object.recognized = true;
  // TODO infer object orientation
  object.orientation.w = 1.0;
  object.grasps.clear();

  // extract possible grasps for this model
  vector<rail_pick_and_place_msgs::GraspWithSuccessRate> possible_grasps;
  this->transformGrasps(min_icp_tf, min_swapped, object.centroid,
      candidates[min_index].toROSGraspModelMessage().grasps, possible_grasps);

  // sort and remove any grasps with 0 success rates
  vector<double> success_rates;
  for (size_t i = 0; i < possible_grasps.size(); i++)
  {
    double rate = (possible_grasps[i].attempts > 0) ?
        ((double) possible_grasps[i].successes) / ((double) possible_grasps[i].attempts) : 0.0;

    // check the success rate
    if (rate > 0)
    {
      // place it in order
      bool inserted = false;
      for (size_t j = 0; j < success_rates.size(); j++)
      {
        if (rate <= success_rates[j])
        {
          object.grasps.insert(object.grasps.begin() + j, possible_grasps[i].grasp_pose);
          success_rates.insert(success_rates.begin() + j, rate);
          inserted = true;
          break;
        }
      }

      if (!inserted)
      {
        object.grasps.push_back(possible_grasps[i].grasp_pose);
        success_rates.push_back(rate);
      }
    }
  }

  return true;
}

void PointCloudRecognizer::filterPointCloudOutliers(PointCloud<PointXYZRGB>::Ptr pc) const
{
  // use a KD tree to search
  pcl::KdTreeFLANN<pcl::PointXYZRGB> search_tree;
  search_tree.setInputCloud(pc);

  // check each point
  pcl::IndicesPtr to_keep(new vector<int>);
  for (size_t i = 0; i < pc->size(); i++)
  {
    vector<int> indices;
    vector<float> distances;
    // check how many neighbors pass the test
    int neighbors = search_tree.radiusSearch(pc->at(i), FILTER_SEARCH_RADIUS, indices, distances);
    if (neighbors >= FILTER_MIN_NUM_NEIGHBORS)
    {
      to_keep->push_back(i);
    }
  }

  // extract the point we wish to keep
  pcl::ExtractIndices<pcl::PointXYZRGB> extract;
  extract.setInputCloud(pc);
  extract.setIndices(to_keep);
  extract.filter(*pc);
}

void PointCloudRecognizer::translateToOrigin(pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc,
    const geometry_msgs::Point &centroid) const
{
  // transformation matrix
  Eigen::Matrix4f tf;
  tf << 1, 0, 0, -centroid.x,
      0, 1, 0, -centroid.y,
      0, 0, 1, -centroid.z,
      0, 0, 0, 1;

  // transform the point cloud
  pcl::transformPointCloud(*pc, *pc, tf);
}

double PointCloudRecognizer::scoreRegistration(PointCloud<PointXYZRGB>::ConstPtr candidate,
    PointCloud<PointXYZRGB>::ConstPtr object, Eigen::Matrix4f &icp_tf, bool &icp_swapped) const
{
  // use the larger as the base cloud
  icp_swapped = object->size() > candidate->size();
  // use ICP to for matching
  pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
  if (icp_swapped)
  {
    icp.setInputSource(candidate);
    icp.setInputTarget(object);
  } else
  {
    icp.setInputSource(object);
    icp.setInputTarget(candidate);
  }

  // run ICP on the two point clouds
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZRGB>);
  icp.align(*aligned);
  // extract the final transform
  icp_tf = icp.getFinalTransformation();

  // calculate the distance and color error
  double distance_error = this->calculateRegistrationMetricDistanceError(candidate, aligned);
  double color_error = this->calculateRegistrationMetricOverlap(candidate, aligned);

  // calculate the final weighted result
  double result = ALPHA * (3.0 * distance_error) + (1.0 - ALPHA) * (color_error / 100.0);
  return result;
}

double PointCloudRecognizer::calculateRegistrationMetricDistanceError(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr base,
    pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target) const
{
  // search using a KD tree
  pcl::KdTreeFLANN<PointXYZRGB> search_tree;
  search_tree.setInputCloud(base);

  // search for the nearest point to each point
  double score = 0;
  for (size_t i = 0; i < target->size(); i++)
  {
    vector<int> indices;
    vector<float> distances;
    search_tree.nearestKSearch(target->at(i), 1, indices, distances);
    score += (double) distances[0];
  }

  return score;
}

double PointCloudRecognizer::calculateRegistrationMetricOverlap(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr base,
    pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr target) const
{
  // search with a KD tree
  pcl::KdTreeFLANN<pcl::PointXYZRGB> search_tree;
  search_tree.setInputCloud(base);

  // search each point
  int score = 0;
  float error = 0;
  for (size_t i = 0; i < target->size(); i++)
  {
    // get the current point
    vector<int> indices;
    vector<float> distances;
    const pcl::PointXYZRGB &search_point = target->at(i);
    // perform a radius search to see how many neighbors are found
    int neighbors = search_tree.radiusSearch(search_point, OVERLAP_SEARCH_RADIUS, indices, distances);
    // check if there are enough neighbors
    if (neighbors > 0)
    {
      score++;
      // check the average RGB color distance
      double rgb_distance = 0;
      for (size_t j = 0; j < indices.size(); j++)
      {
        const pcl::PointXYZRGB &point = base->at(indices[j]);
        rgb_distance += sqrt(pow(search_point.r - point.r, 2) + pow(search_point.g - point.g, 2) +
            pow(search_point.b - point.b, 2));
      }
      // normalize the distance
      rgb_distance /= neighbors;
      error += rgb_distance;
    }
  }

  // normalize the error
  error /= ((double) score);
  return error;
}

void PointCloudRecognizer::transformGrasps(const Eigen::Matrix4f &icp_transform, const bool icp_swapped,
    const geometry_msgs::Point &centroid,
    const vector<rail_pick_and_place_msgs::GraspWithSuccessRate> &candidate_grasps,
    vector<rail_pick_and_place_msgs::GraspWithSuccessRate> &grasps) const
{
  // convert to a TF2 matrix
  tf2::Matrix3x3 rotation(icp_transform(0, 0), icp_transform(0, 1), icp_transform(0, 2),
      icp_transform(1, 0), icp_transform(1, 1), icp_transform(1, 2),
      icp_transform(2, 0), icp_transform(2, 1), icp_transform(2, 2));
  tf2::Vector3 translation(icp_transform(0, 3), icp_transform(1, 3), icp_transform(2, 3));
  tf2::Transform tf_icp(rotation, translation);

  // transform each pose
  for (size_t i = 0; i < candidate_grasps.size(); i++)
  {
    // convert to tf2 matrix
    const geometry_msgs::Pose &pose = candidate_grasps[i].grasp_pose.pose;
    tf2::Quaternion q_pose(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
    tf2::Vector3 v_pose(pose.position.x, pose.position.y, pose.position.z);
    tf2::Transform tf_pose(q_pose, v_pose);

    // push back the basic information
    grasps.push_back(candidate_grasps[i]);

    tf2::Transform result;
    if (icp_swapped)
    {
      tf2::Transform result;
      result.mult(tf_icp, tf_pose);
    } else
    {
      // use the invese for the result
      tf2::Transform inverse = tf_icp.inverse();
      result.mult(inverse, tf_pose);
    }

    // copy over the values
    grasps[i].grasp_pose.pose.position.x = result.getOrigin().x();
    grasps[i].grasp_pose.pose.position.y = result.getOrigin().y();
    grasps[i].grasp_pose.pose.position.z = result.getOrigin().z();
    grasps[i].grasp_pose.pose.orientation.x = result.getRotation().x();
    grasps[i].grasp_pose.pose.orientation.y = result.getRotation().y();
    grasps[i].grasp_pose.pose.orientation.z = result.getRotation().z();
    grasps[i].grasp_pose.pose.orientation.w = result.getRotation().w();

    // correct for the origin transform
    grasps[i].grasp_pose.pose.position.x += centroid.x;
    grasps[i].grasp_pose.pose.position.y += centroid.y;
    grasps[i].grasp_pose.pose.position.z += centroid.z;
  }
}
<|endoftext|>


<commit_before>/*
    Copyright 2005-2007 Adobe Systems Incorporated
    Distributed under the MIT License (see accompanying file LICENSE_1_0_0.txt
    or a copy at http://stlab.adobe.com/licenses.html)
*/

#include "platform_listbox.hpp"

#include <GG/adobe/future/widgets/headers/display.hpp>
#include <GG/adobe/future/widgets/headers/widget_utils.hpp>
#include <GG/adobe/future/widgets/headers/platform_metrics.hpp>
#include <GG/adobe/future/widgets/headers/platform_widget_utils.hpp>
#include <GG/adobe/placeable_concept.hpp>

#include <GG/ClrConstants.h>
#include <GG/GUI.h>
#include <GG/ListBox.h>
#include <GG/StyleFactory.h>
#include <GG/TextControl.h>

namespace {

    GG::Y RowHeight()
    {
        static GG::Y retval = GG::Y0;
        if (!retval) {
            const GG::Y DEFAULT_MARGIN(8); // DEFAULT_ROW_HEIGHT from ListBox.cpp, minus the default font's lineskip of 14
            retval = adobe::implementation::DefaultFont()->Lineskip() + DEFAULT_MARGIN;
        }
        return retval;
    }

    GG::Y Height(unsigned int lines)
    { return RowHeight() * static_cast<int>(lines) + static_cast<int>(2 * GG::ListBox::BORDER_THICK); }

    enum metrics {
        gap = 4 // Measured as space from listbox to label.
    };

    void clear_items(adobe::listbox_t& control)
    {
        assert(control.control_m);
        control.items_m.clear();
        control.control_m->Clear();
    }

    void listbox_selection_changed(adobe::listbox_t& listbox, const GG::ListBox::SelectionSet& selections)
    {
        assert(listbox.control_m);

        if (listbox.value_proc_m) {
            for (GG::ListBox::SelectionSet::const_iterator it = selections.begin(), end_it = selections.end();
                 it != end_it;
                 ++it) {
                std::ptrdiff_t index = std::distance(listbox.control_m->begin(), *it);
                if (listbox.value_proc_m)
                    listbox.value_proc_m(listbox.items_m.at(index).second);
            }
        }

        if (listbox.selection_changed_proc_m)
            listbox.selection_changed_proc_m(listbox, selections);
    }

    void listbox_dropped(adobe::listbox_t& listbox, GG::ListBox::iterator row)
    {
        if (listbox.dropped_proc_m)
            listbox.dropped_proc_m(listbox, row);
    }

    void listbox_drop_acceptable(adobe::listbox_t& listbox, GG::ListBox::const_iterator row)
    {
        if (listbox.drop_acceptable_proc_m)
            listbox.drop_acceptable_proc_m(listbox, row);
    }

    void listbox_left_clicked(adobe::listbox_t& listbox, GG::ListBox::iterator row, const GG::Pt& pt)
    {
        if (listbox.left_clicked_proc_m)
            listbox.left_clicked_proc_m(listbox, row, pt);
    }

    void listbox_right_clicked(adobe::listbox_t& listbox, GG::ListBox::iterator row, const GG::Pt& pt)
    {
        if (listbox.right_clicked_proc_m)
            listbox.right_clicked_proc_m(listbox, row, pt);
    }

    void listbox_double_clicked(adobe::listbox_t& listbox, GG::ListBox::iterator row)
    {
        if (listbox.double_clicked_proc_m)
            listbox.double_clicked_proc_m(listbox, row);
    }

    void listbox_erased(adobe::listbox_t& listbox, GG::ListBox::iterator row)
    {
        if (listbox.erased_proc_m)
            listbox.erased_proc_m(listbox, row);
    }

    void listbox_browsed(adobe::listbox_t& listbox, GG::ListBox::iterator row)
    {
        if (listbox.browsed_proc_m)
            listbox.browsed_proc_m(listbox, row);
    }

    void message_item_set(adobe::listbox_t& listbox)
    {
        assert(listbox.control_m);

        for (adobe::listbox_t::item_set_t::const_iterator
                 first = listbox.items_m.begin(), last = listbox.items_m.end();
             first != last;
             ++first) {
            GG::ListBox::Row* row = new GG::ListBox::Row(GG::X1, RowHeight(), "");
            row->push_back(
                adobe::implementation::Factory().NewTextControl(GG::X0, GG::Y0, first->first,
                                                                adobe::implementation::DefaultFont(),
                                                                listbox.item_text_color_m, GG::FORMAT_LEFT)
            );
            listbox.control_m->Insert(row);
        }
    }

}

namespace adobe {

    listbox_t::listbox_t(const std::string& name,
                         const std::string& alt_text,
                         int characters,
                         int rows,
                         GG::Flags<GG::ListBoxStyle> style,
                         const item_set_t& items,
                         GG::Clr color,
                         GG::Clr interior_color,
                         GG::Clr label_color,
                         GG::Clr item_text_color,
                         const std::vector<std::string>& drop_types,
                         name_t signal_id) :
        control_m(0),
        name_m(name, alt_text, 0, GG::FORMAT_LEFT | GG::FORMAT_TOP, label_color),
        alt_text_m(alt_text),
        using_label_m(!name.empty()),
        items_m(items),
        characters_m(characters),
        rows_m(rows),
        style_m(style),
        color_m(color),
        interior_color_m(interior_color),
        item_text_color_m(item_text_color),
        drop_types_m(drop_types),
        signal_id_m(signal_id)
    {}

    void listbox_t::measure(extents_t& result)
    {
        assert(control_m);

        //
        // The listbox_t has multiple text items. We need to find the one with
        // the widest extents (when drawn). Then we can make sure that we get
        // enough space to draw our largest text item.
        //
        const boost::shared_ptr<GG::Font>& font = implementation::DefaultFont();

        GG::X width = implementation::CharWidth() * characters_m;

        GG::Pt non_client_size = implementation::NonClientSize(*control_m);

        result.width() = Value(width + non_client_size.x);
        result.height() = original_height_m;
        GG::Y baseline =
            (static_cast<int>(result.height()) - implementation::CharHeight()) / 2 +
            implementation::DefaultFont()->Ascent();
        result.vertical().guide_set_m.push_back(Value(baseline));

        //
        // If we have a label (always on our left side?) then we
        // need to add the size of the label to our result. We try
        // to align the label with the listbox by baseline. Which is
        // kind of what Eve does, so it's bad that we do this
        // ourselves, really...
        //
        if (!using_label_m)
            return;
        //
        // We store the height of the label, from this we can
        // figure out how much to offset it when positioning
        // the widgets in set_bounds.
        //
        extents_t label_bounds(measure_text(name_m.name_m, font));
        label_bounds.vertical().guide_set_m.push_back(Value(font->Ascent()));
        //
        // Now we can align the label within the vertical
        // slice of the result. This doesn't do anything if
        // the label is shorter than the listbox.
        //
        align_slices(result.vertical(), label_bounds.vertical());
        //
        // Add the width of the label (plus a gap) to the
        // resulting width.
        //
        result.width() += gap + label_bounds.width();

        //
        // Add a point-of-interest where the label ends.
        // We put the label to the left of the listbox.
        //
        result.horizontal().guide_set_m.push_back(label_bounds.width());
    }

    void listbox_t::place(const place_data_t& place_data)
    {
        using adobe::place;

        assert(control_m);

        place_data_t local_place_data(place_data);

        if (using_label_m) {
            //
            // The vertical offset of the label is the geometry's
            // baseline - the label's baseline.
            //
            assert(place_data.vertical().guide_set_m.empty() == false);

            place_data_t label_place_data;
            label_place_data.horizontal().position_m = left(place_data);
            label_place_data.vertical().position_m = top(place_data);

            //
            // The width of the label is the first horizontal
            // point of interest.
            //
            assert(place_data.horizontal().guide_set_m.empty() == false);
            width(label_place_data) = place_data.horizontal().guide_set_m[0];
            height(label_place_data) = height(place_data);

            //
            // Set the label dimensions.
            //
            place(name_m, label_place_data);

            //
            // Now we need to adjust the position of the listbox
            // widget.
            //
            long width = gap + adobe::width(label_place_data);
            local_place_data.horizontal().position_m += width;
            adobe::width(local_place_data) -= width;
        }

        implementation::set_control_bounds(control_m, local_place_data);
    }

    void listbox_t::enable(bool make_enabled)
    {
        assert(control_m);
        control_m->Disable(!make_enabled);
    }

    void listbox_t::reset_item_set(const item_t* first, const item_t* last)
    {
        assert(control_m);
        clear_items(*this);
        for (; first != last; ++first) {
            items_m.push_back(*first);
        }
        ::message_item_set(*this);
    }

    void listbox_t::display(const model_type& value)
    {
        assert(control_m);

        GG::ListBox::iterator it = control_m->begin();
        for (item_set_t::iterator first = items_m.begin(), last = items_m.end();
             first != last;
             ++first, ++it) {
            if ((*first).second == value) {
                control_m->SelectRow(it);
                return;
            }
        }
    }

    void listbox_t::monitor(const setter_type& proc)
    {
        assert(control_m);
        value_proc_m = proc;
    }

    template <>
    platform_display_type insert<listbox_t>(display_t& display,
                                            platform_display_type& parent,
                                            listbox_t& element)
    {
        if (element.using_label_m)
            insert(display, parent, element.name_m);

        assert(!element.control_m);

        int lines = std::min(element.items_m.size(), std::size_t(20u));
        if (1 < element.rows_m)
            lines = element.rows_m;
        element.control_m =
            implementation::Factory().NewListBox(GG::X0, GG::Y0, GG::X1, Height(lines),
                                                 element.color_m, element.interior_color_m);
        element.control_m->SetStyle(element.style_m);

        element.original_height_m = Value(element.control_m->Height());

        for (std::size_t i = 0; i < element.drop_types_m.size(); ++i) {
            element.control_m->AllowDropType(element.drop_types_m[i]);
        }

        GG::Connect(element.control_m->SelChangedSignal,
                    boost::bind(&listbox_selection_changed, boost::ref(element), _1));

        GG::Connect(element.control_m->DroppedSignal,
                    boost::bind(&listbox_dropped, boost::ref(element), _1));

        GG::Connect(element.control_m->DropAcceptableSignal,
                    boost::bind(&listbox_drop_acceptable, boost::ref(element), _1));

        GG::Connect(element.control_m->LeftClickedSignal,
                    boost::bind(&listbox_left_clicked, boost::ref(element), _1, _2));

        GG::Connect(element.control_m->RightClickedSignal,
                    boost::bind(&listbox_right_clicked, boost::ref(element), _1, _2));

        GG::Connect(element.control_m->DoubleClickedSignal,
                    boost::bind(&listbox_double_clicked, boost::ref(element), _1));

        GG::Connect(element.control_m->ErasedSignal,
                    boost::bind(&listbox_erased, boost::ref(element), _1));

        GG::Connect(element.control_m->BrowsedSignal,
                    boost::bind(&listbox_browsed, boost::ref(element), _1));

        if (!element.alt_text_m.empty())
            adobe::implementation::set_control_alt_text(element.control_m, element.alt_text_m);

        ::message_item_set(element);

        return display.insert(parent, element.control_m);
    }

}
<commit_msg>Added handling of multiple listbox selections when interactingg with the property sheet.<commit_after>/*
    Copyright 2005-2007 Adobe Systems Incorporated
    Distributed under the MIT License (see accompanying file LICENSE_1_0_0.txt
    or a copy at http://stlab.adobe.com/licenses.html)
*/

#include "platform_listbox.hpp"

#include <GG/adobe/future/widgets/headers/display.hpp>
#include <GG/adobe/future/widgets/headers/widget_utils.hpp>
#include <GG/adobe/future/widgets/headers/platform_metrics.hpp>
#include <GG/adobe/future/widgets/headers/platform_widget_utils.hpp>
#include <GG/adobe/array.hpp>
#include <GG/adobe/placeable_concept.hpp>

#include <GG/ClrConstants.h>
#include <GG/GUI.h>
#include <GG/ListBox.h>
#include <GG/StyleFactory.h>
#include <GG/TextControl.h>

namespace {

    GG::Y RowHeight()
    {
        static GG::Y retval = GG::Y0;
        if (!retval) {
            const GG::Y DEFAULT_MARGIN(8); // DEFAULT_ROW_HEIGHT from ListBox.cpp, minus the default font's lineskip of 14
            retval = adobe::implementation::DefaultFont()->Lineskip() + DEFAULT_MARGIN;
        }
        return retval;
    }

    GG::Y Height(unsigned int lines)
    { return RowHeight() * static_cast<int>(lines) + static_cast<int>(2 * GG::ListBox::BORDER_THICK); }

    enum metrics {
        gap = 4 // Measured as space from listbox to label.
    };

    void clear_items(adobe::listbox_t& control)
    {
        assert(control.control_m);
        control.items_m.clear();
        control.control_m->Clear();
    }

    void listbox_selection_changed(adobe::listbox_t& listbox, const GG::ListBox::SelectionSet& selections)
    {
        assert(listbox.control_m);

        if (listbox.value_proc_m) {
            adobe::any_regular_t value;
            if (selections.size() == 1u) {
                std::ptrdiff_t index = std::distance(listbox.control_m->begin(), *selections.begin());
                value = listbox.items_m.at(index).second;
            } else {
                value.assign(adobe::array_t());
                adobe::array_t& array = value.cast<adobe::array_t>();
                for (GG::ListBox::SelectionSet::const_iterator it = selections.begin(), end_it = selections.end();
                     it != end_it;
                     ++it) {
                    std::ptrdiff_t index = std::distance(listbox.control_m->begin(), *it);
                    array.push_back(listbox.items_m.at(index).second);
                }
            }
            listbox.value_proc_m(value);
        }

        if (listbox.selection_changed_proc_m)
            listbox.selection_changed_proc_m(listbox, selections);
    }

    void listbox_dropped(adobe::listbox_t& listbox, GG::ListBox::iterator row)
    {
        if (listbox.dropped_proc_m)
            listbox.dropped_proc_m(listbox, row);
    }

    void listbox_drop_acceptable(adobe::listbox_t& listbox, GG::ListBox::const_iterator row)
    {
        if (listbox.drop_acceptable_proc_m)
            listbox.drop_acceptable_proc_m(listbox, row);
    }

    void listbox_left_clicked(adobe::listbox_t& listbox, GG::ListBox::iterator row, const GG::Pt& pt)
    {
        if (listbox.left_clicked_proc_m)
            listbox.left_clicked_proc_m(listbox, row, pt);
    }

    void listbox_right_clicked(adobe::listbox_t& listbox, GG::ListBox::iterator row, const GG::Pt& pt)
    {
        if (listbox.right_clicked_proc_m)
            listbox.right_clicked_proc_m(listbox, row, pt);
    }

    void listbox_double_clicked(adobe::listbox_t& listbox, GG::ListBox::iterator row)
    {
        if (listbox.double_clicked_proc_m)
            listbox.double_clicked_proc_m(listbox, row);
    }

    void listbox_erased(adobe::listbox_t& listbox, GG::ListBox::iterator row)
    {
        if (listbox.erased_proc_m)
            listbox.erased_proc_m(listbox, row);
    }

    void listbox_browsed(adobe::listbox_t& listbox, GG::ListBox::iterator row)
    {
        if (listbox.browsed_proc_m)
            listbox.browsed_proc_m(listbox, row);
    }

    void message_item_set(adobe::listbox_t& listbox)
    {
        assert(listbox.control_m);

        for (adobe::listbox_t::item_set_t::const_iterator
                 first = listbox.items_m.begin(), last = listbox.items_m.end();
             first != last;
             ++first) {
            GG::ListBox::Row* row = new GG::ListBox::Row(GG::X1, RowHeight(), "");
            row->push_back(
                adobe::implementation::Factory().NewTextControl(GG::X0, GG::Y0, first->first,
                                                                adobe::implementation::DefaultFont(),
                                                                listbox.item_text_color_m, GG::FORMAT_LEFT)
            );
            listbox.control_m->Insert(row);
        }
    }

}

namespace adobe {

    listbox_t::listbox_t(const std::string& name,
                         const std::string& alt_text,
                         int characters,
                         int rows,
                         GG::Flags<GG::ListBoxStyle> style,
                         const item_set_t& items,
                         GG::Clr color,
                         GG::Clr interior_color,
                         GG::Clr label_color,
                         GG::Clr item_text_color,
                         const std::vector<std::string>& drop_types,
                         name_t signal_id) :
        control_m(0),
        name_m(name, alt_text, 0, GG::FORMAT_LEFT | GG::FORMAT_TOP, label_color),
        alt_text_m(alt_text),
        using_label_m(!name.empty()),
        items_m(items),
        characters_m(characters),
        rows_m(rows),
        style_m(style),
        color_m(color),
        interior_color_m(interior_color),
        item_text_color_m(item_text_color),
        drop_types_m(drop_types),
        signal_id_m(signal_id)
    {}

    void listbox_t::measure(extents_t& result)
    {
        assert(control_m);

        //
        // The listbox_t has multiple text items. We need to find the one with
        // the widest extents (when drawn). Then we can make sure that we get
        // enough space to draw our largest text item.
        //
        const boost::shared_ptr<GG::Font>& font = implementation::DefaultFont();

        GG::X width = implementation::CharWidth() * characters_m;

        GG::Pt non_client_size = implementation::NonClientSize(*control_m);

        result.width() = Value(width + non_client_size.x);
        result.height() = original_height_m;
        GG::Y baseline =
            (static_cast<int>(result.height()) - implementation::CharHeight()) / 2 +
            implementation::DefaultFont()->Ascent();
        result.vertical().guide_set_m.push_back(Value(baseline));

        //
        // If we have a label (always on our left side?) then we
        // need to add the size of the label to our result. We try
        // to align the label with the listbox by baseline. Which is
        // kind of what Eve does, so it's bad that we do this
        // ourselves, really...
        //
        if (!using_label_m)
            return;
        //
        // We store the height of the label, from this we can
        // figure out how much to offset it when positioning
        // the widgets in set_bounds.
        //
        extents_t label_bounds(measure_text(name_m.name_m, font));
        label_bounds.vertical().guide_set_m.push_back(Value(font->Ascent()));
        //
        // Now we can align the label within the vertical
        // slice of the result. This doesn't do anything if
        // the label is shorter than the listbox.
        //
        align_slices(result.vertical(), label_bounds.vertical());
        //
        // Add the width of the label (plus a gap) to the
        // resulting width.
        //
        result.width() += gap + label_bounds.width();

        //
        // Add a point-of-interest where the label ends.
        // We put the label to the left of the listbox.
        //
        result.horizontal().guide_set_m.push_back(label_bounds.width());
    }

    void listbox_t::place(const place_data_t& place_data)
    {
        using adobe::place;

        assert(control_m);

        place_data_t local_place_data(place_data);

        if (using_label_m) {
            //
            // The vertical offset of the label is the geometry's
            // baseline - the label's baseline.
            //
            assert(place_data.vertical().guide_set_m.empty() == false);

            place_data_t label_place_data;
            label_place_data.horizontal().position_m = left(place_data);
            label_place_data.vertical().position_m = top(place_data);

            //
            // The width of the label is the first horizontal
            // point of interest.
            //
            assert(place_data.horizontal().guide_set_m.empty() == false);
            width(label_place_data) = place_data.horizontal().guide_set_m[0];
            height(label_place_data) = height(place_data);

            //
            // Set the label dimensions.
            //
            place(name_m, label_place_data);

            //
            // Now we need to adjust the position of the listbox
            // widget.
            //
            long width = gap + adobe::width(label_place_data);
            local_place_data.horizontal().position_m += width;
            adobe::width(local_place_data) -= width;
        }

        implementation::set_control_bounds(control_m, local_place_data);
    }

    void listbox_t::enable(bool make_enabled)
    {
        assert(control_m);
        control_m->Disable(!make_enabled);
    }

    void listbox_t::reset_item_set(const item_t* first, const item_t* last)
    {
        assert(control_m);
        clear_items(*this);
        for (; first != last; ++first) {
            items_m.push_back(*first);
        }
        ::message_item_set(*this);
    }

    void listbox_t::display(const model_type& value)
    {
        assert(control_m);

        const bool value_is_array = value.type_info() == adobe::type_info<adobe::array_t>();

        adobe::array_t single_value;
        if (!value_is_array)
            single_value.push_back(value);
        const adobe::array_t& values = value_is_array ? value.cast<adobe::array_t>() : single_value;

        control_m->DeselectAll();

        for (adobe::array_t::const_iterator it = values.begin(), end_it = values.end(); it != end_it; ++it) {
            GG::ListBox::iterator row_it = control_m->begin();
            for (item_set_t::iterator first = items_m.begin(), last = items_m.end();
                 first != last;
                 ++first, ++row_it) {
                if ((*first).second == value) {
                    control_m->SelectRow(row_it);
                    break;
                }
            }
        }
    }

    void listbox_t::monitor(const setter_type& proc)
    {
        assert(control_m);
        value_proc_m = proc;
    }

    template <>
    platform_display_type insert<listbox_t>(display_t& display,
                                            platform_display_type& parent,
                                            listbox_t& element)
    {
        if (element.using_label_m)
            insert(display, parent, element.name_m);

        assert(!element.control_m);

        int lines = std::min(element.items_m.size(), std::size_t(20u));
        if (1 < element.rows_m)
            lines = element.rows_m;
        element.control_m =
            implementation::Factory().NewListBox(GG::X0, GG::Y0, GG::X1, Height(lines),
                                                 element.color_m, element.interior_color_m);
        element.control_m->SetStyle(element.style_m);

        element.original_height_m = Value(element.control_m->Height());

        for (std::size_t i = 0; i < element.drop_types_m.size(); ++i) {
            element.control_m->AllowDropType(element.drop_types_m[i]);
        }

        GG::Connect(element.control_m->SelChangedSignal,
                    boost::bind(&listbox_selection_changed, boost::ref(element), _1));

        GG::Connect(element.control_m->DroppedSignal,
                    boost::bind(&listbox_dropped, boost::ref(element), _1));

        GG::Connect(element.control_m->DropAcceptableSignal,
                    boost::bind(&listbox_drop_acceptable, boost::ref(element), _1));

        GG::Connect(element.control_m->LeftClickedSignal,
                    boost::bind(&listbox_left_clicked, boost::ref(element), _1, _2));

        GG::Connect(element.control_m->RightClickedSignal,
                    boost::bind(&listbox_right_clicked, boost::ref(element), _1, _2));

        GG::Connect(element.control_m->DoubleClickedSignal,
                    boost::bind(&listbox_double_clicked, boost::ref(element), _1));

        GG::Connect(element.control_m->ErasedSignal,
                    boost::bind(&listbox_erased, boost::ref(element), _1));

        GG::Connect(element.control_m->BrowsedSignal,
                    boost::bind(&listbox_browsed, boost::ref(element), _1));

        if (!element.alt_text_m.empty())
            adobe::implementation::set_control_alt_text(element.control_m, element.alt_text_m);

        ::message_item_set(element);

        return display.insert(parent, element.control_m);
    }

}
<|endoftext|>


<commit_before>#include "stdafx.h"
#include "CppUnitTest.h"
#include "../DNAnalyzerServer/Dictionnaire.h"
#include "../DNAnalyzerServer/Mots.h"
#include <exception>
#include <unordered_set>
#include <cstring>
#include <string>

using namespace Microsoft::VisualStudio::CppUnitTestFramework;

namespace DNAnalyzerServerTest
{
	TEST_CLASS(DictionnaireTest)
	{
	public:
		TEST_METHOD_CLEANUP(CleanUp)
		{
			Mots::RafraichirInstance();
			Dictionnaire::RafraichirInstance();
		}

		//ObtenirInstance
		TEST_METHOD(ObtenirInstance_NotNull)
		{
			// L'instance retourne ne doit pas tre null
			Assert::IsTrue(&(Dictionnaire::ObtenirInstance())!=(Dictionnaire*) nullptr);
		}

		TEST_METHOD(ObtenirInstance_SameReference)
		{
			// L'instance retourne doit toujours tre la mme
			Assert::IsTrue(&(Dictionnaire::ObtenirInstance()) == &(Dictionnaire::ObtenirInstance()));
		}
		TEST_METHOD(ObtenirMaladie_KnownMaladie) {

		}
		TEST_METHOD(ObtenirMaladie_UnKnownMaladie) {

		}
		TEST_METHOD(ObtenirMaladies_NoMaladies) {
			char motInexistant[] = "ATCGINEXISTANT";
			unsigned int indexMotInexistant = Mots::ObtenirInstance().InsererMot(motInexistant);
			const unordered_set<Maladie*> resultat = Dictionnaire::ObtenirInstance().ObtenirMaladies(indexMotInexistant);
			Assert::AreEqual((int)resultat.size(), 0);
		}
		TEST_METHOD(ObtenirMaladies_OneMaladies) {

		}
		TEST_METHOD(ObtenirMaladies_TwoMaladies) {

		}
		TEST_METHOD(ObtenirNomMaladies_EmptyDictionnaire) {

		}
		TEST_METHOD(ObtenirNomMaladies_NotEmptyDictionnaire) {

		}
	};
}<commit_msg>ajout de TEST_CLASS_INITIALIZE<commit_after>#include "stdafx.h"
#include "CppUnitTest.h"
#include "../DNAnalyzerServer/Dictionnaire.h"
#include "../DNAnalyzerServer/Mots.h"
#include <exception>
#include <unordered_set>
#include <cstring>
#include <string>

using namespace Microsoft::VisualStudio::CppUnitTestFramework;

namespace DNAnalyzerServerTest
{
	TEST_CLASS(DictionnaireTest)
	{
	public:
		TEST_CLASS_INITIALIZE(initialize) {
			Mots::RafraichirInstance();
			Dictionnaire::RafraichirInstance();
		}

		TEST_METHOD_CLEANUP(CleanUp)
		{
			Mots::RafraichirInstance();
			Dictionnaire::RafraichirInstance();
		}

		//ObtenirInstance
		TEST_METHOD(ObtenirInstance_NotNull)
		{
			// L'instance retourne ne doit pas tre null
			Assert::IsTrue(&(Dictionnaire::ObtenirInstance())!=(Dictionnaire*) nullptr);
		}

		TEST_METHOD(ObtenirInstance_SameReference)
		{
			// L'instance retourne doit toujours tre la mme
			Assert::IsTrue(&(Dictionnaire::ObtenirInstance()) == &(Dictionnaire::ObtenirInstance()));
		}
		TEST_METHOD(ObtenirMaladie_KnownMaladie) {
			Assert::Fail();// TODO
		}
		TEST_METHOD(ObtenirMaladie_UnKnownMaladie) {
			Assert::Fail();// TODO
		}
		TEST_METHOD(ObtenirMaladies_NoMaladies) {
			char motInexistant[] = "ATCGINEXISTANT";
			unsigned int indexMotInexistant = Mots::ObtenirInstance().InsererMot(motInexistant);
			const unordered_set<Maladie*> resultat = Dictionnaire::ObtenirInstance().ObtenirMaladies(indexMotInexistant);
			Assert::AreEqual((int)resultat.size(), 0);
		}
		TEST_METHOD(ObtenirMaladies_OneMaladies) {
			Assert::Fail();// TODO
		}
		TEST_METHOD(ObtenirMaladies_TwoMaladies) {
			Assert::Fail();// TODO
		}
		TEST_METHOD(ObtenirNomMaladies_EmptyDictionnaire) {
			Assert::Fail();// TODO
		}
		TEST_METHOD(ObtenirNomMaladies_NotEmptyDictionnaire) {
			Assert::Fail();// TODO
		}
	};
}<|endoftext|>


<commit_before>/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT 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.
 *
 */

#include <pcl/console/parse.h>
#include <pcl/console/print.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>
#include <pcl/registration/transformation_estimation_lm.h>
#include <pcl/registration/warp_point_rigid_3d.h>

#include <string>
#include <iostream>
#include <fstream>
#include <vector>

typedef pcl::PointXYZ PointType;
typedef pcl::PointCloud<PointType> Cloud;
typedef Cloud::ConstPtr CloudConstPtr;
typedef Cloud::Ptr CloudPtr;

void
selfTest ()
{
  CloudPtr model (new Cloud);
  model->points.push_back (PointType (1,1,0));
  model->points.push_back (PointType (4,4,0));
  model->points.push_back (PointType (5,6,0));
  model->points.push_back (PointType (3,3,0));
  model->points.push_back (PointType (6,7,0));
  model->points.push_back (PointType (7,11,0));
  model->points.push_back (PointType (12,15,0));
  model->points.push_back (PointType (7,12,0));

  CloudPtr data (new Cloud);
  data->points.push_back (PointType (3,1,0));
  data->points.push_back (PointType (7,4,0));
  data->points.push_back (PointType (9,6,0));

  pcl::console::setVerbosityLevel (pcl::console::L_DEBUG);

  pcl::NormalDistributionsTransform<PointType, PointType> ndt;

  ndt.setMaximumIterations (40);
  ndt.setGridCentre (Eigen::Vector2f (0,0));
  ndt.setGridExtent (Eigen::Vector2f (20,20));
  ndt.setGridStep (Eigen::Vector2f (20,20));
  ndt.setOptimisationStepSize (Eigen::Vector3d (0.4,0.4,0.1));
  ndt.setTransformationEpsilon (1e-9);

  ndt.setInputTarget (model);
  ndt.setInputCloud (data);

  CloudPtr tmp (new Cloud);
  ndt.align (*tmp);
  std::cout << ndt.getFinalTransformation () << std::endl;
}

int
main (int argc, char **argv)
{
  int iter = 10;
  double grid_step = 3.0;
  double grid_extent = 25.0;
  double optim_step = 1.0;

  pcl::console::parse_argument (argc, argv, "-i", iter);
  pcl::console::parse_argument (argc, argv, "-g", grid_step);
  pcl::console::parse_argument (argc, argv, "-e", grid_extent);
  pcl::console::parse_argument (argc, argv, "-s", optim_step);

  std::vector<int> pcd_indices;
  pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");

  CloudPtr model (new Cloud);
  if (pcl::io::loadPCDFile (argv[pcd_indices[0]], *model) == -1)
  {
    std::cout << "Could not read file" << std::endl;
    return -1;
  }
  std::cout << argv[pcd_indices[0]] << " width: " << model->width << " height: " << model->height << std::endl;

  std::string result_filename (argv[pcd_indices[0]]);
  result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
  try
  {
    pcl::io::savePCDFile (result_filename.c_str (), *model);
    std::cout << "saving first model to " << result_filename << std::endl;
  }
  catch(pcl::IOException& e)
  {
    std::cerr << e.what() << std::endl;
  }

  Eigen::Matrix4f t (Eigen::Matrix4f::Identity ());

  for (size_t i = 1; i < pcd_indices.size (); i++)
  {
    CloudPtr data (new Cloud);
    if (pcl::io::loadPCDFile (argv[pcd_indices[i]], *data) == -1)
    {
      std::cout << "Could not read file" << std::endl;
      return -1;
    }
    std::cout << argv[pcd_indices[i]] << " width: " << data->width << " height: " << data->height << std::endl;

    //pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);

    pcl::NormalDistributionsTransform<PointType, PointType> ndt;

    ndt.setMaximumIterations (iter);
    ndt.setGridCentre (Eigen::Vector2f (15,0));
    ndt.setGridExtent (Eigen::Vector2f (grid_extent,grid_extent));
    ndt.setGridStep (Eigen::Vector2f (grid_step,grid_step));
    ndt.setOptimisationStepSize (optim_step);
    ndt.setTransformationEpsilon (1e-5);

    ndt.setInputTarget (model);
    ndt.setInputCloud (data);

    CloudPtr tmp (new Cloud);
    ndt.align (*tmp);

    t = ndt.getFinalTransformation () * t;

    pcl::transformPointCloud (*data, *tmp, t);

    std::cout << ndt.getFinalTransformation () << std::endl;

    *model = *data;

    try
    {
      std::string result_filename (argv[pcd_indices[i]]);
      result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
      pcl::io::savePCDFileBinary (result_filename.c_str (), *tmp);
      std::cout << "saving result to " << result_filename << std::endl;
    }
    catch(pcl::IOException& e)
    {
      std::cerr << e.what() << std::endl;
    }
  }

  return 0;
}
<commit_msg>optimisation -> optimization<commit_after>/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT 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.
 *
 */

#include <pcl/console/parse.h>
#include <pcl/console/print.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>
#include <pcl/registration/transformation_estimation_lm.h>
#include <pcl/registration/warp_point_rigid_3d.h>

#include <string>
#include <iostream>
#include <fstream>
#include <vector>

typedef pcl::PointXYZ PointType;
typedef pcl::PointCloud<PointType> Cloud;
typedef Cloud::ConstPtr CloudConstPtr;
typedef Cloud::Ptr CloudPtr;

void
selfTest ()
{
  CloudPtr model (new Cloud);
  model->points.push_back (PointType (1,1,0));
  model->points.push_back (PointType (4,4,0));
  model->points.push_back (PointType (5,6,0));
  model->points.push_back (PointType (3,3,0));
  model->points.push_back (PointType (6,7,0));
  model->points.push_back (PointType (7,11,0));
  model->points.push_back (PointType (12,15,0));
  model->points.push_back (PointType (7,12,0));

  CloudPtr data (new Cloud);
  data->points.push_back (PointType (3,1,0));
  data->points.push_back (PointType (7,4,0));
  data->points.push_back (PointType (9,6,0));

  pcl::console::setVerbosityLevel (pcl::console::L_DEBUG);

  pcl::NormalDistributionsTransform<PointType, PointType> ndt;

  ndt.setMaximumIterations (40);
  ndt.setGridCentre (Eigen::Vector2f (0,0));
  ndt.setGridExtent (Eigen::Vector2f (20,20));
  ndt.setGridStep (Eigen::Vector2f (20,20));
  ndt.setOptimizationStepSize (Eigen::Vector3d (0.4,0.4,0.1));
  ndt.setTransformationEpsilon (1e-9);

  ndt.setInputTarget (model);
  ndt.setInputCloud (data);

  CloudPtr tmp (new Cloud);
  ndt.align (*tmp);
  std::cout << ndt.getFinalTransformation () << std::endl;
}

int
main (int argc, char **argv)
{
  int iter = 10;
  double grid_step = 3.0;
  double grid_extent = 25.0;
  double optim_step = 1.0;

  pcl::console::parse_argument (argc, argv, "-i", iter);
  pcl::console::parse_argument (argc, argv, "-g", grid_step);
  pcl::console::parse_argument (argc, argv, "-e", grid_extent);
  pcl::console::parse_argument (argc, argv, "-s", optim_step);

  std::vector<int> pcd_indices;
  pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");

  CloudPtr model (new Cloud);
  if (pcl::io::loadPCDFile (argv[pcd_indices[0]], *model) == -1)
  {
    std::cout << "Could not read file" << std::endl;
    return -1;
  }
  std::cout << argv[pcd_indices[0]] << " width: " << model->width << " height: " << model->height << std::endl;

  std::string result_filename (argv[pcd_indices[0]]);
  result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
  try
  {
    pcl::io::savePCDFile (result_filename.c_str (), *model);
    std::cout << "saving first model to " << result_filename << std::endl;
  }
  catch(pcl::IOException& e)
  {
    std::cerr << e.what() << std::endl;
  }

  Eigen::Matrix4f t (Eigen::Matrix4f::Identity ());

  for (size_t i = 1; i < pcd_indices.size (); i++)
  {
    CloudPtr data (new Cloud);
    if (pcl::io::loadPCDFile (argv[pcd_indices[i]], *data) == -1)
    {
      std::cout << "Could not read file" << std::endl;
      return -1;
    }
    std::cout << argv[pcd_indices[i]] << " width: " << data->width << " height: " << data->height << std::endl;

    //pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);

    pcl::NormalDistributionsTransform<PointType, PointType> ndt;

    ndt.setMaximumIterations (iter);
    ndt.setGridCentre (Eigen::Vector2f (15,0));
    ndt.setGridExtent (Eigen::Vector2f (grid_extent,grid_extent));
    ndt.setGridStep (Eigen::Vector2f (grid_step,grid_step));
    ndt.setOptimizationStepSize (optim_step);
    ndt.setTransformationEpsilon (1e-5);

    ndt.setInputTarget (model);
    ndt.setInputCloud (data);

    CloudPtr tmp (new Cloud);
    ndt.align (*tmp);

    t = ndt.getFinalTransformation () * t;

    pcl::transformPointCloud (*data, *tmp, t);

    std::cout << ndt.getFinalTransformation () << std::endl;

    *model = *data;

    try
    {
      std::string result_filename (argv[pcd_indices[i]]);
      result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
      pcl::io::savePCDFileBinary (result_filename.c_str (), *tmp);
      std::cout << "saving result to " << result_filename << std::endl;
    }
    catch(pcl::IOException& e)
    {
      std::cerr << e.what() << std::endl;
    }
  }

  return 0;
}
<|endoftext|>