Skip to content
Snippets Groups Projects
Commit bfc3abfd authored by Paloma de la Puente's avatar Paloma de la Puente
Browse files

some initial code for struct prior

parent 7fbbeba6
No related branches found
No related tags found
No related merge requests found
SET(COMPILE_HSM 0)
SET(COMPILE_STRUCTPRIOR 1)
...@@ -159,6 +159,8 @@ SET(csm_sources ...@@ -159,6 +159,8 @@ SET(csm_sources
./csm/orientation.c ./csm/orientation.c
./csm/sm_options.c ./csm/sm_options.c
./csm/utils.c ./csm/utils.c
./csm/structprior/ConstraintManager.cpp
./csm/structprior/Constraint.cpp
./lib/egsl/egsl.c ./lib/egsl/egsl.c
./lib/egsl/egsl_conversions.c ./lib/egsl/egsl_conversions.c
./lib/egsl/egsl_misc.c ./lib/egsl/egsl_misc.c
...@@ -203,6 +205,17 @@ INSTALL(PROGRAMS hsm_test00 DESTINATION bin) ...@@ -203,6 +205,17 @@ INSTALL(PROGRAMS hsm_test00 DESTINATION bin)
ENDIF(COMPILE_HSM) ENDIF(COMPILE_HSM)
IF(COMPILE_STRUCTPRIOR)
INCLUDE_DIRECTORIES(/sw/include)
LINK_DIRECTORIES(/sw/lib)
ADD_EXECUTABLE(structprior ./csm/structprior/structprior_test.cpp)
TARGET_LINK_LIBRARIES(structprior csm-static)
INSTALL(PROGRAMS structprior DESTINATION bin)
ENDIF(COMPILE_STRUCTPRIOR)
SUBDIRS(pkg-config) SUBDIRS(pkg-config)
......
...@@ -31,6 +31,9 @@ SET(csm_sources ...@@ -31,6 +31,9 @@ SET(csm_sources
mbicp/sp_matrix.c mbicp/sp_matrix.c
hsm/hsm.c hsm/hsm.c
hsm/hsm_interface.c hsm/hsm_interface.c
structprior/ConstraintManager.cpp
structprior/Constraint.cpp
gpm/gpm.c gpm/gpm.c
) )
...@@ -40,7 +43,7 @@ IF(CAIRO_FOUND) ...@@ -40,7 +43,7 @@ IF(CAIRO_FOUND)
ENDIF(CAIRO_FOUND) ENDIF(CAIRO_FOUND)
foreach(dir . hsm mbicp icp gpm) foreach(dir . hsm mbicp icp gpm structprior)
FILE(GLOB csm_headers "${dir}/*.h") FILE(GLOB csm_headers "${dir}/*.h")
foreach(header ${csm_headers} ) foreach(header ${csm_headers} )
INSTALL(FILES ${header} DESTINATION include/csm/${dir}) INSTALL(FILES ${header} DESTINATION include/csm/${dir})
......
# This builds the code as itself
cmake_minimum_required(VERSION 2.4)
SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -pedantic -std=c99")
SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -ggdb")
SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall")
ADD_EXECUTABLE(structprior_test structprior_test.cpp ConstraintManager.cpp Constraint.cpp)
#include "Constraint.h"
Constraint::Constraint(int t)
{
type = t;
}
Constraint::~Constraint(void)
{
}
void Constraint::SetType(int t)
{
type = t;
}
double Constraint::ApplyConstraint(int* indices, double* params)
{
double err = 0;
switch(type)
{
case EQUAL_TO_EITHER:
{
break;
}
case LOCK_DIFF:
{
break;
}
}
return err;
}
#ifndef H_CONSTRAINT
#define H_CONSTRAINT
#define EQUAL_TO_EITHER 1
#define LOCK_DIFF 2
#define PI 3.14159
#include <csm/csm_all.h>
class Constraint
{
public:
//constructors
Constraint(int t);
virtual ~Constraint(void);
//class variables
double e;
double grad;
protected:
int type;
// class methods
public:
double ApplyConstraint(int* indices, double* params = NULL);
void SetType(int t);
};
#endif
#include "ConstraintManager.h"
ConstraintManager::ConstraintManager(LDP ld, std::vector<int> constraint_types)
{
equal_to_either_num = 10;
lock_diff_threshold = deg2rad(5);
laser_data = ld;
constraint_types_to_apply = constraint_types;
}
ConstraintManager::~ConstraintManager()
{
for (int i=0; i<constraints.size();i++)
{
delete constraints[i];
}
constraints.clear();
}
void ConstraintManager::ClearConstraints()
{
for (int i=0; i<constraints.size();i++)
{
delete constraints[i];
}
constraints.clear();
}
void ConstraintManager::ApplyConstraints()
{
//add as many types of constraints as wished
bool equal_to_either_active = false;
bool apply_equal_to_either = false;
bool lock_diff_active = false;
bool apply_lock_diff = false;
int n = laser_data->nrays;
for (int i=0;i< constraint_types_to_apply.size();i++)
{
if (equal_to_either_active && constraint_types_to_apply[i] == EQUAL_TO_EITHER)
{
apply_equal_to_either = true;
continue;
}
if (lock_diff_active && constraint_types_to_apply[i] == LOCK_DIFF)
{
apply_lock_diff = true;
continue;
}
}
if(apply_equal_to_either) //add equal_to_either constraints
{
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++)
{
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);
}
}
}
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);
p[0] = -PI/2;
e+= c->ApplyConstraint(ind,p);
constraints.push_back(c);
}
}
}
#ifndef H_CONSTRAINTMANAGER
#define H_CONSTRAINTMANAGER
#include <vector>
#include "Constraint.h"
class ConstraintManager
{
public:
//constructors
ConstraintManager(LDP ld, 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 ClearConstraints();
};
#endif
#include <stdio.h>
#include <csm/csm_all.h>
#include "ConstraintManager.h"
int main(int argc, const char** argv)
{
if (argc < 2)
{
sm_error("Provide input file's name as an argument\n");
return 0;
}
const char* file_name = argv[0];
FILE* input_file = fopen(file_name, "r");
LDP laserdata;
laserdata = ld_read_smart(input_file);
std::vector<int> cons_types;
cons_types.push_back(EQUAL_TO_EITHER);
//cons_types.push_back(LOCK_DIFF);
ConstraintManager cons_manager(laserdata, cons_types);
cons_manager.ApplyConstraints();
//minimizer.Minimize(x,cons_manager.constraints...)...
return 1;
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment