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