rtcMeshSet3DHelper.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 .......: MeshSet3DHelper.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 "rtc/rtcGeometry3D.h"
00022 #include "rtc/rtcTransform.h"
00023 #include "rtc/rtcMesh3DHelper.h"
00024 #include "rtc/rtcMeshSet3DHelper.h"
00025 
00026 //== NAMESPACES ================================================================
00027 namespace rtc {
00028 
00029 MeshSet3DHelper::MeshSet3DHelper()
00030 {
00031 }
00032 
00033 MeshSet3DHelper::~MeshSet3DHelper()
00034 {
00035 }
00036 
00037 void MeshSet3DHelper::addMotionNoise(MeshSet3D& meshset, Vec4f& alpha)
00038 {
00039 // Vec3f sigma;
00040 // float rot1, rot2, trans;
00041 // std::vector<Vec3f> rel_motion;
00042 //
00043 // // FIRST PASS: first calculate relative motion informations
00044 // rel_motion.push_back(Vec3f(0.0f));
00045 // for(unsigned int i=1;i<meshset.meshes.size();++i) {
00046 //   Pose3D& pose1 = meshset.meshes[i-1]->pose;
00047 //   Pose3D& pose2 = meshset.meshes[i]->pose;
00048 //
00049 //   // calculate relative motion
00050 //   float d_x = pose2.x()-pose1.x();
00051 //   float d_y = pose2.y()-pose1.y();
00052 //   float d_theta = pose2.theta()-pose1.theta();
00053 //
00054 //   // first rotation d_rot_1
00055 //   rot1 = atan2(d_y,d_x)-pose1.theta();
00056 //   // translation d_trans
00057 //   trans = sqrt(sqr(d_x)+sqr(d_y));
00058 //   // second rotation d_rot_2
00059 //   rot2 = d_theta-rot1;
00060 //
00061 //   rel_motion.push_back(Vec3f(rot1,trans,rot2));
00062 // }
00063 //
00064 // // SECOND PASS: calculate noise and update pose
00065 // for(unsigned int i=1;i<meshset.meshes.size();++i) {
00066 //   Pose3D& pose1 = meshset.meshes[i-1]->pose;
00067 //   Pose3D& pose2 = meshset.meshes[i]->pose;
00068 //
00069 //   rot1 = rel_motion[i][0];
00070 //   trans = rel_motion[i][1];
00071 //   rot2 = rel_motion[i][2];
00072 //
00073 //   // calculate standard deviations
00074 //   sigma[0] = alpha[0]*abs(rot1) + alpha[1]*abs(trans);
00075 //   sigma[1] = alpha[2]*abs(trans) + alpha[3]*abs(rot1+rot2);
00076 //   sigma[2] = alpha[0]*abs(rot2) + alpha[1]*abs(trans);
00077 //
00078 //   // add noise
00079 //   rot1 = rot1+rtc::normalRand(0.0f, sigma[0]);
00080 //   trans = trans+rtc::normalRand(0.0f, sigma[1]);
00081 //   rot2 = rot2+rtc::normalRand(0.0f, sigma[2]);
00082 //
00083 //   // calculate new pose
00084 //   float x = pose1.x() + trans*cos(pose1.theta()+rot1);
00085 //   float y = pose1.y() + trans*sin(pose1.theta()+rot1);
00086 //   float theta = rtc_normalize_theta(pose1.theta() + rot1 + rot2);
00087 //
00089 //   pose2.set(x,y,theta);
00090 // }
00091 }
00092 
00093 void MeshSet3DHelper::addMeasurementNoise(MeshSet3D& meshset, Vec3f& alpha_measurement)
00094 {
00095 }
00096 
00097 float MeshSet3DHelper::meanDistance(const MeshSet3D& meshset1, const MeshSet3D& meshset2)
00098 {
00099   return 0;
00100 }
00101 
00108 void MeshSet3DHelper::removeIsolatedVertices(MeshSet3D& srcdest)
00109 {
00110   MeshSet3D meshset_new;
00111   meshset_new.flags = srcdest.flags;
00112   meshset_new.topleft = srcdest.topleft;
00113   meshset_new.bottomright = srcdest.bottomright;
00114 
00115   for (int i=0; i<srcdest.numMeshes();++i) {
00116     Mesh3D* mesh = srcdest.meshes[i];
00117     Mesh3DHelper::removeIsolatedVertices(*mesh);
00118   }
00119   srcdest.set(meshset_new);
00120 }
00121 
00122 //==============================================================================
00123 } // NAMESPACE rtc
00124 //==============================================================================


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