From d532e9ea266beb49dbe6671c0bec888fa7ed19b2 Mon Sep 17 00:00:00 2001
From: Paloma de la Puente <paloma.delapuente@upm.es>
Date: Wed, 21 Oct 2009 18:15:06 +0000
Subject: [PATCH] some debugging
---
sm/csm/structprior/Constraint.cpp | 9 ++++++--
sm/csm/structprior/Constraint.h | 9 +++++++-
sm/csm/structprior/ConstraintManager.cpp | 29 +++++++++++++++---------
sm/csm/structprior/structprior_test.cpp | 13 +++++++----
4 files changed, 42 insertions(+), 18 deletions(-)
diff --git a/sm/csm/structprior/Constraint.cpp b/sm/csm/structprior/Constraint.cpp
index 023df6f..b4fa932 100644
--- a/sm/csm/structprior/Constraint.cpp
+++ b/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;
}
diff --git a/sm/csm/structprior/Constraint.h b/sm/csm/structprior/Constraint.h
index 2329606..b1edb26 100644
--- a/sm/csm/structprior/Constraint.h
+++ b/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);
};
diff --git a/sm/csm/structprior/ConstraintManager.cpp b/sm/csm/structprior/ConstraintManager.cpp
index decffbf..aece759 100644
--- a/sm/csm/structprior/ConstraintManager.cpp
+++ b/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;
+
}
}
diff --git a/sm/csm/structprior/structprior_test.cpp b/sm/csm/structprior/structprior_test.cpp
index 5c96821..6503250 100644
--- a/sm/csm/structprior/structprior_test.cpp
+++ b/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;
}
--
GitLab