hv_go.h
/*
* go.h
*
* Created on: Jun 4, 2012
* Author: aitor
*/
#ifndef GO_H_
#define GO_H_
#include <pcl/pcl_macros.h>
#include <pcl/recognition/hv/hypotheses_verification.h>
#include <pcl/common/common.h>
#include <pcl/recognition/3rdparty/metslib/mets.hh>
#include <pcl/features/normal_3d.h>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
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_;
};
typedef GlobalHypothesesVerification<ModelT, SceneT> SAOptimizerT;
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
{
return cost_;
}
void copy_from(const mets::copyable& o)
{
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 udpate 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
{
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
{
}
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;
typedef typename std::vector<move*>::iterator 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::random_shuffle (moves_m.begin (), moves_m.end ());
}
};
//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
typedef typename pcl::NormalEstimation<SceneT, pcl::Normal> NormalEstimator_;
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<boost::shared_ptr<RecognitionModel> > recognition_models_;
std::vector<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 (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 (size_t i = 0; i < explained.size (); i++)
{
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[explained[i]] == 0) && (unexplained_by_RM[explained[i]] > 0))
{
add_to_unexplained += unexplained_by_RM[explained[i]]; //the points become unexplained
}
} else
{
//std::cout << "being added..." << add_to_unexplained << " " << unexplained_by_RM[explained[i]] << std::endl;
if ((explained_by_RM[explained[i]] == 1) && (unexplained_by_RM[explained[i]] > 0))
{ //the only hypothesis explaining that point
add_to_unexplained -= unexplained_by_RM[explained[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 (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 (size_t i = 0; i < vec.size (); i++)
{
bool prev_dup = occupancy_vec[vec[i]] > 1;
occupancy_vec[vec[i]] += static_cast<int> (sign);
if ((occupancy_vec[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[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[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 (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<boost::shared_ptr<RecognitionModel> > & recog_models)
{
float bad_info = 0;
for (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 (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,
boost::shared_ptr<RecognitionModel> & recog_model);
void
computeClutterCue(boost::shared_ptr<RecognitionModel> & 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();
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
#endif /* GO_H_ */
pcl::GlobalHypothesesVerification::setClutterRegularizer
void setClutterRegularizer(float cr)
Definition: hv_go.h:476
pcl
Definition: convolution.h:45
pcl::GlobalHypothesesVerification::setRadiusNormals
void setRadiusNormals(float r)
Definition: hv_go.h:451
pcl::NormalEstimation
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
Definition: normal_3d.h:199
pcl::PointCloud::Ptr
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
common.h
Define standard C methods and C++ classes that are common to all methods.
pcl::GlobalHypothesesVerification::setDetectClutter
void setDetectClutter(bool d)
Definition: hv_go.h:481
pcl::GlobalHypothesesVerification::setRadiusClutter
void setRadiusClutter(float r)
Definition: hv_go.h:471
pcl::GlobalHypothesesVerification::setInitialTemp
void setInitialTemp(float t)
Definition: hv_go.h:461
pcl::GlobalHypothesesVerification::setRegularizer
void setRegularizer(float r)
Definition: hv_go.h:466
pcl::HypothesisVerification
Abstract class for hypotheses verification methods.
Definition: hypotheses_verification.h:56
pcl::PointCloud::ConstPtr
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
pcl::GlobalHypothesesVerification::GlobalHypothesesVerification
GlobalHypothesesVerification()
Definition: hv_go.h:434
pcl::GlobalHypothesesVerification::setMaxIterations
void setMaxIterations(int i)
Definition: hv_go.h:456
pcl::GlobalHypothesesVerification
A hypothesis verification method proposed in "A Global Hypotheses Verification Method for 3D Object R...
Definition: hv_go.h:27