505 lines
16 KiB
C++
505 lines
16 KiB
C++
/*
|
|
* go.h
|
|
*
|
|
* Created on: Jun 4, 2012
|
|
* Author: aitor
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <random>
|
|
|
|
#include <boost/graph/graph_traits.hpp>
|
|
#include <boost/graph/adjacency_list.hpp>
|
|
|
|
//includes required by mets.hh
|
|
#include <boost/random/linear_congruential.hpp>
|
|
#include <boost/random/mersenne_twister.hpp>
|
|
#include <boost/random/variate_generator.hpp>
|
|
|
|
#include <pcl/pcl_macros.h>
|
|
#include <pcl/recognition/hv/hypotheses_verification.h>
|
|
#include <pcl/recognition/3rdparty/metslib/mets.hh>
|
|
#include <pcl/features/normal_3d.h>
|
|
|
|
#include <memory>
|
|
|
|
namespace pcl
|
|
{
|
|
|
|
/** \brief A hypothesis verification method proposed in
|
|
* "A Global Hypotheses Verification Method for 3D Object Recognition", A. Aldoma and F. Tombari and L. Di Stefano and Markus Vincze, ECCV 2012
|
|
* \author Aitor Aldoma
|
|
*/
|
|
template<typename ModelT, typename SceneT>
|
|
class PCL_EXPORTS GlobalHypothesesVerification: public HypothesisVerification<ModelT, SceneT>
|
|
{
|
|
private:
|
|
|
|
//Helper classes
|
|
struct RecognitionModel
|
|
{
|
|
public:
|
|
std::vector<int> explained_; //indices vector referencing explained_by_RM_
|
|
std::vector<float> explained_distances_; //closest distances to the scene for point i
|
|
std::vector<int> unexplained_in_neighborhood; //indices vector referencing unexplained_by_RM_neighboorhods
|
|
std::vector<float> unexplained_in_neighborhood_weights; //weights for the points not being explained in the neighborhood of a hypothesis
|
|
std::vector<int> outlier_indices_; //outlier indices of this model
|
|
std::vector<int> complete_cloud_occupancy_indices_;
|
|
typename pcl::PointCloud<ModelT>::Ptr cloud_;
|
|
typename pcl::PointCloud<ModelT>::Ptr complete_cloud_;
|
|
int bad_information_;
|
|
float outliers_weight_;
|
|
pcl::PointCloud<pcl::Normal>::Ptr normals_;
|
|
int id_;
|
|
};
|
|
|
|
using RecognitionModelPtr = std::shared_ptr<RecognitionModel>;
|
|
|
|
using SAOptimizerT = GlobalHypothesesVerification<ModelT, SceneT>;
|
|
class SAModel: public mets::evaluable_solution
|
|
{
|
|
public:
|
|
std::vector<bool> solution_;
|
|
SAOptimizerT * opt_;
|
|
mets::gol_type cost_;
|
|
|
|
//Evaluates the current solution
|
|
mets::gol_type cost_function() const override
|
|
{
|
|
return cost_;
|
|
}
|
|
|
|
void copy_from(const mets::copyable& o) override
|
|
{
|
|
const SAModel& s = dynamic_cast<const SAModel&> (o);
|
|
solution_ = s.solution_;
|
|
opt_ = s.opt_;
|
|
cost_ = s.cost_;
|
|
}
|
|
|
|
mets::gol_type what_if(int /*index*/, bool /*val*/) const
|
|
{
|
|
/*std::vector<bool> tmp (solution_);
|
|
tmp[index] = val;
|
|
mets::gol_type sol = opt_->evaluateSolution (solution_, index); //evaluate without updating status
|
|
return sol;*/
|
|
return static_cast<mets::gol_type>(0);
|
|
}
|
|
|
|
mets::gol_type apply_and_evaluate(int index, bool val)
|
|
{
|
|
solution_[index] = val;
|
|
mets::gol_type sol = opt_->evaluateSolution (solution_, index); //this will update the state of the solution
|
|
cost_ = sol;
|
|
return sol;
|
|
}
|
|
|
|
void apply(int /*index*/, bool /*val*/)
|
|
{
|
|
|
|
}
|
|
|
|
void unapply(int index, bool val)
|
|
{
|
|
solution_[index] = val;
|
|
//update optimizer solution
|
|
cost_ = opt_->evaluateSolution (solution_, index); //this will update the cost function in opt_
|
|
}
|
|
void setSolution(std::vector<bool> & sol)
|
|
{
|
|
solution_ = sol;
|
|
}
|
|
|
|
void setOptimizer(SAOptimizerT * opt)
|
|
{
|
|
opt_ = opt;
|
|
}
|
|
};
|
|
|
|
/*
|
|
* Represents a move, deactivate a hypothesis
|
|
*/
|
|
|
|
class move: public mets::move
|
|
{
|
|
int index_;
|
|
public:
|
|
move(int i) :
|
|
index_ (i)
|
|
{
|
|
}
|
|
|
|
mets::gol_type evaluate(const mets::feasible_solution& /*cs*/) const override
|
|
{
|
|
return static_cast<mets::gol_type>(0);
|
|
}
|
|
|
|
mets::gol_type apply_and_evaluate(mets::feasible_solution& cs)
|
|
{
|
|
SAModel& model = dynamic_cast<SAModel&> (cs);
|
|
return model.apply_and_evaluate (index_, !model.solution_[index_]);
|
|
}
|
|
|
|
void apply(mets::feasible_solution& /*s*/) const override
|
|
{
|
|
}
|
|
|
|
void unapply(mets::feasible_solution& s) const
|
|
{
|
|
SAModel& model = dynamic_cast<SAModel&> (s);
|
|
model.unapply (index_, !model.solution_[index_]);
|
|
}
|
|
};
|
|
|
|
class move_manager
|
|
{
|
|
public:
|
|
std::vector<move*> moves_m;
|
|
using iterator = typename std::vector<move *>::iterator;
|
|
iterator begin()
|
|
{
|
|
return moves_m.begin ();
|
|
}
|
|
iterator end()
|
|
{
|
|
return moves_m.end ();
|
|
}
|
|
|
|
move_manager(int problem_size)
|
|
{
|
|
for (int ii = 0; ii != problem_size; ++ii)
|
|
moves_m.push_back (new move (ii));
|
|
}
|
|
|
|
~move_manager()
|
|
{
|
|
// delete all moves
|
|
for (iterator ii = begin (); ii != end (); ++ii)
|
|
delete (*ii);
|
|
}
|
|
|
|
void refresh(mets::feasible_solution& /*s*/)
|
|
{
|
|
std::shuffle (moves_m.begin (), moves_m.end (), std::mt19937(std::random_device()()));
|
|
}
|
|
|
|
};
|
|
|
|
//inherited class attributes
|
|
using HypothesisVerification<ModelT, SceneT>::mask_;
|
|
using HypothesisVerification<ModelT, SceneT>::scene_cloud_downsampled_;
|
|
using HypothesisVerification<ModelT, SceneT>::scene_downsampled_tree_;
|
|
using HypothesisVerification<ModelT, SceneT>::visible_models_;
|
|
using HypothesisVerification<ModelT, SceneT>::complete_models_;
|
|
using HypothesisVerification<ModelT, SceneT>::resolution_;
|
|
using HypothesisVerification<ModelT, SceneT>::inliers_threshold_;
|
|
|
|
//class attributes
|
|
using NormalEstimator_ = pcl::NormalEstimation<SceneT, pcl::Normal>;
|
|
pcl::PointCloud<pcl::Normal>::Ptr scene_normals_;
|
|
pcl::PointCloud<pcl::PointXYZI>::Ptr clusters_cloud_;
|
|
|
|
std::vector<int> complete_cloud_occupancy_by_RM_;
|
|
float res_occupancy_grid_;
|
|
float w_occupied_multiple_cm_;
|
|
|
|
std::vector<int> explained_by_RM_; //represents the points of scene_cloud_ that are explained by the recognition models
|
|
std::vector<float> explained_by_RM_distance_weighted; //represents the points of scene_cloud_ that are explained by the recognition models
|
|
std::vector<float> unexplained_by_RM_neighboorhods; //represents the points of scene_cloud_ that are not explained by the active hypotheses in the neighboorhod of the recognition models
|
|
std::vector<RecognitionModelPtr> recognition_models_;
|
|
std::vector<std::size_t> indices_;
|
|
|
|
float regularizer_;
|
|
float clutter_regularizer_;
|
|
bool detect_clutter_;
|
|
float radius_neighborhood_GO_;
|
|
float radius_normals_;
|
|
|
|
float previous_explained_value;
|
|
int previous_duplicity_;
|
|
int previous_duplicity_complete_models_;
|
|
float previous_bad_info_;
|
|
float previous_unexplained_;
|
|
|
|
int max_iterations_; //max iterations without improvement
|
|
SAModel best_seen_;
|
|
float initial_temp_;
|
|
|
|
int n_cc_;
|
|
std::vector<std::vector<int> > cc_;
|
|
|
|
void setPreviousBadInfo(float f)
|
|
{
|
|
previous_bad_info_ = f;
|
|
}
|
|
|
|
float getPreviousBadInfo()
|
|
{
|
|
return previous_bad_info_;
|
|
}
|
|
|
|
void setPreviousExplainedValue(float v)
|
|
{
|
|
previous_explained_value = v;
|
|
}
|
|
|
|
void setPreviousDuplicity(int v)
|
|
{
|
|
previous_duplicity_ = v;
|
|
}
|
|
|
|
void setPreviousDuplicityCM(int v)
|
|
{
|
|
previous_duplicity_complete_models_ = v;
|
|
}
|
|
|
|
void setPreviousUnexplainedValue(float v)
|
|
{
|
|
previous_unexplained_ = v;
|
|
}
|
|
|
|
float getPreviousUnexplainedValue()
|
|
{
|
|
return previous_unexplained_;
|
|
}
|
|
|
|
float getExplainedValue()
|
|
{
|
|
return previous_explained_value;
|
|
}
|
|
|
|
int getDuplicity()
|
|
{
|
|
return previous_duplicity_;
|
|
}
|
|
|
|
int getDuplicityCM()
|
|
{
|
|
return previous_duplicity_complete_models_;
|
|
}
|
|
|
|
void updateUnexplainedVector(std::vector<int> & unexplained_, std::vector<float> & unexplained_distances, std::vector<float> & unexplained_by_RM,
|
|
std::vector<int> & explained, std::vector<int> & explained_by_RM, float val)
|
|
{
|
|
{
|
|
|
|
float add_to_unexplained = 0.f;
|
|
|
|
for (std::size_t i = 0; i < unexplained_.size (); i++)
|
|
{
|
|
|
|
bool prev_unexplained = (unexplained_by_RM[unexplained_[i]] > 0) && (explained_by_RM[unexplained_[i]] == 0);
|
|
unexplained_by_RM[unexplained_[i]] += val * unexplained_distances[i];
|
|
|
|
if (val < 0) //the hypothesis is being removed
|
|
{
|
|
if (prev_unexplained)
|
|
{
|
|
//decrease by 1
|
|
add_to_unexplained -= unexplained_distances[i];
|
|
}
|
|
} else //the hypothesis is being added and unexplains unexplained_[i], so increase by 1 unless its explained by another hypothesis
|
|
{
|
|
if (explained_by_RM[unexplained_[i]] == 0)
|
|
add_to_unexplained += unexplained_distances[i];
|
|
}
|
|
}
|
|
|
|
for (const int &i : explained)
|
|
{
|
|
if (val < 0)
|
|
{
|
|
//the hypothesis is being removed, check that there are no points that become unexplained and have clutter unexplained hypotheses
|
|
if ((explained_by_RM[i] == 0) && (unexplained_by_RM[i] > 0))
|
|
{
|
|
add_to_unexplained += unexplained_by_RM[i]; //the points become unexplained
|
|
}
|
|
} else
|
|
{
|
|
//std::cout << "being added..." << add_to_unexplained << " " << unexplained_by_RM[explained[i]] << std::endl;
|
|
if ((explained_by_RM[i] == 1) && (unexplained_by_RM[i] > 0))
|
|
{ //the only hypothesis explaining that point
|
|
add_to_unexplained -= unexplained_by_RM[i]; //the points are not unexplained any longer because this hypothesis explains them
|
|
}
|
|
}
|
|
}
|
|
|
|
//std::cout << add_to_unexplained << std::endl;
|
|
previous_unexplained_ += add_to_unexplained;
|
|
}
|
|
}
|
|
|
|
void updateExplainedVector(std::vector<int> & vec, std::vector<float> & vec_float, std::vector<int> & explained_,
|
|
std::vector<float> & explained_by_RM_distance_weighted, float sign)
|
|
{
|
|
float add_to_explained = 0.f;
|
|
int add_to_duplicity_ = 0;
|
|
|
|
for (std::size_t i = 0; i < vec.size (); i++)
|
|
{
|
|
bool prev_dup = explained_[vec[i]] > 1;
|
|
|
|
explained_[vec[i]] += static_cast<int> (sign);
|
|
explained_by_RM_distance_weighted[vec[i]] += vec_float[i] * sign;
|
|
|
|
add_to_explained += vec_float[i] * sign;
|
|
|
|
if ((explained_[vec[i]] > 1) && prev_dup)
|
|
{ //its still a duplicate, we are adding
|
|
add_to_duplicity_ += static_cast<int> (sign); //so, just add or remove one
|
|
} else if ((explained_[vec[i]] == 1) && prev_dup)
|
|
{ //if was duplicate before, now its not, remove 2, we are removing the hypothesis
|
|
add_to_duplicity_ -= 2;
|
|
} else if ((explained_[vec[i]] > 1) && !prev_dup)
|
|
{ //it was not a duplicate but it is now, add 2, we are adding a conflicting hypothesis for the point
|
|
add_to_duplicity_ += 2;
|
|
}
|
|
}
|
|
|
|
//update explained and duplicity values...
|
|
previous_explained_value += add_to_explained;
|
|
previous_duplicity_ += add_to_duplicity_;
|
|
}
|
|
|
|
void updateCMDuplicity(std::vector<int> & vec, std::vector<int> & occupancy_vec, float sign) {
|
|
int add_to_duplicity_ = 0;
|
|
for (const int &i : vec)
|
|
{
|
|
bool prev_dup = occupancy_vec[i] > 1;
|
|
occupancy_vec[i] += static_cast<int> (sign);
|
|
if ((occupancy_vec[i] > 1) && prev_dup)
|
|
{ //its still a duplicate, we are adding
|
|
add_to_duplicity_ += static_cast<int> (sign); //so, just add or remove one
|
|
} else if ((occupancy_vec[i] == 1) && prev_dup)
|
|
{ //if was duplicate before, now its not, remove 2, we are removing the hypothesis
|
|
add_to_duplicity_ -= 2;
|
|
} else if ((occupancy_vec[i] > 1) && !prev_dup)
|
|
{ //it was not a duplicate but it is now, add 2, we are adding a conflicting hypothesis for the point
|
|
add_to_duplicity_ += 2;
|
|
}
|
|
}
|
|
|
|
previous_duplicity_complete_models_ += add_to_duplicity_;
|
|
}
|
|
|
|
float getTotalExplainedInformation(std::vector<int> & explained_, std::vector<float> & explained_by_RM_distance_weighted, int * duplicity_)
|
|
{
|
|
float explained_info = 0;
|
|
int duplicity = 0;
|
|
|
|
for (std::size_t i = 0; i < explained_.size (); i++)
|
|
{
|
|
if (explained_[i] > 0)
|
|
explained_info += explained_by_RM_distance_weighted[i];
|
|
|
|
if (explained_[i] > 1)
|
|
duplicity += explained_[i];
|
|
}
|
|
|
|
*duplicity_ = duplicity;
|
|
|
|
return explained_info;
|
|
}
|
|
|
|
float getTotalBadInformation(std::vector<RecognitionModelPtr> & recog_models)
|
|
{
|
|
float bad_info = 0;
|
|
for (std::size_t i = 0; i < recog_models.size (); i++)
|
|
bad_info += recog_models[i]->outliers_weight_ * static_cast<float> (recog_models[i]->bad_information_);
|
|
|
|
return bad_info;
|
|
}
|
|
|
|
float getUnexplainedInformationInNeighborhood(std::vector<float> & unexplained, std::vector<int> & explained)
|
|
{
|
|
float unexplained_sum = 0.f;
|
|
for (std::size_t i = 0; i < unexplained.size (); i++)
|
|
{
|
|
if (unexplained[i] > 0 && explained[i] == 0)
|
|
unexplained_sum += unexplained[i];
|
|
}
|
|
|
|
return unexplained_sum;
|
|
}
|
|
|
|
//Performs smooth segmentation of the scene cloud and compute the model cues
|
|
void
|
|
initialize();
|
|
|
|
mets::gol_type
|
|
evaluateSolution(const std::vector<bool> & active, int changed);
|
|
|
|
bool
|
|
addModel(typename pcl::PointCloud<ModelT>::ConstPtr & model, typename pcl::PointCloud<ModelT>::ConstPtr & complete_model, RecognitionModelPtr & recog_model);
|
|
|
|
void
|
|
computeClutterCue(RecognitionModelPtr & recog_model);
|
|
|
|
void
|
|
SAOptimize(std::vector<int> & cc_indices, std::vector<bool> & sub_solution);
|
|
|
|
public:
|
|
GlobalHypothesesVerification() : HypothesisVerification<ModelT, SceneT>()
|
|
{
|
|
resolution_ = 0.005f;
|
|
max_iterations_ = 5000;
|
|
regularizer_ = 1.f;
|
|
radius_normals_ = 0.01f;
|
|
initial_temp_ = 1000;
|
|
detect_clutter_ = true;
|
|
radius_neighborhood_GO_ = 0.03f;
|
|
clutter_regularizer_ = 5.f;
|
|
res_occupancy_grid_ = 0.01f;
|
|
w_occupied_multiple_cm_ = 4.f;
|
|
}
|
|
|
|
void
|
|
verify() override;
|
|
|
|
void setResolutionOccupancyGrid(float r)
|
|
{
|
|
res_occupancy_grid_ = r;
|
|
}
|
|
|
|
void setRadiusNormals(float r)
|
|
{
|
|
radius_normals_ = r;
|
|
}
|
|
|
|
void setMaxIterations(int i)
|
|
{
|
|
max_iterations_ = i;
|
|
}
|
|
|
|
void setInitialTemp(float t)
|
|
{
|
|
initial_temp_ = t;
|
|
}
|
|
|
|
void setRegularizer(float r)
|
|
{
|
|
regularizer_ = r;
|
|
}
|
|
|
|
void setRadiusClutter(float r)
|
|
{
|
|
radius_neighborhood_GO_ = r;
|
|
}
|
|
|
|
void setClutterRegularizer(float cr)
|
|
{
|
|
clutter_regularizer_ = cr;
|
|
}
|
|
|
|
void setDetectClutter(bool d)
|
|
{
|
|
detect_clutter_ = d;
|
|
}
|
|
};
|
|
}
|
|
|
|
#ifdef PCL_NO_PRECOMPILE
|
|
#include <pcl/recognition/impl/hv/hv_go.hpp>
|
|
#endif
|