diff --git a/include/pose/solver-callback.hxx b/include/pose/solver-callback.hxx index 1b03986..f382fc9 100644 --- a/include/pose/solver-callback.hxx +++ b/include/pose/solver-callback.hxx @@ -17,7 +17,7 @@ namespace pose { -template +template struct FeasibleSolutionCallbackEmpty { typedef S size_type; typedef Solution SolutionType; @@ -34,7 +34,7 @@ class PoseEstimator { public: typedef ILP_SOLVER IlpSolver; typedef FEASBILE_SOLUTION_CALLBACK FeasibleSolutionCallback; - typedef std::size_t size_type; + typedef size_t size_type; typedef Problem ProblemType; typedef Solution SolutionType; typedef andres::Timer TimerType; @@ -53,13 +53,14 @@ private: SubgraphMask(const PoseEstimatorType& poseEstimator) : poseEstimator_(poseEstimator) {} - bool vertex(const std::size_t v) const + bool vertex(const size_t v) const { return true; } - bool edge(const std::size_t e) const + bool edge(const size_t e) const { - const size_type v0 = poseEstimator_.detectionGraph_.vertexOfEdge(e, 0); - const size_type v1 = poseEstimator_.detectionGraph_.vertexOfEdge(e, 1); - return poseEstimator_.ilpSolver_.label(poseEstimator_.y(v0, v1)) == 1; + size_t const v0 = poseEstimator_.detectionGraph_.vertexOfEdge(e, 0); + size_t const v1 = poseEstimator_.detectionGraph_.vertexOfEdge(e, 1); + + return poseEstimator_.ilpSolver_.label(poseEstimator_.y(v0, v1)) > .5; } const PoseEstimatorType& poseEstimator_; }; @@ -78,13 +79,13 @@ private: SubgraphMask(CallbackType& callback) : callback_(callback) {} - bool vertex(const std::size_t v) const + bool vertex(const size_t v) const { return true; } - bool edge(const std::size_t e) const + bool edge(const size_t e) const { const size_type v0 = callback_.poseEstimator_.detectionGraph_.vertexOfEdge(e, 0); const size_type v1 = callback_.poseEstimator_.detectionGraph_.vertexOfEdge(e, 1); - return callback_.label(callback_.poseEstimator_.y(v0, v1)) == 1; + return callback_.label(callback_.poseEstimator_.y(v0, v1)) > .5; } Callback& callback_; }; @@ -93,12 +94,8 @@ private: void separateAndAddLazyConstraints(); private: - size_type separateAndAddViolatedUniquenessConstraints(); size_type separateAndAddViolatedSingleClusterConstraints(); - size_type separateAndAddViolatedCouplingConstraints(); size_type separateAndAddViolated3CycleConstraints(); - size_type separateAndAddViolatedLinearizationConstraints(); - size_type separateAndAddViolatedMustSelectClassConstraints(); PoseEstimatorType& poseEstimator_; }; @@ -108,7 +105,7 @@ private: size_type addAllImpossiblePartClassConstraints(); size_type addAllLinearizationConstraints(); size_type addAllCouplingConstraints(); - size_type addAllUniquenessConstraints(); + size_type addAllUniquenessConstraints(bool = false); size_type addPartialSolutionConstraints(const SolutionType &partial); @@ -116,9 +113,9 @@ private: size_type x(const size_type, const size_type) const; size_type y(const size_type, const size_type) const; - size_type numberOfVariablesX_; - size_type numberOfVariablesY_; - size_type numberOfVariablesZ_; + size_type numberOfVariablesX_ { 0 }; + size_type numberOfVariablesY_ { 0 }; + size_type numberOfVariablesZ_ { 0 }; ProblemType& problem_; double epsilonProbability_; CompleteGraphType detectionGraph_; @@ -138,19 +135,16 @@ PoseEstimator::PoseEstimator( const int timeLimit, const double epsilonProbability ) -: numberOfVariablesX_(), - numberOfVariablesY_(), - numberOfVariablesZ_(), +: problem_(problem), epsilonProbability_(epsilonProbability), detectionGraph_(problem.numberOfDetections()), - timer_(), feasibleSolutionCallback_(feasibleSolutionCallback), - ilpSolver_(), withSingleClusterConstraints_(withSingleClusterConstraints) { ilpSolver_.setVerbosity(false); - ilpSolver_.setRelativeGap(0.01); + ilpSolver_.setRelativeGap(.0); + // ilpSolver_.setAbsoluteGap(.0); ilpSolver_.setNumberOfThreads(1); ilpSolver_.setTimeLimit(timeLimit); @@ -175,15 +169,17 @@ PoseEstimator::PoseEstimator( ComponentsType components; components.build(detectionGraph_, SubgraphMask(*this)); solution.resize(problem_.numberOfDetections()); - for(size_type d = 0; d < problem.numberOfDetections(); ++d) { - solution[d].clusterIndex_ = components.labels_[d]; - solution[d].partClass_ = std::numeric_limits::max(); // suppressed - for(size_type c = 0; c < problem.numberOfPartClasses(); ++c) { - if(ilpSolver_.label(x(d, c)) == 1) { + for (size_t d = 0; d < problem.numberOfDetections(); ++d) + { + solution[d].clusterIndex_ = components.labels_[d]; + solution[d].partClass_ = std::numeric_limits::max(); // suppressed + + for (size_t c = 0; c < problem.numberOfPartClasses(); ++c) + if(ilpSolver_.label(x(d, c)) > .5) + { solution[d].partClass_ = c; break; } - } } } @@ -198,33 +194,32 @@ PoseEstimator::setObjectiveFunction() { std::vector coefficients(numberOfVariablesX_ + numberOfVariablesY_); // set coefficients of variables x - for(size_type d = 0; d < problem_.numberOfDetections(); ++d) - for(size_type c = 0; c < problem_.numberOfPartClasses(); ++c) { - const double p = problem_.getPartClassProbability(d, c); - if(p > epsilonProbability_) { - const size_type vi = x(d, c); - coefficients[vi] = std::log( (1.0 - p) / p ); + for (size_t d = 0; d < problem_.numberOfDetections(); ++d) + for (size_t c = 0; c < problem_.numberOfPartClasses(); ++c) + { + double const p = problem_.getPartClassProbability(d, c); + + if (p > epsilonProbability_) + coefficients[x(d, c)] = std::log( (1.0 - p) / p ); } - } // introduce variables z and set their coefficients: - for(size_type d0 = 0; d0 < problem_.numberOfDetections(); ++d0) - for(size_type d1 = d0 + 1; d1 < problem_.numberOfDetections(); ++d1) { - for(size_type c0 = 0; c0 < problem_.numberOfPartClasses(); ++c0) - for(size_type c1 = 0; c1 < problem_.numberOfPartClasses(); ++c1) { - const JoinIndexType joinIndex(d0, d1, c0, c1); - auto it = problem_.joinMap().find(joinIndex); - if(it != problem_.joinMap().end()) { - const double p = it->second.getProbability(); - if(p > epsilonProbability_) { - const double c = std::log( (1.0 - p) / p ); - it->second.setVariableIndex(coefficients.size()); - coefficients.push_back(c); - ++numberOfVariablesZ_; + for (size_t d0 = 0; d0 < problem_.numberOfDetections(); ++d0) + for (size_t d1 = d0 + 1; d1 < problem_.numberOfDetections(); ++d1) + for (size_t c0 = 0; c0 < problem_.numberOfPartClasses(); ++c0) + for (size_t c1 = 0; c1 < problem_.numberOfPartClasses(); ++c1) + { + auto it = problem_.joinMap().find(JoinIndexType(d0, d1, c0, c1)); + + double const p = it->second.getProbability(); + + if (p > epsilonProbability_) + { + it->second.setVariableIndex(coefficients.size()); + coefficients.push_back(std::log( (1.0 - p) / p )); + ++numberOfVariablesZ_; + } } - } - } - } ilpSolver_.initModel(coefficients.size(), coefficients.data()); @@ -235,22 +230,23 @@ PoseEstimator::setObjectiveFunction() { template typename PoseEstimator::size_type -PoseEstimator::addAllImpossiblePartClassConstraints() { +PoseEstimator::addAllImpossiblePartClassConstraints() +{ std::cout << "adding all impossible part class constraints: " << std::flush; - size_type n = 0; - for(size_type d = 0; d < problem_.numberOfDetections(); ++d) - for(size_type c = 0; c < problem_.numberOfPartClasses(); ++c) { - const double p = problem_.getPartClassProbability(d, c); - if(p <= epsilonProbability_) { - const size_type vi[] = {x(d, c)}; - const double c[] = {1.0}; - const double lowerBound = 0.0; - const double upperBound = 0.0; - ilpSolver_.addConstraint(vi, vi + 1, c, lowerBound, upperBound); // x_{d, c} = 0 - n++; - } - } + size_t n = 0; + + size_t vi[1]; + double const c[] = { 1.0 }; + for (size_t d = 0; d < problem_.numberOfDetections(); ++d) + for (size_t l = 0; l < problem_.numberOfPartClasses(); ++l) + if (problem_.getPartClassProbability(d, l) <= epsilonProbability_) + { + vi[0] = x(d, l); + + ilpSolver_.addConstraint(vi, vi + 1, c, .0, .0); // x_{d, c} = 0 + ++n; + } std::cout << n << std::endl; @@ -259,78 +255,92 @@ PoseEstimator::addAllImpossiblePartClass template typename PoseEstimator::size_type -PoseEstimator::addAllLinearizationConstraints() { +PoseEstimator::addAllLinearizationConstraints() +{ std::cout << "adding all linearization constraints: " << std::flush; - std::size_t n = 0; - size_type vi[] = {0, 0, 0, 0}; - const double lowerBound = -std::numeric_limits::infinity(); - const double upperBound = 2.0; - for(size_type d0 = 0; d0 < problem_.numberOfDetections(); ++d0) - for(size_type d1 = d0 + 1; d1 < problem_.numberOfDetections(); ++d1) { - vi[0] = y(d0, d1); - { // if y_{d0, d1} = 1 - const double c[] = {1.0, 1.0, 1.0, -1.0}; - for(size_type c0 = 0; c0 < problem_.numberOfPartClasses(); ++c0) { - vi[1] = x(d0, c0); - for(size_type c1 = 0; c1 < problem_.numberOfPartClasses(); ++c1) { - vi[2] = x(d1, c1); - const JoinIndexType joinIndex(d0, d1, c0, c1); - auto it = problem_.joinMap().find(joinIndex); - if(it == problem_.joinMap().end() - || it->second.getProbability() <= epsilonProbability_) { // if z_{d0, d1, c0, c1} is not explicitly in the ILP - // z_{d0, d1, c0, c1} = 0. Thus: - ilpSolver_.addConstraint(vi, vi + 3, c, lowerBound, upperBound); - ++n; - } - else { // if z_{d0, d1, c0, c1} is explicitly in the ILP - vi[3] = it->second.getVariableIndex(); // z_{d0, d1, c0, c1} - ilpSolver_.addConstraint(vi, vi + 4, c, lowerBound, upperBound); - ++n; + size_t n = 0; + + size_t vi[] = { 0, 0, 0, 0 }; + + double const lowerBound = -std::numeric_limits::infinity(); + double const upperBound = 2.0; + for (size_t d0 = 0; d0 < problem_.numberOfDetections(); ++d0) + for (size_t d1 = d0 + 1; d1 < problem_.numberOfDetections(); ++d1) + { + vi[0] = y(d0, d1); + + { // if y_{d0, d1} = 1 + double const c[] = { 1.0, 1.0, 1.0, -1.0 }; + + for (size_t c0 = 0; c0 < problem_.numberOfPartClasses(); ++c0) + { + vi[1] = x(d0, c0); + + for (size_t c1 = 0; c1 < problem_.numberOfPartClasses(); ++c1) + { + vi[2] = x(d1, c1); + + auto it = problem_.joinMap().find(JoinIndexType(d0, d1, c0, c1)); + + if (it->second.getProbability() <= epsilonProbability_) // if z_{d0, d1, c0, c1} is not explicitly in the ILP + { + // z_{d0, d1, c0, c1} = 0. Thus: + ilpSolver_.addConstraint(vi, vi + 3, c, lowerBound, upperBound); + ++n; + } + else // if z_{d0, d1, c0, c1} is explicitly in the ILP + { + vi[3] = it->second.getVariableIndex(); // z_{d0, d1, c0, c1} + + ilpSolver_.addConstraint(vi, vi + 4, c, lowerBound, upperBound); + ++n; + } } } } + { // if y_{d0, d1} = 0 + const double c[] = {-1.0, 1.0}; + + for (size_t c0 = 0; c0 < problem_.numberOfPartClasses(); ++c0) + for (size_t c1 = 0; c1 < problem_.numberOfPartClasses(); ++c1) + { + auto it = problem_.joinMap().find(JoinIndexType(d0, d1, c0, c1)); + + if (it->second.getProbability() > epsilonProbability_) // if z_{d0, d1, c0, c1} is explicitly in the ILP + { + vi[1] = it->second.getVariableIndex(); // z_{d0, d1, c0, c1} + + ilpSolver_.addConstraint(vi, vi + 2, c, lowerBound, .0); + ++n; + } + } + } } - { // if y_{d0, d1} = 0 - const double c[] = {-1.0, 1.0}; - for(size_type c0 = 0; c0 < problem_.numberOfPartClasses(); ++c0) - for(size_type c1 = 0; c1 < problem_.numberOfPartClasses(); ++c1) { - const JoinIndexType joinIndex(d0, d1, c0, c1); - auto it = problem_.joinMap().find(joinIndex); - if (it != problem_.joinMap().end()) {// if z_{d0, d1, c0, c1} is explicitly in the ILP - vi[1] = it->second.getVariableIndex(); // z_{d0, d1, c0, c1} - ilpSolver_.addConstraint(vi, vi + 2, c, lowerBound, 0.0); - ++n; - } - } - } - } - const double c[] = {-1.0, 1.0}; - for(size_type d0 = 0; d0 < problem_.numberOfDetections(); ++d0) - for(size_type c0 = 0; c0 < problem_.numberOfPartClasses(); ++c0) { - vi[0] = x(d0, c0); - for(size_type d1 = d0 + 1; d1 < problem_.numberOfDetections(); ++d1) - for(size_type c1 = 0; c1 < problem_.numberOfPartClasses(); ++c1) { - { - const JoinIndexType joinIndex(d0, d1, c0, c1); - auto it = problem_.joinMap().find(joinIndex); - if (it != problem_.joinMap().end()) { // if z_{d0, d1, c0, c1} is explicitly in the ILP - vi[1] = it->second.getVariableIndex(); // z_{d0, d1, c0, c1} - ilpSolver_.addConstraint(vi, vi + 2, c, lowerBound, 0.0); - ++n; - } - }{ - const JoinIndexType joinIndex(d1, d0, c1, c0); // note: different order - auto it = problem_.joinMap().find(joinIndex); - if (it != problem_.joinMap().end()) { // if z_{d1, d0, c1, c0} is explicitly in the ILP - vi[1] = it->second.getVariableIndex(); // z_{d1, d0, c1, c0} - ilpSolver_.addConstraint(vi, vi + 2, c, lowerBound, 0.0); + double const c[] = { 1.0, -1.0 }; + for (size_t d0 = 0; d0 < problem_.numberOfDetections(); ++d0) + for (size_t d1 = d0 + 1; d1 < problem_.numberOfDetections(); ++d1) + for (size_t c0 = 0; c0 < problem_.numberOfPartClasses(); ++c0) + for (size_t c1 = 0; c1 < problem_.numberOfPartClasses(); ++c1) + { + auto it = problem_.joinMap().find(JoinIndexType(d0, d1, c0, c1)); + + if (it->second.getProbability() <= epsilonProbability_) + continue; + + vi[0] = it->second.getVariableIndex(); + + vi[1] = x(d0, c0); + + ilpSolver_.addConstraint(vi, vi + 2, c, lowerBound, .0); ++n; + + vi[1] = x(d1, c1); + + ilpSolver_.addConstraint(vi, vi + 2, c, lowerBound, .0); + ++n; } - } - } - } std::cout << n << std::endl; @@ -339,35 +349,39 @@ PoseEstimator::addAllLinearizationConstr template typename PoseEstimator::size_type -PoseEstimator::addAllCouplingConstraints() { +PoseEstimator::addAllCouplingConstraints() +{ std::cout << "adding all coupling constraints: " << std::flush; - size_type n = 0; - std::vector vi(1 + problem_.numberOfPartClasses()); + size_t n = 0; + + std::vector vi(1 + problem_.numberOfPartClasses()); std::vector c(1 + problem_.numberOfPartClasses(), 1.0); + c.back() = -1.0; - const double lowerBound = 0.0; - const double upperBound = std::numeric_limits::infinity(); - for(size_type d0 = 0; d0 < problem_.numberOfDetections(); ++d0) - for(size_type d1 = d0 + 1; d1 < problem_.numberOfDetections(); ++d1) { - vi.back() = y(d0, d1); - // d0 + + double const lowerBound = 0.0; + double const upperBound = std::numeric_limits::infinity(); + + for (size_t d0 = 0; d0 < problem_.numberOfDetections(); ++d0) + for (size_t d1 = d0 + 1; d1 < problem_.numberOfDetections(); ++d1) { - for(size_type c = 0; c < problem_.numberOfPartClasses(); ++c) { + vi.back() = y(d0, d1); + + // d0 + for(size_t c = 0; c < problem_.numberOfPartClasses(); ++c) vi[c] = x(d0, c); // d0 - } + ilpSolver_.addConstraint(vi.begin(), vi.end(), c.begin(), lowerBound, upperBound); // 0 <= (sum_{c \in C} x_{d0, c}) - y_{d0, d1} ++n; - } - // d1 - { - for(size_type c = 0; c < problem_.numberOfPartClasses(); ++c) { + + // d1 + for(size_t c = 0; c < problem_.numberOfPartClasses(); ++c) vi[c] = x(d1, c); // d1 - } + ilpSolver_.addConstraint(vi.begin(), vi.end(), c.begin(), lowerBound, upperBound); // 0 <= (sum_{c \in C} x_{d1, c}) - y_{d0, d1} ++n; } - } std::cout << n << std::endl; @@ -376,22 +390,43 @@ PoseEstimator::addAllCouplingConstraints template typename PoseEstimator::size_type -PoseEstimator::addAllUniquenessConstraints() { +PoseEstimator::addAllUniquenessConstraints(bool mustSelect) +{ std::cout << "adding all uniqueness constraints: " << std::flush; - size_type n = 0; - size_type vi[] = {0, 0}; - const double c[] = {1.0, 1.0}; - const double lowerBound = -std::numeric_limits::infinity(); - const double upperBound = 1.0; - for(size_type d = 0; d < problem_.numberOfDetections(); ++d) { - for(size_type c0 = 0; c0 < problem_.numberOfPartClasses(); ++c0) { - vi[0] = x(d, c0); - for(size_type c1 = c0 + 1; c1 < problem_.numberOfPartClasses(); ++c1) { - vi[1] = x(d, c1); - ilpSolver_.addConstraint(vi, vi + 2, c, lowerBound, upperBound); // x_{d, c0} + x_{d, c1} <= 1 - ++n; + size_t n = 0; + + if (!mustSelect) + { + size_t vi[] = { 0, 0 }; + double const c[] = { 1.0, 1.0 }; + + for (size_t d = 0; d < problem_.numberOfDetections(); ++d) + for (size_t c0 = 0; c0 < problem_.numberOfPartClasses(); ++c0) + { + vi[0] = x(d, c0); + + for(size_t c1 = c0 + 1; c1 < problem_.numberOfPartClasses(); ++c1) + { + vi[1] = x(d, c1); + + ilpSolver_.addConstraint(vi, vi + 2, c, -std::numeric_limits::infinity(), 1.0); // x_{d, c0} + x_{d, c1} <= 1 + ++n; + } } + } + else + { + vector vi(problem_.numberOfPartClasses()); + vector const c(problem_.numberOfPartClasses(), 1.0); + + for (size_t d = 0; d < problem_.numberOfDetections(); ++d) + { + for (size_t l = 0; l < problem_.numberOfPartClasses(); ++l) + vi[l] = x(d, l); + + ilpSolver_.addConstraint(vi.begin(), vi.end(), c.begin(), 1.0, 1.0); // \sum_{l} x_{d, l} = 1 + ++n; } } @@ -525,104 +560,51 @@ PoseEstimator::Callback::Callback( template inline void -PoseEstimator::Callback::separateAndAddLazyConstraints() { +PoseEstimator::Callback::separateAndAddLazyConstraints() +{ poseEstimator_.timer_.stop(); const double elapsedSeconds = poseEstimator_.timer_.elapsedSeconds(); poseEstimator_.timer_.start(); - std::cout - << elapsedSeconds + std::cout << elapsedSeconds << '\t' << IlpSolverCallback::objectiveBound() << '\t' << IlpSolverCallback::objectiveBest() << std::flush; - const size_type nLinearization = separateAndAddViolatedLinearizationConstraints(); - std::cout << '\t' << nLinearization << std::flush; - - const size_type nUniqueness = separateAndAddViolatedUniquenessConstraints(); - std::cout << '\t' << nUniqueness << std::flush; - - const size_type nCoupling = separateAndAddViolatedCouplingConstraints(); - std::cout << '\t' << nCoupling << std::flush; - - const size_type nCycle = separateAndAddViolated3CycleConstraints(); + size_t const nCycle = separateAndAddViolated3CycleConstraints(); std::cout << '\t' << nCycle << std::flush; - //const size_type nMustSelectClass = separateAndAddViolatedMustSelectClassConstraints(); - //std::cout << '\t' << nMustSelectClass << std::flush; - - size_type nSingleCluster = 0; - if(poseEstimator_.withSingleClusterConstraints_) { + size_t nSingleCluster = 0; + if (poseEstimator_.withSingleClusterConstraints_) + { nSingleCluster = separateAndAddViolatedSingleClusterConstraints(); + std::cout << '\t' << nSingleCluster << std::flush; } - std::cout << '\t' << nSingleCluster << std::flush; std::cout << std::endl; - if(nLinearization + nUniqueness + nCoupling + nCycle + nSingleCluster == 0) { + if (nCycle + nSingleCluster == 0) + { // save intermediate feasible solution ComponentsType components; components.build(poseEstimator_.detectionGraph_, SubgraphMask(*this)); + typename PoseEstimatorType::SolutionType solution(poseEstimator_.problem_.numberOfDetections()); - for(size_type d = 0; d < poseEstimator_.problem_.numberOfDetections(); ++d) { - solution[d].clusterIndex_ = components.labels_[d]; - solution[d].partClass_ = std::numeric_limits::max(); // suppressed - for(size_type c = 0; c < poseEstimator_.problem_.numberOfPartClasses(); ++c) { - if(this->label(poseEstimator_.x(d, c)) == 1) { + for (size_t d = 0; d < poseEstimator_.problem_.numberOfDetections(); ++d) + { + solution[d].clusterIndex_ = components.labels_[d]; + solution[d].partClass_ = std::numeric_limits::max(); // suppressed + + for (size_t c = 0; c < poseEstimator_.problem_.numberOfPartClasses(); ++c) + if (this->label(poseEstimator_.x(d, c)) > .5) + { solution[d].partClass_ = c; break; } - } - } - poseEstimator_.feasibleSolutionCallback_(solution); - } -} - -template -typename PoseEstimator::Callback::size_type -PoseEstimator::Callback::separateAndAddViolatedUniquenessConstraints() { - size_type n = 0; - size_type vi[] = {0, 0}; - const double c[] = {1.0, 1.0}; - const double lowerBound = -std::numeric_limits::infinity(); - const double upperBound = 1.0; - for(size_type d = 0; d < poseEstimator_.problem_.numberOfDetections(); ++d) { - for(size_type c0 = 0; c0 < poseEstimator_.problem_.numberOfPartClasses(); ++c0) { - vi[0] = poseEstimator_.x(d, c0); - if(this->label(vi[0]) == 1) { - for(size_type c1 = c0 + 1; c1 < poseEstimator_.problem_.numberOfPartClasses(); ++c1) { - vi[1] = poseEstimator_.x(d, c1); - if(this->label(vi[1]) == 1) { - this->addLazyConstraint(vi, vi + 2, c, lowerBound, upperBound); // x_{d, c0} + x_{d, c1} <= 1 - ++n; - } - } - } } - } - return n; -} -template -typename PoseEstimator::Callback::size_type -PoseEstimator::Callback::separateAndAddViolatedMustSelectClassConstraints() { - size_type n = 0; - std::vector vi(problem_.numberOfPartClasses()); - std::vector c(problem_.numberOfPartClasses(), 1.0); - const double lowerBound = 1; - const double upperBound = std::numeric_limits::infinity(); - for(size_type d0 = 0; d0 < problem_.numberOfDetections(); ++d0) { - double sum = 0; - for(size_type c = 0; c < problem_.numberOfPartClasses(); ++c) { - vi[c] = x(d0, c); // d0 - sum += ilpSolver_.label(vi[c]); - } - if(sum == 0) { - ilpSolver_.addConstraint(vi.begin(), vi.end(), c.begin(), lowerBound, upperBound); // 1 <= (sum_{c \in C} x_{d0, c}) - ++n; - } + poseEstimator_.feasibleSolutionCallback_(solution); } - return n; } template @@ -656,200 +638,33 @@ PoseEstimator::Callback::separateAndAddV template typename PoseEstimator::Callback::size_type -PoseEstimator::Callback::separateAndAddViolatedCouplingConstraints() { - size_type n = 0; - std::vector vi(1 + poseEstimator_.problem_.numberOfPartClasses()); - std::vector c(1 + poseEstimator_.problem_.numberOfPartClasses(), 1.0); - c.back() = -1.0; - const double lowerBound = 0.0; - const double upperBound = std::numeric_limits::infinity(); - for(size_type d0 = 0; d0 < poseEstimator_.problem_.numberOfDetections(); ++d0) - for(size_type d1 = d0 + 1; d1 < poseEstimator_.problem_.numberOfDetections(); ++d1) { - vi.back() = poseEstimator_.y(d0, d1); - if(this->label(vi.back()) == 1) { - // d0 - { - double sum = 0; - for(size_type c = 0; c < poseEstimator_.problem_.numberOfPartClasses(); ++c) { - vi[c] = poseEstimator_.x(d0, c); // d0 - sum += this->label(vi[c]); - } - if(sum == 0) { - this->addLazyConstraint(vi.begin(), vi.end(), c.begin(), lowerBound, upperBound); // 0 <= (sum_{c \in C} x_{d0, c}) - y_{d0, d1} - ++n; - } - } - // d1 - { - double sum = 0; - for(size_type c = 0; c < poseEstimator_.problem_.numberOfPartClasses(); ++c) { - vi[c] = poseEstimator_.x(d1, c); // d1 - sum += this->label(vi[c]); - } - if(sum == 0) { - this->addLazyConstraint(vi.begin(), vi.end(), c.begin(), lowerBound, upperBound); // 0 <= (sum_{c \in C} x_{d1, c}) - y_{d0, d1} - ++n; - } - } - } - } +PoseEstimator::Callback::separateAndAddViolated3CycleConstraints() { + size_t n = 0; - return n; -} + double const coefficients[] = {-1.0, 1.0, 1.0}; + double const lowerBound = -std::numeric_limits::infinity(); + double const upperBound = 1.0; -template -typename PoseEstimator::Callback::size_type -PoseEstimator::Callback::separateAndAddViolated3CycleConstraints() { - std::size_t n = 0; - /* working Theta(n^3) implementation - std::size_t vi[] = {0, 0, 0}; - for(std::size_t j = 0; j < poseEstimator_.problem_.numberOfDetections(); ++j) { - for(std::size_t k = j + 1; k < poseEstimator_.problem_.numberOfDetections(); ++k) { - vi[0] = poseEstimator_.y(j, k); - for(std::size_t l = k + 1; l < poseEstimator_.problem_.numberOfDetections(); ++l) { - vi[1] = poseEstimator_.y(k, l); - vi[2] = poseEstimator_.y(j, l); - const double lowerBound = -1.0; - const double upperBound = 1.0;//std::numeric_limits::infinity(); - if(this->label(vi[0]) == 1) { - if(this->label(vi[1]) == 1) { - if(this->label(vi[2]) == 0) { - const double coefficients[] = {1.0, 1.0, -1.0}; - this->addLazyConstraint(vi, vi + 3, coefficients, lowerBound, upperBound); - ++n; - } - } - else { - if(this->label(vi[2]) == 1) { - const double coefficients[] = {1.0, -1.0, 1.0}; - this->addLazyConstraint(vi, vi + 3, coefficients, lowerBound, upperBound); - ++n; - } - } - } - else { - if(this->label(vi[1]) == 1 && this->label(vi[2]) == 1) { - const double coefficients[] = {-1.0, 1.0, 1.0}; - this->addLazyConstraint(vi, vi + 3, coefficients, lowerBound, upperBound); - ++n; - } - } - } - } - } - */ + size_t vi[] = { 0, 0, 0 }; + for (size_t edge = 0; edge < poseEstimator_.detectionGraph_.numberOfEdges(); ++edge) + { + size_t const v0 = poseEstimator_.detectionGraph_.vertexOfEdge(edge, 0); + size_t const v1 = poseEstimator_.detectionGraph_.vertexOfEdge(edge, 1); - // Theta(n^2) implementation - ComponentsType components; - components.build(poseEstimator_.detectionGraph_, SubgraphMask(*this)); - std::size_t vi[] = {0, 0, 0}; - for(std::size_t edge = 0; edge < poseEstimator_.detectionGraph_.numberOfEdges(); ++edge) { - const std::size_t v0 = poseEstimator_.detectionGraph_.vertexOfEdge(edge, 0); - const std::size_t v1 = poseEstimator_.detectionGraph_.vertexOfEdge(edge, 1); vi[0] = poseEstimator_.y(v0, v1); - if(this->label(vi[0]) == 0) { // cut - for(size_t v2 = 0; v2 < poseEstimator_.detectionGraph_.numberOfVertices(); ++v2) { + + if (this->label(vi[0]) < .5) // cut + for (size_t v2 = 0; v2 < poseEstimator_.detectionGraph_.numberOfVertices(); ++v2) + { vi[1] = poseEstimator_.y(v2, v0); vi[2] = poseEstimator_.y(v2, v1); - if(this->label(vi[1]) == 1 && this->label(vi[2]) == 1) { // join, join - const double coefficients[] = {-1.0, 1.0, 1.0}; - const double lowerBound = -1.0; - const double upperBound = 1.0;//std::numeric_limits::infinity(); - this->addLazyConstraint(vi, vi + 3, coefficients, lowerBound, upperBound); - ++n; - } - } - } - } - return n; -} -template -typename PoseEstimator::Callback::size_type -PoseEstimator::Callback::separateAndAddViolatedLinearizationConstraints() { - std::size_t n = 0; - size_type vi[] = {0, 0, 0, 0}; - const double lowerBound = -std::numeric_limits::infinity(); - const double upperBound = 2.0; - for(size_type d0 = 0; d0 < poseEstimator_.problem_.numberOfDetections(); ++d0) - for(size_type d1 = d0 + 1; d1 < poseEstimator_.problem_.numberOfDetections(); ++d1) { - vi[0] = poseEstimator_.y(d0, d1); - if(this->label(vi[0]) == 1) { // if y_{d0, d1} = 1 - const double c[] = {1.0, 1.0, 1.0, -1.0}; - for(size_type c0 = 0; c0 < poseEstimator_.problem_.numberOfPartClasses(); ++c0) { - vi[1] = poseEstimator_.x(d0, c0); - if(this->label(vi[1]) == 1) { // if x_{d0, c0} = 1 - for(size_type c1 = 0; c1 < poseEstimator_.problem_.numberOfPartClasses(); ++c1) { - vi[2] = poseEstimator_.x(d1, c1); - if(this->label(vi[2]) == 1) { // if x_{d1, c1} = 1 - const JoinIndexType joinIndex(d0, d1, c0, c1); - auto it = poseEstimator_.problem_.joinMap().find(joinIndex); - if(it == poseEstimator_.problem_.joinMap().end() - || it->second.getProbability() <= poseEstimator_.epsilonProbability_) { // if z_{d0, d1, c0, c1} is not explicitly in the ILP - // z_{d0, d1, c0, c1} = 0. Thus: - this->addLazyConstraint(vi, vi + 3, c, lowerBound, upperBound); - ++n; - } - else { // if z_{d0, d1, c0, c1} is explicitly in the ILP - vi[3] = it->second.getVariableIndex(); // z_{d0, d1, c0, c1} - if(this->label(vi[3]) == 0) { // if z_{d0, d1, c0, c1} = 0 - this->addLazyConstraint(vi, vi + 4, c, lowerBound, upperBound); - ++n; - } - - } - } - } - } - } - } - else { // if y_{d0, d1} = 0 - const double c[] = {-1.0, 1.0}; - for(size_type c0 = 0; c0 < poseEstimator_.problem_.numberOfPartClasses(); ++c0) - for(size_type c1 = 0; c1 < poseEstimator_.problem_.numberOfPartClasses(); ++c1) { - const JoinIndexType joinIndex(d0, d1, c0, c1); - auto it = poseEstimator_.problem_.joinMap().find(joinIndex); - if (it != poseEstimator_.problem_.joinMap().end()) {// if z_{d0, d1, c0, c1} is explicitly in the ILP - vi[1] = it->second.getVariableIndex(); // z_{d0, d1, c0, c1} - if(this->label(vi[1]) == 1) { // if z_{d0, d1, c0, c1} = 1 - this->addLazyConstraint(vi, vi + 2, c, lowerBound, 0.0); - ++n; - } - } - } - } - } - - const double c[] = {-1.0, 1.0}; - for(size_type d0 = 0; d0 < poseEstimator_.problem_.numberOfDetections(); ++d0) - for(size_type c0 = 0; c0 < poseEstimator_.problem_.numberOfPartClasses(); ++c0) { - vi[0] = poseEstimator_.x(d0, c0); - if(this->label(vi[0]) == 0) { // if x_{d0, c0} = 0 - for(size_type d1 = d0 + 1; d1 < poseEstimator_.problem_.numberOfDetections(); ++d1) - for(size_type c1 = 0; c1 < poseEstimator_.problem_.numberOfPartClasses(); ++c1) { + if (this->label(vi[1]) > .5 && this->label(vi[2]) > .5) // join, join { - const JoinIndexType joinIndex(d0, d1, c0, c1); - auto it = poseEstimator_.problem_.joinMap().find(joinIndex); - if (it != poseEstimator_.problem_.joinMap().end()) { // if z_{d0, d1, c0, c1} is explicitly in the ILP - vi[1] = it->second.getVariableIndex(); // z_{d0, d1, c0, c1} - if(this->label(vi[1]) == 1) { // if z_{d0, d1, c0, c1} = 1 - this->addLazyConstraint(vi, vi + 2, c, lowerBound, 0.0); - ++n; - } - } - }{ - const JoinIndexType joinIndex(d1, d0, c1, c0); // note: different order - auto it = poseEstimator_.problem_.joinMap().find(joinIndex); - if (it != poseEstimator_.problem_.joinMap().end()) { // if z_{d1, d0, c1, c0} is explicitly in the ILP - vi[1] = it->second.getVariableIndex(); // z_{d1, d0, c1, c0} - if(this->label(vi[1]) == 1) { // if z_{d1, d0, c1, c0} = 1 - this->addLazyConstraint(vi, vi + 2, c, lowerBound, 0.0); - ++n; - } - } + this->addLazyConstraint(vi, vi + 3, coefficients, lowerBound, upperBound); + ++n; } } - } } return n; diff --git a/include/pose/solver.hxx b/include/pose/solver.hxx index dfbf4e2..10249e2 100644 --- a/include/pose/solver.hxx +++ b/include/pose/solver.hxx @@ -56,7 +56,7 @@ private: size_type separateAndAddViolatedSingleClusterConstraints(); size_type separateAndAddViolatedCouplingConstraints(); size_type separateAndAddViolated3CycleConstraints(); - size_type separateAndAddViolatedLinearizationConstraints(); + size_type addAllViolatedLinearizationConstraints(); size_type separateAndAddViolatedMustSelectClassConstraints(); // variable indexing @@ -93,34 +93,33 @@ Solver::Solver( solverIndex_(solverIndex) { ilpSolver_.setVerbosity(true); - ilpSolver_.setRelativeGap(0.01);// + ilpSolver_.setRelativeGap(.0);// //ilpSolver_.setNumberOfThreads(2); ilpSolver_.setNumberOfThreads(1); //ilpSolver_.setTimeLimit(timeLimit); setObjectiveFunction(); addAllImpossiblePartClassConstraints(); + addAllViolatedLinearizationConstraints(); for(;;) { // cutting plane loop ilpSolver_.optimize(); const size_type nUniqueness = separateAndAddViolatedUniquenessConstraints(); - const size_type nCoupling = separateAndAddViolatedCouplingConstraints(); const size_type nCycle = separateAndAddViolated3CycleConstraints(); - const size_type nLinearization = separateAndAddViolatedLinearizationConstraints(); //const size_type nMustSelectClass = separateAndAddViolatedMustSelectClassConstraints(); // single cluster contraints if(solverIndex.compare("s") == 0){ const size_type nSingleCluster = separateAndAddViolatedSingleClusterConstraints(); - if(nUniqueness + nCoupling + nCycle + nLinearization + nSingleCluster == 0) { + if(nUniqueness + nCoupling + nCycle + nSingleCluster == 0) { break; } } // multiple clusters cases - if(nUniqueness + nCoupling + nCycle + nLinearization == 0) { + if(nUniqueness + nCoupling + nCycle == 0) { break; } } @@ -423,95 +422,94 @@ Solver::separateAndAddViolated3CycleConstraints() { template typename Solver::size_type -Solver::separateAndAddViolatedLinearizationConstraints() { - std::cout << "separating and adding violated linearization constraints: " << std::flush; +Solver::addAllViolatedLinearizationConstraints() { + std::cout << "adding all linearization constraints: " << std::flush; - std::size_t n = 0; - size_type vi[] = {0, 0, 0, 0}; - const double lowerBound = -std::numeric_limits::infinity(); - const double upperBound = 2.0; - for(size_type d0 = 0; d0 < problem_.numberOfDetections(); ++d0) - for(size_type d1 = d0 + 1; d1 < problem_.numberOfDetections(); ++d1) { - vi[0] = y(d0, d1); - if(ilpSolver_.label(vi[0]) == 1) { // if y_{d0, d1} = 1 - const double c[] = {1.0, 1.0, 1.0, -1.0}; - for(size_type c0 = 0; c0 < problem_.numberOfPartClasses(); ++c0) { - vi[1] = x(d0, c0); - if(ilpSolver_.label(vi[1]) == 1) { // if x_{d0, c0} = 1 - for(size_type c1 = 0; c1 < problem_.numberOfPartClasses(); ++c1) { + size_t n = 0; + + size_t vi[] = { 0, 0, 0, 0 }; + + double const lowerBound = -std::numeric_limits::infinity(); + double const upperBound = 2.0; + for (size_t d0 = 0; d0 < problem_.numberOfDetections(); ++d0) + for (size_t d1 = d0 + 1; d1 < problem_.numberOfDetections(); ++d1) + { + vi[0] = y(d0, d1); + + { // if y_{d0, d1} = 1 + double const c[] = { 1.0, 1.0, 1.0, -1.0 }; + + for (size_t c0 = 0; c0 < problem_.numberOfPartClasses(); ++c0) + { + vi[1] = x(d0, c0); + + for (size_t c1 = 0; c1 < problem_.numberOfPartClasses(); ++c1) + { vi[2] = x(d1, c1); - if(ilpSolver_.label(vi[2]) == 1) { // if x_{d1, c1} = 1 - const JoinIndexType joinIndex(d0, d1, c0, c1); - auto it = problem_.joinMap().find(joinIndex); - if(it == problem_.joinMap().end() - || it->second.getProbability() <= epsilonProbability_) { // if z_{d0, d1, c0, c1} is not explicitly in the ILP - // z_{d0, d1, c0, c1} = 0. Thus: - ilpSolver_.addConstraint(vi, vi + 3, c, lowerBound, upperBound); - ++n; - } - else { // if z_{d0, d1, c0, c1} is explicitly in the ILP - vi[3] = it->second.getVariableIndex(); // z_{d0, d1, c0, c1} - if(ilpSolver_.label(vi[3]) == 0) { // if z_{d0, d1, c0, c1} = 0 - ilpSolver_.addConstraint(vi, vi + 4, c, lowerBound, upperBound); - ++n; - } - - } + + auto it = problem_.joinMap().find(JoinIndexType(d0, d1, c0, c1)); + + if (it->second.getProbability() <= epsilonProbability_) // if z_{d0, d1, c0, c1} is not explicitly in the ILP + { + // z_{d0, d1, c0, c1} = 0. Thus: + ilpSolver_.addConstraint(vi, vi + 3, c, lowerBound, upperBound); + ++n; } - } - } - } - } - else { // if y_{d0, d1} = 0 - const double c[] = {-1.0, 1.0}; - for(size_type c0 = 0; c0 < problem_.numberOfPartClasses(); ++c0) - for(size_type c1 = 0; c1 < problem_.numberOfPartClasses(); ++c1) { - const JoinIndexType joinIndex(d0, d1, c0, c1); - auto it = problem_.joinMap().find(joinIndex); - if (it != problem_.joinMap().end()) {// if z_{d0, d1, c0, c1} is explicitly in the ILP - vi[1] = it->second.getVariableIndex(); // z_{d0, d1, c0, c1} - if(ilpSolver_.label(vi[1]) == 1) { // if z_{d0, d1, c0, c1} = 1 - ilpSolver_.addConstraint(vi, vi + 2, c, lowerBound, 0.0); - ++n; - } - } - } - } - } + else // if z_{d0, d1, c0, c1} is explicitly in the ILP + { + vi[3] = it->second.getVariableIndex(); // z_{d0, d1, c0, c1} - const double c[] = {-1.0, 1.0}; - for(size_type d0 = 0; d0 < problem_.numberOfDetections(); ++d0) - for(size_type c0 = 0; c0 < problem_.numberOfPartClasses(); ++c0) { - vi[0] = x(d0, c0); - if(ilpSolver_.label(vi[0]) == 0) { // if x_{d0, c0} = 0 - for(size_type d1 = d0 + 1; d1 < problem_.numberOfDetections(); ++d1) - for(size_type c1 = 0; c1 < problem_.numberOfPartClasses(); ++c1) { - { - const JoinIndexType joinIndex(d0, d1, c0, c1); - auto it = problem_.joinMap().find(joinIndex); - if (it != problem_.joinMap().end()) { // if z_{d0, d1, c0, c1} is explicitly in the ILP - vi[1] = it->second.getVariableIndex(); // z_{d0, d1, c0, c1} - if(ilpSolver_.label(vi[1]) == 1) { // if z_{d0, d1, c0, c1} = 1 - ilpSolver_.addConstraint(vi, vi + 2, c, lowerBound, 0.0); + ilpSolver_.addConstraint(vi, vi + 4, c, lowerBound, upperBound); ++n; } } - }{ - const JoinIndexType joinIndex(d1, d0, c1, c0); // note: different order - auto it = problem_.joinMap().find(joinIndex); - if (it != problem_.joinMap().end()) { // if z_{d1, d0, c1, c0} is explicitly in the ILP - vi[1] = it->second.getVariableIndex(); // z_{d1, d0, c1, c0} - if(ilpSolver_.label(vi[1]) == 1) { // if z_{d1, d0, c1, c0} = 1 - ilpSolver_.addConstraint(vi, vi + 2, c, lowerBound, 0.0); + } + } + { // if y_{d0, d1} = 0 + const double c[] = {-1.0, 1.0}; + + for (size_t c0 = 0; c0 < problem_.numberOfPartClasses(); ++c0) + for (size_t c1 = 0; c1 < problem_.numberOfPartClasses(); ++c1) + { + auto it = problem_.joinMap().find(JoinIndexType(d0, d1, c0, c1)); + + if (it->second.getProbability() > epsilonProbability_) // if z_{d0, d1, c0, c1} is explicitly in the ILP + { + vi[1] = it->second.getVariableIndex(); // z_{d0, d1, c0, c1} + + ilpSolver_.addConstraint(vi, vi + 2, c, lowerBound, .0); ++n; } } - } } } - } + + double const c[] = { 1.0, -1.0 }; + for (size_t d0 = 0; d0 < problem_.numberOfDetections(); ++d0) + for (size_t d1 = d0 + 1; d1 < problem_.numberOfDetections(); ++d1) + for (size_t c0 = 0; c0 < problem_.numberOfPartClasses(); ++c0) + for (size_t c1 = 0; c1 < problem_.numberOfPartClasses(); ++c1) + { + auto it = problem_.joinMap().find(JoinIndexType(d0, d1, c0, c1)); + + if (it->second.getProbability() <= epsilonProbability_) + continue; + + vi[0] = it->second.getVariableIndex(); + + vi[1] = x(d0, c0); + + ilpSolver_.addConstraint(vi, vi + 2, c, lowerBound, .0); + ++n; + + vi[1] = x(d1, c1); + + ilpSolver_.addConstraint(vi, vi + 2, c, lowerBound, .0); + ++n; + } std::cout << n << std::endl; + return n; }