3dpcp/.svn/pristine/71/710407e183bf64deebc7a48bfab28501c0f54cdd.svn-base
2012-09-16 14:33:11 +02:00

231 lines
5.8 KiB
Text

// =====================================================================================
//
// Filename: numbers.h
//
// Description:
//
// Version: 1.0
// Created: 09/24/2010 02:03:48 PM
// Revision: none
// Compiler: g++
//
// Author: Jan Elseberg (), jelseber@uos.de
// Company: Universitaet Osnabrueck
//
// =====================================================================================
#ifndef __NUMBERS_H_
#define __NUMBERS_H_
class numberState {
public:
numberState() : number(0), char_p(0), board_p(0) {
for (int i = 0; i < 3; i++) {
position[i] = 0.0;
}
};
numberState(int _number) : number(_number), char_p(0), board_p(0) {
for (int i = 0; i < 3; i++) {
position[i] = 0.0;
}
};
numberState(int _number, double _char_p, double _board_p, double _position[3])
: number(_number), char_p(_char_p), board_p(_board_p)
{
for (int i = 0; i < 3; i++) {
position[i] = _position[i];
}
for (int i = 0; i < 4; i++) {
plane[i] = 0;
}
}
numberState(int _number, double _char_p, double _board_p, double _position[3], double _plane[4])
: number(_number), char_p(_char_p), board_p(_board_p)
{
for (int i = 0; i < 3; i++) {
position[i] = _position[i];
}
for (int i = 0; i < 4; i++) {
plane[i] = _plane[i];
}
}
double dist(numberState &ns) {
double x = ns.position[0] - position[0];
double y = ns.position[1] - position[1];
double z = ns.position[2] - position[2];
return sqrt( x*x + y*y +z*z );
}
int number;
double char_p;
double board_p;
double position[3];
double plane[4];
};
inline ostream& operator<<(ostream& os, const numberState ns)
{
os << "P(" << ns.number << ") = " << ns.char_p;
os << " P(" << ns.position[0] << ", " << ns.position[1] << ", " << ns.position[2] << ") = " << ns.board_p;
return os;
}
class numberConsensus {
public:
numberConsensus() {
sum_board_probability = 0.0;
sum_char_probability = 0.0;
consensus = numberState();
}
numberConsensus(int number) {
sum_board_probability = 0.0;
sum_char_probability = 0.0;
consensus = numberState(number);
}
void addObservation(int number, double cp, double bp, double pos[3], double plane[4]) {
addObservation( numberState(number, cp, bp, pos, plane) );
}
bool isClose(double pos[3]) {
return isClose( numberState(0, 0, 0, pos) );
}
bool isClose(numberState ns) {
for (unsigned int i = 0; i < clusters.size(); i++) {
if (ccenters[i].dist(ns) < 100.0 ) { // should be good enough
return true;
}
}
return false;
}
void addObservation(numberState ns) {
if (ns.number != consensus.number) return;
// observations.push_back(ns);
sum_board_probability += ns.board_p;
sum_char_probability += ns.char_p;
if (clusters.empty()) {
vector<numberState> vec;
vec.push_back(ns);
ccenters.push_back(ns);
clusters.push_back(vec);
updateConsensus(0);
return;
}
for (unsigned int i = 0; i < clusters.size(); i++) {
if (ccenters[i].dist(ns) < 100.0 ) { // should be good enough
cout << "new point is close to previous cluster..." << endl;
clusters[i].push_back(ns);
updateConsensus(i);
return;
}
}
// new cluster
vector<numberState> vec;
vec.push_back(ns);
ccenters.push_back(ns);
clusters.push_back(vec);
updateConsensus(clusters.size() -1);
return;
}
void updateConsensus(int changed) {
// First compute new representative for the changed cluster
double pos[3]; pos[0] = pos[1] = pos[2] = 0.0;
vector<numberState> changed_cluster = clusters[changed];
for (unsigned int i = 0; i < changed_cluster.size(); i++) {
for (int j = 0; j < 3; j++)
pos[j] += changed_cluster[i].position[j];
}
for (int j = 0; j < 3; j++)
pos[j] = pos[j] / changed_cluster.size();
// set representative
ccenters[changed] = numberState(consensus.number, 0, 0, pos ) ;
if (clusters.size() == 1) { // special case
if (clusters[0].size() == 1) { //
cout << "first number found..." << endl;
ccenters[0].char_p = clusters[0][0].char_p;
ccenters[0].board_p = clusters[0][0].board_p;
consensus = ccenters[0];
return;
}
}
for (unsigned int i = 0; i < clusters.size(); i++) {
double sum_bp = 0.0;
double sum_cp = 0.0;
for (unsigned int j = 0; j < clusters[i].size(); j++) {
sum_bp += clusters[i][j].board_p;
sum_cp += clusters[i][j].char_p;
}
ccenters[i].char_p = sum_cp / sum_char_probability;
ccenters[i].board_p = sum_bp / sum_board_probability;
}
consensus = ccenters[0];
for (unsigned int i = 1; i < ccenters.size(); i++) {
if (consensus.char_p * consensus.board_p < ccenters[i].char_p * ccenters[i].board_p) {
consensus = ccenters[i];
}
}
}
/*
void updateConsensus() {
double pos[3]; pos[0] = pos[1] = pos[2] = 0.0;
double plane[4];
double bp = 0.0;
double cp = 0.0;
for (unsigned int i = 0; i < observations.size(); i++) {
for (int j = 0; j < 3; j++)
pos[j] += observations[i].board_p * observations[i].position[j];
cp += observations[i].char_p;
bp += observations[i].board_p;
}
for (int j = 0; j < 3; j++)
pos[j] = pos[j] / observations.size();
cp = cp / observations.size();
bp = bp / observations.size();
consensus = numberState(consensus.number, cp, bp, pos, plane);
}
*/
numberState getConsensus() {
return consensus;
}
private:
numberState consensus;
// vector<numberState> observations;
vector< vector<numberState> > clusters;
vector<numberState> ccenters;
double sum_char_probability;
double sum_board_probability;
};
#endif