Commit a16b7bca authored by daniel's avatar daniel
Browse files

added overlap

parent 859eb9c9
#ifndef REGISTRATION_H
#define REGISTRATION_H
typedef enum regstatus{SUCCESS=0,FAILURE=1,MAX_ITR=2}regStatus;
typedef enum regstatus{ SUCCESS=0, DELTA_SCORE_LIMIT=1, GRADIENT_VANISHED=2, MAX_ITR=3, WRONG_DIRECTION=4, FAILURE=5 }regStatus;
/* SUCCESS = registration success due to any condition
* DELTA_SCORE_LIMIT = registration converged after delta score < th
* GRADIENT_VANISHED = converged, norm of gradient is small
* MAX_ITR = No convergence after max interations has been reached
* WRONG_DIRECTION = Moving in the direction of the gradient didn't improve score
* FAILURE = Unclassified major problem (unreliable)
* */
#endif // REGISTRATION_H
......@@ -230,6 +230,7 @@ bool NDTMatcherD2D::match( NDTMap& targetNDT,
if(nextNDT[i]!=NULL)
delete nextNDT[i];
}
finalscore=score_here;
return true;
}
}
......@@ -246,6 +247,7 @@ bool NDTMatcherD2D::match( NDTMap& targetNDT,
delete nextNDT[i];
}
// std::cout<<"itr "<<itr_ctr<<" dScore "<< 0 <<std::endl;
finalscore=score_here;
return true;
}
// pose_increment_v.block<3,1>(0,0) = -Hessian.block<3,3>(0,0).ldlt().solve(score_gradient.block<3,1>(0,0));
......@@ -313,6 +315,7 @@ bool NDTMatcherD2D::match( NDTMap& targetNDT,
delete nextNDT[i];
}
// std::cout<<"itr "<<itr_ctr<<" dScore "<< 0 <<std::endl;
finalscore=score_here;
return true;
}
// std::cout<<"score("<<itr_ctr+1<<") = "<<score_here<<";\n";
......@@ -395,7 +398,7 @@ bool NDTMatcherD2D::match( NDTMap& targetNDT,
if(itr_ctr>0){
if(convergence = ((pose_increment_v.norm()) < DELTA_SCORE))
status_=SUCCESS;
status_=DELTA_SCORE_LIMIT;
}
if(!convergence && itr_ctr>ITR_MAX)
{
......@@ -425,7 +428,7 @@ bool NDTMatcherD2D::match( NDTMap& targetNDT,
//std::cout<<"res "<<current_resolution<<" itr "<<itr_ctr<<std::endl;
// this->finalscore = score/NUMBER_OF_ACTIVE_CELLS;
finalscore=score_here;
return ret;
}
......
......@@ -176,25 +176,28 @@ bool NDTMatcherD2D_2D::match( NDTMap& targetNDT,
double score_here = derivativesNDT_2d(nextNDT,targetNDT,score_gradient_2d,H,true);
scg = score_gradient_2d;
//temporarily commented std::cout<<"itr "<<itr_ctr<<" score "<<score_here<<std::endl;
if(score_here < score_best)
{
if(score_here < score_best){
Tbest = T;
score_best = score_here;
//temporarily commented std::cout<<"best score "<<score_best<<" at "<<itr_ctr<<std::endl;
}
if (score_gradient_2d.norm()<= 10e-2*DELTA_SCORE)
{
if (score_gradient_2d.norm()<= 10e-2*DELTA_SCORE){
std::cout<<"\%gradient vanished at itr: "<<itr_ctr<<", norm : " << score_gradient_2d.norm() << std::endl;
for(unsigned int i=0; i<nextNDT.size(); i++)
{
if(nextNDT[i]!=NULL)
delete nextNDT[i];
}
if(score_here > score_best)
{
if(score_here > score_best){
std::cout<<"crap iterations, best was "<<score_best<<" last was "<<score_here<<std::endl;
T = Tbest;
finalscore=score_best;
status_=WRONG_DIRECTION;
}
else{
finalscore=score_here;
status_=DELTA_SCORE_LIMIT;
}
return true;
}
......@@ -238,6 +241,7 @@ bool NDTMatcherD2D_2D::match( NDTMap& targetNDT,
//std::cout<<"crap iterations, best was "<<score_best<<" last was "<<score_here<<std::endl;
T = Tbest;
}
finalscore=score_here;
return true;
}
if(step_control) {
......@@ -296,7 +300,7 @@ bool NDTMatcherD2D_2D::match( NDTMap& targetNDT,
if(nextNDT[i]!=NULL)
delete nextNDT[i];
}
finalscore=score_here;
return ret;
}
......
......@@ -523,9 +523,8 @@ bool NDTMatcherD2DSC::match( const NDTMap& targetNDT,
Hessian = evecs*Lam*(evecs.transpose());
} else {
if(score_here > score_best)
{
T = Tbest;
}
//de-alloc nextNDT
for(unsigned int i=0; i<nextNDT.size(); i++)
{
......@@ -533,22 +532,21 @@ bool NDTMatcherD2DSC::match( const NDTMap& targetNDT,
delete nextNDT[i];
}
status_=SUCCESS;
finalscore=score_here;
return true;
}
}
if (score_gradient.norm()<= DELTA_SCORE)
{
if(score_here > score_best)
{
if (score_gradient.norm()<= DELTA_SCORE){
if(score_here > score_best){
T = Tbest;
}
//de-alloc nextNDT
for(unsigned int i=0; i<nextNDT.size(); i++)
{
for(unsigned int i=0; i<nextNDT.size(); i++){
if(nextNDT[i]!=NULL)
delete nextNDT[i];
}
status_=SUCCESS;
finalscore=score_here;
status_=DELTA_SCORE_LIMIT;
return true;
}
......@@ -557,27 +555,26 @@ bool NDTMatcherD2DSC::match( const NDTMap& targetNDT,
pose_increment_v = -Hessian.ldlt().solve(score_gradient);
double dginit = pose_increment_v.dot(scg);
if(dginit > 0)
{
if(dginit > 0){
//de-alloc nextNDT
if(score_here > score_best)
{
if(score_here > score_best){
T = Tbest;
}
for(unsigned int i=0; i<nextNDT.size(); i++)
{
for(unsigned int i=0; i<nextNDT.size(); i++){
if(nextNDT[i]!=NULL)
delete nextNDT[i];
}
finalscore=score_here;
status_=SUCCESS;
return true;
}
//check direction here:
if(step_control) {
if(step_control)
step_size = lineSearchMTSC(pose_increment_v,nextNDT,targetNDT,pose_local_v, Q, T);
} else {
else
step_size = 1;
}
pose_increment_v = step_size*pose_increment_v;
// const double max_step = 1.0;
......@@ -604,8 +601,7 @@ bool NDTMatcherD2DSC::match( const NDTMap& targetNDT,
pose_increment_v_Tframe(2) = Tposition_diff(2);
pose_local_v += pose_increment_v_Tframe;
for(unsigned int i=0; i<nextNDT.size(); i++)
{
for(unsigned int i=0; i<nextNDT.size(); i++){
//TRANSFORM
Eigen::Vector3d meanC = nextNDT[i]->getMean();
Eigen::Matrix3d covC = nextNDT[i]->getCov();
......@@ -615,12 +611,11 @@ bool NDTMatcherD2DSC::match( const NDTMap& targetNDT,
nextNDT[i]->setCov(covC);
}
if(itr_ctr>0)
{
convergence = ((pose_increment_v.norm()) < DELTA_SCORE);
if(itr_ctr>0){
if(convergence = ((pose_increment_v.norm()) < DELTA_SCORE))
status_=DELTA_SCORE_LIMIT;
}
if(itr_ctr>ITR_MAX)
{
if(itr_ctr>ITR_MAX){
convergence = true;
ret = false;
status_=MAX_ITR;
......@@ -644,15 +639,13 @@ bool NDTMatcherD2DSC::match( const NDTMap& targetNDT,
score_here += alpha*computeScoreMahalanobis(X,Q);
if(score_here > score_best)
{
T = Tbest;
}
for(unsigned int i=0; i<nextNDT.size(); i++)
{
for(unsigned int i=0; i<nextNDT.size(); i++){
if(nextNDT[i]!=NULL)
delete nextNDT[i];
}
finalscore=score_here;
return ret;
}
} // namespace
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment