rtcMesh3DHelper.cpp

Go to the documentation of this file.

/*
 * Copyright (C) 2007
 * Robert Bosch LLC
 * Research and Technology Center North America
 * Palo Alto, California
 *
 * All rights reserved.
 *
 *------------------------------------------------------------------------------
 * project ....: Autonomous Technologies
 * file .......: Mesh3DHelper.cpp
 * authors ....: Benjamin Pitzer
 * organization: Robert Bosch LLC
 * creation ...: 02/29/2008
 * modified ...: $Date:2008-03-03 10:26:02 -0800 (Mon, 03 Mar 2008) $
 * changed by .: $Author:benjaminpitzer $
 * revision ...: $Revision:141 $
 */

//== INCLUDES ==================================================================
#include <map>
#include "rtc/rtcGeometry3D.h"
#include "rtc/rtcTransform.h"
#include "rtc/rtcVarMat.h"
#include "rtc/rtcPCA.h"
#include "rtc/rtcPoint3DKdTree.h"
#include "rtc/rtcMesh3DHelper.h"


//== NAMESPACES ================================================================
namespace rtc {

Mesh3DHelper::Mesh3DHelper()
{
}

Mesh3DHelper::~Mesh3DHelper()
{
}

void Mesh3DHelper::addMeasurementNoise(Mesh3D& mesh, Vec3f& alpha_measurement)
{

}

void Mesh3DHelper::updateVertexNormals(Mesh3D& mesh, int n_neighbors)
{
  PrincipalComponentAnalysis<float,3> pca;
  int num_points = mesh.numVertices();
  int *closest_points=new int[n_neighbors];
  int n;

  // create kd-tree
  float *x, *y, *z;
  x = (float *) calloc(num_points, sizeof(float)); rtc_test_alloc(x);
  y = (float *) calloc(num_points, sizeof(float)); rtc_test_alloc(y);
  z = (float *) calloc(num_points, sizeof(float)); rtc_test_alloc(y);
  for(int i=0;i<num_points; i++) {
    Vertex3D* v = mesh.vertices[i];
    x[i]=v->p[0];
    y[i]=v->p[1];
    z[i]=v->p[2];
  }
  Point3DKdTree point_kd_tree(x, y, z, num_points);
  free(x);
  free(y);
  free(z);

  VarMatf data;
  data.setSize(n_neighbors,3);

  for(int i=0;i<num_points; i++) {
   Vertex3D* v = mesh.vertices[i];
   // find the n nearest neighbors
   point_kd_tree.findNearest(v->p, n_neighbors,closest_points);

   // fill data matrix
   for(int j = 0; j < n_neighbors; j++) {
     n = closest_points[j];
     data.setRow(j,mesh.vertices[n]->p);
   }

   // perform pca
   SMat<float,3>& ev = pca.computeTransformMatrix(data);

   // set normal to third eigenvector
   v->n = ev.getCol(2);

   // flip normal
   Vec3f tmp = v->p.normalized();
   tmp*=-1;
   if(v->n.dot(tmp)<0)
     v->n*=-1;
  }

  delete closest_points;
}

void Mesh3DHelper::removeIsolatedVertices(Mesh3D& srcdest)
{
  Mesh3D mesh_new;
  mesh_new.pose = srcdest.pose;
  mesh_new.flags = srcdest.flags;
  #ifndef NO_GRAPHICS
  mesh_new.texture = srcdest.texture;
  #endif
  mesh_new.teximage = srcdest.teximage;
  mesh_new.texmask = srcdest.texmask;

  std::map<int,int> vertex_mapping;
  //== Vertices =======================================================
  int j_new=0;
  for (int j=0;j<srcdest.numVertices();++j)
  {
    Vertex3D* v = srcdest.vertices[j];
    if(v->faces.size()==0) continue;
    vertex_mapping[j]=j_new;
    Vertex3D* v_new = mesh_new.addVertex(v->p);
    v_new->n = v->n;
    v_new->c = v->c;
    v_new->flags = v->flags;
    v_new->t = v->t;
    j_new++;
  }

  //== Faces ==========================================================
  for (int j=0;j<srcdest.numFaces();++j)
  {
    Face3D* f = srcdest.faces[j];
    Face3D* f_new = mesh_new.addFace(vertex_mapping[f->v[0]],
                                      vertex_mapping[f->v[1]],
                                      vertex_mapping[f->v[2]]);
    f_new->flags = f->flags;
    f_new->n = f->n;
  }
  srcdest.set(mesh_new);
}


//==============================================================================
} // NAMESPACE rtc
//==============================================================================