windows.h, nonfree\gpu.hpp, and SSBA

Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Apps/CMakeLists.txt

@@ -0,0 +1,24 @@
+include_directories(${V3D_INCLUDE_DIRS} ${EXTRA_INC_DIRS})
+link_directories(${V3D_DIR} ${EXTRA_LIB_DIRS})
+link_libraries (V3D ${EXTRA_LIBRARIES} ${EXTRA_LIBS})
+
+set (CMAKE_BUILD_TYPE Release)
+#set (CMAKE_BUILD_TYPE Debug)
+
+if (V3DLIB_ENABLE_CUDA)
+  add_subdirectory(CUDA)
+endif (V3DLIB_ENABLE_CUDA)
+
+if (V3DLIB_ENABLE_SUITESPARSE)
+    add_v3d_executable (bundle_common bundle_common.cpp)
+    add_v3d_executable (bundle_varying bundle_varying.cpp)
+    add_v3d_executable (bundle_varying_nonlinlsq bundle_varying_nonlinlsq.cpp)
+endif (V3DLIB_ENABLE_SUITESPARSE)
+
+if (V3DLIB_ENABLE_CUDA AND V3DLIB_ENABLE_GPGPU AND (V3DLIB_ENABLE_SIFTGPU OR V3DLIB_ENABLE_VLFEAT OR V3DLIB_ENABLE_OPENCV))
+   add_subdirectory (SfM)
+endif (V3DLIB_ENABLE_CUDA AND V3DLIB_ENABLE_GPGPU AND (V3DLIB_ENABLE_SIFTGPU OR V3DLIB_ENABLE_VLFEAT OR V3DLIB_ENABLE_OPENCV))
+
+if (V3DLIB_ENABLE_LIBDAI)
+   add_subdirectory (CycleInference)
+endif (V3DLIB_ENABLE_LIBDAI)

Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Apps/bundle_common.cpp

