From 1acdb9276dc48050552754bcfa184e5e25b4f89f Mon Sep 17 00:00:00 2001 From: Paloma de la Puente <paloma.delapuente@upm.es> Date: Wed, 21 Oct 2009 18:42:07 +0000 Subject: [PATCH] alphas instead of indices --- sm/csm/structprior/Constraint.cpp | 3 ++- sm/csm/structprior/Constraint.h | 2 +- sm/csm/structprior/ConstraintManager.cpp | 16 +++++++++++----- sm/csm/structprior/structprior_test.cpp | 1 + 4 files changed, 15 insertions(+), 7 deletions(-) diff --git a/sm/csm/structprior/Constraint.cpp b/sm/csm/structprior/Constraint.cpp index b4fa932..1e914a7 100644 --- a/sm/csm/structprior/Constraint.cpp +++ b/sm/csm/structprior/Constraint.cpp @@ -16,7 +16,7 @@ void Constraint::SetType(int t) type = t; } -Values Constraint::ApplyConstraint(int* indices, double* params) +Values Constraint::ApplyConstraint(double alphas[], double params[]) { Values v; double err = 0; @@ -27,6 +27,7 @@ Values Constraint::ApplyConstraint(int* indices, double* params) case EQUAL_TO_EITHER: { + break; } case LOCK_DIFF: diff --git a/sm/csm/structprior/Constraint.h b/sm/csm/structprior/Constraint.h index b1edb26..a98a54a 100644 --- a/sm/csm/structprior/Constraint.h +++ b/sm/csm/structprior/Constraint.h @@ -29,7 +29,7 @@ protected: int type; // class methods public: - Values ApplyConstraint(int* indices, double* params = NULL); + Values ApplyConstraint(double alphas[], double params[] = NULL); void SetType(int t); }; diff --git a/sm/csm/structprior/ConstraintManager.cpp b/sm/csm/structprior/ConstraintManager.cpp index aece759..9145cd4 100644 --- a/sm/csm/structprior/ConstraintManager.cpp +++ b/sm/csm/structprior/ConstraintManager.cpp @@ -41,6 +41,7 @@ void ConstraintManager::ApplyConstraints() int n = laser_data->nrays; + // select the types of constraints we want to apply for (int i=0;i< constraint_types_to_apply.size();i++) { if (equal_to_either_active && constraint_types_to_apply[i] == EQUAL_TO_EITHER) @@ -70,9 +71,11 @@ void ConstraintManager::ApplyConstraints() for (int j=0; j< mb;j++) { Constraint* c = new Constraint(EQUAL_TO_EITHER); - int* ind; - ind[0] = i-j; ind[1] =i; ind[2] =i+j; - Values v = c->ApplyConstraint(ind); + double* alpha_values; + alpha_values[0]= laser_data->alpha[i-j]; + alpha_values[1]= laser_data->alpha[i]; + alpha_values[2]= laser_data->alpha[i+j]; + Values v = c->ApplyConstraint(alpha_values); constraints.push_back(c); e += v.error; @@ -88,15 +91,18 @@ void ConstraintManager::ApplyConstraints() { int* ind; ind[0] = i; ind[1]=i+1; + double* alpha_values; + alpha_values[0]= laser_data->alpha[i]; + alpha_values[1]= laser_data->alpha[i+1]; double* p; p[0] = PI/2; p[1] = lock_diff_threshold; Constraint* c1 = new Constraint(LOCK_DIFF); - Values v = c1->ApplyConstraint(ind,p); + Values v = c1->ApplyConstraint(alpha_values,p); constraints.push_back(c1); e += v.error; p[0] = -PI/2; Constraint* c2 = new Constraint(LOCK_DIFF); - v = c2->ApplyConstraint(ind,p); + v = c2->ApplyConstraint(alpha_values,p); constraints.push_back(c2); e += v.error; diff --git a/sm/csm/structprior/structprior_test.cpp b/sm/csm/structprior/structprior_test.cpp index 6503250..1d2bafe 100644 --- a/sm/csm/structprior/structprior_test.cpp +++ b/sm/csm/structprior/structprior_test.cpp @@ -24,6 +24,7 @@ int main(int argc, const char** argv) } + // the types of constraints we want to apply (i.e. depending on the environment...) std::vector<int> cons_types; cons_types.push_back(EQUAL_TO_EITHER); //cons_types.push_back(LOCK_DIFF); -- GitLab