rtcMeshSet3DHelper.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 .......: MeshSet3DHelper.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 "rtc/rtcGeometry3D.h"
#include "rtc/rtcTransform.h"
#include "rtc/rtcMesh3DHelper.h"
#include "rtc/rtcMeshSet3DHelper.h"

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

MeshSet3DHelper::MeshSet3DHelper()
{
}

MeshSet3DHelper::~MeshSet3DHelper()
{
}

void MeshSet3DHelper::addMotionNoise(MeshSet3D& meshset, Vec4f& alpha)
{
// Vec3f sigma;
// float rot1, rot2, trans;
// std::vector<Vec3f> rel_motion;
//
// // FIRST PASS: first calculate relative motion informations
// rel_motion.push_back(Vec3f(0.0f));
// for(unsigned int i=1;i<meshset.meshes.size();++i) {
//   Pose3D& pose1 = meshset.meshes[i-1]->pose;
//   Pose3D& pose2 = meshset.meshes[i]->pose;
//
//   // calculate relative motion
//   float d_x = pose2.x()-pose1.x();
//   float d_y = pose2.y()-pose1.y();
//   float d_theta = pose2.theta()-pose1.theta();
//
//   // first rotation d_rot_1
//   rot1 = atan2(d_y,d_x)-pose1.theta();
//   // translation d_trans
//   trans = sqrt(sqr(d_x)+sqr(d_y));
//   // second rotation d_rot_2
//   rot2 = d_theta-rot1;
//
//   rel_motion.push_back(Vec3f(rot1,trans,rot2));
// }
//
// // SECOND PASS: calculate noise and update pose
// for(unsigned int i=1;i<meshset.meshes.size();++i) {
//   Pose3D& pose1 = meshset.meshes[i-1]->pose;
//   Pose3D& pose2 = meshset.meshes[i]->pose;
//
//   rot1 = rel_motion[i][0];
//   trans = rel_motion[i][1];
//   rot2 = rel_motion[i][2];
//
//   // calculate standard deviations
//   sigma[0] = alpha[0]*abs(rot1) + alpha[1]*abs(trans);
//   sigma[1] = alpha[2]*abs(trans) + alpha[3]*abs(rot1+rot2);
//   sigma[2] = alpha[0]*abs(rot2) + alpha[1]*abs(trans);
//
//   // add noise
//   rot1 = rot1+rtc::normalRand(0.0f, sigma[0]);
//   trans = trans+rtc::normalRand(0.0f, sigma[1]);
//   rot2 = rot2+rtc::normalRand(0.0f, sigma[2]);
//
//   // calculate new pose
//   float x = pose1.x() + trans*cos(pose1.theta()+rot1);
//   float y = pose1.y() + trans*sin(pose1.theta()+rot1);
//   float theta = rtc_normalize_theta(pose1.theta() + rot1 + rot2);
//
//   pose2.set(x,y,theta);
// }
}

void MeshSet3DHelper::addMeasurementNoise(MeshSet3D& meshset, Vec3f& alpha_measurement)
{
}

float MeshSet3DHelper::meanDistance(const MeshSet3D& meshset1, const MeshSet3D& meshset2)
{
  return 0;
}

void MeshSet3DHelper::removeIsolatedVertices(MeshSet3D& srcdest)
{
  MeshSet3D meshset_new;
  meshset_new.flags = srcdest.flags;
  meshset_new.topleft = srcdest.topleft;
  meshset_new.bottomright = srcdest.bottomright;

  for (int i=0; i<srcdest.numMeshes();++i) {
    Mesh3D* mesh = srcdest.meshes[i];
    Mesh3DHelper::removeIsolatedVertices(*mesh);
  }
  srcdest.set(meshset_new);
}

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