some debugging

sm/csm/structprior/Constraint.cpp

@@ -16,9 +16,11 @@ void Constraint::SetType(int t)
 	type = t;
 }
 
-double Constraint::ApplyConstraint(int* indices, double* params)
+Values Constraint::ApplyConstraint(int* indices, double* params)
 {
+	Values v;
 	double err = 0;
+	double grd = 0;
 	
 	switch(type)
 	{
@@ -36,5 +38,8 @@ double Constraint::ApplyConstraint(int* indices, double* params)
 	
 	}
 
-	return err;
+	v.error = err;
+	v.grad = grd;
+
+	return v;
 }

sm/csm/structprior/Constraint.h

@@ -8,6 +8,13 @@
 
 #include <csm/csm_all.h>
 
+struct Values{
+
+	double error;
+	double grad;
+
+};
+
 
 class Constraint
 {
@@ -22,7 +29,7 @@ protected:
 	int type;
 // class methods
 public:
-	double ApplyConstraint(int* indices, double* params = NULL);
+	Values ApplyConstraint(int* indices, double* params = NULL);
 	void SetType(int t);
 };
 

sm/csm/structprior/ConstraintManager.cpp

@@ -34,9 +34,9 @@ void ConstraintManager::ClearConstraints()
 void ConstraintManager::ApplyConstraints()
 {
 	//add as many types of constraints as wished
-	bool equal_to_either_active = false;
+	bool equal_to_either_active = true;
 	bool apply_equal_to_either = false;
-	bool lock_diff_active = false;
+	bool lock_diff_active = true;
 	bool apply_lock_diff = false;
 	
 	int n = laser_data->nrays;
@@ -46,12 +46,14 @@ void ConstraintManager::ApplyConstraints()
 		if (equal_to_either_active && constraint_types_to_apply[i] == EQUAL_TO_EITHER)
 		{
 			apply_equal_to_either = true;
+			equal_to_either_active = false;
 			continue;
 		}
 		
 		if (lock_diff_active && constraint_types_to_apply[i] == LOCK_DIFF)
 		{
 			apply_lock_diff = true;
+			lock_diff_active = false;
 			continue;
 		}
 		
@@ -69,9 +71,11 @@ void ConstraintManager::ApplyConstraints()
 			{
 				Constraint* c = new Constraint(EQUAL_TO_EITHER);
 				int* ind;
-				ind[0] = i-j; ind[1] =i; ind[2] =i+j;							
-				e += c->ApplyConstraint(ind);
-				constraints.push_back(c);
+				ind[0] = i-j; ind[1] =i; ind[2] =i+j;
+				Values v = c->ApplyConstraint(ind);
+				constraints.push_back(c);							
+				e += v.error;
+				
 			}
 			
 		}
@@ -80,19 +84,22 @@ void ConstraintManager::ApplyConstraints()
 	
 	if(apply_lock_diff)				//add lock_diff constraints
 	{
-		Constraint* c = new Constraint(LOCK_DIFF);
 		for (int i=0;i<n-1;i++)
 		{
 			int* ind;
 			ind[0] = i; ind[1]=i+1;
 			double* p;
 			p[0] = PI/2; p[1] = lock_diff_threshold;
-			constraints.push_back(c);
-			e+= c->ApplyConstraint(ind,p);
+			Constraint* c1 = new Constraint(LOCK_DIFF);
+			Values v = c1->ApplyConstraint(ind,p);
+			constraints.push_back(c1);							
+			e += v.error;
 			p[0] = -PI/2;
-			e+= c->ApplyConstraint(ind,p);
-			constraints.push_back(c);
-	
+			Constraint* c2 = new Constraint(LOCK_DIFF);
+			v = c2->ApplyConstraint(ind,p);
+			constraints.push_back(c2);							
+			e += v.error;
+
 		}
 	}
 	

sm/csm/structprior/structprior_test.cpp

@@ -10,14 +10,19 @@ int main(int argc, const char** argv)
 	if (argc < 2)
 	{
 		sm_error("Provide input file's name as an argument\n");
-		return 0;
+		return -1;
 	}
 
-	const char* file_name = argv[0];
+	const char* file_name = argv[1];
 	FILE* input_file = fopen(file_name, "r");
+	
 
 	LDP laserdata;
-	laserdata = ld_read_smart(input_file);
+	if(!(laserdata = ld_read_smart(input_file))) {
+		sm_error("Could not read scan.\n");
+		return -1;
+	}
+	
 	
 	std::vector<int> cons_types;
 	cons_types.push_back(EQUAL_TO_EITHER);
@@ -30,6 +35,6 @@ int main(int argc, const char** argv)
 
 
 
-	return 1;
+	return 0;
 	
 }