@@ -0,0 +1,264 @@
+// Bundle adjustment application for datasets captured with the same camera (common intrinsics).
+
+#include "Math/v3d_linear.h"
+#include "Base/v3d_vrmlio.h"
+#include "Geometry/v3d_metricbundle.h"
+
+#include <iostream>
+#include <fstream>
+#include <map>
+
+using namespace V3D;
+using namespace std;
+
+namespace
+{
+
+   inline void
+   showErrorStatistics(double const f0,
+                       StdDistortionFunction const& distortion,
+                       vector<CameraMatrix> const& cams,
+                       vector<Vector3d> const& Xs,
+                       vector<Vector2d> const& measurements,
+                       vector<int> const& correspondingView,
+                       vector<int> const& correspondingPoint)
+   {
+      int const K = measurements.size();
+
+      double meanReprojectionError = 0.0;
+      for (int k = 0; k < K; ++k)
+      {
+         int const i = correspondingView[k];
+         int const j = correspondingPoint[k];
+         Vector2d p = cams[i].projectPoint(distortion, Xs[j]);
+
+         double reprojectionError = norm_L2(f0 * (p - measurements[k]));
+         meanReprojectionError += reprojectionError;
+
+         /*cout << "i=" << i << " j=" << j << " k=" << k << "\n";
+         displayVector(Xs[j]);
+         displayVector(f0*p);
+         displayVector(f0*measurements[k]);
+         displayMatrix(cams[i].getRotation());
+         displayVector(cams[i].getTranslation());
+         cout << "##################### error = " << reprojectionError << "\n";
+         if(reprojectionError > 2)
+             cout << "!\n";*/
+      }
+      cout << "mean reprojection error (in pixels): " << meanReprojectionError/K << endl;
+   }
+} // end namespace <>
+
+int
+main(int argc, char * argv[])
+{
+   if (argc != 3)
+   {
+      cerr << "Usage: " << argv[0] << " <sparse reconstruction file> <mode>" << endl;
+      cout << "<mode> is one of metric, focal, principal, radial, tangental." << endl;
+      return -1;
+   }
+
+   ifstream is(argv[1]);
+   if (!is)
+   {
+      cerr << "Cannot open " << argv[1] << endl;
+      return -2;
+   }
+
+   int mode = 0;
+   if (strcmp(argv[2], "metric") == 0)
+      mode = FULL_BUNDLE_METRIC;
+   else if (strcmp(argv[2], "focal") == 0)
+      mode = FULL_BUNDLE_FOCAL_LENGTH;
+   else if (strcmp(argv[2], "principal") == 0)
+      mode = FULL_BUNDLE_FOCAL_LENGTH_PP;
+   else if (strcmp(argv[2], "radial") == 0)
+      mode = FULL_BUNDLE_RADIAL;
+   else if (strcmp(argv[2], "tangential") == 0)
+      mode = FULL_BUNDLE_RADIAL_TANGENTIAL;
+   else
+   {
+      cerr << "Unknown bundle mode: " << argv[2] << endl;
+      return -2;
+   }
+
+   int N, M, K;
+   is >> M >> N >> K;
+   cout << "N (cams) = " << N << " M (points) = " << M << " K (measurements) = " << K << endl;
+
+   Matrix3x3d KMat;
+   StdDistortionFunction distortion;
+
+   makeIdentityMatrix(KMat);
+   is >> KMat[0][0] >> KMat[0][1] >> KMat[0][2] >> KMat[1][1] >> KMat[1][2]
+      >> distortion.k1 >> distortion.k2 >> distortion.p1 >> distortion.p2;
+
+   double const f0 = KMat[0][0];
+   cout << "intrinsic before bundle = "; displayMatrix(KMat);
+   Matrix3x3d Knorm = KMat;
+   // Normalize the intrinsic to have unit focal length.
+   scaleMatrixIP(1.0/f0, Knorm);
+   Knorm[2][2] = 1.0;
+
+   vector<int> pointIdFwdMap(M);
+   map<int, int> pointIdBwdMap;
+
+   vector<Vector3d > Xs(M);
+   for (int j = 0; j < M; ++j)
+   {
+      int pointId;
+      is >> pointId >> Xs[j][0] >> Xs[j][1] >> Xs[j][2];
+      pointIdFwdMap[j] = pointId;
+      pointIdBwdMap.insert(make_pair(pointId, j));
+   }
+   cout << "Read the 3D points." << endl;
+
+   vector<int> camIdFwdMap(N,-1);
+   map<int, int> camIdBwdMap;
+
+   vector<CameraMatrix> cams(N);
+   for (int i = 0; i < N; ++i)
+   {
+      int camId;
+      Matrix3x3d R;
+      Vector3d T;
+
+      is >> camId;
+      is >> R[0][0] >> R[0][1] >> R[0][2] >> T[0];
+      is >> R[1][0] >> R[1][1] >> R[1][2] >> T[1];
+      is >> R[2][0] >> R[2][1] >> R[2][2] >> T[2];
+
+      camIdFwdMap[i] = camId;
+      camIdBwdMap.insert(make_pair(camId, i));
+
+      cams[i].setIntrinsic(Knorm);
+#if 1
+      cams[i].setRotation(R);
+      cams[i].setTranslation(T);
+#else
+      cams[i].setRotation(transposedMatrix(R));
+      cams[i].setTranslation(transposedMatrix(R) * (-1.0 * T));
+#endif
+   }
+   cout << "Read the cameras." << endl;
+
+   vector<Vector2d > measurements;
+   vector<int> correspondingView;
+   vector<int> correspondingPoint;
+
+   measurements.reserve(K);
+   correspondingView.reserve(K);
+   correspondingPoint.reserve(K);
+
+   for (int k = 0; k < K; ++k)
+   {
+      int view, point;
+      Vector3d p, np;
+
+      is >> view >> point;
+      is >> p[0] >> p[1] >> p[2];
+
+      if (camIdBwdMap.find(view) != camIdBwdMap.end() &&
+          pointIdBwdMap.find(point) != pointIdBwdMap.end())
+      {
+         // Normalize the measurements to match the unit focal length.
+         scaleVectorIP(1.0/f0, p);
+         measurements.push_back(Vector2d(p[0], p[1]));
+         correspondingView.push_back(camIdBwdMap[view]);
+         correspondingPoint.push_back(pointIdBwdMap[point]);
+      }
+   } // end for (k)
+
+   K = measurements.size();
+
+   cout << "Read " << K << " valid 2D measurements." << endl;
+
+   showErrorStatistics(f0, distortion, cams, Xs, measurements, correspondingView, correspondingPoint);
+
+   V3D::optimizerVerbosenessLevel = 1;
+   double const inlierThreshold = 2.0 / fabs(f0);
+
+   Matrix3x3d K0 = cams[0].getIntrinsic();
+   cout << "K0 = "; displayMatrix(K0);
+
+   {
+      ScopedBundleExtrinsicNormalizer extNorm(cams, Xs);
+      CommonInternalsMetricBundleOptimizer opt(mode, inlierThreshold, K0, distortion, cams, Xs,
+                                               measurements, correspondingView, correspondingPoint);
+      opt.tau = 1e-3;
+      opt.maxIterations = 50;
+      opt.minimize();
+
+      cout << "optimizer status = " << opt.status << endl;
+   }
+
+   cout << "refined K = "; displayMatrix(K0);
+   cout << "distortion = " << distortion.k1 << " " << distortion.k2 << " "
+        << distortion.p1 << " " << distortion.p2 << endl;
+
+   for (int i = 0; i < N; ++i) cams[i].setIntrinsic(K0);
+
+   Matrix3x3d Knew = K0;
+   scaleMatrixIP(f0, Knew);
+   Knew[2][2] = 1.0;
+   cout << "Knew = "; displayMatrix(Knew);
+
+   showErrorStatistics(f0, distortion, cams, Xs, measurements, correspondingView, correspondingPoint);
+
+   ofstream os("refined.txt");
+   os << M << " " << N << " 0\n";
+   os.precision(15);
+   os << Knew[0][0] << " " << Knew[0][1] << " " << Knew[0][2] << " " << Knew[1][1] << " " << Knew[1][2] << " "
+      << distortion.k1 << " " << distortion.k2 << " "
+      << distortion.p1 << " " << distortion.p2 << " " << endl;
+
+   for (int j = 0; j < M; ++j)
+   {
+      os << pointIdFwdMap[j] << " " << Xs[j][0] << " " << Xs[j][1] << " " << Xs[j][2] << endl;
+   }
+
+   for (int i = 0; i < N; ++i)
+   {
+      if(camIdFwdMap[i] != -1) {
+         os << camIdFwdMap[i] << " ";
+         Matrix3x4d const RT = cams[i].getOrientation();
+         os << RT[0][0] << " " << RT[0][1] << " " << RT[0][2] << " " << RT[0][3] << " ";
+         os << RT[1][0] << " " << RT[1][1] << " " << RT[1][2] << " " << RT[1][3] << " ";
+         os << RT[2][0] << " " << RT[2][1] << " " << RT[2][2] << " " << RT[2][3] << endl;
+      }
+   }
+
+   {
+
+      Vector3d mean(0.0, 0.0, 0.0);
+      for (int j = 0; j < Xs.size(); ++j) addVectorsIP(Xs[j], mean);
+      scaleVectorIP(1.0/Xs.size(), mean);
+
+      vector<float> norms(Xs.size());
+      for (int j = 0; j < Xs.size(); ++j)
+         norms[j] = distance_L2(Xs[j], mean);
+
+      std::sort(norms.begin(), norms.end());
+      float distThr = norms[int(norms.size() * 0.9f)];
+      cout << "90% quantile distance: " << distThr << endl;
+
+      for (int j = 0; j < Xs.size(); ++j)
+      {
+         if (distance_L2(Xs[j], mean) > 3*distThr) makeZeroVector(Xs[j]);
+      }
+
+      writePointsToVRML(Xs, "refined.wrl");
+
+      for (size_t i = 0; i < cams.size(); ++i)
+      {
+         float const r = 256.0 * (1.0 - float(i) / cams.size());
+         float const g = 256.0 * (float(i) / cams.size());
+         float b = 0;
+         Vector3f color(r, g, b);
+         writeCameraFrustumToVRML(cams[i], 1.0, 1.0, 0.1 * distThr, color, "refined.wrl", true);
+      }
+   }
+
+   return 0;
+}