rtcMesh3DHelper.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2007
00003  * Robert Bosch LLC
00004  * Research and Technology Center North America
00005  * Palo Alto, California
00006  *
00007  * All rights reserved.
00008  *
00009  *------------------------------------------------------------------------------
00010  * project ....: Autonomous Technologies
00011  * file .......: Mesh3DHelper.cpp
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 02/29/2008
00015  * modified ...: $Date:2008-03-03 10:26:02 -0800 (Mon, 03 Mar 2008) $
00016  * changed by .: $Author:benjaminpitzer $
00017  * revision ...: $Revision:141 $
00018  */
00019 
00020 //== INCLUDES ==================================================================
00021 #include <map>
00022 #include "rtc/rtcGeometry3D.h"
00023 #include "rtc/rtcTransform.h"
00024 #include "rtc/rtcVarMat.h"
00025 #include "rtc/rtcPCA.h"
00026 #include "rtc/rtcPoint3DKdTree.h"
00027 #include "rtc/rtcMesh3DHelper.h"
00028 
00029 
00030 //== NAMESPACES ================================================================
00031 namespace rtc {
00032 
00033 Mesh3DHelper::Mesh3DHelper()
00034 {
00035 }
00036 
00037 Mesh3DHelper::~Mesh3DHelper()
00038 {
00039 }
00040 
00041 void Mesh3DHelper::addMeasurementNoise(Mesh3D& mesh, Vec3f& alpha_measurement)
00042 {
00043 
00044 }
00045 
00053 void Mesh3DHelper::updateVertexNormals(Mesh3D& mesh, int n_neighbors)
00054 {
00055   PrincipalComponentAnalysis<float,3> pca;
00056   int num_points = mesh.numVertices();
00057   int *closest_points=new int[n_neighbors];
00058   int n;
00059 
00060   // create kd-tree
00061   float *x, *y, *z;
00062   x = (float *) calloc(num_points, sizeof(float)); rtc_test_alloc(x);
00063   y = (float *) calloc(num_points, sizeof(float)); rtc_test_alloc(y);
00064   z = (float *) calloc(num_points, sizeof(float)); rtc_test_alloc(y);
00065   for(int i=0;i<num_points; i++) {
00066     Vertex3D* v = mesh.vertices[i];
00067     x[i]=v->p[0];
00068     y[i]=v->p[1];
00069     z[i]=v->p[2];
00070   }
00071   Point3DKdTree point_kd_tree(x, y, z, num_points);
00072   free(x);
00073   free(y);
00074   free(z);
00075 
00076   VarMatf data;
00077   data.setSize(n_neighbors,3);
00078 
00079   for(int i=0;i<num_points; i++) {
00080    Vertex3D* v = mesh.vertices[i];
00081    // find the n nearest neighbors
00082    point_kd_tree.findNearest(v->p, n_neighbors,closest_points);
00083 
00084    // fill data matrix
00085    for(int j = 0; j < n_neighbors; j++) {
00086      n = closest_points[j];
00087      data.setRow(j,mesh.vertices[n]->p);
00088    }
00089 
00090    // perform pca
00091    SMat<float,3>& ev = pca.computeTransformMatrix(data);
00092 
00093    // set normal to third eigenvector
00094    v->n = ev.getCol(2);
00095 
00096    // flip normal
00097    Vec3f tmp = v->p.normalized();
00098    tmp*=-1;
00099    if(v->n.dot(tmp)<0)
00100      v->n*=-1;
00101   }
00102 
00103   delete closest_points;
00104 }
00105 
00112 void Mesh3DHelper::removeIsolatedVertices(Mesh3D& srcdest)
00113 {
00114   Mesh3D mesh_new;
00115   mesh_new.pose = srcdest.pose;
00116   mesh_new.flags = srcdest.flags;
00117   #ifndef NO_GRAPHICS
00118   mesh_new.texture = srcdest.texture;
00119   #endif
00120   mesh_new.teximage = srcdest.teximage;
00121   mesh_new.texmask = srcdest.texmask;
00122 
00123   std::map<int,int> vertex_mapping;
00124   //== Vertices =======================================================
00125   int j_new=0;
00126   for (int j=0;j<srcdest.numVertices();++j)
00127   {
00128     Vertex3D* v = srcdest.vertices[j];
00129     if(v->faces.size()==0) continue;
00130     vertex_mapping[j]=j_new;
00131     Vertex3D* v_new = mesh_new.addVertex(v->p);
00132     v_new->n = v->n;
00133     v_new->c = v->c;
00134     v_new->flags = v->flags;
00135     v_new->t = v->t;
00136     j_new++;
00137   }
00138 
00139   //== Faces ==========================================================
00140   for (int j=0;j<srcdest.numFaces();++j)
00141   {
00142     Face3D* f = srcdest.faces[j];
00143     Face3D* f_new = mesh_new.addFace(vertex_mapping[f->v[0]],
00144                                       vertex_mapping[f->v[1]],
00145                                       vertex_mapping[f->v[2]]);
00146     f_new->flags = f->flags;
00147     f_new->n = f->n;
00148   }
00149   srcdest.set(mesh_new);
00150 }
00151 
00152 
00153 //==============================================================================
00154 } // NAMESPACE rtc
00155 //==============================================================================


rtc
Author(s): Benjamin Pitzer
autogenerated on Mon Oct 6 2014 10:07:35