sdf_tracker.cpp
Go to the documentation of this file.
00001 #include <cmath>
00002 #include <iostream>
00003 #include <limits>
00004 #include <cstddef>
00005 
00006 #include <Eigen/Core>
00007 #include <Eigen/StdVector>
00008 #include <Eigen/Geometry>
00009 #include <unsupported/Eigen/MatrixFunctions>
00010 
00011 #include <vtkImageData.h>
00012 #include <vtkFloatArray.h>
00013 #include <vtkXMLImageDataWriter.h>
00014 #include <vtkXMLImageDataReader.h>
00015 #include <vtkPointData.h>
00016 #include <vtkSmartPointer.h>
00017 
00018 #include <opencv2/core/core.hpp>
00019 
00020 #include <time.h>
00021 #include "sdf_tracker/sdf_tracker.h"
00022 
00023 SDF_Parameters::SDF_Parameters()
00024 {  
00025   image_width = 640;
00026   image_height = 480;
00027   interactive_mode = true;
00028   Wmax = 64.0;
00029   resolution = 0.01;
00030   XSize = 256;
00031   YSize = 256;
00032   ZSize = 256;
00033   Dmax = 0.1;
00034   Dmin = -0.04;
00035   pose_offset = Eigen::MatrixXd::Identity(4,4);
00036   robust_statistic_coefficient = 0.02;
00037   regularization = 0.01;
00038   min_pose_change = 0.01;
00039   min_parameter_update = 0.0001;
00040   raycast_steps = 12;
00041   fx = 520.0;
00042   fy = 520.0;
00043   cx = 319.5;
00044   cy = 239.5;
00045   render_window = "Render";
00046 }
00047 
00048 SDF_Parameters::~SDF_Parameters()
00049 {}
00050 
00051 SDFTracker::SDFTracker() {
00052     SDF_Parameters myparams = SDF_Parameters();
00053     this->Init(myparams);
00054 }
00055 
00056 SDFTracker::SDFTracker(SDF_Parameters &parameters)
00057 {
00058   this->Init(parameters);
00059 }
00060 
00061 SDFTracker::~SDFTracker()
00062 {
00063   
00064   this->DeleteGrids();
00065 
00066   for (int i = 0; i < parameters_.image_height; ++i)
00067   {
00068     if ( validityMask_[i]!=NULL)
00069     delete[] validityMask_[i];
00070   }
00071   delete[] validityMask_;
00072   
00073   if(depthImage_!=NULL)
00074   delete depthImage_;
00075 
00076   if(depthImage_denoised_!=NULL)
00077   delete depthImage_denoised_;
00078   
00079 };
00080 
00081 void SDFTracker::Init(SDF_Parameters &parameters)
00082 {
00083   parameters_ = parameters;
00084 
00085   int downsample=1;
00086   switch(parameters_.image_height)
00087   {
00088   case 480: downsample = 1; break; //VGA
00089   case 240: downsample = 2; break; //QVGA
00090   case 120: downsample = 4; break; //QQVGA
00091   }
00092   parameters_.fx /= downsample;
00093   parameters_.fy /= downsample;
00094   parameters_.cx /= downsample;
00095   parameters_.cy /= downsample;
00096 
00097   depthImage_ = new cv::Mat(parameters_.image_height,parameters_.image_width,CV_32FC1); 
00098   depthImage_denoised_ = new cv::Mat( parameters_.image_height,parameters_.image_width,CV_32FC1);
00099 
00100   validityMask_ = new bool*[parameters_.image_height];
00101   for (int i = 0; i < parameters_.image_height; ++i)
00102   {
00103     validityMask_[i] = new bool[parameters_.image_width];
00104     memset(validityMask_[i],0,parameters_.image_width);
00105   }   
00106 
00107   myGrid_ = new float**[parameters_.XSize];
00108 
00109   for (int i = 0; i < parameters_.XSize; ++i)
00110   {
00111     myGrid_[i] = new float*[parameters_.YSize];
00112 
00113     for (int j = 0; j < parameters_.YSize; ++j)
00114     {
00115       myGrid_[i][j] = new float[parameters_.ZSize*2];
00116     }
00117   }
00118     
00119   for (int x = 0; x < parameters_.XSize; ++x)
00120   {
00121     for (int y = 0; y < parameters_.YSize; ++y)
00122     {
00123       for (int z = 0; z < parameters_.ZSize; ++z)
00124       {
00125         myGrid_[x][y][z*2]=parameters_.Dmax;
00126         myGrid_[x][y][z*2+1]=0.0f;
00127       }
00128     }
00129   }
00130   quit_ = false;
00131   first_frame_ = true;
00132   Pose_ << 0.0,0.0,0.0,0.0,0.0,0.0;
00133   cumulative_pose_ << 0.0,0.0,0.0,0.0,0.0,0.0;
00134   Transformation_=parameters_.pose_offset*Eigen::MatrixXd::Identity(4,4);
00135 
00136   if(parameters_.interactive_mode)
00137   {
00138     cv::namedWindow( parameters_.render_window, 0 );
00139   }
00140 };
00141 
00142 void SDFTracker::DeleteGrids(void)
00143 {
00144   for (int i = 0; i < parameters_.XSize; ++i)
00145   {
00146     for (int j = 0; j < parameters_.YSize; ++j)
00147     {
00148       if (myGrid_[i][j]!=NULL)
00149       delete[] myGrid_[i][j];
00150     }
00151      
00152     if (myGrid_[i]!=NULL)
00153     delete[] myGrid_[i];   
00154   }
00155   if(myGrid_!=NULL)
00156   delete[] myGrid_;
00157 }
00158 
00159 void SDFTracker::MakeTriangles(void)
00160 {
00161   for (int i = 1; i < parameters_.XSize-2; ++i)
00162   {
00163     for (int j = 1; j < parameters_.YSize-2; ++j)
00164     {
00165       for (int k = 1; k < parameters_.ZSize-2; ++k)
00166       {
00167         Eigen::Vector4d CellOrigin = Eigen::Vector4d(double(i),double(j),double(k),1.0);
00168         //if(!validGradient(CellOrigin*parameters_.resolution)) continue;
00169         /*1*/MarchingTetrahedrons(CellOrigin,1);
00170         /*2*/MarchingTetrahedrons(CellOrigin,2);
00171         /*3*/MarchingTetrahedrons(CellOrigin,3);
00172         /*4*/MarchingTetrahedrons(CellOrigin,4);
00173         /*5*/MarchingTetrahedrons(CellOrigin,5);
00174         /*6*/MarchingTetrahedrons(CellOrigin,6);
00175       }
00176     }
00177   }
00178 }
00179 
00180 void SDFTracker::SaveTriangles(const std::string filename)
00181 {
00182   std::ofstream triangle_stream;
00183   triangle_stream.open(filename.c_str());
00184   
00185   for (int i = 0; i < triangles_.size()-3; i+=3)
00186   {
00187     triangle_stream << "v " << triangles_[i](0) << " " << triangles_[i](1) << " " << triangles_[i](2) <<std::endl;
00188     triangle_stream << "v " << triangles_[i+1](0) << " " << triangles_[i+1](1) << " " << triangles_[i+1](2) <<std::endl;
00189     triangle_stream << "v " << triangles_[i+2](0) << " " << triangles_[i+2](1) << " " << triangles_[i+2](2) <<std::endl; 
00190     triangle_stream << "f -1 -2 -3" << std::endl; 
00191   }
00192   triangle_stream.close();
00193 }
00194 
00195 Eigen::Vector4d 
00196 SDFTracker::VertexInterp(double iso, Eigen::Vector4d &p1d, Eigen::Vector4d &p2d,double valp1, double valp2)
00197 {
00198   double mu;
00199   Eigen::Vector4d p;
00200 
00201   if (fabs(iso-valp1) < 0.000001)
00202     {
00203       p <<  p1d(0) - parameters_.XSize/2*parameters_.resolution,
00204             p1d(1) - parameters_.YSize/2*parameters_.resolution,
00205             p1d(2) - parameters_.ZSize/2*parameters_.resolution, 
00206             p1d(3); 
00207       return(p);
00208     }
00209   if (fabs(iso-valp2) < 0.000001)
00210     {
00211       p <<  p2d(0) - parameters_.XSize/2*parameters_.resolution,
00212             p2d(1) - parameters_.YSize/2*parameters_.resolution,
00213             p2d(2) - parameters_.ZSize/2*parameters_.resolution, 
00214             p2d(3); 
00215       return(p);
00216     }
00217   if (fabs(valp1-valp2) < 0.000001)
00218     {
00219       p <<  p1d(0) - parameters_.XSize/2*parameters_.resolution,
00220             p1d(1) - parameters_.YSize/2*parameters_.resolution,
00221             p1d(2) - parameters_.ZSize/2*parameters_.resolution, 
00222             p1d(3); 
00223       return(p);
00224     }
00225 
00226   mu = (iso - valp1) / (valp2 - valp1);
00227   p(0) = p1d(0) + mu * (p2d(0) - p1d(0)) - parameters_.XSize/2*parameters_.resolution;
00228   p(1) = p1d(1) + mu * (p2d(1) - p1d(1)) - parameters_.YSize/2*parameters_.resolution;
00229   p(2) = p1d(2) + mu * (p2d(2) - p1d(2)) - parameters_.ZSize/2*parameters_.resolution;
00230   p(3) = 1.0;
00231   return(p);  
00232 };
00233 
00234 Eigen::Matrix4d 
00235 SDFTracker::Twist(const Vector6d &xi)
00236 {
00237   Eigen::Matrix4d M;
00238   
00239   M << 0.0  , -xi(2),  xi(1), xi(3),
00240        xi(2), 0.0   , -xi(0), xi(4),
00241       -xi(1), xi(0) , 0.0   , xi(5),
00242        0.0,   0.0   , 0.0   , 0.0  ;
00243   
00244   return M;
00245 };
00246 
00247 Eigen::Vector4d 
00248 SDFTracker::To3D(int row, int column, double depth, double fx, double fy, double cx, double cy)
00249 {
00250 
00251   Eigen::Vector4d ret(double(column-cx)*depth/(fx),
00252                       double(row-cy)*depth/(fy),
00253                       double(depth),
00254                       1.0f);
00255   return ret;
00256 };
00257 
00258 cv::Point2d 
00259 SDFTracker::To2D(const Eigen::Vector4d &location, double fx, double fy, double cx, double cy)
00260 {
00261   cv::Point2d pixel(0,0);  
00262   if(location(2) != 0)
00263   {
00264      pixel.x = (cx) + location(0)/location(2)*(fx);
00265      pixel.y = (cy) + location(1)/location(2)*(fy);
00266   }
00267   
00268   return pixel;  
00269 };
00270 
00271 bool
00272 SDFTracker::ValidGradient(const Eigen::Vector4d &location)
00273 {
00274  /* 
00275  The function tests the current location and its adjacent
00276  voxels for valid values (written at least once) to 
00277  determine if derivatives at this location are 
00278  computable in all three directions.
00279 
00280  Since the function SDF(Eigen::Vector4d &location) is a 
00281  trilinear interpolation between neighbours, testing the
00282  validity of the gradient involves looking at all the 
00283  values that would contribute to the final  gradient. 
00284  If any of these have a weight equal to zero, the result
00285  is false.
00286                       X--------X
00287                     /        / |
00288                   X--------X   ----X
00289                   |        |   | / |
00290               X----        |   X-------X
00291             /     |        | /       / |
00292           X-------X--------X-------X   |
00293           |     /        / |       |   |
00294           |   X--------X   |       |   X
00295      J    |   |        |   |       | /
00296      ^    X----        |   X-------X
00297      |        |        | / |  |
00298       --->I   X--------X   |  X
00299     /             |        | /
00300    v              X--------X
00301   K                                                */
00302 
00303   float eps = 10e-9; 
00304 
00305   double i,j,k;
00306   modf(location(0)/parameters_.resolution + parameters_.XSize/2, &i);
00307   modf(location(1)/parameters_.resolution + parameters_.YSize/2, &j);  
00308   modf(location(2)/parameters_.resolution + parameters_.ZSize/2, &k);
00309   
00310   if(std::isnan(i) || std::isnan(j) || std::isnan(k)) return false;
00311 
00312   int I = int(i)-1; int J = int(j)-1;   int K = int(k)-1;  
00313   
00314   if(I>=parameters_.XSize-4 || J>=parameters_.YSize-3 || K>=parameters_.ZSize-3 || I<=1 || J<=1 || K<=1)return false;
00315 
00316   // 2*K because weights and distances are packed into the same array
00317   float* D10 = &myGrid_[I+1][J+0][2*(K+1)];
00318   float* D20 = &myGrid_[I+2][J+0][2*(K+1)];
00319  
00320   float* D01 = &myGrid_[I+0][J+1][2*(K+1)];
00321   float* D11 = &myGrid_[I+1][J+1][2*(K+0)];
00322   float* D21 = &myGrid_[I+2][J+1][2*(K+0)];
00323   float* D31 = &myGrid_[I+3][J+1][2*(K+1)];
00324   
00325   float* D02 = &myGrid_[I+0][J+2][2*(K+1)];
00326   float* D12 = &myGrid_[I+1][J+2][2*(K+0)];
00327   float* D22 = &myGrid_[I+2][J+2][2*(K+0)];
00328   float* D32 = &myGrid_[I+3][J+2][2*(K+1)];
00329 
00330   float* D13 = &myGrid_[I+1][J+3][2*(K+1)];
00331   float* D23 = &myGrid_[I+2][J+3][2*(K+1)];
00332 
00333   if( D10[0] > parameters_.Dmax-eps || D10[2*1] > parameters_.Dmax-eps || 
00334       D20[0] > parameters_.Dmax-eps || D20[2*1] > parameters_.Dmax-eps || 
00335       
00336       D01[0] > parameters_.Dmax-eps || D01[2*1] > parameters_.Dmax-eps ||
00337       D11[0] > parameters_.Dmax-eps || D11[2*1] > parameters_.Dmax-eps || D11[2*2] > parameters_.Dmax-eps || D11[2*3] > parameters_.Dmax-eps ||
00338       D21[0] > parameters_.Dmax-eps || D21[2*1] > parameters_.Dmax-eps || D21[2*2] > parameters_.Dmax-eps || D21[2*3] > parameters_.Dmax-eps ||
00339       D31[0] > parameters_.Dmax-eps || D31[2*1] > parameters_.Dmax-eps ||
00340       
00341       D02[0] > parameters_.Dmax-eps || D02[2*1] > parameters_.Dmax-eps ||
00342       D12[0] > parameters_.Dmax-eps || D12[2*1] > parameters_.Dmax-eps || D12[2*2] > parameters_.Dmax-eps || D12[2*3] > parameters_.Dmax-eps ||
00343       D22[0] > parameters_.Dmax-eps || D22[2*1] > parameters_.Dmax-eps || D22[2*2] > parameters_.Dmax-eps || D22[2*3] > parameters_.Dmax-eps ||
00344       D32[0] > parameters_.Dmax-eps || D32[2*1] > parameters_.Dmax-eps ||
00345       
00346       D13[0] > parameters_.Dmax-eps || D13[2*1] > parameters_.Dmax-eps ||
00347       D23[0] > parameters_.Dmax-eps || D23[2*1] > parameters_.Dmax-eps 
00348       ) return false;
00349   else return true;
00350 }
00351 
00352 
00353 double 
00354 SDFTracker::SDFGradient(const Eigen::Vector4d &location, int stepSize, int dim )
00355 {
00356   double delta=parameters_.resolution*stepSize;
00357   Eigen::Vector4d location_offset = Eigen::Vector4d(0,0,0,1);
00358   location_offset(dim) = delta;
00359 
00360   return ((SDF(location+location_offset)) - (SDF(location-location_offset)))/(2.0*delta);
00361 };
00362 
00363 void 
00364 SDFTracker::MarchingTetrahedrons(Eigen::Vector4d &Origin, int tetrahedron)
00365 {
00366   /*
00367   The following part is adapted from code found at:
00368   http://paulbourke.net/geometry/polygonise/
00369 
00370   (Paul Bourke / David Thoth)
00371 
00372 
00373   Function that outputs polygons from the SDF. The function is called
00374   giving a 3D location  of the (zero, zero) vertex and an index. 
00375   The index indicates which of the six possible tetrahedrons that can 
00376   be formed within a cube should be checked. 
00377 
00378       04===============05
00379       |\\              |\\
00380       ||\\             | \\
00381       || \\            |  \\
00382       ||  07===============06
00383       ||  ||           |   ||
00384       ||  ||           |   ||
00385       00--||-----------01  ||
00386        \\ ||            \  ||
00387         \\||             \ ||
00388          \||              \||
00389           03===============02
00390 
00391   Polygonise a tetrahedron given its vertices within a cube
00392   This is an alternative algorithm to Marching Cubes.
00393   It results in a smoother surface but more triangular facets.
00394 
00395               + 0
00396              /|\
00397             / | \
00398            /  |  \
00399           /   |   \
00400          /    |    \
00401         /     |     \
00402        +------|------+ 1
00403       3 \     |     /
00404          \    |    /
00405           \   |   /
00406            \  |  /
00407             \ | /
00408              \|/
00409               + 2
00410 
00411   Typically, for each location in space one would call:
00412 
00413   marchingTetrahedrons(CellOrigin,1);
00414   marchingTetrahedrons(CellOrigin,2);
00415   marchingTetrahedrons(CellOrigin,3);
00416   marchingTetrahedrons(CellOrigin,4);
00417   marchingTetrahedrons(CellOrigin,5);
00418   marchingTetrahedrons(CellOrigin,6);              
00419   */
00420 
00421 
00422   float val0, val1, val2, val3;
00423   val0 = val1 = val2 = val3 = parameters_.Dmax;
00424 
00425   Eigen::Vector4d V0, V1, V2, V3;
00426     
00427   int i = int(Origin(0));
00428   int j = int(Origin(1));
00429   int k = int(Origin(2));
00430 
00431   switch(tetrahedron)
00432   {
00433     case 1:
00434     val0 = myGrid_[i][j][k*2+2];
00435     V0 = Eigen::Vector4d(Origin(0),Origin(1),Origin(2)+1,1.0)*parameters_.resolution;
00436     val1 = myGrid_[i+1][j][k*2];
00437     V1 = Eigen::Vector4d(Origin(0)+1,Origin(1),Origin(2),1.0)*parameters_.resolution;
00438     val2 = myGrid_[i][j][k*2];
00439     V2 = Eigen::Vector4d(Origin(0),Origin(1),Origin(2),1.0)*parameters_.resolution;
00440     val3 = myGrid_[i][j+1][k*2];
00441     V3 = Eigen::Vector4d(Origin(0),Origin(1)+1,Origin(2),1.0)*parameters_.resolution;      
00442     break;
00443     
00444     case 2:  
00445     val0 = myGrid_[i][j][k*2+2];
00446     V0 = Eigen::Vector4d(Origin(0),Origin(1),Origin(2)+1,1.0)*parameters_.resolution;
00447     val1 = myGrid_[i+1][j][k*2];
00448     V1 = Eigen::Vector4d(Origin(0)+1,Origin(1),Origin(2),1.0)*parameters_.resolution;
00449     val2 = myGrid_[i+1][j+1][k*2];
00450     V2 = Eigen::Vector4d(Origin(0)+1,Origin(1)+1,Origin(2),1.0)*parameters_.resolution;
00451     val3 = myGrid_[i][j+1][k*2];
00452     V3 = Eigen::Vector4d(Origin(0),Origin(1)+1,Origin(2),1.0)*parameters_.resolution;      
00453     break;
00454     
00455     case 3:
00456     val0 = myGrid_[i][j][k*2+2];
00457     V0 = Eigen::Vector4d(Origin(0),Origin(1),Origin(2)+1,1.0)*parameters_.resolution;
00458     val1 = myGrid_[i][j+1][k*2+2];
00459     V1 = Eigen::Vector4d(Origin(0),Origin(1)+1,Origin(2)+1,1.0)*parameters_.resolution;
00460     val2 = myGrid_[i+1][j+1][k*2];
00461     V2 = Eigen::Vector4d(Origin(0)+1,Origin(1)+1,Origin(2),1.0)*parameters_.resolution;
00462     val3 = myGrid_[i][j+1][k*2];
00463     V3 = Eigen::Vector4d(Origin(0),Origin(1)+1,Origin(2),1.0)*parameters_.resolution;      
00464     break;
00465     
00466     case 4:      
00467     val0 = myGrid_[i][j][k*2+2];
00468     V0 = Eigen::Vector4d(Origin(0),Origin(1),Origin(2)+1,1.0)*parameters_.resolution;
00469     val1 = myGrid_[i+1][j+1][k*2];
00470     V1 = Eigen::Vector4d(Origin(0)+1,Origin(1)+1,Origin(2),1.0)*parameters_.resolution;
00471     val2 = myGrid_[i+1][j][k*2+2];
00472     V2 = Eigen::Vector4d(Origin(0)+1,Origin(1),Origin(2)+1,1.0)*parameters_.resolution;
00473     val3 = myGrid_[i+1][j][k*2];
00474     V3 = Eigen::Vector4d(Origin(0)+1,Origin(1),Origin(2),1.0)*parameters_.resolution;      
00475     break;
00476       
00477     case 5:
00478     val0 = myGrid_[i][j][k*2+2];
00479     V0 = Eigen::Vector4d(Origin(0),Origin(1),Origin(2)+1,1.0)*parameters_.resolution;
00480     val1 = myGrid_[i+1][j+1][k*2];
00481     V1 = Eigen::Vector4d(Origin(0)+1,Origin(1)+1,Origin(2),1.0)*parameters_.resolution;
00482     val2 = myGrid_[i+1][j][k*2+2];
00483     V2 = Eigen::Vector4d(Origin(0)+1,Origin(1),Origin(2)+1,1.0)*parameters_.resolution;
00484     val3 = myGrid_[i][j+1][k*2+2];
00485     V3 = Eigen::Vector4d(Origin(0),Origin(1)+1,Origin(2)+1,1.0)*parameters_.resolution;      
00486     break;
00487     
00488     case 6:
00489     val0 = myGrid_[i+1][j+1][k*2+2];
00490     V0 = Eigen::Vector4d(Origin(0)+1,Origin(1)+1,Origin(2)+1,1.0)*parameters_.resolution;
00491     val1 = myGrid_[i+1][j+1][k*2];
00492     V1 = Eigen::Vector4d(Origin(0)+1,Origin(1)+1,Origin(2),1.0)*parameters_.resolution;
00493     val2 = myGrid_[i+1][j][k*2+2];
00494     V2 = Eigen::Vector4d(Origin(0)+1,Origin(1),Origin(2)+1,1.0)*parameters_.resolution;
00495     val3 = myGrid_[i][j+1][k*2+2];
00496     V3 = Eigen::Vector4d(Origin(0),Origin(1)+1,Origin(2)+1,1.0)*parameters_.resolution;      
00497     break;
00498   }  
00499     
00500   if(val0>parameters_.Dmax-parameters_.resolution || val1>parameters_.Dmax-parameters_.resolution || val2>parameters_.Dmax-parameters_.resolution || val3>parameters_.Dmax-parameters_.resolution )
00501   
00502     return;
00503 
00504   int count = 0;
00505   if(val0 < 0)count++;
00506   if(val1 < 0)count++;
00507   if(val2 < 0)count++;
00508   if(val3 < 0)count++;
00509 
00510   {
00511     switch(count)
00512     {
00513       case 0:
00514       case 4:
00515       break;
00516 
00517       case 1:
00518       /*One voxel has material*/
00519       if(val0 < 0) /*03,02,01*/
00520       {       
00521         if(tetrahedron == 1 ||tetrahedron == 3 ||tetrahedron == 4 || tetrahedron == 6)
00522         {
00523           /*correct tetrahedrons for this winding: 1,3,4,6*/
00524           /*wrong tetrahedrons for this winding: 2,5*/
00525           triangles_.push_back(VertexInterp(0,V0,V3,val0,val3));
00526           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00527           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00528         }
00529         if(tetrahedron == 2 || tetrahedron == 5)
00530         {
00531           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00532           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00533           triangles_.push_back(VertexInterp(0,V0,V3,val0,val3));
00534         }
00535       }
00536       else if(val1 < 0) /*01,13,12*/
00537       {     
00538         if(tetrahedron == 2 ||tetrahedron == 5)
00539         {
00540           /*correct tetrahedrons for this winding: 2,5*/
00541           /*wrong tetrahedrons for this winding: 1,3,4,6*/
00542           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00543           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00544           triangles_.push_back(VertexInterp(0,V1,V2,val1,val2));
00545         }
00546         else if(tetrahedron == 1 ||tetrahedron == 3||tetrahedron == 4|| tetrahedron == 6)
00547         {
00548           triangles_.push_back(VertexInterp(0,V1,V2,val1,val2));
00549           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00550           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00551         }
00552       }
00553       else if(val2 < 0) /*02,12,23*/
00554       {     
00555         if(tetrahedron == 2 || tetrahedron == 5)
00556         {
00557           /*correct tetrahedrons for this winding: 2,5*/
00558           /*wrong tetrahedrons for this winding: 1,3,4,6*/
00559           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00560           triangles_.push_back(VertexInterp(0,V1,V2,val1,val2));
00561           triangles_.push_back(VertexInterp(0,V2,V3,val2,val3));
00562         }
00563         else if(tetrahedron == 3||tetrahedron == 1 ||tetrahedron == 4 ||tetrahedron == 6)
00564         {
00565           triangles_.push_back(VertexInterp(0,V2,V3,val2,val3));
00566           triangles_.push_back(VertexInterp(0,V1,V2,val1,val2));
00567           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00568         }
00569       }
00570       else if(val3 < 0) /*03,32,31*/
00571       {     
00572         if(tetrahedron == 2 ||tetrahedron == 5)
00573         {
00574           /*correct tetrahedrons for this winding: 2,5*/
00575           /*wrong tetrahedrons for this winding: 1,3,4,6*/
00576           triangles_.push_back(VertexInterp(0,V0,V3,val0,val3));
00577           triangles_.push_back(VertexInterp(0,V3,V2,val3,val2));
00578           triangles_.push_back(VertexInterp(0,V3,V1,val3,val1));
00579         }
00580         else if(tetrahedron == 1 ||tetrahedron == 3 ||tetrahedron == 4 ||tetrahedron == 6)
00581         {
00582           triangles_.push_back(VertexInterp(0,V3,V1,val3,val1));
00583           triangles_.push_back(VertexInterp(0,V3,V2,val3,val2));
00584           triangles_.push_back(VertexInterp(0,V0,V3,val0,val3));
00585         }
00586       }
00587       break;
00588 
00589       case 2:
00590       /*two voxels have material*/
00591       if(val0 < 0 && val3 < 0)   /*01,02,31;31,02,32*/
00592       { 
00593         if(tetrahedron == 2 ||tetrahedron == 5 )
00594         {
00595           /*correct tetrahedrons for this winding: 2,5*/
00596           /*wrong tetrahedrons for this winding: 1,3,4,6*/
00597           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00598           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00599           triangles_.push_back(VertexInterp(0,V3,V1,val3,val1));
00600           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00601           triangles_.push_back(VertexInterp(0,V2,V0,val2,val0));
00602           triangles_.push_back(VertexInterp(0,V3,V2,val3,val2));
00603         }
00604         else if(tetrahedron == 4||tetrahedron == 1 ||tetrahedron == 3 ||tetrahedron == 6)
00605         {
00606           triangles_.push_back(VertexInterp(0,V3,V1,val3,val1));
00607           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00608           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00609           triangles_.push_back(VertexInterp(0,V3,V2,val3,val2));
00610           triangles_.push_back(VertexInterp(0,V2,V0,val2,val0));
00611           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00612         }
00613       }
00614       else if(val1 < 0 && val2 < 0) /*13,32,02;02,01,13*/
00615       {     
00616         if(tetrahedron == 1 ||tetrahedron == 4 || tetrahedron == 6 || tetrahedron == 3)
00617         {
00618           /*correct tetrahedrons for this winding: 1,3,4,6*/
00619           /*wrong tetrahedrons for this winding: 2,5*/
00620           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00621           triangles_.push_back(VertexInterp(0,V3,V2,val3,val2));
00622           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00623           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00624           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00625           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00626         }
00627         else if(tetrahedron == 2||tetrahedron == 5)
00628         {
00629           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00630           triangles_.push_back(VertexInterp(0,V3,V2,val3,val2));
00631           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00632           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00633           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00634           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00635         }
00636       }
00637       else if(val2 < 0 && val3 < 0)/*03,02,13;13,02,12*/
00638       {     
00639         if(tetrahedron == 2 || tetrahedron == 5)
00640         {
00641           /*correct tetrahedrons for this winding: 2,5*/
00642           /*wrong tetrahedrons for this winding: 1,3,4,6*/
00643           triangles_.push_back(VertexInterp(0,V0,V3,val0,val3));
00644           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00645           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00646           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00647           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00648           triangles_.push_back(VertexInterp(0,V1,V2,val1,val2));
00649         }
00650         else if(tetrahedron == 1 ||tetrahedron == 3 ||tetrahedron == 4 ||tetrahedron == 6)
00651         {
00652           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00653           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00654           triangles_.push_back(VertexInterp(0,V0,V3,val0,val3));
00655           triangles_.push_back(VertexInterp(0,V1,V2,val1,val2));
00656           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00657           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00658         }
00659       }
00660       else if(val0 < 0 && val1 < 0)/*03,02,13;13,02,12*/
00661       {     
00662         if(tetrahedron == 3 ||tetrahedron == 6 || tetrahedron == 1 || tetrahedron == 4)
00663         {
00664           /*correct tetrahedrons for this winding: 1,3,4,6*/
00665           /*wrong tetrahedrons for this winding: 2,5*/
00666           triangles_.push_back(VertexInterp(0,V0,V3,val0,val3));
00667           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00668           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00669           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00670           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00671           triangles_.push_back(VertexInterp(0,V1,V2,val1,val2));
00672         }
00673         else if(tetrahedron == 2 || tetrahedron == 5 )
00674         {
00675           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00676           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00677           triangles_.push_back(VertexInterp(0,V0,V3,val0,val3));
00678           triangles_.push_back(VertexInterp(0,V1,V2,val1,val2));
00679           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00680           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00681         }
00682       }
00683       else if(val1 < 0 && val3 < 0)/*01,12,32;32,30,01*/
00684       {     
00685         if(tetrahedron == 1 ||tetrahedron == 3 ||tetrahedron == 4 || tetrahedron == 6)
00686         {
00687           /*correct tetrahedrons for this winding: 1,3,4,6*/
00688           /*wrong tetrahedrons for this winding: 2,5*/
00689           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00690           triangles_.push_back(VertexInterp(0,V1,V2,val1,val2));
00691           triangles_.push_back(VertexInterp(0,V2,V3,val2,val3));
00692           triangles_.push_back(VertexInterp(0,V2,V3,val2,val3));
00693           triangles_.push_back(VertexInterp(0,V3,V0,val3,val0));
00694           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00695         }
00696         else if(tetrahedron == 2 ||tetrahedron == 5)
00697         {
00698           triangles_.push_back(VertexInterp(0,V2,V3,val2,val3));
00699           triangles_.push_back(VertexInterp(0,V1,V2,val1,val2));
00700           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00701           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00702           triangles_.push_back(VertexInterp(0,V3,V0,val3,val0));
00703           triangles_.push_back(VertexInterp(0,V2,V3,val2,val3));
00704         }
00705       }
00706       else if(val0 < 0 && val2 < 0)/*01,03,32;32,12,01*/
00707       {
00708         if(tetrahedron == 1  ||tetrahedron == 3 ||tetrahedron == 4||tetrahedron == 6)
00709         {
00710           /*correct tetrahedrons for this winding: 1,3,4,6*/
00711           /*wrong tetrahedrons for this winding: 2,5*/
00712           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00713           triangles_.push_back(VertexInterp(0,V0,V3,val0,val3));
00714           triangles_.push_back(VertexInterp(0,V2,V3,val2,val3));
00715           triangles_.push_back(VertexInterp(0,V2,V3,val2,val3));
00716           triangles_.push_back(VertexInterp(0,V1,V2,val1,val2));
00717           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00718         }
00719         else if(tetrahedron == 5||tetrahedron == 2)
00720         {
00721           triangles_.push_back(VertexInterp(0,V2,V3,val2,val3));
00722           triangles_.push_back(VertexInterp(0,V0,V3,val0,val3));
00723           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00724           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00725           triangles_.push_back(VertexInterp(0,V1,V2,val1,val2));
00726           triangles_.push_back(VertexInterp(0,V2,V3,val2,val3));
00727         }
00728       }
00729       break;
00730       
00731       case 3:
00732       /*three voxels have material*/
00733       if(val0 > 0)/*03,01,02*/
00734       {       
00735         if(tetrahedron == 1 ||tetrahedron == 3 ||tetrahedron == 4 ||tetrahedron == 6 )
00736         {
00737           /*correct tetrahedrons for this winding: 1,3,4,6*/
00738           /*wrong tetrahedrons for this winding: 2,5*/
00739           triangles_.push_back(VertexInterp(0,V0,V3,val0,val3));
00740           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00741           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00742         }
00743         else if(tetrahedron == 2 || tetrahedron == 5)
00744         {
00745           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00746           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00747           triangles_.push_back(VertexInterp(0,V0,V3,val0,val3));
00748         }
00749       }
00750       else if(val1 > 0)/*10,12,13*/
00751       {
00752         if(tetrahedron == 2 ||tetrahedron == 5 )
00753         {
00754           /*correct tetrahedrons for this winding: 2,5*/
00755           /*wrong tetrahedrons for this winding: 1,3,4,6*/
00756           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00757           triangles_.push_back(VertexInterp(0,V1,V2,val1,val2));
00758           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00759         }
00760         else if(tetrahedron == 1 ||tetrahedron == 3 ||tetrahedron == 4 ||tetrahedron == 6 )
00761         {
00762           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00763           triangles_.push_back(VertexInterp(0,V1,V2,val1,val2));
00764           triangles_.push_back(VertexInterp(0,V0,V1,val0,val1));
00765         }
00766       }
00767       else if(val2 > 0) /*20,23,21*/
00768       {
00769         if(tetrahedron == 2 || tetrahedron == 5 )
00770         {
00771           /*correct tetrahedrons for this winding: 2,5*/
00772           /*wrong tetrahedrons for this winding: 1,3,4,6*/
00773           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00774           triangles_.push_back(VertexInterp(0,V2,V3,val2,val3));
00775           triangles_.push_back(VertexInterp(0,V2,V1,val2,val1));
00776         } 
00777         else if(tetrahedron == 1 ||tetrahedron == 3 ||tetrahedron == 4 ||tetrahedron == 6)
00778         {
00779           triangles_.push_back(VertexInterp(0,V2,V1,val2,val1));
00780           triangles_.push_back(VertexInterp(0,V2,V3,val2,val3));
00781           triangles_.push_back(VertexInterp(0,V0,V2,val0,val2));
00782         }
00783       }
00784       else if(val3 > 0)/*30,31,32*/
00785       {
00786         if(tetrahedron == 2 ||tetrahedron == 5  )
00787         {
00788           /*correct tetrahedrons for this winding: 2,5*/
00789           /*wrong tetrahedrons for this winding: 1,3,4,6*/
00790           triangles_.push_back(VertexInterp(0,V0,V3,val0,val3));
00791           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00792           triangles_.push_back(VertexInterp(0,V2,V3,val2,val3));
00793         }
00794         else if(tetrahedron == 3 ||tetrahedron == 1 ||tetrahedron == 4 ||tetrahedron == 6)
00795         {
00796           triangles_.push_back(VertexInterp(0,V2,V3,val2,val3));
00797           triangles_.push_back(VertexInterp(0,V1,V3,val1,val3));
00798           triangles_.push_back(VertexInterp(0,V0,V3,val0,val3));
00799         }
00800       }
00801       break;
00802     }
00803   }
00804 };
00805 
00806 void
00807 SDFTracker::UpdateDepth(const cv::Mat &depth)
00808 {
00809   depth_mutex_.lock();
00810   depth.copyTo(*depthImage_);
00811   depth_mutex_.unlock();
00812 
00813   for(int row=0; row<depthImage_->rows-0; ++row)
00814   { 
00815     const float* Drow = depthImage_->ptr<float>(row);
00816     #pragma omp parallel for 
00817     for(int col=0; col<depthImage_->cols-0; ++col)
00818     { 
00819       if(!std::isnan(Drow[col]) && Drow[col]>0.4)
00820       {
00821       validityMask_[row][col]=true;
00822       }else
00823       {
00824         validityMask_[row][col]=false;
00825       }
00826     }
00827   }
00828 }
00829 
00830 void
00831 SDFTracker::UpdatePoints(const std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d> > &Points)
00832 {
00833   points_mutex_.lock();
00834   this->Points_ = Points;
00835   points_mutex_.unlock();
00836 }
00837 
00838 
00839 void 
00840 SDFTracker::FuseDepth(void)
00841 {
00842   
00843   const float Wslope = 1/(parameters_.Dmax - parameters_.Dmin);
00844   Eigen::Matrix4d worldToCam = Transformation_.inverse();
00845   Eigen::Vector4d camera = worldToCam * Eigen::Vector4d(0.0,0.0,0.0,1.0);
00846   
00847   //Main 3D reconstruction loop
00848   for(int x = 0; x<parameters_.XSize; ++x)
00849   { 
00850   #pragma omp parallel for \
00851   shared(x)
00852     for(int y = 0; y<parameters_.YSize;++y)
00853     { 
00854       float* previousD = &myGrid_[x][y][0];
00855       float* previousW = &myGrid_[x][y][1];      
00856       for(int z = 0; z<parameters_.ZSize; ++z)
00857       {           
00858         //define a ray and point it into the center of a node
00859         Eigen::Vector4d ray((x-parameters_.XSize/2)*parameters_.resolution, (y- parameters_.YSize/2)*parameters_.resolution , (z- parameters_.ZSize/2)*parameters_.resolution, 1);        
00860         ray = worldToCam*ray;
00861         if(ray(2)-camera(2) < 0) continue;
00862         
00863         cv::Point2d uv;
00864         uv=To2D(ray,parameters_.fx,parameters_.fy,parameters_.cx,parameters_.cy );
00865         
00866         int j=floor(uv.x);
00867         int i=floor(uv.y);      
00868         
00869         //if the projected coordinate is within image bounds
00870         if(i>0 && i<depthImage_->rows-1 && j>0 && j <depthImage_->cols-1 && validityMask_[i][j] &&    
00871             validityMask_[i-1][j] && validityMask_[i][j-1])
00872         {
00873           const float* Di = depthImage_->ptr<float>(i);
00874           double Eta; 
00875           // const float W=1/((1+Di[j])*(1+Di[j]));
00876             
00877           Eta=(double(Di[j])-ray(2));       
00878             
00879           if(Eta >= parameters_.Dmin)
00880           {
00881               
00882             double D = std::min(Eta,parameters_.Dmax);
00883 
00884             float W = ((D - 1e-6) < parameters_.Dmax) ? 1.0f : Wslope*D - Wslope*parameters_.Dmin;
00885                 
00886             previousD[z*2] = (previousD[z*2] * previousW[z*2] + float(D) * W) /
00887                       (previousW[z*2] + W);
00888 
00889             previousW[z*2] = std::min(previousW[z*2] + W , float(parameters_.Wmax));
00890 
00891           }//within visible region 
00892         }//within bounds      
00893       }//z   
00894     }//y
00895   }//x
00896   return;
00897 };
00898 
00899 
00900 void 
00901 SDFTracker::FusePoints()
00902 {
00903   
00904   const float Wslope = 1/(parameters_.Dmax - parameters_.Dmin);
00905   const Eigen::Matrix4d camToWorld = Transformation_;
00906   const Eigen::Vector4d origin = camToWorld * Eigen::Vector4d(0.0,0.0,0.0,1.0);
00907   const double max_ray_length = 50.0;
00908 
00909   //3D reconstruction loop
00910   #pragma omp parallel for 
00911   for(int i = 0; i < Points_.size(); ++i)
00912   {
00913     bool hit = false;    
00914 
00915     Eigen::Vector4d p = camToWorld*Points_.at(i) - origin;
00916     
00917     if(std::isnan(p(0))) continue;
00918     
00919     // l is the length of the ray that hits the measured point along p.
00920     double l = p.norm();
00921     
00922     // normalize p to get a unit vector
00923     Eigen::Vector4d q = p/l;
00924 
00925     //set initial scaling
00926     double scaling = 0.0;
00927     double scaling_prev = 0.0;
00928     int steps=0;
00929     
00930     bool belowX, aboveX, belowY, aboveY, belowZ, aboveZ;
00931     belowX = aboveX = belowY = aboveY = belowZ = aboveZ = false;
00932 
00933     bool increasingX, increasingY, increasingZ;
00934     increasingX = increasingY = increasingZ  = false;
00935 
00936     while(steps<parameters_.raycast_steps && scaling < max_ray_length && !hit)
00937     { 
00938       //define current and previous raycast locations
00939       Eigen::Vector4d location = origin + scaling*q;
00940       Eigen::Vector4d location_prev = origin + scaling_prev*q;
00941 
00942       //get the floored grid location
00943       double i, j, k;
00944       modf(location(0)/parameters_.resolution + parameters_.XSize/2, &i);
00945       modf(location(1)/parameters_.resolution + parameters_.YSize/2, &j);  
00946       modf(location(2)/parameters_.resolution + parameters_.ZSize/2, &k);
00947       int I = int(i); 
00948       int J = int(j);
00949       int K = int(k);
00950 
00951       //is this grid location less than the first index in any dimension?
00952       belowX = (int(i) < 0) ? true : false;
00953       belowY = (int(j) < 0) ? true : false;
00954       belowZ = (int(k) < 0) ? true : false;
00955 
00956       //is this grid location greater than the laast index in any dimension?
00957       aboveX = (int(i) >= parameters_.XSize) ? true : false;
00958       aboveY = (int(j) >= parameters_.YSize) ? true : false;
00959       aboveZ = (int(k) >= parameters_.ZSize) ? true : false;
00960 
00961       //is the position increasing in any dimension?
00962       increasingX = (location(0) > location_prev(0)) ? true : false;
00963       increasingY = (location(1) > location_prev(1)) ? true : false;
00964       increasingZ = (location(2) > location_prev(2)) ? true : false;
00965 
00966       //Based on the previous queries, has the ray already missed the volume?
00967       //current and previous locations are identical when scaling is zero, so "increasing" will likely be false in all Dim.
00968       if( ((aboveX && increasingX) || (belowX && !increasingX) ||
00969            (aboveY && increasingY) || (belowY && !increasingY) ||
00970            (aboveZ && increasingZ) || (belowZ && !increasingZ) ) && (scaling-1e-6 > 0.0)) 
00971       {
00972         hit = true;
00973       }
00974 
00975       //the ray may not have missed, but no update can be made at this location.
00976       if( aboveX || aboveY || aboveZ || belowX || belowY || belowZ )
00977       {
00978         //update scaling factors and skip forward
00979         scaling_prev = scaling;
00980         scaling += parameters_.resolution;  
00981         ++steps;  
00982         continue;
00983       }
00984         Eigen::Vector4d cellVector = Eigen::Vector4d( (i-parameters_.XSize/2)*parameters_.resolution, 
00985                                                       (j-parameters_.YSize/2)*parameters_.resolution, 
00986                                                       (k-parameters_.ZSize/2)*parameters_.resolution, 1.0) - origin;
00987         // cell update: 
00988         // a = point - origin        
00989         // b = cell - origin        
00990         // D = a.norm - b.norm
00991 
00992       double Eta = l - cellVector.norm();
00993 
00994       //maximum "penetration" depth not yet achieved - keep updating    
00995       if(Eta > parameters_.Dmin)
00996       {
00997         
00998         double D = std::min(Eta,parameters_.Dmax);
00999 
01000         float* previousD = &myGrid_[I][J][K*2];
01001         float* previousW = &myGrid_[I][J][K*2+1];   
01002 
01003         
01004         float W = ((D - 1e-6) < parameters_.Dmax) ? 1.0f : Wslope*D - Wslope*parameters_.Dmin;
01005 
01006         previousD[0] = (previousD[0] * previousW[0] + float(D) * W) / (previousW[0] + W);
01007         previousW[0] = std::min(previousW[0] + W , float(parameters_.Wmax));
01008 
01009       } 
01010       else hit = true;
01011       scaling_prev = scaling;
01012       scaling += parameters_.resolution;  
01013       ++steps;        
01014     }//ray
01015   }//points
01016   return;
01017 };
01018 
01019 
01020 void 
01021 SDFTracker::FuseDepth(const cv::Mat& depth)
01022 {
01023   const float Wslope = 1/(parameters_.Dmax - parameters_.Dmin);
01024   this->UpdateDepth(depth);
01025   bool hasfused;
01026   if(!first_frame_)
01027   {
01028     hasfused = true;
01029     Pose_ = EstimatePoseFromDepth();
01030   } 
01031   else
01032   {
01033     hasfused = false;
01034     first_frame_ = false;
01035     if(depthImage_->rows!=parameters_.image_height) std::cout << "depth image rows do not match given image height parameter"<<std::endl;
01036 
01037   } 
01038   
01039   Transformation_ = Twist(Pose_).exp()*Transformation_;
01040   
01041   transformations_.push_back(Transformation_);
01042   cumulative_pose_ += Pose_;
01043   Pose_ = Pose_ * 0.0;
01044 
01045   if(cumulative_pose_.norm() < parameters_.min_pose_change && hasfused ){Render(); return;}
01046   cumulative_pose_ *= 0.0;
01047   
01048   Eigen::Matrix4d worldToCam = Transformation_.inverse();
01049   Eigen::Vector4d camera = worldToCam * Eigen::Vector4d(0.0,0.0,0.0,1.0);
01050   //Main 3D reconstruction loop
01051   
01052   for(int x = 0; x<parameters_.XSize; ++x)
01053   { 
01054   #pragma omp parallel for \
01055   shared(x)
01056     for(int y = 0; y<parameters_.YSize;++y)
01057     { 
01058       float* previousD = &myGrid_[x][y][0];
01059       float* previousW = &myGrid_[x][y][1];      
01060       for(int z = 0; z<parameters_.ZSize; ++z)
01061       {           
01062         //define a ray and point it into the center of a node
01063         Eigen::Vector4d ray((x-parameters_.XSize/2)*parameters_.resolution, (y- parameters_.YSize/2)*parameters_.resolution , (z- parameters_.ZSize/2)*parameters_.resolution, 1);        
01064         ray = worldToCam*ray;
01065         if(ray(2)-camera(2) < 0) continue;
01066         
01067         cv::Point2d uv;
01068         uv=To2D(ray,parameters_.fx,parameters_.fy,parameters_.cx,parameters_.cy );
01069         
01070         int j=floor(uv.x);
01071         int i=floor(uv.y);      
01072         
01073         //if the projected coordinate is within image bounds
01074         if(i>0 && i<depthImage_->rows-1 && j>0 && j <depthImage_->cols-1 && validityMask_[i][j] &&    
01075             validityMask_[i-1][j] && validityMask_[i][j-1])
01076         {
01077           const float* Di = depthImage_->ptr<float>(i);
01078           double Eta; 
01079        //   const float W=1/((1+Di[j])*(1+Di[j]));
01080             
01081           Eta=(double(Di[j])-ray(2));       
01082             
01083           if(Eta >= parameters_.Dmin)
01084           {
01085               
01086             double D = std::min(Eta,parameters_.Dmax);
01087          
01088             float W = ((D - 1e-6) < parameters_.Dmax) ? 1.0f : Wslope*D - Wslope*parameters_.Dmin;
01089 
01090             W *= 1/((1+Di[j])*(1+Di[j]));
01091 
01092             previousD[z*2] = (previousD[z*2] * previousW[z*2] + float(D) * W) /
01093                       (previousW[z*2] + W);
01094 
01095             previousW[z*2] = std::min(previousW[z*2] + W , float(parameters_.Wmax));
01096 
01097           }//within visible region 
01098         }//within bounds      
01099       }//z   
01100     }//y
01101   }//x
01102   Render();
01103   return;
01104 };
01105 
01106 
01107 double 
01108 SDFTracker::SDF(const Eigen::Vector4d &location)
01109 {
01110   double i,j,k;
01111   double x,y,z;
01112   
01113   if(std::isnan(location(0)+location(1)+location(2))) return parameters_.Dmax;
01114   
01115   x = modf(location(0)/parameters_.resolution + parameters_.XSize/2, &i);
01116   y = modf(location(1)/parameters_.resolution + parameters_.YSize/2, &j);  
01117   z = modf(location(2)/parameters_.resolution + parameters_.ZSize/2, &k);
01118     
01119   if(i>=parameters_.XSize-1 || j>=parameters_.YSize-1 || k>=parameters_.ZSize-1 || i<0 || j<0 || k<0)return parameters_.Dmax;
01120 
01121   int I = int(i); int J = int(j);   int K = int(k);
01122   
01123   float* N1 = &myGrid_[I][J][K*2];
01124   float* N2 = &myGrid_[I][J+1][K*2];
01125   float* N3 = &myGrid_[I+1][J][K*2];
01126   float* N4 = &myGrid_[I+1][J+1][K*2];
01127 
01128   double a1,a2,b1,b2;
01129   a1 = double(N1[0]*(1-z)+N1[2]*z);
01130   a2 = double(N2[0]*(1-z)+N2[2]*z);
01131   b1 = double(N3[0]*(1-z)+N3[2]*z);
01132   b2 = double(N4[0]*(1-z)+N4[2]*z);
01133     
01134   return double((a1*(1-y)+a2*y)*(1-x) + (b1*(1-y)+b2*y)*x);
01135 };
01136 
01137 Vector6d 
01138 SDFTracker::EstimatePoseFromDepth(void) 
01139 {
01140   Vector6d xi;
01141   xi<<0.0,0.0,0.0,0.0,0.0,0.0; // + (Pose-previousPose)*0.1;
01142   Vector6d xi_prev = xi;
01143   const double eps = 10e-9;
01144   const double c = parameters_.robust_statistic_coefficient*parameters_.Dmax;
01145   
01146   const int iterations[3]={12, 8, 2};
01147   const int stepSize[3] = {4, 2, 1};
01148 
01149   for(int lvl=0; lvl < 3; ++lvl)
01150   {
01151     for(int k=0; k<iterations[lvl]; ++k)
01152     {
01153 
01154       const Eigen::Matrix4d camToWorld = Twist(xi).exp()*Transformation_;
01155       
01156       double A00=0.0;//,A01=0.0,A02=0.0,A03=0.0,A04=0.0,A05=0.0;
01157       double A10=0.0,A11=0.0;//,A12=0.0,A13=0.0,A14=0.0,A15=0.0;
01158       double A20=0.0,A21=0.0,A22=0.0;//,A23=0.0,A24=0.0,A25=0.0;
01159       double A30=0.0,A31=0.0,A32=0.0,A33=0.0;//,A34=0.0,A35=0.0;
01160       double A40=0.0,A41=0.0,A42=0.0,A43=0.0,A44=0.0;//,A45=0.0;
01161       double A50=0.0,A51=0.0,A52=0.0,A53=0.0,A54=0.0,A55=0.0;
01162       
01163       double g0=0.0, g1=0.0, g2=0.0, g3=0.0, g4=0.0, g5=0.0;
01164       
01165       for(int row=0; row<depthImage_->rows-0; row+=stepSize[lvl])
01166       {          
01167         #pragma omp parallel for \
01168         default(shared) \
01169         reduction(+:g0,g1,g2,g3,g4,g5,A00,A10,A11,A20,A21,A22,A30,A31,A32,A33,A40,A41,A42,A43,A44,A50,A51,A52,A53,A54,A55)
01170         for(int col=0; col<depthImage_->cols-0; col+=stepSize[lvl])
01171         {
01172           if(!validityMask_[row][col]) continue;
01173           double depth = double(depthImage_->ptr<float>(row)[col]); 
01174           Eigen::Vector4d currentPoint = camToWorld*To3D(row,col,depth,parameters_.fx,parameters_.fy,parameters_.cx,parameters_.cy);
01175           
01176           if(!ValidGradient(currentPoint)) continue;
01177           double D = (SDF(currentPoint));
01178           double Dabs = fabs(D);
01179           if(D > parameters_.Dmax - eps || D < parameters_.Dmin + eps) continue;
01180           
01181           //partial derivative of SDF wrt position  
01182           Eigen::Matrix<double,1,3> dSDF_dx(SDFGradient(currentPoint,1,0),
01183                                             SDFGradient(currentPoint,1,1),
01184                                             SDFGradient(currentPoint,1,2) 
01185                                             );
01186           //partial derivative of position wrt optimizaiton parameters
01187           Eigen::Matrix<double,3,6> dx_dxi; 
01188           dx_dxi << 0, currentPoint(2), -currentPoint(1), 1, 0, 0,
01189                     -currentPoint(2), 0, currentPoint(0), 0, 1, 0,
01190                     currentPoint(1), -currentPoint(0), 0, 0, 0, 1;
01191 
01192           //jacobian = derivative of SDF wrt xi (chain rule)
01193           Eigen::Matrix<double,1,6> J = dSDF_dx*dx_dxi;
01194           
01195           //double tukey = (1-(Dabs/c)*(Dabs/c))*(1-(Dabs/c)*(Dabs/c));
01196           double huber = Dabs < c ? 1.0 : c/Dabs;
01197           
01198           //Gauss - Newton approximation to hessian
01199           Eigen::Matrix<double,6,6> T1 = huber * J.transpose() * J;
01200           Eigen::Matrix<double,1,6> T2 = huber * J.transpose() * D;
01201           
01202           g0 = g0 + T2(0); g1 = g1 + T2(1); g2 = g2 + T2(2);
01203           g3 = g3 + T2(3); g4 = g4 + T2(4); g5 = g5 + T2(5);
01204           
01205           A00+=T1(0,0);//A01+=T1(0,1);A02+=T1(0,2);A03+=T1(0,3);A04+=T1(0,4);A05+=T1(0,5);
01206                 A10+=T1(1,0);A11+=T1(1,1);//A12+=T1(1,2);A13+=T1(1,3);A14+=T1(1,4);A15+=T1(1,5);
01207           A20+=T1(2,0);A21+=T1(2,1);A22+=T1(2,2);//A23+=T1(2,3);A24+=T1(2,4);A25+=T1(2,5);
01208           A30+=T1(3,0);A31+=T1(3,1);A32+=T1(3,2);A33+=T1(3,3);//A34+=T1(3,4);A35+=T1(3,5);
01209           A40+=T1(4,0);A41+=T1(4,1);A42+=T1(4,2);A43+=T1(4,3);A44+=T1(4,4);//A45+=T1(4,5);
01210           A50+=T1(5,0);A51+=T1(5,1);A52+=T1(5,2);A53+=T1(5,3);A54+=T1(5,4);A55+=T1(5,5);
01211 
01212         }//col
01213       }//row
01214   
01215       Eigen::Matrix<double,6,6> A;
01216 
01217       A<< A00,A10,A20,A30,A40,A50,
01218           A10,A11,A21,A31,A41,A51,
01219           A20,A21,A22,A32,A42,A52,
01220           A30,A31,A32,A33,A43,A53,
01221           A40,A41,A42,A43,A44,A54,
01222           A50,A51,A52,A53,A54,A55;
01223      double scaling = 1/A.maxCoeff();
01224       
01225       Vector6d g;
01226       g<< g0, g1, g2, g3, g4, g5;
01227       
01228       g *= scaling;
01229       A *= scaling;
01230       
01231       A = A + (parameters_.regularization)*Eigen::MatrixXd::Identity(6,6);
01232       xi = xi - A.ldlt().solve(g);
01233       Vector6d Change = xi-xi_prev;  
01234       double Cnorm = Change.norm();
01235       xi_prev = xi;
01236       if(Cnorm < parameters_.min_parameter_update) break;
01237     }//k
01238   }//level
01239   if(std::isnan(xi.sum())) xi << 0.0,0.0,0.0,0.0,0.0,0.0;
01240   return xi;
01241 };//function
01242 
01243 
01244 Vector6d 
01245 SDFTracker::EstimatePoseFromPoints(void) 
01246 {
01247   Vector6d xi;
01248   xi<<0.0,0.0,0.0,0.0,0.0,0.0; // + (Pose-previousPose)*0.1;
01249   Vector6d xi_prev = xi;
01250   const double eps = 10e-9;
01251   const double c = parameters_.robust_statistic_coefficient*parameters_.Dmax;
01252   
01253   const int iterations[3]={12, 8, 2};
01254   const int stepSize[3] = {4, 2, 1};
01255 
01256   for(int lvl=0; lvl < 3; ++lvl)
01257   {
01258     for(int k=0; k<iterations[lvl]; ++k)
01259     {
01260       const Eigen::Matrix4d camToWorld = Twist(xi).exp()*Transformation_;
01261             
01262       double A00=0.0;//,A01=0.0,A02=0.0,A03=0.0,A04=0.0,A05=0.0;
01263       double A10=0.0,A11=0.0;//,A12=0.0,A13=0.0,A14=0.0,A15=0.0;
01264       double A20=0.0,A21=0.0,A22=0.0;//,A23=0.0,A24=0.0,A25=0.0;
01265       double A30=0.0,A31=0.0,A32=0.0,A33=0.0;//,A34=0.0,A35=0.0;
01266       double A40=0.0,A41=0.0,A42=0.0,A43=0.0,A44=0.0;//,A45=0.0;
01267       double A50=0.0,A51=0.0,A52=0.0,A53=0.0,A54=0.0,A55=0.0;
01268       
01269       double g0=0.0, g1=0.0, g2=0.0, g3=0.0, g4=0.0, g5=0.0;
01270       
01271       #pragma omp parallel for \
01272       default(shared) \
01273         reduction(+:g0,g1,g2,g3,g4,g5,A00,A10,A11,A20,A21,A22,A30,A31,A32,A33,A40,A41,A42,A43,A44,A50,A51,A52,A53,A54,A55)
01274       for(int idx=0; idx<Points_.size(); idx+=stepSize[lvl])
01275       {          
01276           if(std::isnan(Points_.at(idx)(0))) continue;
01277           Eigen::Vector4d currentPoint = camToWorld*Points_.at(idx);
01278           
01279           if(!ValidGradient(currentPoint)) continue;
01280           double D = (SDF(currentPoint));
01281           double Dabs = fabs(D);
01282           if(D > parameters_.Dmax - eps || D < parameters_.Dmin + eps) continue;
01283           
01284           //partial derivative of SDF wrt position  
01285           Eigen::Matrix<double,1,3> dSDF_dx(SDFGradient(currentPoint,1,0),
01286                                             SDFGradient(currentPoint,1,1),
01287                                             SDFGradient(currentPoint,1,2) 
01288                                             );
01289           //partial derivative of position wrt optimizaiton parameters
01290           Eigen::Matrix<double,3,6> dx_dxi; 
01291           dx_dxi << 0, currentPoint(2), -currentPoint(1), 1, 0, 0,
01292                     -currentPoint(2), 0, currentPoint(0), 0, 1, 0,
01293                     currentPoint(1), -currentPoint(0), 0, 0, 0, 1;
01294 
01295           //jacobian = derivative of SDF wrt xi (chain rule)
01296           Eigen::Matrix<double,1,6> J = dSDF_dx*dx_dxi;
01297           
01298           //double tukey = (1-(Dabs/c)*(Dabs/c))*(1-(Dabs/c)*(Dabs/c));
01299           double huber = Dabs < c ? 1.0 : c/Dabs;
01300           
01301           //Gauss - Newton approximation to hessian
01302           Eigen::Matrix<double,6,6> T1 = huber * J.transpose() * J;
01303           Eigen::Matrix<double,1,6> T2 = huber * J.transpose() * D;
01304           
01305           g0 = g0 + T2(0); g1 = g1 + T2(1); g2 = g2 + T2(2);
01306           g3 = g3 + T2(3); g4 = g4 + T2(4); g5 = g5 + T2(5);
01307           
01308           A00+=T1(0,0);//A01+=T1(0,1);A02+=T1(0,2);A03+=T1(0,3);A04+=T1(0,4);A05+=T1(0,5);
01309           A10+=T1(1,0);A11+=T1(1,1);//A12+=T1(1,2);A13+=T1(1,3);A14+=T1(1,4);A15+=T1(1,5);
01310           A20+=T1(2,0);A21+=T1(2,1);A22+=T1(2,2);//A23+=T1(2,3);A24+=T1(2,4);A25+=T1(2,5);
01311           A30+=T1(3,0);A31+=T1(3,1);A32+=T1(3,2);A33+=T1(3,3);//A34+=T1(3,4);A35+=T1(3,5);
01312           A40+=T1(4,0);A41+=T1(4,1);A42+=T1(4,2);A43+=T1(4,3);A44+=T1(4,4);//A45+=T1(4,5);
01313           A50+=T1(5,0);A51+=T1(5,1);A52+=T1(5,2);A53+=T1(5,3);A54+=T1(5,4);A55+=T1(5,5);
01314 
01315       }//idx
01316   
01317       Eigen::Matrix<double,6,6> A;
01318 
01319       A<< A00,A10,A20,A30,A40,A50,
01320           A10,A11,A21,A31,A41,A51,
01321           A20,A21,A22,A32,A42,A52,
01322           A30,A31,A32,A33,A43,A53,
01323           A40,A41,A42,A43,A44,A54,
01324           A50,A51,A52,A53,A54,A55;
01325      double scaling = 1/A.maxCoeff();
01326       
01327       Vector6d g;
01328       g<< g0, g1, g2, g3, g4, g5;
01329       
01330       g *= scaling;
01331       A *= scaling;
01332       
01333       A = A + (parameters_.regularization)*Eigen::MatrixXd::Identity(6,6);
01334       xi = xi - A.ldlt().solve(g);
01335       Vector6d Change = xi-xi_prev;  
01336       double Cnorm = Change.norm();
01337       xi_prev = xi;
01338       if(Cnorm < parameters_.min_parameter_update) break;
01339     }//k
01340   }//level
01341   if(std::isnan(xi.sum())) xi << 0.0,0.0,0.0,0.0,0.0,0.0;
01342   return xi;
01343 };//function
01344 
01345 
01346 void 
01347 SDFTracker::Render(void)
01348 {
01349   //double minStep = parameters_.resolution/4;
01350   cv::Mat depthImage_out(parameters_.image_height,parameters_.image_width,CV_32FC1);
01351   cv::Mat preview(parameters_.image_height,parameters_.image_width,CV_8UC3);
01352   
01353   const Eigen::Matrix4d camToWorld = Transformation_;
01354   const Eigen::Vector4d camera = camToWorld * Eigen::Vector4d(0.0,0.0,0.0,1.0);
01355   const Eigen::Vector4d viewAxis = (camToWorld * Eigen::Vector4d(0.0,0.0,1.0+1e-12,0.0) - camera).normalized();
01356   const double max_ray_length = 15.0;
01357   
01358   //Rendering loop
01359  #pragma omp parallel for 
01360   for(int u = 0; u < parameters_.image_height; ++u)
01361   {
01362     for(int v = 0; v < parameters_.image_width; ++v)
01363     {
01364       bool hit = false;
01365 
01366       Eigen::Vector4d p = camToWorld*To3D(u,v,1.0,parameters_.fx,parameters_.fy,parameters_.cx,parameters_.cy) - camera;
01367       p.normalize();
01368             
01369       double scaling = validityMask_[u][v] ? double(depthImage_->ptr<float>(u)[v])*0.7 : parameters_.Dmax;
01370       
01371       double scaling_prev=0;
01372       int steps=0;
01373       double D = parameters_.resolution;
01374       while(steps<parameters_.raycast_steps && scaling < max_ray_length && !hit)
01375       { 
01376 
01377         double D_prev = D;
01378         D = SDF(camera + p*scaling);
01379      
01380         if(D < 0.0)
01381         {
01382           scaling = scaling_prev + (scaling-scaling_prev)*D_prev/(D_prev - D);
01383 
01384           hit = true;
01385           Eigen::Vector4d normal_vector = Eigen::Vector4d::Zero();
01386  
01387           if(parameters_.interactive_mode)
01388           {  
01389             for(int ii=0; ii<3; ++ii)
01390             {
01391               normal_vector(ii) = SDFGradient(camera + p*scaling,1,ii);            
01392             }   
01393             normal_vector.normalize();
01394 
01395             preview.at<cv::Vec3b>(u,v)[1]=128-rint(normal_vector(0)*127);
01396             preview.at<cv::Vec3b>(u,v)[2]=128-rint(normal_vector(1)*127);
01397             preview.at<cv::Vec3b>(u,v)[0]=128-rint(normal_vector(2)*127);
01398           }
01399           
01400           depthImage_out.at<float>(u,v)=scaling*(viewAxis.dot(p));
01401           break;
01402         }
01403         scaling_prev = scaling;
01404         scaling += std::max(parameters_.resolution,D);  
01405         ++steps;        
01406       }//ray
01407       if(!hit)     
01408       {
01409         //Input values are better than nothing.
01410         depthImage_out.at<float>(u,v)=depthImage_->ptr<float>(u)[v];  
01411   
01412         if(parameters_.interactive_mode)
01413         {
01414           preview.at<cv::Vec3b>(u,v)[0]=uchar(30);
01415           preview.at<cv::Vec3b>(u,v)[1]=uchar(30);
01416           preview.at<cv::Vec3b>(u,v)[2]=uchar(30);
01417         }
01418       }//no hit
01419     }//col
01420   }//row
01421 
01422   depthDenoised_mutex_.lock();
01423   depthImage_out.copyTo(*depthImage_denoised_);
01424   depthDenoised_mutex_.unlock();    
01425  
01426   if(parameters_.interactive_mode)
01427   {
01428  
01429     cv::imshow(parameters_.render_window, preview);//depthImage_denoised);
01430     char q = cv::waitKey(3);
01431     if(q == 'q' || q  == 27 || q  == 71 ) { quit_ = true; }//int(key)
01432   }
01433   return;
01434 };
01435 
01436 void SDFTracker::GetDenoisedImage(cv::Mat &img) 
01437 {
01438     depthDenoised_mutex_.lock();
01439     depthImage_denoised_->copyTo(img);
01440     depthDenoised_mutex_.unlock();          
01441 }
01442 
01443 bool SDFTracker::Quit(void)
01444 {
01445   return quit_;
01446 }
01447 
01448 
01449 Eigen::Matrix4d 
01450 SDFTracker::GetCurrentTransformation(void)
01451 {
01452   Eigen::Matrix4d T;
01453   transformation_mutex_.lock();
01454   T = Transformation_;
01455   transformation_mutex_.unlock();
01456   return T;
01457 }
01458 
01459 void
01460 SDFTracker::SetCurrentTransformation(const Eigen::Matrix4d &T)
01461 {
01462   transformation_mutex_.lock();
01463   Transformation_= T;
01464   transformation_mutex_.unlock();
01465 }
01466 
01467 void SDFTracker::SaveSDF(const std::string &filename)
01468 {
01469 
01470 // http://www.vtk.org/Wiki/VTK/Examples/Cxx/IO/WriteVTI
01471 
01472   //vtkImageData *sdf_volume = vtkImageData::New();
01473   
01474   vtkSmartPointer<vtkImageData> sdf_volume = vtkSmartPointer<vtkImageData>::New();
01475 
01476   sdf_volume->SetDimensions(parameters_.XSize,parameters_.YSize,parameters_.ZSize);
01477   sdf_volume->SetOrigin(  parameters_.resolution*parameters_.XSize/2,
01478                           parameters_.resolution*parameters_.YSize/2,
01479                           parameters_.resolution*parameters_.ZSize/2);
01480    
01481   float spc = parameters_.resolution;
01482   sdf_volume->SetSpacing(spc,spc,spc);
01483   
01484   vtkSmartPointer<vtkFloatArray> distance = vtkSmartPointer<vtkFloatArray>::New();
01485   vtkSmartPointer<vtkFloatArray> weight = vtkSmartPointer<vtkFloatArray>::New();
01486   
01487   int numCells = parameters_.ZSize * parameters_.YSize * parameters_.XSize;
01488 
01489   distance->SetNumberOfTuples(numCells);
01490   weight->SetNumberOfTuples(numCells);
01491 
01492   int i, j, k, offset_k, offset_j;
01493   for(k=0;k < parameters_.ZSize; ++k)
01494   {
01495     offset_k = k*parameters_.XSize*parameters_.YSize;
01496     for(j=0; j<parameters_.YSize; ++j)
01497     {
01498       offset_j = j*parameters_.XSize;
01499       for(i=0; i<parameters_.XSize; ++i)
01500       {
01501         
01502         int offset = i + offset_j + offset_k;
01503         distance->SetValue(offset, myGrid_[i][j][k*2]);
01504         weight->SetValue(offset, myGrid_[i][j][k*2+1]);
01505 
01506      }
01507    }
01508  }
01509 
01510   sdf_volume->GetPointData()->AddArray(distance);
01511   distance->SetName("Distance");
01512 
01513   sdf_volume->GetPointData()->AddArray(weight);
01514   weight->SetName("Weight");
01515 
01516   vtkSmartPointer<vtkXMLImageDataWriter> writer =
01517   vtkSmartPointer<vtkXMLImageDataWriter>::New();
01518   writer->SetFileName(filename.c_str());
01519 
01520   //writer->SetInput(sdf_volume);
01521 #if VTK_MAJOR_VERSION <= 5
01522   writer->SetInput(sdf_volume);
01523 #else
01524   writer->SetInputData(sdf_volume);
01525 #endif
01526   writer->Write();
01527 }
01528 
01529 void SDFTracker::LoadSDF(const std::string &filename)
01530 {
01531  
01532   // //double valuerange[2];
01533   vtkXMLImageDataReader *reader  = vtkXMLImageDataReader::New();
01534   reader->SetFileName(filename.c_str());
01535   // //reader->GetOutput()->GetScalarRange(valuerange);
01536   reader->Update();
01537   reader->UpdateWholeExtent();
01538   reader->UpdateInformation();
01539 
01540   vtkSmartPointer<vtkImageData> sdf_volume = vtkSmartPointer<vtkImageData>::New();
01541   sdf_volume = reader->GetOutput();
01542   this->DeleteGrids();
01543 
01544   //will segfault if not specified
01545   int* sizes = sdf_volume->GetDimensions();
01546   parameters_.XSize = sizes[0];
01547   parameters_.YSize = sizes[1];
01548   parameters_.ZSize = sizes[2];
01549 
01550   double* cell_sizes = sdf_volume->GetSpacing();
01551   parameters_.resolution = float(cell_sizes[0]);  //TODO add support for different scalings along x,y,z. 
01552 
01553   this->Init(parameters_);
01554 
01555   vtkFloatArray *distance =vtkFloatArray::New();
01556   vtkFloatArray *weight =vtkFloatArray::New();
01557   distance = (vtkFloatArray *)reader->GetOutput()->GetPointData()->GetScalars("Distance");
01558   weight = (vtkFloatArray *)reader->GetOutput()->GetPointData()->GetScalars("Weight");
01559 
01560   int i, j, k, offset_k, offset_j;
01561   for(k=0;k < parameters_.ZSize; ++k)
01562   {
01563     offset_k = k*parameters_.XSize*parameters_.YSize;
01564     for(j=0; j<parameters_.YSize; ++j)
01565     {
01566       offset_j = j*parameters_.XSize;
01567       for(i=0; i<parameters_.XSize; ++i)
01568       {
01569         int offset = i + offset_j + offset_k;
01570         myGrid_[i][j][k*2] = distance->GetValue(offset);
01571         myGrid_[i][j][k*2+1] = weight->GetValue(offset);        
01572       }
01573     }
01574   }
01575 }


sdf_tracker
Author(s): Daniel Canelhas
autogenerated on Mon Oct 6 2014 03:19:15