You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1250 lines
44 KiB
C++
1250 lines
44 KiB
C++
#include "LocalizerV2.h"
|
|
#include "math.h"
|
|
#include "iostream"
|
|
#include "World.h"
|
|
|
|
using namespace std;
|
|
|
|
static World& world = SWorld::getInstance();
|
|
|
|
|
|
/**
|
|
* Compute 3D position and 3D orientation
|
|
* */
|
|
void LocalizerV2::run(){
|
|
|
|
Field& fd = SField::getInstance();
|
|
|
|
stats_change_state(RUNNING);
|
|
|
|
//------------------ WORKFLOW: 0
|
|
|
|
_is_uptodate = false;
|
|
_is_head_z_uptodate = false;
|
|
_steps_since_last_update++;
|
|
|
|
prelim_reset(); //reset preliminary transformation matrix
|
|
|
|
fd.update(); //update visible collections
|
|
int lines_no = fd.list_segments.size();
|
|
int landmarks_no = fd.list_landmarks.size();
|
|
|
|
if( (landmarks_no == 0 && lines_no < 2) || (lines_no == 0) ){
|
|
if(lines_no==0 && landmarks_no==0){ stats_change_state(BLIND); } else { stats_change_state(MINFAIL); }
|
|
return;
|
|
}
|
|
|
|
//------------------ WORKFLOW: 1-2
|
|
|
|
if( ! find_z_axis_orient_vec() ){ return; }
|
|
|
|
//------------------ WORKFLOW: 3-4
|
|
|
|
if(!( landmarks_no >1 ? find_xy() : guess_xy() )){ return; }
|
|
|
|
//------------------ Update public variables
|
|
|
|
commit_everything();
|
|
|
|
stats_change_state(DONE);
|
|
|
|
//------------------ Statistics
|
|
|
|
//Ball position stats
|
|
if(world.ball_seen){
|
|
counter_ball += stats_sample_position_error(prelimHeadToField * world.ball_rel_pos_cart,
|
|
world.ball_cheat_abs_cart_pos, errorSum_ball);
|
|
}
|
|
|
|
//print_report(); //uncomment to enable report (average errors + solution analysis)
|
|
|
|
}
|
|
|
|
|
|
|
|
//=================================================================================================
|
|
//=================================================================================== GSL Utilities
|
|
//=================================================================================================
|
|
|
|
|
|
void add_gsl_regression_sample(gsl_matrix* m, gsl_vector* v, int sample_no, const Vector3f& relativeCoord, double absoluteCoord, double translCoeffMult=1){
|
|
|
|
gsl_matrix_set(m, sample_no, 0, relativeCoord.x);
|
|
gsl_matrix_set(m, sample_no, 1, relativeCoord.y);
|
|
gsl_matrix_set(m, sample_no, 2, relativeCoord.z);
|
|
gsl_matrix_set(m, sample_no, 3, translCoeffMult);
|
|
gsl_vector_set(v, sample_no, absoluteCoord );
|
|
|
|
}
|
|
|
|
template<std::size_t SIZE>
|
|
gsl_vector* create_gsl_vector(const std::array<double, SIZE> &content){
|
|
|
|
gsl_vector* v = gsl_vector_alloc (SIZE);
|
|
|
|
for(int i=0; i<SIZE; i++){
|
|
gsl_vector_set(v, i, content[i]);
|
|
}
|
|
return v;
|
|
}
|
|
|
|
|
|
//=================================================================================================
|
|
//================================================================================== Math utilities
|
|
//=================================================================================================
|
|
|
|
|
|
/**
|
|
* Get unit vector on plane Z=0, perpendicular to given vector
|
|
* Mathematically this is (0,0,1)x(vec)/|vec| with some additional checks
|
|
*/
|
|
Vector get_ground_unit_vec_perpendicular_to(const Vector3f& vec){
|
|
|
|
float gx = 1, gy = 0; //rotation axis unit vector, default:(1,0,0)
|
|
const float aux = sqrtf(vec.x*vec.x + vec.y*vec.y); //rotation axis length before becoming a unit vector
|
|
|
|
if(aux > 0){ //if vec is (0,0,1) or (0,0,-1) we keep the default rotation axis
|
|
gx = -vec.y / aux;
|
|
gy = vec.x / aux;
|
|
}
|
|
|
|
return Vector(gx,gy);
|
|
}
|
|
|
|
|
|
/**
|
|
* Rotate vector around ground axis defined as u=(0,0,1)x(Zvec)/|Zvec|
|
|
* Direction of rotation from Zvec to (0,0,1)
|
|
* To invert the rotation direction, invert Zvec.x and Zvec.y
|
|
* @param v vector to be rotated
|
|
* @param Zvec unit normal vector of rotated ground plane
|
|
*/
|
|
Vector3f fast_rotate_around_ground_axis(Vector3f v, Vector3f Zvec){
|
|
|
|
Vector u = get_ground_unit_vec_perpendicular_to(Zvec);
|
|
|
|
//Angle between unit normal vector of original plane and unit normal vector of rotated plane:
|
|
//cos(a) = (ov.rv)/(|ov||rv|) = ((0,0,1).(rvx,rvy,rvz))/(1*1) = rvz
|
|
float& cos_a = Zvec.z;
|
|
//assert(cos_a <= 1);
|
|
if(cos_a > 1) cos_a = 1; //Fix: it happens rarely, no cause was yet detected
|
|
float sin_a = -sqrtf(1 - cos_a*cos_a); //invert sin_a to invert a (direction was defined in method description)
|
|
|
|
const float i_cos_a = 1-cos_a;
|
|
const float uxuy_i = u.x*u.y*i_cos_a;
|
|
const float uxux_i = u.x*u.x*i_cos_a;
|
|
const float uyuy_i = u.y*u.y*i_cos_a;
|
|
const float uxsin_a = u.x*sin_a;
|
|
const float uysin_a = u.y*sin_a;
|
|
|
|
float x = (cos_a + uxux_i ) * v.x + uxuy_i * v.y + uysin_a * v.z;
|
|
float y = uxuy_i * v.x + (cos_a + uyuy_i ) * v.y - uxsin_a * v.z;
|
|
float z = - uysin_a * v.x + uxsin_a * v.y + cos_a * v.z;
|
|
|
|
return Vector3f(x,y,z);
|
|
|
|
}
|
|
|
|
|
|
/**
|
|
* Compute Xvec and Yvec from Zvec and angle
|
|
* @param Zvec input z-axis orientation vector
|
|
* @param angle input rotation of Xvec around Zvec (rads)
|
|
* @param Xvec output x-axis orientation vector
|
|
* @param Yvec output y-axis orientation vector
|
|
*/
|
|
void fast_compute_XYvec_from_Zvec(const Vector3f& Zvec, float agent_angle, Vector3f& Xvec, Vector3f& Yvec){
|
|
/**
|
|
* There are two coordinate systems being considered in this method:
|
|
* - The actual agent's vision (RELATIVE system -> RELsys)
|
|
* - A rotated perspective where the agent's seen Zvec is the real Zvec (ROTATED system -> ROTsys)
|
|
* (the agent's optical axis / line of sight is parallel to ground )
|
|
*
|
|
* SUMMARY:
|
|
* I provide an angle which defines the agent's rotation around Zvec (in the ROTsys)
|
|
* E.g. suppose the agent is rotated 5deg, then in the ROTsys, the agent sees Xvec as being rotated -5deg
|
|
* I then rotate Xvec to the RELsys, and compute the Yvec using cross product
|
|
*
|
|
* STEPS:
|
|
* 1st. Compute ROTsys, by finding rotation of plane defined by normal vector Zvec, in relation to seen XY plane
|
|
* (whose seen normal vector is (0,0,1)). We need: axis of rotation (unit vector lying on XY plane) and angle (rads):
|
|
* rotation axis:
|
|
* u = (0,0,1)x(Zvec) (vector perpendicular to XY plane and Zvec)
|
|
* = (-ZvecY,ZvecX,0) (rotation will be counterclockwise when u points towards the observer)
|
|
* (so a negative angle will bring the agent to the ROTsys)
|
|
* angle between Zvec and (0,0,1):
|
|
* a = acos( ((0,0,1).(Zvec)) / (|(0,0,1)|*|Zvec|) )
|
|
* = acos( ((0,0,1).(Zvec)) )
|
|
* = acos( ZvecZ )
|
|
*
|
|
* 2nd. Establish Xvec in ROTsys:
|
|
* Let agent_angle be the agent's angle. Then Xvec's angle is (b=-agent_angle).
|
|
* Xvec = (cos(b),sin(b),0)
|
|
*
|
|
* 3rd. Rotate Xvec to RELsys:
|
|
* Let R be the rotation matrix that rotates from ROTsys to RELsys (positive angle using u):
|
|
* Xvec = R * Xvec
|
|
* = R * (cos(b),sin(b),0)
|
|
* = (R00 * cos(b) + R01 * sin(b), R10 * cos(b) + R11 * sin(b), R20 * cos(b) + R21 * sin(b))
|
|
* where R is: (rotation matrix from axis and angle https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle)
|
|
* R00 = cos(a) + ux*ux(1-cos(a)) R01 = ux*uy(1-cos(a))
|
|
* R10 = uy*ux(1-cos(a)) R11 = cos(a) + uy*uy(1-cos(a))
|
|
* R20 = -uy*sin(a) R21 = ux*sin(a)
|
|
* so Xvec becomes:
|
|
* XvecX = cos(a)*cos(b) + (1-cos(a))*(ux*ux*cos(b) + ux*uy*sin(b))
|
|
* XvecY = cos(a)*sin(b) + (1-cos(a))*(uy*uy*sin(b) + ux*uy*cos(b))
|
|
* XvecZ = sin(a)*(ux*sin(b) - uy*cos(b))
|
|
*
|
|
* 4th. To find Yvec we have two options:
|
|
* A. add pi/2 to b and compute Yvec with the same expression used for Xvec
|
|
* B. Yvec = Zvec x Xvec (specifically in this order for original coordinate system)
|
|
*/
|
|
|
|
Vector u = get_ground_unit_vec_perpendicular_to(Zvec);
|
|
|
|
const float& cos_a = Zvec.z;
|
|
const float sin_a = sqrtf(1 - cos_a*cos_a);
|
|
const float uxuy = u.x * u.y;
|
|
const float b = -agent_angle; //Xvec's angle
|
|
const float cos_b = cosf(b);
|
|
const float sin_b = sinf(b);
|
|
const float i_cos_a = 1-cos_a;
|
|
|
|
Xvec.x = cos_a * cos_b + i_cos_a * ( u.x*u.x*cos_b + uxuy*sin_b );
|
|
Xvec.y = cos_a * sin_b + i_cos_a * ( u.y*u.y*sin_b + uxuy*cos_b );
|
|
Xvec.z = sin_a * ( u.x*sin_b - u.y*cos_b );
|
|
|
|
Yvec = Zvec.crossProduct(Xvec); //Using original coordinate system
|
|
}
|
|
|
|
|
|
//=================================================================================================
|
|
//================================================================================== Main algorithm
|
|
//=================================================================================================
|
|
|
|
/**
|
|
* WORKFLOW: 1
|
|
* See workflow description on header
|
|
*/
|
|
bool LocalizerV2::find_z_axis_orient_vec(){
|
|
|
|
Field& fd = SField::getInstance();
|
|
|
|
const int goalNo = fd.list_landmarks_goalposts.size();
|
|
|
|
if(fd.non_collinear_ground_markers >= 3){
|
|
fit_ground_plane();
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* At this point we have > 0 lines. Having 1 line is the most common option.
|
|
* But having 2 lines is also possible if at least one of them is too short to create 2 reference points.
|
|
* But those 2 lines can be both too short, which would not be ideal for the algorithms below.
|
|
* Steps: find largest line, check if it is large enough
|
|
*/
|
|
const Line6f* l = &fd.list_segments.front();
|
|
if(fd.list_segments.size() > 1){
|
|
for(const auto& ll: fd.list_segments){
|
|
if(ll.length > l->length) l = ≪
|
|
}
|
|
}
|
|
|
|
if(l->length < 1){ //larger line is too short
|
|
stats_change_state(FAILzLine);
|
|
return false;
|
|
}
|
|
|
|
if(goalNo == 0){ //Case e.g.: 1 corner and 1 line (or 2 short lines)
|
|
stats_change_state(FAILzNOgoal);
|
|
return false;
|
|
}
|
|
|
|
|
|
//------------------------------ Prepare variables for solution A & B
|
|
|
|
// Get any crossbar point
|
|
Vector3f crossbar_pt;
|
|
auto& glist = fd.list_landmarks_goalposts;
|
|
if(goalNo == 1){
|
|
crossbar_pt = glist.front().relPosCart;
|
|
}else{
|
|
if(glist[0].absPos.x == glist[1].absPos.x){
|
|
crossbar_pt = Vector3f::determineMidpoint( glist[0].relPosCart, glist[1].relPosCart);
|
|
}else{
|
|
//extremely rare: there are other solutions when goalNo>2 but cost/benefit not worth it
|
|
crossbar_pt = glist.front().relPosCart;
|
|
}
|
|
}
|
|
|
|
//------------------------------ Identify and apply solution A & B
|
|
|
|
//Get line point closest to crossbar point
|
|
Vector3f p = l->linePointClosestToCartPoint( crossbar_pt );
|
|
Vector3f possibleZvec = crossbar_pt - p;
|
|
float possibleZvecLength = possibleZvec.length();
|
|
|
|
if(fabsf(possibleZvecLength - 0.8) < 0.05){ //Solution A & B
|
|
Vector3f unit_zvec = possibleZvec / possibleZvecLength;
|
|
|
|
// save as the new z axis orientation vector
|
|
prelimHeadToField.set(2,0,unit_zvec.x);
|
|
prelimHeadToField.set(2,1,unit_zvec.y);
|
|
prelimHeadToField.set(2,2,unit_zvec.z);
|
|
|
|
find_z(unit_zvec);
|
|
return true;
|
|
}
|
|
|
|
//------------------------------ Apply solution C
|
|
|
|
Vector3f crossbar_left_vec, crossbar_midp; //this crossbar vector points left if seen from the midfield (this is important for the cross product)
|
|
|
|
const auto& goal_mm = Field::list_8_landmarks::goal_mm;
|
|
const auto& goal_mp = Field::list_8_landmarks::goal_mp;
|
|
const auto& goal_pm = Field::list_8_landmarks::goal_pm;
|
|
const auto& goal_pp = Field::list_8_landmarks::goal_pp;
|
|
|
|
if( goal_mm.visible && goal_mp.visible){
|
|
crossbar_left_vec = goal_mm.relPosCart - goal_mp.relPosCart;
|
|
crossbar_midp = (goal_mm.relPosCart + goal_mp.relPosCart)/2;
|
|
}else if( goal_pp.visible && goal_pm.visible){
|
|
crossbar_left_vec = goal_pp.relPosCart - goal_pm.relPosCart;
|
|
crossbar_midp = (goal_pp.relPosCart + goal_pm.relPosCart)/2;
|
|
}
|
|
|
|
/**
|
|
* Check if angle between line and crossbar is between 45deg and 135deg
|
|
* 45deg < acos(line.crossbar / |line||crossbar|) < 135deg <=>
|
|
* | line.crossbar / |line||crossbar| | < cos(45deg) <=>
|
|
* | line.crossbar | < cos(45deg) * |line| * ~2.1 <=>
|
|
* | line.crossbar | < 1.485 * |line|
|
|
*/
|
|
Vector3f lvec = l->get_cart_vector();
|
|
if( goalNo > 1 && fabsf(lvec.innerProduct(crossbar_left_vec)) < 1.485 * l->length ){
|
|
Vector3f Zvec;
|
|
if(l->startc.dist(crossbar_midp) > l->endc.dist(crossbar_midp)){
|
|
Zvec = lvec.crossProduct(crossbar_left_vec);
|
|
}else{
|
|
Zvec = crossbar_left_vec.crossProduct(lvec);
|
|
}
|
|
Zvec = Zvec.normalize(); // get unit vector
|
|
|
|
// save as the new z axis orientation vector
|
|
prelimHeadToField.set(2,0,Zvec.x);
|
|
prelimHeadToField.set(2,1,Zvec.y);
|
|
prelimHeadToField.set(2,2,Zvec.z);
|
|
|
|
find_z(Zvec);
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
stats_change_state(FAILz);
|
|
return false; //no solution was found
|
|
}
|
|
|
|
/**
|
|
* Find the best fitting ground plane's normal vector using Singular Value Decomposition
|
|
* Also compute the agent's height based on the ground references' centroid
|
|
* Dependency: at least 3 ground references
|
|
*/
|
|
void LocalizerV2::fit_ground_plane(){
|
|
|
|
Field& fd = SField::getInstance();
|
|
|
|
const auto& ground_markers = fd.list_weighted_ground_markers;
|
|
const int ground_m_size = ground_markers.size();
|
|
|
|
//------------------------------------ Compute groundmarks plane (if we have at least 3 groundmarks)
|
|
|
|
gsl_matrix *A, *V;
|
|
gsl_vector *S, *work;
|
|
|
|
A = gsl_matrix_alloc(ground_m_size, 3);
|
|
V = gsl_matrix_alloc(3, 3);
|
|
S = gsl_vector_alloc(3);
|
|
work = gsl_vector_alloc(3);
|
|
|
|
// Find the centroid
|
|
Vector3f centroid(0,0,0);
|
|
for(const auto& g : ground_markers){ // Insert all weighted groundmarks in matrix
|
|
centroid += g.relPosCart;
|
|
}
|
|
centroid /= (float)ground_m_size;
|
|
|
|
// Insert all groundmarks in matrix after subtracting the centroid
|
|
for(int i=0; i<ground_m_size; i++){
|
|
gsl_matrix_set(A,i,0, ground_markers[i].relPosCart.x - centroid.x );
|
|
gsl_matrix_set(A,i,1, ground_markers[i].relPosCart.y - centroid.y );
|
|
gsl_matrix_set(A,i,2, ground_markers[i].relPosCart.z - centroid.z );
|
|
}
|
|
|
|
// Singular value decomposition to find best fitting plane
|
|
gsl_linalg_SV_decomp(A,V,S,work);
|
|
|
|
// plane: ax + by + cz = d
|
|
double a = gsl_matrix_get(V,0,2);
|
|
double b = gsl_matrix_get(V,1,2);
|
|
double c = gsl_matrix_get(V,2,2);
|
|
|
|
/**
|
|
* Plane equation ax + by + cz = d
|
|
* therefore, considering the centroid(x,y,z) and normalvec(a,b,c):
|
|
* d = ax + by + cz
|
|
* = normalvec . centroid
|
|
*/
|
|
double d = a*centroid.x + b*centroid.y + c*centroid.z;
|
|
|
|
//Note: |d| is an estimate of the agent's height, but we can do better by including aerial references in the prediction later
|
|
|
|
gsl_matrix_free (A);
|
|
gsl_matrix_free (V);
|
|
gsl_vector_free (S);
|
|
gsl_vector_free (work);
|
|
|
|
/**
|
|
* Unfortunately, the normal vector doesn't always point up
|
|
* The plane is defined by (ax + by + cz - d = 0)
|
|
* Replacing [x,y,z] by a random point p, the sign of (ax + by + cz - d) gives us p's side
|
|
* Proof that the sign of (ax + by + cz - d) is positive for (rnd_ground_pt(i,j,k) + normal vector(a,b,c)):
|
|
* ax + by + cz - d =
|
|
* a(i+a) + b(j+b) + c(k+c) - (a*i + b*j + c*k) =
|
|
* ai + aa + bj + bb + ck + cc - ai - bj - ck =
|
|
* aa + bb + cc (which is always positive and equal to 1 because it is a unit vector)
|
|
* Since the agent is always above the field, we only need to make sure it's on the same side as the normal vector
|
|
* To do that, we check if the origin of the relative coordinate system (agent's head) gives a positive sign for (ax + by + cz - d)
|
|
* ax + by + cz - d > 0 (replacing x=0, y=0, z=0)
|
|
* d < 0
|
|
*
|
|
* However, if the agent is lying down, the optimized plane may be slightly above its head due to vision errors
|
|
* So, if we have a better reference point, such as a goal post, we use it
|
|
*/
|
|
|
|
if(!fd.list_landmarks_goalposts.empty()){ //If there are visible goal posts
|
|
const Vector3f& aerialpt = fd.list_landmarks_goalposts.front().relPosCart; //random aerial point (goal post)
|
|
if( a*aerialpt.x + b*aerialpt.y + c*aerialpt.z < d ){ //the goalpost is on the negative side, so we invert the normal vector
|
|
a=-a; b=-b; c=-c;
|
|
}
|
|
}else{ //If there are no visible goal posts, we rely on the agent's head
|
|
if(d > 0){// the normal vectors points down, so we invert it (we ignore 'd' from this point forward)
|
|
a=-a; b=-b; c=-c;
|
|
}
|
|
}
|
|
|
|
|
|
// save the ground plane's normal vector as the new z axis orientation vector
|
|
prelimHeadToField.set(2,0,a);
|
|
prelimHeadToField.set(2,1,b);
|
|
prelimHeadToField.set(2,2,c);
|
|
|
|
// compute the agent's height
|
|
float h = max( -centroid.x*a - centroid.y*b - centroid.z*c ,0.064);
|
|
prelimHeadToField.set(2,3, h );
|
|
|
|
//Set public independent coordinate z (Not a problem: may be out of sync with transf matrix)
|
|
last_z = final_z;
|
|
final_z = h;
|
|
_is_head_z_uptodate = true;
|
|
|
|
}
|
|
|
|
/**
|
|
* Compute translation in z (height)
|
|
* Note: Apparently there's no real benefit in involving goalposts (weighted or not), only when the
|
|
* visible objects are below 5/6, and even then the difference is minimal.
|
|
*/
|
|
void LocalizerV2::find_z(const Vector3f& Zvec){
|
|
|
|
Field& fd = SField::getInstance();
|
|
|
|
Vector3f zsum;
|
|
for(const auto& g: fd.list_weighted_ground_markers){
|
|
zsum += g.relPosCart;
|
|
}
|
|
|
|
//Minimum height: 0.064m
|
|
float z = max( -(zsum/fd.list_weighted_ground_markers.size()).innerProduct(Zvec) ,0.064f);
|
|
|
|
prelimHeadToField.set( 2,3,z );
|
|
|
|
//Set public independent coordinate z (Not a problem: may be out of sync with transf matrix)
|
|
last_z = final_z;
|
|
final_z = z;
|
|
_is_head_z_uptodate = true;
|
|
|
|
}
|
|
|
|
|
|
/**
|
|
* Computes mapping error using distance probabilities
|
|
* @return negative log of ["normalized" probability = (p1*p2*p3*...*pn)^(1/n)]
|
|
*/
|
|
double LocalizerV2::map_error_logprob(const gsl_vector *v, void *params){
|
|
|
|
float angle;
|
|
Field& fd = SField::getInstance();
|
|
|
|
//Get angle from optimization vector, or from params (as a constant)
|
|
if(v->size == 3){
|
|
angle = gsl_vector_get(v,2);
|
|
}else{
|
|
angle = *(float *)params;
|
|
}
|
|
|
|
Matrix4D& transfMat = SLocalizerV2::getInstance().prelimHeadToField;
|
|
Vector3f Zvec(transfMat.get(2,0), transfMat.get(2,1), transfMat.get(2,2));
|
|
|
|
Vector3f Xvec, Yvec;
|
|
fast_compute_XYvec_from_Zvec(Zvec, angle, Xvec, Yvec );
|
|
|
|
//These are the transformation coefficients that are being optimized
|
|
transfMat.set(0,0,Xvec.x);
|
|
transfMat.set(0,1,Xvec.y);
|
|
transfMat.set(0,2,Xvec.z);
|
|
transfMat.set(0,3,gsl_vector_get(v, 0));
|
|
transfMat.set(1,0,Yvec.x);
|
|
transfMat.set(1,1,Yvec.y);
|
|
transfMat.set(1,2,Yvec.z);
|
|
transfMat.set(1,3,gsl_vector_get(v, 1));
|
|
|
|
Matrix4D inverseTransMat = transfMat.inverse_tranformation_matrix();
|
|
|
|
|
|
double total_logprob = 0;
|
|
int total_err_cnt =0;
|
|
|
|
//Add log probability of unknown markers (with known corresponding field segment)
|
|
for(const auto& u : fd.list_unknown_markers){
|
|
|
|
|
|
//We know the closest field segment, so we can bring it to the agent's frame
|
|
Vector3f rel_field_s_start = inverseTransMat * u.fieldSeg->point[0]->get_vector();
|
|
Vector3f rel_field_s_end = inverseTransMat * u.fieldSeg->point[1]->get_vector();
|
|
|
|
Line6f rel_field_s(rel_field_s_start, rel_field_s_end, u.fieldSeg->length); //Convert to Line6f
|
|
|
|
Vector3f closest_polar_pt = rel_field_s.segmentPointClosestToCartPoint(u.relPosCart).toPolar();
|
|
|
|
total_logprob += FieldNoise::log_prob_r(closest_polar_pt.x, u.relPosPolar.x);
|
|
total_logprob += FieldNoise::log_prob_h(closest_polar_pt.y, u.relPosPolar.y);
|
|
total_logprob += FieldNoise::log_prob_v(closest_polar_pt.z, u.relPosPolar.z);
|
|
total_err_cnt++;
|
|
|
|
}
|
|
|
|
//Add log probability of known markers
|
|
for(const auto& k : fd.list_known_markers){
|
|
|
|
//Bring marker to agent's frame
|
|
Vector3f rel_k = (inverseTransMat * k.absPos.get_vector()).toPolar();
|
|
|
|
total_logprob += FieldNoise::log_prob_r(rel_k.x, k.relPosPolar.x);
|
|
total_logprob += FieldNoise::log_prob_h(rel_k.y, k.relPosPolar.y);
|
|
total_logprob += FieldNoise::log_prob_v(rel_k.z, k.relPosPolar.z);
|
|
total_err_cnt++;
|
|
|
|
}
|
|
|
|
//return log of "normalized" probability = (p1*p2*p3*...*pn)^(1/n)
|
|
//negative because the optimization method minimizes the loss function
|
|
double logNormProb = -total_logprob / total_err_cnt;
|
|
|
|
if(!gsl_finite(logNormProb)) return 1e6; //fix
|
|
|
|
return logNormProb;
|
|
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
* Computes mapping error using 2d euclidian distances
|
|
* @return average distance
|
|
*/
|
|
double LocalizerV2::map_error_2d(const gsl_vector *v, void *params){
|
|
|
|
float angle;
|
|
Field& fd = SField::getInstance();
|
|
|
|
//Get angle from optimization vector, or from params (as a constant)
|
|
if(v->size == 3){
|
|
angle = gsl_vector_get(v,2);
|
|
}else{
|
|
angle = *(float *)params;
|
|
}
|
|
|
|
Matrix4D& transfMat = SLocalizerV2::getInstance().prelimHeadToField;
|
|
Vector3f Zvec(transfMat.get(2,0), transfMat.get(2,1), transfMat.get(2,2));
|
|
|
|
Vector3f Xvec, Yvec;
|
|
fast_compute_XYvec_from_Zvec(Zvec, angle, Xvec, Yvec );
|
|
|
|
|
|
//These are the transformation coefficients that are being optimized
|
|
transfMat.set(0,0,Xvec.x);
|
|
transfMat.set(0,1,Xvec.y);
|
|
transfMat.set(0,2,Xvec.z);
|
|
transfMat.set(0,3,gsl_vector_get(v, 0));
|
|
transfMat.set(1,0,Yvec.x);
|
|
transfMat.set(1,1,Yvec.y);
|
|
transfMat.set(1,2,Yvec.z);
|
|
transfMat.set(1,3,gsl_vector_get(v, 1));
|
|
|
|
|
|
float total_err = 0;
|
|
int total_err_cnt =0;
|
|
for(const Line6f& l : fd.list_segments){
|
|
|
|
//Compute line absolute coordinates according to current transformation
|
|
Vector3f ls = transfMat * l.startc;
|
|
Vector3f le = transfMat * l.endc;
|
|
|
|
//Compute line angle and establish a tolerance
|
|
float l_angle = 0;
|
|
float l_angle_tolerance = 10; //default full tolerance (no point in being larger than pi/2 but no harm either)
|
|
|
|
if(l.length > 0.8){
|
|
//This is the easy case: find the angle and establish a small tolerance (which allows some visual rotation attempts)
|
|
l_angle = atan2f(le.y - ls.y, le.x - ls.x);
|
|
if(l_angle < 0) { l_angle += 3.14159265f; } //this is a line, not a vector, so positive angles are enough
|
|
l_angle_tolerance = 0.35f; //20 degrees
|
|
|
|
} else if(fd.list_segments.size() <= 3) {
|
|
//It gets to a point where the cost/benefit is not very inviting. If there are many lines (>3),
|
|
//the small ones are not as decisive for the mapping error. Otherwise, we proceed:
|
|
|
|
//If the small line is touching a big line, they have different orientations (it's a characteristic from the field lines)
|
|
|
|
for(const Line6f& lbig : fd.list_segments){
|
|
if(lbig.length < 2 || &lbig == &l ) continue; //check if line is big and different from current
|
|
|
|
if(lbig.segmentDistToSegment(l)<0.5){
|
|
//this would only generate false positives with the halfway line and 4 ring lines (if enough vision error)
|
|
//but even then, their orientation would be very different, so the method still holds
|
|
|
|
//---------- get angle perpendicular to bigline (that is either the small line's angle, or at least close enough)
|
|
|
|
//get bigline angle
|
|
Vector3f lbigs = transfMat * lbig.startc;
|
|
Vector3f lbige = transfMat * lbig.endc;
|
|
l_angle = atan2f(lbige.y - lbigs.y, lbige.x - lbigs.x);
|
|
|
|
// add 90deg while keeping the angle between 0-180deg (same logic used when l.length > 0.8)
|
|
if (l_angle < -1.57079632f){ l_angle += 4.71238898f; } //Q3 -> add pi*3/2
|
|
else if(l_angle < 0 ){ l_angle += 1.57079632f; } //Q4 -> add pi/2
|
|
else if(l_angle < 1.57079632f ){ l_angle += 1.57079632f; } //Q1 -> add pi/2
|
|
else { l_angle -= 1.57079632f; } //Q2 -> subtract pi/2
|
|
|
|
//This large tolerance means that this small line can be matched with almost everything except perpendicular lines
|
|
l_angle_tolerance = 1.22f; //70 deg tolerance
|
|
break; //end search for close big lines
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
//this default error of 1e6f is applied when there is no match (which means the transf. matrix's Xvec/Yvec are wrong)
|
|
float min_err = 1e6f;
|
|
for(const auto& s : Field::cFieldLineSegments::list){ //find distance to closest field line
|
|
|
|
//Skip field line if seen line is substantially larger
|
|
if( l.length > (s.length + 0.7) ){ continue; }
|
|
|
|
//Skip field line if orientation does not match
|
|
float angle_difference = fabsf(l_angle - s.angle);
|
|
if(angle_difference > 1.57079632f) angle_difference = 3.14159265f - angle_difference;
|
|
if(angle_difference > l_angle_tolerance) continue;
|
|
|
|
//Error is the sum of the distance of a single line segment to both endpoints of seen line
|
|
float err = Field::fieldLineSegmentDistToCart2DPoint(s,ls.to2d());
|
|
if(err < min_err) err += Field::fieldLineSegmentDistToCart2DPoint(s,le.to2d());
|
|
|
|
if(err < min_err) min_err = err;
|
|
}
|
|
|
|
total_err += min_err;
|
|
total_err_cnt+=2; //a line has 2 points, double the weight of a single landmark
|
|
|
|
}
|
|
|
|
|
|
for(const Field::sMarker& m : fd.list_landmarks){
|
|
|
|
Vector3f lpt = transfMat * m.relPosCart; //compute absolute coordinates according to current transformation
|
|
|
|
float err = lpt.to2d().getDistanceTo(Vector(m.absPos.x,m.absPos.y));
|
|
total_err += err > 0.5 ? err * 100 : err;
|
|
total_err_cnt++;
|
|
|
|
}
|
|
|
|
double avg_error = total_err / total_err_cnt;
|
|
|
|
if(!gsl_finite(avg_error)) return 1e6; //fix
|
|
|
|
return avg_error; //return average error
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
* Apply fine tuning directly on the prelimHeadToField matrix
|
|
* 1st - improve map fitting
|
|
* 2nd - identify line segments and their endpoints
|
|
* 3rd - fine tune again using known markers
|
|
* @param initial_angle initial angle of Xvec around Zvec
|
|
* @param initial_x initial translation in x
|
|
* @param initial_y initial translation in y
|
|
*/
|
|
bool LocalizerV2::fine_tune(float initial_angle, float initial_x, float initial_y){
|
|
|
|
Field& fd = SField::getInstance();
|
|
|
|
//Statistics before fine tune
|
|
counter_fineTune += stats_sample_position_error(Vector3f(initial_x,initial_y,prelimHeadToField.get(11)), world.my_cheat_abs_cart_pos, errorSum_fineTune_before);
|
|
|
|
//Fine tune, changing the initial parameters directly
|
|
if(!fine_tune_aux(initial_angle, initial_x, initial_y, false)) return false;
|
|
|
|
//Statistics for 1st fine tune
|
|
stats_sample_position_error(Vector3f(initial_x,initial_y,prelimHeadToField.get(11)), world.my_cheat_abs_cart_pos, errorSum_fineTune_euclidianDist);
|
|
|
|
//Identify new markers
|
|
fd.update_from_transformation(prelimHeadToField);
|
|
|
|
//Probabilistic fine tune
|
|
fine_tune_aux(initial_angle, initial_x, initial_y, true);
|
|
|
|
//Statistics for 2nd fine tune
|
|
stats_sample_position_error(prelimHeadToField.toVector3f(), world.my_cheat_abs_cart_pos, errorSum_fineTune_probabilistic);
|
|
|
|
//Update unknown markers absolute position based on refined transformation matrix
|
|
fd.update_unknown_markers(prelimHeadToField);
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
/**
|
|
* Apply fine tuning:
|
|
* - directly on: initial_angle, initial_x, initial_y (if use_probabilities == false)
|
|
* - directly on the prelimHeadToField matrix using probabilities (if use_probabilities == true)
|
|
* @param initial_angle initial angle of Xvec around Zvec
|
|
* @param initial_x initial translation in x
|
|
* @param initial_y initial translation in y
|
|
*/
|
|
bool LocalizerV2::fine_tune_aux(float &initial_angle, float &initial_x, float &initial_y, bool use_probabilities){
|
|
|
|
int status, iter=0;
|
|
gsl_vector* x = create_gsl_vector<3>({initial_x, initial_y, initial_angle}); // Initial transformation
|
|
gsl_vector* ss = create_gsl_vector<3>({0.02, 0.02, 0.03}); // Set initial step sizes
|
|
gsl_multimin_function minex_func = {map_error_2d, 3, nullptr}; // error func, variables no., params
|
|
if(use_probabilities) minex_func.f = map_error_logprob; // probablity-based error function
|
|
|
|
const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex2; // algorithm type
|
|
gsl_multimin_fminimizer *s = gsl_multimin_fminimizer_alloc (T, 3); // allocate workspace
|
|
gsl_multimin_fminimizer_set (s, &minex_func, x, ss); // set workspace
|
|
|
|
float best_x, best_y, best_ang;
|
|
|
|
|
|
do{
|
|
iter++;
|
|
status = gsl_multimin_fminimizer_iterate(s);
|
|
|
|
//*s holds the best solution, not the last solution
|
|
best_x = gsl_vector_get (s->x, 0);
|
|
best_y = gsl_vector_get (s->x, 1);
|
|
best_ang = gsl_vector_get (s->x, 2);
|
|
|
|
if (status) break;
|
|
|
|
double size = gsl_multimin_fminimizer_size (s); //minimizer-specific characteristic size
|
|
status = gsl_multimin_test_size (size, 1e-3); //This size can be used as a stopping criteria, as the simplex contracts itself near the minimum
|
|
|
|
}
|
|
while ((status == GSL_CONTINUE || use_probabilities) && iter < 40);
|
|
|
|
float best_map_error = s->fval;
|
|
|
|
gsl_vector_free(x);
|
|
gsl_vector_free(ss);
|
|
gsl_multimin_fminimizer_free (s);
|
|
|
|
if(!use_probabilities){
|
|
if(best_map_error > 0.10){
|
|
stats_change_state(FAILtune);
|
|
return false;
|
|
}else{
|
|
initial_angle = best_ang;
|
|
initial_x = best_x;
|
|
initial_y = best_y;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* At this point, use_probabilities is true
|
|
* Note: The transformations are directly tested on prelimHeadToField but it currently
|
|
* holds the last test, so we set it manually here to the best found solution
|
|
*/
|
|
|
|
//Convert angle into Xvec and Yvec
|
|
Vector3f Zvec(prelimHeadToField.get(2,0), prelimHeadToField.get(2,1), prelimHeadToField.get(2,2));
|
|
Vector3f Xvec, Yvec;
|
|
fast_compute_XYvec_from_Zvec(Zvec, best_ang, Xvec, Yvec );
|
|
|
|
prelimHeadToField.set(0,0, Xvec.x);
|
|
prelimHeadToField.set(0,1, Xvec.y);
|
|
prelimHeadToField.set(0,2, Xvec.z);
|
|
prelimHeadToField.set(0,3, best_x);
|
|
prelimHeadToField.set(1,0, Yvec.x);
|
|
prelimHeadToField.set(1,1, Yvec.y);
|
|
prelimHeadToField.set(1,2, Yvec.z);
|
|
prelimHeadToField.set(1,3, best_y);
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* Find XY translation/rotation
|
|
* A unique solution is guaranteed if Zvec points in the right direction
|
|
* Requirement: 2 visible landmarks
|
|
*/
|
|
bool LocalizerV2::find_xy(){
|
|
|
|
Field& fd = SField::getInstance();
|
|
|
|
Vector3f Zvec(prelimHeadToField.get(2,0), prelimHeadToField.get(2,1), prelimHeadToField.get(2,2));
|
|
|
|
Field::sMarker *m1 = nullptr, *m2 = nullptr;
|
|
|
|
//Get as many corners as possible
|
|
if(fd.list_landmarks_corners.size()>1){
|
|
m1 = &fd.list_landmarks_corners[0];
|
|
m2 = &fd.list_landmarks_corners[1];
|
|
}else if(fd.list_landmarks_corners.size()==1){
|
|
m1 = &fd.list_landmarks_corners[0];
|
|
m2 = &fd.list_landmarks_goalposts[0];
|
|
}else{
|
|
m1 = &fd.list_landmarks_goalposts[0];
|
|
m2 = &fd.list_landmarks_goalposts[1];
|
|
}
|
|
|
|
Vector3f realVec(m2->absPos.x - m1->absPos.x, m2->absPos.y - m1->absPos.y, m2->absPos.z - m1->absPos.z);
|
|
float real_angle = atan2f(realVec.y, realVec.x); //angle of real vector
|
|
|
|
Vector3f seenVec(m2->relPosCart - m1->relPosCart);
|
|
Vector3f rotated_abs_vec = fast_rotate_around_ground_axis(seenVec, Zvec);
|
|
float seen_angle = atan2f(rotated_abs_vec.y, rotated_abs_vec.x); //angle of real vector
|
|
|
|
float AgentAngle = real_angle - seen_angle; //no normalization is needed
|
|
|
|
|
|
Vector3f Xvec, Yvec;
|
|
fast_compute_XYvec_from_Zvec(Zvec, AgentAngle, Xvec, Yvec );
|
|
|
|
/**
|
|
* Let m be a landmark, rel:(mx,my,mz), abs:(mabsx, mabsy, mabsz)
|
|
* XvecX*mx + XvecY*my + XvecZ*mz + AgentX = mabsx
|
|
* AgentX = mabsx - (XvecX*mx + XvecY*my + XvecZ*mz)
|
|
* AgentX = mabsx - (XvecX . m)
|
|
*
|
|
* Generalizing for N estimates:
|
|
* AgentX = sum( mabsx - (XvecX . m) )/N
|
|
*/
|
|
float initial_x = 0, initial_y = 0;
|
|
for(const Field::sMarker& m : fd.list_landmarks){
|
|
initial_x += m.absPos.x - Xvec.innerProduct(m.relPosCart);
|
|
initial_y += m.absPos.y - Yvec.innerProduct(m.relPosCart);
|
|
}
|
|
|
|
initial_x /= fd.list_landmarks.size();
|
|
initial_y /= fd.list_landmarks.size();
|
|
|
|
|
|
return fine_tune(AgentAngle, initial_x, initial_y);
|
|
|
|
}
|
|
|
|
bool LocalizerV2::guess_xy(){
|
|
Field& fd = SField::getInstance();
|
|
|
|
//Get Zvec from previous steps
|
|
Vector3f Zvec(prelimHeadToField.get(2,0), prelimHeadToField.get(2,1), prelimHeadToField.get(2,2));
|
|
Vector last_known_position(head_position.x, head_position.y);
|
|
|
|
//------------------------------------------------------------ Get longest line and use it as X or Y vector
|
|
|
|
const Line6f* longestLine = &fd.list_segments.front();
|
|
for(const Line6f& l : fd.list_segments){
|
|
if(l.length > longestLine->length) longestLine = &l;
|
|
}
|
|
|
|
if(longestLine->length < 1.6){
|
|
stats_change_state(FAILguessLine);
|
|
return false; //largest line is too short, it could be mistaken for a ring line
|
|
}
|
|
|
|
//Rotate line to real ground plane, where it loses the 3rd dimension
|
|
Vector3f longestLineVec = longestLine->endc - longestLine->startc;
|
|
Vector3f rotated_abs_line = fast_rotate_around_ground_axis(longestLineVec, Zvec);
|
|
|
|
//The line can be aligned with X or Y, positively or negatively (these angles don't need to be normalized)
|
|
float fixed_angle[4];
|
|
fixed_angle[0] = -atan2f(rotated_abs_line.y,rotated_abs_line.x); //if longestLineVec is Xvec
|
|
fixed_angle[1] = fixed_angle[0] + 3.14159265f; //if longestLineVec is -Xvec
|
|
fixed_angle[2] = fixed_angle[0] + 1.57079633f; //if longestLineVec is Yvec
|
|
fixed_angle[3] = fixed_angle[0] - 1.57079633f; //if longestLineVec is -Yvec
|
|
|
|
//------------------------------------------------------------ Get initial translation
|
|
|
|
//if we see 1 landmark, we use it, if not, we get the last position
|
|
|
|
float initial_x[4], initial_y[4];
|
|
bool noLandmarks = fd.list_landmarks.empty();
|
|
|
|
if(noLandmarks){
|
|
|
|
for(int i=0; i<4; i++){
|
|
initial_x[i] = last_known_position.x;
|
|
initial_y[i] = last_known_position.y;
|
|
}
|
|
|
|
} else {
|
|
|
|
Vector3f Xvec = longestLineVec / longestLine->length;
|
|
Vector3f Yvec(Zvec.crossProduct(Xvec));
|
|
|
|
/**
|
|
* Let m be a landmark, rel:(mx,my,mz), abs:(mabsx, mabsy, mabsz)
|
|
* XvecX*mx + XvecY*my + XvecZ*mz + AgentX = mabsx
|
|
* AgentX = mabsx - (XvecX*mx + XvecY*my + XvecZ*mz)
|
|
* AgentX = mabsx - (XvecX . m)
|
|
*/
|
|
|
|
const Field::sMarker& m = fd.list_landmarks.front();
|
|
const float x_aux = Xvec.innerProduct(m.relPosCart);
|
|
const float y_aux = Yvec.innerProduct(m.relPosCart);
|
|
|
|
initial_x[0] = m.absPos.x - x_aux;
|
|
initial_y[0] = m.absPos.y - y_aux;
|
|
initial_x[1] = m.absPos.x + x_aux; //2nd version: X is inverted
|
|
initial_y[1] = m.absPos.y + y_aux; //2nd version: Y is inverted
|
|
initial_x[2] = m.absPos.x + y_aux; //3rd version: X is inverted Y
|
|
initial_y[2] = m.absPos.y - x_aux; //3rd version: Y is X
|
|
initial_x[3] = m.absPos.x - y_aux; //4th version: X is Y
|
|
initial_y[3] = m.absPos.y + x_aux; //4th version: Y is inverted X
|
|
|
|
}
|
|
|
|
|
|
//------------------------------------------------------------ Optimize XY rotation for each possible orientation
|
|
|
|
|
|
const gsl_multimin_fminimizer_type *T = gsl_multimin_fminimizer_nmsimplex2;
|
|
gsl_multimin_fminimizer *s[4] = {nullptr,nullptr,nullptr,nullptr};
|
|
gsl_vector *ss[4], *x[4];
|
|
gsl_multimin_function minex_func[4];
|
|
|
|
size_t iter = 0;
|
|
int status;
|
|
double size;
|
|
|
|
for(int i=0; i<4; i++){
|
|
x[i] = create_gsl_vector<2>({initial_x[i], initial_y[i]}); // Initial transformation
|
|
ss[i] = create_gsl_vector<2>({1, 1}); //Set initial step sizes to 1
|
|
|
|
/* Initialize method */
|
|
minex_func[i].n = 2;
|
|
minex_func[i].f = map_error_2d;
|
|
minex_func[i].params = &fixed_angle[i];
|
|
|
|
s[i] = gsl_multimin_fminimizer_alloc (T, 2);
|
|
gsl_multimin_fminimizer_set (s[i], &minex_func[i], x[i], ss[i]);
|
|
}
|
|
|
|
/* start iterating */
|
|
bool running[4] = {true,true,true,true};
|
|
float current_error[4] = {1e6,1e6,1e6,1e6};
|
|
float lowest_error = 1e6;
|
|
Vector best_xy[4];
|
|
const int maximum_iterations = 50;
|
|
bool plausible_solution[4] = {false,false,false,false};
|
|
do{
|
|
iter++;
|
|
for(int i=0; i<4; i++){
|
|
if(!running[i]) continue;
|
|
|
|
status = gsl_multimin_fminimizer_iterate(s[i]);
|
|
|
|
current_error[i] = s[i]->fval;
|
|
if(current_error[i] < lowest_error) lowest_error = current_error[i];
|
|
|
|
// Possible errors:
|
|
// GSL_ERROR ("incompatible size of x", GSL_EINVAL); This should only be a concern during code design
|
|
// GSL_ERROR ("contraction failed", GSL_EFAILED); Evaluation function produced non finite value
|
|
if (status) {
|
|
running[i]=false; //This is not a valid solution
|
|
continue;
|
|
}
|
|
|
|
size = gsl_multimin_fminimizer_size (s[i]); //minimizer-specific characteristic size
|
|
status = gsl_multimin_test_size (size, 1e-2); //This size can be used as a stopping criteria, as the simplex contracts itself near the minimum
|
|
|
|
if(status != GSL_CONTINUE || (lowest_error * 50 < current_error[i])) { //finished or aborted
|
|
best_xy[i].x = gsl_vector_get (s[i]->x, 0);
|
|
best_xy[i].y = gsl_vector_get (s[i]->x, 1);
|
|
running[i]=false;
|
|
plausible_solution[i]=(status == GSL_SUCCESS); //only valid if it converged to local minimum
|
|
continue;
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
} while (iter < maximum_iterations && (running[0] || running[1] || running[2] || running[3]));
|
|
|
|
for(int i=0; i<4; i++){
|
|
gsl_vector_free(x[i]);
|
|
gsl_vector_free(ss[i]);
|
|
gsl_multimin_fminimizer_free (s[i]);
|
|
}
|
|
|
|
|
|
//At this point, a solution is plausible if it converged to a local minimum
|
|
//So, we apply the remaining criteria for plausiblity
|
|
int plausible_count = 0;
|
|
int last_i;
|
|
for(int i=0; i<4; i++){
|
|
if(!plausible_solution[i]) continue;
|
|
bool isDistanceOk = (!noLandmarks) || last_known_position.getDistanceTo(best_xy[i]) < 0.5; // distance to last known position
|
|
if(current_error[i] < 0.12 && isDistanceOk){ // mapping error
|
|
plausible_count++;
|
|
last_i = i;
|
|
}
|
|
}
|
|
|
|
// If there is 1 landmark, and multiple options, the distance to last known pos is now used to eliminate candidates
|
|
if(!noLandmarks && plausible_count>1){
|
|
plausible_count = 0;
|
|
for(int i=0; i<4; i++){
|
|
if(plausible_solution[i] && last_known_position.getDistanceTo(best_xy[i]) < 0.5){ // distance to last known position
|
|
plausible_count++;
|
|
last_i = i;
|
|
}
|
|
}
|
|
}
|
|
|
|
//Check if best solution is good if all others are not even plausible
|
|
if(plausible_count==0){
|
|
stats_change_state(FAILguessNone);
|
|
return false;
|
|
}else if(plausible_count>1){
|
|
stats_change_state(FAILguessMany);
|
|
return false;
|
|
}else if(current_error[last_i] > 0.06 || (noLandmarks && last_known_position.getDistanceTo(best_xy[last_i]) > 0.3)){ // mapping error / distance to last known position
|
|
stats_change_state(FAILguessTest);
|
|
return false;
|
|
}
|
|
|
|
return fine_tune(fixed_angle[last_i],best_xy[last_i].x, best_xy[last_i].y);
|
|
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
* Called to update every public variable (rotation + translation)
|
|
*/
|
|
void LocalizerV2::commit_everything(){
|
|
|
|
final_headTofieldTransform = prelimHeadToField; //Full transformation (relative to absolute)
|
|
|
|
final_headTofieldTransform.inverse_tranformation_matrix( final_fieldToheadTransform ); //Full transformation (absolute to relative)
|
|
|
|
for(int i=0; i<3; i++){
|
|
|
|
//Rotation (relative to absolute)
|
|
final_headTofieldRotate.set(i ,final_headTofieldTransform.get(i )); //Copy rotation line 1
|
|
final_headTofieldRotate.set(i+4,final_headTofieldTransform.get(i+4)); //Copy rotation line 2
|
|
final_headTofieldRotate.set(i+8,final_headTofieldTransform.get(i+8)); //Copy rotation line 3
|
|
|
|
//Rotation (absolute to relative)
|
|
final_fieldToheadRotate.set(i ,final_fieldToheadTransform.get(i )); //Copy rotation line 1
|
|
final_fieldToheadRotate.set(i+4,final_fieldToheadTransform.get(i+4)); //Copy rotation line 2
|
|
final_fieldToheadRotate.set(i+8,final_fieldToheadTransform.get(i+8)); //Copy rotation line 3
|
|
}
|
|
|
|
final_translation = final_headTofieldTransform.toVector3f();
|
|
|
|
_is_uptodate = true;
|
|
|
|
_steps_since_last_update = 0;
|
|
|
|
//Add current 3D position to history
|
|
position_history[position_history_ptr++] = final_translation;
|
|
if(position_history_ptr >= position_history.size()) position_history_ptr=0;
|
|
|
|
}
|
|
|
|
|
|
Vector3f LocalizerV2::relativeToAbsoluteCoordinates(const Vector3f relativeCoordinates) const{
|
|
return headTofieldTransform * relativeCoordinates;
|
|
}
|
|
|
|
Vector3f LocalizerV2::absoluteToRelativeCoordinates(const Vector3f absoluteCoordinates) const{
|
|
return fieldToheadTransform * absoluteCoordinates;
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
* Reset preliminary matrix with a constant for the first 3 rows
|
|
*/
|
|
void LocalizerV2::prelim_reset(){
|
|
//Since it was initialized as identity matrix we never need to change the last row
|
|
for(int i=0; i<12; i++){
|
|
prelimHeadToField.set(i,9999); //flag unchanged cells with a constant
|
|
}
|
|
}
|
|
|
|
//=================================================================================================
|
|
//=============================================================================== useful statistics
|
|
//=================================================================================================
|
|
|
|
Vector3f LocalizerV2::get_velocity(unsigned int n) const{
|
|
//assert(n > 0 && n < position_history.size() && "LocalizerV2::get_velocity(unsigned int n) -> n must be between 1 and 9!");
|
|
|
|
int l = position_history.size() - 1;
|
|
|
|
Vector3f current_pos = position_history[(position_history_ptr + l) % position_history.size()];
|
|
Vector3f last_pos = position_history[(position_history_ptr + l - n) % position_history.size()];
|
|
|
|
return current_pos - last_pos;
|
|
}
|
|
|
|
//=================================================================================================
|
|
//================================================================================ debug statistics
|
|
//=================================================================================================
|
|
|
|
|
|
void LocalizerV2::print_report() const{
|
|
|
|
if(counter_fineTune == 0){
|
|
cout << "LocalizerV2 Report - Check if the server is providing cheat data.\n";
|
|
return;
|
|
}
|
|
|
|
if(counter_fineTune < 2) return; //otherwise, c-1=0
|
|
const int &c = counter_fineTune;
|
|
const int &cb = counter_ball;
|
|
const int c1 = c-1;
|
|
const int cb1 = cb-1;
|
|
|
|
const double* ptr = errorSum_fineTune_before;
|
|
float e1_2d_var = (ptr[4] - (ptr[3]*ptr[3]) / c) / c1;
|
|
float e1_3d_var = (ptr[6] - (ptr[5]*ptr[5]) / c) / c1;
|
|
float e1[] = { ptr[3]/c, sqrt(e1_2d_var), ptr[5]/c, sqrt(e1_3d_var), ptr[0]/c, ptr[1]/c, ptr[2]/c };
|
|
|
|
ptr = errorSum_fineTune_euclidianDist;
|
|
float e2_2d_var = (ptr[4] - (ptr[3]*ptr[3]) / c) / c1;
|
|
float e2_3d_var = (ptr[6] - (ptr[5]*ptr[5]) / c) / c1;
|
|
float e2[] = { ptr[3]/c, sqrt(e2_2d_var), ptr[5]/c, sqrt(e2_3d_var), ptr[0]/c, ptr[1]/c, ptr[2]/c };
|
|
|
|
ptr = errorSum_fineTune_probabilistic;
|
|
float e3_2d_var = (ptr[4] - (ptr[3]*ptr[3]) / c) / c1;
|
|
float e3_3d_var = (ptr[6] - (ptr[5]*ptr[5]) / c) / c1;
|
|
float e3[] = { ptr[3]/c, sqrt(e3_2d_var), ptr[5]/c, sqrt(e3_3d_var), ptr[0]/c, ptr[1]/c, ptr[2]/c };
|
|
|
|
ptr = errorSum_ball;
|
|
float e4_2d_var=0, e4_3d_var=0;
|
|
if(cb1 > 0){
|
|
e4_2d_var = (ptr[4] - (ptr[3]*ptr[3]) / cb) / cb1;
|
|
e4_3d_var = (ptr[6] - (ptr[5]*ptr[5]) / cb) / cb1;
|
|
}
|
|
float e4[] = { ptr[3]/cb, sqrt(e4_2d_var), ptr[5]/cb, sqrt(e4_3d_var), ptr[0]/cb, ptr[1]/cb, ptr[2]/cb };
|
|
|
|
const int* st = state_counter;
|
|
printf("---------------------------------- LocalizerV2 Report ----------------------------------\n");
|
|
printf("SAMPLING STAGE 2D-MAE 2D-STD 3D-MAE 3D-STD x-MBE y-MBE z-MBE\n");
|
|
printf("Before fine-tune: %.4f %.4f %.4f %.4f %7.4f %7.4f %7.4f\n", e1[0],e1[1],e1[2],e1[3],e1[4],e1[5],e1[6]);
|
|
printf("After Euclidian dist. fit: %.4f %.4f %.4f %.4f %7.4f %7.4f %7.4f\n", e2[0],e2[1],e2[2],e2[3],e2[4],e2[5],e2[6]);
|
|
printf("After probabilistic fit: %.4f %.4f %.4f %.4f %7.4f %7.4f %7.4f\n", e3[0],e3[1],e3[2],e3[3],e3[4],e3[5],e3[6]);
|
|
printf("Ball: %.4f %.4f %.4f %.4f %7.4f %7.4f %7.4f\n\n", e4[0],e4[1],e4[2],e4[3],e4[4],e4[5],e4[6]);
|
|
printf("* MBE(Mean Bias Error) MAE(Mean Abs Error) STD(Standard Deviation)\n");
|
|
printf("* Note: the cheat positions should be active in server (preferably with >2 decimal places)\n\n");
|
|
printf("------------------LocalizerV2::run calls analysis:\n");
|
|
printf("- Total: %i \n", st[RUNNING]);
|
|
printf("- Successful: %i \n", st[DONE]);
|
|
printf("- Blind agent: %i \n", st[BLIND]);
|
|
printf("- Almost blind: %i \n", st[MINFAIL] + st[FAILzNOgoal] + st[FAILzLine] + st[FAILz]);
|
|
printf("- Guess location fail: %i \n", st[FAILguessLine] + st[FAILguessNone] + st[FAILguessMany] + st[FAILguessTest]);
|
|
printf("--- Lines too short: %i \n", st[FAILguessLine]);
|
|
printf("--- No solution: %i \n", st[FAILguessNone]);
|
|
printf("--- >1 solution: %i \n", st[FAILguessMany]);
|
|
printf("--- Weak solution: %i \n", st[FAILguessTest]);
|
|
printf("- Eucl. tune fail: %i \n", st[FAILtune]); //Euclidian distance tune error above 6cm
|
|
printf("----------------------------------------------------------------------------------------\n");
|
|
|
|
}
|
|
|
|
void LocalizerV2::stats_reset(){
|
|
|
|
counter_fineTune = 0;
|
|
for(int i=0; i<sizeof(errorSum_fineTune_before)/sizeof(errorSum_fineTune_before[0]); i++){
|
|
errorSum_fineTune_before[i] = 0;
|
|
errorSum_fineTune_euclidianDist[i] = 0;
|
|
errorSum_fineTune_probabilistic[i] = 0;
|
|
}
|
|
|
|
for(int i=0; i<STATE::ENUMSIZE; i++){
|
|
state_counter[i] = 0;
|
|
}
|
|
|
|
}
|
|
|
|
void LocalizerV2::stats_change_state(enum STATE s){
|
|
|
|
state_counter[s]++;
|
|
state = s;
|
|
|
|
}
|
|
|
|
/**
|
|
* Take a sample of the current position error
|
|
* @param position estimated agent's position
|
|
* @param cheat actual position provided by server
|
|
* @param error_placeholder variable where result is stored
|
|
*/
|
|
int LocalizerV2::stats_sample_position_error(const Vector3f sample, const Vector3f& cheat, double error_placeholder[]){
|
|
|
|
if(world.my_cheat_abs_cart_pos == Vector3f(0,0,0)) return 0;
|
|
|
|
double x_err = sample.x - cheat.x;
|
|
double y_err = sample.y - cheat.y;
|
|
double z_err = sample.z - cheat.z;
|
|
double xx_err = x_err * x_err;
|
|
double yy_err = y_err * y_err;
|
|
double zz_err = z_err * z_err;
|
|
|
|
error_placeholder[0] += x_err;
|
|
error_placeholder[1] += y_err;
|
|
error_placeholder[2] += z_err;
|
|
|
|
double sq_err_2d = xx_err + yy_err;
|
|
error_placeholder[3] += sqrt(sq_err_2d);
|
|
error_placeholder[4] += sq_err_2d;
|
|
|
|
double sq_err_3d = xx_err + yy_err + zz_err;
|
|
error_placeholder[5] += sqrt(sq_err_3d);
|
|
error_placeholder[6] += sq_err_3d;
|
|
|
|
return 1;
|
|
}
|
|
|