changes

sm/CMakeLists.txt

@@ -161,6 +161,7 @@ SET(csm_sources
 ./csm/utils.c
 ./csm/structprior/ConstraintManager.cpp
 ./csm/structprior/Constraint.cpp
+./csm/structprior/MeasurementsLikelihood.cpp
 ./lib/egsl/egsl.c
 ./lib/egsl/egsl_conversions.c
 ./lib/egsl/egsl_misc.c

sm/csm/CMakeLists.txt

@@ -34,6 +34,7 @@ SET(csm_sources
 	
 	structprior/ConstraintManager.cpp
 	structprior/Constraint.cpp
+	structprior/MeasurementsLikelihood.cpp
 
 	gpm/gpm.c
 )

sm/csm/structprior/Constraint.cpp

@@ -20,13 +20,16 @@ Values Constraint::ApplyConstraint(double alphas[], double params[])
 {
 	Values v;
 	double err = 0;
-	double grd[3] = {0,0,0};
+	double* grd;
+	int size = 3;
 	
 	switch(type)
 	{
+		//*********************************************************************************
 		case EQUAL_TO_EITHER:
 		{
-			double e1 = 0.5*(alphas[1]-alphas[0]);
+			size = 3;
+			double e1 = 0.5*(alphas[1]-alphas[0])*(alphas[1]-alphas[0]);
 			double e2 = 0.5*(alphas[2]-alphas[1]); 
 	
 			if (e1 < e2)
@@ -43,17 +46,31 @@ Values Constraint::ApplyConstraint(double alphas[], double params[])
 			
 			break;
 		}
+		//*********************************************************************************
 		case LOCK_DIFF:
 		{
+			size = 2;
+			double bias = params[0]; double threshold = params[1];
+			if (abs( alphas[1] - (alphas[0]+bias) ) < threshold)
+			{
+        		e = 0.5*( alphas[1] - (alphas[0]+bias) );
+        	}
+        	else
+        	{
+        	
+        	}
 			
 			break;
 		}
-	
+		//*********************************************************************************
+		
+		default:
+			sm_debug("Unrecognized type of constraint \n");
 	
 	}
 
 	v.error = err;
-	for (int i=0;i< 3;i++)
+	for (int i=0;i<size;i++)
 		v.grad[i] = grd[i];
 
 	return v;

sm/csm/structprior/ConstraintManager.cpp

 #include "ConstraintManager.h"
 
 
-ConstraintManager::ConstraintManager(LDP ld, std::vector<int> constraint_types)
+ConstraintManager::ConstraintManager(std::vector<int> constraint_types)
 {
 	equal_to_either_num = 10;
 	lock_diff_threshold = deg2rad(5);
 	
-	laser_data = ld;
 	constraint_types_to_apply = constraint_types;
 
 }
@@ -31,7 +30,7 @@ void ConstraintManager::ClearConstraints()
 
 }
 
-void ConstraintManager::ApplyConstraints()
+void ConstraintManager::ApplyConstraintsAlphas(std::vector<double> x_vector)
 {
 	//add as many types of constraints as wished
 	bool equal_to_either_active = true;
@@ -39,8 +38,9 @@ void ConstraintManager::ApplyConstraints()
 	bool lock_diff_active = true;
 	bool apply_lock_diff = false;
 	
-	int n = laser_data->nrays;
+	int n = x_vector.size();
 	
+	//***************************************************************************
 	// select the types of constraints we want to apply
 	for (int i=0;i< constraint_types_to_apply.size();i++)
 	{
@@ -59,22 +59,22 @@ void ConstraintManager::ApplyConstraints()
 		}
 		
 	}
-	
+	//***************************************************************************
 	if(apply_equal_to_either)		//add equal_to_either constraints
 	{
-		
+		//this all could be substituted by a function ApplyEqualToEither
 		for (int i=equal_to_either_num-1;i<n-equal_to_either_num;i++)
 		{
 			
 			int mb = min (i-1, n-i-1);
 			mb = min (mb, equal_to_either_num-1);
-			for (int j=0; j< mb;j++)
+			for (int j=0; j< mb;j++)				//constraints with no consecutive points
 			{
 				Constraint* c = new Constraint(EQUAL_TO_EITHER);
 				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];
+				alpha_values[0]= x_vector[i-j]; 
+				alpha_values[1]= x_vector[i]; 
+				alpha_values[2]= x_vector[i+j];
 				Values v = c->ApplyConstraint(alpha_values);
 				constraints.push_back(c);							
 				e += v.error;
@@ -84,7 +84,7 @@ void ConstraintManager::ApplyConstraints()
 		}
 
 	}
-	
+	//***************************************************************************
 	if(apply_lock_diff)				//add lock_diff constraints
 	{
 		for (int i=0;i<n-1;i++)
@@ -92,8 +92,8 @@ 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];
+			alpha_values[0]= x_vector[i]; 
+			alpha_values[1]= x_vector[i+1];
 			double* p;
 			p[0] = PI/2; p[1] = lock_diff_threshold;
 			Constraint* c1 = new Constraint(LOCK_DIFF);

sm/csm/structprior/ConstraintManager.h

@@ -10,22 +10,25 @@ class ConstraintManager
 {
 public:
 //constructors
-	ConstraintManager(LDP ld, std::vector<int> constraint_types);
+	ConstraintManager(std::vector<int> constraint_types);
 	virtual ~ConstraintManager(void);
 
 //class variables
 	double e;
 	std::vector<Constraint*> constraints;
 protected:
-	LDP laser_data;
 	std::vector<int> constraint_types_to_apply;
 	int equal_to_either_num;
 	double lock_diff_threshold;
 	
 //methods
 public:
-	void ApplyConstraints();
+	void ApplyConstraintsAlphas(std::vector<double> x_vector);
 	void ClearConstraints();
+	
+protected:
+	void ApplyEqualToEither();
+	void ApplyLockDiff();
 
 
 	

sm/csm/structprior/structprior_test.cpp

@@ -2,6 +2,7 @@
 #include <csm/csm_all.h>
 
 #include "ConstraintManager.h"
+#include "MeasurementsLikelihood.h"
 
 
 int main(int argc, const char** argv) 
@@ -29,10 +30,12 @@ int main(int argc, const char** argv)
 	cons_types.push_back(EQUAL_TO_EITHER);
 	//cons_types.push_back(LOCK_DIFF);
 
-	ConstraintManager cons_manager(laserdata, cons_types);
-	cons_manager.ApplyConstraints();
+	ConstraintManager cons_manager(cons_types);
 	
-	//minimizer.Minimize(x,cons_manager.constraints...)...
+	MeasurementsLikelihood f(L2);
+
+	
+	//minimizer.Minimize(laserdata,f,cons_manager...)...