Stefan Röttger

   // Calculating the plate center by found markers centroids
   std::vector<Eigen::Vector3f> centers_vector;
   for(int ctr = 0; ctr < 5; ctr++) {
      Eigen::Vector3f marker_a, marker_b;      
      int ctr_a = ctr;
      int ctr_b = ctr<4?ctr+1:0;

      if(!IS_NAN(marker_centroids_vector[ctr_a][0]) && !IS_NAN(marker_centroids_vector[ctr_b][0])) {
         marker_a = marker_centroids_vector[ctr_a];
         marker_b = marker_centroids_vector[ctr_b];

         // Calculate two possible centers  
         Eigen::Vector3f distance_vector = marker_b - marker_a;
         Eigen::Vector3f possible_center_a = distance_vector.cross(plane_coefficients.head<3>());
         possible_center_a = possible_center_a.normalized() * 0.1 * distance_vector.norm() * std::sqrt( 25.0 + 10.0*std::sqrt(5.0) );
         Eigen::Vector3f possible_center_b = -possible_center_a;

         possible_center_a += marker_a + 0.5*distance_vector;
         possible_center_b += marker_a + 0.5*distance_vector;

         // Select the center, which lies closer to centroid of green points
         Eigen::Vector3f center;
         if((possible_center_a - green_centroid.head<3>()).norm() < (possible_center_b - green_centroid.head<3>()).norm()) {
            center = possible_center_a;
         }
         else {
            center = possible_center_b;
         }

         centers_vector.push_back(center);
      }
   }