tgErrorMetric.cpp
Go to the documentation of this file.
00001 #include <blort/TomGine/tgErrorMetric.h>
00002 #include <blort/TomGine/tgCollission.h>
00003 
00004 using namespace TomGine;
00005 
00006 vec3 tgErrorMetric::GetRandPointInTriangle(const vec3& v0, const vec3& v1, const vec3& v2, unsigned trials) const
00007 {
00008         vec3 e1 = v1 - v0;
00009         vec3 e2 = v2 - v0;
00010         vec3 p;
00011         
00012         unsigned i=0;
00013 //      float x = float(rand()) / RAND_MAX;
00014 //      float y = float(rand()) / RAND_MAX;
00015 //      p = v0 + e1*x + e2*y;
00016         do{
00017                 float x = float(rand()) / RAND_MAX;
00018                 float y = float(rand()) / RAND_MAX;
00019                 p = v0 + e1*x + e2*y;
00020                 i++;
00021         }while(!tgCollission::PointInTriangle(p,v0,v1,v2) && i<trials);
00022         
00023         if(i==trials){
00024                 printf("[tgErrorMetric::GetRandPointInTriangle] Warning: Number of trials exceeded, no point in triangle found.\n");
00025                 p = vec3();
00026         }
00027 //      printf("[tgErrorMetric::GetRandPointInTriangle] Trials: %d\n", i);
00028         return p;
00029 }
00030 
00031 tgErrorMetric::tgErrorMetric(tgModel model, unsigned num_points)
00032 {
00033 //      double dRandMax = 1.0f/RAND_MAX;
00034 //      
00035 //      printf("%d %e\n", RAND_MAX, dRandMax);
00036         
00037         // calculate area of model surface
00038         double Area = 0.0;
00039         double dArea = 0.0;
00040         for(unsigned f=0; f<model.m_faces.size(); f++){
00041                 vec3 e1 = model.m_vertices[model.m_faces[f].v[1]].pos - model.m_vertices[model.m_faces[f].v[0]].pos;
00042                 vec3 e2 = model.m_vertices[model.m_faces[f].v[2]].pos - model.m_vertices[model.m_faces[f].v[0]].pos;
00043                 vec3 c;
00044                 c.cross(e1,e2);
00045                 Area += c.length() * 0.5;
00046         }
00047         dArea = 1.0/Area;
00048         
00049         // for each face uniformly distribute random points over surface
00050         unsigned points_used = 0;
00051         for(unsigned f=0; f<model.m_faces.size(); f++){
00052                 // calculate area of face
00053                 vec3 v0 = model.m_vertices[model.m_faces[f].v[0]].pos;
00054                 vec3 v1 = model.m_vertices[model.m_faces[f].v[1]].pos;
00055                 vec3 v2 = model.m_vertices[model.m_faces[f].v[2]].pos;
00056                 vec3 e1 = v1 - v0;
00057                 vec3 e2 = v2 - v0;
00058                 vec3 c;
00059                 vec3 p;
00060                 c.cross(e1,e2);
00061                 double area = c.length() * 0.5;
00062                 
00063                 // calculate number of points with respect to area of the face
00064                 unsigned n = 0;
00065                 if(f==model.m_faces.size()-1)
00066                         n = num_points - points_used;  // use up remaining points
00067                 else{
00068                         n = (unsigned)round(area * dArea * num_points);
00069                         points_used += n;
00070                 }
00071                 
00072                 // distribute points randomly over face (distinguish triangles and quads)
00073                 if(model.m_faces[f].v.size() == 4){
00074                         vec3 v3 = model.m_vertices[model.m_faces[f].v[3]].pos;
00075                         unsigned m = n/2;
00076                         for(unsigned i=0; i<m; i++){
00077                                 p = GetRandPointInTriangle(v0, v1, v3);
00078                                 pointlist.push_back(p);
00079                         }
00080                         for(unsigned i=0; i<(n-m); i++){
00081                                 p = GetRandPointInTriangle(v1, v2, v3);
00082                                 pointlist.push_back(p);
00083                         }
00084                 }else if(model.m_faces[f].v.size() == 3){
00085                         for(unsigned i=0; i<n; i++){
00086                                 p = GetRandPointInTriangle(v0, v1, v2);
00087                                 pointlist.push_back(p);
00088                         }
00089                 }else{
00090                         printf("[tgErrorMetric::tgErrorMetric] Warning face size not supported (only quads or triangles)\n");
00091                 }
00092         }
00093 }
00094         
00095 vec3 tgErrorMetric::Compare(const TomGine::tgPose &p1, const TomGine::tgPose &p2)
00096 {
00097         double err_x = 0.0;
00098         double err_y = 0.0;
00099         double err_z = 0.0;
00100         double dN = 1.0 / pointlist.size();
00101         for(unsigned i=0; i<pointlist.size(); i++){
00102                 mat3 R1, R2;
00103                 vec3 t1, t2;
00104                 
00105                 p1.GetPose(R1,t1);
00106                 p2.GetPose(R2,t2);
00107                 
00108                 vec3 verr = (R1 * pointlist[i] + t1) - (R2 * pointlist[i] + t2);
00109                 
00110                 err_x += double(fabs(verr.x)) * dN;
00111                 err_y += double(fabs(verr.y)) * dN;
00112                 err_z += double(fabs(verr.z)) * dN;
00113         }
00114         vec3 err = vec3(err_x, err_y, err_z);
00115         return err;
00116 }
00117 


blort
Author(s): Michael Zillich, Thomas Mörwald, Johann Prankl, Andreas Richtsfeld, Bence Magyar (ROS version)
autogenerated on Thu Jan 2 2014 11:38:26