diff --git a/Analysis.cpp b/Analysis.cpp
index 4b2c821629dbc3d818cb604f9c05ffe1bb00cb1f..ff25423d7fd385d22e5e6e713323f1934d61363d 100644
--- a/Analysis.cpp
+++ b/Analysis.cpp
@@ -119,6 +119,11 @@ std::string Analysis::GetFilename (const std::string& str)
 void Analysis::InitArgs(ArgumentParser* args)
 {
      string s = "Parameter:\n";
+     _building = new Building();
+     _building->LoadGeometry(args->GetGeometryFilename());
+     // create the polygons
+     _building->InitGeometry();
+     // _building->AddSurroundingRoom();
 
      if(args->GetIsMethodA()) {
           _DoesUseMethodA = true;
@@ -177,6 +182,11 @@ void Analysis::InitArgs(ArgumentParser* args)
           _geoPoly = ReadGeometry(args->GetGeometryFilename(), _areaForMethod_I);
      }
 
+     if( _DoesUseMethodD &&  _DoesUseMethodI)
+     {
+          Log->Write("Warning:\t Using both method D and I is not safe!");
+          // because ReadGeomtry() may be called twice
+     }
      _deltaF = args->GetDelatT_Vins();
      _cutByCircle = args->GetIsCutByCircle();
      _getProfile = args->GetIsGetProfile();
@@ -200,12 +210,7 @@ void Analysis::InitArgs(ArgumentParser* args)
 
 std::map<int, polygon_2d> Analysis::ReadGeometry(const fs::path& geometryFile, const std::vector<MeasurementArea_B*>& areas)
 {
-     _building = new Building();
-     _building->LoadGeometry(geometryFile.string());
-     // create the polygons
-     _building->InitGeometry();
-     // _building->AddSurroundingRoom();
-
+     Log->Write("INFO:\tReadGeometry with %s", geometryFile.string().c_str());
      double geo_minX  = FLT_MAX;
      double geo_minY  = FLT_MAX;
      double geo_maxX  = -FLT_MAX;
diff --git a/methods/Method_I.h b/methods/Method_I.h
index 53100c07fdcbab4fc9b0c6b5b5756e1b81e63ab6..01ec100843c7d3e951eede75814b31a83eaf87a0 100644
--- a/methods/Method_I.h
+++ b/methods/Method_I.h
@@ -104,7 +104,7 @@ private:
      void GetProfiles(const std::string& frameId, const std::vector<polygon_2d>& polygons, const std::vector<double>& velocity);
      void OutputVoroGraph(const std::string & frameId,  std::vector<std::pair<polygon_2d, int> >& polygons, int numPedsInFrame,const std::vector<double>& VInFrame);
      void GetIndividualFD(const std::vector<polygon_2d>& polygon, const std::vector<double>& Velocity, const std::vector<int>& Id, const std::string& frid);
-     void GetIndividualFD(const std::vector<polygon_2d>& polygon, const std::vector<double>& Velocity, const std::vector<int>& Id, const std::string& frid, std::vector<double>& XInFrame, std::vector<double>& YInFrame, double ZInFrame);
+     void GetIndividualFD(const std::vector<polygon_2d>& polygon, const std::vector<double>& Velocity, const std::vector<int>& Id, const std::string& frid, std::vector<double>& XInFrame, std::vector<double>& YInFrame, std::vector<double>& ZInFrame);
      /**
       * Reduce the precision of the points to two digits
       * @param polygon
diff --git a/methods/PedData.cpp b/methods/PedData.cpp
index 94a15c5f69651155ce62ecbede372c939a421351..66b88d2ec228d2ed16837a561ca4b9976cf313b1 100644
--- a/methods/PedData.cpp
+++ b/methods/PedData.cpp
@@ -587,6 +587,27 @@ vector<double> PedData::GetZInFrame(int frame, const vector<int>& ids) const
      }
      return ZInFrame;
 }
+vector<double> PedData::GetZInFrame(int frame, const vector<int>& ids, double zPos) const
+{
+     vector<double> ZInFrame;
+     for(unsigned int i=0; i<ids.size();i++)
+     {
+          int id = ids[i];
+          if(zPos<1000000.0)
+          {
+               if(fabs(_zCor(id,frame)-zPos*M2CM)<J_EPS_EVENT)
+               {
+                    ZInFrame.push_back(_zCor(id,frame));
+               }
+          }
+          else
+          {
+               ZInFrame.push_back(_zCor(id,frame));
+          }
+
+     }
+     return ZInFrame;
+}
 
 vector<int> PedData::GetIdInFrame(int frame, const vector<int>& ids) const
 {
diff --git a/methods/PedData.h b/methods/PedData.h
index d71576de0ec03c6ac76150b2074709dd9f1f642f..3fa72be24da4d96cff2bbb81fc3f4c7e00026711 100644
--- a/methods/PedData.h
+++ b/methods/PedData.h
@@ -77,6 +77,7 @@ public:
      std::vector<double> GetXInFrame(int frame, const std::vector<int>& ids) const;
      std::vector<double> GetYInFrame(int frame, const std::vector<int>& ids) const;
      std::vector<double> GetZInFrame(int frame, const std::vector<int>& ids) const;
+     std::vector<double> GetZInFrame(int frame, const std::vector<int>& ids, double zPos) const;
      std::vector<double> GetVInFrame(int frame, const std::vector<int>& ids, double zPos) const;
      bool ReadData(const fs::path& projectRootDir,const fs::path& outputDir, const fs::path& path, const fs::path& filename, const FileFormat& _trajformat, int deltaF, std::string vComponent, const bool IgnoreBackwardMovement);
      fs::path GetOutputLocation() const;