diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Apps/CMakeLists.txt b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Apps/CMakeLists.txt new file mode 100644 index 0000000..0e7abb4 --- /dev/null +++ b/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) diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Apps/bundle_common.cpp b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Apps/bundle_common.cpp new file mode 100644 index 0000000..c0699fe --- /dev/null +++ b/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 +#include +#include + +using namespace V3D; +using namespace std; + +namespace +{ + + inline void + showErrorStatistics(double const f0, + StdDistortionFunction const& distortion, + vector const& cams, + vector const& Xs, + vector const& measurements, + vector const& correspondingView, + vector 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] << " " << endl; + cout << " 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 pointIdFwdMap(M); + map pointIdBwdMap; + + vector 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 camIdFwdMap(N,-1); + map camIdBwdMap; + + vector 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 measurements; + vector correspondingView; + vector 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 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; +} diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Apps/bundle_varying.cpp b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Apps/bundle_varying.cpp new file mode 100644 index 0000000..2e53c0c --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Apps/bundle_varying.cpp @@ -0,0 +1,233 @@ +// Bundle adjustment application for datasets captured with different cameras (varying intrinsics). + +#include "Math/v3d_linear.h" +#include "Geometry/v3d_metricbundle.h" + +#include +#include +#include + +using namespace V3D; +using namespace std; + +namespace +{ + + inline void + showErrorStatistics(vector const& origFocalLengths, + vector const& distortions, + vector const& cams, + vector const& Xs, + vector const& measurements, + vector const& correspondingView, + vector 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(distortions[i], Xs[j]); + + double const f0 = origFocalLengths[i]; + double reprojectionError = norm_L2(f0 * (p - measurements[k])); + meanReprojectionError += reprojectionError; + } + cout << "mean reprojection error (in pixels): " << meanReprojectionError/K << endl; + } + +} // end namespace <> + +int +main(int argc, char * argv[]) +{ + if (argc != 3) + { + cerr << "Usage: " << argv[0] << " " << endl; + cout << " is one of metric, focal, prinipal, 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; + + vector distortions(N); + vector origFocalLengths(N); + vector cams(N); + + double avgFocalLength = 0.0; + + for (int i = 0; i < N; ++i) + { + Matrix3x3d KMat; + makeIdentityMatrix(KMat); + is >> KMat[0][0] >> KMat[0][1] >> KMat[0][2] >> KMat[1][1] >> KMat[1][2] + >> distortions[i].k1 >> distortions[i].k2 >> distortions[i].p1 >> distortions[i].p2; + + //cout << "K = "; displayMatrix(KMat); + + double const f0 = KMat[0][0]; + Matrix3x3d Knorm = KMat; + // Normalize the intrinsic to have unit focal length. + scaleMatrixIP(1.0/f0, Knorm); + Knorm[2][2] = 1.0; + + origFocalLengths[i] = f0; + cams[i].setIntrinsic(Knorm); + avgFocalLength += f0; + } // end for (i) + + avgFocalLength /= N; + cout << "mean focal length = " << avgFocalLength << endl; + + vector pointIdFwdMap(M); + map pointIdBwdMap; + + vector 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 camIdFwdMap(N); + map camIdBwdMap; + + 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)); + +#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 measurements; + vector correspondingView; + vector 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()) + { + int const i = (*camIdBwdMap.find(view)).second; + double const f0 = origFocalLengths[i]; + // 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(origFocalLengths, distortions, cams, Xs, + measurements, correspondingView, correspondingPoint); + + V3D::optimizerVerbosenessLevel = 1; + double const inlierThreshold = 2.0 / avgFocalLength; + //double const inlierThreshold = 1000.0; + + Matrix3x3d K0 = cams[0].getIntrinsic(); + //cout << "K0 = "; displayMatrix(K0); + + VaryingInternalsMetricBundleOptimizer opt(mode, inlierThreshold, distortions, cams, Xs, + measurements, correspondingView, correspondingPoint); + //opt.setNonvaryingCounts(0, Xs.size(), 0); + opt.maxIterations = 200; + opt.minimize(); + cout << "optimizer status = " << opt.status << endl; + cout << "currentIteration = " << opt.currentIteration << endl; + + showErrorStatistics(origFocalLengths, distortions, cams, Xs, + measurements, correspondingView, correspondingPoint); + + ofstream os("refined.txt"); + + os << M << " " << N << " " << K << endl; + + for (int i = 0; i < N; ++i) + { + Matrix3x3d Knew = cams[i].getIntrinsic(); + scaleMatrixIP(origFocalLengths[i], Knew); + Knew[2][2] = 1.0; + os << Knew[0][0] << " " << Knew[0][1] << " " << Knew[0][2] << " " << Knew[1][1] << " " << Knew[1][2] << " " + << distortions[i].k1 << " " << distortions[i].k2 << " " + << distortions[i].p1 << " " << distortions[i].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) + { + 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; + } + + return 0; +} diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Apps/bundle_varying_nonlinlsq.cpp b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Apps/bundle_varying_nonlinlsq.cpp new file mode 100644 index 0000000..4a714a1 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Apps/bundle_varying_nonlinlsq.cpp @@ -0,0 +1,590 @@ +// Bundle adjustment application for datasets captured with different cameras (varying intrinsics). + +#include "Base/v3d_vrmlio.h" +#include "Math/v3d_linear.h" +#include "Math/v3d_nonlinlsq.h" +#include "Geometry/v3d_mviewutilities.h" +#include "Geometry/v3d_distortion.h" +#include "Geometry/v3d_metricbundle.h" + +#include +#include +#include + +using namespace V3D; +using namespace std; + +namespace +{ + +#define CAMERA_PARAM_TYPE 0 +#define POINT_PARAM_TYPE 1 + + typedef V3D::InlineMatrix Matrix2x6d; + typedef V3D::InlineMatrix Matrix3x6d; + + struct VaryingIntrinsicBundleCostFunction : public NLSQ_CostFunction + { + static int extParamDimensionFromMode(int mode) + { + switch (mode) + { + case FULL_BUNDLE_NO_ROTATIONS: + case FULL_BUNDLE_METRIC: return 0; + case FULL_BUNDLE_FOCAL_LENGTH: return 1; + case FULL_BUNDLE_FOCAL_LENGTH_PP: return 3; + case FULL_BUNDLE_RADIAL: return 5; + case FULL_BUNDLE_RADIAL_TANGENTIAL: return 7; + } + return 0; + } + + VaryingIntrinsicBundleCostFunction(int const mode, + std::vector const& usedParamTypes, + double inlierThreshold, + std::vector const& distortions, + vector const& cams, + vector& Xs, + vector const& measurements, + Matrix const& correspondingParams) + : NLSQ_CostFunction(usedParamTypes, correspondingParams, 2), + _cams(cams), _distortions(distortions), _Xs(Xs), _measurements(measurements), + _inlierThreshold(inlierThreshold), _mode(mode) + { + assert(usedParamTypes.size() == 2); + } + + Vector2d projectPoint(Vector3d const& X, int i) const + { + return _cams[i].projectPoint(_distortions[i], X); + } + + virtual void evalResidual(int const k, Vector& e) const + { + unsigned view = _correspondingParams[k][0]; + unsigned point = _correspondingParams[k][1]; + + Vector3d const& X = _Xs[point]; + + Vector2d const q = this->projectPoint(_Xs[point], view); + e[0] = q[0] - _measurements[k][0]; + e[1] = q[1] - _measurements[k][1]; + } + + virtual double getWeight(Vector const& r) const + { + double const e = norm_L2(r); + return (e < _inlierThreshold) ? 1.0 : sqrt(_inlierThreshold / e); + } + + virtual void fillJacobian(int const whichParam, int const paramIx, int const k, Matrix& Jdst) const + { + int const view = _correspondingParams[k][0]; + int const point = _correspondingParams[k][1]; + + double const focalLength = _cams[view].getFocalLength(); + double const aspectRatio = _cams[view].getAspectRatio(); + + Vector3d const XX = _cams[view].transformPointIntoCameraSpace(_Xs[point]); + Vector2d xu; // undistorted image point + xu[0] = XX[0] / XX[2]; + xu[1] = XX[1] / XX[2]; + + Vector2d const xd = _distortions[view](xu); // distorted image point + + Matrix2x2d dp_dxd; + dp_dxd[0][0] = focalLength; dp_dxd[0][1] = 0; + dp_dxd[1][0] = 0; dp_dxd[1][1] = aspectRatio * focalLength; + + // First, lets do the derivative wrt the structure and motion parameters. + Matrix2x3d dxu_dXX; + dxu_dXX[0][0] = 1.0f / XX[2]; dxu_dXX[0][1] = 0; dxu_dXX[0][2] = -XX[0]/(XX[2]*XX[2]); + dxu_dXX[1][0] = 0; dxu_dXX[1][1] = 1.0f / XX[2]; dxu_dXX[1][2] = -XX[1]/(XX[2]*XX[2]); + + Matrix2x2d dxd_dxu = _distortions[view].derivativeWrtUndistortedPoint(xu); + + Matrix2x2d dp_dxu = dp_dxd * dxd_dxu; + Matrix2x3d dp_dXX = dp_dxu * dxu_dXX; + + if (whichParam == 1) + { + Matrix3x3d dXX_dX; + // The derivative of Rx+T wrt x is just R. + copyMatrix(_cams[view].getRotation(), dXX_dX); + + // Derivative w.r.t. 3D point + multiply_A_B(dp_dXX, dXX_dX, Jdst); + return; + } + + Matrix3x6d dXX_dRT; + // See Frank Dellaerts bundle adjustment tutorial. + // d(dR * R0 * X + t)/d omega = -[R0 * X]_x + Matrix3x3d J; + makeCrossProductMatrix(XX - _cams[view].getTranslation(), J); + scaleMatrixIP(-1.0, J); + + // Now the transformation from world coords into camera space is xx = Rx + T + // Hence the derivative of x wrt. T is just the identity matrix. + makeIdentityMatrix(dXX_dRT); + copyMatrixSlice(J, 0, 0, 3, 3, dXX_dRT, 0, 3); + + if (_mode == FULL_BUNDLE_NO_ROTATIONS) + { + dXX_dRT[0][0] = dXX_dRT[0][1] = dXX_dRT[0][2] = 0.0; + dXX_dRT[1][0] = dXX_dRT[1][1] = dXX_dRT[1][2] = 0.0; + dXX_dRT[2][0] = dXX_dRT[2][1] = dXX_dRT[2][2] = 0.0; + } + + Matrix2x6d dp_dRT; + + multiply_A_B(dp_dXX, dXX_dRT, dp_dRT); + copyMatrixSlice(dp_dRT, 0, 0, 2, 6, Jdst, 0, 0); + + switch (_mode) + { + case FULL_BUNDLE_RADIAL_TANGENTIAL: + { + Matrix2x2d dxd_dp1p2 = _distortions[view].derivativeWrtTangentialParameters(xu); + Matrix2x2d d_dp1p2 = dp_dxd * dxd_dp1p2; + copyMatrixSlice(d_dp1p2, 0, 0, 2, 2, Jdst, 0, 11); + // No break here! + } + case FULL_BUNDLE_RADIAL: + { + Matrix2x2d dxd_dk1k2 = _distortions[view].derivativeWrtRadialParameters(xu); + Matrix2x2d d_dk1k2 = dp_dxd * dxd_dk1k2; + copyMatrixSlice(d_dk1k2, 0, 0, 2, 2, Jdst, 0, 9); + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH_PP: + { + Jdst[0][7] = 1; Jdst[0][8] = 0; + Jdst[1][7] = 0; Jdst[1][8] = 1; + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH: + { + Jdst[0][6] = xd[0]; + Jdst[1][6] = xd[1]; + } + case FULL_BUNDLE_METRIC: + { + } + } // end switch + } // end fillJacobian() + + protected: + vector const& _cams; + vector const& _distortions; + vector const& _Xs; + + double _inlierThreshold; + vector const& _measurements; + int const _mode; + }; // end struct VaryingIntrinsicBundleCostFunction + + struct VaryingIntrinsicBundleOptimizer : public NLSQ_LM_Optimizer + { + typedef NLSQ_LM_Optimizer Base; + + VaryingIntrinsicBundleOptimizer(int const mode, NLSQ_ParamDesc const& paramDesc, + std::vector const& costFunctions, + std::vector& distortions, + vector& cams, + vector& Xs) + : Base(paramDesc, costFunctions), + _mode(mode), _cams(cams), _Xs(Xs), _distortions(distortions), + _savedTranslations(cams.size()), _savedRotations(cams.size()), _savedKs(cams.size()), _savedDistortions(cams.size()), + _savedXs(Xs.size()), _cachedParamLength(0.0) + { + // Since we assume that BA does not alter the inputs too much, + // we compute the overall length of the parameter vector in advance + // and return that value as the result of getParameterLength(). + for (int i = 0; i < _cams.size(); ++i) + { + _cachedParamLength += sqrNorm_L2(_cams[i].getTranslation()); + _cachedParamLength += 3.0; // Assume eye(3) for R. + } + for (int j = 0; j < _Xs.size(); ++j) + _cachedParamLength += sqrNorm_L2(_Xs[j]); + + _cachedParamLength = sqrt(_cachedParamLength); + } + + virtual double getParameterLength() const + { + return _cachedParamLength; + } + + virtual void updateParameters(int const paramType, VectorArrayAdapter const& delta) + { + switch (paramType) + { + case CAMERA_PARAM_TYPE: + { + Vector3d T, omega; + Matrix3x3d R0, dR, K; + + for (int i = 0; i < _cams.size(); ++i) + { + T = _cams[i].getTranslation(); + T[0] += delta[i][0]; T[1] += delta[i][1]; T[2] += delta[i][2]; + _cams[i].setTranslation(T); + + // Create incremental rotation using Rodriguez formula. + R0 = _cams[i].getRotation(); + omega[0] = delta[i][3]; omega[1] = delta[i][4]; omega[2] = delta[i][5]; + createRotationMatrixRodrigues(omega, dR); + _cams[i].setRotation(dR * R0); + + K = _cams[i].getIntrinsic(); + + switch (_mode) + { + case FULL_BUNDLE_RADIAL_TANGENTIAL: + { + _distortions[i].p1 += delta[i][11]; + _distortions[i].p2 += delta[i][12]; + // No break here! + } + case FULL_BUNDLE_RADIAL: + { + _distortions[i].k1 += delta[i][9]; + _distortions[i].k2 += delta[i][10]; + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH_PP: + { + K[0][2] += delta[i][7]; + K[1][2] += delta[i][8]; + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH: + { + double const ar = K[1][1] / K[0][0]; + K[0][0] += delta[i][6]; + K[1][1] = ar * K[0][0]; + } + case FULL_BUNDLE_METRIC: + { + } + } // end switch + _cams[i].setIntrinsic(K); + } + break; + } + case POINT_PARAM_TYPE: + { + for (int j = 0; j < _Xs.size(); ++j) + { + _Xs[j][0] += delta[j][0]; + _Xs[j][1] += delta[j][1]; + _Xs[j][2] += delta[j][2]; + } + break; + } + default: + assert(false); + } // end switch (paramType) + } // end updateParametersA() + + virtual void saveAllParameters() + { + for (int i = 0; i < _cams.size(); ++i) + { + _savedTranslations[i] = _cams[i].getTranslation(); + _savedRotations[i] = _cams[i].getRotation(); + _savedKs[i] = _cams[i].getIntrinsic(); + _savedDistortions[i] = _distortions[i]; + } + _savedXs = _Xs; + } + + virtual void restoreAllParameters() + { + for (int i = 0; i < _cams.size(); ++i) + { + _cams[i].setTranslation(_savedTranslations[i]); + _cams[i].setRotation(_savedRotations[i]); + _cams[i].setIntrinsic(_savedKs[i]); + _distortions[i] = _savedDistortions[i]; + } + _Xs = _savedXs; + } + + protected: + int _mode; + + vector& _cams; + vector& _Xs; + vector& _distortions; + + vector _savedTranslations; + vector _savedRotations; + vector _savedKs; + vector _savedXs; + vector _savedDistortions; + + double _cachedParamLength; + }; // end struct VaryingIntrinsicBundleOptimizer + + inline void + showErrorStatistics(vector const& distortions, + vector const& cams, + vector const& Xs, + vector const& measurements, + vector const& correspondingView, + vector 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(distortions[i], Xs[j]); + + double const f0 = 1.0; //origFocalLengths[i]; + double reprojectionError = norm_L2(f0 * (p - measurements[k])); + meanReprojectionError += reprojectionError; + } + cout << "mean reprojection error (in pixels): " << meanReprojectionError/K << endl; + } + +} // end namespace <> + +int +main(int argc, char * argv[]) +{ + if (argc != 3) + { + cerr << "Usage: " << argv[0] << " " << endl; + cout << " is one of metric, focal, prinipal, 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; + + vector distortions(N); + vector origFocalLengths(N); + vector cams(N); + + double avgFocalLength = 0.0; + + for (int i = 0; i < N; ++i) + { + Matrix3x3d KMat; + makeIdentityMatrix(KMat); + is >> KMat[0][0] >> KMat[0][1] >> KMat[0][2] >> KMat[1][1] >> KMat[1][2] + >> distortions[i].k1 >> distortions[i].k2 >> distortions[i].p1 >> distortions[i].p2; + + //cout << "K = "; displayMatrix(KMat); + + double const f0 = KMat[0][0]; + Matrix3x3d Knorm = KMat; + // Normalize the intrinsic to have unit focal length. + scaleMatrixIP(1.0/f0, Knorm); + Knorm[2][2] = 1.0; + + origFocalLengths[i] = f0; + cams[i].setIntrinsic(Knorm); + avgFocalLength += f0; + } // end for (i) + + avgFocalLength /= N; + cout << "mean focal length = " << avgFocalLength << endl; + + vector pointIdFwdMap(M); + map pointIdBwdMap; + + vector 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 camIdFwdMap(N); + map camIdBwdMap; + + 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].setRotation(R); + cams[i].setTranslation(T); + } + cout << "Read the cameras." << endl; + + vector measurements; + vector correspondingView; + vector 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()) + { + int const i = (*camIdBwdMap.find(view)).second; + double const f0 = origFocalLengths[i]; + // 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(distortions, cams, Xs, measurements, correspondingView, correspondingPoint); + + V3D::optimizerVerbosenessLevel = 1; + double const inlierThreshold = 2.0 / avgFocalLength; + cout << "inlierThreshold = " << inlierThreshold << endl; + + { + NLSQ_ParamDesc paramDesc; + paramDesc.nParamTypes = 2; + paramDesc.dimension[CAMERA_PARAM_TYPE] = 6 + VaryingIntrinsicBundleCostFunction::extParamDimensionFromMode(mode); + paramDesc.dimension[POINT_PARAM_TYPE] = 3; + + paramDesc.count[CAMERA_PARAM_TYPE] = cams.size(); + paramDesc.count[POINT_PARAM_TYPE] = Xs.size(); + + vector usedParamTypes; + usedParamTypes.push_back(CAMERA_PARAM_TYPE); + usedParamTypes.push_back(POINT_PARAM_TYPE); + + Matrix correspondingParams(measurements.size(), 2); + for (int k = 0; k < correspondingParams.num_rows(); ++k) + { + correspondingParams[k][0] = correspondingView[k]; + correspondingParams[k][1] = correspondingPoint[k]; + } + + VaryingIntrinsicBundleCostFunction costFun(mode, usedParamTypes, inlierThreshold, distortions, cams, + Xs, measurements, correspondingParams); + vector costFunctions; + costFunctions.push_back(&costFun); + + VaryingIntrinsicBundleOptimizer opt(mode, paramDesc, costFunctions, distortions, cams, Xs); + opt.updateThreshold = 1e-8; + + opt.maxIterations = 200; + opt.minimize(); + cout << "optimizer status = " << opt.status << endl; + } // end scope + + showErrorStatistics(distortions, cams, Xs, measurements, correspondingView, correspondingPoint); + + { + ofstream os("refined-nonlinlsq.txt"); + + os << M << " " << N << " " << K << endl; + + for (int i = 0; i < N; ++i) + { + Matrix3x3d Knew = cams[i].getIntrinsic(); + scaleMatrixIP(origFocalLengths[i], Knew); + Knew[2][2] = 1.0; + os << Knew[0][0] << " " << Knew[0][1] << " " << Knew[0][2] << " " << Knew[1][1] << " " << Knew[1][2] << " " + << distortions[i].k1 << " " << distortions[i].k2 << " " + << distortions[i].p1 << " " << distortions[i].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) + { + 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; + } + } // end scope + + vector norms(Xs.size()); + + for (size_t i = 0; i < Xs.size(); ++i) + { + Vector3d& X = Xs[i]; + norms[i] = norm_L2(X); + } + std::sort(norms.begin(), norms.end()); + float distThr = norms[int(norms.size() * 0.9f)]; + cout << "90% quantile distance: " << distThr << endl; + + for (size_t i = 0; i < Xs.size(); ++i) + { + Vector3d& X = Xs[i]; + if (norm_L2(X) > 3*distThr) makeZeroVector(X); + } + + for (int i = 0; i < cams.size(); ++i) + { + cout << "cam[" << i << "]: K = "; displayMatrix(cams[i].getIntrinsic()); + cout << " distortion: k1 = " << distortions[i].k1 << " k2 = " << distortions[i].k2 << endl; + } // end for (i) + + writePointsToVRML(Xs, "after-ba-nonlinlsq.wrl", false); + + return 0; +} diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Base/v3d_exception.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Base/v3d_exception.h new file mode 100644 index 0000000..e125c6e --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Base/v3d_exception.h @@ -0,0 +1,64 @@ +// -*- C++ -*- + +#ifndef V3D_EXCEPTION_H +#define V3D_EXCEPTION_H + +#include +#include +#include +#include +#include + +#define verify(condition, message) do \ + { \ + if (!(condition)) { \ + std::cout << "VERIFY FAILED: " << (message) << "\n" \ + << " " << __FILE__ << ", " << __LINE__ << "\n"; \ + assert(false); \ + exit(0); \ + } \ + } while(false); + +#define throwV3DErrorHere(reason) throw V3D::Exception(__FILE__, __LINE__, reason) + +namespace V3D +{ + + struct Exception : public std::exception + { + Exception(char const * reason) + : _reason(reason) + { } + + Exception(std::string const& reason) + : _reason(reason) + { } + + Exception(char const * file, int line, char const * reason) + { + std::ostringstream os; + os << file << ":" << line << ": " << reason; + _reason = os.str(); + } + + Exception(char const * file, int line, std::string const& reason) + { + std::ostringstream os; + os << file << ":" << line << ": " << reason; + _reason = os.str(); + } + + virtual ~Exception() throw() { } + + virtual const char * what() const throw() + { + return _reason.c_str(); + } + + protected: + std::string _reason; + }; // end struct Exception + +} // end namespace V3D + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Base/v3d_serialization.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Base/v3d_serialization.h new file mode 100644 index 0000000..2c9d6a7 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Base/v3d_serialization.h @@ -0,0 +1,1109 @@ +// -*- C++ -*- + +#ifndef V3D_SERIALIZATION_H +#define V3D_SERIALIZATION_H + +#include +#include +#include +#include +#include +#include +#include + +#include "Base/v3d_exception.h" + +//! Adds load and save routines to the serializable struct. +#define V3D_DEFINE_LOAD_SAVE(T) \ + template \ + void save(Ar& ar) const { T& self = const_cast(*this); self.serialize(ar); } \ + template \ + void load(Ar& ar) { this->serialize(ar); } \ + template \ + void serialize(Ar& ar,const unsigned int& file_version) const { \ + T& self = const_cast(*this); self.serialize(ar); \ + } + +//! Implements \c << and \c >> operators for streams for serializable structs. +#define V3D_DEFINE_IOSTREAM_OPS(T) \ + inline std::istream& operator>>(std::istream& is, T& v) \ + { \ + return V3D::loadFromIStream(is, v); \ + } \ + inline std::ostream& operator<<(std::ostream& os, T const& v) \ + { \ + return V3D::saveToOStream(os, v); \ + } + +//! Implements \c << and \c >> operators for streams for templated serializable structs. +#define V3D_DEFINE_TEMPLATE_IOSTREAM_OPS(T) \ + template \ + inline std::istream& operator>>(std::istream& is, T& v) \ + { \ + return V3D::loadFromIStream(is, v); \ + } \ + template \ + inline std::ostream& operator<<(std::ostream& os, T const& v) \ + { \ + return V3D::saveToOStream(os, v); \ + } + +namespace V3D +{ + + template + struct OArchiveProtocol + { + Archive * archive() + { + return static_cast(this); + } + + static bool isLoading() { return false; } + static bool isSaving() { return true; } + + template + Archive& operator<<(T const& val) + { + this->archive()->save(val); + return *this->archive(); + } + + template + Archive& operator&(T const& val) + { + return (*this) << val; + } + + void serializeBlob(void * address, size_t count) + { + this->archive()->serializeBlob(address, count); + } + + //! Do not use whitespace in \param tag! + void tag(char const * tag) + { + this->archive()->saveTag(tag); + } + + void enterScope() { this->archive()->saveTag("{"); } + void leaveScope() { this->archive()->saveTag("}"); } + void endl() { this->archive()->endl(); } + }; + + template + struct IArchiveProtocol + { + Archive * archive() + { + return static_cast(this); + } + + static bool isLoading() { return true; } + static bool isSaving() { return false; } + + template + Archive& operator>>(T& val) + { + this->archive()->load(val); + return *this->archive(); + } + + template + Archive& operator&(T& val) + { + return (*this) >> val; + } + + void serializeBlob(void * address, size_t count) + { + this->archive()->serializeBlob(address, count); + } + + //! Do not use whitespace in \param tag! + void tag(char const * tag) + { + std::string stag(tag); + std::string s; + this->archive()->loadTag(s); + if (stag != s) + throwV3DErrorHere(std::string("Tag mismatch <" + s + "> instead of <") + stag + std::string(">")); + } + + void enterScope() + { + std::string s; + this->archive()->loadTag(s); + if (s != "{") throwV3DErrorHere("Bracket mismatch <{>"); + } + + void leaveScope() + { + std::string s; + this->archive()->loadTag(s); + if (s != "}") throwV3DErrorHere("Bracket mismatch <}>"); + } + + void endl() { } + }; + + template + struct SerializationScope + { + SerializationScope(Archive& ar) + : _ar(ar) + { + ar.enterScope(); + } + + SerializationScope(Archive& ar, char const * tag) + : _ar(ar) + { + ar.tag(tag); + ar.enterScope(); + } + + ~SerializationScope() + { + _ar.leaveScope(); + } + + protected: + Archive& _ar; + }; // end struct SerializationScope + +//---------------------------------------------------------------------- + + struct TextOStreamArchive : public OArchiveProtocol + { + private: + typedef OArchiveProtocol Base; + + public: + TextOStreamArchive(std::ostream& os) + : Base(), _os(os), + _indentation(0) + { } + + template void save(T const& v) { v.T::save(*this); } + + void save(bool val) { this->put((int)val); } + void save(unsigned int val) { this->put(val); } + void save(int val) { this->put(val); } + void save(long val) { this->put(val); } + void save(unsigned long val) { this->put(val); } + void save(short int val) { this->put(val); } + void save(unsigned short int val) { this->put(val); } + void save(float val) { this->put(val); } + void save(double val) { this->put(val); } + void save(char val) { this->put(int(val)); } + void save(unsigned char val) { this->put((unsigned int)val); } + + void save(char const * str) + { + unsigned int len = std::strlen(str); + this->save(len); + _os.write(str, len); + _os << " "; + } + + void save(std::string const& str) + { + unsigned int len = str.length(); + this->save(len); + _os.write(str.c_str(), len); + _os << " "; + } + + template + void save(std::pair const& v) { + save(const_cast(v.first)); + save(const_cast(v.second)); + } + + void serializeBlob(void * address, size_t count) + { + char const * buffer = static_cast(address); + _os.write(buffer, count); + } + + void saveTag(char const * tag) { this->put(tag); } + + void enterScope() + { + _indentation += 2; + Base::enterScope(); + } + + void leaveScope() + { + _indentation -= 2; + Base::leaveScope(); + this->endl(); + } + + void endl() + { + _os << std::endl; + this->indent(); + } + + protected: + template + void put(T const& v) + { + _os << v << " "; + } + + void indent() + { + for (int i = 0; i < _indentation; ++i) _os << " "; + } + + std::ostream& _os; + int _indentation; + }; + + struct TextIStreamArchive : public IArchiveProtocol + { + private: + typedef IArchiveProtocol Base; + + public: + TextIStreamArchive(std::istream& is) + : Base(), _is(is) + { } + + template void load(T& v) { v.T::load(*this); } + + void load(bool& val) { int v; get(v); val = (v != 0) ? true : false; } + void load(unsigned int& val) { get(val); } + void load(int& val) { get(val); } + void load(long& val) { get(val); } + void load(unsigned long& val) { get(val); } + void load(short int& val) { get(val); } + void load(unsigned short int& val) { get(val); } + void load(float& val) { get(val); } + void load(double& val) { get(val); } + void load(char& val) { int v; get(v); val = v; } + void load(unsigned char& val) { unsigned v; get(v); val = v; } + + void load(char * str) + { + unsigned int len; + this->load(len); + _is.ignore(); // Ignore the extra blank after len + _is.read(str, len); + str[len] = 0; + } + + void load(std::string& str) + { + unsigned int len; + this->load(len); + _is.ignore(); // Ignore the extra blank after len + std::vector buf(len+1); + _is.read(&buf[0], len); + buf[len] = 0; + str = &buf[0]; + } + + template + void load(std::pair & v) { + load(v.first); + load(v.second); + } + + void serializeBlob(void * address, size_t count) + { + char * buffer = static_cast(address); + _is.read(buffer, count); + } + + void loadTag(std::string& tag) + { + _is >> std::ws >> tag; + } + + void enterScope() { Base::enterScope(); } + void leaveScope() { Base::leaveScope(); } + + protected: + template + void get(T& v) + { + _is >> v; + } + + std::istream& _is; + }; + +//---------------------------------------------------------------------- + + //! Output archive using a binary stream to write. + /*! Integer values (short, int, long) are stored as 32 bit entities. + * Hence binary serialization is not dependent on sizeof(int) etc. + * Floating point types are directly written, but all relevant platforms use + * IEEE fp format anyway. + * Chars and bools are written as 8 bit entities. + * Do not forget the ios::binary attribute for stream opening! + */ + struct BinaryOStreamArchive : public OArchiveProtocol + { + private: + typedef OArchiveProtocol Base; + + public: + BinaryOStreamArchive(std::ostream& os) + : Base(),_os(os) + { } + + template void save(T const& v) { v.save(*this); } + + void save(bool val) { this->put_byte(val); } + void save(unsigned int val) { this->put_uint32(val); } + void save(int val) { this->put_int32(val); } + void save(long val) { this->put_int32(val); } + void save(unsigned long val) { this->put_uint32(val); } + void save(short int val) { this->put_int32(val); } + void save(unsigned short int val) { this->put_uint32(val); } + void save(float val) { this->put_blob(val); } + void save(double val) { this->put_blob(val); } + void save(char val) { this->put_byte(val); } + void save(unsigned char val) { this->put_byte(val); } + + void save(char const * str) + { + //assert(static_cast(std::strlen(str)) < std::numeric_limits::max()); + unsigned int len = static_cast(std::strlen(str)); + save(len); + _os.write(str, len); + } + + void save(std::string const& str) + { + //assert(static_cast(str.length()) < std::numeric_limits::max()); + unsigned int len = static_cast(str.length()); + save(len); + _os.write(str.c_str(), len); + } + template + void save(std::pair const& v) { + save(const_cast(v.first)); + save(const_cast(v.second)); + } + + void serializeBlob(void * address, size_t count) + { + char const * buffer = static_cast(address); + _os.write(buffer, count); + } + + void saveTag(char const *) { } + void enterScope() { } + void leaveScope() { } + void endl() { } + + protected: + template + void put_byte(T v) + { + char val = v; + _os.write(&val, 1); + } + + void put_int32(signed long v) + { + unsigned char buf[4]; + buf[0] = static_cast(v & 0xff); + buf[1] = static_cast((v >> 8) & 0xff); + buf[2] = static_cast((v >> 16) & 0xff); + buf[3] = static_cast((v >> 24) & 0xff); + _os.write((char *)buf, 4); + } + + void put_uint32(unsigned long v) + { + unsigned char buf[4]; + buf[0] = static_cast(v & 0xff); + buf[1] = static_cast((v >> 8) & 0xff); + buf[2] = static_cast((v >> 16) & 0xff); + buf[3] = static_cast((v >> 24) & 0xff); + _os.write((char *)buf, 4); + } + + template + void put_blob(T v) + { + _os.write((char *)&v, sizeof(v)); + } + + std::ostream& _os; + }; + + struct BinaryIStreamArchive : public IArchiveProtocol + { + private: + typedef IArchiveProtocol Base; + + public: + BinaryIStreamArchive(std::istream& is) + : Base(), _is(is) + { } + + template void load(T& v) { v.load(*this); } + + void load(bool& val) { get_uchar(val); } + void load(unsigned int& val) { getUnsigned(val); } + void load(int& val) { getSigned(val); } + void load(long& val) { getSigned(val); } + void load(unsigned long& val) { getUnsigned(val); } + void load(short int& val) { getSigned(val); } + void load(unsigned short int& val) { getUnsigned(val); } + void load(float& val) { getGeneric(val); } + void load(double& val) { getGeneric(val); } + void load(char& val) { get_schar(val); } + void load(unsigned char& val) { get_uchar(val); } + + void load(char * str) + { + unsigned int len; + load(len); + _is.read(str, len); + str[len] = 0; + } + + void load(std::string& str) + { + unsigned int len; + this->load(len); + std::vector buf(len+1); + _is.read(&buf[0], len); + buf[len] = 0; + str = &buf[0]; + } + + template + void load(std::pair & v) { + load(v.first); + load(v.second); + } + + void serializeBlob(void * address, size_t count) + { + char * buffer = static_cast(address); + _is.read(buffer, count); + } + + void loadTag(std::string& /*tag*/) { } + void tag(std::string const& /*tag*/) { } + void enterScope() { } + void leaveScope() { } + + void skip(unsigned int nBytes) { _is.ignore(nBytes); } + + protected: + template + void get_uchar(T& v) + { + unsigned char val; + _is.read((char *)&val, 1); + v = val; + } + + template + void get_schar(T& v) + { + char val; + _is.read((char *)&val, 1); + v = val; + } + + template + void getSigned(T& v) + { + signed long val; + get_int32(val); + v = val; + } + + template + void getUnsigned(T& v) + { + unsigned long val; + get_uint32(val); + v = val; + } + + void get_uint32(unsigned long& v) + { + unsigned char buf[4]; + _is.read((char *)buf, 4); + v = buf[0] + (buf[1] << 8) + (buf[2] << 16) + (buf[3] << 24); + } + + void get_int32(signed long& v) + { + unsigned char buf[4]; + _is.read((char *)buf, 4); + v = buf[0] + (buf[1] << 8) + (buf[2] << 16); + // The following is somewhat magic, interpret the most significant byte as signed char. + v += (signed char)(buf[3]) << 24; + } + + template + void getGeneric(T& v) + { + _is.read((char *)&v, sizeof(v)); + } + + std::istream& _is; + }; + +//---------------------------------------------------------------------- + + struct BinaryArchiveSizeAccumulator : public OArchiveProtocol + { + BinaryArchiveSizeAccumulator() + : _byteSize(0) + { } + + template void save(T const& v) { v.save(*this); } + + void save(bool) { _byteSize += 1; } + void save(unsigned int) { _byteSize += 4; } + void save(int) { _byteSize += 4; } + void save(long) { _byteSize += 4; } + void save(unsigned long) { _byteSize += 4; } + void save(short int) { _byteSize += 4; } + void save(unsigned short int) { _byteSize += 4; } + void save(float) { _byteSize += 4; } + void save(double) { _byteSize += 8; } + void save(char) { _byteSize += 1; } + void save(unsigned char) { _byteSize += 1; } + + void save(char const * str) + { + unsigned int len = std::strlen(str); + this->save(len); + _byteSize += len; + } + + void save(std::string const& str) + { + unsigned int len = str.length(); + this->save(len); + _byteSize += len; + } + + template + void save(std::pair const& v) { + save(const_cast(v.first)); + save(const_cast(v.second)); + } + + void serializeBlob(void *, size_t count) + { + _byteSize += count; + } + + void tag(std::string const&) { } + void enterScope() { } + void leaveScope() { } + void endl() { } + + unsigned int byteSize() const { return _byteSize; } + + protected: + unsigned int _byteSize; + }; // end struct BinaryArchiveSizeAccumulator + +//---------------------------------------------------------------------- + + // Serialize to a blob (contiguous memory) + struct BlobOArchive : public OArchiveProtocol + { + private: + typedef OArchiveProtocol Base; + + public: + BlobOArchive(int sz = 0) + : Base() + { + if (sz > 0) _blob.reserve(sz); + } + + void clear() { _blob.clear(); } + + unsigned char const * getBlob() const { return &_blob[0]; } + int blobSize() const { return _blob.size(); } + + template void save(T const& v) { v.save(*this); } + + void save(bool val) { this->put_byte(val); } + void save(unsigned int val) { this->put_uint32(val); } + void save(int val) { this->put_int32(val); } + void save(long val) { this->put_int32(val); } + void save(unsigned long val) { this->put_uint32(val); } + void save(short int val) { this->put_int32(val); } + void save(unsigned short int val) { this->put_uint32(val); } + void save(float val) { this->put_blob(val); } + void save(double val) { this->put_blob(val); } + void save(char val) { this->put_byte(val); } + void save(unsigned char val) { this->put_byte(val); } + + void save(char const * str) + { + unsigned int len = static_cast(std::strlen(str)); + this->save(len); + this->serializeBlob(str, len); + } + + void save(std::string const& str) + { + unsigned int len = static_cast(str.length()); + this->save(len); + this->serializeBlob(str.c_str(), len); + } + + template + void save(std::pair const& v) { + save(const_cast(v.first)); + save(const_cast(v.second)); + } + + void serializeBlob(void const * address, size_t count) + { + unsigned char const * buffer = static_cast(address); + for (size_t i = 0; i < count; ++i) + _blob.push_back(buffer[i]); + } + + void saveTag(char const *) { } + void enterScope() { } + void leaveScope() { } + void endl() { } + + protected: + template + void put_byte(T v) + { + unsigned char val = v; + _blob.push_back(val); + } + + void put_int32(signed long v) + { + unsigned char buf[4]; + buf[0] = static_cast(v & 0xff); + buf[1] = static_cast((v >> 8) & 0xff); + buf[2] = static_cast((v >> 16) & 0xff); + buf[3] = static_cast((v >> 24) & 0xff); + _blob.push_back(buf[0]); + _blob.push_back(buf[1]); + _blob.push_back(buf[2]); + _blob.push_back(buf[3]); + } + + void put_uint32(unsigned long v) + { + unsigned char buf[4]; + buf[0] = static_cast(v & 0xff); + buf[1] = static_cast((v >> 8) & 0xff); + buf[2] = static_cast((v >> 16) & 0xff); + buf[3] = static_cast((v >> 24) & 0xff); + _blob.push_back(buf[0]); + _blob.push_back(buf[1]); + _blob.push_back(buf[2]); + _blob.push_back(buf[3]); + } + + template + void put_blob(T v) + { + this->serializeBlob(&v, sizeof(v)); + } + + std::vector _blob; + }; + + struct BlobIArchive : public IArchiveProtocol + { + private: + typedef IArchiveProtocol Base; + + public: + BlobIArchive(unsigned char const * blobStart) + : Base(), _blobPtr(blobStart) + { } + + template void load(T& v) { v.load(*this); } + + void load(bool& val) { get_uchar(val); } + void load(unsigned int& val) { getUnsigned(val); } + void load(int& val) { getSigned(val); } + void load(long& val) { getSigned(val); } + void load(unsigned long& val) { getUnsigned(val); } + void load(short int& val) { getSigned(val); } + void load(unsigned short int& val) { getUnsigned(val); } + void load(float& val) { getGeneric(val); } + void load(double& val) { getGeneric(val); } + void load(char& val) { get_schar(val); } + void load(unsigned char& val) { get_uchar(val); } + + void load(char * str) + { + unsigned int len; + this->load(len); + this->serializeBlob(str, len); + str[len] = 0; + } + + void load(std::string& str) + { + unsigned int len; + this->load(len); + std::vector buf(len+1); + this->serializeBlob(&buf[0], len); + buf[len] = 0; + str = &buf[0]; + } + + template + void load(std::pair & v) { + load(v.first); + load(v.second); + } + + void serializeBlob(void * address, size_t count) + { + unsigned char * buffer = static_cast(address); + for (size_t i = 0; i < count; ++i) + buffer[i] = _blobPtr[i]; + _blobPtr += count; + } + + void loadTag(std::string& /*tag*/) { } + void tag(std::string const& /*tag*/) { } + void enterScope() { } + void leaveScope() { } + + void skip(unsigned int nBytes) { _blobPtr += nBytes; } + + protected: + template + void get_uchar(T& v) + { + v = *_blobPtr++; + } + + template + void get_schar(T& v) + { + v = *_blobPtr++; + } + + template + void getSigned(T& v) + { + signed long val; + get_int32(val); + v = val; + } + + template + void getUnsigned(T& v) + { + unsigned long val; + get_uint32(val); + v = val; + } + + void get_uint32(unsigned long& v) + { + unsigned char const * buf = _blobPtr; + v = buf[0] + (buf[1] << 8) + (buf[2] << 16) + (buf[3] << 24); + _blobPtr += 4; + } + + void get_int32(signed long& v) + { + unsigned char const * buf = _blobPtr; + v = buf[0] + (buf[1] << 8) + (buf[2] << 16); + // The following is somewhat magic, interpret the most significant byte as signed char. + v += (signed char)(buf[3]) << 24; + _blobPtr += 4; + } + + template + void getGeneric(T& v) + { + this->serializeBlob(&v, sizeof(v)); + } + + unsigned char const * _blobPtr; + }; + +//---------------------------------------------------------------------- + + //! Serializes a vector of serializable items. + template + inline void + serializeVector(std::vector& v, Archive& ar) + { + unsigned int sz = v.size(); + ar & sz; + if (ar.isLoading()) v.resize(sz); + SerializationScope s(ar); + for (unsigned i = 0; i < sz; ++i) ar & v[i]; + } + + //! Serializes a vector of bool (needed as it is NOT a normal vector) + template + inline void + serializeVector(std::vector& v, Archive& ar) + { + unsigned int sz = v.size(); + ar & sz; + if (ar.isLoading()) v.resize(sz); + SerializationScope s(ar); + const unsigned int packsize=32; + unsigned int realsz=(sz-1)/packsize+1; + for (unsigned i = 0; i >j)&1)!=0; + } + } + + template + inline void + serializeVector(char const * tag, std::vector& v, Archive& ar) + { + ar.tag(tag); + serializeVector(v, ar); + } + + template + inline void + serializeSet(std::set& v, Archive& ar) + { + unsigned int sz = v.size(); + ar & sz; + + SerializationScope s(ar); + + if (ar.isLoading()) + { + T elem; + for (unsigned i = 0; i < sz; ++i) + { + ar & elem; + v.insert(elem); + } + } + else + { + T elem; + for (typename std::set::iterator p = v.begin(); p != v.end(); ++p) + { + elem = *p; + ar & elem; + } + } // end if + } // end serializeSet() + + template + inline void + serializeMap(std::map& v, Archive& ar) + { + unsigned int sz = v.size(); + ar & sz; + + SerializationScope s(ar); + + if (ar.isLoading()) + { + v.clear(); + Key key; + T elem; + for (unsigned i = 0; i < sz; ++i) + { + ar & key & elem; + v.insert(make_pair(key, elem)); + } + } + else + { + Key key; + for (typename std::map::iterator p = v.begin(); p != v.end(); ++p) + { + key = p->first; + ar & key & p->second; + } + } // end if + } // end serializeMap() + + template + inline void + serializeDataToFile(char const * archiveName, T const& data, bool writeBinary = true) + { + using namespace std; + + int const tagLength = 6; + + if (writeBinary) + { + char const * magicTag = "V3DBIN"; + ofstream os(archiveName, ios::binary); + os.write(magicTag, tagLength); + BinaryOStreamArchive ar(os); + ar & data; + } + else + { + char const * magicTag = "V3DTXT"; + ofstream os(archiveName); + os << magicTag << endl; + TextOStreamArchive ar(os); + ar & data; + } + } // end serializeDataToFile() + + template + inline void + serializeDataFromFile(char const * archiveName, T& data) + { + using namespace std; + + int const tagLength = 6; + + bool isBinary = true; + + { + // Determine archive format from the first 8 chars + char magicTag[tagLength]; + ifstream is(archiveName, ios::binary); + is.read(magicTag, tagLength); + if (strncmp(magicTag, "V3DBIN", tagLength) == 0) + isBinary = true; + else if (strncmp(magicTag, "V3DTXT", tagLength) == 0) + isBinary = false; + else + throwV3DErrorHere("Unknown archive magic tag"); + } + + if (isBinary) + { + ifstream is(archiveName, ios::binary); + is.ignore(tagLength); + BinaryIStreamArchive ar(is); + ar & data; + } + else + { + ifstream is(archiveName); + is.ignore(tagLength); + TextIStreamArchive ar(is); + ar & data; + } + } // end serializeDataFromFile() + + template + inline std::ostream& + saveToOStream(std::ostream& os, T const& v) + { + using namespace std; + TextOStreamArchive ar(os); + ar << v; + return os; + } + + template + inline std::istream& + loadFromIStream(std::istream& is, T& v) + { + using namespace std; + TextIStreamArchive ar(is); + ar >> v; + return is; + } + +//---------------------------------------------------------------------- + + template + struct SerializableVector : public std::vector + { + SerializableVector() + : std::vector() + { } + + SerializableVector(size_t sz) + : std::vector(sz) + { } + SerializableVector(std::vector const & v) + : std::vector(v) + { } + template void serialize(Archive& ar) + { + serializeVector(*this, ar); + } + V3D_DEFINE_LOAD_SAVE(SerializableVector) + }; // end struct SerializableVector + V3D_DEFINE_TEMPLATE_IOSTREAM_OPS(SerializableVector) + + template + struct SerializableSet : public std::set + { + SerializableSet() + : std::set() + { } + + SerializableSet(size_t sz) + : std::set(sz) + { } + SerializableSet(std::set const & v) + : std::set(v) + { } + + template void serialize(Archive& ar) + { + serializeSet(*this, ar); + } + + V3D_DEFINE_LOAD_SAVE(SerializableSet) + }; // end struct SerializableSet + V3D_DEFINE_TEMPLATE_IOSTREAM_OPS(SerializableSet) + + template + struct SerializableMap : public std::map + { + SerializableMap() + : std::map() + { } + + SerializableMap(size_t sz) + : std::map(sz) + { } + SerializableMap(std::map const & v) + : std::map(v) + { } + template void serialize(Archive& ar) + { + serializeMap(*this, ar); + } + + V3D_DEFINE_LOAD_SAVE(SerializableMap) + }; // end struct SerializableSet + //V3D_DEFINE_TEMPLATE_IOSTREAM_OPS(SerializableMap) //Does not work with 2 template names + +} // end namespace V3D + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Base/v3d_vrmlio.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Base/v3d_vrmlio.h new file mode 100644 index 0000000..b521c37 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Base/v3d_vrmlio.h @@ -0,0 +1,344 @@ +#ifndef V3D_VRMLIO_H +#define V3D_VRMLIO_H + +#include "Geometry/v3d_cameramatrix.h" +#include +#include +#include + +namespace V3D +{ + + template + void writeColoredPointsToVRML(std::vector const& points, std::vector const& colors255, + char const * filename, bool append = false) + { + using namespace std; + std::ofstream os; + + os.open(filename, append ? ios::app : ios::out); + os.precision(15); + + if (!append) os <<"#VRML V2.0 utf8" << endl; + os << "Shape {" << endl; + + os << " appearance Appearance {" << endl; + os << " material Material {" << endl; + os << " ambientIntensity 0.1" << endl; + os << " diffuseColor 1.0 0.7 0.2" << endl; + os << " } } " << endl; + os << " geometry PointSet {" << endl; + + // write point coordinates + os << " coord Coordinate {" << endl; + os << " point [" << endl; + for (size_t i = 0; i < points.size(); ++i) + os << points[i][0] <<" " << points[i][1] << " " << points[i][2] << "," << endl; + os << " ]\n } # end points"< + void writeCameraFrustumToVRML(CameraMatrix const& cam, int width, int height, double scale, + U const& color255, const char *filename, bool append) + { + using namespace std; + + ofstream os; + + os.open(filename, append ? ios::app : ios::out); + os.precision(15); + + double r = ((double) color255[0]) / 255.0; + double g = ((double) color255[1]) / 255.0; + double b = ((double) color255[2]) / 255.0; + + if (!append) os <<"#VRML V2.0 utf8" << endl; + os << " Shape {" << endl; + os << " appearance Appearance {" << endl; + os << " material Material { emissiveColor 0.2 0.2 0.2" << endl; + os << " diffuseColor " << r << " " << g << " " << b << endl; + os << " }" << endl; + os << " }" << endl; + + os <<" geometry IndexedFaceSet {" << endl; + os <<" solid FALSE " << endl; + os <<" ccw TRUE " << endl; + os <<" coord Coordinate {" << endl; + os <<" point [" << endl; + + Vector3d const center = cam.cameraCenter(); + Vector3d const ll = cam.unprojectPixel(makeVector2(0.0, double(height)), scale); + Vector3d const lr = cam.unprojectPixel(makeVector2(double(width), double(height)), scale); + Vector3d const ul = cam.unprojectPixel(makeVector2(0.0, 0.0), scale); + Vector3d const ur = cam.unprojectPixel(makeVector2(double(width), 0.0), scale); + + // write point coordinates + os << ll[0] << " " << ll[1] << " " << ll[2] << "," << endl; + os << lr[0] << " " << lr[1] << " " << lr[2] << "," << endl; + os << ul[0] << " " << ul[1] << " " << ul[2] << "," << endl; + os << ur[0] << " " << ur[1] << " " << ur[2] << "," << endl; + os << center[0] << " " << center[1] << " " << center[2] << "," << endl; + + os << " ]\n } "<< endl; + + os << " coordIndex [\n"; + os << " 0,1,2,3,-1" << endl; + os << " 0,1,4,-1" << endl; + os << " 1,3,4,-1" << endl; + os << " 3,2,4,-1" << endl; + os << " 2,0,4,-1\n ]" << endl; + os << " }\n}" << endl; + } // end writeCameraFrustumToVRML() + + template + void writeMatchesToVRML(std::vector const& points, char const * filename, bool append = false) + { + using namespace std; + std::ofstream os; + + os.open(filename, append ? ios::app : ios::out); + os.precision(15); + + os << "#VRML V2.0 utf8" << endl; + os << " Shape {" << endl; + + os << " appearance Appearance {" << endl; + os << " material Material {" << endl; + os << " ambientIntensity 0.1" << endl; + os << " diffuseColor 1.0 0.0 0.0" << endl; + os << " transparency 0.5" << endl; + os << " } } " << endl; + os << " geometry IndexedLineSet {" << endl; + + // write lines between matches coordinates + os <<" coord Coordinate {" << endl; + os <<" point [" << endl; + for (size_t i = 0; i < points.size(); ++i) + os << points[i][0] <<" " << points[i][1] << " " << points[i][2] << "," << endl; + os << " ]\n } # end coord"< + void writePointsToVRML(std::vector const& points, U const & color255,char const * filename, bool append = false) + { + using namespace std; + std::ofstream os; + + os.open(filename, append ? ios::app : ios::out); + os.precision(4);// Precision for color + + os << "#VRML V2.0 utf8" << endl; + os << " Shape {" << endl; + writeAppearanceToVRMLStream(os,color255); + + os << " geometry PointSet {" << endl; + os.precision(15);// Precision for points + + // write point coordinates + os <<" coord Coordinate {" << endl; + os <<" point [" << endl; + for (size_t i = 0; i < points.size(); ++i) + os << points[i][0] <<" " << points[i][1] << " " << points[i][2] << "," << endl; + os << " ]\n } # end coord" << endl; + + os << " } # end geometry" << endl; + os << "} # end shape" << endl; + } + + template + void writePointsToVRML(std::vector const& points, char const * filename, bool append = false) + { + Vector3f color255(makeVector3(255.0,180.0,50.0)); + writePointsToVRML(points, color255, filename, append); + } + + template + void writeCameraFrustumToVRMLStream(ofstream & os,CameraMatrix const& cam, double scale, + U const& color255) + { + using namespace std; + + os.precision(3); + os << " Shape {" << endl; + writeAppearanceToVRMLStream(os,color255); + + os.precision(15); + os <<" geometry IndexedFaceSet {" << endl; + os <<" solid FALSE " << endl; + os <<" ccw TRUE " << endl; + os <<" coord Coordinate {" << endl; + os <<" point [" << endl; + Vector3d const center = cam.cameraCenter(); + Vector3d ll = cam.unprojectPixel(makeVector2(0.0, (double)cam.getHeight()), scale); + Vector3d lr = cam.unprojectPixel(makeVector2((double)cam.getWidth(), (double)cam.getHeight()), scale); + Vector3d ul = cam.unprojectPixel(makeVector2(0.0, 0.0), scale); + Vector3d ur = cam.unprojectPixel(makeVector2((double)cam.getWidth(), 0.0), scale); + Matrix3x3d I;makeIdentityMatrix(I); + if(cam.getIntrinsic()==I ){//If normalized Camera, assume 4/3 AspectRatio for Frustrum + ll = cam.unprojectPixel(makeVector2(-0.5, 0.375), scale); + lr = cam.unprojectPixel(makeVector2( 0.5, 0.375), scale); + ul = cam.unprojectPixel(makeVector2(-0.5, -0.375), scale); + ur = cam.unprojectPixel(makeVector2( 0.5, -0.375), scale); + } + // write point coordinates + os << ll[0] << " " << ll[1] << " " << ll[2] << "," << endl; + os << lr[0] << " " << lr[1] << " " << lr[2] << "," << endl; + os << ul[0] << " " << ul[1] << " " << ul[2] << "," << endl; + os << ur[0] << " " << ur[1] << " " << ur[2] << "," << endl; + os << center[0] << " " << center[1] << " " << center[2] << "," << endl; + + os << " ]\n } "<< endl; + + os << " coordIndex [\n"; + //os << " 0,1,2,3,-1" << endl; + os << " 0,1,4,-1" << endl; + os << " 1,3,4,-1" << endl; + os << " 3,2,4,-1" << endl; + os << " 2,0,4,-1\n ]" << endl; + os << " }\n}" << endl; + } // end writeCameraFrustumToVRML() + template + void writeCameraFrustumToVRML(CameraMatrix const& cam, double scale, + U const& color255, const char *filename, bool append) + { + using namespace std; + ofstream os; + os.open(filename, append ? ios::app : ios::out); + os.precision(3); + if (!append) os <<"#VRML V2.0 utf8" << endl; + writeCameraFrustumToVRMLStream(os,cam, scale, color255); + } // end writeCameraFrustumToVRML() + + template + void writeAppearanceToVRMLStream(ofstream & os,U const& color255) + { + double r = ((double) color255[0]) / 255.0; + double g = ((double) color255[1]) / 255.0; + double b = ((double) color255[2]) / 255.0; + os << " appearance Appearance {" << endl; + os << " material Material { ambientIntensity 0.1 " << endl; + os << " diffuseColor " << r << " " << g << " " << b << endl; + os << " transparency 0.5" << endl; + os << " }" << endl; + os << " }" << endl; + } + + template + void write2DnormalizedVs3DMatchesToVRMLStream(ofstream & os,CameraMatrix const& cam, std::vector > const& points2D,std::vector > const& points3D,double scale, + vector > const& colors255) + { + using namespace std; + os.precision(15); + assert(points3D.size()==points2D.size()); + assert(colors255.size()==points2D.size()); + os << " Shape {" << endl; + Vector3b color255(255,255,255); + writeAppearanceToVRMLStream(os,color255); + os << " geometry IndexedFaceSet {" << endl; + + // write lines between matches coordinates + os <<" coord Coordinate {" << endl; + os <<" point [" << endl; + os << cam.cameraCenter()[0] <<" " << cam.cameraCenter()[1] << " " << cam.cameraCenter()[2] << "," << endl; + for (size_t i = 0; i < points2D.size(); ++i){ + Vector3d p2=cam.unprojectPixel(points2D[i],scale); + os << p2[0] <<" " << p2[1] << " " << p2[2] << "," << endl; + os << p2[0] <<" " << p2[1] << " " << p2[2] << "," << endl; + os << points3D[i][0] <<" " << points3D[i][1] << " " << points3D[i][2] << "," << endl; + } + os << " ]\n } # end coord"< + void write2DnormalizedVs3DMatchesToVRMLStream(ofstream & os,CameraMatrix const& cam, std::vector > const& points2D,std::vector > const& points3D,double scale, + InlineVector const& color255) + { + using namespace std; + os.precision(15); + + os << " Shape {" << endl; + writeAppearanceToVRMLStream(os,color255); + os << " geometry IndexedFaceSet {" << endl; + + // write lines between matches coordinates + os <<" coord Coordinate {" << endl; + os <<" point [" << endl; + os << cam.cameraCenter()[0] <<" " << cam.cameraCenter()[1] << " " << cam.cameraCenter()[2] << "," << endl; + for (size_t i = 0; i < points2D.size(); ++i){ + Vector3d p2=cam.unprojectPixel(points2D[i],scale); + os << p2[0] <<" " << p2[1] << " " << p2[2] << "," << endl; + os << p2[0] <<" " << p2[1] << " " << p2[2] << "," << endl; + os << points3D[i][0] <<" " << points3D[i][1] << " " << points3D[i][2] << "," << endl; + } + os << " ]\n } # end coord"< + void write2DnormalizedVs3DMatchesToVRML(CameraMatrix const& cam, std::vector > const& points2D,std::vector > const& points3D,double scale, + U const& color255,const char *filename, bool append) + { + using namespace std; + ofstream os; + os.open(filename, append ? ios::app : ios::out); + os.precision(3); + if (!append) os <<"#VRML V2.0 utf8" << endl; + write2DnormalizedVs3DMatchesToVRMLStream(os, cam, points2D, points3D, scale, color255); + } + +} + +#endif // V3D_VRMLIO_H diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/CMakeLists.txt b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/CMakeLists.txt new file mode 100644 index 0000000..924b176 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 2.6) + +project(V3D) + +set (V3D_DIR ${CMAKE_CURRENT_SOURCE_DIR}) +set (V3D_INCLUDE_DIRS ${V3D_DIR}/.) + +include (Config/v3d_macros.cmake) + +include (Config/local_config.cmake) + + +file(GLOB COLAMD_SRC + COLAMD/Source/* + COLAMD/Include/* + SuiteSparse_config/SuiteSparse_config.h + SuiteSparse_config/SuiteSparse_config.c + ) +include_directories("SuiteSparse_config/") +add_library(colamd ${COLAMD_SRC}) +set ( V3D_INCLUDE_DIRS ${V3D_INCLUDE_DIRS} "COLAMD/Include" ) + + + +include_directories(${V3D_INCLUDE_DIRS} ${EXTRA_INC_DIRS}) + + +source_group("Base" REGULAR_EXPRESSION Base/.*cpp|Base.*h) +set (BASE_SRC + Base/v3d_exception.h + Base/v3d_serialization.h + Base/v3d_vrmlio.h +) +if (V3DLIB_ENABLE_SOCKETS) + set (BASE_SRC ${BASE_SRC} Base/v3d_socket.h) +endif (V3DLIB_ENABLE_SOCKETS) + +source_group("Math" REGULAR_EXPRESSION Math/.*cpp|Math.*h) +set (MATH_SRC + Math/v3d_linear.h + Math/v3d_linearbase.h + Math/v3d_mathutilities.h + Math/v3d_nonlinlsq.h + Math/v3d_nonlinlsq.cpp + Math/v3d_optimization.h + Math/v3d_optimization.cpp + Math/v3d_optimization_lm.cpp +) + +source_group("Geometry" REGULAR_EXPRESSION Geometry/.*cpp|Geometry.*h) +set (GEOMETRY_SRC + Geometry/v3d_cameramatrix.h + Geometry/v3d_distortion.h + Geometry/v3d_metricbundle.h + Geometry/v3d_metricbundle.cpp + Geometry/v3d_stereobundle.h + Geometry/v3d_stereobundle.cpp +) + +set (ALL_SRC + ${BASE_SRC} ${MATH_SRC} ${GEOMETRY_SRC} ${GL_SRC} ${CUDA_SRC} +) + +add_library(V3D ${ALL_SRC}) +add_subdirectory(Apps) diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/CMakeLists.txt b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/CMakeLists.txt new file mode 100644 index 0000000..9587057 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/CMakeLists.txt @@ -0,0 +1,15 @@ +project(colamd) +cmake_minimum_required(VERSION 2.8) + +file(GLOB SRCS + Source/* + Include/*) + +include_directories( + "Include" + "../SuiteSparse_config/" + ) + +add_library( colamd ${SRCS} ) + +target_link_libraries( colamd SuiteSparse_config ) \ No newline at end of file diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Demo/Makefile b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Demo/Makefile new file mode 100644 index 0000000..90e9661 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Demo/Makefile @@ -0,0 +1,44 @@ +#----------------------------------------------------------------------------- +# compile the COLAMD demo +#----------------------------------------------------------------------------- + +default: colamd_example colamd_l_example + +include ../../SuiteSparse_config/SuiteSparse_config.mk + +I = -I../Include -I../../SuiteSparse_config + +C = $(CC) $(CF) $(I) + +library: + ( cd ../Lib ; $(MAKE) ) + +#------------------------------------------------------------------------------ +# Create the demo program, run it, and compare the output +#------------------------------------------------------------------------------ + +dist: + +colamd_example: colamd_example.c library + $(C) -o colamd_example colamd_example.c ../Lib/libcolamd.a -lm + - ./colamd_example > my_colamd_example.out + - diff colamd_example.out my_colamd_example.out + +colamd_l_example: colamd_l_example.c library + $(C) -o colamd_l_example colamd_l_example.c ../Lib/libcolamd.a -lm + - ./colamd_l_example > my_colamd_l_example.out + - diff colamd_l_example.out my_colamd_l_example.out + +#------------------------------------------------------------------------------ +# Remove all but the files in the original distribution +#------------------------------------------------------------------------------ + +clean: + - $(RM) $(CLEAN) + +purge: distclean + +distclean: clean + - $(RM) colamd_example colamd_l_example + - $(RM) my_colamd_example.out my_colamd_l_example.out + - $(RM) -r *.dSYM diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Demo/colamd_example.c b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Demo/colamd_example.c new file mode 100644 index 0000000..3295186 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Demo/colamd_example.c @@ -0,0 +1,180 @@ +/* ========================================================================== */ +/* === colamd and symamd example ============================================ */ +/* ========================================================================== */ + +/* COLAMD / SYMAMD example + + colamd example of use, to order the columns of a 5-by-4 matrix with + 11 nonzero entries in the following nonzero pattern, with default knobs. + + x 0 x 0 + x 0 x x + 0 x x 0 + 0 0 x x + x x 0 0 + + symamd example of use, to order the rows and columns of a 5-by-5 + matrix with 13 nonzero entries in the following nonzero pattern, + with default knobs. + + x x 0 0 0 + x x x x 0 + 0 x x 0 0 + 0 x 0 x x + 0 0 0 x x + + (where x denotes a nonzero value). + + See the colamd.c for the routines this program calls, and for the License. +*/ + +/* ========================================================================== */ + +#include +#include "colamd.h" + +#define A_NNZ 11 +#define A_NROW 5 +#define A_NCOL 4 +#define ALEN 150 + +#define B_NNZ 4 +#define B_N 5 + +int main (void) +{ + + /* ====================================================================== */ + /* input matrix A definition */ + /* ====================================================================== */ + + int A [ALEN] = { + + 0, 1, 4, /* row indices of nonzeros in column 0 */ + 2, 4, /* row indices of nonzeros in column 1 */ + 0, 1, 2, 3, /* row indices of nonzeros in column 2 */ + 1, 3} ; /* row indices of nonzeros in column 3 */ + + int p [ ] = { + + 0, /* column 0 is in A [0..2] */ + 3, /* column 1 is in A [3..4] */ + 5, /* column 2 is in A [5..8] */ + 9, /* column 3 is in A [9..10] */ + A_NNZ} ; /* number of nonzeros in A */ + + /* ====================================================================== */ + /* input matrix B definition */ + /* ====================================================================== */ + + int B [ ] = { /* Note: only strictly lower triangular part */ + /* is included, since symamd ignores the */ + /* diagonal and upper triangular part of B. */ + + 1, /* row indices of nonzeros in column 0 */ + 2, 3, /* row indices of nonzeros in column 1 */ + /* row indices of nonzeros in column 2 (none) */ + 4 /* row indices of nonzeros in column 3 */ + } ; /* row indices of nonzeros in column 4 (none) */ + + int q [ ] = { + + 0, /* column 0 is in B [0] */ + 1, /* column 1 is in B [1..2] */ + 3, /* column 2 is empty */ + 3, /* column 3 is in B [3] */ + 4, /* column 4 is empty */ + B_NNZ} ; /* number of nonzeros in strictly lower B */ + + /* ====================================================================== */ + /* other variable definitions */ + /* ====================================================================== */ + + int perm [B_N+1] ; /* note the size is N+1 */ + int stats [COLAMD_STATS] ; /* for colamd and symamd output statistics */ + + int row, col, pp, length, ok ; + + /* ====================================================================== */ + /* dump the input matrix A */ + /* ====================================================================== */ + + printf ("colamd %d-by-%d input matrix:\n", A_NROW, A_NCOL) ; + for (col = 0 ; col < A_NCOL ; col++) + { + length = p [col+1] - p [col] ; + printf ("Column %d, with %d entries:\n", col, length) ; + for (pp = p [col] ; pp < p [col+1] ; pp++) + { + row = A [pp] ; + printf (" row %d\n", row) ; + } + } + + /* ====================================================================== */ + /* order the matrix. Note that this destroys A and overwrites p */ + /* ====================================================================== */ + + ok = colamd (A_NROW, A_NCOL, ALEN, A, p, (double *) NULL, stats) ; + colamd_report (stats) ; + + if (!ok) + { + printf ("colamd error!\n") ; + exit (1) ; + } + + /* ====================================================================== */ + /* print the column ordering */ + /* ====================================================================== */ + + printf ("colamd column ordering:\n") ; + printf ("1st column: %d\n", p [0]) ; + printf ("2nd column: %d\n", p [1]) ; + printf ("3rd column: %d\n", p [2]) ; + printf ("4th column: %d\n", p [3]) ; + + /* ====================================================================== */ + /* dump the strictly lower triangular part of symmetric input matrix B */ + /* ====================================================================== */ + + printf ("\n\nsymamd %d-by-%d input matrix:\n", B_N, B_N) ; + printf ("Entries in strictly lower triangular part:\n") ; + for (col = 0 ; col < B_N ; col++) + { + length = q [col+1] - q [col] ; + printf ("Column %d, with %d entries:\n", col, length) ; + for (pp = q [col] ; pp < q [col+1] ; pp++) + { + row = B [pp] ; + printf (" row %d\n", row) ; + } + } + + /* ====================================================================== */ + /* order the matrix B. Note that this does not modify B or q. */ + /* ====================================================================== */ + + ok = symamd (B_N, B, q, perm, (double *) NULL, stats, &calloc, &free) ; + symamd_report (stats) ; + + if (!ok) + { + printf ("symamd error!\n") ; + exit (1) ; + } + + /* ====================================================================== */ + /* print the symmetric ordering */ + /* ====================================================================== */ + + printf ("symamd column ordering:\n") ; + printf ("1st row/column: %d\n", perm [0]) ; + printf ("2nd row/column: %d\n", perm [1]) ; + printf ("3rd row/column: %d\n", perm [2]) ; + printf ("4th row/column: %d\n", perm [3]) ; + printf ("5th row/column: %d\n", perm [4]) ; + + exit (0) ; +} + diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Demo/colamd_example.out b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Demo/colamd_example.out new file mode 100644 index 0000000..54f1a0f --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Demo/colamd_example.out @@ -0,0 +1,50 @@ +colamd 5-by-4 input matrix: +Column 0, with 3 entries: + row 0 + row 1 + row 4 +Column 1, with 2 entries: + row 2 + row 4 +Column 2, with 4 entries: + row 0 + row 1 + row 2 + row 3 +Column 3, with 2 entries: + row 1 + row 3 + +colamd version 2.8, Jun 1, 2012: OK. +colamd: number of dense or empty rows ignored: 0 +colamd: number of dense or empty columns ignored: 0 +colamd: number of garbage collections performed: 0 +colamd column ordering: +1st column: 1 +2nd column: 0 +3rd column: 2 +4th column: 3 + + +symamd 5-by-5 input matrix: +Entries in strictly lower triangular part: +Column 0, with 1 entries: + row 1 +Column 1, with 2 entries: + row 2 + row 3 +Column 2, with 0 entries: +Column 3, with 1 entries: + row 4 +Column 4, with 0 entries: + +symamd version 2.8, Jun 1, 2012: OK. +symamd: number of dense or empty rows ignored: 0 +symamd: number of dense or empty columns ignored: 0 +symamd: number of garbage collections performed: 0 +symamd column ordering: +1st row/column: 0 +2nd row/column: 2 +3rd row/column: 1 +4th row/column: 3 +5th row/column: 4 diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Demo/colamd_l_example.c b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Demo/colamd_l_example.c new file mode 100644 index 0000000..e1345e9 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Demo/colamd_l_example.c @@ -0,0 +1,182 @@ +/* ========================================================================== */ +/* === colamd and symamd example ============================================ */ +/* ========================================================================== */ + +/* COLAMD / SYMAMD example + + colamd example of use, to order the columns of a 5-by-4 matrix with + 11 nonzero entries in the following nonzero pattern, with default knobs. + + x 0 x 0 + x 0 x x + 0 x x 0 + 0 0 x x + x x 0 0 + + symamd example of use, to order the rows and columns of a 5-by-5 + matrix with 13 nonzero entries in the following nonzero pattern, + with default knobs. + + x x 0 0 0 + x x x x 0 + 0 x x 0 0 + 0 x 0 x x + 0 0 0 x x + + (where x denotes a nonzero value). + + + See the colamd.c for the routines this program calls, and for the License. +*/ + +/* ========================================================================== */ + +#include +#include "colamd.h" +#define Long SuiteSparse_long + +#define A_NNZ 11 +#define A_NROW 5 +#define A_NCOL 4 +#define ALEN 150 + +#define B_NNZ 4 +#define B_N 5 + +int main (void) +{ + + /* ====================================================================== */ + /* input matrix A definition */ + /* ====================================================================== */ + + Long A [ALEN] = { + + 0, 1, 4, /* row indices of nonzeros in column 0 */ + 2, 4, /* row indices of nonzeros in column 1 */ + 0, 1, 2, 3, /* row indices of nonzeros in column 2 */ + 1, 3} ; /* row indices of nonzeros in column 3 */ + + Long p [ ] = { + + 0, /* column 0 is in A [0..2] */ + 3, /* column 1 is in A [3..4] */ + 5, /* column 2 is in A [5..8] */ + 9, /* column 3 is in A [9..10] */ + A_NNZ} ; /* number of nonzeros in A */ + + /* ====================================================================== */ + /* input matrix B definition */ + /* ====================================================================== */ + + Long B [ ] = { /* Note: only strictly lower triangular part */ + /* is included, since symamd ignores the */ + /* diagonal and upper triangular part of B. */ + + 1, /* row indices of nonzeros in column 0 */ + 2, 3, /* row indices of nonzeros in column 1 */ + /* row indices of nonzeros in column 2 (none) */ + 4 /* row indices of nonzeros in column 3 */ + } ; /* row indices of nonzeros in column 4 (none) */ + + Long q [ ] = { + + 0, /* column 0 is in B [0] */ + 1, /* column 1 is in B [1..2] */ + 3, /* column 2 is empty */ + 3, /* column 3 is in B [3] */ + 4, /* column 4 is empty */ + B_NNZ} ; /* number of nonzeros in strictly lower B */ + + /* ====================================================================== */ + /* other variable definitions */ + /* ====================================================================== */ + + Long perm [B_N+1] ; /* note the size is N+1 */ + Long stats [COLAMD_STATS] ; /* for colamd and symamd output statistics */ + + Long row, col, pp, length, ok ; + + /* ====================================================================== */ + /* dump the input matrix A */ + /* ====================================================================== */ + + printf ("colamd %d-by-%d input matrix:\n", A_NROW, A_NCOL) ; + for (col = 0 ; col < A_NCOL ; col++) + { + length = p [col+1] - p [col] ; + printf ("Column %ld, with %ld entries:\n", col, length) ; + for (pp = p [col] ; pp < p [col+1] ; pp++) + { + row = A [pp] ; + printf (" row %ld\n", row) ; + } + } + + /* ====================================================================== */ + /* order the matrix. Note that this destroys A and overwrites p */ + /* ====================================================================== */ + + ok = colamd_l (A_NROW, A_NCOL, ALEN, A, p, (double *) NULL, stats) ; + colamd_l_report (stats) ; + + if (!ok) + { + printf ("colamd error!\n") ; + exit (1) ; + } + + /* ====================================================================== */ + /* print the column ordering */ + /* ====================================================================== */ + + printf ("colamd_l column ordering:\n") ; + printf ("1st column: %ld\n", p [0]) ; + printf ("2nd column: %ld\n", p [1]) ; + printf ("3rd column: %ld\n", p [2]) ; + printf ("4th column: %ld\n", p [3]) ; + + /* ====================================================================== */ + /* dump the strictly lower triangular part of symmetric input matrix B */ + /* ====================================================================== */ + + printf ("\n\nsymamd_l %d-by-%d input matrix:\n", B_N, B_N) ; + printf ("Entries in strictly lower triangular part:\n") ; + for (col = 0 ; col < B_N ; col++) + { + length = q [col+1] - q [col] ; + printf ("Column %ld, with %ld entries:\n", col, length) ; + for (pp = q [col] ; pp < q [col+1] ; pp++) + { + row = B [pp] ; + printf (" row %ld\n", row) ; + } + } + + /* ====================================================================== */ + /* order the matrix B. Note that this does not modify B or q. */ + /* ====================================================================== */ + + ok = symamd_l (B_N, B, q, perm, (double *) NULL, stats, &calloc, &free) ; + symamd_l_report (stats) ; + + if (!ok) + { + printf ("symamd error!\n") ; + exit (1) ; + } + + /* ====================================================================== */ + /* print the symmetric ordering */ + /* ====================================================================== */ + + printf ("symamd_l column ordering:\n") ; + printf ("1st row/column: %ld\n", perm [0]) ; + printf ("2nd row/column: %ld\n", perm [1]) ; + printf ("3rd row/column: %ld\n", perm [2]) ; + printf ("4th row/column: %ld\n", perm [3]) ; + printf ("5th row/column: %ld\n", perm [4]) ; + + exit (0) ; +} + diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Demo/colamd_l_example.out b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Demo/colamd_l_example.out new file mode 100644 index 0000000..e001675 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Demo/colamd_l_example.out @@ -0,0 +1,50 @@ +colamd 5-by-4 input matrix: +Column 0, with 3 entries: + row 0 + row 1 + row 4 +Column 1, with 2 entries: + row 2 + row 4 +Column 2, with 4 entries: + row 0 + row 1 + row 2 + row 3 +Column 3, with 2 entries: + row 1 + row 3 + +colamd version 2.8, Jun 1, 2012: OK. +colamd: number of dense or empty rows ignored: 0 +colamd: number of dense or empty columns ignored: 0 +colamd: number of garbage collections performed: 0 +colamd_l column ordering: +1st column: 1 +2nd column: 0 +3rd column: 2 +4th column: 3 + + +symamd_l 5-by-5 input matrix: +Entries in strictly lower triangular part: +Column 0, with 1 entries: + row 1 +Column 1, with 2 entries: + row 2 + row 3 +Column 2, with 0 entries: +Column 3, with 1 entries: + row 4 +Column 4, with 0 entries: + +symamd version 2.8, Jun 1, 2012: OK. +symamd: number of dense or empty rows ignored: 0 +symamd: number of dense or empty columns ignored: 0 +symamd: number of garbage collections performed: 0 +symamd_l column ordering: +1st row/column: 0 +2nd row/column: 2 +3rd row/column: 1 +4th row/column: 3 +5th row/column: 4 diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Doc/ChangeLog b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Doc/ChangeLog new file mode 100644 index 0000000..302dcd0 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Doc/ChangeLog @@ -0,0 +1,145 @@ +Jun 1, 2012: version 2.8.0 + + * UFconfig replaced with SuiteSparse_config + +Dec 7, 2011: version 2.7.4 + + * fixed the Makefile to better align with CFLAGS and other standards + +Jan 25, 2011: version 2.7.3 + + * minor fix to "make install" + +Nov 30, 2009: version 2.7.2 + + * added "make install" and "make uninstall" + +May 31, 2007: version 2.7.0 + + * ported to 64-bit MATLAB + + * subdirectories added (Source/, Include/, Lib/, Doc/, MATLAB/, Demo/) + +Dec 12, 2006, version 2.5.2 + + * minor MATLAB cleanup. MATLAB functions renamed colamd2 and symamd2, + so that they do not conflict with the built-in versions. Note that + the MATLAB built-in functions colamd and symamd are identical to + the colamd and symamd functions here. + +Aug 31, 2006: Version 2.5.1 + + * minor change to colamd.m and symamd.m, to use etree instead + of sparsfun. + +Apr. 30, 2006: Version 2.5 + + * colamd_recommended modified, to do more careful integer overflow + checking. It now returns size_t, not int. colamd_l_recommended + also returns size_t. A zero is returned if an error occurs. A + postive return value denotes success. In v2.4 and earlier, + -1 was returned on error (an int or long). + + * long replaced with UF_long integer, which is long except on WIN64. + +Nov 15, 2005: + + * minor editting of comments; version number (2.4) unchanged. + +Changes from Version 2.3 to 2.4 (Aug 30, 2005) + + * Makefile now relies on ../UFconfig/UFconfig.mk + + * changed the dense row/col detection. The meaning of the knobs + has thus changed. + + * added an option to turn off aggressive absorption. It was + always on in versions 2.3 and earlier. + + * added a #define'd version number + + * added a function pointer (colamd_printf) for COLAMD's printing. + + * added a -DNPRINT option, to turn off printing at compile-time. + + * added a check for integer overflow in colamd_recommended + + * minor changes to allow for more simpler 100% test coverage + + * bug fix. If symamd v2.3 fails to allocate its copy of the input + matrix, then it erroneously frees a calloc'd workspace twice. + This bug has no effect on the MATLAB symamd mexFunction, since + mxCalloc terminates the mexFunction if it fails to allocate + memory. Similarly, UMFPACK is not affected because it does not + use symamd. The bug has no effect on the colamd ordering + routine in v2.3. + +Changes from Version 2.2 to 2.3 (Sept. 8, 2003) + + * removed the call to the MATLAB spparms ('spumoni') function. + This can take a lot of time if you are ordering many small + matrices. Only affects the MATLAB interface (colamdmex.c, + symamdmex.c, colamdtestmex.c, and symamdtestmex.c). The + usage of the optional 2nd argument to the colamd and symamd + mexFunctions was changed accordingly. + +Changes from Version 2.1 to 2.2 (Sept. 23, 2002) + + * extensive testing routines added (colamd_test.m, colamdtestmex.c, + and symamdtestmex.c), and the Makefile modified accordingly. + + * a few typos in the comments corrected + + * use of the MATLAB "flops" command removed from colamd_demo, and an + m-file routine luflops.m added. + + * an explicit typecast from unsigned to int added, for COLAMD_C and + COLAMD_R in colamd.h. + + * #include added to colamd_example.c + + +Changes from Version 2.0 to 2.1 (May 4, 2001) + + * TRUE and FALSE are predefined on some systems, so they are defined + here only if not already defined. + + * web site changed + + * UNIX Makefile modified, to handle the case if "." is not in your path. + + +Changes from Version 1.0 to 2.0 (January 31, 2000) + + No bugs were found in version 1.1. These changes merely add new + functionality. + + * added the COLAMD_RECOMMENDED (nnz, n_row, n_col) macro. + + * moved the output statistics, from A, to a separate output argument. + The arguments changed for the C-callable routines. + + * added colamd_report and symamd_report. + + * added a C-callable symamd routine. Formerly, symamd was only + available as a mexFunction from MATLAB. + + * added error-checking to symamd. Formerly, it assumed its input + was error-free. + + * added the optional stats and knobs arguments to the symamd mexFunction + + * deleted colamd_help. A help message is still available from + "help colamd" and "help symamd" in MATLAB. + + * deleted colamdtree.m and symamdtree.m. Now, colamd.m and symamd.m + also do the elimination tree post-ordering. The Version 1.1 + colamd and symamd mexFunctions, which do not do the post- + ordering, are now visible as colamdmex and symamdmex from + MATLAB. Essentialy, the post-ordering is now the default + behavior of colamd.m and symamd.m, to match the behavior of + colmmd and symmmd. The post-ordering is only available in the + MATLAB interface, not the C-callable interface. + + * made a slight change to the dense row/column detection in symamd, + to match the stated specifications. diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Doc/lesser.txt b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Doc/lesser.txt new file mode 100644 index 0000000..8add30a --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Doc/lesser.txt @@ -0,0 +1,504 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 2.1, February 1999 + + Copyright (C) 1991, 1999 Free Software Foundation, Inc. + 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + +[This is the first released version of the Lesser GPL. It also counts + as the successor of the GNU Library Public License, version 2, hence + the version number 2.1.] + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +Licenses are intended to guarantee your freedom to share and change +free software--to make sure the software is free for all its users. + + This license, the Lesser General Public License, applies to some +specially designated software packages--typically libraries--of the +Free Software Foundation and other authors who decide to use it. You +can use it too, but we suggest you first think carefully about whether +this license or the ordinary General Public License is the better +strategy to use in any particular case, based on the explanations below. + + When we speak of free software, we are referring to freedom of use, +not price. Our General Public Licenses are designed to make sure that +you have the freedom to distribute copies of free software (and charge +for this service if you wish); that you receive source code or can get +it if you want it; that you can change the software and use pieces of +it in new free programs; and that you are informed that you can do +these things. + + To protect your rights, we need to make restrictions that forbid +distributors to deny you these rights or to ask you to surrender these +rights. These restrictions translate to certain responsibilities for +you if you distribute copies of the library or if you modify it. + + For example, if you distribute copies of the library, whether gratis +or for a fee, you must give the recipients all the rights that we gave +you. You must make sure that they, too, receive or can get the source +code. If you link other code with the library, you must provide +complete object files to the recipients, so that they can relink them +with the library after making changes to the library and recompiling +it. And you must show them these terms so they know their rights. + + We protect your rights with a two-step method: (1) we copyright the +library, and (2) we offer you this license, which gives you legal +permission to copy, distribute and/or modify the library. + + To protect each distributor, we want to make it very clear that +there is no warranty for the free library. Also, if the library is +modified by someone else and passed on, the recipients should know +that what they have is not the original version, so that the original +author's reputation will not be affected by problems that might be +introduced by others. + + Finally, software patents pose a constant threat to the existence of +any free program. We wish to make sure that a company cannot +effectively restrict the users of a free program by obtaining a +restrictive license from a patent holder. Therefore, we insist that +any patent license obtained for a version of the library must be +consistent with the full freedom of use specified in this license. + + Most GNU software, including some libraries, is covered by the +ordinary GNU General Public License. This license, the GNU Lesser +General Public License, applies to certain designated libraries, and +is quite different from the ordinary General Public License. We use +this license for certain libraries in order to permit linking those +libraries into non-free programs. + + When a program is linked with a library, whether statically or using +a shared library, the combination of the two is legally speaking a +combined work, a derivative of the original library. The ordinary +General Public License therefore permits such linking only if the +entire combination fits its criteria of freedom. The Lesser General +Public License permits more lax criteria for linking other code with +the library. + + We call this license the "Lesser" General Public License because it +does Less to protect the user's freedom than the ordinary General +Public License. It also provides other free software developers Less +of an advantage over competing non-free programs. These disadvantages +are the reason we use the ordinary General Public License for many +libraries. However, the Lesser license provides advantages in certain +special circumstances. + + For example, on rare occasions, there may be a special need to +encourage the widest possible use of a certain library, so that it becomes +a de-facto standard. To achieve this, non-free programs must be +allowed to use the library. A more frequent case is that a free +library does the same job as widely used non-free libraries. In this +case, there is little to gain by limiting the free library to free +software only, so we use the Lesser General Public License. + + In other cases, permission to use a particular library in non-free +programs enables a greater number of people to use a large body of +free software. For example, permission to use the GNU C Library in +non-free programs enables many more people to use the whole GNU +operating system, as well as its variant, the GNU/Linux operating +system. + + Although the Lesser General Public License is Less protective of the +users' freedom, it does ensure that the user of a program that is +linked with the Library has the freedom and the wherewithal to run +that program using a modified version of the Library. + + The precise terms and conditions for copying, distribution and +modification follow. Pay close attention to the difference between a +"work based on the library" and a "work that uses the library". The +former contains code derived from the library, whereas the latter must +be combined with the library in order to run. + + GNU LESSER GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License Agreement applies to any software library or other +program which contains a notice placed by the copyright holder or +other authorized party saying it may be distributed under the terms of +this Lesser General Public License (also called "this License"). +Each licensee is addressed as "you". + + A "library" means a collection of software functions and/or data +prepared so as to be conveniently linked with application programs +(which use some of those functions and data) to form executables. + + The "Library", below, refers to any such software library or work +which has been distributed under these terms. A "work based on the +Library" means either the Library or any derivative work under +copyright law: that is to say, a work containing the Library or a +portion of it, either verbatim or with modifications and/or translated +straightforwardly into another language. (Hereinafter, translation is +included without limitation in the term "modification".) + + "Source code" for a work means the preferred form of the work for +making modifications to it. For a library, complete source code means +all the source code for all modules it contains, plus any associated +interface definition files, plus the scripts used to control compilation +and installation of the library. + + Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running a program using the Library is not restricted, and output from +such a program is covered only if its contents constitute a work based +on the Library (independent of the use of the Library in a tool for +writing it). Whether that is true depends on what the Library does +and what the program that uses the Library does. + + 1. You may copy and distribute verbatim copies of the Library's +complete source code as you receive it, in any medium, provided that +you conspicuously and appropriately publish on each copy an +appropriate copyright notice and disclaimer of warranty; keep intact +all the notices that refer to this License and to the absence of any +warranty; and distribute a copy of this License along with the +Library. + + You may charge a fee for the physical act of transferring a copy, +and you may at your option offer warranty protection in exchange for a +fee. + + 2. You may modify your copy or copies of the Library or any portion +of it, thus forming a work based on the Library, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) The modified work must itself be a software library. + + b) You must cause the files modified to carry prominent notices + stating that you changed the files and the date of any change. + + c) You must cause the whole of the work to be licensed at no + charge to all third parties under the terms of this License. + + d) If a facility in the modified Library refers to a function or a + table of data to be supplied by an application program that uses + the facility, other than as an argument passed when the facility + is invoked, then you must make a good faith effort to ensure that, + in the event an application does not supply such function or + table, the facility still operates, and performs whatever part of + its purpose remains meaningful. + + (For example, a function in a library to compute square roots has + a purpose that is entirely well-defined independent of the + application. Therefore, Subsection 2d requires that any + application-supplied function or table used by this function must + be optional: if the application does not supply it, the square + root function must still compute square roots.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Library, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Library, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote +it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Library. + +In addition, mere aggregation of another work not based on the Library +with the Library (or with a work based on the Library) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may opt to apply the terms of the ordinary GNU General Public +License instead of this License to a given copy of the Library. To do +this, you must alter all the notices that refer to this License, so +that they refer to the ordinary GNU General Public License, version 2, +instead of to this License. (If a newer version than version 2 of the +ordinary GNU General Public License has appeared, then you can specify +that version instead if you wish.) Do not make any other change in +these notices. + + Once this change is made in a given copy, it is irreversible for +that copy, so the ordinary GNU General Public License applies to all +subsequent copies and derivative works made from that copy. + + This option is useful when you wish to copy part of the code of +the Library into a program that is not a library. + + 4. You may copy and distribute the Library (or a portion or +derivative of it, under Section 2) in object code or executable form +under the terms of Sections 1 and 2 above provided that you accompany +it with the complete corresponding machine-readable source code, which +must be distributed under the terms of Sections 1 and 2 above on a +medium customarily used for software interchange. + + If distribution of object code is made by offering access to copy +from a designated place, then offering equivalent access to copy the +source code from the same place satisfies the requirement to +distribute the source code, even though third parties are not +compelled to copy the source along with the object code. + + 5. A program that contains no derivative of any portion of the +Library, but is designed to work with the Library by being compiled or +linked with it, is called a "work that uses the Library". Such a +work, in isolation, is not a derivative work of the Library, and +therefore falls outside the scope of this License. + + However, linking a "work that uses the Library" with the Library +creates an executable that is a derivative of the Library (because it +contains portions of the Library), rather than a "work that uses the +library". The executable is therefore covered by this License. +Section 6 states terms for distribution of such executables. + + When a "work that uses the Library" uses material from a header file +that is part of the Library, the object code for the work may be a +derivative work of the Library even though the source code is not. +Whether this is true is especially significant if the work can be +linked without the Library, or if the work is itself a library. The +threshold for this to be true is not precisely defined by law. + + If such an object file uses only numerical parameters, data +structure layouts and accessors, and small macros and small inline +functions (ten lines or less in length), then the use of the object +file is unrestricted, regardless of whether it is legally a derivative +work. (Executables containing this object code plus portions of the +Library will still fall under Section 6.) + + Otherwise, if the work is a derivative of the Library, you may +distribute the object code for the work under the terms of Section 6. +Any executables containing that work also fall under Section 6, +whether or not they are linked directly with the Library itself. + + 6. As an exception to the Sections above, you may also combine or +link a "work that uses the Library" with the Library to produce a +work containing portions of the Library, and distribute that work +under terms of your choice, provided that the terms permit +modification of the work for the customer's own use and reverse +engineering for debugging such modifications. + + You must give prominent notice with each copy of the work that the +Library is used in it and that the Library and its use are covered by +this License. You must supply a copy of this License. If the work +during execution displays copyright notices, you must include the +copyright notice for the Library among them, as well as a reference +directing the user to the copy of this License. Also, you must do one +of these things: + + a) Accompany the work with the complete corresponding + machine-readable source code for the Library including whatever + changes were used in the work (which must be distributed under + Sections 1 and 2 above); and, if the work is an executable linked + with the Library, with the complete machine-readable "work that + uses the Library", as object code and/or source code, so that the + user can modify the Library and then relink to produce a modified + executable containing the modified Library. (It is understood + that the user who changes the contents of definitions files in the + Library will not necessarily be able to recompile the application + to use the modified definitions.) + + b) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (1) uses at run time a + copy of the library already present on the user's computer system, + rather than copying library functions into the executable, and (2) + will operate properly with a modified version of the library, if + the user installs one, as long as the modified version is + interface-compatible with the version that the work was made with. + + c) Accompany the work with a written offer, valid for at + least three years, to give the same user the materials + specified in Subsection 6a, above, for a charge no more + than the cost of performing this distribution. + + d) If distribution of the work is made by offering access to copy + from a designated place, offer equivalent access to copy the above + specified materials from the same place. + + e) Verify that the user has already received a copy of these + materials or that you have already sent this user a copy. + + For an executable, the required form of the "work that uses the +Library" must include any data and utility programs needed for +reproducing the executable from it. However, as a special exception, +the materials to be distributed need not include anything that is +normally distributed (in either source or binary form) with the major +components (compiler, kernel, and so on) of the operating system on +which the executable runs, unless that component itself accompanies +the executable. + + It may happen that this requirement contradicts the license +restrictions of other proprietary libraries that do not normally +accompany the operating system. Such a contradiction means you cannot +use both them and the Library together in an executable that you +distribute. + + 7. You may place library facilities that are a work based on the +Library side-by-side in a single library together with other library +facilities not covered by this License, and distribute such a combined +library, provided that the separate distribution of the work based on +the Library and of the other library facilities is otherwise +permitted, and provided that you do these two things: + + a) Accompany the combined library with a copy of the same work + based on the Library, uncombined with any other library + facilities. This must be distributed under the terms of the + Sections above. + + b) Give prominent notice with the combined library of the fact + that part of it is a work based on the Library, and explaining + where to find the accompanying uncombined form of the same work. + + 8. You may not copy, modify, sublicense, link with, or distribute +the Library except as expressly provided under this License. Any +attempt otherwise to copy, modify, sublicense, link with, or +distribute the Library is void, and will automatically terminate your +rights under this License. However, parties who have received copies, +or rights, from you under this License will not have their licenses +terminated so long as such parties remain in full compliance. + + 9. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Library or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Library (or any work based on the +Library), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Library or works based on it. + + 10. Each time you redistribute the Library (or any work based on the +Library), the recipient automatically receives a license from the +original licensor to copy, distribute, link with or modify the Library +subject to these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties with +this License. + + 11. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Library at all. For example, if a patent +license would not permit royalty-free redistribution of the Library by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Library. + +If any portion of this section is held invalid or unenforceable under any +particular circumstance, the balance of the section is intended to apply, +and the section as a whole is intended to apply in other circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 12. If the distribution and/or use of the Library is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Library under this License may add +an explicit geographical distribution limitation excluding those countries, +so that distribution is permitted only in or among countries not thus +excluded. In such case, this License incorporates the limitation as if +written in the body of this License. + + 13. The Free Software Foundation may publish revised and/or new +versions of the Lesser General Public License from time to time. +Such new versions will be similar in spirit to the present version, +but may differ in detail to address new problems or concerns. + +Each version is given a distinguishing version number. If the Library +specifies a version number of this License which applies to it and +"any later version", you have the option of following the terms and +conditions either of that version or of any later version published by +the Free Software Foundation. If the Library does not specify a +license version number, you may choose any version ever published by +the Free Software Foundation. + + 14. If you wish to incorporate parts of the Library into other free +programs whose distribution conditions are incompatible with these, +write to the author to ask for permission. For software which is +copyrighted by the Free Software Foundation, write to the Free +Software Foundation; we sometimes make exceptions for this. Our +decision will be guided by the two goals of preserving the free status +of all derivatives of our free software and of promoting the sharing +and reuse of software generally. + + NO WARRANTY + + 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO +WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. +EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR +OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY +KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE +LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME +THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN +WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY +AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU +FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR +CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE +LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING +RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A +FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF +SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH +DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Libraries + + If you develop a new library, and you want it to be of the greatest +possible use to the public, we recommend making it free software that +everyone can redistribute and change. You can do so by permitting +redistribution under these terms (or, alternatively, under the terms of the +ordinary General Public License). + + To apply these terms, attach the following notices to the library. It is +safest to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least the +"copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +Also add information on how to contact you by electronic and paper mail. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the library, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the + library `Frob' (a library for tweaking knobs) written by James Random Hacker. + + , 1 April 1990 + Ty Coon, President of Vice + +That's all there is to it! + + diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Include/colamd.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Include/colamd.h new file mode 100644 index 0000000..59e6c6b --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Include/colamd.h @@ -0,0 +1,251 @@ +/* ========================================================================== */ +/* === colamd/symamd prototypes and definitions ============================= */ +/* ========================================================================== */ + +/* COLAMD / SYMAMD include file + + You must include this file (colamd.h) in any routine that uses colamd, + symamd, or the related macros and definitions. + + Authors: + + The authors of the code itself are Stefan I. Larimore and Timothy A. + Davis (DrTimothyAldenDavis@gmail.com). The algorithm was + developed in collaboration with John Gilbert, Xerox PARC, and Esmond + Ng, Oak Ridge National Laboratory. + + Acknowledgements: + + This work was supported by the National Science Foundation, under + grants DMS-9504974 and DMS-9803599. + + Notice: + + Copyright (c) 1998-2007, Timothy A. Davis, All Rights Reserved. + + THIS MATERIAL IS PROVIDED AS IS, WITH ABSOLUTELY NO WARRANTY + EXPRESSED OR IMPLIED. ANY USE IS AT YOUR OWN RISK. + + Permission is hereby granted to use, copy, modify, and/or distribute + this program, provided that the Copyright, this License, and the + Availability of the original version is retained on all copies and made + accessible to the end-user of any code or package that includes COLAMD + or any modified version of COLAMD. + + Availability: + + The colamd/symamd library is available at http://www.suitesparse.com + This file is required by the colamd.c, colamdmex.c, and symamdmex.c + files, and by any C code that calls the routines whose prototypes are + listed below, or that uses the colamd/symamd definitions listed below. + +*/ + +#ifndef COLAMD_H +#define COLAMD_H + +/* make it easy for C++ programs to include COLAMD */ +#ifdef __cplusplus +extern "C" { +#endif + +/* ========================================================================== */ +/* === Include files ======================================================== */ +/* ========================================================================== */ + +#include + +/* ========================================================================== */ +/* === COLAMD version ======================================================= */ +/* ========================================================================== */ + +/* COLAMD Version 2.4 and later will include the following definitions. + * As an example, to test if the version you are using is 2.4 or later: + * + * #ifdef COLAMD_VERSION + * if (COLAMD_VERSION >= COLAMD_VERSION_CODE (2,4)) ... + * #endif + * + * This also works during compile-time: + * + * #if defined(COLAMD_VERSION) && (COLAMD_VERSION >= COLAMD_VERSION_CODE (2,4)) + * printf ("This is version 2.4 or later\n") ; + * #else + * printf ("This is an early version\n") ; + * #endif + * + * Versions 2.3 and earlier of COLAMD do not include a #define'd version number. + */ + +#define COLAMD_DATE "Jun 1, 2012" +#define COLAMD_VERSION_CODE(main,sub) ((main) * 1000 + (sub)) +#define COLAMD_MAIN_VERSION 2 +#define COLAMD_SUB_VERSION 8 +#define COLAMD_SUBSUB_VERSION 0 +#define COLAMD_VERSION \ + COLAMD_VERSION_CODE(COLAMD_MAIN_VERSION,COLAMD_SUB_VERSION) + +/* ========================================================================== */ +/* === Knob and statistics definitions ====================================== */ +/* ========================================================================== */ + +/* size of the knobs [ ] array. Only knobs [0..1] are currently used. */ +#define COLAMD_KNOBS 20 + +/* number of output statistics. Only stats [0..6] are currently used. */ +#define COLAMD_STATS 20 + +/* knobs [0] and stats [0]: dense row knob and output statistic. */ +#define COLAMD_DENSE_ROW 0 + +/* knobs [1] and stats [1]: dense column knob and output statistic. */ +#define COLAMD_DENSE_COL 1 + +/* knobs [2]: aggressive absorption */ +#define COLAMD_AGGRESSIVE 2 + +/* stats [2]: memory defragmentation count output statistic */ +#define COLAMD_DEFRAG_COUNT 2 + +/* stats [3]: colamd status: zero OK, > 0 warning or notice, < 0 error */ +#define COLAMD_STATUS 3 + +/* stats [4..6]: error info, or info on jumbled columns */ +#define COLAMD_INFO1 4 +#define COLAMD_INFO2 5 +#define COLAMD_INFO3 6 + +/* error codes returned in stats [3]: */ +#define COLAMD_OK (0) +#define COLAMD_OK_BUT_JUMBLED (1) +#define COLAMD_ERROR_A_not_present (-1) +#define COLAMD_ERROR_p_not_present (-2) +#define COLAMD_ERROR_nrow_negative (-3) +#define COLAMD_ERROR_ncol_negative (-4) +#define COLAMD_ERROR_nnz_negative (-5) +#define COLAMD_ERROR_p0_nonzero (-6) +#define COLAMD_ERROR_A_too_small (-7) +#define COLAMD_ERROR_col_length_negative (-8) +#define COLAMD_ERROR_row_index_out_of_bounds (-9) +#define COLAMD_ERROR_out_of_memory (-10) +#define COLAMD_ERROR_internal_error (-999) + + +/* ========================================================================== */ +/* === Prototypes of user-callable routines ================================= */ +/* ========================================================================== */ + +#include "SuiteSparse_config.h" + +size_t colamd_recommended /* returns recommended value of Alen, */ + /* or 0 if input arguments are erroneous */ +( + int nnz, /* nonzeros in A */ + int n_row, /* number of rows in A */ + int n_col /* number of columns in A */ +) ; + +size_t colamd_l_recommended /* returns recommended value of Alen, */ + /* or 0 if input arguments are erroneous */ +( + SuiteSparse_long nnz, /* nonzeros in A */ + SuiteSparse_long n_row, /* number of rows in A */ + SuiteSparse_long n_col /* number of columns in A */ +) ; + +void colamd_set_defaults /* sets default parameters */ +( /* knobs argument is modified on output */ + double knobs [COLAMD_KNOBS] /* parameter settings for colamd */ +) ; + +void colamd_l_set_defaults /* sets default parameters */ +( /* knobs argument is modified on output */ + double knobs [COLAMD_KNOBS] /* parameter settings for colamd */ +) ; + +int colamd /* returns (1) if successful, (0) otherwise*/ +( /* A and p arguments are modified on output */ + int n_row, /* number of rows in A */ + int n_col, /* number of columns in A */ + int Alen, /* size of the array A */ + int A [], /* row indices of A, of size Alen */ + int p [], /* column pointers of A, of size n_col+1 */ + double knobs [COLAMD_KNOBS],/* parameter settings for colamd */ + int stats [COLAMD_STATS] /* colamd output statistics and error codes */ +) ; + +SuiteSparse_long colamd_l /* returns (1) if successful, (0) otherwise*/ +( /* A and p arguments are modified on output */ + SuiteSparse_long n_row, /* number of rows in A */ + SuiteSparse_long n_col, /* number of columns in A */ + SuiteSparse_long Alen, /* size of the array A */ + SuiteSparse_long A [], /* row indices of A, of size Alen */ + SuiteSparse_long p [], /* column pointers of A, of size n_col+1 */ + double knobs [COLAMD_KNOBS],/* parameter settings for colamd */ + SuiteSparse_long stats [COLAMD_STATS] /* colamd output statistics + * and error codes */ +) ; + +int symamd /* return (1) if OK, (0) otherwise */ +( + int n, /* number of rows and columns of A */ + int A [], /* row indices of A */ + int p [], /* column pointers of A */ + int perm [], /* output permutation, size n_col+1 */ + double knobs [COLAMD_KNOBS], /* parameters (uses defaults if NULL) */ + int stats [COLAMD_STATS], /* output statistics and error codes */ + void * (*allocate) (size_t, size_t), + /* pointer to calloc (ANSI C) or */ + /* mxCalloc (for MATLAB mexFunction) */ + void (*release) (void *) + /* pointer to free (ANSI C) or */ + /* mxFree (for MATLAB mexFunction) */ +) ; + +SuiteSparse_long symamd_l /* return (1) if OK, (0) otherwise */ +( + SuiteSparse_long n, /* number of rows and columns of A */ + SuiteSparse_long A [], /* row indices of A */ + SuiteSparse_long p [], /* column pointers of A */ + SuiteSparse_long perm [], /* output permutation, size n_col+1 */ + double knobs [COLAMD_KNOBS], /* parameters (uses defaults if NULL) */ + SuiteSparse_long stats [COLAMD_STATS], /* output stats and error codes */ + void * (*allocate) (size_t, size_t), + /* pointer to calloc (ANSI C) or */ + /* mxCalloc (for MATLAB mexFunction) */ + void (*release) (void *) + /* pointer to free (ANSI C) or */ + /* mxFree (for MATLAB mexFunction) */ +) ; + +void colamd_report +( + int stats [COLAMD_STATS] +) ; + +void colamd_l_report +( + SuiteSparse_long stats [COLAMD_STATS] +) ; + +void symamd_report +( + int stats [COLAMD_STATS] +) ; + +void symamd_l_report +( + SuiteSparse_long stats [COLAMD_STATS] +) ; + +#ifndef EXTERN +#define EXTERN extern +#endif + +EXTERN int (*colamd_printf) (const char *, ...) ; + +#ifdef __cplusplus +} +#endif + +#endif /* COLAMD_H */ diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Lib/Makefile b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Lib/Makefile new file mode 100644 index 0000000..7f955ac --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Lib/Makefile @@ -0,0 +1,33 @@ +#------------------------------------------------------------------------------- +# COLAMD Makefile +#------------------------------------------------------------------------------- + +default: libcolamd.a + +include ../../SuiteSparse_config/SuiteSparse_config.mk + +I = -I../Include -I../../SuiteSparse_config + +INC = ../Include/colamd.h ../../SuiteSparse_config/SuiteSparse_config.h + +SRC = ../Source/colamd.c ../Source/colamd_global.c + +# creates libcolamd.a, a C-callable COLAMD library +libcolamd.a: $(SRC) $(INC) + $(CC) $(CF) $(I) -c ../Source/colamd_global.c + $(CC) $(CF) $(I) -c ../Source/colamd.c + $(CC) $(CF) $(I) -c ../Source/colamd.c -DDLONG -o colamd_l.o + $(ARCHIVE) libcolamd.a colamd.o colamd_l.o colamd_global.o + - $(RANLIB) libcolamd.a + +ccode: libcolamd.a + +library: libcolamd.a + +clean: + - $(RM) $(CLEAN) + +purge: distclean + +distclean: clean + - $(RM) libcolamd.a diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/Contents.m b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/Contents.m new file mode 100644 index 0000000..960fd14 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/Contents.m @@ -0,0 +1,19 @@ +% COLAMD, column approximate minimum degree ordering +% +% Primary: +% colamd2 - Column approximate minimum degree permutation. +% symamd2 - SYMAMD Symmetric approximate minimum degree permutation. +% +% helper and test functions: +% colamd_demo - demo for colamd, column approx minimum degree ordering algorithm +% colamd_make - compiles COLAMD2 and SYMAMD2 for MATLAB +% colamd_make - compiles and installs COLAMD2 and SYMAMD2 for MATLAB +% colamd_test - test colamd2 and symamd2 +% luflops - compute the flop count for sparse LU factorization +% +% Example: +% p = colamd2 (A) +% + +% Copyright 1998-2007, Timothy A. Davis, and Stefan Larimore +% Developed in collaboration with J. Gilbert and E. Ng. diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamd2.m b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamd2.m new file mode 100644 index 0000000..9916cc7 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamd2.m @@ -0,0 +1,87 @@ +function [p,stats] = colamd2 (S, knobs) +%COLAMD2 Column approximate minimum degree permutation. +% P = COLAMD2(S) returns the column approximate minimum degree permutation +% vector for the sparse matrix S. For a non-symmetric matrix S, S(:,P) +% tends to have sparser LU factors than S. The Cholesky factorization of +% S(:,P)'*S(:,P) also tends to be sparser than that of S'*S. The ordering +% is followed by a column elimination tree post-ordering. +% +% Note that this function is the source code for the built-in MATLAB colamd +% function. It has been renamed here to colamd2 to avoid a filename clash. +% colamd and colamd2 are identical. +% +% See also COLAMD, AMD, SYMAMD, SYMAMD2. +% +% Example: +% P = colamd2 (S) +% [P, stats] = colamd2 (S, knobs) +% +% knobs is an optional one- to three-element input vector. If S is m-by-n, +% then rows with more than max(16,knobs(1)*sqrt(n)) entries are ignored. +% Columns with more than max(16,knobs(2)*sqrt(min(m,n))) entries are +% removed prior to ordering, and ordered last in the output permutation P. +% Only completely dense rows or columns are removed if knobs(1) and knobs(2) +% are < 0, respectively. If knobs(3) is nonzero, stats and knobs are +% printed. The default is knobs = [10 10 0]. Note that knobs differs from +% earlier versions of colamd. + +% Copyright 1998-2007, Timothy A. Davis, and Stefan Larimore +% Developed in collaboration with J. Gilbert and E. Ng. +% +% Acknowledgements: This work was supported by the National Science +% Foundation, under grants DMS-9504974 and DMS-9803599. + +%------------------------------------------------------------------------------- +% Perform the colamd ordering: +%------------------------------------------------------------------------------- + +if (nargout <= 1 & nargin == 1) %#ok + p = colamd2mex (S) ; +elseif (nargout <= 1 & nargin == 2) %#ok + p = colamd2mex (S, knobs) ; +elseif (nargout == 2 & nargin == 1) %#ok + [p, stats] = colamd2mex (S) ; +elseif (nargout == 2 & nargin == 2) %#ok + [p, stats] = colamd2mex (S, knobs) ; +else + error ('colamd: incorrect number of input and/or output arguments') ; +end + +%------------------------------------------------------------------------------- +% column elimination tree post-ordering: +%------------------------------------------------------------------------------- + +[ignore, q] = etree (S (:,p), 'col') ; +p = p (q) ; + +% stats is an optional 20-element output vector that provides data about the +% ordering and the validity of the input matrix S. Ordering statistics are +% in stats (1:3). stats (1) and stats (2) are the number of dense or empty +% rows and columns ignored by COLAMD and stats (3) is the number of +% garbage collections performed on the internal data structure used by +% COLAMD (roughly of size 2.2*nnz(S) + 4*m + 7*n integers). +% +% MATLAB built-in functions are intended to generate valid sparse matrices, +% with no duplicate entries, with ascending row indices of the nonzeros +% in each column, with a non-negative number of entries in each column (!) +% and so on. If a matrix is invalid, then COLAMD may or may not be able +% to continue. If there are duplicate entries (a row index appears two or +% more times in the same column) or if the row indices in a column are out +% of order, then COLAMD can correct these errors by ignoring the duplicate +% entries and sorting each column of its internal copy of the matrix S (the +% input matrix S is not repaired, however). If a matrix is invalid in other +% ways then COLAMD cannot continue, an error message is printed, and no +% output arguments (P or stats) are returned. COLAMD is thus a simple way +% to check a sparse matrix to see if it's valid. +% +% stats (4:7) provide information if COLAMD was able to continue. The +% matrix is OK if stats (4) is zero, or 1 if invalid. stats (5) is the +% rightmost column index that is unsorted or contains duplicate entries, +% or zero if no such column exists. stats (6) is the last seen duplicate +% or out-of-order row index in the column index given by stats (5), or zero +% if no such row index exists. stats (7) is the number of duplicate or +% out-of-order row indices. +% +% stats (8:20) is always zero in the current version of COLAMD (reserved +% for future use). + diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamd_demo.m b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamd_demo.m new file mode 100644 index 0000000..47176f4 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamd_demo.m @@ -0,0 +1,178 @@ +%COLAMD_DEMO demo for colamd, column approx minimum degree ordering algorithm +% +% Example: +% colamd_demo +% +% The following m-files and mexFunctions provide alternative sparse matrix +% ordering methods for MATLAB. They are typically faster (sometimes much +% faster) and typically provide better orderings than their MATLAB counterparts: +% +% colamd a replacement for colmmd. +% +% Typical usage: p = colamd (A) ; +% +% symamd a replacement for symmmd. Based on colamd. +% +% Typical usage: p = symamd (A) ; +% +% For a description of the methods used, see the colamd.c file. +% http://www.suitesparse.com +% +% See also colamd, symamd + +% Minor changes: in MATLAB 7, symmmd and colmmd are flagged as "obsolete". +% This demo checks if they exist, so it should still work when they are removed. + +% Copyright 1998-2007, Timothy A. Davis, and Stefan Larimore +% Developed in collaboration with J. Gilbert and E. Ng. + +%------------------------------------------------------------------------------- +% Print the introduction, the help info, and compile the mexFunctions +%------------------------------------------------------------------------------- + +fprintf (1, '\n-----------------------------------------------------------\n') ; +fprintf (1, 'Colamd2/symamd2 demo.') ; +fprintf (1, '\n-----------------------------------------------------------\n') ; +help colamd_demo ; + +fprintf (1, '\n-----------------------------------------------------------\n') ; +fprintf (1, 'Colamd help information:') ; +fprintf (1, '\n-----------------------------------------------------------\n') ; +help colamd2 ; + +fprintf (1, '\n-----------------------------------------------------------\n') ; +fprintf (1, 'Symamd help information:') ; +fprintf (1, '\n-----------------------------------------------------------\n') ; +help symamd2 ; + +%------------------------------------------------------------------------------- +% Solving Ax=b +%------------------------------------------------------------------------------- + +n = 100 ; +fprintf (1, '\n-----------------------------------------------------------\n') ; +fprintf (1, 'Solving Ax=b for a small %d-by-%d random matrix:', n, n) ; +fprintf (1, '\n-----------------------------------------------------------\n') ; +fprintf (1, '\nNote: Random sparse matrices are AWFUL test cases.\n') ; +fprintf (1, 'They''re just easy to generate in a demo.\n') ; + +% set up the system + +rand ('state', 0) ; +randn ('state', 0) ; +spparms ('default') ; +A = sprandn (n, n, 5/n) + speye (n) ; +b = (1:n)' ; + +fprintf (1, '\n\nSolving via lu (PAQ = LU), where Q is from colamd2:\n') ; +q = colamd2 (A) ; +I = speye (n) ; +Q = I (:, q) ; +[L,U,P] = lu (A*Q) ; +fl = luflops (L, U) ; +x = Q * (U \ (L \ (P * b))) ; +fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ; +fprintf (1, 'residual: %e\n', norm (A*x-b)); + +try + fprintf (1, '\n\nSolving via lu (PAQ = LU), where Q is from colmmd:\n') ; + q = colmmd (A) ; + I = speye (n) ; + Q = I (:, q) ; + [L,U,P] = lu (A*Q) ; + fl = luflops (L, U) ; + x = Q * (U \ (L \ (P * b))) ; + fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ; + fprintf (1, 'residual: %e\n', ... + norm (A*x-b)) ; +catch + fprintf (1, 'colmmd is obsolete; test skipped\n') ; +end + +fprintf (1, '\n\nSolving via lu (PA = LU), without regard for sparsity:\n') ; +[L,U,P] = lu (A) ; +fl = luflops (L, U) ; +x = U \ (L \ (P * b)) ; +fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ; +fprintf (1, 'residual: %e\n', norm (A*x-b)); + +%------------------------------------------------------------------------------- +% Large demo for colamd2 +%------------------------------------------------------------------------------- + +fprintf (1, '\n-----------------------------------------------------------\n') ; +fprintf (1, 'Large demo for colamd2 (symbolic analysis only):') ; +fprintf (1, '\n-----------------------------------------------------------\n') ; + +rand ('state', 0) ; +randn ('state', 0) ; +spparms ('default') ; +n = 1000 ; +fprintf (1, 'Generating a random %d-by-%d sparse matrix.\n', n, n) ; +A = sprandn (n, n, 5/n) + speye (n) ; + +fprintf (1, '\n\nUnordered matrix:\n') ; +lnz = symbfact (A, 'col') ; +fprintf (1, 'nz in Cholesky factors of A''A: %d\n', sum (lnz)) ; +fprintf (1, 'flop count for Cholesky of A''A: %d\n', sum (lnz.^2)) ; + +tic ; +p = colamd2 (A) ; +t = toc ; +lnz = symbfact (A (:,p), 'col') ; +fprintf (1, '\n\nColamd run time: %f\n', t) ; +fprintf (1, 'colamd2 ordering quality: \n') ; +fprintf (1, 'nz in Cholesky factors of A(:,p)''A(:,p): %d\n', sum (lnz)) ; +fprintf (1, 'flop count for Cholesky of A(:,p)''A(:,p): %d\n', sum (lnz.^2)) ; + +try + tic ; + p = colmmd (A) ; + t = toc ; + lnz = symbfact (A (:,p), 'col') ; + fprintf (1, '\n\nColmmd run time: %f\n', t) ; + fprintf (1, 'colmmd ordering quality: \n') ; + fprintf (1, 'nz in Cholesky factors of A(:,p)''A(:,p): %d\n', sum (lnz)) ; + fprintf (1, 'flop count for Cholesky of A(:,p)''A(:,p): %d\n', ... + sum (lnz.^2)) ; +catch + fprintf (1, 'colmmd is obsolete; test skipped\n') ; +end + +%------------------------------------------------------------------------------- +% Large demo for symamd2 +%------------------------------------------------------------------------------- + +fprintf (1, '\n-----------------------------------------------------------\n') ; +fprintf (1, 'Large demo for symamd2 (symbolic analysis only):') ; +fprintf (1, '\n-----------------------------------------------------------\n') ; + +fprintf (1, 'Generating a random symmetric %d-by-%d sparse matrix.\n', n, n) ; +A = A+A' ; + +fprintf (1, '\n\nUnordered matrix:\n') ; +lnz = symbfact (A, 'sym') ; +fprintf (1, 'nz in Cholesky factors of A: %d\n', sum (lnz)) ; +fprintf (1, 'flop count for Cholesky of A: %d\n', sum (lnz.^2)) ; + +tic ; +p = symamd2 (A) ; +t = toc ; +lnz = symbfact (A (p,p), 'sym') ; +fprintf (1, '\n\nSymamd run time: %f\n', t) ; +fprintf (1, 'symamd2 ordering quality: \n') ; +fprintf (1, 'nz in Cholesky factors of A(p,p): %d\n', sum (lnz)) ; +fprintf (1, 'flop count for Cholesky of A(p,p): %d\n', sum (lnz.^2)) ; + +try + tic ; + p = symmmd (A) ; + t = toc ; + lnz = symbfact (A (p,p), 'sym') ; + fprintf (1, '\n\nSymmmd run time: %f\n', t) ; + fprintf (1, 'symmmd ordering quality: \n') ; + fprintf (1, 'nz in Cholesky factors of A(p,p): %d\n', sum (lnz)) ; + fprintf (1, 'flop count for Cholesky of A(p,p): %d\n', sum (lnz.^2)) ; +catch + fprintf (1, 'symmmd is obsolete\n') ; +end diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamd_install.m b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamd_install.m new file mode 100644 index 0000000..7ac0110 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamd_install.m @@ -0,0 +1,18 @@ +function colamd_install +%COLAMD_MAKE to compile and install the colamd2 and symamd2 mexFunction. +% Your current directory must be COLAMD/MATLAB for this function to work. +% +% Example: +% colamd_install +% +% See also colamd2, symamd2. + +% Copyright 1998-2007, Timothy A. Davis, and Stefan Larimore +% Developed in collaboration with J. Gilbert and E. Ng. + +colamd_make +addpath (pwd) +fprintf ('\nThe following path has been added. You may wish to add it\n') ; +fprintf ('permanently, using the MATLAB pathtool command.\n') ; +fprintf ('%s\n\n', pwd) ; +colamd_demo diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamd_make.m b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamd_make.m new file mode 100644 index 0000000..950a54e --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamd_make.m @@ -0,0 +1,30 @@ +function colamd_make +%COLAMD_MAKE compiles COLAMD2 and SYMAMD2 for MATLAB +% +% Example: +% colamd_make +% +% See also colamd, symamd + +% Copyright 1998-2007, Timothy A. Davis, and Stefan Larimore +% Developed in collaboration with J. Gilbert and E. Ng. + +details = 0 ; % 1 if details of each command are to be printed +d = '' ; +if (~isempty (strfind (computer, '64'))) + d = '-largeArrayDims' ; +end +src = '../Source/colamd.c ../Source/colamd_global.c' ; +cmd = sprintf ( ... + 'mex -DDLONG -O %s -I../../SuiteSparse_config -I../Include -output ', d) ; +s = [cmd 'colamd2mex colamdmex.c ' src] ; +if (details) + fprintf ('%s\n', s) ; +end +eval (s) ; +s = [cmd 'symamd2mex symamdmex.c ' src] ; +if (details) + fprintf ('%s\n', s) ; +end +eval (s) ; +fprintf ('COLAMD2 and SYMAMD2 successfully compiled.\n') ; diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamd_test.m b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamd_test.m new file mode 100644 index 0000000..21f3950 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamd_test.m @@ -0,0 +1,506 @@ +function colamd_test +%COLAMD_TEST test colamd2 and symamd2 +% Example: +% colamd_test +% +% COLAMD and SYMAMD testing function. Here we try to give colamd2 and symamd2 +% every possible type of matrix and erroneous input that they may encounter. +% We want either a valid permutation returned or we want them to fail +% gracefully. +% +% You are prompted as to whether or not the colamd2 and symand routines and +% the test mexFunctions are to be compiled. +% +% See also colamd2, symamd2 + +% Copyright 1998-2007, Timothy A. Davis, and Stefan Larimore +% Developed in collaboration with J. Gilbert and E. Ng. + + +help colamd_test + + + fprintf ('Compiling colamd2, symamd2, and test mexFunctions.\n') ; + colamd_make ; + + d = '' ; + if (~isempty (strfind (computer, '64'))) + d = '-largeArrayDims' ; + end + cmd = sprintf (... + 'mex -DDLONG -O %s -I../../SuiteSparse_config -I../Include ', d) ; + src = '../Source/colamd.c ../Source/colamd_global.c' ; + eval ([cmd 'colamdtestmex.c ' src]) ; + eval ([cmd 'symamdtestmex.c ' src]) ; + fprintf ('Done compiling.\n') ; + + +fprintf ('\nThe following codes will be tested:\n') ; +which colamd2 +which symamd2 +which colamd2mex +which symamd2mex +which colamdtestmex +which symamdtestmex + +fprintf ('\nStarting the tests. Please be patient.\n') ; + +h = waitbar (0, 'COLAMD test') ; + +rand ('state', 0) ; +randn ('state', 0) ; + +A = sprandn (500,500,0.4) ; + +p = colamd2 (A, [10 10 1]) ; check_perm (p, A) ; +p = colamd2 (A, [2 7 1]) ; check_perm (p, A) ; +p = symamd2 (A, [10 1]) ; check_perm (p, A) ; +p = symamd2 (A, [7 1]) ; check_perm (p, A) ; +p = symamd2 (A, [4 1]) ; check_perm (p, A) ; + + +fprintf ('Null matrices') ; +A = zeros (0,0) ; +A = sparse (A) ; + +[p, stats] = colamd2 (A, [10 10 0]) ; %#ok +check_perm (p, A) ; + +[p, stats] = symamd2 (A, [10 0]) ; %#ok +check_perm (p, A) ; + +A = zeros (0, 100) ; +A = sparse (A) ; +[p, stats] = colamd2 (A, [10 10 0]) ; %#ok +check_perm (p, A) ; + +A = zeros (100, 0) ; +A = sparse (A) ; +[p, stats] = colamd2 (A, [10 10 0]) ; +check_perm (p, A) ; +fprintf (' OK\n') ; + + +fprintf ('Matrices with a few dense row/cols\n') ; + +for trial = 1:20 + + waitbar (trial/20, h, 'COLAMD: with dense rows/cols') ; + + % random square unsymmetric matrix + A = rand_matrix (1000, 1000, 1, 10, 20) ; + + for tol = [0:.1:2 3:20 1e6] + + [p, stats] = colamd2 (A, [tol tol 0]) ; %#ok + check_perm (p, A) ; + + B = A + A' ; + [p, stats] = symamd2 (B, [tol 0]) ; %#ok + check_perm (p, A) ; + + [p, stats] = colamd2 (A, [tol 1 0]) ; %#ok + check_perm (p, A) ; + + [p, stats] = colamd2 (A, [1 tol 0]) ; %#ok + check_perm (p, A) ; + end +end +fprintf (' OK\n') ; + +fprintf ('General matrices\n') ; +for trial = 1:400 + + waitbar (trial/400, h, 'COLAMD: general') ; + + % matrix of random mtype + mtype = irand (3) ; + A = rand_matrix (2000, 2000, mtype, 0, 0) ; + p = colamd2 (A) ; + check_perm (p, A) ; + if (mtype == 3) + p = symamd2 (A) ; + check_perm (p, A) ; + end + +end +fprintf (' OK\n') ; + +fprintf ('Test error handling with invalid inputs\n') ; + +% Check different erroneous input. +for trial = 1:30 + + waitbar (trial/30, h, 'COLAMD: error handling') ; + + A = rand_matrix (1000, 1000, 2, 0, 0) ; + [m n] = size (A) ; + + for err = 1:13 + + p = Tcolamd (A, [n n 0 0 err]) ; + if (p ~= -1) %#ok + check_perm (p, A) ; + end + + if (err == 1) + % check different (valid) input args to colamd2 + p = Acolamd (A) ; + + p2 = Acolamd (A, [10 10 0 0 0]) ; + if (any (p ~= p2)) + error ('colamd2: mismatch 1!') ; + end + [p2 stats] = Acolamd (A) ; %#ok + if (any (p ~= p2)) + error ('colamd2: mismatch 2!') ; + end + [p2 stats] = Acolamd (A, [10 10 0 0 0]) ; + if (any (p ~= p2)) + error ('colamd2: mismatch 3!') ; + end + end + + B = A'*A ; + p = Tsymamd (B, [n 0 err]) ; + if (p ~= -1) %#ok + check_perm (p, A) ; + end + + if (err == 1) + + % check different (valid) input args to symamd2 + p = Asymamd (B) ; + check_perm (p, A) ; + p2 = Asymamd (B, [10 0 0]) ; + if (any (p ~= p2)) + error ('symamd2: mismatch 1!') ; + end + [p2 stats] = Asymamd (B) ; %#ok + if (any (p ~= p2)) + error ('symamd2: mismatch 2!') ; + end + [p2 stats] = Asymamd (B, [10 0 0]) ; %#ok + if (any (p ~= p2)) + error ('symamd2: mismatch 3!') ; + end + end + + end + +end +fprintf (' OK\n') ; + +fprintf ('Matrices with a few empty columns\n') ; + +for trial = 1:400 + + % some are square, some are rectangular + n = 0 ; + while (n < 5) + A = rand_matrix (1000, 1000, irand (2), 0, 0) ; + [m n] = size (A) ; + end + + % Add 5 null columns at random locations. + null_col = randperm (n) ; + null_col = sort (null_col (1:5)) ; + A (:, null_col) = 0 ; + + % Order the matrix and make sure that the null columns are ordered last. + [p, stats] = colamd2 (A, [1e6 1e6 0]) ; + check_perm (p, A) ; + +% if (stats (2) ~= 5) +% stats (2) +% error ('colamd2: wrong number of null columns') ; +% end + + % find all null columns in A + null_col = find (sum (spones (A), 1) == 0) ; + nnull = length (null_col) ; %#ok + if (any (null_col ~= p ((n-4):n))) + error ('colamd2: Null cols are not ordered last in natural order') ; + end + + +end +fprintf (' OK\n') ; + +fprintf ('Matrices with a few empty rows and columns\n') ; + +for trial = 1:400 + + waitbar (trial/400, h, 'COLAMD: with empty rows/cols') ; + + % symmetric matrices + n = 0 ; + while (n < 5) + A = rand_matrix (1000, 1000, 3, 0, 0) ; + [m n] = size (A) ; + end + + % Add 5 null columns and rows at random locations. + null_col = randperm (n) ; + null_col = sort (null_col (1:5)) ; + A (:, null_col) = 0 ; + A (null_col, :) = 0 ; + + % Order the matrix and make sure that the null rows/cols are ordered last. + [p,stats] = symamd2 (A, [10 0]) ; + check_perm (p, A) ; + + % find actual number of null rows and columns + Alo = tril (A, -1) ; + nnull = length (find (sum (Alo') == 0 & sum (Alo) == 0)) ; %#ok + + if (stats (2) ~= nnull | nnull < 5) %#ok + error ('symamd2: wrong number of null columns') ; + end + if (any (null_col ~= p ((n-4):n))) + error ('symamd2: Null cols are not ordered last in natural order') ; + end + +end +fprintf (' OK\n') ; + +fprintf ('Matrices with a few empty rows\n') ; + +% Test matrices with null rows inserted. + +for trial = 1:400 + + waitbar (trial/400, h, 'COLAMD: with null rows') ; + + m = 0 ; + while (m < 5) + A = rand_matrix (1000, 1000, 2, 0, 0) ; + [m n] = size (A) ; %#ok + end + + % Add 5 null rows at random locations. + null_row = randperm (m) ; + null_row = sort (null_row (1:5)) ; + A (null_row, :) = 0 ; + + p = colamd2 (A, [10 10 0]) ; + check_perm (p, A) ; + if (stats (1) ~= 5) + error ('colamd2: wrong number of null rows') ; + end + +end +fprintf (' OK\n') ; + + +fprintf ('\ncolamd2 and symamd2: all tests passed\n\n') ; +close (h) ; + +%------------------------------------------------------------------------------- + +function [p,stats] = Acolamd (S, knobs) +% Acolamd: compare colamd2 and Tcolamd results + +if (nargin < 3) + if (nargout == 1) + [p] = colamd2 (S) ; + [p1] = Tcolamd (S, [10 10 0 0 0]) ; + else + [p, stats] = colamd2 (S) ; + [p1, stats1] = Tcolamd (S, [10 10 0 0 0]) ; %#ok + end +else + if (nargout == 1) + [p] = colamd2 (S, knobs (1:3)) ; + [p1] = Tcolamd (S, knobs) ; + else + [p, stats] = colamd2 (S, knobs (1:3)) ; + [p1, stats1] = Tcolamd (S, knobs) ; %#ok + end +end + +check_perm (p, S) ; +check_perm (p1, S) ; + +if (any (p1 ~= p)) + error ('Acolamd mismatch!') ; +end + + +%------------------------------------------------------------------------------- + +function [p,stats] = Asymamd (S, knobs) +% Asymamd: compare symamd2 and Tsymamd results + +if (nargin < 3) + if (nargout == 1) + [p] = symamd2 (S) ; + [p1] = Tsymamd (S, [10 0 0]) ; + else + [p, stats] = symamd2 (S) ; + [p1, stats1] = Tsymamd (S, [10 0 0]) ; %#ok + end +else + if (nargout == 1) + [p] = symamd2 (S, knobs (1:2)) ; + [p1] = Tsymamd (S, knobs) ; + else + [p, stats] = symamd2 (S, knobs (1:2)) ; + [p1, stats1] = Tsymamd (S, knobs) ; %#ok + end +end + +if (any (p1 ~= p)) + error ('Asymamd mismatch!') ; +end + + +%------------------------------------------------------------------------------- + +function check_perm (p, A) +% check_perm: check for a valid permutation vector + +if (isempty (A) & isempty (p)) %#ok + % empty permutation vectors of empty matrices are OK + return +end + +if (isempty (p)) + error ('bad permutation: cannot be empty') ; +end + +[m n] = size (A) ; +[pm pn] = size (p) ; +if (pn == 1) + % force p to be a row vector + p = p' ; + [pm pn] = size (p) ; +end + +if (n ~= pn) + error ('bad permutation: wrong size') ; +end + +if (pm ~= 1) ; + % p must be a vector + error ('bad permutation: not a vector') ; +else + if (any (sort (p) - (1:pn))) + error ('bad permutation') ; + end +end + +%------------------------------------------------------------------------------- + +function i = irand (n) +% irand: return a random integer between 1 and n +i = min (n, 1 + floor (rand * n)) ; + +%------------------------------------------------------------------------------- + +function A = rand_matrix (nmax, mmax, mtype, drows, dcols) +% rand_matrix: return a random sparse matrix +% +% A = rand_matrix (nmax, mmax, mtype, drows, dcols) +% +% A binary matrix of random size, at most nmax-by-mmax, with drows dense rows +% and dcols dense columns. +% +% mtype 1: square unsymmetric (mmax is ignored) +% mtype 2: rectangular +% mtype 3: symmetric (mmax is ignored) + +n = irand (nmax) ; +if (mtype ~= 2) + % square + m = n ; +else + m = irand (mmax) ; +end + +A = sprand (m, n, 10 / max (m,n)) ; + +if (drows > 0) + % add dense rows + for k = 1:drows + i = irand (m) ; + nz = irand (n) ; + p = randperm (n) ; + p = p (1:nz) ; + A (i,p) = 1 ; + end +end + +if (dcols > 0) + % add dense cols + for k = 1:dcols + j = irand (n) ; + nz = irand (m) ; + p = randperm (m) ; + p = p (1:nz) ; + A (p,j) = 1 ; + end +end + +A = spones (A) ; + +% ensure that there are no empty columns +d = find (full (sum (A)) == 0) ; %#ok +A (m,d) = 1 ; %#ok + +% ensure that there are no empty rows +d = find (full (sum (A,2)) == 0) ; %#ok +A (d,n) = 1 ; %#ok + +if (mtype == 3) + % symmetric + A = A + A' + speye (n) ; +end + +A = spones (A) ; + +%------------------------------------------------------------------------------- + +function [p,stats] = Tcolamd (S, knobs) +% Tcolamd: run colamd2 in a testing mode + +if (nargout <= 1 & nargin == 1) %#ok + p = colamdtestmex (S) ; +elseif (nargout <= 1 & nargin == 2) %#ok + p = colamdtestmex (S, knobs) ; +elseif (nargout == 2 & nargin == 1) %#ok + [p, stats] = colamdtestmex (S) ; +elseif (nargout == 2 & nargin == 2) %#ok + [p, stats] = colamdtestmex (S, knobs) ; +else + error ('colamd2: incorrect number of input and/or output arguments') ; +end + +if (p (1) ~= -1) + [ignore, q] = etree (S (:,p), 'col') ; + p = p (q) ; + check_perm (p, S) ; +end + +%------------------------------------------------------------------------------- + +function [p, stats] = Tsymamd (S, knobs) +% Tsymamd: run symamd2 in a testing mode + +if (nargout <= 1 & nargin == 1) %#ok + p = symamdtestmex (S) ; +elseif (nargout <= 1 & nargin == 2) %#ok + p = symamdtestmex (S, knobs) ; +elseif (nargout == 2 & nargin == 1) %#ok + [p, stats] = symamdtestmex (S) ; +elseif (nargout == 2 & nargin == 2) %#ok + [p, stats] = symamdtestmex (S, knobs) ; +else + error ('symamd2: incorrect number of input and/or output arguments') ; +end + +if (p (1) ~= -1) + [ignore, q] = etree (S (p,p)) ; + p = p (q) ; + check_perm (p, S) ; +end diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamdmex.c b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamdmex.c new file mode 100644 index 0000000..d804cc1 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamdmex.c @@ -0,0 +1,212 @@ +/* ========================================================================== */ +/* === colamd mexFunction =================================================== */ +/* ========================================================================== */ + +/* Usage: + + P = colamd2 (A) ; + [ P, stats ] = colamd2 (A, knobs) ; + + see colamd.m for a description. + + Authors: + + The authors of the code itself are Stefan I. Larimore and Timothy A. + Davis (DrTimothyAldenDavis@gmail.com). The algorithm was + developed in collaboration with John Gilbert, Xerox PARC, and Esmond + Ng, Oak Ridge National Laboratory. + + Acknowledgements: + + This work was supported by the National Science Foundation, under + grants DMS-9504974 and DMS-9803599. + + Notice: + + Copyright (c) 1998-2007, Timothy A. Davis, All Rights Reserved. + + Availability: + + The colamd/symamd library is available at http://www.suitesparse.com + +*/ + +/* ========================================================================== */ +/* === Include files ======================================================== */ +/* ========================================================================== */ + +#include "colamd.h" +#include "mex.h" +#include "matrix.h" +#include +#include +#define Long SuiteSparse_long + +/* ========================================================================== */ +/* === colamd mexFunction =================================================== */ +/* ========================================================================== */ + +void mexFunction +( + /* === Parameters ======================================================= */ + + int nlhs, /* number of left-hand sides */ + mxArray *plhs [], /* left-hand side matrices */ + int nrhs, /* number of right--hand sides */ + const mxArray *prhs [] /* right-hand side matrices */ +) +{ + /* === Local variables ================================================== */ + + Long *A ; /* colamd's copy of the matrix, and workspace */ + Long *p ; /* colamd's copy of the column pointers */ + Long Alen ; /* size of A */ + Long n_col ; /* number of columns of A */ + Long n_row ; /* number of rows of A */ + Long nnz ; /* number of entries in A */ + Long full ; /* TRUE if input matrix full, FALSE if sparse */ + double knobs [COLAMD_KNOBS] ; /* colamd user-controllable parameters */ + double *out_perm ; /* output permutation vector */ + double *out_stats ; /* output stats vector */ + double *in_knobs ; /* input knobs vector */ + Long i ; /* loop counter */ + mxArray *Ainput ; /* input matrix handle */ + Long spumoni ; /* verbosity variable */ + Long stats [COLAMD_STATS] ; /* stats for colamd */ + + colamd_printf = mexPrintf ; /* COLAMD printf routine */ + + /* === Check inputs ===================================================== */ + + if (nrhs < 1 || nrhs > 2 || nlhs < 0 || nlhs > 2) + { + mexErrMsgTxt ( + "colamd: incorrect number of input and/or output arguments") ; + } + + /* === Get knobs ======================================================== */ + + colamd_l_set_defaults (knobs) ; + spumoni = 0 ; + + /* check for user-passed knobs */ + if (nrhs == 2) + { + in_knobs = mxGetPr (prhs [1]) ; + i = mxGetNumberOfElements (prhs [1]) ; + if (i > 0) knobs [COLAMD_DENSE_ROW] = in_knobs [0] ; + if (i > 1) knobs [COLAMD_DENSE_COL] = in_knobs [1] ; + if (i > 2) spumoni = (Long) (in_knobs [2] != 0) ; + } + + /* print knob settings if spumoni is set */ + if (spumoni) + { + mexPrintf ("\ncolamd version %d.%d, %s:\n", + COLAMD_MAIN_VERSION, COLAMD_SUB_VERSION, COLAMD_DATE) ; + if (knobs [COLAMD_DENSE_ROW] >= 0) + { + mexPrintf ("knobs(1): %g, rows with > max(16,%g*sqrt(size(A,2)))" + " entries removed\n", in_knobs [0], knobs [COLAMD_DENSE_ROW]) ; + } + else + { + mexPrintf ("knobs(1): %g, only completely dense rows removed\n", + in_knobs [0]) ; + } + if (knobs [COLAMD_DENSE_COL] >= 0) + { + mexPrintf ("knobs(2): %g, cols with > max(16,%g*sqrt(min(size(A)))" + " entries removed\n", in_knobs [1], knobs [COLAMD_DENSE_COL]) ; + } + else + { + mexPrintf ("knobs(2): %g, only completely dense columns removed\n", + in_knobs [1]) ; + } + mexPrintf ("knobs(3): %g, statistics and knobs printed\n", + in_knobs [2]) ; + } + + /* === If A is full, convert to a sparse matrix ========================= */ + + Ainput = (mxArray *) prhs [0] ; + if (mxGetNumberOfDimensions (Ainput) != 2) + { + mexErrMsgTxt ("colamd: input matrix must be 2-dimensional") ; + } + full = !mxIsSparse (Ainput) ; + if (full) + { + mexCallMATLAB (1, &Ainput, 1, (mxArray **) prhs, "sparse") ; + } + + /* === Allocate workspace for colamd ==================================== */ + + /* get size of matrix */ + n_row = mxGetM (Ainput) ; + n_col = mxGetN (Ainput) ; + + /* get column pointer vector so we can find nnz */ + p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; + (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ; + nnz = p [n_col] ; + Alen = (Long) colamd_l_recommended (nnz, n_row, n_col) ; + if (Alen == 0) + { + mexErrMsgTxt ("colamd: problem too large") ; + } + + /* === Copy input matrix into workspace ================================= */ + + A = (Long *) mxCalloc (Alen, sizeof (Long)) ; + (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ; + + if (full) + { + mxDestroyArray (Ainput) ; + } + + /* === Order the columns (destroys A) =================================== */ + + if (!colamd_l (n_row, n_col, Alen, A, p, knobs, stats)) + { + colamd_l_report (stats) ; + mexErrMsgTxt ("colamd error!") ; + } + mxFree (A) ; + + /* === Return the permutation vector ==================================== */ + + plhs [0] = mxCreateDoubleMatrix (1, n_col, mxREAL) ; + out_perm = mxGetPr (plhs [0]) ; + for (i = 0 ; i < n_col ; i++) + { + /* colamd is 0-based, but MATLAB expects this to be 1-based */ + out_perm [i] = p [i] + 1 ; + } + mxFree (p) ; + + /* === Return the stats vector ========================================== */ + + /* print stats if spumoni is set */ + if (spumoni) + { + colamd_l_report (stats) ; + } + + if (nlhs == 2) + { + plhs [1] = mxCreateDoubleMatrix (1, COLAMD_STATS, mxREAL) ; + out_stats = mxGetPr (plhs [1]) ; + for (i = 0 ; i < COLAMD_STATS ; i++) + { + out_stats [i] = stats [i] ; + } + + /* fix stats (5) and (6), for 1-based information on jumbled matrix. */ + /* note that this correction doesn't occur if symamd returns FALSE */ + out_stats [COLAMD_INFO1] ++ ; + out_stats [COLAMD_INFO2] ++ ; + } +} diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamdtestmex.c b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamdtestmex.c new file mode 100644 index 0000000..b1f1f2a --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/colamdtestmex.c @@ -0,0 +1,569 @@ +/* ========================================================================== */ +/* === colamdtest mexFunction =============================================== */ +/* ========================================================================== */ + +/* COLAMD test function + + This MATLAB mexFunction is for testing only. It is not meant for + production use. See colamdmex.c instead. + + Usage: + + [ P, stats ] = colamdtest (A, knobs) ; + + See colamd.m for a description. knobs is required. + + knobs (1) dense row control + knobs (2) dense column control + knobs (3) spumoni + knobs (4) for testing only. Controls the workspace used by + colamd. + + knobs (5) for testing only. Controls how the input matrix is + jumbled prior to calling colamd, to test its error + handling capability. + + Authors: + + The authors of the code itself are Stefan I. Larimore and Timothy A. + Davis (DrTimothyAldenDavis@gmail.com). The algorithm was + developed in collaboration with John Gilbert, Xerox PARC, and Esmond + Ng, Oak Ridge National Laboratory. + + Acknowledgements: + + This work was supported by the National Science Foundation, under + grants DMS-9504974 and DMS-9803599. + + Notice: + + Copyright (c) 1998-2007, Timothy A. Davis, All Rights Reserved. + See the colamd.c file for the License. + + Availability: + + The colamd/symamd library is available at http://www.suitesparse.com + +*/ + +/* ========================================================================== */ +/* === Include files ======================================================== */ +/* ========================================================================== */ + +#include "colamd.h" +#include "mex.h" +#include "matrix.h" +#include +#include +#define Long SuiteSparse_long + +static void dump_matrix +( + Long A [ ], + Long p [ ], + Long n_row, + Long n_col, + Long Alen, + Long limit +) ; + +/* ========================================================================== */ +/* === colamd mexFunction =================================================== */ +/* ========================================================================== */ + +void mexFunction +( + /* === Parameters ======================================================= */ + + int nlhs, /* number of left-hand sides */ + mxArray *plhs [], /* left-hand side matrices */ + int nrhs, /* number of right--hand sides */ + const mxArray *prhs [] /* right-hand side matrices */ +) +{ + /* === Local variables ================================================== */ + + Long *A ; /* colamd's copy of the matrix, and workspace */ + Long *p ; /* colamd's copy of the column pointers */ + Long Alen ; /* size of A */ + Long n_col ; /* number of columns of A */ + Long n_row ; /* number of rows of A */ + Long nnz ; /* number of entries in A */ + Long full ; /* TRUE if input matrix full, FALSE if sparse */ + double knobs [COLAMD_KNOBS] ; /* colamd user-controllable parameters */ + double *out_perm ; /* output permutation vector */ + double *out_stats ; /* output stats vector */ + double *in_knobs ; /* input knobs vector */ + Long i ; /* loop counter */ + mxArray *Ainput ; /* input matrix handle */ + Long spumoni ; /* verbosity variable */ + Long stats2 [COLAMD_STATS] ;/* stats for colamd */ + + Long *cp, *cp_end, result, col, length ; + Long *stats ; + stats = stats2 ; + + colamd_printf = mexPrintf ; /* COLAMD printf routine */ + + /* === Check inputs ===================================================== */ + + if (nrhs < 1 || nrhs > 2 || nlhs < 0 || nlhs > 2) + { + mexErrMsgTxt ( + "colamd: incorrect number of input and/or output arguments") ; + } + + if (nrhs != 2) + { + mexErrMsgTxt ("colamdtest: knobs are required") ; + } + /* for testing we require all 5 knobs */ + if (mxGetNumberOfElements (prhs [1]) != 5) + { + mexErrMsgTxt ("colamd: must have all 5 knobs for testing") ; + } + + /* === Get knobs ======================================================== */ + + colamd_l_set_defaults (knobs) ; + spumoni = 0 ; + + /* check for user-passed knobs */ + if (nrhs == 2) + { + in_knobs = mxGetPr (prhs [1]) ; + i = mxGetNumberOfElements (prhs [1]) ; + if (i > 0) knobs [COLAMD_DENSE_ROW] = in_knobs [0] ; + if (i > 1) knobs [COLAMD_DENSE_COL] = in_knobs [1] ; + if (i > 2) spumoni = (Long) in_knobs [2] ; + } + + /* print knob settings if spumoni is set */ + if (spumoni) + { + mexPrintf ("\ncolamd version %d.%d, %s:\n", + COLAMD_MAIN_VERSION, COLAMD_SUB_VERSION, COLAMD_DATE) ; + if (knobs [COLAMD_DENSE_ROW] >= 0) + { + mexPrintf ("knobs(1): %g, rows with > max(16,%g*sqrt(size(A,2)))" + " entries removed\n", in_knobs [0], knobs [COLAMD_DENSE_ROW]) ; + } + else + { + mexPrintf ("knobs(1): %g, only completely dense rows removed\n", + in_knobs [0]) ; + } + if (knobs [COLAMD_DENSE_COL] >= 0) + { + mexPrintf ("knobs(2): %g, cols with > max(16,%g*sqrt(min(size(A)))" + " entries removed\n", in_knobs [1], knobs [COLAMD_DENSE_COL]) ; + } + else + { + mexPrintf ("knobs(2): %g, only completely dense columns removed\n", + in_knobs [1]) ; + } + mexPrintf ("knobs(3): %g, statistics and knobs printed\n", + in_knobs [2]) ; + } + + /* === If A is full, convert to a sparse matrix ========================= */ + + Ainput = (mxArray *) prhs [0] ; + if (mxGetNumberOfDimensions (Ainput) != 2) + { + mexErrMsgTxt ("colamd: input matrix must be 2-dimensional") ; + } + full = !mxIsSparse (Ainput) ; + if (full) + { + mexCallMATLAB (1, &Ainput, 1, (mxArray **) prhs, "sparse") ; + } + + /* === Allocate workspace for colamd ==================================== */ + + /* get size of matrix */ + n_row = mxGetM (Ainput) ; + n_col = mxGetN (Ainput) ; + + /* get column pointer vector so we can find nnz */ + p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; + (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ; + nnz = p [n_col] ; + Alen = (Long) colamd_l_recommended (nnz, n_row, n_col) ; + if (Alen == 0) + { + mexErrMsgTxt ("colamd: problem too large") ; + } + + +/* === Modify size of Alen if testing ======================================= */ + +/* + knobs [3] amount of workspace given to colamd. + < 0 : TIGHT memory + > 0 : MIN + knob [3] - 1 + == 0 : RECOMMENDED memory +*/ + +/* Here only for testing */ +/* size of the Col and Row structures */ +#define COLAMD_C(n_col) (((n_col) + 1) * 24 / sizeof (Long)) +#define COLAMD_R(n_row) (((n_row) + 1) * 16 / sizeof (Long)) +#ifdef MIN +#undef MIN +#endif +#define MIN(a,b) (((a) < (b)) ? (a) : (b)) +#define COLAMD_MIN_MEMORY(nnz,n_row,n_col) \ + (2 * (nnz) + COLAMD_C (n_col) + COLAMD_R (n_row)) + + /* get knob [3], if negative */ + if (in_knobs [3] < 0) + { + Alen = COLAMD_MIN_MEMORY (nnz, n_row, n_col) + n_col ; + } + else if (in_knobs [3] > 0) + { + Alen = COLAMD_MIN_MEMORY (nnz, n_row, n_col) + in_knobs [3] - 1 ; + } + + /* otherwise, we use the recommended amount set above */ + + /* === Copy input matrix into workspace ================================= */ + + A = (Long *) mxCalloc (Alen, sizeof (Long)) ; + (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ; + + if (full) + { + mxDestroyArray (Ainput) ; + } + + +/* === Jumble matrix ======================================================== */ + +/* + knobs [4] FOR TESTING ONLY: Specifies how to jumble matrix + 0 : No jumbling + 1 : Make n_row less than zero + 2 : Make first pointer non-zero + 3 : Make column pointers not non-decreasing + 4 : Make a column pointer greater or equal to Alen + 5 : Make row indices not strictly increasing + 6 : Make a row index greater or equal to n_row + 7 : Set A = NULL + 8 : Set p = NULL + 9 : Repeat row index + 10: make row indices not sorted + 11: jumble columns massively (note this changes + the pattern of the matrix A.) + 12: Set stats = NULL + 13: Make n_col less than zero +*/ + + /* jumble appropriately */ + switch ((Long) in_knobs [4]) + { + + case 0 : + if (spumoni > 0) + { + mexPrintf ("colamdtest: no errors expected\n") ; + } + result = 1 ; /* no errors */ + break ; + + case 1 : + if (spumoni > 0) + { + mexPrintf ("colamdtest: nrow out of range\n") ; + } + result = 0 ; /* nrow out of range */ + n_row = -1 ; + break ; + + case 2 : + if (spumoni > 0) + { + mexPrintf ("colamdtest: p [0] nonzero\n") ; + } + result = 0 ; /* p [0] must be zero */ + p [0] = 1 ; + break ; + + case 3 : + if (spumoni > 0) + { + mexPrintf ("colamdtest: negative length last column\n") ; + } + result = (n_col == 0) ; /* p must be monotonically inc. */ + p [n_col] = p [0] ; + break ; + + case 4 : + if (spumoni > 0) + { + mexPrintf ("colamdtest: Alen too small\n") ; + } + result = 0 ; /* out of memory */ + p [n_col] = Alen ; + break ; + + case 5 : + if (spumoni > 0) + { + mexPrintf ("colamdtest: row index out of range (-1)\n") ; + } + if (nnz > 0) /* row index out of range */ + { + result = 0 ; + A [nnz-1] = -1 ; + } + else + { + if (spumoni > 0) + { + mexPrintf ("Note: no row indices to put out of range\n") ; + } + result = 1 ; + } + break ; + + case 6 : + if (spumoni > 0) + { + mexPrintf ("colamdtest: row index out of range (n_row)\n") ; + } + if (nnz > 0) /* row index out of range */ + { + if (spumoni > 0) + { + mexPrintf ("Changing A[nnz-1] from %d to %d\n", + A [nnz-1], n_row) ; + } + result = 0 ; + A [nnz-1] = n_row ; + } + else + { + if (spumoni > 0) + { + mexPrintf ("Note: no row indices to put out of range\n") ; + } + result = 1 ; + } + break ; + + case 7 : + if (spumoni > 0) + { + mexPrintf ("colamdtest: A not present\n") ; + } + result = 0 ; /* A not present */ + A = (Long *) NULL ; + break ; + + case 8 : + if (spumoni > 0) + { + mexPrintf ("colamdtest: p not present\n") ; + } + result = 0 ; /* p not present */ + p = (Long *) NULL ; + break ; + + case 9 : + if (spumoni > 0) + { + mexPrintf ("colamdtest: duplicate row index\n") ; + } + result = 1 ; /* duplicate row index */ + + for (col = 0 ; col < n_col ; col++) + { + length = p [col+1] - p [col] ; + if (length > 1) + { + A [p [col]] = A [p [col] + 1] ; + if (spumoni > 0) + { + mexPrintf ("Made duplicate row %d in col %d\n", + A [p [col] + 1], col) ; + } + break ; + } + } + + if (spumoni > 1) + { + dump_matrix (A, p, n_row, n_col, Alen, col+2) ; + } + break ; + + case 10 : + if (spumoni > 0) + { + mexPrintf ("colamdtest: unsorted column\n") ; + } + result = 1 ; /* jumbled columns */ + + for (col = 0 ; col < n_col ; col++) + { + length = p [col+1] - p [col] ; + if (length > 1) + { + i = A[p [col]] ; + A [p [col]] = A[p [col] + 1] ; + A [p [col] + 1] = i ; + if (spumoni > 0) + { + mexPrintf ("Unsorted column %d \n", col) ; + } + break ; + } + } + + if (spumoni > 1) + { + dump_matrix (A, p, n_row, n_col, Alen, col+2) ; + } + break ; + + case 11 : + if (spumoni > 0) + { + mexPrintf ("colamdtest: massive jumbling\n") ; + } + result = 1 ; /* massive jumbling, but no errors */ + srand (1) ; + for (i = 0 ; i < n_col ; i++) + { + cp = &A [p [i]] ; + cp_end = &A [p [i+1]] ; + while (cp < cp_end) + { + *cp++ = rand() % n_row ; + } + } + if (spumoni > 1) + { + dump_matrix (A, p, n_row, n_col, Alen, n_col) ; + } + break ; + + case 12 : + if (spumoni > 0) + { + mexPrintf ("colamdtest: stats not present\n") ; + } + result = 0 ; /* stats not present */ + stats = (Long *) NULL ; + break ; + + case 13 : + if (spumoni > 0) + { + mexPrintf ("colamdtest: ncol out of range\n") ; + } + result = 0 ; /* ncol out of range */ + n_col = -1 ; + break ; + + } + + + /* === Order the columns (destroys A) =================================== */ + + if (!colamd_l (n_row, n_col, Alen, A, p, knobs, stats)) + { + + /* return p = -1 if colamd failed */ + plhs [0] = mxCreateDoubleMatrix (1, 1, mxREAL) ; + out_perm = mxGetPr (plhs [0]) ; + out_perm [0] = -1 ; + mxFree (p) ; + mxFree (A) ; + + if (spumoni > 0 || result) + { + colamd_l_report (stats) ; + } + + if (result) + { + mexErrMsgTxt ("colamd should have returned TRUE\n") ; + } + + return ; + /* mexErrMsgTxt ("colamd error!") ; */ + } + + if (!result) + { + colamd_l_report (stats) ; + mexErrMsgTxt ("colamd should have returned FALSE\n") ; + } + mxFree (A) ; + + /* === Return the permutation vector ==================================== */ + + plhs [0] = mxCreateDoubleMatrix (1, n_col, mxREAL) ; + out_perm = mxGetPr (plhs [0]) ; + for (i = 0 ; i < n_col ; i++) + { + /* colamd is 0-based, but MATLAB expects this to be 1-based */ + out_perm [i] = p [i] + 1 ; + } + mxFree (p) ; + + /* === Return the stats vector ========================================== */ + + /* print stats if spumoni > 0 */ + if (spumoni > 0) + { + colamd_l_report (stats) ; + } + + if (nlhs == 2) + { + plhs [1] = mxCreateDoubleMatrix (1, COLAMD_STATS, mxREAL) ; + out_stats = mxGetPr (plhs [1]) ; + for (i = 0 ; i < COLAMD_STATS ; i++) + { + out_stats [i] = stats [i] ; + } + + /* fix stats (5) and (6), for 1-based information on jumbled matrix. */ + /* note that this correction doesn't occur if symamd returns FALSE */ + out_stats [COLAMD_INFO1] ++ ; + out_stats [COLAMD_INFO2] ++ ; + } +} + + +static void dump_matrix +( + Long A [ ], + Long p [ ], + Long n_row, + Long n_col, + Long Alen, + Long limit +) +{ + Long col, k, row ; + + mexPrintf ("dump matrix: nrow %d ncol %d Alen %d\n", n_row, n_col, Alen) ; + + for (col = 0 ; col < MIN (n_col, limit) ; col++) + { + mexPrintf ("column %d, p[col] %d, p [col+1] %d, length %d\n", + col, p [col], p [col+1], p [col+1] - p [col]) ; + for (k = p [col] ; k < p [col+1] ; k++) + { + row = A [k] ; + mexPrintf (" %d", row) ; + } + mexPrintf ("\n") ; + } +} diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/luflops.m b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/luflops.m new file mode 100644 index 0000000..f3c9e45 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/luflops.m @@ -0,0 +1,34 @@ +function fl = luflops (L, U) +%LUFLOPS compute the flop count for sparse LU factorization +% +% Example: +% fl = luflops (L,U) +% +% Given a sparse LU factorization (L and U), return the flop count required +% by a conventional LU factorization algorithm to compute it. L and U can +% be either sparse or full matrices. L must be lower triangular and U must +% be upper triangular. Do not attempt to use this on the permuted L from +% [L,U] = lu (A). Instead, use [L,U,P] = lu (A) or [L,U,P,Q] = lu (A). +% +% Note that there is a subtle undercount in this estimate. Suppose A is +% completely dense, but during LU factorization exact cancellation occurs, +% causing some of the entries in L and U to become identically zero. The +% flop count returned by this routine is an undercount. There is a simple +% way to fix this (L = spones (L) + spones (tril (A))), but the fix is partial. +% It can also occur that some entry in L is a "symbolic" fill-in (zero in +% A, but a fill-in entry and thus must be computed), but numerically +% zero. The only way to get a reliable LU factorization would be to do a +% purely symbolic factorization of A. This cannot be done with +% symbfact (A, 'col'). +% +% See NA Digest, Vol 00, #50, Tuesday, Dec. 5, 2000 +% +% See also symbfact + +% Copyright 1998-2007, Timothy A. Davis + + +Lnz = full (sum (spones (L))) - 1 ; % off diagonal nz in cols of L +Unz = full (sum (spones (U')))' - 1 ; % off diagonal nz in rows of U +fl = 2*Lnz*Unz + sum (Lnz) ; + diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/symamd2.m b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/symamd2.m new file mode 100644 index 0000000..ecae450 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/symamd2.m @@ -0,0 +1,86 @@ +function [p, stats] = symamd2 (S, knobs) +%SYMAMD Symmetric approximate minimum degree permutation. +% P = SYMAMD2(S) for a symmetric positive definite matrix S, returns the +% permutation vector p such that S(p,p) tends to have a sparser Cholesky +% factor than S. Sometimes SYMAMD works well for symmetric indefinite +% matrices too. The matrix S is assumed to be symmetric; only the +% strictly lower triangular part is referenced. S must be square. +% Note that p = amd(S) is much faster and generates comparable orderings. +% The ordering is followed by an elimination tree post-ordering. +% +% Note that this function is source code for the built-in MATLAB symamd +% function. It has been renamed here to symamd2 to avoid a filename clash. +% symamd and symamd2 are identical. +% +% See also SYMAMD, AMD, COLAMD, COLAMD2. +% +% Example: +% P = symamd2 (S) +% [P, stats] = symamd2 (S, knobs) +% +% knobs is an optional one- to two-element input vector. If S is n-by-n, +% then rows and columns with more than max(16,knobs(1)*sqrt(n)) entries are +% removed prior to ordering, and ordered last in the output permutation P. +% No rows/columns are removed if knobs(1)<0. If knobs(2) is nonzero, stats +% and knobs are printed. The default is knobs = [10 0]. Note that knobs +% differs from earlier versions of symamd. + +% Copyright 1998-2007, Timothy A. Davis, and Stefan Larimore +% Developed in collaboration with J. Gilbert and E. Ng. +% Acknowledgements: This work was supported by the National Science +% Foundation, under grants DMS-9504974 and DMS-9803599. + +%------------------------------------------------------------------------------- +% perform the symamd ordering: +%------------------------------------------------------------------------------- + +if (nargout <= 1 & nargin == 1) %#ok + p = symamd2mex (S) ; +elseif (nargout <= 1 & nargin == 2) %#ok + p = symamd2mex (S, knobs) ; +elseif (nargout == 2 & nargin == 1) %#ok + [p, stats] = symamd2mex (S) ; +elseif (nargout == 2 & nargin == 2) %#ok + [p, stats] = symamd2mex (S, knobs) ; +else + error('symamd: incorrect number of input and/or output arguments.') ; +end + +%------------------------------------------------------------------------------- +% symmetric elimination tree post-ordering: +%------------------------------------------------------------------------------- + +[ignore, q] = etree (S (p,p)) ; +p = p (q) ; + + +% stats is an optional 20-element output vector that provides data about the +% ordering and the validity of the input matrix S. Ordering statistics are +% in stats (1:3). stats (1) = stats (2) is the number of dense or empty +% rows and columns ignored by SYMAMD and stats (3) is the number of +% garbage collections performed on the internal data structure used by +% SYMAMD (roughly of size 8.4*nnz(tril(S,-1)) + 9*n integers). +% +% MATLAB built-in functions are intended to generate valid sparse matrices, +% with no duplicate entries, with ascending row indices of the nonzeros +% in each column, with a non-negative number of entries in each column (!) +% and so on. If a matrix is invalid, then SYMAMD may or may not be able +% to continue. If there are duplicate entries (a row index appears two or +% more times in the same column) or if the row indices in a column are out +% of order, then SYMAMD can correct these errors by ignoring the duplicate +% entries and sorting each column of its internal copy of the matrix S (the +% input matrix S is not repaired, however). If a matrix is invalid in other +% ways then SYMAMD cannot continue, an error message is printed, and no +% output arguments (P or stats) are returned. SYMAMD is thus a simple way +% to check a sparse matrix to see if it's valid. +% +% stats (4:7) provide information if SYMAMD was able to continue. The +% matrix is OK if stats (4) is zero, or 1 if invalid. stats (5) is the +% rightmost column index that is unsorted or contains duplicate entries, +% or zero if no such column exists. stats (6) is the last seen duplicate +% or out-of-order row index in the column index given by stats (5), or zero +% if no such row index exists. stats (7) is the number of duplicate or +% out-of-order row indices. +% +% stats (8:20) is always zero in the current version of SYMAMD (reserved +% for future use). diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/symamdmex.c b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/symamdmex.c new file mode 100644 index 0000000..041d8f9 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/symamdmex.c @@ -0,0 +1,194 @@ +/* ========================================================================== */ +/* === symamd mexFunction =================================================== */ +/* ========================================================================== */ + +/* SYMAMD mexFunction + + Usage: + + P = symamd2 (A) ; + [ P, stats ] = symamd2 (A, knobs) ; + + See symamd.m for a description. + + Authors: + + The authors of the code itself are Stefan I. Larimore and Timothy A. + Davis (DrTimothyAldenDavis@gmail.com). The algorithm was + developed in collaboration with John Gilbert, Xerox PARC, and Esmond + Ng, Oak Ridge National Laboratory. + + Acknowledgements: + + This work was supported by the National Science Foundation, under + grants DMS-9504974 and DMS-9803599. + + Notice: + + Copyright (c) 1998-2007, Timothy A. Davis. All Rights Reserved. + See the colamd.c file for the License. + + Availability: + + The colamd/symamd library is available at http://www.suitesparse.com + +*/ + +/* ========================================================================== */ +/* === Include files ======================================================== */ +/* ========================================================================== */ + +#include "colamd.h" +#include "mex.h" +#include "matrix.h" +#include +#define Long SuiteSparse_long + +/* ========================================================================== */ +/* === symamd mexFunction =================================================== */ +/* ========================================================================== */ + +void mexFunction +( + /* === Parameters ======================================================= */ + + int nlhs, /* number of left-hand sides */ + mxArray *plhs [], /* left-hand side matrices */ + int nrhs, /* number of right--hand sides */ + const mxArray *prhs [] /* right-hand side matrices */ +) +{ + /* === Local variables ================================================== */ + + Long *perm ; /* column ordering of M and ordering of A */ + Long *A ; /* row indices of input matrix A */ + Long *p ; /* column pointers of input matrix A */ + Long n_col ; /* number of columns of A */ + Long n_row ; /* number of rows of A */ + Long full ; /* TRUE if input matrix full, FALSE if sparse */ + double knobs [COLAMD_KNOBS] ; /* colamd user-controllable parameters */ + double *out_perm ; /* output permutation vector */ + double *out_stats ; /* output stats vector */ + double *in_knobs ; /* input knobs vector */ + Long i ; /* loop counter */ + mxArray *Ainput ; /* input matrix handle */ + Long spumoni ; /* verbosity variable */ + Long stats [COLAMD_STATS] ; /* stats for symamd */ + + colamd_printf = mexPrintf ; /* COLAMD printf routine */ + + /* === Check inputs ===================================================== */ + + if (nrhs < 1 || nrhs > 2 || nlhs < 0 || nlhs > 2) + { + mexErrMsgTxt ( + "symamd: incorrect number of input and/or output arguments.") ; + } + + /* === Get knobs ======================================================== */ + + colamd_l_set_defaults (knobs) ; + spumoni = 0 ; + + /* check for user-passed knobs */ + if (nrhs == 2) + { + in_knobs = mxGetPr (prhs [1]) ; + i = mxGetNumberOfElements (prhs [1]) ; + if (i > 0) knobs [COLAMD_DENSE_ROW] = in_knobs [0] ; + if (i > 1) spumoni = (Long) (in_knobs [1] != 0) ; + } + + /* print knob settings if spumoni is set */ + if (spumoni) + { + mexPrintf ("\nsymamd version %d.%d, %s:\n", + COLAMD_MAIN_VERSION, COLAMD_SUB_VERSION, COLAMD_DATE) ; + if (knobs [COLAMD_DENSE_ROW] >= 0) + { + mexPrintf ("knobs(1): %g, rows/cols with > " + "max(16,%g*sqrt(size(A,2))) entries removed\n", + in_knobs [0], knobs [COLAMD_DENSE_ROW]) ; + } + else + { + mexPrintf ("knobs(1): %g, no dense rows removed\n", in_knobs [0]) ; + } + mexPrintf ("knobs(2): %g, statistics and knobs printed\n", + in_knobs [1]) ; + } + + /* === If A is full, convert to a sparse matrix ========================= */ + + Ainput = (mxArray *) prhs [0] ; + if (mxGetNumberOfDimensions (Ainput) != 2) + { + mexErrMsgTxt ("symamd: input matrix must be 2-dimensional.") ; + } + full = !mxIsSparse (Ainput) ; + if (full) + { + mexCallMATLAB (1, &Ainput, 1, (mxArray **) prhs, "sparse") ; + } + + /* === Allocate workspace for symamd ==================================== */ + + /* get size of matrix */ + n_row = mxGetM (Ainput) ; + n_col = mxGetN (Ainput) ; + if (n_col != n_row) + { + mexErrMsgTxt ("symamd: matrix must be square.") ; + } + + A = (Long *) mxGetIr (Ainput) ; + p = (Long *) mxGetJc (Ainput) ; + perm = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; + + /* === Order the rows and columns of A (does not destroy A) ============= */ + + if (!symamd_l (n_col, A, p, perm, knobs, stats, &mxCalloc, &mxFree)) + { + symamd_l_report (stats) ; + mexErrMsgTxt ("symamd error!") ; + } + + if (full) + { + mxDestroyArray (Ainput) ; + } + + /* === Return the permutation vector ==================================== */ + + plhs [0] = mxCreateDoubleMatrix (1, n_col, mxREAL) ; + out_perm = mxGetPr (plhs [0]) ; + for (i = 0 ; i < n_col ; i++) + { + /* symamd is 0-based, but MATLAB expects this to be 1-based */ + out_perm [i] = perm [i] + 1 ; + } + mxFree (perm) ; + + /* === Return the stats vector ========================================== */ + + /* print stats if spumoni is set */ + if (spumoni) + { + symamd_l_report (stats) ; + } + + if (nlhs == 2) + { + plhs [1] = mxCreateDoubleMatrix (1, COLAMD_STATS, mxREAL) ; + out_stats = mxGetPr (plhs [1]) ; + for (i = 0 ; i < COLAMD_STATS ; i++) + { + out_stats [i] = stats [i] ; + } + + /* fix stats (5) and (6), for 1-based information on jumbled matrix. */ + /* note that this correction doesn't occur if symamd returns FALSE */ + out_stats [COLAMD_INFO1] ++ ; + out_stats [COLAMD_INFO2] ++ ; + } +} diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/symamdtestmex.c b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/symamdtestmex.c new file mode 100644 index 0000000..7544cdd --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/MATLAB/symamdtestmex.c @@ -0,0 +1,535 @@ +/* ========================================================================== */ +/* === symamdtest mexFunction =============================================== */ +/* ========================================================================== */ + +/* SYMAMD test function + + This MATLAB mexFunction is for testing only. It is not meant for + production use. See symamdmex.c instead. + + Usage: + + [ P, stats ] = symamdtest (A, knobs) ; + + See symamd.m for a description. knobs is required. + + knobs (1) dense row control + knobs (2) spumoni + knobs (3) for testing only. Controls how the input matrix is + jumbled prior to calling symamd, to test its error + handling capability. + + Authors: + + The authors of the code itself are Stefan I. Larimore and Timothy A. + Davis (DrTimothyAldenDavis@gmail.com). The algorithm was + developed in collaboration with John Gilbert, Xerox PARC, and Esmond + Ng, Oak Ridge National Laboratory. + + Acknowledgements: + + This work was supported by the National Science Foundation, under + grants DMS-9504974 and DMS-9803599. + + Notice: + + Copyright (c) 1998-2007, Timothy A. Davis. All Rights Reserved. + See the colamd.c file for the License. + + Availability: + + The colamd/symamd library is available at http://www.suitesparse.com + +*/ + +/* ========================================================================== */ +/* === Include files ======================================================== */ +/* ========================================================================== */ + +#include "colamd.h" +#include "mex.h" +#include "matrix.h" +#include +#include +#define Long SuiteSparse_long + +static void dump_matrix +( + Long A [ ], + Long p [ ], + Long n_row, + Long n_col, + Long Alen, + Long limit +) ; + +/* ========================================================================== */ +/* === symamd mexFunction =================================================== */ +/* ========================================================================== */ + +void mexFunction +( + /* === Parameters ======================================================= */ + + int nlhs, /* number of left-hand sides */ + mxArray *plhs [], /* left-hand side matrices */ + int nrhs, /* number of right--hand sides */ + const mxArray *prhs [] /* right-hand side matrices */ +) +{ + /* === Local variables ================================================== */ + + Long *perm ; /* column ordering of M and ordering of A */ + Long *A ; /* row indices of input matrix A */ + Long *p ; /* column pointers of input matrix A */ + Long n_col ; /* number of columns of A */ + Long n_row ; /* number of rows of A */ + Long full ; /* TRUE if input matrix full, FALSE if sparse */ + double knobs [COLAMD_KNOBS] ; /* colamd user-controllable parameters */ + double *out_perm ; /* output permutation vector */ + double *out_stats ; /* output stats vector */ + double *in_knobs ; /* input knobs vector */ + Long i ; /* loop counter */ + mxArray *Ainput ; /* input matrix handle */ + Long spumoni ; /* verbosity variable */ + Long stats2 [COLAMD_STATS] ;/* stats for symamd */ + + Long *cp, *cp_end, result, nnz, col, length ; + Long *stats ; + stats = stats2 ; + + colamd_printf = mexPrintf ; /* COLAMD printf routine */ + + /* === Check inputs ===================================================== */ + + if (nrhs < 1 || nrhs > 2 || nlhs < 0 || nlhs > 2) + { + mexErrMsgTxt ( + "symamd: incorrect number of input and/or output arguments.") ; + } + + if (nrhs != 2) + { + mexErrMsgTxt ("symamdtest: knobs are required") ; + } + /* for testing we require all 3 knobs */ + if (mxGetNumberOfElements (prhs [1]) != 3) + { + mexErrMsgTxt ("symamdtest: must have all 3 knobs for testing") ; + } + + /* === Get knobs ======================================================== */ + + colamd_l_set_defaults (knobs) ; + spumoni = 0 ; + + /* check for user-passed knobs */ + if (nrhs == 2) + { + in_knobs = mxGetPr (prhs [1]) ; + i = mxGetNumberOfElements (prhs [1]) ; + if (i > 0) knobs [COLAMD_DENSE_ROW] = in_knobs [0] ; + if (i > 1) spumoni = (Long) in_knobs [1] ; + } + + /* print knob settings if spumoni is set */ + if (spumoni) + { + mexPrintf ("\nsymamd version %d.%d, %s:\n", + COLAMD_MAIN_VERSION, COLAMD_SUB_VERSION, COLAMD_DATE) ; + if (knobs [COLAMD_DENSE_ROW] >= 0) + { + mexPrintf ("knobs(1): %g, rows/cols with > " + "max(16,%g*sqrt(size(A,2))) entries removed\n", + in_knobs [0], knobs [COLAMD_DENSE_ROW]) ; + } + else + { + mexPrintf ("knobs(1): %g, no dense rows removed\n", in_knobs [0]) ; + } + mexPrintf ("knobs(2): %g, statistics and knobs printed\n", + in_knobs [1]) ; + mexPrintf ("Testing %d\n", in_knobs [2]) ; + } + + /* === If A is full, convert to a sparse matrix ========================= */ + + Ainput = (mxArray *) prhs [0] ; + if (mxGetNumberOfDimensions (Ainput) != 2) + { + mexErrMsgTxt ("symamd: input matrix must be 2-dimensional.") ; + } + full = !mxIsSparse (Ainput) ; + if (full) + { + mexCallMATLAB (1, &Ainput, 1, (mxArray **) prhs, "sparse") ; + } + + /* === Allocate workspace for symamd ==================================== */ + + /* get size of matrix */ + n_row = mxGetM (Ainput) ; + n_col = mxGetN (Ainput) ; + if (n_col != n_row) + { + mexErrMsgTxt ("symamd: matrix must be square.") ; + } + + /* p = mxGetJc (Ainput) ; */ + p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; + (void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ; + + nnz = p [n_col] ; + if (spumoni > 0) + { + mexPrintf ("symamdtest: nnz %d\n", nnz) ; + } + + /* A = mxGetIr (Ainput) ; */ + A = (Long *) mxCalloc (nnz+1, sizeof (Long)) ; + (void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ; + + perm = (Long *) mxCalloc (n_col+1, sizeof (Long)) ; + +/* === Jumble matrix ======================================================== */ + + +/* + knobs [2] FOR TESTING ONLY: Specifies how to jumble matrix + 0 : No jumbling + 1 : (no errors) + 2 : Make first pointer non-zero + 3 : Make column pointers not non-decreasing + 4 : (no errors) + 5 : Make row indices not strictly increasing + 6 : Make a row index greater or equal to n_row + 7 : Set A = NULL + 8 : Set p = NULL + 9 : Repeat row index + 10: make row indices not sorted + 11: jumble columns massively (note this changes + the pattern of the matrix A.) + 12: Set stats = NULL + 13: Make n_col less than zero +*/ + + /* jumble appropriately */ + switch ((Long) in_knobs [2]) + { + + case 0 : + if (spumoni > 0) + { + mexPrintf ("symamdtest: no errors expected\n") ; + } + result = 1 ; /* no errors */ + break ; + + case 1 : + if (spumoni > 0) + { + mexPrintf ("symamdtest: no errors expected (1)\n") ; + } + result = 1 ; + break ; + + case 2 : + if (spumoni > 0) + { + mexPrintf ("symamdtest: p [0] nonzero\n") ; + } + result = 0 ; /* p [0] must be zero */ + p [0] = 1 ; + break ; + + case 3 : + if (spumoni > 0) + { + mexPrintf ("symamdtest: negative length last column\n") ; + } + result = (n_col == 0) ; /* p must be monotonically inc. */ + p [n_col] = p [0] ; + break ; + + case 4 : + if (spumoni > 0) + { + mexPrintf ("symamdtest: no errors expected (4)\n") ; + } + result = 1 ; + break ; + + case 5 : + if (spumoni > 0) + { + mexPrintf ("symamdtest: row index out of range (-1)\n") ; + } + if (nnz > 0) /* row index out of range */ + { + result = 0 ; + A [nnz-1] = -1 ; + } + else + { + if (spumoni > 0) + { + mexPrintf ("Note: no row indices to put out of range\n") ; + } + result = 1 ; + } + break ; + + case 6 : + if (spumoni > 0) + { + mexPrintf ("symamdtest: row index out of range (ncol)\n") ; + } + if (nnz > 0) /* row index out of range */ + { + result = 0 ; + A [nnz-1] = n_col ; + } + else + { + if (spumoni > 0) + { + mexPrintf ("Note: no row indices to put out of range\n") ; + } + result = 1 ; + } + break ; + + case 7 : + if (spumoni > 0) + { + mexPrintf ("symamdtest: A not present\n") ; + } + result = 0 ; /* A not present */ + A = (Long *) NULL ; + break ; + + case 8 : + if (spumoni > 0) + { + mexPrintf ("symamdtest: p not present\n") ; + } + result = 0 ; /* p not present */ + p = (Long *) NULL ; + break ; + + case 9 : + if (spumoni > 0) + { + mexPrintf ("symamdtest: duplicate row index\n") ; + } + result = 1 ; /* duplicate row index */ + + for (col = 0 ; col < n_col ; col++) + { + length = p [col+1] - p [col] ; + if (length > 1) + { + A [p [col+1]-2] = A [p [col+1] - 1] ; + if (spumoni > 0) + { + mexPrintf ("Made duplicate row %d in col %d\n", + A [p [col+1] - 1], col) ; + } + break ; + } + } + + if (spumoni > 1) + { + dump_matrix (A, p, n_row, n_col, nnz, col+2) ; + } + break ; + + case 10 : + if (spumoni > 0) + { + mexPrintf ("symamdtest: unsorted column\n") ; + } + result = 1 ; /* jumbled columns */ + + for (col = 0 ; col < n_col ; col++) + { + length = p [col+1] - p [col] ; + if (length > 1) + { + i = A[p [col]] ; + A [p [col]] = A[p [col] + 1] ; + A [p [col] + 1] = i ; + if (spumoni > 0) + { + mexPrintf ("Unsorted column %d \n", col) ; + } + break ; + } + } + + if (spumoni > 1) + { + dump_matrix (A, p, n_row, n_col, nnz, col+2) ; + } + break ; + + case 11 : + if (spumoni > 0) + { + mexPrintf ("symamdtest: massive jumbling\n") ; + } + result = 1 ; /* massive jumbling, but no errors */ + srand (1) ; + for (i = 0 ; i < n_col ; i++) + { + cp = &A [p [i]] ; + cp_end = &A [p [i+1]] ; + while (cp < cp_end) + { + *cp++ = rand() % n_row ; + } + } + if (spumoni > 1) + { + dump_matrix (A, p, n_row, n_col, nnz, n_col) ; + } + break ; + + case 12 : + if (spumoni > 0) + { + mexPrintf ("symamdtest: stats not present\n") ; + } + result = 0 ; /* stats not present */ + stats = (Long *) NULL ; + break ; + + case 13 : + if (spumoni > 0) + { + mexPrintf ("symamdtest: ncol out of range\n") ; + } + result = 0 ; /* ncol out of range */ + n_col = -1 ; + break ; + + } + + /* === Order the rows and columns of A (does not destroy A) ============= */ + + if (!symamd_l (n_col, A, p, perm, knobs, stats, &mxCalloc, &mxFree)) + { + + /* return p = -1 if colamd failed */ + plhs [0] = mxCreateDoubleMatrix (1, 1, mxREAL) ; + out_perm = mxGetPr (plhs [0]) ; + out_perm [0] = -1 ; + mxFree (p) ; + mxFree (A) ; + + if (spumoni > 0 || result) + { + symamd_l_report (stats) ; + } + + if (result) + { + mexErrMsgTxt ("symamd should have returned TRUE\n") ; + } + + return ; + /* mexErrMsgTxt ("symamd error!") ; */ + } + + if (!result) + { + symamd_l_report (stats) ; + mexErrMsgTxt ("symamd should have returned FALSE\n") ; + } + + if (full) + { + mxDestroyArray (Ainput) ; + } + + /* === Return the permutation vector ==================================== */ + + plhs [0] = mxCreateDoubleMatrix (1, n_col, mxREAL) ; + out_perm = mxGetPr (plhs [0]) ; + for (i = 0 ; i < n_col ; i++) + { + /* symamd is 0-based, but MATLAB expects this to be 1-based */ + out_perm [i] = perm [i] + 1 ; + } + mxFree (perm) ; + + /* === Return the stats vector ========================================== */ + + /* print stats if spumoni > 0 */ + if (spumoni > 0) + { + symamd_l_report (stats) ; + } + + if (nlhs == 2) + { + plhs [1] = mxCreateDoubleMatrix (1, COLAMD_STATS, mxREAL) ; + out_stats = mxGetPr (plhs [1]) ; + for (i = 0 ; i < COLAMD_STATS ; i++) + { + out_stats [i] = stats [i] ; + } + + /* fix stats (5) and (6), for 1-based information on jumbled matrix. */ + /* note that this correction doesn't occur if symamd returns FALSE */ + out_stats [COLAMD_INFO1] ++ ; + out_stats [COLAMD_INFO2] ++ ; + } +} + + +#ifdef MIN +#undef MIN +#endif +#define MIN(a,b) (((a) < (b)) ? (a) : (b)) + + +static void dump_matrix +( + Long A [ ], + Long p [ ], + Long n_row, + Long n_col, + Long Alen, + Long limit +) +{ + Long col, k, row ; + + mexPrintf ("dump matrix: nrow %d ncol %d Alen %d\n", n_row, n_col, Alen) ; + + if (!A) + { + mexPrintf ("A not present\n") ; + return ; + } + + if (!p) + { + mexPrintf ("p not present\n") ; + return ; + } + + for (col = 0 ; col < MIN (n_col, limit) ; col++) + { + mexPrintf ("column %d, p[col] %d, p [col+1] %d, length %d\n", + col, p [col], p [col+1], p [col+1] - p [col]) ; + for (k = p [col] ; k < p [col+1] ; k++) + { + row = A [k] ; + mexPrintf (" %d", row) ; + } + mexPrintf ("\n") ; + } +} diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Makefile b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Makefile new file mode 100644 index 0000000..622f793 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Makefile @@ -0,0 +1,56 @@ +#------------------------------------------------------------------------------ +# COLAMD Makefile +#------------------------------------------------------------------------------ + +VERSION = 2.8.0 + +default: all + +include ../SuiteSparse_config/SuiteSparse_config.mk + +demos: all + +# Compile all C code +all: + ( cd Lib ; $(MAKE) ) + ( cd Demo ; $(MAKE) ) + +# compile just the C-callable libraries (not Demos) +library: + ( cd Lib ; $(MAKE) ) + +# remove object files, but keep the compiled programs and library archives +clean: + ( cd Lib ; $(MAKE) clean ) + ( cd Demo ; $(MAKE) clean ) + ( cd MATLAB ; $(RM) $(CLEAN) ) + +# clean, and then remove compiled programs and library archives +purge: + ( cd Lib ; $(MAKE) purge ) + ( cd Demo ; $(MAKE) purge ) + ( cd MATLAB ; $(RM) $(CLEAN) ; $(RM) *.mex* ) + +distclean: purge + +# get ready for distribution +dist: purge + ( cd Demo ; $(MAKE) dist ) + +ccode: library + +lib: library + +# install COLAMD +install: + $(CP) Lib/libcolamd.a $(INSTALL_LIB)/libcolamd.$(VERSION).a + ( cd $(INSTALL_LIB) ; ln -sf libcolamd.$(VERSION).a libcolamd.a ) + $(CP) Include/colamd.h $(INSTALL_INCLUDE) + chmod 644 $(INSTALL_LIB)/libcolamd*.a + chmod 644 $(INSTALL_INCLUDE)/colamd.h + +# uninstall COLAMD +uninstall: + $(RM) $(INSTALL_LIB)/libcolamd*.a + $(RM) $(INSTALL_INCLUDE)/colamd.h + diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/README.txt b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/README.txt new file mode 100644 index 0000000..6c5edf0 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/README.txt @@ -0,0 +1,118 @@ +COLAMD, Copyright 1998-2012, Timothy A. Davis. http://www.suitesparse.com +------------------------------------------------------------------------------- + +The COLAMD column approximate minimum degree ordering algorithm computes +a permutation vector P such that the LU factorization of A (:,P) +tends to be sparser than that of A. The Cholesky factorization of +(A (:,P))'*(A (:,P)) will also tend to be sparser than that of A'*A. +SYMAMD is a symmetric minimum degree ordering method based on COLAMD, +available as a MATLAB-callable function. It constructs a matrix M such +that M'*M has the same pattern as A, and then uses COLAMD to compute a column +ordering of M. Colamd and symamd tend to be faster and generate better +orderings than their MATLAB counterparts, colmmd and symmmd. + +To compile and test the colamd m-files and mexFunctions, just unpack the +COLAMD/ directory from the COLAMD.tar.gz file, and run MATLAB from +within that directory. Next, type colamd_test to compile and test colamd +and symamd. This will work on any computer with MATLAB (Unix, PC, or Mac). +Alternatively, type "make" (in Unix) to compile and run a simple example C +code, without using MATLAB. + +To compile and install the colamd m-files and mexFunctions, just cd to +COLAMD/MATLAB and type colamd_install in the MATLAB command window. +A short demo will run. Optionally, type colamd_test to run an extensive tests. +Type "make" in Unix in the COLAMD directory to compile the C-callable +library and to run a short demo. + +Colamd is a built-in routine in MATLAB, available from The +Mathworks, Inc. Under most cases, the compiled COLAMD from Versions 2.0 to the +current version do not differ. Colamd Versions 2.2 and 2.3 differ only in their +mexFunction interaces to MATLAB. v2.4 fixes a bug in the symamd routine in +v2.3. The bug (in v2.3 and earlier) has no effect on the MATLAB symamd +mexFunction. v2.5 adds additional checks for integer overflow, so that +the "int" version can be safely used with 64-bit pointers. Refer to the +ChangeLog for more details. + +To use colamd and symamd within an application written in C, all you need are +colamd.c, colamd_global.c, and colamd.h, which are the C-callable +colamd/symamd codes. See colamd.c for more information on how to call +colamd from a C program. + +Requires SuiteSparse_config, in the ../SuiteSparse_config directory relative to +this directory. + +See the colamd.c file or http://www.suitesparse.com for the license to COLAMD. + +Related papers: + + T. A. Davis, J. R. Gilbert, S. Larimore, E. Ng, An approximate column + minimum degree ordering algorithm, ACM Transactions on Mathematical + Software, vol. 30, no. 3., pp. 353-376, 2004. + + T. A. Davis, J. R. Gilbert, S. Larimore, E. Ng, Algorithm 836: COLAMD, + an approximate column minimum degree ordering algorithm, ACM + Transactions on Mathematical Software, vol. 30, no. 3., pp. 377-380, + 2004. + + "An approximate minimum degree column ordering algorithm", + S. I. Larimore, MS Thesis, Dept. of Computer and Information + Science and Engineering, University of Florida, Gainesville, FL, + 1998. CISE Tech Report TR-98-016. + + Approximate Deficiency for Ordering the Columns of a Matrix, + J. L. Kern, Senior Thesis, Dept. of Computer and Information + Science and Engineering, University of Florida, Gainesville, FL, + 1999. + + +Authors: Stefan I. Larimore and Timothy A. Davis, +in collaboration with John Gilbert, Xerox PARC (now at UC Santa Barbara), +and Esmong Ng, Lawrence Berkeley National Laboratory (much of this work +he did while at Oak Ridge National Laboratory). + +COLAMD files: + + Demo simple demo + Doc additional documentation (see colamd.c for more) + Include include file + Lib compiled C-callable library + Makefile primary Unix Makefile + MATLAB MATLAB functions + README.txt this file + Source C source code + + ./Demo: + colamd_example.c simple example + colamd_example.out output of colamd_example.c + colamd_l_example.c simple example, long integers + colamd_l_example.out output of colamd_l_example.c + Makefile Makefile for C demos + + ./Doc: + ChangeLog change log + lesser.txt license + + ./Include: + colamd.h include file + + ./Lib: + Makefile Makefile for C-callable library + + ./MATLAB: + colamd2.m MATLAB interface for colamd2 + colamd_demo.m simple demo + colamd_install.m compile and install colamd2 and symamd2 + colamd_make.m compile colamd2 and symamd2 + colamdmex.ca MATLAB mexFunction for colamd2 + colamd_test.m extensive test + colamdtestmex.c test function for colamd + Contents.m contents of the MATLAB directory + luflops.m test code + Makefile Makefile for MATLAB functions + symamd2.m MATLAB interface for symamd2 + symamdmex.c MATLAB mexFunction for symamd2 + symamdtestmex.c test function for symamd + + ./Source: + colamd.c primary source code + colamd_global.c globally defined function pointers (malloc, free, ...) diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Source/colamd.c b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Source/colamd.c new file mode 100644 index 0000000..a20e9e1 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Source/colamd.c @@ -0,0 +1,3604 @@ +/* ========================================================================== */ +/* === colamd/symamd - a sparse matrix column ordering algorithm ============ */ +/* ========================================================================== */ + +/* COLAMD / SYMAMD + + colamd: an approximate minimum degree column ordering algorithm, + for LU factorization of symmetric or unsymmetric matrices, + QR factorization, least squares, interior point methods for + linear programming problems, and other related problems. + + symamd: an approximate minimum degree ordering algorithm for Cholesky + factorization of symmetric matrices. + + Purpose: + + Colamd computes a permutation Q such that the Cholesky factorization of + (AQ)'(AQ) has less fill-in and requires fewer floating point operations + than A'A. This also provides a good ordering for sparse partial + pivoting methods, P(AQ) = LU, where Q is computed prior to numerical + factorization, and P is computed during numerical factorization via + conventional partial pivoting with row interchanges. Colamd is the + column ordering method used in SuperLU, part of the ScaLAPACK library. + It is also available as built-in function in MATLAB Version 6, + available from MathWorks, Inc. (http://www.mathworks.com). This + routine can be used in place of colmmd in MATLAB. + + Symamd computes a permutation P of a symmetric matrix A such that the + Cholesky factorization of PAP' has less fill-in and requires fewer + floating point operations than A. Symamd constructs a matrix M such + that M'M has the same nonzero pattern of A, and then orders the columns + of M using colmmd. The column ordering of M is then returned as the + row and column ordering P of A. + + Authors: + + The authors of the code itself are Stefan I. Larimore and Timothy A. + Davis (DrTimothyAldenDavis@gmail.com). The algorithm was + developed in collaboration with John Gilbert, Xerox PARC, and Esmond + Ng, Oak Ridge National Laboratory. + + Acknowledgements: + + This work was supported by the National Science Foundation, under + grants DMS-9504974 and DMS-9803599. + + Copyright and License: + + Copyright (c) 1998-2007, Timothy A. Davis, All Rights Reserved. + COLAMD is also available under alternate licenses, contact T. Davis + for details. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 + USA + + Permission is hereby granted to use or copy this program under the + terms of the GNU LGPL, provided that the Copyright, this License, + and the Availability of the original version is retained on all copies. + User documentation of any code that uses this code or any modified + version of this code must cite the Copyright, this License, the + Availability note, and "Used by permission." Permission to modify + the code and to distribute modified code is granted, provided the + Copyright, this License, and the Availability note are retained, + and a notice that the code was modified is included. + + Availability: + + The colamd/symamd library is available at http://www.suitesparse.com + Appears as ACM Algorithm 836. + + See the ChangeLog file for changes since Version 1.0. + + References: + + T. A. Davis, J. R. Gilbert, S. Larimore, E. Ng, An approximate column + minimum degree ordering algorithm, ACM Transactions on Mathematical + Software, vol. 30, no. 3., pp. 353-376, 2004. + + T. A. Davis, J. R. Gilbert, S. Larimore, E. Ng, Algorithm 836: COLAMD, + an approximate column minimum degree ordering algorithm, ACM + Transactions on Mathematical Software, vol. 30, no. 3., pp. 377-380, + 2004. + +*/ + +/* ========================================================================== */ +/* === Description of user-callable routines ================================ */ +/* ========================================================================== */ + +/* COLAMD includes both int and SuiteSparse_long versions of all its routines. + The description below is for the int version. For SuiteSparse_long, all + int arguments become SuiteSparse_long. SuiteSparse_long is normally + defined as long, except for WIN64. + + ---------------------------------------------------------------------------- + colamd_recommended: + ---------------------------------------------------------------------------- + + C syntax: + + #include "colamd.h" + size_t colamd_recommended (int nnz, int n_row, int n_col) ; + size_t colamd_l_recommended (SuiteSparse_long nnz, + SuiteSparse_long n_row, SuiteSparse_long n_col) ; + + Purpose: + + Returns recommended value of Alen for use by colamd. Returns 0 + if any input argument is negative. The use of this routine + is optional. Not needed for symamd, which dynamically allocates + its own memory. + + Note that in v2.4 and earlier, these routines returned int or long. + They now return a value of type size_t. + + Arguments (all input arguments): + + int nnz ; Number of nonzeros in the matrix A. This must + be the same value as p [n_col] in the call to + colamd - otherwise you will get a wrong value + of the recommended memory to use. + + int n_row ; Number of rows in the matrix A. + + int n_col ; Number of columns in the matrix A. + + ---------------------------------------------------------------------------- + colamd_set_defaults: + ---------------------------------------------------------------------------- + + C syntax: + + #include "colamd.h" + colamd_set_defaults (double knobs [COLAMD_KNOBS]) ; + colamd_l_set_defaults (double knobs [COLAMD_KNOBS]) ; + + Purpose: + + Sets the default parameters. The use of this routine is optional. + + Arguments: + + double knobs [COLAMD_KNOBS] ; Output only. + + NOTE: the meaning of the dense row/col knobs has changed in v2.4 + + knobs [0] and knobs [1] control dense row and col detection: + + Colamd: rows with more than + max (16, knobs [COLAMD_DENSE_ROW] * sqrt (n_col)) + entries are removed prior to ordering. Columns with more than + max (16, knobs [COLAMD_DENSE_COL] * sqrt (MIN (n_row,n_col))) + entries are removed prior to + ordering, and placed last in the output column ordering. + + Symamd: uses only knobs [COLAMD_DENSE_ROW], which is knobs [0]. + Rows and columns with more than + max (16, knobs [COLAMD_DENSE_ROW] * sqrt (n)) + entries are removed prior to ordering, and placed last in the + output ordering. + + COLAMD_DENSE_ROW and COLAMD_DENSE_COL are defined as 0 and 1, + respectively, in colamd.h. Default values of these two knobs + are both 10. Currently, only knobs [0] and knobs [1] are + used, but future versions may use more knobs. If so, they will + be properly set to their defaults by the future version of + colamd_set_defaults, so that the code that calls colamd will + not need to change, assuming that you either use + colamd_set_defaults, or pass a (double *) NULL pointer as the + knobs array to colamd or symamd. + + knobs [2]: aggressive absorption + + knobs [COLAMD_AGGRESSIVE] controls whether or not to do + aggressive absorption during the ordering. Default is TRUE. + + + ---------------------------------------------------------------------------- + colamd: + ---------------------------------------------------------------------------- + + C syntax: + + #include "colamd.h" + int colamd (int n_row, int n_col, int Alen, int *A, int *p, + double knobs [COLAMD_KNOBS], int stats [COLAMD_STATS]) ; + SuiteSparse_long colamd_l (SuiteSparse_long n_row, + SuiteSparse_long n_col, SuiteSparse_long Alen, + SuiteSparse_long *A, SuiteSparse_long *p, double knobs + [COLAMD_KNOBS], SuiteSparse_long stats [COLAMD_STATS]) ; + + Purpose: + + Computes a column ordering (Q) of A such that P(AQ)=LU or + (AQ)'AQ=LL' have less fill-in and require fewer floating point + operations than factorizing the unpermuted matrix A or A'A, + respectively. + + Returns: + + TRUE (1) if successful, FALSE (0) otherwise. + + Arguments: + + int n_row ; Input argument. + + Number of rows in the matrix A. + Restriction: n_row >= 0. + Colamd returns FALSE if n_row is negative. + + int n_col ; Input argument. + + Number of columns in the matrix A. + Restriction: n_col >= 0. + Colamd returns FALSE if n_col is negative. + + int Alen ; Input argument. + + Restriction (see note): + Alen >= 2*nnz + 6*(n_col+1) + 4*(n_row+1) + n_col + Colamd returns FALSE if these conditions are not met. + + Note: this restriction makes an modest assumption regarding + the size of the two typedef's structures in colamd.h. + We do, however, guarantee that + + Alen >= colamd_recommended (nnz, n_row, n_col) + + will be sufficient. Note: the macro version does not check + for integer overflow, and thus is not recommended. Use + the colamd_recommended routine instead. + + int A [Alen] ; Input argument, undefined on output. + + A is an integer array of size Alen. Alen must be at least as + large as the bare minimum value given above, but this is very + low, and can result in excessive run time. For best + performance, we recommend that Alen be greater than or equal to + colamd_recommended (nnz, n_row, n_col), which adds + nnz/5 to the bare minimum value given above. + + On input, the row indices of the entries in column c of the + matrix are held in A [(p [c]) ... (p [c+1]-1)]. The row indices + in a given column c need not be in ascending order, and + duplicate row indices may be be present. However, colamd will + work a little faster if both of these conditions are met + (Colamd puts the matrix into this format, if it finds that the + the conditions are not met). + + The matrix is 0-based. That is, rows are in the range 0 to + n_row-1, and columns are in the range 0 to n_col-1. Colamd + returns FALSE if any row index is out of range. + + The contents of A are modified during ordering, and are + undefined on output. + + int p [n_col+1] ; Both input and output argument. + + p is an integer array of size n_col+1. On input, it holds the + "pointers" for the column form of the matrix A. Column c of + the matrix A is held in A [(p [c]) ... (p [c+1]-1)]. The first + entry, p [0], must be zero, and p [c] <= p [c+1] must hold + for all c in the range 0 to n_col-1. The value p [n_col] is + thus the total number of entries in the pattern of the matrix A. + Colamd returns FALSE if these conditions are not met. + + On output, if colamd returns TRUE, the array p holds the column + permutation (Q, for P(AQ)=LU or (AQ)'(AQ)=LL'), where p [0] is + the first column index in the new ordering, and p [n_col-1] is + the last. That is, p [k] = j means that column j of A is the + kth pivot column, in AQ, where k is in the range 0 to n_col-1 + (p [0] = j means that column j of A is the first column in AQ). + + If colamd returns FALSE, then no permutation is returned, and + p is undefined on output. + + double knobs [COLAMD_KNOBS] ; Input argument. + + See colamd_set_defaults for a description. + + int stats [COLAMD_STATS] ; Output argument. + + Statistics on the ordering, and error status. + See colamd.h for related definitions. + Colamd returns FALSE if stats is not present. + + stats [0]: number of dense or empty rows ignored. + + stats [1]: number of dense or empty columns ignored (and + ordered last in the output permutation p) + Note that a row can become "empty" if it + contains only "dense" and/or "empty" columns, + and similarly a column can become "empty" if it + only contains "dense" and/or "empty" rows. + + stats [2]: number of garbage collections performed. + This can be excessively high if Alen is close + to the minimum required value. + + stats [3]: status code. < 0 is an error code. + > 1 is a warning or notice. + + 0 OK. Each column of the input matrix contained + row indices in increasing order, with no + duplicates. + + 1 OK, but columns of input matrix were jumbled + (unsorted columns or duplicate entries). Colamd + had to do some extra work to sort the matrix + first and remove duplicate entries, but it + still was able to return a valid permutation + (return value of colamd was TRUE). + + stats [4]: highest numbered column that + is unsorted or has duplicate + entries. + stats [5]: last seen duplicate or + unsorted row index. + stats [6]: number of duplicate or + unsorted row indices. + + -1 A is a null pointer + + -2 p is a null pointer + + -3 n_row is negative + + stats [4]: n_row + + -4 n_col is negative + + stats [4]: n_col + + -5 number of nonzeros in matrix is negative + + stats [4]: number of nonzeros, p [n_col] + + -6 p [0] is nonzero + + stats [4]: p [0] + + -7 A is too small + + stats [4]: required size + stats [5]: actual size (Alen) + + -8 a column has a negative number of entries + + stats [4]: column with < 0 entries + stats [5]: number of entries in col + + -9 a row index is out of bounds + + stats [4]: column with bad row index + stats [5]: bad row index + stats [6]: n_row, # of rows of matrx + + -10 (unused; see symamd.c) + + -999 (unused; see symamd.c) + + Future versions may return more statistics in the stats array. + + Example: + + See colamd_example.c for a complete example. + + To order the columns of a 5-by-4 matrix with 11 nonzero entries in + the following nonzero pattern + + x 0 x 0 + x 0 x x + 0 x x 0 + 0 0 x x + x x 0 0 + + with default knobs and no output statistics, do the following: + + #include "colamd.h" + #define ALEN 100 + int A [ALEN] = {0, 1, 4, 2, 4, 0, 1, 2, 3, 1, 3} ; + int p [ ] = {0, 3, 5, 9, 11} ; + int stats [COLAMD_STATS] ; + colamd (5, 4, ALEN, A, p, (double *) NULL, stats) ; + + The permutation is returned in the array p, and A is destroyed. + + ---------------------------------------------------------------------------- + symamd: + ---------------------------------------------------------------------------- + + C syntax: + + #include "colamd.h" + int symamd (int n, int *A, int *p, int *perm, + double knobs [COLAMD_KNOBS], int stats [COLAMD_STATS], + void (*allocate) (size_t, size_t), void (*release) (void *)) ; + SuiteSparse_long symamd_l (SuiteSparse_long n, SuiteSparse_long *A, + SuiteSparse_long *p, SuiteSparse_long *perm, double knobs + [COLAMD_KNOBS], SuiteSparse_long stats [COLAMD_STATS], void + (*allocate) (size_t, size_t), void (*release) (void *)) ; + + Purpose: + + The symamd routine computes an ordering P of a symmetric sparse + matrix A such that the Cholesky factorization PAP' = LL' remains + sparse. It is based on a column ordering of a matrix M constructed + so that the nonzero pattern of M'M is the same as A. The matrix A + is assumed to be symmetric; only the strictly lower triangular part + is accessed. You must pass your selected memory allocator (usually + calloc/free or mxCalloc/mxFree) to symamd, for it to allocate + memory for the temporary matrix M. + + Returns: + + TRUE (1) if successful, FALSE (0) otherwise. + + Arguments: + + int n ; Input argument. + + Number of rows and columns in the symmetrix matrix A. + Restriction: n >= 0. + Symamd returns FALSE if n is negative. + + int A [nnz] ; Input argument. + + A is an integer array of size nnz, where nnz = p [n]. + + The row indices of the entries in column c of the matrix are + held in A [(p [c]) ... (p [c+1]-1)]. The row indices in a + given column c need not be in ascending order, and duplicate + row indices may be present. However, symamd will run faster + if the columns are in sorted order with no duplicate entries. + + The matrix is 0-based. That is, rows are in the range 0 to + n-1, and columns are in the range 0 to n-1. Symamd + returns FALSE if any row index is out of range. + + The contents of A are not modified. + + int p [n+1] ; Input argument. + + p is an integer array of size n+1. On input, it holds the + "pointers" for the column form of the matrix A. Column c of + the matrix A is held in A [(p [c]) ... (p [c+1]-1)]. The first + entry, p [0], must be zero, and p [c] <= p [c+1] must hold + for all c in the range 0 to n-1. The value p [n] is + thus the total number of entries in the pattern of the matrix A. + Symamd returns FALSE if these conditions are not met. + + The contents of p are not modified. + + int perm [n+1] ; Output argument. + + On output, if symamd returns TRUE, the array perm holds the + permutation P, where perm [0] is the first index in the new + ordering, and perm [n-1] is the last. That is, perm [k] = j + means that row and column j of A is the kth column in PAP', + where k is in the range 0 to n-1 (perm [0] = j means + that row and column j of A are the first row and column in + PAP'). The array is used as a workspace during the ordering, + which is why it must be of length n+1, not just n. + + double knobs [COLAMD_KNOBS] ; Input argument. + + See colamd_set_defaults for a description. + + int stats [COLAMD_STATS] ; Output argument. + + Statistics on the ordering, and error status. + See colamd.h for related definitions. + Symamd returns FALSE if stats is not present. + + stats [0]: number of dense or empty row and columns ignored + (and ordered last in the output permutation + perm). Note that a row/column can become + "empty" if it contains only "dense" and/or + "empty" columns/rows. + + stats [1]: (same as stats [0]) + + stats [2]: number of garbage collections performed. + + stats [3]: status code. < 0 is an error code. + > 1 is a warning or notice. + + 0 OK. Each column of the input matrix contained + row indices in increasing order, with no + duplicates. + + 1 OK, but columns of input matrix were jumbled + (unsorted columns or duplicate entries). Symamd + had to do some extra work to sort the matrix + first and remove duplicate entries, but it + still was able to return a valid permutation + (return value of symamd was TRUE). + + stats [4]: highest numbered column that + is unsorted or has duplicate + entries. + stats [5]: last seen duplicate or + unsorted row index. + stats [6]: number of duplicate or + unsorted row indices. + + -1 A is a null pointer + + -2 p is a null pointer + + -3 (unused, see colamd.c) + + -4 n is negative + + stats [4]: n + + -5 number of nonzeros in matrix is negative + + stats [4]: # of nonzeros (p [n]). + + -6 p [0] is nonzero + + stats [4]: p [0] + + -7 (unused) + + -8 a column has a negative number of entries + + stats [4]: column with < 0 entries + stats [5]: number of entries in col + + -9 a row index is out of bounds + + stats [4]: column with bad row index + stats [5]: bad row index + stats [6]: n_row, # of rows of matrx + + -10 out of memory (unable to allocate temporary + workspace for M or count arrays using the + "allocate" routine passed into symamd). + + Future versions may return more statistics in the stats array. + + void * (*allocate) (size_t, size_t) + + A pointer to a function providing memory allocation. The + allocated memory must be returned initialized to zero. For a + C application, this argument should normally be a pointer to + calloc. For a MATLAB mexFunction, the routine mxCalloc is + passed instead. + + void (*release) (size_t, size_t) + + A pointer to a function that frees memory allocated by the + memory allocation routine above. For a C application, this + argument should normally be a pointer to free. For a MATLAB + mexFunction, the routine mxFree is passed instead. + + + ---------------------------------------------------------------------------- + colamd_report: + ---------------------------------------------------------------------------- + + C syntax: + + #include "colamd.h" + colamd_report (int stats [COLAMD_STATS]) ; + colamd_l_report (SuiteSparse_long stats [COLAMD_STATS]) ; + + Purpose: + + Prints the error status and statistics recorded in the stats + array on the standard error output (for a standard C routine) + or on the MATLAB output (for a mexFunction). + + Arguments: + + int stats [COLAMD_STATS] ; Input only. Statistics from colamd. + + + ---------------------------------------------------------------------------- + symamd_report: + ---------------------------------------------------------------------------- + + C syntax: + + #include "colamd.h" + symamd_report (int stats [COLAMD_STATS]) ; + symamd_l_report (SuiteSparse_long stats [COLAMD_STATS]) ; + + Purpose: + + Prints the error status and statistics recorded in the stats + array on the standard error output (for a standard C routine) + or on the MATLAB output (for a mexFunction). + + Arguments: + + int stats [COLAMD_STATS] ; Input only. Statistics from symamd. + + +*/ + +/* ========================================================================== */ +/* === Scaffolding code definitions ======================================== */ +/* ========================================================================== */ + +/* Ensure that debugging is turned off: */ +#ifndef NDEBUG +#define NDEBUG +#endif + +/* turn on debugging by uncommenting the following line + #undef NDEBUG +*/ + +/* + Our "scaffolding code" philosophy: In our opinion, well-written library + code should keep its "debugging" code, and just normally have it turned off + by the compiler so as not to interfere with performance. This serves + several purposes: + + (1) assertions act as comments to the reader, telling you what the code + expects at that point. All assertions will always be true (unless + there really is a bug, of course). + + (2) leaving in the scaffolding code assists anyone who would like to modify + the code, or understand the algorithm (by reading the debugging output, + one can get a glimpse into what the code is doing). + + (3) (gasp!) for actually finding bugs. This code has been heavily tested + and "should" be fully functional and bug-free ... but you never know... + + The code will become outrageously slow when debugging is + enabled. To control the level of debugging output, set an environment + variable D to 0 (little), 1 (some), 2, 3, or 4 (lots). When debugging, + you should see the following message on the standard output: + + colamd: debug version, D = 1 (THIS WILL BE SLOW!) + + or a similar message for symamd. If you don't, then debugging has not + been enabled. + +*/ + +/* ========================================================================== */ +/* === Include files ======================================================== */ +/* ========================================================================== */ + +#include "colamd.h" +#include +#include + +#ifdef MATLAB_MEX_FILE +#include "mex.h" +#include "matrix.h" +#endif /* MATLAB_MEX_FILE */ + +#if !defined (NPRINT) || !defined (NDEBUG) +#include +#endif + +#ifndef NULL +#define NULL ((void *) 0) +#endif + +/* ========================================================================== */ +/* === int or SuiteSparse_long ============================================== */ +/* ========================================================================== */ + +#ifdef DLONG + +#define Int SuiteSparse_long +#define ID SuiteSparse_long_id +#define Int_MAX SuiteSparse_long_max + +#define COLAMD_recommended colamd_l_recommended +#define COLAMD_set_defaults colamd_l_set_defaults +#define COLAMD_MAIN colamd_l +#define SYMAMD_MAIN symamd_l +#define COLAMD_report colamd_l_report +#define SYMAMD_report symamd_l_report + +#else + +#define Int int +#define ID "%d" +#define Int_MAX INT_MAX + +#define COLAMD_recommended colamd_recommended +#define COLAMD_set_defaults colamd_set_defaults +#define COLAMD_MAIN colamd +#define SYMAMD_MAIN symamd +#define COLAMD_report colamd_report +#define SYMAMD_report symamd_report + +#endif + +/* ========================================================================== */ +/* === Row and Column structures ============================================ */ +/* ========================================================================== */ + +/* User code that makes use of the colamd/symamd routines need not directly */ +/* reference these structures. They are used only for colamd_recommended. */ + +typedef struct Colamd_Col_struct +{ + Int start ; /* index for A of first row in this column, or DEAD */ + /* if column is dead */ + Int length ; /* number of rows in this column */ + union + { + Int thickness ; /* number of original columns represented by this */ + /* col, if the column is alive */ + Int parent ; /* parent in parent tree super-column structure, if */ + /* the column is dead */ + } shared1 ; + union + { + Int score ; /* the score used to maintain heap, if col is alive */ + Int order ; /* pivot ordering of this column, if col is dead */ + } shared2 ; + union + { + Int headhash ; /* head of a hash bucket, if col is at the head of */ + /* a degree list */ + Int hash ; /* hash value, if col is not in a degree list */ + Int prev ; /* previous column in degree list, if col is in a */ + /* degree list (but not at the head of a degree list) */ + } shared3 ; + union + { + Int degree_next ; /* next column, if col is in a degree list */ + Int hash_next ; /* next column, if col is in a hash list */ + } shared4 ; + +} Colamd_Col ; + +typedef struct Colamd_Row_struct +{ + Int start ; /* index for A of first col in this row */ + Int length ; /* number of principal columns in this row */ + union + { + Int degree ; /* number of principal & non-principal columns in row */ + Int p ; /* used as a row pointer in init_rows_cols () */ + } shared1 ; + union + { + Int mark ; /* for computing set differences and marking dead rows*/ + Int first_column ;/* first column in row (used in garbage collection) */ + } shared2 ; + +} Colamd_Row ; + +/* ========================================================================== */ +/* === Definitions ========================================================== */ +/* ========================================================================== */ + +/* Routines are either PUBLIC (user-callable) or PRIVATE (not user-callable) */ +#define PUBLIC +#define PRIVATE static + +#define DENSE_DEGREE(alpha,n) \ + ((Int) MAX (16.0, (alpha) * sqrt ((double) (n)))) + +#define MAX(a,b) (((a) > (b)) ? (a) : (b)) +#define MIN(a,b) (((a) < (b)) ? (a) : (b)) + +#define ONES_COMPLEMENT(r) (-(r)-1) + +/* -------------------------------------------------------------------------- */ +/* Change for version 2.1: define TRUE and FALSE only if not yet defined */ +/* -------------------------------------------------------------------------- */ + +#ifndef TRUE +#define TRUE (1) +#endif + +#ifndef FALSE +#define FALSE (0) +#endif + +/* -------------------------------------------------------------------------- */ + +#define EMPTY (-1) + +/* Row and column status */ +#define ALIVE (0) +#define DEAD (-1) + +/* Column status */ +#define DEAD_PRINCIPAL (-1) +#define DEAD_NON_PRINCIPAL (-2) + +/* Macros for row and column status update and checking. */ +#define ROW_IS_DEAD(r) ROW_IS_MARKED_DEAD (Row[r].shared2.mark) +#define ROW_IS_MARKED_DEAD(row_mark) (row_mark < ALIVE) +#define ROW_IS_ALIVE(r) (Row [r].shared2.mark >= ALIVE) +#define COL_IS_DEAD(c) (Col [c].start < ALIVE) +#define COL_IS_ALIVE(c) (Col [c].start >= ALIVE) +#define COL_IS_DEAD_PRINCIPAL(c) (Col [c].start == DEAD_PRINCIPAL) +#define KILL_ROW(r) { Row [r].shared2.mark = DEAD ; } +#define KILL_PRINCIPAL_COL(c) { Col [c].start = DEAD_PRINCIPAL ; } +#define KILL_NON_PRINCIPAL_COL(c) { Col [c].start = DEAD_NON_PRINCIPAL ; } + +/* ========================================================================== */ +/* === Colamd reporting mechanism =========================================== */ +/* ========================================================================== */ + +#if defined (MATLAB_MEX_FILE) || defined (MATHWORKS) +/* In MATLAB, matrices are 1-based to the user, but 0-based internally */ +#define INDEX(i) ((i)+1) +#else +/* In C, matrices are 0-based and indices are reported as such in *_report */ +#define INDEX(i) (i) +#endif + +/* All output goes through the PRINTF macro. */ +#define PRINTF(params) { if (colamd_printf != NULL) (void) colamd_printf params ; } + +/* ========================================================================== */ +/* === Prototypes of PRIVATE routines ======================================= */ +/* ========================================================================== */ + +PRIVATE Int init_rows_cols +( + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int A [], + Int p [], + Int stats [COLAMD_STATS] +) ; + +PRIVATE void init_scoring +( + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int A [], + Int head [], + double knobs [COLAMD_KNOBS], + Int *p_n_row2, + Int *p_n_col2, + Int *p_max_deg +) ; + +PRIVATE Int find_ordering +( + Int n_row, + Int n_col, + Int Alen, + Colamd_Row Row [], + Colamd_Col Col [], + Int A [], + Int head [], + Int n_col2, + Int max_deg, + Int pfree, + Int aggressive +) ; + +PRIVATE void order_children +( + Int n_col, + Colamd_Col Col [], + Int p [] +) ; + +PRIVATE void detect_super_cols +( + +#ifndef NDEBUG + Int n_col, + Colamd_Row Row [], +#endif /* NDEBUG */ + + Colamd_Col Col [], + Int A [], + Int head [], + Int row_start, + Int row_length +) ; + +PRIVATE Int garbage_collection +( + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int A [], + Int *pfree +) ; + +PRIVATE Int clear_mark +( + Int tag_mark, + Int max_mark, + Int n_row, + Colamd_Row Row [] +) ; + +PRIVATE void print_report +( + char *method, + Int stats [COLAMD_STATS] +) ; + +/* ========================================================================== */ +/* === Debugging prototypes and definitions ================================= */ +/* ========================================================================== */ + +#ifndef NDEBUG + +#include + +/* colamd_debug is the *ONLY* global variable, and is only */ +/* present when debugging */ + +PRIVATE Int colamd_debug = 0 ; /* debug print level */ + +#define DEBUG0(params) { PRINTF (params) ; } +#define DEBUG1(params) { if (colamd_debug >= 1) PRINTF (params) ; } +#define DEBUG2(params) { if (colamd_debug >= 2) PRINTF (params) ; } +#define DEBUG3(params) { if (colamd_debug >= 3) PRINTF (params) ; } +#define DEBUG4(params) { if (colamd_debug >= 4) PRINTF (params) ; } + +#ifdef MATLAB_MEX_FILE +#define ASSERT(expression) (mxAssert ((expression), "")) +#else +#define ASSERT(expression) (assert (expression)) +#endif /* MATLAB_MEX_FILE */ + +PRIVATE void colamd_get_debug /* gets the debug print level from getenv */ +( + char *method +) ; + +PRIVATE void debug_deg_lists +( + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int head [], + Int min_score, + Int should, + Int max_deg +) ; + +PRIVATE void debug_mark +( + Int n_row, + Colamd_Row Row [], + Int tag_mark, + Int max_mark +) ; + +PRIVATE void debug_matrix +( + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int A [] +) ; + +PRIVATE void debug_structures +( + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int A [], + Int n_col2 +) ; + +#else /* NDEBUG */ + +/* === No debugging ========================================================= */ + +#define DEBUG0(params) ; +#define DEBUG1(params) ; +#define DEBUG2(params) ; +#define DEBUG3(params) ; +#define DEBUG4(params) ; + +#define ASSERT(expression) + +#endif /* NDEBUG */ + +/* ========================================================================== */ +/* === USER-CALLABLE ROUTINES: ============================================== */ +/* ========================================================================== */ + +/* ========================================================================== */ +/* === colamd_recommended =================================================== */ +/* ========================================================================== */ + +/* + The colamd_recommended routine returns the suggested size for Alen. This + value has been determined to provide good balance between the number of + garbage collections and the memory requirements for colamd. If any + argument is negative, or if integer overflow occurs, a 0 is returned as an + error condition. 2*nnz space is required for the row and column + indices of the matrix. COLAMD_C (n_col) + COLAMD_R (n_row) space is + required for the Col and Row arrays, respectively, which are internal to + colamd (roughly 6*n_col + 4*n_row). An additional n_col space is the + minimal amount of "elbow room", and nnz/5 more space is recommended for + run time efficiency. + + Alen is approximately 2.2*nnz + 7*n_col + 4*n_row + 10. + + This function is not needed when using symamd. +*/ + +/* add two values of type size_t, and check for integer overflow */ +static size_t t_add (size_t a, size_t b, int *ok) +{ + (*ok) = (*ok) && ((a + b) >= MAX (a,b)) ; + return ((*ok) ? (a + b) : 0) ; +} + +/* compute a*k where k is a small integer, and check for integer overflow */ +static size_t t_mult (size_t a, size_t k, int *ok) +{ + size_t i, s = 0 ; + for (i = 0 ; i < k ; i++) + { + s = t_add (s, a, ok) ; + } + return (s) ; +} + +/* size of the Col and Row structures */ +#define COLAMD_C(n_col,ok) \ + ((t_mult (t_add (n_col, 1, ok), sizeof (Colamd_Col), ok) / sizeof (Int))) + +#define COLAMD_R(n_row,ok) \ + ((t_mult (t_add (n_row, 1, ok), sizeof (Colamd_Row), ok) / sizeof (Int))) + + +PUBLIC size_t COLAMD_recommended /* returns recommended value of Alen. */ +( + /* === Parameters ======================================================= */ + + Int nnz, /* number of nonzeros in A */ + Int n_row, /* number of rows in A */ + Int n_col /* number of columns in A */ +) +{ + size_t s, c, r ; + int ok = TRUE ; + if (nnz < 0 || n_row < 0 || n_col < 0) + { + return (0) ; + } + s = t_mult (nnz, 2, &ok) ; /* 2*nnz */ + c = COLAMD_C (n_col, &ok) ; /* size of column structures */ + r = COLAMD_R (n_row, &ok) ; /* size of row structures */ + s = t_add (s, c, &ok) ; + s = t_add (s, r, &ok) ; + s = t_add (s, n_col, &ok) ; /* elbow room */ + s = t_add (s, nnz/5, &ok) ; /* elbow room */ + ok = ok && (s < Int_MAX) ; + return (ok ? s : 0) ; +} + + +/* ========================================================================== */ +/* === colamd_set_defaults ================================================== */ +/* ========================================================================== */ + +/* + The colamd_set_defaults routine sets the default values of the user- + controllable parameters for colamd and symamd: + + Colamd: rows with more than max (16, knobs [0] * sqrt (n_col)) + entries are removed prior to ordering. Columns with more than + max (16, knobs [1] * sqrt (MIN (n_row,n_col))) entries are removed + prior to ordering, and placed last in the output column ordering. + + Symamd: Rows and columns with more than max (16, knobs [0] * sqrt (n)) + entries are removed prior to ordering, and placed last in the + output ordering. + + knobs [0] dense row control + + knobs [1] dense column control + + knobs [2] if nonzero, do aggresive absorption + + knobs [3..19] unused, but future versions might use this + +*/ + +PUBLIC void COLAMD_set_defaults +( + /* === Parameters ======================================================= */ + + double knobs [COLAMD_KNOBS] /* knob array */ +) +{ + /* === Local variables ================================================== */ + + Int i ; + + if (!knobs) + { + return ; /* no knobs to initialize */ + } + for (i = 0 ; i < COLAMD_KNOBS ; i++) + { + knobs [i] = 0 ; + } + knobs [COLAMD_DENSE_ROW] = 10 ; + knobs [COLAMD_DENSE_COL] = 10 ; + knobs [COLAMD_AGGRESSIVE] = TRUE ; /* default: do aggressive absorption*/ +} + + +/* ========================================================================== */ +/* === symamd =============================================================== */ +/* ========================================================================== */ + +PUBLIC Int SYMAMD_MAIN /* return TRUE if OK, FALSE otherwise */ +( + /* === Parameters ======================================================= */ + + Int n, /* number of rows and columns of A */ + Int A [], /* row indices of A */ + Int p [], /* column pointers of A */ + Int perm [], /* output permutation, size n+1 */ + double knobs [COLAMD_KNOBS], /* parameters (uses defaults if NULL) */ + Int stats [COLAMD_STATS], /* output statistics and error codes */ + void * (*allocate) (size_t, size_t), + /* pointer to calloc (ANSI C) or */ + /* mxCalloc (for MATLAB mexFunction) */ + void (*release) (void *) + /* pointer to free (ANSI C) or */ + /* mxFree (for MATLAB mexFunction) */ +) +{ + /* === Local variables ================================================== */ + + Int *count ; /* length of each column of M, and col pointer*/ + Int *mark ; /* mark array for finding duplicate entries */ + Int *M ; /* row indices of matrix M */ + size_t Mlen ; /* length of M */ + Int n_row ; /* number of rows in M */ + Int nnz ; /* number of entries in A */ + Int i ; /* row index of A */ + Int j ; /* column index of A */ + Int k ; /* row index of M */ + Int mnz ; /* number of nonzeros in M */ + Int pp ; /* index into a column of A */ + Int last_row ; /* last row seen in the current column */ + Int length ; /* number of nonzeros in a column */ + + double cknobs [COLAMD_KNOBS] ; /* knobs for colamd */ + double default_knobs [COLAMD_KNOBS] ; /* default knobs for colamd */ + +#ifndef NDEBUG + colamd_get_debug ("symamd") ; +#endif /* NDEBUG */ + + /* === Check the input arguments ======================================== */ + + if (!stats) + { + DEBUG0 (("symamd: stats not present\n")) ; + return (FALSE) ; + } + for (i = 0 ; i < COLAMD_STATS ; i++) + { + stats [i] = 0 ; + } + stats [COLAMD_STATUS] = COLAMD_OK ; + stats [COLAMD_INFO1] = -1 ; + stats [COLAMD_INFO2] = -1 ; + + if (!A) + { + stats [COLAMD_STATUS] = COLAMD_ERROR_A_not_present ; + DEBUG0 (("symamd: A not present\n")) ; + return (FALSE) ; + } + + if (!p) /* p is not present */ + { + stats [COLAMD_STATUS] = COLAMD_ERROR_p_not_present ; + DEBUG0 (("symamd: p not present\n")) ; + return (FALSE) ; + } + + if (n < 0) /* n must be >= 0 */ + { + stats [COLAMD_STATUS] = COLAMD_ERROR_ncol_negative ; + stats [COLAMD_INFO1] = n ; + DEBUG0 (("symamd: n negative %d\n", n)) ; + return (FALSE) ; + } + + nnz = p [n] ; + if (nnz < 0) /* nnz must be >= 0 */ + { + stats [COLAMD_STATUS] = COLAMD_ERROR_nnz_negative ; + stats [COLAMD_INFO1] = nnz ; + DEBUG0 (("symamd: number of entries negative %d\n", nnz)) ; + return (FALSE) ; + } + + if (p [0] != 0) + { + stats [COLAMD_STATUS] = COLAMD_ERROR_p0_nonzero ; + stats [COLAMD_INFO1] = p [0] ; + DEBUG0 (("symamd: p[0] not zero %d\n", p [0])) ; + return (FALSE) ; + } + + /* === If no knobs, set default knobs =================================== */ + + if (!knobs) + { + COLAMD_set_defaults (default_knobs) ; + knobs = default_knobs ; + } + + /* === Allocate count and mark ========================================== */ + + count = (Int *) ((*allocate) (n+1, sizeof (Int))) ; + if (!count) + { + stats [COLAMD_STATUS] = COLAMD_ERROR_out_of_memory ; + DEBUG0 (("symamd: allocate count (size %d) failed\n", n+1)) ; + return (FALSE) ; + } + + mark = (Int *) ((*allocate) (n+1, sizeof (Int))) ; + if (!mark) + { + stats [COLAMD_STATUS] = COLAMD_ERROR_out_of_memory ; + (*release) ((void *) count) ; + DEBUG0 (("symamd: allocate mark (size %d) failed\n", n+1)) ; + return (FALSE) ; + } + + /* === Compute column counts of M, check if A is valid ================== */ + + stats [COLAMD_INFO3] = 0 ; /* number of duplicate or unsorted row indices*/ + + for (i = 0 ; i < n ; i++) + { + mark [i] = -1 ; + } + + for (j = 0 ; j < n ; j++) + { + last_row = -1 ; + + length = p [j+1] - p [j] ; + if (length < 0) + { + /* column pointers must be non-decreasing */ + stats [COLAMD_STATUS] = COLAMD_ERROR_col_length_negative ; + stats [COLAMD_INFO1] = j ; + stats [COLAMD_INFO2] = length ; + (*release) ((void *) count) ; + (*release) ((void *) mark) ; + DEBUG0 (("symamd: col %d negative length %d\n", j, length)) ; + return (FALSE) ; + } + + for (pp = p [j] ; pp < p [j+1] ; pp++) + { + i = A [pp] ; + if (i < 0 || i >= n) + { + /* row index i, in column j, is out of bounds */ + stats [COLAMD_STATUS] = COLAMD_ERROR_row_index_out_of_bounds ; + stats [COLAMD_INFO1] = j ; + stats [COLAMD_INFO2] = i ; + stats [COLAMD_INFO3] = n ; + (*release) ((void *) count) ; + (*release) ((void *) mark) ; + DEBUG0 (("symamd: row %d col %d out of bounds\n", i, j)) ; + return (FALSE) ; + } + + if (i <= last_row || mark [i] == j) + { + /* row index is unsorted or repeated (or both), thus col */ + /* is jumbled. This is a notice, not an error condition. */ + stats [COLAMD_STATUS] = COLAMD_OK_BUT_JUMBLED ; + stats [COLAMD_INFO1] = j ; + stats [COLAMD_INFO2] = i ; + (stats [COLAMD_INFO3]) ++ ; + DEBUG1 (("symamd: row %d col %d unsorted/duplicate\n", i, j)) ; + } + + if (i > j && mark [i] != j) + { + /* row k of M will contain column indices i and j */ + count [i]++ ; + count [j]++ ; + } + + /* mark the row as having been seen in this column */ + mark [i] = j ; + + last_row = i ; + } + } + + /* v2.4: removed free(mark) */ + + /* === Compute column pointers of M ===================================== */ + + /* use output permutation, perm, for column pointers of M */ + perm [0] = 0 ; + for (j = 1 ; j <= n ; j++) + { + perm [j] = perm [j-1] + count [j-1] ; + } + for (j = 0 ; j < n ; j++) + { + count [j] = perm [j] ; + } + + /* === Construct M ====================================================== */ + + mnz = perm [n] ; + n_row = mnz / 2 ; + Mlen = COLAMD_recommended (mnz, n_row, n) ; + M = (Int *) ((*allocate) (Mlen, sizeof (Int))) ; + DEBUG0 (("symamd: M is %d-by-%d with %d entries, Mlen = %g\n", + n_row, n, mnz, (double) Mlen)) ; + + if (!M) + { + stats [COLAMD_STATUS] = COLAMD_ERROR_out_of_memory ; + (*release) ((void *) count) ; + (*release) ((void *) mark) ; + DEBUG0 (("symamd: allocate M (size %g) failed\n", (double) Mlen)) ; + return (FALSE) ; + } + + k = 0 ; + + if (stats [COLAMD_STATUS] == COLAMD_OK) + { + /* Matrix is OK */ + for (j = 0 ; j < n ; j++) + { + ASSERT (p [j+1] - p [j] >= 0) ; + for (pp = p [j] ; pp < p [j+1] ; pp++) + { + i = A [pp] ; + ASSERT (i >= 0 && i < n) ; + if (i > j) + { + /* row k of M contains column indices i and j */ + M [count [i]++] = k ; + M [count [j]++] = k ; + k++ ; + } + } + } + } + else + { + /* Matrix is jumbled. Do not add duplicates to M. Unsorted cols OK. */ + DEBUG0 (("symamd: Duplicates in A.\n")) ; + for (i = 0 ; i < n ; i++) + { + mark [i] = -1 ; + } + for (j = 0 ; j < n ; j++) + { + ASSERT (p [j+1] - p [j] >= 0) ; + for (pp = p [j] ; pp < p [j+1] ; pp++) + { + i = A [pp] ; + ASSERT (i >= 0 && i < n) ; + if (i > j && mark [i] != j) + { + /* row k of M contains column indices i and j */ + M [count [i]++] = k ; + M [count [j]++] = k ; + k++ ; + mark [i] = j ; + } + } + } + /* v2.4: free(mark) moved below */ + } + + /* count and mark no longer needed */ + (*release) ((void *) count) ; + (*release) ((void *) mark) ; /* v2.4: free (mark) moved here */ + ASSERT (k == n_row) ; + + /* === Adjust the knobs for M =========================================== */ + + for (i = 0 ; i < COLAMD_KNOBS ; i++) + { + cknobs [i] = knobs [i] ; + } + + /* there are no dense rows in M */ + cknobs [COLAMD_DENSE_ROW] = -1 ; + cknobs [COLAMD_DENSE_COL] = knobs [COLAMD_DENSE_ROW] ; + + /* === Order the columns of M =========================================== */ + + /* v2.4: colamd cannot fail here, so the error check is removed */ + (void) COLAMD_MAIN (n_row, n, (Int) Mlen, M, perm, cknobs, stats) ; + + /* Note that the output permutation is now in perm */ + + /* === get the statistics for symamd from colamd ======================== */ + + /* a dense column in colamd means a dense row and col in symamd */ + stats [COLAMD_DENSE_ROW] = stats [COLAMD_DENSE_COL] ; + + /* === Free M =========================================================== */ + + (*release) ((void *) M) ; + DEBUG0 (("symamd: done.\n")) ; + return (TRUE) ; + +} + +/* ========================================================================== */ +/* === colamd =============================================================== */ +/* ========================================================================== */ + +/* + The colamd routine computes a column ordering Q of a sparse matrix + A such that the LU factorization P(AQ) = LU remains sparse, where P is + selected via partial pivoting. The routine can also be viewed as + providing a permutation Q such that the Cholesky factorization + (AQ)'(AQ) = LL' remains sparse. +*/ + +PUBLIC Int COLAMD_MAIN /* returns TRUE if successful, FALSE otherwise*/ +( + /* === Parameters ======================================================= */ + + Int n_row, /* number of rows in A */ + Int n_col, /* number of columns in A */ + Int Alen, /* length of A */ + Int A [], /* row indices of A */ + Int p [], /* pointers to columns in A */ + double knobs [COLAMD_KNOBS],/* parameters (uses defaults if NULL) */ + Int stats [COLAMD_STATS] /* output statistics and error codes */ +) +{ + /* === Local variables ================================================== */ + + Int i ; /* loop index */ + Int nnz ; /* nonzeros in A */ + size_t Row_size ; /* size of Row [], in integers */ + size_t Col_size ; /* size of Col [], in integers */ + size_t need ; /* minimum required length of A */ + Colamd_Row *Row ; /* pointer into A of Row [0..n_row] array */ + Colamd_Col *Col ; /* pointer into A of Col [0..n_col] array */ + Int n_col2 ; /* number of non-dense, non-empty columns */ + Int n_row2 ; /* number of non-dense, non-empty rows */ + Int ngarbage ; /* number of garbage collections performed */ + Int max_deg ; /* maximum row degree */ + double default_knobs [COLAMD_KNOBS] ; /* default knobs array */ + Int aggressive ; /* do aggressive absorption */ + int ok ; + +#ifndef NDEBUG + colamd_get_debug ("colamd") ; +#endif /* NDEBUG */ + + /* === Check the input arguments ======================================== */ + + if (!stats) + { + DEBUG0 (("colamd: stats not present\n")) ; + return (FALSE) ; + } + for (i = 0 ; i < COLAMD_STATS ; i++) + { + stats [i] = 0 ; + } + stats [COLAMD_STATUS] = COLAMD_OK ; + stats [COLAMD_INFO1] = -1 ; + stats [COLAMD_INFO2] = -1 ; + + if (!A) /* A is not present */ + { + stats [COLAMD_STATUS] = COLAMD_ERROR_A_not_present ; + DEBUG0 (("colamd: A not present\n")) ; + return (FALSE) ; + } + + if (!p) /* p is not present */ + { + stats [COLAMD_STATUS] = COLAMD_ERROR_p_not_present ; + DEBUG0 (("colamd: p not present\n")) ; + return (FALSE) ; + } + + if (n_row < 0) /* n_row must be >= 0 */ + { + stats [COLAMD_STATUS] = COLAMD_ERROR_nrow_negative ; + stats [COLAMD_INFO1] = n_row ; + DEBUG0 (("colamd: nrow negative %d\n", n_row)) ; + return (FALSE) ; + } + + if (n_col < 0) /* n_col must be >= 0 */ + { + stats [COLAMD_STATUS] = COLAMD_ERROR_ncol_negative ; + stats [COLAMD_INFO1] = n_col ; + DEBUG0 (("colamd: ncol negative %d\n", n_col)) ; + return (FALSE) ; + } + + nnz = p [n_col] ; + if (nnz < 0) /* nnz must be >= 0 */ + { + stats [COLAMD_STATUS] = COLAMD_ERROR_nnz_negative ; + stats [COLAMD_INFO1] = nnz ; + DEBUG0 (("colamd: number of entries negative %d\n", nnz)) ; + return (FALSE) ; + } + + if (p [0] != 0) + { + stats [COLAMD_STATUS] = COLAMD_ERROR_p0_nonzero ; + stats [COLAMD_INFO1] = p [0] ; + DEBUG0 (("colamd: p[0] not zero %d\n", p [0])) ; + return (FALSE) ; + } + + /* === If no knobs, set default knobs =================================== */ + + if (!knobs) + { + COLAMD_set_defaults (default_knobs) ; + knobs = default_knobs ; + } + + aggressive = (knobs [COLAMD_AGGRESSIVE] != FALSE) ; + + /* === Allocate the Row and Col arrays from array A ===================== */ + + ok = TRUE ; + Col_size = COLAMD_C (n_col, &ok) ; /* size of Col array of structs */ + Row_size = COLAMD_R (n_row, &ok) ; /* size of Row array of structs */ + + /* need = 2*nnz + n_col + Col_size + Row_size ; */ + need = t_mult (nnz, 2, &ok) ; + need = t_add (need, n_col, &ok) ; + need = t_add (need, Col_size, &ok) ; + need = t_add (need, Row_size, &ok) ; + + if (!ok || need > (size_t) Alen || need > Int_MAX) + { + /* not enough space in array A to perform the ordering */ + stats [COLAMD_STATUS] = COLAMD_ERROR_A_too_small ; + stats [COLAMD_INFO1] = need ; + stats [COLAMD_INFO2] = Alen ; + DEBUG0 (("colamd: Need Alen >= %d, given only Alen = %d\n", need,Alen)); + return (FALSE) ; + } + + Alen -= Col_size + Row_size ; + Col = (Colamd_Col *) &A [Alen] ; + Row = (Colamd_Row *) &A [Alen + Col_size] ; + + /* === Construct the row and column data structures ===================== */ + + if (!init_rows_cols (n_row, n_col, Row, Col, A, p, stats)) + { + /* input matrix is invalid */ + DEBUG0 (("colamd: Matrix invalid\n")) ; + return (FALSE) ; + } + + /* === Initialize scores, kill dense rows/columns ======================= */ + + init_scoring (n_row, n_col, Row, Col, A, p, knobs, + &n_row2, &n_col2, &max_deg) ; + + /* === Order the supercolumns =========================================== */ + + ngarbage = find_ordering (n_row, n_col, Alen, Row, Col, A, p, + n_col2, max_deg, 2*nnz, aggressive) ; + + /* === Order the non-principal columns ================================== */ + + order_children (n_col, Col, p) ; + + /* === Return statistics in stats ======================================= */ + + stats [COLAMD_DENSE_ROW] = n_row - n_row2 ; + stats [COLAMD_DENSE_COL] = n_col - n_col2 ; + stats [COLAMD_DEFRAG_COUNT] = ngarbage ; + DEBUG0 (("colamd: done.\n")) ; + return (TRUE) ; +} + + +/* ========================================================================== */ +/* === colamd_report ======================================================== */ +/* ========================================================================== */ + +PUBLIC void COLAMD_report +( + Int stats [COLAMD_STATS] +) +{ + print_report ("colamd", stats) ; +} + + +/* ========================================================================== */ +/* === symamd_report ======================================================== */ +/* ========================================================================== */ + +PUBLIC void SYMAMD_report +( + Int stats [COLAMD_STATS] +) +{ + print_report ("symamd", stats) ; +} + + + +/* ========================================================================== */ +/* === NON-USER-CALLABLE ROUTINES: ========================================== */ +/* ========================================================================== */ + +/* There are no user-callable routines beyond this point in the file */ + + +/* ========================================================================== */ +/* === init_rows_cols ======================================================= */ +/* ========================================================================== */ + +/* + Takes the column form of the matrix in A and creates the row form of the + matrix. Also, row and column attributes are stored in the Col and Row + structs. If the columns are un-sorted or contain duplicate row indices, + this routine will also sort and remove duplicate row indices from the + column form of the matrix. Returns FALSE if the matrix is invalid, + TRUE otherwise. Not user-callable. +*/ + +PRIVATE Int init_rows_cols /* returns TRUE if OK, or FALSE otherwise */ +( + /* === Parameters ======================================================= */ + + Int n_row, /* number of rows of A */ + Int n_col, /* number of columns of A */ + Colamd_Row Row [], /* of size n_row+1 */ + Colamd_Col Col [], /* of size n_col+1 */ + Int A [], /* row indices of A, of size Alen */ + Int p [], /* pointers to columns in A, of size n_col+1 */ + Int stats [COLAMD_STATS] /* colamd statistics */ +) +{ + /* === Local variables ================================================== */ + + Int col ; /* a column index */ + Int row ; /* a row index */ + Int *cp ; /* a column pointer */ + Int *cp_end ; /* a pointer to the end of a column */ + Int *rp ; /* a row pointer */ + Int *rp_end ; /* a pointer to the end of a row */ + Int last_row ; /* previous row */ + + /* === Initialize columns, and check column pointers ==================== */ + + for (col = 0 ; col < n_col ; col++) + { + Col [col].start = p [col] ; + Col [col].length = p [col+1] - p [col] ; + + if (Col [col].length < 0) + { + /* column pointers must be non-decreasing */ + stats [COLAMD_STATUS] = COLAMD_ERROR_col_length_negative ; + stats [COLAMD_INFO1] = col ; + stats [COLAMD_INFO2] = Col [col].length ; + DEBUG0 (("colamd: col %d length %d < 0\n", col, Col [col].length)) ; + return (FALSE) ; + } + + Col [col].shared1.thickness = 1 ; + Col [col].shared2.score = 0 ; + Col [col].shared3.prev = EMPTY ; + Col [col].shared4.degree_next = EMPTY ; + } + + /* p [0..n_col] no longer needed, used as "head" in subsequent routines */ + + /* === Scan columns, compute row degrees, and check row indices ========= */ + + stats [COLAMD_INFO3] = 0 ; /* number of duplicate or unsorted row indices*/ + + for (row = 0 ; row < n_row ; row++) + { + Row [row].length = 0 ; + Row [row].shared2.mark = -1 ; + } + + for (col = 0 ; col < n_col ; col++) + { + last_row = -1 ; + + cp = &A [p [col]] ; + cp_end = &A [p [col+1]] ; + + while (cp < cp_end) + { + row = *cp++ ; + + /* make sure row indices within range */ + if (row < 0 || row >= n_row) + { + stats [COLAMD_STATUS] = COLAMD_ERROR_row_index_out_of_bounds ; + stats [COLAMD_INFO1] = col ; + stats [COLAMD_INFO2] = row ; + stats [COLAMD_INFO3] = n_row ; + DEBUG0 (("colamd: row %d col %d out of bounds\n", row, col)) ; + return (FALSE) ; + } + + if (row <= last_row || Row [row].shared2.mark == col) + { + /* row index are unsorted or repeated (or both), thus col */ + /* is jumbled. This is a notice, not an error condition. */ + stats [COLAMD_STATUS] = COLAMD_OK_BUT_JUMBLED ; + stats [COLAMD_INFO1] = col ; + stats [COLAMD_INFO2] = row ; + (stats [COLAMD_INFO3]) ++ ; + DEBUG1 (("colamd: row %d col %d unsorted/duplicate\n",row,col)); + } + + if (Row [row].shared2.mark != col) + { + Row [row].length++ ; + } + else + { + /* this is a repeated entry in the column, */ + /* it will be removed */ + Col [col].length-- ; + } + + /* mark the row as having been seen in this column */ + Row [row].shared2.mark = col ; + + last_row = row ; + } + } + + /* === Compute row pointers ============================================= */ + + /* row form of the matrix starts directly after the column */ + /* form of matrix in A */ + Row [0].start = p [n_col] ; + Row [0].shared1.p = Row [0].start ; + Row [0].shared2.mark = -1 ; + for (row = 1 ; row < n_row ; row++) + { + Row [row].start = Row [row-1].start + Row [row-1].length ; + Row [row].shared1.p = Row [row].start ; + Row [row].shared2.mark = -1 ; + } + + /* === Create row form ================================================== */ + + if (stats [COLAMD_STATUS] == COLAMD_OK_BUT_JUMBLED) + { + /* if cols jumbled, watch for repeated row indices */ + for (col = 0 ; col < n_col ; col++) + { + cp = &A [p [col]] ; + cp_end = &A [p [col+1]] ; + while (cp < cp_end) + { + row = *cp++ ; + if (Row [row].shared2.mark != col) + { + A [(Row [row].shared1.p)++] = col ; + Row [row].shared2.mark = col ; + } + } + } + } + else + { + /* if cols not jumbled, we don't need the mark (this is faster) */ + for (col = 0 ; col < n_col ; col++) + { + cp = &A [p [col]] ; + cp_end = &A [p [col+1]] ; + while (cp < cp_end) + { + A [(Row [*cp++].shared1.p)++] = col ; + } + } + } + + /* === Clear the row marks and set row degrees ========================== */ + + for (row = 0 ; row < n_row ; row++) + { + Row [row].shared2.mark = 0 ; + Row [row].shared1.degree = Row [row].length ; + } + + /* === See if we need to re-create columns ============================== */ + + if (stats [COLAMD_STATUS] == COLAMD_OK_BUT_JUMBLED) + { + DEBUG0 (("colamd: reconstructing column form, matrix jumbled\n")) ; + +#ifndef NDEBUG + /* make sure column lengths are correct */ + for (col = 0 ; col < n_col ; col++) + { + p [col] = Col [col].length ; + } + for (row = 0 ; row < n_row ; row++) + { + rp = &A [Row [row].start] ; + rp_end = rp + Row [row].length ; + while (rp < rp_end) + { + p [*rp++]-- ; + } + } + for (col = 0 ; col < n_col ; col++) + { + ASSERT (p [col] == 0) ; + } + /* now p is all zero (different than when debugging is turned off) */ +#endif /* NDEBUG */ + + /* === Compute col pointers ========================================= */ + + /* col form of the matrix starts at A [0]. */ + /* Note, we may have a gap between the col form and the row */ + /* form if there were duplicate entries, if so, it will be */ + /* removed upon the first garbage collection */ + Col [0].start = 0 ; + p [0] = Col [0].start ; + for (col = 1 ; col < n_col ; col++) + { + /* note that the lengths here are for pruned columns, i.e. */ + /* no duplicate row indices will exist for these columns */ + Col [col].start = Col [col-1].start + Col [col-1].length ; + p [col] = Col [col].start ; + } + + /* === Re-create col form =========================================== */ + + for (row = 0 ; row < n_row ; row++) + { + rp = &A [Row [row].start] ; + rp_end = rp + Row [row].length ; + while (rp < rp_end) + { + A [(p [*rp++])++] = row ; + } + } + } + + /* === Done. Matrix is not (or no longer) jumbled ====================== */ + + return (TRUE) ; +} + + +/* ========================================================================== */ +/* === init_scoring ========================================================= */ +/* ========================================================================== */ + +/* + Kills dense or empty columns and rows, calculates an initial score for + each column, and places all columns in the degree lists. Not user-callable. +*/ + +PRIVATE void init_scoring +( + /* === Parameters ======================================================= */ + + Int n_row, /* number of rows of A */ + Int n_col, /* number of columns of A */ + Colamd_Row Row [], /* of size n_row+1 */ + Colamd_Col Col [], /* of size n_col+1 */ + Int A [], /* column form and row form of A */ + Int head [], /* of size n_col+1 */ + double knobs [COLAMD_KNOBS],/* parameters */ + Int *p_n_row2, /* number of non-dense, non-empty rows */ + Int *p_n_col2, /* number of non-dense, non-empty columns */ + Int *p_max_deg /* maximum row degree */ +) +{ + /* === Local variables ================================================== */ + + Int c ; /* a column index */ + Int r, row ; /* a row index */ + Int *cp ; /* a column pointer */ + Int deg ; /* degree of a row or column */ + Int *cp_end ; /* a pointer to the end of a column */ + Int *new_cp ; /* new column pointer */ + Int col_length ; /* length of pruned column */ + Int score ; /* current column score */ + Int n_col2 ; /* number of non-dense, non-empty columns */ + Int n_row2 ; /* number of non-dense, non-empty rows */ + Int dense_row_count ; /* remove rows with more entries than this */ + Int dense_col_count ; /* remove cols with more entries than this */ + Int min_score ; /* smallest column score */ + Int max_deg ; /* maximum row degree */ + Int next_col ; /* Used to add to degree list.*/ + +#ifndef NDEBUG + Int debug_count ; /* debug only. */ +#endif /* NDEBUG */ + + /* === Extract knobs ==================================================== */ + + /* Note: if knobs contains a NaN, this is undefined: */ + if (knobs [COLAMD_DENSE_ROW] < 0) + { + /* only remove completely dense rows */ + dense_row_count = n_col-1 ; + } + else + { + dense_row_count = DENSE_DEGREE (knobs [COLAMD_DENSE_ROW], n_col) ; + } + if (knobs [COLAMD_DENSE_COL] < 0) + { + /* only remove completely dense columns */ + dense_col_count = n_row-1 ; + } + else + { + dense_col_count = + DENSE_DEGREE (knobs [COLAMD_DENSE_COL], MIN (n_row, n_col)) ; + } + + DEBUG1 (("colamd: densecount: %d %d\n", dense_row_count, dense_col_count)) ; + max_deg = 0 ; + n_col2 = n_col ; + n_row2 = n_row ; + + /* === Kill empty columns =============================================== */ + + /* Put the empty columns at the end in their natural order, so that LU */ + /* factorization can proceed as far as possible. */ + for (c = n_col-1 ; c >= 0 ; c--) + { + deg = Col [c].length ; + if (deg == 0) + { + /* this is a empty column, kill and order it last */ + Col [c].shared2.order = --n_col2 ; + KILL_PRINCIPAL_COL (c) ; + } + } + DEBUG1 (("colamd: null columns killed: %d\n", n_col - n_col2)) ; + + /* === Kill dense columns =============================================== */ + + /* Put the dense columns at the end, in their natural order */ + for (c = n_col-1 ; c >= 0 ; c--) + { + /* skip any dead columns */ + if (COL_IS_DEAD (c)) + { + continue ; + } + deg = Col [c].length ; + if (deg > dense_col_count) + { + /* this is a dense column, kill and order it last */ + Col [c].shared2.order = --n_col2 ; + /* decrement the row degrees */ + cp = &A [Col [c].start] ; + cp_end = cp + Col [c].length ; + while (cp < cp_end) + { + Row [*cp++].shared1.degree-- ; + } + KILL_PRINCIPAL_COL (c) ; + } + } + DEBUG1 (("colamd: Dense and null columns killed: %d\n", n_col - n_col2)) ; + + /* === Kill dense and empty rows ======================================== */ + + for (r = 0 ; r < n_row ; r++) + { + deg = Row [r].shared1.degree ; + ASSERT (deg >= 0 && deg <= n_col) ; + if (deg > dense_row_count || deg == 0) + { + /* kill a dense or empty row */ + KILL_ROW (r) ; + --n_row2 ; + } + else + { + /* keep track of max degree of remaining rows */ + max_deg = MAX (max_deg, deg) ; + } + } + DEBUG1 (("colamd: Dense and null rows killed: %d\n", n_row - n_row2)) ; + + /* === Compute initial column scores ==================================== */ + + /* At this point the row degrees are accurate. They reflect the number */ + /* of "live" (non-dense) columns in each row. No empty rows exist. */ + /* Some "live" columns may contain only dead rows, however. These are */ + /* pruned in the code below. */ + + /* now find the initial matlab score for each column */ + for (c = n_col-1 ; c >= 0 ; c--) + { + /* skip dead column */ + if (COL_IS_DEAD (c)) + { + continue ; + } + score = 0 ; + cp = &A [Col [c].start] ; + new_cp = cp ; + cp_end = cp + Col [c].length ; + while (cp < cp_end) + { + /* get a row */ + row = *cp++ ; + /* skip if dead */ + if (ROW_IS_DEAD (row)) + { + continue ; + } + /* compact the column */ + *new_cp++ = row ; + /* add row's external degree */ + score += Row [row].shared1.degree - 1 ; + /* guard against integer overflow */ + score = MIN (score, n_col) ; + } + /* determine pruned column length */ + col_length = (Int) (new_cp - &A [Col [c].start]) ; + if (col_length == 0) + { + /* a newly-made null column (all rows in this col are "dense" */ + /* and have already been killed) */ + DEBUG2 (("Newly null killed: %d\n", c)) ; + Col [c].shared2.order = --n_col2 ; + KILL_PRINCIPAL_COL (c) ; + } + else + { + /* set column length and set score */ + ASSERT (score >= 0) ; + ASSERT (score <= n_col) ; + Col [c].length = col_length ; + Col [c].shared2.score = score ; + } + } + DEBUG1 (("colamd: Dense, null, and newly-null columns killed: %d\n", + n_col-n_col2)) ; + + /* At this point, all empty rows and columns are dead. All live columns */ + /* are "clean" (containing no dead rows) and simplicial (no supercolumns */ + /* yet). Rows may contain dead columns, but all live rows contain at */ + /* least one live column. */ + +#ifndef NDEBUG + debug_structures (n_row, n_col, Row, Col, A, n_col2) ; +#endif /* NDEBUG */ + + /* === Initialize degree lists ========================================== */ + +#ifndef NDEBUG + debug_count = 0 ; +#endif /* NDEBUG */ + + /* clear the hash buckets */ + for (c = 0 ; c <= n_col ; c++) + { + head [c] = EMPTY ; + } + min_score = n_col ; + /* place in reverse order, so low column indices are at the front */ + /* of the lists. This is to encourage natural tie-breaking */ + for (c = n_col-1 ; c >= 0 ; c--) + { + /* only add principal columns to degree lists */ + if (COL_IS_ALIVE (c)) + { + DEBUG4 (("place %d score %d minscore %d ncol %d\n", + c, Col [c].shared2.score, min_score, n_col)) ; + + /* === Add columns score to DList =============================== */ + + score = Col [c].shared2.score ; + + ASSERT (min_score >= 0) ; + ASSERT (min_score <= n_col) ; + ASSERT (score >= 0) ; + ASSERT (score <= n_col) ; + ASSERT (head [score] >= EMPTY) ; + + /* now add this column to dList at proper score location */ + next_col = head [score] ; + Col [c].shared3.prev = EMPTY ; + Col [c].shared4.degree_next = next_col ; + + /* if there already was a column with the same score, set its */ + /* previous pointer to this new column */ + if (next_col != EMPTY) + { + Col [next_col].shared3.prev = c ; + } + head [score] = c ; + + /* see if this score is less than current min */ + min_score = MIN (min_score, score) ; + +#ifndef NDEBUG + debug_count++ ; +#endif /* NDEBUG */ + + } + } + +#ifndef NDEBUG + DEBUG1 (("colamd: Live cols %d out of %d, non-princ: %d\n", + debug_count, n_col, n_col-debug_count)) ; + ASSERT (debug_count == n_col2) ; + debug_deg_lists (n_row, n_col, Row, Col, head, min_score, n_col2, max_deg) ; +#endif /* NDEBUG */ + + /* === Return number of remaining columns, and max row degree =========== */ + + *p_n_col2 = n_col2 ; + *p_n_row2 = n_row2 ; + *p_max_deg = max_deg ; +} + + +/* ========================================================================== */ +/* === find_ordering ======================================================== */ +/* ========================================================================== */ + +/* + Order the principal columns of the supercolumn form of the matrix + (no supercolumns on input). Uses a minimum approximate column minimum + degree ordering method. Not user-callable. +*/ + +PRIVATE Int find_ordering /* return the number of garbage collections */ +( + /* === Parameters ======================================================= */ + + Int n_row, /* number of rows of A */ + Int n_col, /* number of columns of A */ + Int Alen, /* size of A, 2*nnz + n_col or larger */ + Colamd_Row Row [], /* of size n_row+1 */ + Colamd_Col Col [], /* of size n_col+1 */ + Int A [], /* column form and row form of A */ + Int head [], /* of size n_col+1 */ + Int n_col2, /* Remaining columns to order */ + Int max_deg, /* Maximum row degree */ + Int pfree, /* index of first free slot (2*nnz on entry) */ + Int aggressive +) +{ + /* === Local variables ================================================== */ + + Int k ; /* current pivot ordering step */ + Int pivot_col ; /* current pivot column */ + Int *cp ; /* a column pointer */ + Int *rp ; /* a row pointer */ + Int pivot_row ; /* current pivot row */ + Int *new_cp ; /* modified column pointer */ + Int *new_rp ; /* modified row pointer */ + Int pivot_row_start ; /* pointer to start of pivot row */ + Int pivot_row_degree ; /* number of columns in pivot row */ + Int pivot_row_length ; /* number of supercolumns in pivot row */ + Int pivot_col_score ; /* score of pivot column */ + Int needed_memory ; /* free space needed for pivot row */ + Int *cp_end ; /* pointer to the end of a column */ + Int *rp_end ; /* pointer to the end of a row */ + Int row ; /* a row index */ + Int col ; /* a column index */ + Int max_score ; /* maximum possible score */ + Int cur_score ; /* score of current column */ + unsigned Int hash ; /* hash value for supernode detection */ + Int head_column ; /* head of hash bucket */ + Int first_col ; /* first column in hash bucket */ + Int tag_mark ; /* marker value for mark array */ + Int row_mark ; /* Row [row].shared2.mark */ + Int set_difference ; /* set difference size of row with pivot row */ + Int min_score ; /* smallest column score */ + Int col_thickness ; /* "thickness" (no. of columns in a supercol) */ + Int max_mark ; /* maximum value of tag_mark */ + Int pivot_col_thickness ; /* number of columns represented by pivot col */ + Int prev_col ; /* Used by Dlist operations. */ + Int next_col ; /* Used by Dlist operations. */ + Int ngarbage ; /* number of garbage collections performed */ + +#ifndef NDEBUG + Int debug_d ; /* debug loop counter */ + Int debug_step = 0 ; /* debug loop counter */ +#endif /* NDEBUG */ + + /* === Initialization and clear mark ==================================== */ + + max_mark = INT_MAX - n_col ; /* INT_MAX defined in */ + tag_mark = clear_mark (0, max_mark, n_row, Row) ; + min_score = 0 ; + ngarbage = 0 ; + DEBUG1 (("colamd: Ordering, n_col2=%d\n", n_col2)) ; + + /* === Order the columns ================================================ */ + + for (k = 0 ; k < n_col2 ; /* 'k' is incremented below */) + { + +#ifndef NDEBUG + if (debug_step % 100 == 0) + { + DEBUG2 (("\n... Step k: %d out of n_col2: %d\n", k, n_col2)) ; + } + else + { + DEBUG3 (("\n----------Step k: %d out of n_col2: %d\n", k, n_col2)) ; + } + debug_step++ ; + debug_deg_lists (n_row, n_col, Row, Col, head, + min_score, n_col2-k, max_deg) ; + debug_matrix (n_row, n_col, Row, Col, A) ; +#endif /* NDEBUG */ + + /* === Select pivot column, and order it ============================ */ + + /* make sure degree list isn't empty */ + ASSERT (min_score >= 0) ; + ASSERT (min_score <= n_col) ; + ASSERT (head [min_score] >= EMPTY) ; + +#ifndef NDEBUG + for (debug_d = 0 ; debug_d < min_score ; debug_d++) + { + ASSERT (head [debug_d] == EMPTY) ; + } +#endif /* NDEBUG */ + + /* get pivot column from head of minimum degree list */ + while (head [min_score] == EMPTY && min_score < n_col) + { + min_score++ ; + } + pivot_col = head [min_score] ; + ASSERT (pivot_col >= 0 && pivot_col <= n_col) ; + next_col = Col [pivot_col].shared4.degree_next ; + head [min_score] = next_col ; + if (next_col != EMPTY) + { + Col [next_col].shared3.prev = EMPTY ; + } + + ASSERT (COL_IS_ALIVE (pivot_col)) ; + + /* remember score for defrag check */ + pivot_col_score = Col [pivot_col].shared2.score ; + + /* the pivot column is the kth column in the pivot order */ + Col [pivot_col].shared2.order = k ; + + /* increment order count by column thickness */ + pivot_col_thickness = Col [pivot_col].shared1.thickness ; + k += pivot_col_thickness ; + ASSERT (pivot_col_thickness > 0) ; + DEBUG3 (("Pivot col: %d thick %d\n", pivot_col, pivot_col_thickness)) ; + + /* === Garbage_collection, if necessary ============================= */ + + needed_memory = MIN (pivot_col_score, n_col - k) ; + if (pfree + needed_memory >= Alen) + { + pfree = garbage_collection (n_row, n_col, Row, Col, A, &A [pfree]) ; + ngarbage++ ; + /* after garbage collection we will have enough */ + ASSERT (pfree + needed_memory < Alen) ; + /* garbage collection has wiped out the Row[].shared2.mark array */ + tag_mark = clear_mark (0, max_mark, n_row, Row) ; + +#ifndef NDEBUG + debug_matrix (n_row, n_col, Row, Col, A) ; +#endif /* NDEBUG */ + } + + /* === Compute pivot row pattern ==================================== */ + + /* get starting location for this new merged row */ + pivot_row_start = pfree ; + + /* initialize new row counts to zero */ + pivot_row_degree = 0 ; + + /* tag pivot column as having been visited so it isn't included */ + /* in merged pivot row */ + Col [pivot_col].shared1.thickness = -pivot_col_thickness ; + + /* pivot row is the union of all rows in the pivot column pattern */ + cp = &A [Col [pivot_col].start] ; + cp_end = cp + Col [pivot_col].length ; + while (cp < cp_end) + { + /* get a row */ + row = *cp++ ; + DEBUG4 (("Pivot col pattern %d %d\n", ROW_IS_ALIVE (row), row)) ; + /* skip if row is dead */ + if (ROW_IS_ALIVE (row)) + { + rp = &A [Row [row].start] ; + rp_end = rp + Row [row].length ; + while (rp < rp_end) + { + /* get a column */ + col = *rp++ ; + /* add the column, if alive and untagged */ + col_thickness = Col [col].shared1.thickness ; + if (col_thickness > 0 && COL_IS_ALIVE (col)) + { + /* tag column in pivot row */ + Col [col].shared1.thickness = -col_thickness ; + ASSERT (pfree < Alen) ; + /* place column in pivot row */ + A [pfree++] = col ; + pivot_row_degree += col_thickness ; + } + } + } + } + + /* clear tag on pivot column */ + Col [pivot_col].shared1.thickness = pivot_col_thickness ; + max_deg = MAX (max_deg, pivot_row_degree) ; + +#ifndef NDEBUG + DEBUG3 (("check2\n")) ; + debug_mark (n_row, Row, tag_mark, max_mark) ; +#endif /* NDEBUG */ + + /* === Kill all rows used to construct pivot row ==================== */ + + /* also kill pivot row, temporarily */ + cp = &A [Col [pivot_col].start] ; + cp_end = cp + Col [pivot_col].length ; + while (cp < cp_end) + { + /* may be killing an already dead row */ + row = *cp++ ; + DEBUG3 (("Kill row in pivot col: %d\n", row)) ; + KILL_ROW (row) ; + } + + /* === Select a row index to use as the new pivot row =============== */ + + pivot_row_length = pfree - pivot_row_start ; + if (pivot_row_length > 0) + { + /* pick the "pivot" row arbitrarily (first row in col) */ + pivot_row = A [Col [pivot_col].start] ; + DEBUG3 (("Pivotal row is %d\n", pivot_row)) ; + } + else + { + /* there is no pivot row, since it is of zero length */ + pivot_row = EMPTY ; + ASSERT (pivot_row_length == 0) ; + } + ASSERT (Col [pivot_col].length > 0 || pivot_row_length == 0) ; + + /* === Approximate degree computation =============================== */ + + /* Here begins the computation of the approximate degree. The column */ + /* score is the sum of the pivot row "length", plus the size of the */ + /* set differences of each row in the column minus the pattern of the */ + /* pivot row itself. The column ("thickness") itself is also */ + /* excluded from the column score (we thus use an approximate */ + /* external degree). */ + + /* The time taken by the following code (compute set differences, and */ + /* add them up) is proportional to the size of the data structure */ + /* being scanned - that is, the sum of the sizes of each column in */ + /* the pivot row. Thus, the amortized time to compute a column score */ + /* is proportional to the size of that column (where size, in this */ + /* context, is the column "length", or the number of row indices */ + /* in that column). The number of row indices in a column is */ + /* monotonically non-decreasing, from the length of the original */ + /* column on input to colamd. */ + + /* === Compute set differences ====================================== */ + + DEBUG3 (("** Computing set differences phase. **\n")) ; + + /* pivot row is currently dead - it will be revived later. */ + + DEBUG3 (("Pivot row: ")) ; + /* for each column in pivot row */ + rp = &A [pivot_row_start] ; + rp_end = rp + pivot_row_length ; + while (rp < rp_end) + { + col = *rp++ ; + ASSERT (COL_IS_ALIVE (col) && col != pivot_col) ; + DEBUG3 (("Col: %d\n", col)) ; + + /* clear tags used to construct pivot row pattern */ + col_thickness = -Col [col].shared1.thickness ; + ASSERT (col_thickness > 0) ; + Col [col].shared1.thickness = col_thickness ; + + /* === Remove column from degree list =========================== */ + + cur_score = Col [col].shared2.score ; + prev_col = Col [col].shared3.prev ; + next_col = Col [col].shared4.degree_next ; + ASSERT (cur_score >= 0) ; + ASSERT (cur_score <= n_col) ; + ASSERT (cur_score >= EMPTY) ; + if (prev_col == EMPTY) + { + head [cur_score] = next_col ; + } + else + { + Col [prev_col].shared4.degree_next = next_col ; + } + if (next_col != EMPTY) + { + Col [next_col].shared3.prev = prev_col ; + } + + /* === Scan the column ========================================== */ + + cp = &A [Col [col].start] ; + cp_end = cp + Col [col].length ; + while (cp < cp_end) + { + /* get a row */ + row = *cp++ ; + row_mark = Row [row].shared2.mark ; + /* skip if dead */ + if (ROW_IS_MARKED_DEAD (row_mark)) + { + continue ; + } + ASSERT (row != pivot_row) ; + set_difference = row_mark - tag_mark ; + /* check if the row has been seen yet */ + if (set_difference < 0) + { + ASSERT (Row [row].shared1.degree <= max_deg) ; + set_difference = Row [row].shared1.degree ; + } + /* subtract column thickness from this row's set difference */ + set_difference -= col_thickness ; + ASSERT (set_difference >= 0) ; + /* absorb this row if the set difference becomes zero */ + if (set_difference == 0 && aggressive) + { + DEBUG3 (("aggressive absorption. Row: %d\n", row)) ; + KILL_ROW (row) ; + } + else + { + /* save the new mark */ + Row [row].shared2.mark = set_difference + tag_mark ; + } + } + } + +#ifndef NDEBUG + debug_deg_lists (n_row, n_col, Row, Col, head, + min_score, n_col2-k-pivot_row_degree, max_deg) ; +#endif /* NDEBUG */ + + /* === Add up set differences for each column ======================= */ + + DEBUG3 (("** Adding set differences phase. **\n")) ; + + /* for each column in pivot row */ + rp = &A [pivot_row_start] ; + rp_end = rp + pivot_row_length ; + while (rp < rp_end) + { + /* get a column */ + col = *rp++ ; + ASSERT (COL_IS_ALIVE (col) && col != pivot_col) ; + hash = 0 ; + cur_score = 0 ; + cp = &A [Col [col].start] ; + /* compact the column */ + new_cp = cp ; + cp_end = cp + Col [col].length ; + + DEBUG4 (("Adding set diffs for Col: %d.\n", col)) ; + + while (cp < cp_end) + { + /* get a row */ + row = *cp++ ; + ASSERT(row >= 0 && row < n_row) ; + row_mark = Row [row].shared2.mark ; + /* skip if dead */ + if (ROW_IS_MARKED_DEAD (row_mark)) + { + DEBUG4 ((" Row %d, dead\n", row)) ; + continue ; + } + DEBUG4 ((" Row %d, set diff %d\n", row, row_mark-tag_mark)); + ASSERT (row_mark >= tag_mark) ; + /* compact the column */ + *new_cp++ = row ; + /* compute hash function */ + hash += row ; + /* add set difference */ + cur_score += row_mark - tag_mark ; + /* integer overflow... */ + cur_score = MIN (cur_score, n_col) ; + } + + /* recompute the column's length */ + Col [col].length = (Int) (new_cp - &A [Col [col].start]) ; + + /* === Further mass elimination ================================= */ + + if (Col [col].length == 0) + { + DEBUG4 (("further mass elimination. Col: %d\n", col)) ; + /* nothing left but the pivot row in this column */ + KILL_PRINCIPAL_COL (col) ; + pivot_row_degree -= Col [col].shared1.thickness ; + ASSERT (pivot_row_degree >= 0) ; + /* order it */ + Col [col].shared2.order = k ; + /* increment order count by column thickness */ + k += Col [col].shared1.thickness ; + } + else + { + /* === Prepare for supercolumn detection ==================== */ + + DEBUG4 (("Preparing supercol detection for Col: %d.\n", col)) ; + + /* save score so far */ + Col [col].shared2.score = cur_score ; + + /* add column to hash table, for supercolumn detection */ + hash %= n_col + 1 ; + + DEBUG4 ((" Hash = %d, n_col = %d.\n", hash, n_col)) ; + ASSERT (((Int) hash) <= n_col) ; + + head_column = head [hash] ; + if (head_column > EMPTY) + { + /* degree list "hash" is non-empty, use prev (shared3) of */ + /* first column in degree list as head of hash bucket */ + first_col = Col [head_column].shared3.headhash ; + Col [head_column].shared3.headhash = col ; + } + else + { + /* degree list "hash" is empty, use head as hash bucket */ + first_col = - (head_column + 2) ; + head [hash] = - (col + 2) ; + } + Col [col].shared4.hash_next = first_col ; + + /* save hash function in Col [col].shared3.hash */ + Col [col].shared3.hash = (Int) hash ; + ASSERT (COL_IS_ALIVE (col)) ; + } + } + + /* The approximate external column degree is now computed. */ + + /* === Supercolumn detection ======================================== */ + + DEBUG3 (("** Supercolumn detection phase. **\n")) ; + + detect_super_cols ( + +#ifndef NDEBUG + n_col, Row, +#endif /* NDEBUG */ + + Col, A, head, pivot_row_start, pivot_row_length) ; + + /* === Kill the pivotal column ====================================== */ + + KILL_PRINCIPAL_COL (pivot_col) ; + + /* === Clear mark =================================================== */ + + tag_mark = clear_mark (tag_mark+max_deg+1, max_mark, n_row, Row) ; + +#ifndef NDEBUG + DEBUG3 (("check3\n")) ; + debug_mark (n_row, Row, tag_mark, max_mark) ; +#endif /* NDEBUG */ + + /* === Finalize the new pivot row, and column scores ================ */ + + DEBUG3 (("** Finalize scores phase. **\n")) ; + + /* for each column in pivot row */ + rp = &A [pivot_row_start] ; + /* compact the pivot row */ + new_rp = rp ; + rp_end = rp + pivot_row_length ; + while (rp < rp_end) + { + col = *rp++ ; + /* skip dead columns */ + if (COL_IS_DEAD (col)) + { + continue ; + } + *new_rp++ = col ; + /* add new pivot row to column */ + A [Col [col].start + (Col [col].length++)] = pivot_row ; + + /* retrieve score so far and add on pivot row's degree. */ + /* (we wait until here for this in case the pivot */ + /* row's degree was reduced due to mass elimination). */ + cur_score = Col [col].shared2.score + pivot_row_degree ; + + /* calculate the max possible score as the number of */ + /* external columns minus the 'k' value minus the */ + /* columns thickness */ + max_score = n_col - k - Col [col].shared1.thickness ; + + /* make the score the external degree of the union-of-rows */ + cur_score -= Col [col].shared1.thickness ; + + /* make sure score is less or equal than the max score */ + cur_score = MIN (cur_score, max_score) ; + ASSERT (cur_score >= 0) ; + + /* store updated score */ + Col [col].shared2.score = cur_score ; + + /* === Place column back in degree list ========================= */ + + ASSERT (min_score >= 0) ; + ASSERT (min_score <= n_col) ; + ASSERT (cur_score >= 0) ; + ASSERT (cur_score <= n_col) ; + ASSERT (head [cur_score] >= EMPTY) ; + next_col = head [cur_score] ; + Col [col].shared4.degree_next = next_col ; + Col [col].shared3.prev = EMPTY ; + if (next_col != EMPTY) + { + Col [next_col].shared3.prev = col ; + } + head [cur_score] = col ; + + /* see if this score is less than current min */ + min_score = MIN (min_score, cur_score) ; + + } + +#ifndef NDEBUG + debug_deg_lists (n_row, n_col, Row, Col, head, + min_score, n_col2-k, max_deg) ; +#endif /* NDEBUG */ + + /* === Resurrect the new pivot row ================================== */ + + if (pivot_row_degree > 0) + { + /* update pivot row length to reflect any cols that were killed */ + /* during super-col detection and mass elimination */ + Row [pivot_row].start = pivot_row_start ; + Row [pivot_row].length = (Int) (new_rp - &A[pivot_row_start]) ; + ASSERT (Row [pivot_row].length > 0) ; + Row [pivot_row].shared1.degree = pivot_row_degree ; + Row [pivot_row].shared2.mark = 0 ; + /* pivot row is no longer dead */ + + DEBUG1 (("Resurrect Pivot_row %d deg: %d\n", + pivot_row, pivot_row_degree)) ; + } + } + + /* === All principal columns have now been ordered ====================== */ + + return (ngarbage) ; +} + + +/* ========================================================================== */ +/* === order_children ======================================================= */ +/* ========================================================================== */ + +/* + The find_ordering routine has ordered all of the principal columns (the + representatives of the supercolumns). The non-principal columns have not + yet been ordered. This routine orders those columns by walking up the + parent tree (a column is a child of the column which absorbed it). The + final permutation vector is then placed in p [0 ... n_col-1], with p [0] + being the first column, and p [n_col-1] being the last. It doesn't look + like it at first glance, but be assured that this routine takes time linear + in the number of columns. Although not immediately obvious, the time + taken by this routine is O (n_col), that is, linear in the number of + columns. Not user-callable. +*/ + +PRIVATE void order_children +( + /* === Parameters ======================================================= */ + + Int n_col, /* number of columns of A */ + Colamd_Col Col [], /* of size n_col+1 */ + Int p [] /* p [0 ... n_col-1] is the column permutation*/ +) +{ + /* === Local variables ================================================== */ + + Int i ; /* loop counter for all columns */ + Int c ; /* column index */ + Int parent ; /* index of column's parent */ + Int order ; /* column's order */ + + /* === Order each non-principal column ================================== */ + + for (i = 0 ; i < n_col ; i++) + { + /* find an un-ordered non-principal column */ + ASSERT (COL_IS_DEAD (i)) ; + if (!COL_IS_DEAD_PRINCIPAL (i) && Col [i].shared2.order == EMPTY) + { + parent = i ; + /* once found, find its principal parent */ + do + { + parent = Col [parent].shared1.parent ; + } while (!COL_IS_DEAD_PRINCIPAL (parent)) ; + + /* now, order all un-ordered non-principal columns along path */ + /* to this parent. collapse tree at the same time */ + c = i ; + /* get order of parent */ + order = Col [parent].shared2.order ; + + do + { + ASSERT (Col [c].shared2.order == EMPTY) ; + + /* order this column */ + Col [c].shared2.order = order++ ; + /* collaps tree */ + Col [c].shared1.parent = parent ; + + /* get immediate parent of this column */ + c = Col [c].shared1.parent ; + + /* continue until we hit an ordered column. There are */ + /* guarranteed not to be anymore unordered columns */ + /* above an ordered column */ + } while (Col [c].shared2.order == EMPTY) ; + + /* re-order the super_col parent to largest order for this group */ + Col [parent].shared2.order = order ; + } + } + + /* === Generate the permutation ========================================= */ + + for (c = 0 ; c < n_col ; c++) + { + p [Col [c].shared2.order] = c ; + } +} + + +/* ========================================================================== */ +/* === detect_super_cols ==================================================== */ +/* ========================================================================== */ + +/* + Detects supercolumns by finding matches between columns in the hash buckets. + Check amongst columns in the set A [row_start ... row_start + row_length-1]. + The columns under consideration are currently *not* in the degree lists, + and have already been placed in the hash buckets. + + The hash bucket for columns whose hash function is equal to h is stored + as follows: + + if head [h] is >= 0, then head [h] contains a degree list, so: + + head [h] is the first column in degree bucket h. + Col [head [h]].headhash gives the first column in hash bucket h. + + otherwise, the degree list is empty, and: + + -(head [h] + 2) is the first column in hash bucket h. + + For a column c in a hash bucket, Col [c].shared3.prev is NOT a "previous + column" pointer. Col [c].shared3.hash is used instead as the hash number + for that column. The value of Col [c].shared4.hash_next is the next column + in the same hash bucket. + + Assuming no, or "few" hash collisions, the time taken by this routine is + linear in the sum of the sizes (lengths) of each column whose score has + just been computed in the approximate degree computation. + Not user-callable. +*/ + +PRIVATE void detect_super_cols +( + /* === Parameters ======================================================= */ + +#ifndef NDEBUG + /* these two parameters are only needed when debugging is enabled: */ + Int n_col, /* number of columns of A */ + Colamd_Row Row [], /* of size n_row+1 */ +#endif /* NDEBUG */ + + Colamd_Col Col [], /* of size n_col+1 */ + Int A [], /* row indices of A */ + Int head [], /* head of degree lists and hash buckets */ + Int row_start, /* pointer to set of columns to check */ + Int row_length /* number of columns to check */ +) +{ + /* === Local variables ================================================== */ + + Int hash ; /* hash value for a column */ + Int *rp ; /* pointer to a row */ + Int c ; /* a column index */ + Int super_c ; /* column index of the column to absorb into */ + Int *cp1 ; /* column pointer for column super_c */ + Int *cp2 ; /* column pointer for column c */ + Int length ; /* length of column super_c */ + Int prev_c ; /* column preceding c in hash bucket */ + Int i ; /* loop counter */ + Int *rp_end ; /* pointer to the end of the row */ + Int col ; /* a column index in the row to check */ + Int head_column ; /* first column in hash bucket or degree list */ + Int first_col ; /* first column in hash bucket */ + + /* === Consider each column in the row ================================== */ + + rp = &A [row_start] ; + rp_end = rp + row_length ; + while (rp < rp_end) + { + col = *rp++ ; + if (COL_IS_DEAD (col)) + { + continue ; + } + + /* get hash number for this column */ + hash = Col [col].shared3.hash ; + ASSERT (hash <= n_col) ; + + /* === Get the first column in this hash bucket ===================== */ + + head_column = head [hash] ; + if (head_column > EMPTY) + { + first_col = Col [head_column].shared3.headhash ; + } + else + { + first_col = - (head_column + 2) ; + } + + /* === Consider each column in the hash bucket ====================== */ + + for (super_c = first_col ; super_c != EMPTY ; + super_c = Col [super_c].shared4.hash_next) + { + ASSERT (COL_IS_ALIVE (super_c)) ; + ASSERT (Col [super_c].shared3.hash == hash) ; + length = Col [super_c].length ; + + /* prev_c is the column preceding column c in the hash bucket */ + prev_c = super_c ; + + /* === Compare super_c with all columns after it ================ */ + + for (c = Col [super_c].shared4.hash_next ; + c != EMPTY ; c = Col [c].shared4.hash_next) + { + ASSERT (c != super_c) ; + ASSERT (COL_IS_ALIVE (c)) ; + ASSERT (Col [c].shared3.hash == hash) ; + + /* not identical if lengths or scores are different */ + if (Col [c].length != length || + Col [c].shared2.score != Col [super_c].shared2.score) + { + prev_c = c ; + continue ; + } + + /* compare the two columns */ + cp1 = &A [Col [super_c].start] ; + cp2 = &A [Col [c].start] ; + + for (i = 0 ; i < length ; i++) + { + /* the columns are "clean" (no dead rows) */ + ASSERT (ROW_IS_ALIVE (*cp1)) ; + ASSERT (ROW_IS_ALIVE (*cp2)) ; + /* row indices will same order for both supercols, */ + /* no gather scatter nessasary */ + if (*cp1++ != *cp2++) + { + break ; + } + } + + /* the two columns are different if the for-loop "broke" */ + if (i != length) + { + prev_c = c ; + continue ; + } + + /* === Got it! two columns are identical =================== */ + + ASSERT (Col [c].shared2.score == Col [super_c].shared2.score) ; + + Col [super_c].shared1.thickness += Col [c].shared1.thickness ; + Col [c].shared1.parent = super_c ; + KILL_NON_PRINCIPAL_COL (c) ; + /* order c later, in order_children() */ + Col [c].shared2.order = EMPTY ; + /* remove c from hash bucket */ + Col [prev_c].shared4.hash_next = Col [c].shared4.hash_next ; + } + } + + /* === Empty this hash bucket ======================================= */ + + if (head_column > EMPTY) + { + /* corresponding degree list "hash" is not empty */ + Col [head_column].shared3.headhash = EMPTY ; + } + else + { + /* corresponding degree list "hash" is empty */ + head [hash] = EMPTY ; + } + } +} + + +/* ========================================================================== */ +/* === garbage_collection =================================================== */ +/* ========================================================================== */ + +/* + Defragments and compacts columns and rows in the workspace A. Used when + all avaliable memory has been used while performing row merging. Returns + the index of the first free position in A, after garbage collection. The + time taken by this routine is linear is the size of the array A, which is + itself linear in the number of nonzeros in the input matrix. + Not user-callable. +*/ + +PRIVATE Int garbage_collection /* returns the new value of pfree */ +( + /* === Parameters ======================================================= */ + + Int n_row, /* number of rows */ + Int n_col, /* number of columns */ + Colamd_Row Row [], /* row info */ + Colamd_Col Col [], /* column info */ + Int A [], /* A [0 ... Alen-1] holds the matrix */ + Int *pfree /* &A [0] ... pfree is in use */ +) +{ + /* === Local variables ================================================== */ + + Int *psrc ; /* source pointer */ + Int *pdest ; /* destination pointer */ + Int j ; /* counter */ + Int r ; /* a row index */ + Int c ; /* a column index */ + Int length ; /* length of a row or column */ + +#ifndef NDEBUG + Int debug_rows ; + DEBUG2 (("Defrag..\n")) ; + for (psrc = &A[0] ; psrc < pfree ; psrc++) ASSERT (*psrc >= 0) ; + debug_rows = 0 ; +#endif /* NDEBUG */ + + /* === Defragment the columns =========================================== */ + + pdest = &A[0] ; + for (c = 0 ; c < n_col ; c++) + { + if (COL_IS_ALIVE (c)) + { + psrc = &A [Col [c].start] ; + + /* move and compact the column */ + ASSERT (pdest <= psrc) ; + Col [c].start = (Int) (pdest - &A [0]) ; + length = Col [c].length ; + for (j = 0 ; j < length ; j++) + { + r = *psrc++ ; + if (ROW_IS_ALIVE (r)) + { + *pdest++ = r ; + } + } + Col [c].length = (Int) (pdest - &A [Col [c].start]) ; + } + } + + /* === Prepare to defragment the rows =================================== */ + + for (r = 0 ; r < n_row ; r++) + { + if (ROW_IS_DEAD (r) || (Row [r].length == 0)) + { + /* This row is already dead, or is of zero length. Cannot compact + * a row of zero length, so kill it. NOTE: in the current version, + * there are no zero-length live rows. Kill the row (for the first + * time, or again) just to be safe. */ + KILL_ROW (r) ; + } + else + { + /* save first column index in Row [r].shared2.first_column */ + psrc = &A [Row [r].start] ; + Row [r].shared2.first_column = *psrc ; + ASSERT (ROW_IS_ALIVE (r)) ; + /* flag the start of the row with the one's complement of row */ + *psrc = ONES_COMPLEMENT (r) ; +#ifndef NDEBUG + debug_rows++ ; +#endif /* NDEBUG */ + } + } + + /* === Defragment the rows ============================================== */ + + psrc = pdest ; + while (psrc < pfree) + { + /* find a negative number ... the start of a row */ + if (*psrc++ < 0) + { + psrc-- ; + /* get the row index */ + r = ONES_COMPLEMENT (*psrc) ; + ASSERT (r >= 0 && r < n_row) ; + /* restore first column index */ + *psrc = Row [r].shared2.first_column ; + ASSERT (ROW_IS_ALIVE (r)) ; + ASSERT (Row [r].length > 0) ; + /* move and compact the row */ + ASSERT (pdest <= psrc) ; + Row [r].start = (Int) (pdest - &A [0]) ; + length = Row [r].length ; + for (j = 0 ; j < length ; j++) + { + c = *psrc++ ; + if (COL_IS_ALIVE (c)) + { + *pdest++ = c ; + } + } + Row [r].length = (Int) (pdest - &A [Row [r].start]) ; + ASSERT (Row [r].length > 0) ; +#ifndef NDEBUG + debug_rows-- ; +#endif /* NDEBUG */ + } + } + /* ensure we found all the rows */ + ASSERT (debug_rows == 0) ; + + /* === Return the new value of pfree ==================================== */ + + return ((Int) (pdest - &A [0])) ; +} + + +/* ========================================================================== */ +/* === clear_mark =========================================================== */ +/* ========================================================================== */ + +/* + Clears the Row [].shared2.mark array, and returns the new tag_mark. + Return value is the new tag_mark. Not user-callable. +*/ + +PRIVATE Int clear_mark /* return the new value for tag_mark */ +( + /* === Parameters ======================================================= */ + + Int tag_mark, /* new value of tag_mark */ + Int max_mark, /* max allowed value of tag_mark */ + + Int n_row, /* number of rows in A */ + Colamd_Row Row [] /* Row [0 ... n_row-1].shared2.mark is set to zero */ +) +{ + /* === Local variables ================================================== */ + + Int r ; + + if (tag_mark <= 0 || tag_mark >= max_mark) + { + for (r = 0 ; r < n_row ; r++) + { + if (ROW_IS_ALIVE (r)) + { + Row [r].shared2.mark = 0 ; + } + } + tag_mark = 1 ; + } + + return (tag_mark) ; +} + + +/* ========================================================================== */ +/* === print_report ========================================================= */ +/* ========================================================================== */ + +PRIVATE void print_report +( + char *method, + Int stats [COLAMD_STATS] +) +{ + + Int i1, i2, i3 ; + + PRINTF (("\n%s version %d.%d, %s: ", method, + COLAMD_MAIN_VERSION, COLAMD_SUB_VERSION, COLAMD_DATE)) ; + + if (!stats) + { + PRINTF (("No statistics available.\n")) ; + return ; + } + + i1 = stats [COLAMD_INFO1] ; + i2 = stats [COLAMD_INFO2] ; + i3 = stats [COLAMD_INFO3] ; + + if (stats [COLAMD_STATUS] >= 0) + { + PRINTF (("OK. ")) ; + } + else + { + PRINTF (("ERROR. ")) ; + } + + switch (stats [COLAMD_STATUS]) + { + + case COLAMD_OK_BUT_JUMBLED: + + PRINTF(("Matrix has unsorted or duplicate row indices.\n")) ; + + PRINTF(("%s: number of duplicate or out-of-order row indices: %d\n", + method, i3)) ; + + PRINTF(("%s: last seen duplicate or out-of-order row index: %d\n", + method, INDEX (i2))) ; + + PRINTF(("%s: last seen in column: %d", + method, INDEX (i1))) ; + + /* no break - fall through to next case instead */ + + case COLAMD_OK: + + PRINTF(("\n")) ; + + PRINTF(("%s: number of dense or empty rows ignored: %d\n", + method, stats [COLAMD_DENSE_ROW])) ; + + PRINTF(("%s: number of dense or empty columns ignored: %d\n", + method, stats [COLAMD_DENSE_COL])) ; + + PRINTF(("%s: number of garbage collections performed: %d\n", + method, stats [COLAMD_DEFRAG_COUNT])) ; + break ; + + case COLAMD_ERROR_A_not_present: + + PRINTF(("Array A (row indices of matrix) not present.\n")) ; + break ; + + case COLAMD_ERROR_p_not_present: + + PRINTF(("Array p (column pointers for matrix) not present.\n")) ; + break ; + + case COLAMD_ERROR_nrow_negative: + + PRINTF(("Invalid number of rows (%d).\n", i1)) ; + break ; + + case COLAMD_ERROR_ncol_negative: + + PRINTF(("Invalid number of columns (%d).\n", i1)) ; + break ; + + case COLAMD_ERROR_nnz_negative: + + PRINTF(("Invalid number of nonzero entries (%d).\n", i1)) ; + break ; + + case COLAMD_ERROR_p0_nonzero: + + PRINTF(("Invalid column pointer, p [0] = %d, must be zero.\n", i1)); + break ; + + case COLAMD_ERROR_A_too_small: + + PRINTF(("Array A too small.\n")) ; + PRINTF((" Need Alen >= %d, but given only Alen = %d.\n", + i1, i2)) ; + break ; + + case COLAMD_ERROR_col_length_negative: + + PRINTF + (("Column %d has a negative number of nonzero entries (%d).\n", + INDEX (i1), i2)) ; + break ; + + case COLAMD_ERROR_row_index_out_of_bounds: + + PRINTF + (("Row index (row %d) out of bounds (%d to %d) in column %d.\n", + INDEX (i2), INDEX (0), INDEX (i3-1), INDEX (i1))) ; + break ; + + case COLAMD_ERROR_out_of_memory: + + PRINTF(("Out of memory.\n")) ; + break ; + + /* v2.4: internal-error case deleted */ + } +} + + + + +/* ========================================================================== */ +/* === colamd debugging routines ============================================ */ +/* ========================================================================== */ + +/* When debugging is disabled, the remainder of this file is ignored. */ + +#ifndef NDEBUG + + +/* ========================================================================== */ +/* === debug_structures ===================================================== */ +/* ========================================================================== */ + +/* + At this point, all empty rows and columns are dead. All live columns + are "clean" (containing no dead rows) and simplicial (no supercolumns + yet). Rows may contain dead columns, but all live rows contain at + least one live column. +*/ + +PRIVATE void debug_structures +( + /* === Parameters ======================================================= */ + + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int A [], + Int n_col2 +) +{ + /* === Local variables ================================================== */ + + Int i ; + Int c ; + Int *cp ; + Int *cp_end ; + Int len ; + Int score ; + Int r ; + Int *rp ; + Int *rp_end ; + Int deg ; + + /* === Check A, Row, and Col ============================================ */ + + for (c = 0 ; c < n_col ; c++) + { + if (COL_IS_ALIVE (c)) + { + len = Col [c].length ; + score = Col [c].shared2.score ; + DEBUG4 (("initial live col %5d %5d %5d\n", c, len, score)) ; + ASSERT (len > 0) ; + ASSERT (score >= 0) ; + ASSERT (Col [c].shared1.thickness == 1) ; + cp = &A [Col [c].start] ; + cp_end = cp + len ; + while (cp < cp_end) + { + r = *cp++ ; + ASSERT (ROW_IS_ALIVE (r)) ; + } + } + else + { + i = Col [c].shared2.order ; + ASSERT (i >= n_col2 && i < n_col) ; + } + } + + for (r = 0 ; r < n_row ; r++) + { + if (ROW_IS_ALIVE (r)) + { + i = 0 ; + len = Row [r].length ; + deg = Row [r].shared1.degree ; + ASSERT (len > 0) ; + ASSERT (deg > 0) ; + rp = &A [Row [r].start] ; + rp_end = rp + len ; + while (rp < rp_end) + { + c = *rp++ ; + if (COL_IS_ALIVE (c)) + { + i++ ; + } + } + ASSERT (i > 0) ; + } + } +} + + +/* ========================================================================== */ +/* === debug_deg_lists ====================================================== */ +/* ========================================================================== */ + +/* + Prints the contents of the degree lists. Counts the number of columns + in the degree list and compares it to the total it should have. Also + checks the row degrees. +*/ + +PRIVATE void debug_deg_lists +( + /* === Parameters ======================================================= */ + + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int head [], + Int min_score, + Int should, + Int max_deg +) +{ + /* === Local variables ================================================== */ + + Int deg ; + Int col ; + Int have ; + Int row ; + + /* === Check the degree lists =========================================== */ + + if (n_col > 10000 && colamd_debug <= 0) + { + return ; + } + have = 0 ; + DEBUG4 (("Degree lists: %d\n", min_score)) ; + for (deg = 0 ; deg <= n_col ; deg++) + { + col = head [deg] ; + if (col == EMPTY) + { + continue ; + } + DEBUG4 (("%d:", deg)) ; + while (col != EMPTY) + { + DEBUG4 ((" %d", col)) ; + have += Col [col].shared1.thickness ; + ASSERT (COL_IS_ALIVE (col)) ; + col = Col [col].shared4.degree_next ; + } + DEBUG4 (("\n")) ; + } + DEBUG4 (("should %d have %d\n", should, have)) ; + ASSERT (should == have) ; + + /* === Check the row degrees ============================================ */ + + if (n_row > 10000 && colamd_debug <= 0) + { + return ; + } + for (row = 0 ; row < n_row ; row++) + { + if (ROW_IS_ALIVE (row)) + { + ASSERT (Row [row].shared1.degree <= max_deg) ; + } + } +} + + +/* ========================================================================== */ +/* === debug_mark =========================================================== */ +/* ========================================================================== */ + +/* + Ensures that the tag_mark is less that the maximum and also ensures that + each entry in the mark array is less than the tag mark. +*/ + +PRIVATE void debug_mark +( + /* === Parameters ======================================================= */ + + Int n_row, + Colamd_Row Row [], + Int tag_mark, + Int max_mark +) +{ + /* === Local variables ================================================== */ + + Int r ; + + /* === Check the Row marks ============================================== */ + + ASSERT (tag_mark > 0 && tag_mark <= max_mark) ; + if (n_row > 10000 && colamd_debug <= 0) + { + return ; + } + for (r = 0 ; r < n_row ; r++) + { + ASSERT (Row [r].shared2.mark < tag_mark) ; + } +} + + +/* ========================================================================== */ +/* === debug_matrix ========================================================= */ +/* ========================================================================== */ + +/* + Prints out the contents of the columns and the rows. +*/ + +PRIVATE void debug_matrix +( + /* === Parameters ======================================================= */ + + Int n_row, + Int n_col, + Colamd_Row Row [], + Colamd_Col Col [], + Int A [] +) +{ + /* === Local variables ================================================== */ + + Int r ; + Int c ; + Int *rp ; + Int *rp_end ; + Int *cp ; + Int *cp_end ; + + /* === Dump the rows and columns of the matrix ========================== */ + + if (colamd_debug < 3) + { + return ; + } + DEBUG3 (("DUMP MATRIX:\n")) ; + for (r = 0 ; r < n_row ; r++) + { + DEBUG3 (("Row %d alive? %d\n", r, ROW_IS_ALIVE (r))) ; + if (ROW_IS_DEAD (r)) + { + continue ; + } + DEBUG3 (("start %d length %d degree %d\n", + Row [r].start, Row [r].length, Row [r].shared1.degree)) ; + rp = &A [Row [r].start] ; + rp_end = rp + Row [r].length ; + while (rp < rp_end) + { + c = *rp++ ; + DEBUG4 ((" %d col %d\n", COL_IS_ALIVE (c), c)) ; + } + } + + for (c = 0 ; c < n_col ; c++) + { + DEBUG3 (("Col %d alive? %d\n", c, COL_IS_ALIVE (c))) ; + if (COL_IS_DEAD (c)) + { + continue ; + } + DEBUG3 (("start %d length %d shared1 %d shared2 %d\n", + Col [c].start, Col [c].length, + Col [c].shared1.thickness, Col [c].shared2.score)) ; + cp = &A [Col [c].start] ; + cp_end = cp + Col [c].length ; + while (cp < cp_end) + { + r = *cp++ ; + DEBUG4 ((" %d row %d\n", ROW_IS_ALIVE (r), r)) ; + } + } +} + +PRIVATE void colamd_get_debug +( + char *method +) +{ + FILE *f ; + colamd_debug = 0 ; /* no debug printing */ + f = fopen ("debug", "r") ; + if (f == (FILE *) NULL) + { + colamd_debug = 0 ; + } + else + { + fscanf (f, "%d", &colamd_debug) ; + fclose (f) ; + } + DEBUG0 (("%s: debug version, D = %d (THIS WILL BE SLOW!)\n", + method, colamd_debug)) ; +} + +#endif /* NDEBUG */ diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Source/colamd_global.c b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Source/colamd_global.c new file mode 100644 index 0000000..88a2aed --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COLAMD/Source/colamd_global.c @@ -0,0 +1,24 @@ +/* ========================================================================== */ +/* === colamd_global.c ====================================================== */ +/* ========================================================================== */ + +/* ---------------------------------------------------------------------------- + * COLAMD, Copyright (C) 2007, Timothy A. Davis. + * See License.txt for the Version 2.1 of the GNU Lesser General Public License + * http://www.suitesparse.com + * -------------------------------------------------------------------------- */ + +/* Global variables for COLAMD */ + +#ifndef NPRINT +#ifdef MATLAB_MEX_FILE +#include "mex.h" +int (*colamd_printf) (const char *, ...) = mexPrintf ; +#else +#include +int (*colamd_printf) (const char *, ...) = printf ; +#endif +#else +int (*colamd_printf) (const char *, ...) = ((void *) 0) ; +#endif + diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COPYING.TXT b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COPYING.TXT new file mode 100644 index 0000000..fc8a5de --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/COPYING.TXT @@ -0,0 +1,165 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + + This version of the GNU Lesser General Public License incorporates +the terms and conditions of version 3 of the GNU General Public +License, supplemented by the additional permissions listed below. + + 0. Additional Definitions. + + As used herein, "this License" refers to version 3 of the GNU Lesser +General Public License, and the "GNU GPL" refers to version 3 of the GNU +General Public License. + + "The Library" refers to a covered work governed by this License, +other than an Application or a Combined Work as defined below. + + An "Application" is any work that makes use of an interface provided +by the Library, but which is not otherwise based on the Library. +Defining a subclass of a class defined by the Library is deemed a mode +of using an interface provided by the Library. + + A "Combined Work" is a work produced by combining or linking an +Application with the Library. The particular version of the Library +with which the Combined Work was made is also called the "Linked +Version". + + The "Minimal Corresponding Source" for a Combined Work means the +Corresponding Source for the Combined Work, excluding any source code +for portions of the Combined Work that, considered in isolation, are +based on the Application, and not on the Linked Version. + + The "Corresponding Application Code" for a Combined Work means the +object code and/or source code for the Application, including any data +and utility programs needed for reproducing the Combined Work from the +Application, but excluding the System Libraries of the Combined Work. + + 1. Exception to Section 3 of the GNU GPL. + + You may convey a covered work under sections 3 and 4 of this License +without being bound by section 3 of the GNU GPL. + + 2. Conveying Modified Versions. + + If you modify a copy of the Library, and, in your modifications, a +facility refers to a function or data to be supplied by an Application +that uses the facility (other than as an argument passed when the +facility is invoked), then you may convey a copy of the modified +version: + + a) under this License, provided that you make a good faith effort to + ensure that, in the event an Application does not supply the + function or data, the facility still operates, and performs + whatever part of its purpose remains meaningful, or + + b) under the GNU GPL, with none of the additional permissions of + this License applicable to that copy. + + 3. Object Code Incorporating Material from Library Header Files. + + The object code form of an Application may incorporate material from +a header file that is part of the Library. You may convey such object +code under terms of your choice, provided that, if the incorporated +material is not limited to numerical parameters, data structure +layouts and accessors, or small macros, inline functions and templates +(ten or fewer lines in length), you do both of the following: + + a) Give prominent notice with each copy of the object code that the + Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the object code with a copy of the GNU GPL and this license + document. + + 4. Combined Works. + + You may convey a Combined Work under terms of your choice that, +taken together, effectively do not restrict modification of the +portions of the Library contained in the Combined Work and reverse +engineering for debugging such modifications, if you also do each of +the following: + + a) Give prominent notice with each copy of the Combined Work that + the Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the Combined Work with a copy of the GNU GPL and this license + document. + + c) For a Combined Work that displays copyright notices during + execution, include the copyright notice for the Library among + these notices, as well as a reference directing the user to the + copies of the GNU GPL and this license document. + + d) Do one of the following: + + 0) Convey the Minimal Corresponding Source under the terms of this + License, and the Corresponding Application Code in a form + suitable for, and under terms that permit, the user to + recombine or relink the Application with a modified version of + the Linked Version to produce a modified Combined Work, in the + manner specified by section 6 of the GNU GPL for conveying + Corresponding Source. + + 1) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (a) uses at run time + a copy of the Library already present on the user's computer + system, and (b) will operate properly with a modified version + of the Library that is interface-compatible with the Linked + Version. + + e) Provide Installation Information, but only if you would otherwise + be required to provide such information under section 6 of the + GNU GPL, and only to the extent that such information is + necessary to install and execute a modified version of the + Combined Work produced by recombining or relinking the + Application with a modified version of the Linked Version. (If + you use option 4d0, the Installation Information must accompany + the Minimal Corresponding Source and Corresponding Application + Code. If you use option 4d1, you must provide the Installation + Information in the manner specified by section 6 of the GNU GPL + for conveying Corresponding Source.) + + 5. Combined Libraries. + + You may place library facilities that are a work based on the +Library side by side in a single library together with other library +facilities that are not Applications and are not covered by this +License, and convey such a combined library under terms of your +choice, if you do both of the following: + + a) Accompany the combined library with a copy of the same work based + on the Library, uncombined with any other library facilities, + conveyed under the terms of this License. + + b) Give prominent notice with the combined library that part of it + is a work based on the Library, and explaining where to find the + accompanying uncombined form of the same work. + + 6. Revised Versions of the GNU Lesser General Public License. + + The Free Software Foundation may publish revised and/or new versions +of the GNU Lesser General Public License from time to time. Such new +versions will be similar in spirit to the present version, but may +differ in detail to address new problems or concerns. + + Each version is given a distinguishing version number. If the +Library as you received it specifies that a certain numbered version +of the GNU Lesser General Public License "or any later version" +applies to it, you have the option of following the terms and +conditions either of that published version or of any later version +published by the Free Software Foundation. If the Library as you +received it does not specify a version number of the GNU Lesser +General Public License, you may choose any version of the GNU Lesser +General Public License ever published by the Free Software Foundation. + + If the Library as you received it specifies that a proxy can decide +whether future versions of the GNU Lesser General Public License shall +apply, that proxy's public statement of acceptance of any version is +permanent authorization for you to choose that version for the +Library. diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Config/local_config.cmake b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Config/local_config.cmake new file mode 100644 index 0000000..d8c64a1 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Config/local_config.cmake @@ -0,0 +1,8 @@ +# Other optional libraries +enable_feature (V3DLIB_ENABLE_SUITESPARSE) +enable_feature_inc_path (V3DLIB_ENABLE_SUITESPARSE /usr/include/suitesparse) +enable_feature_libraries (V3DLIB_ENABLE_SUITESPARSE colamd) + +# Debug/release mode selection +set (CMAKE_BUILD_TYPE Release) +#set (CMAKE_BUILD_TYPE Debug) diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Config/local_config.cmake.ubuntu_10.04_x86_64 b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Config/local_config.cmake.ubuntu_10.04_x86_64 new file mode 100644 index 0000000..d8c64a1 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Config/local_config.cmake.ubuntu_10.04_x86_64 @@ -0,0 +1,8 @@ +# Other optional libraries +enable_feature (V3DLIB_ENABLE_SUITESPARSE) +enable_feature_inc_path (V3DLIB_ENABLE_SUITESPARSE /usr/include/suitesparse) +enable_feature_libraries (V3DLIB_ENABLE_SUITESPARSE colamd) + +# Debug/release mode selection +set (CMAKE_BUILD_TYPE Release) +#set (CMAKE_BUILD_TYPE Debug) diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Config/local_config.cmake.windows_x86 b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Config/local_config.cmake.windows_x86 new file mode 100644 index 0000000..3e68299 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Config/local_config.cmake.windows_x86 @@ -0,0 +1,105 @@ +############################################################################### +# Set your paths, and enable the features you want to use. + +# Paths +set (BIAS_DIR "D:/Libraries/BIAS-2.6.1") +set (CG_DIR "C:/Program Files/NVIDIA Corporation/Cg") +set (CUDA_DIR "C:/CUDA") +set (GL_DIR "D:/Libraries/OpenGL") +set (GLU_DIR "D:/Libraries/OpenGL") +set (GLEW_DIR "D:/Libraries/OpenGL") +set (IMDEBUG_DIR "D:/Libraries/imdebug") +set (JPEG_DIR "D:/Libraries/JPEG") +set (LAPACK_DIR "D:/Libraries/LAPACK") +set (MRF_DIR "D:/Libraries/MRF2.1") +set (OPENCV_DIR "C:/Program Files/OpenCV") +set (PNG_DIR "D:/Libraries/PNG") +set (SIFTGPU_DIR "D:/Libraries/SiftGPU") +set (SUITESPARSE_DIR "D:/Libraries/SuiteSparse") + +# Debug/release mode selection +set (CMAKE_BUILD_TYPE Release) +#set (CMAKE_BUILD_TYPE Debug) + +# Optional libraries +#enable_feature (V3DLIB_ENABLE_FFMPEG) +enable_feature (V3DLIB_ENABLE_GPGPU) +enable_feature (V3DLIB_ENABLE_IMDEBUG) +enable_feature (V3DLIB_ENABLE_CUDA) +enable_feature (V3DLIB_ENABLE_LIBJPEG) +enable_feature (V3DLIB_ENABLE_LIBPNG) +enable_feature (V3DLIB_ENABLE_MRF) +enable_feature (V3DLIB_ENABLE_OPENCV) +enable_feature (V3DLIB_ENABLE_SIFTGPU) +enable_feature (V3DLIB_ENABLE_SOCKETS) +enable_feature (V3DLIB_ENABLE_SUITESPARSE) + + +############################################################################## +# You shouldn't have to change anything in this section. + +# Settings for compiling under Windows with VisualStudio. +# Warning level +add_definitions ("/W1") +# NOMINMAX for compatibility with windows.h and STL +add_definitions (-DNOMINMAX) + +# BIAS related dirs and settings. Note that BIAS is always required. +enable_feature (V3DLIB_ENABLE_BIAS) +enable_feature_inc_path (V3DLIB_ENABLE_BIAS ${BIAS_DIR}) +enable_feature_lib_path (V3DLIB_ENABLE_BIAS ${BIAS_DIR}/lib/${CMAKE_BUILD_TYPE} ${LAPACK_DIR}) +enable_feature_libraries (V3DLIB_ENABLE_BIAS BIASGeometry BIASGeometryBase BIASMathAlgo BIASMathBase BIASImagebase BIASDebug BIASCommon Lapack Blas) + +# Library paths +# GPGPU +enable_feature_inc_path (V3DLIB_ENABLE_GPGPU ${GL_DIR}/include ${GLU_DIR}/include ${GLEW_DIR}/include) +enable_feature_lib_path (V3DLIB_ENABLE_GPGPU ${GL_DIR}/lib ${GLU_DIR}/lib ${GLEW_DIR}/lib) +enable_feature_libraries (V3DLIB_ENABLE_GPGPU GLEW32 GLU32 OPENGL32) +set (EXTRA_LIBS ${EXTRA_LIBS} glut32) +# Cg +enable_conditional_feature (V3DLIB_GPGPU_ENABLE_CG V3DLIB_ENABLE_GPGPU) +enable_feature_inc_path (V3DLIB_GPGPU_ENABLE_CG ${CG_DIR}/include) +enable_feature_lib_path (V3DLIB_GPGPU_ENABLE_CG ${CG_DIR}/lib) +enable_feature_libraries (V3DLIB_GPGPU_ENABLE_CG Cg CgGL) +# CUDA +enable_feature_inc_path (V3DLIB_ENABLE_CUDA ${CUDA_DIR}/include) +enable_feature_lib_path (V3DLIB_ENABLE_CUDA ${CUDA_DIR}/lib) +enable_feature_libraries (V3DLIB_ENABLE_CUDA cublas cudart) +set (OBJEXT obj) +set (EXTRA_NVCC_ARGS -DWIN32 -D_CONSOLE -D_MBCS -Xcompiler /EHsc,/W3,/nologo,/Wp64,/O2,/Zi,/MD ) +# OpenCV +enable_feature_inc_path (V3DLIB_ENABLE_OPENCV ${OPENCV_DIR}/cv/include ${OPENCV_DIR}/cxcore/include ${OPENCV_DIR}/otherlibs/highgui) +enable_feature_lib_path (V3DLIB_ENABLE_OPENCV ${OPENCV_DIR}/lib) +enable_feature_libraries (V3DLIB_ENABLE_OPENCV highgui cvaux cv cxcore) +# ffmpeg +enable_feature_libraries (V3DLIB_ENABLE_FFMPEG avcodec avformat avcodec avutil) +# imdebug +enable_feature_inc_path (V3DLIB_ENABLE_IMDEBUG ${IMDEBUG_DIR}) +enable_feature_lib_path (V3DLIB_ENABLE_IMDEBUG ${IMDEBUG_DIR}) +enable_feature_libraries (V3DLIB_ENABLE_IMDEBUG imdebug) +# JPEG +enable_feature_inc_path (V3DLIB_ENABLE_LIBJPEG ${JPEG_DIR}) +enable_feature_lib_path (V3DLIB_ENABLE_LIBJPEG ${JPEG_DIR}) +enable_feature_libraries (V3DLIB_ENABLE_LIBJPEG jpeglib) +# PNG +enable_feature_inc_path (V3DLIB_ENABLE_LIBPNG ${PNG_DIR}/include ${PNG_DIR}) +enable_feature_lib_path (V3DLIB_ENABLE_LIBPNG ${PNG_DIR}/lib ${PNG_DIR}) +enable_feature_libraries (V3DLIB_ENABLE_LIBPNG libpng) +# MRF +enable_feature_inc_path (V3DLIB_ENABLE_MRF ${MRF_DIR}) +enable_feature_lib_path (V3DLIB_ENABLE_MRF ${MRF_DIR}) +enable_feature_libraries (V3DLIB_ENABLE_MRF MRF) +# SiftGPU +enable_feature_inc_path (V3DLIB_ENABLE_SIFTGPU ${SIFTGPU_DIR}/SiftGPU/src) +enable_feature_lib_path (V3DLIB_ENABLE_SIFTGPU ${SIFTGPU_DIR}/SiftGPU/bin) +enable_feature_libraries (V3DLIB_ENABLE_SIFTGPU SiftGPU) +# Sockets +if (V3DLIB_ENABLE_SOCKETS) + set (EXTRA_LIBS ${EXTRA_LIBS} Ws2_32 ) +endif (V3DLIB_ENABLE_SOCKETS) +# SuiteSparse +enable_feature_inc_path (V3DLIB_ENABLE_SUITESPARSE ${SUITESPARSE_DIR}/COLAMD/Include ${SUITESPARSE_DIR}/UFconfig) +enable_feature_lib_path (V3DLIB_ENABLE_SUITESPARSE ${SUITESPARSE_DIR}) +enable_feature_libraries (V3DLIB_ENABLE_SUITESPARSE SuiteSparse) +############################################################################## + diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Config/v3d_macros.cmake b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Config/v3d_macros.cmake new file mode 100644 index 0000000..1a41cf8 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Config/v3d_macros.cmake @@ -0,0 +1,48 @@ +# -*- CMake -*- + +macro (enable_feature feature) + set (${feature} 1) + add_definitions(-D${feature}) +endmacro (enable_feature) + +macro (enable_conditional_feature feature dep_feature) + if (${dep_feature}) + set (${feature} 1) + add_definitions(-D${feature}) + endif (${dep_feature}) +endmacro (enable_conditional_feature) + +macro (enable_feature_inc_path feature) + if (${feature}) + set (EXTRA_INC_DIRS ${EXTRA_INC_DIRS} ${ARGN}) + endif (${feature}) +endmacro (enable_feature_inc_path) + +macro (enable_feature_lib_path feature) + if (${feature}) + set (EXTRA_LIB_DIRS ${EXTRA_LIB_DIRS} ${ARGN}) + endif (${feature}) +endmacro (enable_feature_lib_path) + +macro (enable_feature_libraries feature) + if (${feature}) + set (EXTRA_LIBRARIES ${EXTRA_LIBRARIES} ${ARGN}) + endif (${feature}) +endmacro (enable_feature_libraries) + +macro (add_v3d_executable target) + add_executable(${target} ${ARGN}) + add_dependencies(${target} V3D) +endmacro (add_v3d_executable) + +macro (add_v3d_executable_with_openmp target) + add_executable(${target} ${ARGN}) + add_dependencies(${target} V3D) + set_target_properties (${target} PROPERTIES LINK_FLAGS -fopenmp) + set_source_files_properties (${ARGN} PROPERTIES COMPILE_FLAGS -fopenmp) +endmacro (add_v3d_executable_with_openmp) + +macro (add_simple_v3d_executable target) + add_executable(${target} ${target}.cpp) + add_dependencies(${target} V3D) +endmacro (add_simple_v3d_executable) diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_cameramatrix.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_cameramatrix.h new file mode 100644 index 0000000..9886b25 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_cameramatrix.h @@ -0,0 +1,345 @@ +// -*- C++ -*- + +#ifndef V3D_CAMERA_MATRIX_H +#define V3D_CAMERA_MATRIX_H + +#include "Base/v3d_serialization.h" +#include "Math/v3d_linear.h" +#include "Geometry/v3d_distortion.h" + +namespace V3D +{ + + struct CameraMatrix + { + CameraMatrix() + { + makeIdentityMatrix(_K); + makeIdentityMatrix(_R); + makeZeroVector(_T); + this->updateCachedValues(true, true); + } + + CameraMatrix(double f, double cx, double cy) + { + makeIdentityMatrix(_K); + _K[0][0] = f; + _K[1][1] = f; + _K[0][2] = cx; + _K[1][2] = cy; + makeIdentityMatrix(_R); + makeZeroVector(_T); + this->updateCachedValues(true, true); + } + + CameraMatrix(Matrix3x3d const& K, + Matrix3x3d const& R, + Vector3d const& T) + : _K(K), _R(R), _T(T) + { + this->updateCachedValues(true, true); + } + + CameraMatrix(Matrix3x3d const& K, + Matrix3x4d const& RT) + : _K(K) + { + _R = RT.slice<3,3>(0,0); + _T = RT.col(3); + this->updateCachedValues(true, true); + } + + template T sqr(T t) { return t*t; } + + CameraMatrix(Matrix3x4d const& P) + { + Matrix3x3d K; + copyMatrixSlice(P, 0, 0, 3, 3, K, 0, 0); + + // get affine matrix (rq-decomposition of M) + // See Hartley & Zissermann, p552 (1st ed.) + double h = sqrt(sqr(K[2][1]) + sqr(K[2][2])); + double s = K[2][1] / h; + double c = -K[2][2] / h; + + Matrix3x3d Rx; makeZeroMatrix(Rx); + Rx[0][0] = 1; + Rx[1][1] = c; Rx[2][2] = c; + Rx[1][2] = -s; Rx[2][1] = s; + + K = K * Rx; + + h = sqrt(sqr(K[2][0]) + sqr(K[2][2])); + s = K[2][0] / h; + c = -K[2][2] / h; + + Matrix3x3d Ry; makeZeroMatrix(Ry); + Ry[1][1] = 1; + Ry[0][0] = c; Ry[2][2] = c; + Ry[0][2] = -s; Ry[2][0] = s; + + K = K * Ry; + + h = sqrt(sqr(K[1][0]) + sqr(K[1][1])); + s = K[1][0] / h; + c = -K[1][1] / h; + + Matrix3x3d Rz; makeZeroMatrix(Rz); + Rz[2][2] = 1; + Rz[0][0] = c; Rz[1][1] = c; + Rz[0][1] = -s; Rz[1][0] = s; + + K = K * Rz; + + Matrix3x3d Sign; + makeIdentityMatrix(Sign); + + if (K[0][0] < 0) Sign[0][0] = -1; + if (K[1][1] < 0) Sign[1][1] = -1; + if (K[2][2] < 0) Sign[2][2] = -1; + + K = K * Sign; // Change signum of columns + + Matrix3x3d R = Rx * Ry * Rz * Sign; + transposeMatrixIP(R); + + Vector3d P4; + P.getColumnSlice(0, 3, 3, P4); + Vector3d T = invertedMatrix(K) * P4; + + scaleMatrixIP(1.0 / K[2][2], K); // Normalize, such that lower-right element is 1. + + _K = K; + _R = R; + _T = T; + this->updateCachedValues(true, true); + } + + CameraMatrix & operator = ( const CameraMatrix &cam ) + { + _K = cam.getIntrinsic(); + _R = cam.getRotation(); + _T = cam.getTranslation(); + _size=cam._size; + this->updateCachedValues(true, true); + return *this; + } + + void setIntrinsic(Matrix3x3d const& K) { _K = K; this->updateCachedValues(true, false); } + void setRotation(Matrix3x3d const& R) { _R = R; this->updateCachedValues(false, true); } + void setTranslation(Vector3d const& T) { _T = T; this->updateCachedValues(false, true); } + + void setCameraCenter(Vector3d const& c) { this->setTranslation(-1.0 * (_R * c)); } + + template + void setOrientation(Mat const& RT) + { + _R[0][0] = RT[0][0]; _R[0][1] = RT[0][1]; _R[0][2] = RT[0][2]; + _R[1][0] = RT[1][0]; _R[1][1] = RT[1][1]; _R[1][2] = RT[1][2]; + _R[2][0] = RT[2][0]; _R[2][1] = RT[2][1]; _R[2][2] = RT[2][2]; + _T[0] = RT[0][3]; _T[1] = RT[1][3]; _T[2] = RT[2][3]; + this->updateCachedValues(false, true); + } + + Matrix3x3d const& getIntrinsic() const { return _K; } + Matrix3x3d const& getRotation() const { return _R; } + Vector3d const& getTranslation() const { return _T; } + + Matrix3x4d getExtrinsic() const + { + Matrix3x4d RT; + copyMatrixSlice(_R,0,0,3,3,RT,0,0); + RT[0][3] = _T[0]; + RT[1][3] = _T[1]; + RT[2][3] = _T[2]; + return RT; + } + + Matrix3x4d getOrientation() const + { + Matrix3x4d RT; + RT[0][0] = _R[0][0]; RT[0][1] = _R[0][1]; RT[0][2] = _R[0][2]; + RT[1][0] = _R[1][0]; RT[1][1] = _R[1][1]; RT[1][2] = _R[1][2]; + RT[2][0] = _R[2][0]; RT[2][1] = _R[2][1]; RT[2][2] = _R[2][2]; + RT[0][3] = _T[0]; RT[1][3] = _T[1]; RT[2][3] = _T[2]; + return RT; + } + + Matrix3x4d getProjection() const + { + Matrix3x4d const RT = this->getOrientation(); + return _K * RT; + } + + double getFocalLength() const { return _K[0][0]; } + double getAspectRatio() const { return _K[1][1] / _K[0][0]; } + + Vector2d getPrincipalPoint() const + { + Vector2d pp; + pp[0] = _K[0][2]; + pp[1] = _K[1][2]; + return pp; + } + + Vector2d projectPoint(Vector3d const& X) const + { + Vector3d q = _K*(_R*X + _T); + Vector2d res; + res[0] = q[0]/q[2]; res[1] = q[1]/q[2]; + return res; + } + + template + Vector2d projectPoint(Distortion const& distortion, Vector3d const& X) const + { + Vector3d XX = _R*X + _T; + Vector2d p; + p[0] = XX[0] / XX[2]; + p[1] = XX[1] / XX[2]; + p = distortion(p); + + Vector2d res; + res[0] = _K[0][0] * p[0] + _K[0][1] * p[1] + _K[0][2]; + res[1] = _K[1][1] * p[1] + _K[1][2]; + return res; + } + template + Vector3d unprojectPixel(InlineVector const &p, double depth = 1) const + { + Vector3d pp; + pp[0] = p[0]; pp[1] = p[1]; pp[2] = 1.0; + Vector3d ray = _invK * pp; + ray[0] *= depth/ray[2]; + ray[1] *= depth/ray[2]; + ray[2] = depth; + ray = _Rt * ray; + return _center + ray; + } + + Vector3d intersectRayWithPlane( const Vector4d &plane, int x, int y ) const + { + Vector3d ray = getRay(Vector2d(x,y)); + double rho = (-innerProduct(plane.slice<3>(0),_center) - plane[3]) / + innerProduct(plane.slice<3>(0),ray); + return _center + rho*ray; + } + + Vector3d transformPointIntoCameraSpace(Vector3d const& p) const + { + return _R*p + _T; + } + + Vector3d transformPointFromCameraSpace(Vector3d const& p) const + { + return _Rt*(p-_T); + } + + Vector3d transformDirectionFromCameraSpace(Vector3d const& dir) const + { + return _Rt*dir; + } + + Vector3d transformDirectionIntoCameraSpace(Vector3d const& dir) const + { + return _R*dir; + } + template + InlineVector transformPointIntoNormalizedCoordinate(InlineVector const& p) const + { + return InlineVector(_K[0][0] * p[0] + _K[0][1] * p[1] + _K[0][2],_K[1][1] * p[1] + _K[1][2]); + } + template + InlineVector transformPointFromNormalizedCoordinate(InlineVector const& p) const + { + return InlineVector(_invK[0][0] * p[0] + _invK[0][1] * p[1] + _invK[0][2],_invK[1][1] * p[1] + _invK[1][2]); + } + Vector3d const& cameraCenter() const + { + return _center; + } + + Vector3d opticalAxis() const + { + return this->transformDirectionFromCameraSpace(Vector3d(0.0, 0.0, 1.0)); + } + + Vector3d upVector() const + { + return this->transformDirectionFromCameraSpace(Vector3d(0.0, 1.0, 0.0)); + } + + Vector3d rightVector() const + { + return this->transformDirectionFromCameraSpace(Vector3d(1.0, 0.0, 0.0)); + } + + template + Vector3d getRay(Vec2 const& p) const + { + Vector3d pp = Vector3d(p[0], p[1], 1.0); + Vector3d ray = _invK * pp; + ray = _Rt * ray; + normalizeVector(ray); + return ray; + } + + template + Vector3d getCameraRay(Vec2 const& p) const + { + return _invK * Vector3d(p[0], p[1], 1.0); + } + + template + inline bool isOnGoodSide(Vec3 const &p) const + { + return transformPointIntoCameraSpace(p)[2]>0; + } + template + void serialize(Archive& ar) + { + V3D::SerializationScope scope(ar, "CameraMatrix"); + ar & _K & _R & _T; + if (ar.isLoading()) + this->updateCachedValues(true, true); + } + + float getWidth() const + { + return _size[0]; + } + float getHeight() const + { + return _size[1]; + } + void setSize(float w,float h) + { + _size[0]=w; + _size[1]=h; + } + V3D_DEFINE_LOAD_SAVE(CameraMatrix) + + protected: + void updateCachedValues(bool intrinsic, bool orientation) + { + if (intrinsic) _invK = invertedMatrix(_K); + + if (orientation) + { + makeTransposedMatrix(_R, _Rt); + _center = _Rt * (-1.0 * _T); + } + } + + Matrix3x3d _K, _R; + Vector3d _T; + Matrix3x3d _invK, _Rt; + Vector3d _center; + Vector2f _size; + }; // end struct CameraMatrix + + V3D_DEFINE_IOSTREAM_OPS(CameraMatrix) + +} // end namespace V3D + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_distortion.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_distortion.h new file mode 100644 index 0000000..ba7ba80 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_distortion.h @@ -0,0 +1,165 @@ +// -*- C++ -*- +#ifndef V3D_DISTORTION_H +#define V3D_DISTORTION_H + +#include "Math/v3d_linear.h" + +namespace V3D +{ + + struct StdDistortionFunction + { + double k1, k2, p1, p2; + + StdDistortionFunction() + : k1(0), k2(0), p1(0), p2(0) + { } + + Vector2d operator()(Vector2d const& xu) const + { + double const r2 = xu[0]*xu[0] + xu[1]*xu[1]; + double const r4 = r2*r2; + double const kr = 1 + k1*r2 + k2*r4; + + Vector2d xd; + xd[0] = kr * xu[0] + 2*p1*xu[0]*xu[1] + p2*(r2 + 2*xu[0]*xu[0]); + xd[1] = kr * xu[1] + 2*p2*xu[0]*xu[1] + p1*(r2 + 2*xu[1]*xu[1]); + return xd; + } + + Matrix2x2d derivativeWrtRadialParameters(Vector2d const& xu) const + { + double const r2 = xu[0]*xu[0] + xu[1]*xu[1]; + double const r4 = r2*r2; + //double const kr = 1 + k1*r2 + k2*r4; + + Matrix2x2d deriv; + + deriv[0][0] = xu[0] * r2; // d xd/d k1 + deriv[0][1] = xu[0] * r4; // d xd/d k2 + deriv[1][0] = xu[1] * r2; // d yd/d k1 + deriv[1][1] = xu[1] * r4; // d yd/d k2 + return deriv; + } + + Matrix2x2d derivativeWrtTangentialParameters(Vector2d const& xu) const + { + double const r2 = xu[0]*xu[0] + xu[1]*xu[1]; + //double const r4 = r2*r2; + //double const kr = 1 + k1*r2 + k2*r4; + + Matrix2x2d deriv; + deriv[0][0] = 2*xu[0]*xu[1]; // d xd/d p1 + deriv[0][1] = r2 + 2*xu[0]*xu[0]; // d xd/d p2 + deriv[1][0] = r2 + 2*xu[1]*xu[1]; // d yd/d p1 + deriv[1][1] = deriv[0][0]; // d yd/d p2 + return deriv; + } + + Matrix2x2d derivativeWrtUndistortedPoint(Vector2d const& xu) const + { + double const r2 = xu[0]*xu[0] + xu[1]*xu[1]; + double const r4 = r2*r2; + double const kr = 1 + k1*r2 + k2*r4; + double const dkr = 2*k1 + 4*k2*r2; + + Matrix2x2d deriv; + deriv[0][0] = kr + xu[0] * xu[0] * dkr + 2*p1*xu[1] + 6*p2*xu[0]; // d xd/d xu + deriv[0][1] = xu[0] * xu[1] * dkr + 2*p1*xu[0] + 2*p2*xu[1]; // d xd/d yu + deriv[1][0] = deriv[0][1]; // d yd/d xu + deriv[1][1] = kr + xu[1] * xu[1] * dkr + 6*p1*xu[1] + 2*p2*xu[0]; // d yd/d yu + return deriv; + } + }; // end struct StdDistortionFunction + + struct StdPixelDistortionFunction + { + double k1, k2, p1, p2; + Vector2d center; + + StdPixelDistortionFunction() + : k1(0), k2(0), p1(0), p2(0) + { + makeZeroVector(center); + } + + Vector2d operator()(Vector2d const& xu) const + { + Vector2d const np = xu - center; + double const r2 = np[0]*np[0] + np[1]*np[1]; + double const r4 = r2*r2; + double const kr = 1 + k1*r2 + k2*r4; + + Vector2d xd; + xd[0] = kr * np[0] + center[0] + 2*p1*np[0]*np[1] + p2*(r2 + 2*np[0]*np[0]); + xd[1] = kr * np[1] + center[1] + 2*p2*np[0]*np[1] + p1*(r2 + 2*np[1]*np[1]); + return xd; + } + + Matrix2x2d derivativeWrtRadialParameters(Vector2d const& xu) const + { + Vector2d const np = xu - center; + double const r2 = np[0]*np[0] + np[1]*np[1]; + double const r4 = r2*r2; + //double const kr = 1 + k1*r2 + k2*r4; + + Matrix2x2d deriv; + + deriv[0][0] = np[0] * r2; // d xd/d k1 + deriv[0][1] = np[0] * r4; // d xd/d k2 + deriv[1][0] = np[1] * r2; // d yd/d k1 + deriv[1][1] = np[1] * r4; // d yd/d k2 + return deriv; + } + + Matrix2x2d derivativeWrtTangentialParameters(Vector2d const& xu) const + { + Vector2d const np = xu - center; + double const r2 = np[0]*np[0] + np[1]*np[1]; + //double const r4 = r2*r2; + //double const kr = 1 + k1*r2 + k2*r4; + + Matrix2x2d deriv; + deriv[0][0] = 2*np[0]*np[1]; // d xd/d p1 + deriv[0][1] = r2 + 2*np[0]*np[0]; // d xd/d p2 + deriv[1][0] = r2 + 2*np[1]*np[1]; // d yd/d p1 + deriv[1][1] = deriv[0][0]; // d yd/d p2 + return deriv; + } + + Matrix2x2d derivativeWrtCenter(Vector2d const& xu) const + { + Vector2d const np = xu - center; + double const r2 = np[0]*np[0] + np[1]*np[1]; + double const r4 = r2*r2; + double const kr = 1 + k1*r2 + k2*r4; + double const dkr = 2*k1 + 4*k2*r2; + + Matrix2x2d deriv; + deriv[0][0] = 1 - kr - np[0]*np[0] * dkr - 2*p1*np[1] - 6*p2*np[0]; // d xd/d cx + deriv[0][1] = - np[0]*np[1] * dkr - 2*p1*np[0] - 2*p2*np[1]; // d xd/d cy + deriv[1][0] = deriv[0][1]; // d yd/d cx + deriv[1][1] = 1 - kr - np[1]*np[1] * dkr - 6*p1*np[1] - 2*p2*np[0]; // d yd/d cy + return deriv; + } + + Matrix2x2d derivativeWrtUndistortedPoint(Vector2d const& xu) const + { + Vector2d const np = xu - center; + double const r2 = np[0]*np[0] + np[1]*np[1]; + double const r4 = r2*r2; + double const kr = 1 + k1*r2 + k2*r4; + double const dkr = 2*k1 + 4*k2*r2; + + Matrix2x2d deriv; + deriv[0][0] = kr + np[0] * np[0] * dkr + 2*p1*np[1] + 6*p2*np[0]; // d xd/d xu + deriv[0][1] = np[0] * np[1] * dkr + 2*p1*np[0] + 2*p2*np[1]; // d xd/d yu + deriv[1][0] = deriv[0][1]; // d yd/d xu + deriv[1][1] = kr + np[1] * np[1] * dkr + 6*p1*np[1] + 2*p2*np[0]; // d yd/d yu + return deriv; + } + }; // end struct StdPixelDistortionFunction + +} // end namespace V3D + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_metricbundle.cpp b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_metricbundle.cpp new file mode 100644 index 0000000..4a13dbb --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_metricbundle.cpp @@ -0,0 +1,362 @@ +#include "Geometry/v3d_metricbundle.h" + +#if defined(V3DLIB_ENABLE_SUITESPARSE) + +namespace +{ + + typedef V3D::InlineMatrix Matrix2x4d; + typedef V3D::InlineMatrix Matrix4x2d; + typedef V3D::InlineMatrix Matrix2x6d; + +} // end namespace <> + +namespace V3D +{ + + void + MetricBundleOptimizerBase::updateParametersA(VectorArray const& deltaAi) + { + Vector3d T, omega; + Matrix3x3d R0, dR; + + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + { + T = _cams[i].getTranslation(); + T[0] += deltaAi[i][0]; + T[1] += deltaAi[i][1]; + T[2] += deltaAi[i][2]; + _cams[i].setTranslation(T); + + // Create incremental rotation using Rodriguez formula. + R0 = _cams[i].getRotation(); + omega[0] = deltaAi[i][3]; + omega[1] = deltaAi[i][4]; + omega[2] = deltaAi[i][5]; + createRotationMatrixRodrigues(omega, dR); + _cams[i].setRotation(dR * R0); + } // end for (i) + } // end MetricBundleOptimizerBase::updateParametersA() + + void + MetricBundleOptimizerBase::updateParametersB(VectorArray const& deltaBj) + { + for (int j = _nNonvaryingB; j < _nParametersB; ++j) + { + _Xs[j][0] += deltaBj[j][0]; + _Xs[j][1] += deltaBj[j][1]; + _Xs[j][2] += deltaBj[j][2]; + } + } // end MetricBundleOptimizerBase::updateParametersB() + + void + MetricBundleOptimizerBase::poseDerivatives(int i, int j, Vector3d& XX, + Matrix3x6d& d_dRT, Matrix3x3d& d_dX) const + { + XX = _cams[i].transformPointIntoCameraSpace(_Xs[j]); + + // See Frank Dellaerts bundle adjustment tutorial. + // d(dR * R0 * X + t)/d omega = -[R0 * X]_x + Matrix3x3d J; + makeCrossProductMatrix(XX - _cams[i].getTranslation(), J); + scaleMatrixIP(-1.0, J); + + // Now the transformation from world coords into camera space is xx = Rx + T + // Hence the derivative of x wrt. T is just the identity matrix. + makeIdentityMatrix(d_dRT); + copyMatrixSlice(J, 0, 0, 3, 3, d_dRT, 0, 3); + + // The derivative of Rx+T wrt x is just R. + copyMatrix(_cams[i].getRotation(), d_dX); + } // end MetricBundleOptimizerBase::poseDerivatives() + + +//---------------------------------------------------------------------- + + void + StdMetricBundleOptimizer::fillJacobians(Matrix& Ak, + Matrix& Bk, + Matrix& Ck, + int i, int j, int k) + { + Vector3d XX; + Matrix3x6d d_dRT; + Matrix3x3d d_dX; + this->poseDerivatives(i, j, XX, d_dRT, d_dX); + + double const f = _cams[i].getFocalLength(); + double const ar = _cams[i].getAspectRatio(); + + Matrix2x3d dp_dX; + double const bx = f / (XX[2] * XX[2]); + double const by = ar * bx; + dp_dX[0][0] = bx * XX[2]; dp_dX[0][1] = 0; dp_dX[0][2] = -bx * XX[0]; + dp_dX[1][0] = 0; dp_dX[1][1] = by * XX[2]; dp_dX[1][2] = -by * XX[1]; + + multiply_A_B(dp_dX, d_dRT, Ak); + multiply_A_B(dp_dX, d_dX, Bk); + } // end StdMetricBundleOptimizer::fillJacobians() + + //---------------------------------------------------------------------- + + void + CommonInternalsMetricBundleOptimizer::fillJacobians(Matrix& Ak, + Matrix& Bk, + Matrix& Ck, + int i, int j, int k) + { + double const focalLength = _K[0][0]; + + Vector3d XX; + Matrix3x6d dXX_dRT; + Matrix3x3d dXX_dX; + this->poseDerivatives(i, j, XX, dXX_dRT, dXX_dX); + + if (_mode == FULL_BUNDLE_NO_ROTATIONS) + { + dXX_dRT[0][0] = dXX_dRT[0][1] = dXX_dRT[0][2] = 0.0; + dXX_dRT[1][0] = dXX_dRT[1][1] = dXX_dRT[1][2] = 0.0; + dXX_dRT[2][0] = dXX_dRT[2][1] = dXX_dRT[2][2] = 0.0; + } + + Vector2d xu; // undistorted image point + xu[0] = XX[0] / XX[2]; + xu[1] = XX[1] / XX[2]; + + Vector2d const xd = _distortion(xu); // distorted image point + + Matrix2x2d dp_dxd; + dp_dxd[0][0] = focalLength; dp_dxd[0][1] = 0; + dp_dxd[1][0] = 0; dp_dxd[1][1] = _cachedAspectRatio * focalLength; + + { + // First, lets do the derivative wrt the structure and motion parameters. + Matrix2x3d dxu_dXX; + dxu_dXX[0][0] = 1.0f / XX[2]; dxu_dXX[0][1] = 0; dxu_dXX[0][2] = -XX[0]/(XX[2]*XX[2]); + dxu_dXX[1][0] = 0; dxu_dXX[1][1] = 1.0f / XX[2]; dxu_dXX[1][2] = -XX[1]/(XX[2]*XX[2]); + + Matrix2x2d dxd_dxu = _distortion.derivativeWrtUndistortedPoint(xu); + + Matrix2x2d dp_dxu = dp_dxd * dxd_dxu; + Matrix2x3d dp_dXX = dp_dxu * dxu_dXX; + + multiply_A_B(dp_dXX, dXX_dRT, Ak); + multiply_A_B(dp_dXX, dXX_dX, Bk); + } // end scope + + switch (_mode) + { + case FULL_BUNDLE_RADIAL_TANGENTIAL: + { + Matrix2x2d dxd_dp1p2 = _distortion.derivativeWrtTangentialParameters(xu); + Matrix2x2d d_dp1p2 = dp_dxd * dxd_dp1p2; + copyMatrixSlice(d_dp1p2, 0, 0, 2, 2, Ck, 0, 5); + // No break here! + } + case FULL_BUNDLE_RADIAL: + { + Matrix2x2d dxd_dk1k2 = _distortion.derivativeWrtRadialParameters(xu); + Matrix2x2d d_dk1k2 = dp_dxd * dxd_dk1k2; + copyMatrixSlice(d_dk1k2, 0, 0, 2, 2, Ck, 0, 3); + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH_PP: + { + Ck[0][1] = 1; Ck[0][2] = 0; + Ck[1][1] = 0; Ck[1][2] = 1; + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH: + { + Ck[0][0] = xd[0]; + Ck[1][0] = xd[1]; + } + case FULL_BUNDLE_METRIC: + { + } + } // end switch + } // end CommonInternalsMetricBundleOptimizer::fillJacobians() + + void + CommonInternalsMetricBundleOptimizer::updateParametersC(Vector const& deltaC) + { + switch (_mode) + { + case FULL_BUNDLE_RADIAL_TANGENTIAL: + { + _distortion.p1 += deltaC[5]; + _distortion.p2 += deltaC[6]; + // No break here! + } + case FULL_BUNDLE_RADIAL: + { + _distortion.k1 += deltaC[3]; + _distortion.k2 += deltaC[4]; + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH_PP: + { + _K[0][2] += deltaC[1]; + _K[1][2] += deltaC[2]; + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH: + { + _K[0][0] += deltaC[0]; + _K[1][1] = _cachedAspectRatio * _K[0][0]; + } + case FULL_BUNDLE_METRIC: + { + } + } // end switch + } // end CommonInternalsMetricBundleOptimizer::updateParametersC() + + //---------------------------------------------------------------------- + + void + VaryingInternalsMetricBundleOptimizer::fillJacobians(Matrix& Ak, + Matrix& Bk, + Matrix& Ck, + int i, int j, int k) + { + Vector3d XX; + Matrix3x6d dXX_dRT; + Matrix3x3d dXX_dX; + this->poseDerivatives(i, j, XX, dXX_dRT, dXX_dX); + + if (_mode == FULL_BUNDLE_NO_ROTATIONS) + { + dXX_dRT[0][0] = dXX_dRT[0][1] = dXX_dRT[0][2] = 0.0; + dXX_dRT[1][0] = dXX_dRT[1][1] = dXX_dRT[1][2] = 0.0; + dXX_dRT[2][0] = dXX_dRT[2][1] = dXX_dRT[2][2] = 0.0; + } + + Vector2d xu; // undistorted image point + xu[0] = XX[0] / XX[2]; + xu[1] = XX[1] / XX[2]; + + Vector2d const xd = _distortions[i](xu); // distorted image point + + double const focalLength = _cams[i].getFocalLength(); + double const aspectRatio = _cams[i].getAspectRatio(); + + Matrix2x2d dp_dxd; + dp_dxd[0][0] = focalLength; dp_dxd[0][1] = 0; + dp_dxd[1][0] = 0; dp_dxd[1][1] = aspectRatio * focalLength; + + { + // First, lets do the derivative wrt the structure and motion parameters. + Matrix2x3d dxu_dXX; + dxu_dXX[0][0] = 1.0f / XX[2]; dxu_dXX[0][1] = 0; dxu_dXX[0][2] = -XX[0]/(XX[2]*XX[2]); + dxu_dXX[1][0] = 0; dxu_dXX[1][1] = 1.0f / XX[2]; dxu_dXX[1][2] = -XX[1]/(XX[2]*XX[2]); + + Matrix2x2d dxd_dxu = _distortions[i].derivativeWrtUndistortedPoint(xu); + + Matrix2x2d dp_dxu = dp_dxd * dxd_dxu; + Matrix2x3d dp_dXX = dp_dxu * dxu_dXX; + + Matrix2x6d dp_dRT; + + multiply_A_B(dp_dXX, dXX_dRT, dp_dRT); + copyMatrixSlice(dp_dRT, 0, 0, 2, 6, Ak, 0, 0); + multiply_A_B(dp_dXX, dXX_dX, Bk); + } // end scope + + switch (_mode) + { + case FULL_BUNDLE_RADIAL_TANGENTIAL: + { + Matrix2x2d dxd_dp1p2 = _distortions[i].derivativeWrtTangentialParameters(xu); + Matrix2x2d d_dp1p2 = dp_dxd * dxd_dp1p2; + copyMatrixSlice(d_dp1p2, 0, 0, 2, 2, Ak, 0, 11); + // No break here! + } + case FULL_BUNDLE_RADIAL: + { + Matrix2x2d dxd_dk1k2 = _distortions[i].derivativeWrtRadialParameters(xu); + Matrix2x2d d_dk1k2 = dp_dxd * dxd_dk1k2; + copyMatrixSlice(d_dk1k2, 0, 0, 2, 2, Ak, 0, 9); + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH_PP: + { + Ak[0][7] = 1; Ak[0][8] = 0; + Ak[1][7] = 0; Ak[1][8] = 1; + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH: + { + Ak[0][6] = xd[0]; + Ak[1][6] = xd[1]; + } + case FULL_BUNDLE_METRIC: + { + } + } // end switch + } // end VaryingInternalsMetricBundleOptimizer::fillJacobians() + + void + VaryingInternalsMetricBundleOptimizer::updateParametersA(VectorArray const& deltaAi) + { + Vector3d T, omega; + Matrix3x3d R0, dR, K; + + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + { + Vector const& deltaA = deltaAi[i]; + + //cout << "deltaA[" << i << "] = "; displayVector(deltaA); + + T = _cams[i].getTranslation(); + T[0] += deltaA[0]; + T[1] += deltaA[1]; + T[2] += deltaA[2]; + _cams[i].setTranslation(T); + + // Create incremental rotation using Rodriguez formula. + R0 = _cams[i].getRotation(); + omega[0] = deltaA[3]; + omega[1] = deltaA[4]; + omega[2] = deltaA[5]; + createRotationMatrixRodrigues(omega, dR); + _cams[i].setRotation(dR * R0); + + K = _cams[i].getIntrinsic(); + + switch (_mode) + { + case FULL_BUNDLE_RADIAL_TANGENTIAL: + { + _distortions[i].p1 += deltaA[11]; + _distortions[i].p2 += deltaA[12]; + // No break here! + } + case FULL_BUNDLE_RADIAL: + { + _distortions[i].k1 += deltaA[9]; + _distortions[i].k2 += deltaA[10]; + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH_PP: + { + K[0][2] += deltaA[7]; + K[1][2] += deltaA[8]; + // No break here! + } + case FULL_BUNDLE_FOCAL_LENGTH: + { + double const ar = K[1][1] / K[0][0]; + K[0][0] += deltaA[6]; + K[1][1] = ar * K[0][0]; + } + case FULL_BUNDLE_METRIC: + { + } + } // end switch + _cams[i].setIntrinsic(K); + } // end for (i) + } // end VaryingInternalsMetricBundleOptimizer::updateParametersC() + +} // end namespace V3D + +#endif // defined(V3DLIB_ENABLE_SUITESPARSE) diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_metricbundle.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_metricbundle.h new file mode 100644 index 0000000..fed4a19 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_metricbundle.h @@ -0,0 +1,449 @@ +// -*- C++ -*- +#ifndef V3D_METRICBUNDLE_H +#define V3D_METRICBUNDLE_H + +# if defined(V3DLIB_ENABLE_SUITESPARSE) + +#include "Math/v3d_optimization.h" +#include "Math/v3d_linear.h" +#include "Geometry/v3d_cameramatrix.h" +#include "Geometry/v3d_distortion.h" + +namespace V3D +{ + + // This structure provides some helper functions common to all metric BAs + struct MetricBundleOptimizerBase : public SparseLevenbergOptimizer + { + typedef SparseLevenbergOptimizer Base; + + MetricBundleOptimizerBase(double inlierThreshold, + vector& cams, + vector& Xs, + vector const& measurements, + vector const& corrspondingView, + vector const& corrspondingPoint, + int nAddParamsA, int nParamsC) + : SparseLevenbergOptimizer(2, cams.size(), 6+nAddParamsA, Xs.size(), 3, nParamsC, + corrspondingView, corrspondingPoint), + _cams(cams), _Xs(Xs), _measurements(measurements), + _savedTranslations(cams.size()), _savedRotations(cams.size()), + _savedXs(Xs.size()), + _inlierThreshold(inlierThreshold), _cachedParamLength(0.0) + { + // Since we assume that BA does not alter the inputs too much, + // we compute the overall length of the parameter vector in advance + // and return that value as the result of getParameterLength(). + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + { + _cachedParamLength += sqrNorm_L2(_cams[i].getTranslation()); + _cachedParamLength += 3.0; // Assume eye(3) for R. + } + for (int j = _nNonvaryingB; j < _nParametersB; ++j) + _cachedParamLength += sqrNorm_L2(_Xs[j]); + + _cachedParamLength = sqrt(_cachedParamLength); + } + + // Huber robust cost function. + virtual void fillWeights(VectorArray const& residual, Vector& w) + { + for (unsigned int k = 0; k < w.size(); ++k) + { + Vector const& r = residual[k]; + double const e = norm_L2(r); + w[k] = (e < _inlierThreshold) ? 1.0 : sqrt(_inlierThreshold / e); + } // end for (k) + } + + virtual double getParameterLength() const + { + return _cachedParamLength; + } + + virtual void updateParametersA(VectorArray const& deltaAi); + virtual void updateParametersB(VectorArray const& deltaBj); + virtual void updateParametersC(Vector const& deltaC) { } + + virtual void saveAllParameters() + { + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + { + _savedTranslations[i] = _cams[i].getTranslation(); + _savedRotations[i] = _cams[i].getRotation(); + } + _savedXs = _Xs; + } + + virtual void restoreAllParameters() + { + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + { + _cams[i].setTranslation(_savedTranslations[i]); + _cams[i].setRotation(_savedRotations[i]); + } + _Xs = _savedXs; + } + + protected: + typedef InlineMatrix Matrix3x6d; + + void poseDerivatives(int i, int j, Vector3d& XX, + Matrix3x6d& d_dRT, Matrix3x3d& d_dX) const; + + vector& _cams; + vector& _Xs; + + vector const& _measurements; + + vector _savedTranslations; + vector _savedRotations; + vector _savedXs; + + double const _inlierThreshold; + double _cachedParamLength; + }; // end struct MetricBundleOptimizerBase + + struct StdMetricBundleOptimizer : public MetricBundleOptimizerBase + { + typedef MetricBundleOptimizerBase Base; + + StdMetricBundleOptimizer(double inlierThreshold, + vector& cams, + vector& Xs, + vector const& measurements, + vector const& corrspondingView, + vector const& corrspondingPoint) + : MetricBundleOptimizerBase(inlierThreshold, cams, Xs, measurements, + corrspondingView, corrspondingPoint, 0, 0) + { } + + virtual void evalResidual(VectorArray& e) + { + for (unsigned int k = 0; k < e.count(); ++k) + { + int const i = _correspondingParamA[k]; + int const j = _correspondingParamB[k]; + + Vector2d const q = _cams[i].projectPoint(_Xs[j]); + e[k][0] = q[0] - _measurements[k][0]; + e[k][1] = q[1] - _measurements[k][1]; + } + } + + virtual void fillJacobians(Matrix& Ak, Matrix& Bk, Matrix& Ck, + int i, int j, int k); + }; // end struct StdMetricBundleOptimizer + +//---------------------------------------------------------------------- + + enum + { + FULL_BUNDLE_NO_ROTATIONS = -1, // do not optimize the camera rotations + FULL_BUNDLE_METRIC = 0, + FULL_BUNDLE_FOCAL_LENGTH = 1, // f + FULL_BUNDLE_FOCAL_LENGTH_PP = 2, // f, cx, cy + FULL_BUNDLE_RADIAL = 3, // f, cx, cy, k1, k2 + FULL_BUNDLE_RADIAL_TANGENTIAL = 4 // f, cx, cy, k1, k2, p1, p2 + }; + + struct CommonInternalsMetricBundleOptimizer : public MetricBundleOptimizerBase + { + static int globalParamDimensionFromMode(int mode) + { + switch (mode) + { + case FULL_BUNDLE_NO_ROTATIONS: + case FULL_BUNDLE_METRIC: return 0; + case FULL_BUNDLE_FOCAL_LENGTH: return 1; + case FULL_BUNDLE_FOCAL_LENGTH_PP: return 3; + case FULL_BUNDLE_RADIAL: return 5; + case FULL_BUNDLE_RADIAL_TANGENTIAL: return 7; + } + return 0; + } + + typedef MetricBundleOptimizerBase Base; + + CommonInternalsMetricBundleOptimizer(int mode, + double inlierThreshold, + Matrix3x3d& K, + StdDistortionFunction& distortion, + vector& cams, + vector& Xs, + vector const& measurements, + vector const& corrspondingView, + vector const& corrspondingPoint) + : MetricBundleOptimizerBase(inlierThreshold, cams, Xs, measurements, + corrspondingView, corrspondingPoint, + 0, globalParamDimensionFromMode(mode)), + _mode(mode), _K(K), _distortion(distortion) + { + _cachedAspectRatio = K[1][1] / K[0][0]; + } + + Vector2d projectPoint(Vector3d const& X, int i) const + { + Vector3d const XX = _cams[i].transformPointIntoCameraSpace(X); + Vector2d p; + p[0] = XX[0] / XX[2]; + p[1] = XX[1] / XX[2]; + p = _distortion(p); + Vector2d res; + res[0] = _K[0][0] * p[0] + _K[0][1] * p[1] + _K[0][2]; + res[1] = _K[1][1] * p[1] + _K[1][2]; + return res; + } + + virtual void evalResidual(VectorArray& e) + { + for (unsigned int k = 0; k < e.count(); ++k) + { + int const i = _correspondingParamA[k]; + int const j = _correspondingParamB[k]; + + Vector2d const q = this->projectPoint(_Xs[j], i); + e[k][0] = q[0] - _measurements[k][0]; + e[k][1] = q[1] - _measurements[k][1]; + } + } + + virtual void fillJacobians(Matrix& Ak, Matrix& Bk, Matrix& Ck, + int i, int j, int k); + + virtual void updateParametersC(Vector const& deltaC); + + virtual void saveAllParameters() + { + Base::saveAllParameters(); + _savedK = _K; + _savedDistortion = _distortion; + } + + virtual void restoreAllParameters() + { + Base::restoreAllParameters(); + _K = _savedK; + _distortion = _savedDistortion; + } + + protected: + int _mode; + Matrix3x3d& _K; + StdDistortionFunction& _distortion; + + Matrix3x3d _savedK; + StdDistortionFunction _savedDistortion; + double _cachedAspectRatio; + }; // end struct CommonInternalsMetricBundleOptimizer + +//---------------------------------------------------------------------- + + struct VaryingInternalsMetricBundleOptimizer : public MetricBundleOptimizerBase + { + static int extParamDimensionFromMode(int mode) + { + switch (mode) + { + case FULL_BUNDLE_NO_ROTATIONS: + case FULL_BUNDLE_METRIC: return 0; + case FULL_BUNDLE_FOCAL_LENGTH: return 1; + case FULL_BUNDLE_FOCAL_LENGTH_PP: return 3; + case FULL_BUNDLE_RADIAL: return 5; + case FULL_BUNDLE_RADIAL_TANGENTIAL: return 7; + } + return 0; + } + + typedef MetricBundleOptimizerBase Base; + + VaryingInternalsMetricBundleOptimizer(int mode, + double inlierThreshold, + std::vector& distortions, + vector& cams, + vector& Xs, + vector const& measurements, + vector const& corrspondingView, + vector const& corrspondingPoint) + : MetricBundleOptimizerBase(inlierThreshold, cams, Xs, measurements, + corrspondingView, corrspondingPoint, + extParamDimensionFromMode(mode), 0), + _mode(mode), _distortions(distortions), + _savedKs(cams.size()), _savedDistortions(cams.size()) + { } + + Vector2d projectPoint(Vector3d const& X, int i) const + { + return _cams[i].projectPoint(_distortions[i], X); + } + + virtual void evalResidual(VectorArray& e) + { + for (unsigned int k = 0; k < e.count(); ++k) + { + int const i = _correspondingParamA[k]; + int const j = _correspondingParamB[k]; + + Vector2d const q = this->projectPoint(_Xs[j], i); + e[k][0] = q[0] - _measurements[k][0]; + e[k][1] = q[1] - _measurements[k][1]; + } + } + + virtual void fillJacobians(Matrix& Ak, Matrix& Bk, Matrix& Ck, + int i, int j, int k); + + virtual void updateParametersA(VectorArray const& deltaAi); + + virtual void saveAllParameters() + { + Base::saveAllParameters(); + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + _savedKs[i] = _cams[i].getIntrinsic(); + std::copy(_distortions.begin(), _distortions.end(), _savedDistortions.begin()); + } + + virtual void restoreAllParameters() + { + Base::restoreAllParameters(); + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + _cams[i].setIntrinsic(_savedKs[i]); + std::copy(_savedDistortions.begin(), _savedDistortions.end(), _distortions.begin()); + } + + protected: + int _mode; + std::vector& _distortions; + + std::vector _savedKs; + std::vector _savedDistortions; + }; // end struct VaryingInternalsMetricBundleOptimizer + +//---------------------------------------------------------------------- + + //! Normalizes the 3D points and camera centers to have zero mean and unit covariance. + struct ScopedBundleExtrinsicNormalizer + { + ScopedBundleExtrinsicNormalizer(vector& cameras, + vector& Xs) + : _cameras(cameras), _Xs(Xs) + { + int const N = cameras.size(); + int const M = Xs.size(); + + vector camCenters(N); + for (int i = 0; i < N; ++i) camCenters[i] = cameras[i].cameraCenter(); + + makeZeroVector(_mean); + for (int i = 0; i < N; ++i) addVectorsIP(camCenters[i], _mean); + for (int j = 0; j < M; ++j) addVectorsIP(Xs[j], _mean); + scaleVectorIP(1.0 / double(N+M), _mean); + + for (int i = 0; i < N; ++i) subtractVectors(camCenters[i], _mean, camCenters[i]); + for (int j = 0; j < M; ++j) subtractVectors(Xs[j], _mean, Xs[j]); + + double sqrNorm = 0.0; + for (int i = 0; i < N; ++i) sqrNorm += sqrNorm_L2(camCenters[i]); + for (int j = 0; j < M; ++j) sqrNorm += sqrNorm_L2(Xs[j]); + + _scale = sqrt(sqrNorm); + double const rcpScale = 1.0 / _scale; + + for (int i = 0; i < N; ++i) scaleVectorIP(rcpScale, camCenters[i]); + for (int j = 0; j < M; ++j) scaleVectorIP(rcpScale, Xs[j]); + + for (int i = 0; i < N; ++i) cameras[i].setCameraCenter(camCenters[i]); + } + + ~ScopedBundleExtrinsicNormalizer() + { + // Undo all the transformations saved in _mean and _scale. + + int const N = _cameras.size(); + int const M = _Xs.size(); + + vector camCenters(N); + for (int i = 0; i < N; ++i) camCenters[i] = _cameras[i].cameraCenter(); + + for (int i = 0; i < N; ++i) scaleVectorIP(_scale, camCenters[i]); + for (int j = 0; j < M; ++j) scaleVectorIP(_scale, _Xs[j]); + + for (int i = 0; i < N; ++i) addVectorsIP(_mean, camCenters[i]); + for (int j = 0; j < M; ++j) addVectorsIP(_mean, _Xs[j]); + + for (int i = 0; i < N; ++i) _cameras[i].setCameraCenter(camCenters[i]); + } + + protected: + vector& _cameras; + vector& _Xs; + + Vector3d _mean; + double _scale; + }; // end struct ScopedBundleExtrinsicNormalizer + + // Normalized the camera intrinsics and measurements to have unit focal length. + struct ScopedBundleIntrinsicNormalizer + { + ScopedBundleIntrinsicNormalizer(vector& cameras, + vector& measurements, + vector const& correspondingView) + : _cameras(cameras), _measurements(measurements), _correspondingView(correspondingView), + _savedFs(cameras.size()) + { + int const N = cameras.size(); + int const K = measurements.size(); + + for (int i = 0; i < N; ++i) + { + Matrix3x3d KK = cameras[i].getIntrinsic(); + double const f = KK[0][0]; + _savedFs[i] = f; + scaleMatrixIP(1.0/f, KK); + KK[2][2] = 1.0; + cameras[i].setIntrinsic(KK); + } + + for (int k = 0; k < K; ++k) + { + int const view = correspondingView[k]; + double const f = _savedFs[view]; + scaleVectorIP(1.0/f, measurements[k]); + } + } + + ~ScopedBundleIntrinsicNormalizer() + { + // Undo normalization + int const N = _cameras.size(); + int const K = _measurements.size(); + + for (int i = 0; i < N; ++i) + { + Matrix3x3d KK = _cameras[i].getIntrinsic(); + scaleMatrixIP(_savedFs[i], KK); + KK[2][2] = 1.0; + _cameras[i].setIntrinsic(KK); + } + + for (int k = 0; k < K; ++k) + { + int const view = _correspondingView[k]; + double const f = _savedFs[view]; + scaleVectorIP(f, _measurements[k]); + } + } + + protected: + vector& _cameras; + vector& _measurements; + vector const& _correspondingView; + + vector _savedFs; + }; // end struct ScopedBundleIntrinsicNormalizer + +} // end namespace V3D + +# endif + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_mviewutilities.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_mviewutilities.h new file mode 100644 index 0000000..1962ea1 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_mviewutilities.h @@ -0,0 +1,438 @@ +// -*- C++ -*- + +#ifndef V3D_GEOM_UTILITIES_H +#define V3D_GEOM_UTILITIES_H + +#include "Base/v3d_serialization.h" +#include "Math/v3d_linear.h" +#include "Math/v3d_linear_tnt.h" +#include "Geometry/v3d_cameramatrix.h" + +namespace V3D +{ + + struct PointMeasurement + { + V3D::Vector2f pos; + int view, id; + + PointMeasurement() + : id(-1) + { } + + PointMeasurement(V3D::Vector2f const& pos_, int view_, int id_ = -1) + : pos(pos_), view(view_), id(id_) + { } + + bool operator==(PointMeasurement const& rhs) const + { + // Note: if view number and feature id are the same, we assume that the + // 2D position is the same. + return (this->id == rhs.id) && (this->view == rhs.view); + } + + template void serialize(Archive& ar) + { + V3D::SerializationScope scope(ar, "PointMeasurement"); + ar & pos[0] & pos[1] & view & id; + } + + V3D_DEFINE_LOAD_SAVE(PointMeasurement) + }; // end struct PointMeasurement + + V3D_DEFINE_IOSTREAM_OPS(PointMeasurement) + + struct PointCorrespondence + { + PointMeasurement left, right; + + PointCorrespondence() { } + + PointCorrespondence(PointMeasurement const& l, PointMeasurement const& r) + : left(l), right(r) + { } + + bool operator==(PointCorrespondence const& rhs) const + { + return (this->left == rhs.left) && (this->right == rhs.right); + } + + template void serialize(Archive& ar) + { + V3D::SerializationScope scope(ar, "PointCorrespondence"); + ar & left; + ar & right; + } + + V3D_DEFINE_LOAD_SAVE(PointCorrespondence) + }; // end struct PointCorrespondence + + V3D_DEFINE_IOSTREAM_OPS(PointCorrespondence) + + struct TriangulatedPoint + { + V3D::Vector3d pos; + std::vector measurements; + + TriangulatedPoint() : measurements() + { + V3D::makeZeroVector(pos); + } + + TriangulatedPoint(V3D::Vector3d const& p, std::vector const& ms) + : pos(p), measurements(ms) + { } + + template void serialize(Archive& ar) + { + V3D::SerializationScope scope(ar, "TriangulatedPoint"); + ar & pos; + serializeVector(measurements, ar); + } + + V3D_DEFINE_LOAD_SAVE(TriangulatedPoint) + + static void connectTracks(std::vector const& corrs, + std::vector& points, + int nRequiredMeasurements = 2); + + }; // end struct TriangulatedPoint + + V3D_DEFINE_IOSTREAM_OPS(TriangulatedPoint) + + // Logical filtering of 3D points: check if every view appears at most once in the measurements + inline void + filterConsistentSparsePoints(std::vector& model) + { + using namespace std; + + vector const origModel(model); + + model.clear(); + + for (size_t j = 0; j < origModel.size(); ++j) + { + TriangulatedPoint const& X = origModel[j]; + set views; + for (int k = 0; k < X.measurements.size(); ++k) + { + int const view = X.measurements[k].view; + if (views.find(view) != views.end()) + goto loop_end; + + views.insert(view); + } // end for (k) + model.push_back(X); + loop_end:; + } // end for(j) + } // end filterConsistentSparsePoints() + +//====================================================================== + + struct BundlePointStructure + { + BundlePointStructure() + { } + + BundlePointStructure(std::vector const& reconstruction, + int nRequiredMeasurements = 2) + { + int const nPoints = reconstruction.size(); + + for (int i = 0; i < nPoints; ++i) + { + TriangulatedPoint const& X = reconstruction[i]; + if (X.measurements.size() >= nRequiredMeasurements) + { + for (size_t k = 0; k < X.measurements.size(); ++k) + { + Vector2f const& p = X.measurements[k].pos; + measurements.push_back(makeVector2(p[0], p[1])); + measurementIds.push_back(X.measurements[k].id); + correspondingView.push_back(X.measurements[k].view); + correspondingPoint.push_back(points3d.size()); + } + points3d.push_back(X.pos); + xsIndices.push_back(i); + } + } // end for (p) + } // end VRBundlePointStructure() + + void createPointStructure(std::vector& reconstruction, + bool createFromScratch = true) + { + size_t const nPoints = points3d.size(); + + if (createFromScratch) + { + reconstruction.resize(nPoints); + for (size_t i = 0; i < nPoints; ++i) + { + TriangulatedPoint& X = reconstruction[i]; + X.measurements.clear(); + X.pos = points3d[i]; + } + + for (size_t k = 0; k < measurements.size(); ++k) + { + int i = correspondingPoint[k]; + PointMeasurement m; + m.id = measurementIds[k]; + m.view = correspondingView[k]; + copyVector(measurements[k], m.pos); + reconstruction[i].measurements.push_back(m); + } + } + else + { + size_t m = 0; + for (size_t i = 0; i < nPoints; ++i) + { + TriangulatedPoint& X = reconstruction[xsIndices[i]]; + X.pos = points3d[i]; + + for (size_t k = 0; k < X.measurements.size(); ++k, ++m) + copyVector(measurements[m], X.measurements[k].pos); + } + } // end if (createFromScratch) + } // end createPointStructure() + + std::vector points3d; + std::vector measurements; + std::vector measurementIds; + std::vector correspondingView; + std::vector correspondingPoint; + std::vector xsIndices; + }; // end struct BundleReconstruction + + template + inline Vector3d + triangulateLinear(Mat const& P0, Mat const& P1, PointCorrespondence const& corr) + { + return triangulateLinear(P0, P1, corr.left.pos,corr.right.pos); + } // end triangulateLinear() + + template + inline Vector3d + triangulateLinear(Mat const& P0, Mat const& P1, Vec2 const& m0,Vec2 const& m1) + { + assert(P0.num_rows() == 3); + assert(P0.num_cols() == 4); + assert(P1.num_rows() == 3); + assert(P1.num_cols() == 4); + + Matrix A(4, 3); + Vector b(4); + + { + Matrix3x4d const& P = P0; + double x = m0[0]; + double y = m0[1]; + + A[0][0] = x*P[2][0] - P[0][0]; + A[0][1] = x*P[2][1] - P[0][1]; + A[0][2] = x*P[2][2] - P[0][2]; + + A[1][0] = y*P[2][0] - P[1][0]; + A[1][1] = y*P[2][1] - P[1][1]; + A[1][2] = y*P[2][2] - P[1][2]; + + b[0] = P[0][3] - x*P[2][3]; + b[1] = P[1][3] - y*P[2][3]; + } + + { + Matrix3x4d const& P = P1; + double x = m1[0]; + double y = m1[1]; + + A[2][0] = x*P[2][0] - P[0][0]; + A[2][1] = x*P[2][1] - P[0][1]; + A[2][2] = x*P[2][2] - P[0][2]; + + A[3][0] = y*P[2][0] - P[1][0]; + A[3][1] = y*P[2][1] - P[1][1]; + A[3][2] = y*P[2][2] - P[1][2]; + + b[2] = P[0][3] - x*P[2][3]; + b[3] = P[1][3] - y*P[2][3]; + } + + QR qr(A); + Vector const X = qr.solve(b); + + return makeVector3(X[0], X[1], X[2]); + } // end triangulateLinear() + + template + inline Vector3d + triangulateLinear(std::vector const& Ps, std::vector > const& ms) + { + assert(Ps.size()==ms.size()); + int const K = ms.size(); + int const nRows = 2 * K; + int const nCols = 3; + + Matrix A(nRows, nCols); + Vector b(nRows); + + for (int i = 0; i < K; ++i) + { + double x = ms[i][0]; + double y = ms[i][1]; + + Matrix3x4d const& P = Ps[i]; + + A[2*i+0][0] = x*P[2][0] - P[0][0]; + A[2*i+0][1] = x*P[2][1] - P[0][1]; + A[2*i+0][2] = x*P[2][2] - P[0][2]; + + A[2*i+1][0] = y*P[2][0] - P[1][0]; + A[2*i+1][1] = y*P[2][1] - P[1][1]; + A[2*i+1][2] = y*P[2][2] - P[1][2]; + + b[2*i+0] = P[0][3] - x*P[2][3]; + b[2*i+1] = P[1][3] - y*P[2][3]; + } // end for (i) + + QR qr(A); + Vector const X = qr.solve(b); + + return makeVector3(X[0], X[1], X[2]); + } // end triangulateLinear() + + inline Vector3d + triangulateLinear(std::vector const& Ps, std::vector const& ms) + { + int const K = ms.size(); + int const nRows = 2 * K; + int const nCols = 3; + + Matrix A(nRows, nCols); + Vector b(nRows); + + for (int i = 0; i < K; ++i) + { + double x = ms[i].pos[0]; + double y = ms[i].pos[1]; + + Matrix3x4d const& P = Ps[ms[i].view]; + + A[2*i+0][0] = x*P[2][0] - P[0][0]; + A[2*i+0][1] = x*P[2][1] - P[0][1]; + A[2*i+0][2] = x*P[2][2] - P[0][2]; + + A[2*i+1][0] = y*P[2][0] - P[1][0]; + A[2*i+1][1] = y*P[2][1] - P[1][1]; + A[2*i+1][2] = y*P[2][2] - P[1][2]; + + b[2*i+0] = P[0][3] - x*P[2][3]; + b[2*i+1] = P[1][3] - y*P[2][3]; + } // end for (i) + + QR qr(A); + Vector const X = qr.solve(b); + + return makeVector3(X[0], X[1], X[2]); + } // end triangulateLinear() +/** + * Triangulate a point from 2D normalized measurments + * @warning take the camera[PointMeasurement.view] to unproject each point !! + * + */ + inline Vector3d + triangulateLinear(std::vector const& cams, std::vector const& ms) + { + int const N = cams.size(); + std::vector Ps(N); + for (int i = 0; i < N; ++i) + Ps[i] = cams[i].getProjection(); + return triangulateLinear(Ps, ms); + } // end triangulateLinear() + + inline Vector3d + triangulateLinear(std::vector const& Ps, std::vector const& isValidMatrix, + std::vector const& ms) + { + assert(isValidMatrix.size() == Ps.size()); + + int const K = ms.size(); + int const nRows = 2 * K; + int const nCols = 3; + + Matrix A(nRows, nCols); + Vector b(nRows); + + for (int i = 0; i < K; ++i) + { + if (!isValidMatrix[i]) continue; + + double x = ms[i].pos[0]; + double y = ms[i].pos[1]; + + Matrix3x4d const& P = Ps[ms[i].view]; + + A[2*i+0][0] = x*P[2][0] - P[0][0]; + A[2*i+0][1] = x*P[2][1] - P[0][1]; + A[2*i+0][2] = x*P[2][2] - P[0][2]; + + A[2*i+1][0] = y*P[2][0] - P[1][0]; + A[2*i+1][1] = y*P[2][1] - P[1][1]; + A[2*i+1][2] = y*P[2][2] - P[1][2]; + + b[2*i+0] = P[0][3] - x*P[2][3]; + b[2*i+1] = P[1][3] - y*P[2][3]; + } // end for (i) + + QR qr(A); + Vector const X = qr.solve(b); + + return makeVector3(X[0], X[1], X[2]); + } // end triangulateLinear() + + enum + { + V3D_CONSISTENT_ROTATION_METHOD_SVD = 0, // direct dense SVD approach + V3D_CONSISTENT_ROTATION_METHOD_SVD_ATA = 1, // SVD on A^T*A, with sparse matrix multiplication + V3D_CONSISTENT_ROTATION_METHOD_EIG_ATA = 2, // Eigendecomposition on A^T*A, with sparse matrix multiplication + V3D_CONSISTENT_ROTATION_METHOD_SPARSE_EIG = 3, // Eigendecomposition using ARPACK + }; + + void computeConsistentRotations(int const nViews, + std::vector const& relativeRotations, + std::vector > const& viewPairs, + std::vector& rotations, + int method = V3D_CONSISTENT_ROTATION_METHOD_EIG_ATA); + + void computeConsistentRotations_Linf(double const sigma, int const nIterations, int const nViews, + std::vector const& relativeRotations, + std::vector > const& viewPairs, + std::vector& rotations, std::vector& zs); + + void computeConsistentRotations_L1(double const sigma, int const nIterations, int const nViews, + std::vector const& relativeRotations, + std::vector > const& viewPairs, + std::vector& rotations); + + void computeConsistentRotations_LSQ(int const nIterations, int const nViews, + std::vector const& relativeRotations, + std::vector > const& viewPairs, + std::vector& rotations); + + + void refineConsistentRotations(int const nViews, + std::vector const& relativeRotations, + std::vector > const& viewPairs, + std::vector& rotations); + + Matrix3x3d computeIntersectionCovariance(vector const& projections, + vector const& measurements, + double sigma); + + double computeIntersectionRoundness(vector const& projections, + vector const& measurements, + double sigma); + +} // end namespace V3D + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_stereobundle.cpp b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_stereobundle.cpp new file mode 100644 index 0000000..8bded74 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_stereobundle.cpp @@ -0,0 +1,71 @@ +#include "Geometry/v3d_stereobundle.h" + +#if defined(V3DLIB_ENABLE_SUITESPARSE) + +using namespace V3D; + +namespace +{ + typedef InlineMatrix Matrix3x6d; + + void + poseDerivatives(Matrix3x3d const& R, Vector3d const& T, CameraMatrix const& rigCam, Vector3d const& X, + Vector3d& XX, Matrix3x6d& d_dRT, Matrix3x3d& d_dX) + { + Vector3d const RX = R * X; + Vector3d const Y = RX + T; + Matrix3x6d dY_dRT; + + // See Frank Dellaerts bundle adjustment tutorial. + // d(dR * R0 * X + t)/d omega = -[R0 * X]_x + Matrix3x3d J; + makeCrossProductMatrix(RX, J); + scaleMatrixIP(-1.0, J); + + // Now the transformation from world coords into camera space is xx = Rx + T + // Hence the derivative of x wrt. T is just the identity matrix. + makeIdentityMatrix(dY_dRT); + copyMatrixSlice(J, 0, 0, 3, 3, dY_dRT, 0, 3); + + XX = rigCam.transformPointIntoCameraSpace(Y); + + // dXX/dY is just rigCam.getRotation(); dXX/dRT = dXX/dY * dY/dRT + multiply_A_B(rigCam.getRotation(), dY_dRT, d_dRT); + + // The derivative of Rx+T wrt x is just R -- here R is the combined rotation. + multiply_A_B(rigCam.getRotation(), R, d_dX); + } // end poseDerivatives() + +} // end namespace <> + +namespace V3D +{ + + void + StereoMetricBundleOptimizer::fillJacobians(Matrix& Ak, Matrix& Bk, Matrix& Ck, + int i, int j, int k) + { + int const l = _correspondingSubCamera[k]; + + Vector3d XX; + Matrix3x6d d_dRT; + Matrix3x3d d_dX; + + ::poseDerivatives(_rotations[i], _translations[i], _rigCameras[l], _Xs[j], XX, d_dRT, d_dX); + + double const f = _rigCameras[l].getFocalLength(); + double const ar = _rigCameras[l].getAspectRatio(); + + Matrix2x3d dp_dX; + double const bx = f / (XX[2] * XX[2]); + double const by = ar * bx; + dp_dX[0][0] = bx * XX[2]; dp_dX[0][1] = 0; dp_dX[0][2] = -bx * XX[0]; + dp_dX[1][0] = 0; dp_dX[1][1] = by * XX[2]; dp_dX[1][2] = -by * XX[1]; + + multiply_A_B(dp_dX, d_dRT, Ak); + multiply_A_B(dp_dX, d_dX, Bk); + } // end StereoMetricBundleOptimizer::fillJacobians() + +} // end namespace V3D + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_stereobundle.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_stereobundle.h new file mode 100644 index 0000000..e22caf3 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Geometry/v3d_stereobundle.h @@ -0,0 +1,169 @@ +// -*- C++ -*- +#ifndef V3D_STEREOBUNDLE_H +#define V3D_STEREOBUNDLE_H + +# if defined(V3DLIB_ENABLE_SUITESPARSE) + +#include "Math/v3d_optimization.h" +#include "Math/v3d_linear.h" +#include "Geometry/v3d_cameramatrix.h" + +namespace V3D +{ + + /*! Although it is called StereoMetricBundleOptimizer, it really optimized general + * rigidly moving multi-camera systems (binocular, trinocular etc.). + * View refer here to poses of the rigid system; subcamera indices denote the particular + * camera on the platform (e.g. 0 and 1 for binocular rigs). + * The poses of the moving platforms and the 3d points are optimized. + * If X_j is visible in view i with pose RT_i in subcamera l (with projection P_l), + * then the projected measurements is p = P_l RT_i X_j. + */ + struct StereoMetricBundleOptimizer : public SparseLevenbergOptimizer + { + typedef SparseLevenbergOptimizer Base; + + StereoMetricBundleOptimizer(double inlierThreshold, + vector& rotations, // The poses of the rigs (rotation part), + vector& translations, // translation part + vector const& rigCameras, // poses of the cameras on the rig + vector& Xs, + vector const& measurements, + vector const& corrspondingView, + vector const& corrspondingPoint, + vector const& correspondingSubCamera) + : SparseLevenbergOptimizer(2, rotations.size(), 6, Xs.size(), 3, 0, + corrspondingView, corrspondingPoint), + _rotations(rotations), _translations(translations), + _Xs(Xs), _measurements(measurements), + _rigCameras(rigCameras), _correspondingSubCamera(correspondingSubCamera), + _savedTranslations(rotations.size()), _savedRotations(rotations.size()), + _savedXs(Xs.size()), + _inlierThreshold(inlierThreshold), _cachedParamLength(0.0) + { + // Since we assume that BA does not alter the inputs too much, + // we compute the overall length of the parameter vector in advance + // and return that value as the result of getParameterLength(). + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + { + _cachedParamLength += sqrNorm_L2(_translations[i]); + _cachedParamLength += 3.0; // Assume eye(3) for R. + } + for (int j = _nNonvaryingB; j < _nParametersB; ++j) + _cachedParamLength += sqrNorm_L2(_Xs[j]); + + _cachedParamLength = sqrt(_cachedParamLength); + } + + // Huber robust cost function. + virtual void fillWeights(VectorArray const& residual, Vector& w) + { + for (unsigned int k = 0; k < w.size(); ++k) + { + Vector const& r = residual[k]; + double const e = norm_L2(r); + w[k] = (e < _inlierThreshold) ? 1.0 : sqrt(_inlierThreshold / e); + } // end for (k) + } + + virtual void evalResidual(VectorArray& e) + { + Vector3d X; + + for (unsigned int k = 0; k < e.count(); ++k) + { + int const i = _correspondingParamA[k]; + int const j = _correspondingParamB[k]; + int const l = _correspondingSubCamera[k]; + + multiply_A_v(_rotations[i], _Xs[j], X); // Transform into rig space + X = X + _translations[i]; + Vector2d const q = _rigCameras[l].projectPoint(X); // project into respective camera of the rig + e[k][0] = q[0] - _measurements[k][0]; + e[k][1] = q[1] - _measurements[k][1]; + } + } + + virtual void fillJacobians(Matrix& Ak, Matrix& Bk, Matrix& Ck, + int i, int j, int k); + + virtual double getParameterLength() const + { + return _cachedParamLength; + } + + void updateParametersA(VectorArray const& deltaAi) + { + Vector3d T, omega; + Matrix3x3d R0, dR; + + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + { + _translations[i][0] += deltaAi[i][0]; + _translations[i][1] += deltaAi[i][1]; + _translations[i][2] += deltaAi[i][2]; + + // Create incremental rotation using Rodriguez formula. + R0 = _rotations[i]; + omega[0] = deltaAi[i][3]; + omega[1] = deltaAi[i][4]; + omega[2] = deltaAi[i][5]; + createRotationMatrixRodrigues(omega, dR); + _rotations[i] = dR * R0; + } // end for (i) + } // end updateParametersA() + + void updateParametersB(VectorArray const& deltaBj) + { + for (int j = _nNonvaryingB; j < _nParametersB; ++j) + { + _Xs[j][0] += deltaBj[j][0]; + _Xs[j][1] += deltaBj[j][1]; + _Xs[j][2] += deltaBj[j][2]; + } + } // end updateParametersB() + + virtual void updateParametersC(Vector const& deltaC) { } + + virtual void saveAllParameters() + { + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + { + _savedTranslations[i] = _translations[i]; + _savedRotations[i] = _rotations[i]; + } + _savedXs = _Xs; + } + + virtual void restoreAllParameters() + { + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + { + _translations[i] = _savedTranslations[i]; + _rotations[i] = _savedRotations[i]; + } + _Xs = _savedXs; + } + + protected: + vector& _rotations; + vector& _translations; + vector& _Xs; + + vector const& _measurements; + vector const& _rigCameras; + vector const& _correspondingSubCamera; + + vector _savedTranslations; + vector _savedRotations; + vector _savedXs; + + double const _inlierThreshold; + double _cachedParamLength; + }; // end struct StereoMetricBundleOptimizer + +} // end namespace V3D + +# endif + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_blockmatrix.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_blockmatrix.h new file mode 100644 index 0000000..5418fb6 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_blockmatrix.h @@ -0,0 +1,962 @@ +// -*- C++ -*- +#ifndef V3D_BLOCK_MATRIX_H +#define V3D_BLOCK_MATRIX_H + +#include "Math/v3d_linear.h" +#include "Math/v3d_linear_ldlt.h" + +namespace V3D +{ + + template struct DiagonalBlockMatrix; + + template + inline void + makeZeroMatrix(DiagonalBlockMatrix& dst); + + template + inline void + addMatrices(DiagonalBlockMatrix const& A, DiagonalBlockMatrix const& B, DiagonalBlockMatrix& dst); + + template + inline void + scaleMatrix(DiagonalBlockMatrix const& A, Elem scale, DiagonalBlockMatrix& dst); + + template + inline void + multiply_A_B(DiagonalBlockMatrix const& A, DiagonalBlockMatrix const& B, DiagonalBlockMatrix& dst); + + template + inline void + multiply_A_Bt(DiagonalBlockMatrix const& A, DiagonalBlockMatrix const& B, DiagonalBlockMatrix& dst); + + template + inline void + multiply_At_A(DiagonalBlockMatrix const& A, DiagonalBlockMatrix& dst); + + // This is a matrix consisting of diagonal matrices of the same size as blocks: + // A = [ D_11 D_12 ... D_1N ] + // [ D_21 D_22 ... D_2N ] + // [ ... ] + // [ D_M1 D_M2 ... D_MN ] + // This is not the same as a block diagonal matrix! + template + struct DiagonalBlockMatrix + { + DiagonalBlockMatrix(int const M, int const N, int const P) + : _M(M), _N(N), _P(P) + { + _values = new Elem[_M*_N*_P]; + //std::fill(_values, _values + _M*_N*_P, Elem(0)); + } + + ~DiagonalBlockMatrix() + { + delete [] _values; + } + + int const getM() const { return _M; } + int const getN() const { return _N; } + int const getP() const { return _P; } + + int const num_rows() const { return _M*_P; } + int const num_cols() const { return _N*_P; } + + template + void getBlock(int const m, int const n, Vec& dst) const + { + assert(dst.size() == _P); + + int const ofs = this->getBlockOfs(m, n); + for (int i = 0; i < _P; ++i) dst[i] = _values[ofs + i]; + } + + Elem& elemAt(int const m, int const n, int const p) + { + return _values[this->getBlockOfs(m, n) + p]; + } + + Elem const& elemAt(int const m, int const n, int const p) const + { + return _values[this->getBlockOfs(m, n) + p]; + } + + Elem operator()(int const row1, int const col1) const + { + int const row = row1 - 1; + int const col = col1 - 1; + int const m = row / _P; + int const n = col / _P; + int const pr = row - m*_P; + int const pc = col - n*_P; + + if (pr != pc) return 0; + return this->elemAt(m, n, pr); + } + + Elem * getBlock(int const m, int const n) + { + return _values + this->getBlockOfs(m, n); + } + + Elem const * getBlock(int const m, int const n) const + { + return _values + this->getBlockOfs(m, n); + } + + void augmentDiagonal(Elem const mu) + { + for (int k = 0; k < std::min(_M, _N); ++k) + { + int const ofs = this->getBlockOfs(k, k); + for (int i = 0; i < _P; ++i) _values[ofs + i] += mu; + } + } // end augmentDiagonal() + + template + void scaleRows(Vec const& w) + { + assert(w.size() == this->num_rows()); + + for (int m = 0; m < _M; ++m) + for (int p = 0; p < _P; ++p) + { + Elem const s = w[m*_P + p]; + for (int n = 0; n < _N; ++n) + this->getBlock(m, n)[p] *= s; + } // end for (p) + } // end scaleRows() + + template + void scaleColumns(Vec const& w) + { + assert(w.size() == this->num_cols()); + + for (int n = 0; n < _N; ++n) + for (int p = 0; p < _P; ++p) + { + Elem const s = w[n*_P + p]; + for (int m = 0; m < _M; ++m) + this->getBlock(m, n)[p] *= s; + } // end for (p) + } // end scaleRows() + + int getColumnNonzeroCount(unsigned int col) const { return _M; } + + template + void getSparseColumn(unsigned int col, VecA& rows, VecB& values) const + { + int const n = col / _P; + int const p = col - n*_P; + + for (int m = 0; m < _M; ++m) + { + rows[m] = m*_P + p; + values[m] = this->elemAt(m, n, p); + } + } + + int getRowNonzeroCount(unsigned int row) const { return _N; } + + template + void getSparseRow(unsigned int row, VecA& cols, VecB& values) const + { + int const m = row / _P; + int const p = row - m*_P; + + for (int n = 0; n < _N; ++n) + { + cols[n] = n*_P + p; + values[n] = this->elemAt(m, n, p); + } + } + + protected: + int getBlockOfs(int const m, int const n) const + { + return (m*_N + n)*_P; + } + + int const _M, _N, _P; + Elem * _values; + + friend void makeZeroMatrix<>(DiagonalBlockMatrix& dst); + friend void addMatrices<>(DiagonalBlockMatrix const& A, DiagonalBlockMatrix const& B, DiagonalBlockMatrix& dst); + friend void scaleMatrix<>(DiagonalBlockMatrix const& A, Elem scale, DiagonalBlockMatrix& dst); + friend void multiply_A_B<>(DiagonalBlockMatrix const& A, DiagonalBlockMatrix const& B, DiagonalBlockMatrix& dst); + friend void multiply_A_Bt<>(DiagonalBlockMatrix const& A, DiagonalBlockMatrix const& B, DiagonalBlockMatrix& dst); + friend void multiply_At_A<>(DiagonalBlockMatrix const& A, DiagonalBlockMatrix& dst); + }; // end struct DiagonalBlockMatrix + + template + inline void + makeZeroMatrix(DiagonalBlockMatrix& dst) + { + std::fill(dst._values, dst._values + dst._M*dst._N*dst._P, Elem(0)); + } + + template + inline void + addMatrices(DiagonalBlockMatrix const& A, DiagonalBlockMatrix const& B, DiagonalBlockMatrix& dst) + { + assert(A._P == B._P); + assert(A._P == dst._P); + assert(A._M == B._M); + assert(A._N == B._N); + assert(A._M == dst._M); + assert(A._N == dst._N); + + int const sz = A._P * A._M * A._N; + for (int i = 0; i < sz; ++i) dst._values[i] = A._values[i] + B._values[i]; + } // end addMatrices() + + template + inline void + scaleMatrix(DiagonalBlockMatrix const& A, Elem scale, DiagonalBlockMatrix& dst) + { + assert(A._P == dst._P); + assert(A._M == dst._M); + assert(A._N == dst._N); + + int const sz = A._P * A._M * A._N; + for (int i = 0; i < sz; ++i) dst._values[i] = scale * A._values[i]; + } // end scaleMatrix() + + template + inline void + multiply_A_B(DiagonalBlockMatrix const& A, DiagonalBlockMatrix const& B, DiagonalBlockMatrix& dst) + { + assert(A._P == B._P); + assert(A._P == dst._P); + assert(A._N == B._M); + assert(A._M == dst._M); + assert(B._N == dst._N); + + makeZeroMatrix(dst); + for (int i = 0; i < dst._M; ++i) + for (int j = 0; j < dst._N; ++j) + { + Elem * valsC = dst.getBlock(i, j); + + for (int k = 0; k < A._N; ++k) + { + Elem const * valsA = A.getBlock(i, k); + Elem const * valsB = B.getBlock(k, j); + + for (int p = 0; p < A._P; ++p) valsC[p] += valsA[p] * valsB[p]; + } // end for (k) + } // end for (j) + } // end multiply_A_B() + + template + inline void + multiply_A_Bt(DiagonalBlockMatrix const& A, DiagonalBlockMatrix const& B, DiagonalBlockMatrix& dst) + { + assert(A._P == B._P); + assert(A._P == dst._P); + assert(A._N == B._N); + assert(A._M == dst._M); + assert(B._M == dst._N); + + makeZeroMatrix(dst); + for (int i = 0; i < dst._M; ++i) + for (int j = 0; j < dst._N; ++j) + { + Elem * valsC = dst.getBlock(i, j); + + for (int k = 0; k < A._N; ++k) + { + Elem const * valsA = A.getBlock(i, k); + Elem const * valsB = B.getBlock(j, k); + + for (int p = 0; p < A._P; ++p) valsC[p] += valsA[p] * valsB[p]; + } // end for (k) + } // end for (j) + } // end multiply_A_B() + + template + inline void + multiply_At_A(DiagonalBlockMatrix const& A, DiagonalBlockMatrix& dst) + { + assert(A._P == dst._P); + assert(A._N == dst._M); + assert(A._N == dst._N); + + makeZeroMatrix(dst); + for (int i = 0; i < A._N; ++i) + for (int j = 0; j < A._N; ++j) + { + Elem * valsC = dst.getBlock(i, j); + + for (int k = 0; k < A._M; ++k) + { + Elem const * valsAt = A.getBlock(k, i); + Elem const * valsA = A.getBlock(k, j); + + for (int p = 0; p < A._P; ++p) valsC[p] += valsAt[p] * valsA[p]; + } // end for (k) + } // end for (j) + } // end multiply_At_A() + + template + inline void + multiply_A_B(DiagonalBlockMatrix const& A, MatB const& B, MatC& dst) + { + assert(B.num_rows() == A.num_cols()); + assert(dst.num_rows() == A.num_rows()); + assert(dst.num_cols() == B.num_cols()); + + int const M = A.getM(); + int const N = A.getN(); + int const P = A.getP(); + + Vector accums(P); + + for (int i = 0; i < M; ++i) + { + int const row0 = i*P; + + for (int dstCol = 0; dstCol < dst.num_cols(); ++ dstCol) + { + makeZeroVector(accums); + + for (int j = 0; j < N; ++j) + { + int const col0 = j*P; + Elem const * valsA = A.getBlock(i, j); + for (int p = 0; p < P; ++p) accums[p] += valsA[p] * B[col0 + p][dstCol]; + } // end for (j) + for (int p = 0; p < P; ++p) dst[row0 + p][dstCol] = accums[p]; + } // end for (dstCol) + } // end for (i) + } // end multiply_A_B() + + + template + inline void + multiply_A_v(DiagonalBlockMatrix const& A, VecA const& v, VecB& dst) + { + assert(v.size() == A.num_cols()); + assert(dst.size() == A.num_rows()); + + int const M = A.getM(); + int const N = A.getN(); + int const P = A.getP(); + + makeZeroVector(dst); + + for (int i = 0; i < M; ++i) + { + int const row0 = i*P; + for (int j = 0; j < N; ++j) + { + int const col0 = j*P; + Elem const * valsA = A.getBlock(i, j); + for (int p = 0; p < P; ++p) dst[row0 + p] += valsA[p] * v[col0 + p]; + } // end for (j) + } // end for (i) + } // end multiply_A_v() + + template + inline void + multiply_At_v(DiagonalBlockMatrix const& A, VecA const& v, VecB& dst) + { + assert(v.size() == A.num_rows()); + assert(dst.size() == A.num_cols()); + + int const M = A.getM(); + int const N = A.getN(); + int const P = A.getP(); + + makeZeroVector(dst); + + for (int j = 0; j < N; ++j) + { + int const row0 = j*P; + for (int i = 0; i < M; ++i) + { + int const col0 = i*P; + Elem const * valsA = A.getBlock(i, j); + for (int p = 0; p < P; ++p) dst[row0 + p] += valsA[p] * v[col0 + p]; + } // end for (i) + } // end for (j) + } // end multiply_At_v() + + template + struct DiagonalBlock_LDL + { + DiagonalBlock_LDL(DiagonalBlockMatrix const& A) + : _L(A.getM(), A.getN(), A.getP()), + _D(A.num_rows(), Elem(0)) + { + if (A.num_rows() != A.num_cols()) + throwV3DErrorHere("DiagonalBlock_LDL(): matrix is not square."); + + int const N = A.getN(); + int const P = A.getP(); + + makeZeroMatrix(_L); + for (int n = 0; n < N; ++n) + for (int p = 0; p < P; ++p) + _L.elemAt(n, n, p) = 1; + + for (int n = 0; n < N; ++n) + { + Elem const * diagA = A.getBlock(n, n); + for (int p = 0; p < P; ++p) + { + int const j = n*P + p; + + // First, compute the current element of D + Elem d = diagA[p]; + for (int l = 0; l < n; ++l) + { + int const col = l*P + p; + Elem const Ljk = _L.elemAt(n, l, p); + d -= Ljk*Ljk*_D[col]; + } // end for (l) + _D[j] = d; + + // Update L below position (j,j) + // Note: only rows j+l*P are nonzero in L! + for (int m = n+1; m < N; ++m) + { + Elem Lij = A.elemAt(m, n, p); // Aij + for (int l = 0; l < n; ++l) + { + int const col = l*P + p; + Elem const Lik = _L.elemAt(m, l, p); + Elem const Ljk = _L.elemAt(n, l, p); + Lij -= Lik*Ljk*_D[col]; + } + Lij /= d; + _L.elemAt(m, n, p) = Lij; + } // end for (m) + } // end for (p) + } // end for (n) + } // end DiagonalBlock_LDL() + + DiagonalBlockMatrix const& getL() const { return _L; } + std::vector const& getD() const { return _D; } + + protected: + // This routine is not needed anymore + // int extractSparseLRow(int const m, int const p, std::vector& vals, std::vector& cols) const + // { + // int const P = _L.getP(); + + // int const nnz = m; // note, L is lower diagonal, and we have to ignore the main diagonal (which is 1 for LDL^T decomposition) + + // for (int i = 0; i < nnz; ++i) + // { + // cols[i] = P*i + p; + // vals[i] = _L.getBlock(m, i)[p]; + // } + + // return nnz; + // } // end extractSparseLRow() + + DiagonalBlockMatrix _L; + std::vector _D; + }; // end struct DiagonalBlock_LDL + + template + inline void + makeInvertedMatrix_PSD(DiagonalBlockMatrix const& A, DiagonalBlockMatrix& dst) + { + assert(dst.getN() == A.getN()); + assert(dst.getM() == A.getM()); + assert(dst.getP() == A.getP()); + + int const N = A.getN(); + int const P = A.getP(); + + Matrix Ai(N, N); + Matrix I(N, N); makeIdentityMatrix(I); + + for (int p = 0; p < P; ++p) + { + for (int m = 0; m < N; ++m) + for (int n = 0; n < N; ++n) + Ai[m][n] = A.elemAt(m, n, p); + +#if 0 + Cholesky chol(Ai); + Ai = chol.solve(I); +#else + LDLt ldlt(Ai); + Ai = ldlt.solveMat(I); +#endif + + for (int m = 0; m < N; ++m) + for (int n = 0; n < N; ++n) + dst.elemAt(m, n, p) = Ai[m][n]; + } // end for (p) + } // end makeInvertedMatrix() + + template + inline Elem + matrixNorm_Linf(DiagonalBlockMatrix const& A) + { + Elem res = 0; + for (int i = 0; i < A.getM(); ++i) + for (int p = 0; p < A.getP(); ++p) + { + Elem rowSum = 0; + for (int j = 0; j < A.getN(); ++j) + { + Elem const * vals = A.getBlock(i, j); + rowSum += fabs(vals[p]); + } // end for (j) + res = std::max(res, rowSum); + } // end for (p) + return res; + } // end matrixNorm_Linf() + + template + inline Elem + matrixNorm_L1(DiagonalBlockMatrix const& A) + { + Elem res = 0; + for (int j = 0; j < A.getN(); ++j) + for (int p = 0; p < A.getP(); ++p) + { + Elem colSum = 0; + for (int i = 0; i < A.getM(); ++i) + { + Elem const * vals = A.getBlock(i, j); + colSum += fabs(vals[p]); + } // end for (i) + res = std::max(res, colSum); + } // end for (p) + return res; + } // end matrixNorm_L1() + + template + inline void + displayMatrix(DiagonalBlockMatrix const& A) + { + using namespace std; + + cout << "[ "; + for (int r = 0; r < A.num_rows(); ++r) + { + for (int c = 0; c < A.num_cols(); ++c) + cout << A(r+1, c+1) << " "; + if (r < A.num_rows()-1) + cout << endl; + else + cout << "]" << endl; + } + } // end displayMatrix() + +//====================================================================== + + // Matrix with count MxN nonzero blocks along the diagonal (the standard block-diagonal matrix) + template + struct BlockDiagonalMatrix + { + BlockDiagonalMatrix(int const count, int const M, int const N) + : _count(count), _M(M), _N(N), _array(count, M, N) + { } + + int const num_rows() const { return _count*_M; } + int const num_cols() const { return _count*_N; } + + int const getM() const { return _M; } + int const getN() const { return _N; } + int const getCount() const { return _count; } + + Matrix const& getBlock(int ix) const { return _array[ix]; } + Matrix& getBlock(int ix) { return _array[ix]; } + + int getColumnNonzeroCount(unsigned int col) const { return _M; } + + template + void getSparseColumn(unsigned int col, VecA& rows, VecB& values) const + { + int const ix = col / _N; + int const j0 = ix * _N; + int const i0 = ix * _M; + int const j = col - j0; + + Matrix const& A = _array[ix]; + + for (int i = 0; i < _M; ++i) + { + rows[i] = i + i0; + values[i] = A[i][j]; + } + } // end getSparseColumn() + + int getRowNonzeroCount(unsigned int row) const { return _N; } + + template + void getSparseRow(unsigned int row, VecA& cols, VecB& values) const + { + int const ix = row / _M; + int const j0 = ix * _N; + int const i0 = ix * _M; + int const i = row - i0; + + Matrix const& A = _array[ix]; + + for (int j = 0; j < _N; ++j) + { + cols[j] = j + j0; + values[j] = A[i][j]; + } + } // end getSparseRow() + + protected: + int const _count, _M, _N; + MatrixArray _array; + }; // end struct BlockDiagonalMatrix + + template + inline void + multiply_A_v(BlockDiagonalMatrix const& A, VecA const& v, VecB& dst) + { + assert(v.size() == A.num_cols()); + assert(dst.size() == A.num_rows()); + + int const count = A.getCount(); + int const M = A.getM(); + int const N = A.getN(); + + makeZeroVector(dst); + + for (int b = 0; b < count; ++b) + { + int const row0 = b*M; + int const col0 = b*N; + + Matrix const& Asub = A.getBlock(b); + + for (int i = 0; i < M; ++i) + { + for (int j = 0; j < N; ++j) + dst[row0 + i] += Asub[i][j] * v[col0 + j]; + } // end for (i) + } // end for (b) + } // end multiply_A_v() + + template + inline void + multiply_At_v(BlockDiagonalMatrix const& A, VecA const& v, VecB& dst) + { + assert(v.size() == A.num_rows()); + assert(dst.size() == A.num_cols()); + + int const count = A.getCount(); + int const M = A.getM(); + int const N = A.getN(); + + makeZeroVector(dst); + + for (int b = 0; b < count; ++b) + { + int const row0 = b*N; + int const col0 = b*M; + + Matrix const& Asub = A.getBlock(b); + + for (int i = 0; i < N; ++i) + { + for (int j = 0; j < M; ++j) + dst[row0 + i] += Asub[j][i] * v[col0 + j]; + } // end for (i) + } // end for (b) + } // end multiply_At_v() + + // Matrixproduct of a diagonal block with a dense matrix + template + inline void + multiply_A_B(BlockDiagonalMatrix const& A, MatB const& B, MatC& dst) + { + assert(B.num_rows() == A.num_cols()); + assert(dst.num_rows() == A.num_rows()); + assert(dst.num_cols() == B.num_cols()); + + typedef typename MatC::value_type ElemDst; + + int const count = A.getCount(); + int const M = A.getM(); + int const N = A.getN(); + + for (int b = 0; b < count; ++b) + { + int const row0 = b*M; + int const col0 = b*N; + + Matrix const& Asub = A.getBlock(b); + + for (int i = 0; i < M; ++i) + { + for (int j = 0; j < dst.num_cols(); ++j) + { + ElemDst accum = 0; + for (int k = 0; k < N; ++k) accum += Asub[i][k] * B[col0 + k][j]; + dst[row0 + i][j] = accum; + } + } // end for (i) + } // end for (b) + } // end multiply_A_B() + + // Matrixproduct of a diagonal block with a dense matrix + template + inline void + multiply_A_Bt(BlockDiagonalMatrix const& A, MatB const& B, MatC& dst) + { + assert(B.num_cols() == A.num_cols()); + assert(dst.num_rows() == A.num_rows()); + assert(dst.num_cols() == B.num_rows()); + + typedef typename MatC::value_type ElemDst; + + int const count = A.getCount(); + int const M = A.getM(); + int const N = A.getN(); + + for (int b = 0; b < count; ++b) + { + int const row0 = b*M; + int const col0 = b*N; + + Matrix const& Asub = A.getBlock(b); + + for (int i = 0; i < M; ++i) + { + for (int j = 0; j < dst.num_cols(); ++j) + { + ElemDst accum = 0; + for (int k = 0; k < N; ++k) accum += Asub[i][k] * B[j][col0 + k]; + dst[row0 + i][j] = accum; + } + } // end for (i) + } // end for (b) + } // end multiply_A_Bt() + + // Matrixproduct of two block diagonal matrices yielding a block diagonal matrix + template + inline void + multiply_A_Bt(BlockDiagonalMatrix const& A, BlockDiagonalMatrix const& B, BlockDiagonalMatrix& dst) + { + assert(A.getCount() == B.getCount()); + assert(A.getCount() == dst.getCount()); + assert(A.getM() == dst.getM()); + assert(B.getM() == dst.getN()); + assert(A.getN() == B.getN()); + + int const count = dst.getCount(); + + for (int k = 0; k < count; ++k) + { + Matrix const& Ak = A.getBlock(k); + Matrix const& Bk = B.getBlock(k); + Matrix& Ck = dst.getBlock(k); + multiply_A_Bt(Ak, Bk, Ck); + } // end for (k) + } // end multiply_A_Bt() + + // Matrixproduct of two block diagonal matrices yielding a block diagonal matrix + template + inline void + multiply_A_At(BlockDiagonalMatrix const& A, BlockDiagonalMatrix& dst) + { + assert(A.getCount() == dst.getCount()); + assert(A.getM() == dst.getM()); + assert(A.getM() == dst.getN()); + + int const count = dst.getCount(); + + for (int k = 0; k < count; ++k) + { + Matrix const& Ak = A.getBlock(k); + Matrix& Ck = dst.getBlock(k); + multiply_A_At(Ak, Ck); + } // end for (k) + } // end multiply_A_At() + + // Multiplication of certain Kronecker-type matrices with block diagonal matrices + + template + inline void + multiply_AxB_C_DxE(MatA const& A, MatB const& B, BlockDiagonalMatrix const& C, MatD const& D, MatE const& E, MatDst& dst) + { + int const N = C.getCount(); + + assert(A.num_cols() == N); + assert(D.num_rows() == N); + + int const mA = A.num_rows(); + int const nA = A.num_cols(); + int const mB = B.num_rows(); + int const nB = B.num_cols(); + int const mC = C.getM(); + int const nC = C.getN(); + int const mD = D.num_rows(); + int const nD = D.num_cols(); + int const mE = E.num_rows(); + int const nE = E.num_cols(); + + assert(C.getN() == mE); + + Matrix CiE(mC, nE); + Matrix BCiE(mB, nE); + Matrix accum(mB, nE); + + MatrixArray BCE(N, mB, nE); + + for (int i = 0; i < N; ++i) + { + Matrix const& Ci = C.getBlock(i); + multiply_A_B(Ci, E, CiE); + multiply_A_B(B, CiE, BCE[i]); + } // end for (i) + + for (int k1 = 0; k1 < mA; ++k1) + { + int const dstRow0 = k1*mB; + for (int k2 = 0; k2 < nD; ++k2) + { + int const dstCol0 = k2*nE; + + makeZeroMatrix(accum); + + for (int i = 0; i < N; ++i) + { + scaleMatrix(BCE[i], A[k1][i]*D[i][k2], BCiE); + addMatricesIP(BCiE, accum); + } // end for (i) + copyMatrixSlice(accum, 0, 0, mB, nE, dst, dstRow0, dstCol0); + } // end for (k2) + } // end for (k1) + } // end multiply_AxB_C_DxE() + + template + inline void + multiply_VtxU_C_VxUt(MatU const& U, MatV const& Vt, BlockDiagonalMatrix const& C, MatDst& dst) + { + int const N = C.getCount(); + int const P = U.num_rows(); + int const K = Vt.num_rows(); + + // We need to form U*Ci*Ut + assert(U.num_cols() == C.getN()); + assert(U.num_cols() == C.getM()); + + assert(Vt.num_cols() == N); + + // dst has KxK PxP blocks + assert(dst.num_rows() == K*P); + assert(dst.num_cols() == K*P); + + Matrix CiUt(C.getM(), P); + Matrix UCiUt(P, P); + Matrix accum(P, P); + + MatrixArray UCUt(N, P, P); + + for (int i = 0; i < N; ++i) + { + Matrix const& Ci = C.getBlock(i); + multiply_A_Bt(Ci, U, CiUt); + multiply_A_B(U, CiUt, UCUt[i]); + } // end for (i) + + for (int k1 = 0; k1 < K; ++k1) + { + int const dstRow0 = k1*P; + for (int k2 = 0; k2 < K; ++k2) + { + int const dstCol0 = k2*P; + + makeZeroMatrix(accum); + + for (int i = 0; i < N; ++i) + { + scaleMatrix(UCUt[i], Vt[k1][i]*Vt[k2][i], UCiUt); + addMatricesIP(UCiUt, accum); + } // end for (i) + copyMatrixSlice(accum, 0, 0, P, P, dst, dstRow0, dstCol0); + } // end for (k2) + } // end for (k1) + } // end multiply_VtxU_C_VxUt() + + template + inline void + multiply_VtxI_C_VxI(MatV const& Vt, BlockDiagonalMatrix const& C, MatDst& dst) + { + int const N = C.getCount(); + int const P = C.getM(); + int const K = Vt.num_rows(); + + assert(P == C.getN()); + assert(Vt.num_cols() == N); + + // dst has KxK PxP blocks + assert(dst.num_rows() == K*P); + assert(dst.num_cols() == K*P); + + Matrix UCiUt(P, P); + Matrix accum(P, P); + + for (int k1 = 0; k1 < K; ++k1) + { + int const dstRow0 = k1*P; + for (int k2 = 0; k2 < K; ++k2) + { + int const dstCol0 = k2*P; + + makeZeroMatrix(accum); + + for (int i = 0; i < N; ++i) + { + Matrix const& Ci = C.getBlock(i); + scaleMatrix(Ci, Vt[k1][i]*Vt[k2][i], UCiUt); + addMatricesIP(UCiUt, accum); + } // end for (i) + copyMatrixSlice(accum, 0, 0, P, P, dst, dstRow0, dstCol0); + } // end for (k2) + } // end for (k1) + } // end multiply_VtxI_C_VxI() + + template + inline void + multiply_VtxU_C(MatU const& U, MatV const& Vt, BlockDiagonalMatrix const& C, MatDst& dst) + { + int const N = C.getCount(); + int const P = U.num_rows(); + int const K = U.num_cols(); + + assert(Vt.num_rows() == K); + assert(Vt.num_cols() == N); + + assert(C.getN() == K); + assert(C.getM() == K); + + Matrix UCi(P, K); + + MatrixArray UC(N, P, K); + + for (int i = 0; i < N; ++i) + multiply_A_B(U, C.getBlock(i), UC[i]); + + for (int k = 0; k < K; ++k) + { + int const dstRow0 = k*P; + for (int i = 0; i < N; ++i) + { + int const dstCol0 = i*K; + + copyMatrix(UC[i], UCi); + scaleMatrixIP(Vt[k][i], UCi); + copyMatrixSlice(UCi, 0, 0, P, K, dst, dstRow0, dstCol0); + } // end for (i) + } // end for (k) + } // end multiply_VtxU_C_VxUt() + +} // end namespace V3D + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_ldl_private.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_ldl_private.h new file mode 100644 index 0000000..ab498a8 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_ldl_private.h @@ -0,0 +1,276 @@ +// -*- C++ -*- +#ifndef V3D_LDL_PRIVATE_H +#define V3D_LDL_PRIVATE_H + +namespace +{ + + using namespace V3D; + + inline double + squaredResidual(VectorArray const& e) + { + int const N = e.count(); + int const M = e.size(); + + double res = 0.0; + + for (int n = 0; n < N; ++n) + for (int m = 0; m < M; ++m) + res += e[n][m] * e[n][m]; + + return res; + } // end squaredResidual() + +// Since we permute the columns/rows on our own, we can simplify (and +// accelerate) LDL slightly by removing the permutation paramater. +/* LDL Version 1.3, Copyright (c) 2006 by Timothy A Davis, + * University of Florida. All Rights Reserved. Developed while on sabbatical + * at Stanford University and Lawrence Berkeley National Laboratory. Refer to + * the README file for the License. Available at + * http://www.cise.ufl.edu/research/sparse. + */ + + typedef int LDL_int; + + inline void + LDL_symbolic + ( + LDL_int n, /* A and L are n-by-n, where n >= 0 */ + LDL_int Ap [ ], /* input of size n+1, not modified */ + LDL_int Ai [ ], /* input of size nz=Ap[n], not modified */ + LDL_int Lp [ ], /* output of size n+1, not defined on input */ + LDL_int Parent [ ], /* output of size n, not defined on input */ + LDL_int Lnz [ ], /* output of size n, not defined on input */ + LDL_int Flag [ ] /* workspace of size n, not defn. on input or output */ + ) + { + LDL_int i, k, p, kk, p2 ; + for (k = 0 ; k < n ; k++) + { + /* L(k,:) pattern: all nodes reachable in etree from nz in A(0:k-1,k) */ + Parent [k] = -1 ; /* parent of k is not yet known */ + Flag [k] = k ; /* mark node k as visited */ + Lnz [k] = 0 ; /* count of nonzeros in column k of L */ + kk = k ; /* kth original, or permuted, column */ + p2 = Ap [kk+1] ; + for (p = Ap [kk] ; p < p2 ; p++) + { + /* A (i,k) is nonzero (original or permuted A) */ + i = Ai [p] ; + if (i < k) + { + /* follow path from i to root of etree, stop at flagged node */ + for ( ; Flag [i] != k ; i = Parent [i]) + { + /* find parent of i if not yet determined */ + if (Parent [i] == -1) Parent [i] = k ; + Lnz [i]++ ; /* L (k,i) is nonzero */ + Flag [i] = k ; /* mark i as visited */ + } + } + } + } + /* construct Lp index array from Lnz column counts */ + Lp [0] = 0 ; + for (k = 0 ; k < n ; k++) + { + Lp [k+1] = Lp [k] + Lnz [k] ; + } + } + + +/* ========================================================================== */ +/* === ldl_numeric ========================================================== */ +/* ========================================================================== */ + +/* Given a sparse matrix A (the arguments n, Ap, Ai, and Ax) and its symbolic + * analysis (Lp and Parent, and optionally P and Pinv), compute the numeric LDL' + * factorization of A or PAP'. The outputs of this routine are arguments Li, + * Lx, and D. It also requires three size-n workspaces (Y, Pattern, and Flag). + */ + + inline LDL_int + LDL_numeric /* returns n if successful, k if D (k,k) is zero */ + ( + LDL_int n, /* A and L are n-by-n, where n >= 0 */ + LDL_int Ap [ ], /* input of size n+1, not modified */ + LDL_int Ai [ ], /* input of size nz=Ap[n], not modified */ + double Ax [ ], /* input of size nz=Ap[n], not modified */ + LDL_int Lp [ ], /* input of size n+1, not modified */ + LDL_int Parent [ ], /* input of size n, not modified */ + LDL_int Lnz [ ], /* output of size n, not defn. on input */ + LDL_int Li [ ], /* output of size lnz=Lp[n], not defined on input */ + double Lx [ ], /* output of size lnz=Lp[n], not defined on input */ + double D [ ], /* output of size n, not defined on input */ + double Y [ ], /* workspace of size n, not defn. on input or output */ + LDL_int Pattern [ ],/* workspace of size n, not defn. on input or output */ + LDL_int Flag [ ] /* workspace of size n, not defn. on input or output */ + ) + { + double yi, l_ki ; + LDL_int i, k, p, kk, p2, len, top ; + for (k = 0 ; k < n ; k++) + { + /* compute nonzero Pattern of kth row of L, in topological order */ + Y [k] = 0.0 ; /* Y(0:k) is now all zero */ + top = n ; /* stack for pattern is empty */ + Flag [k] = k ; /* mark node k as visited */ + Lnz [k] = 0 ; /* count of nonzeros in column k of L */ + kk = k ; /* kth original, or permuted, column */ + p2 = Ap [kk+1] ; + for (p = Ap [kk] ; p < p2 ; p++) + { + i = Ai [p] ; /* get A(i,k) */ + if (i <= k) + { + Y [i] += Ax [p] ; /* scatter A(i,k) into Y (sum duplicates) */ + for (len = 0 ; Flag [i] != k ; i = Parent [i]) + { + Pattern [len++] = i ; /* L(k,i) is nonzero */ + Flag [i] = k ; /* mark i as visited */ + } + while (len > 0) Pattern [--top] = Pattern [--len] ; + } + } + /* compute numerical values kth row of L (a sparse triangular solve) */ + D [k] = Y [k] ; /* get D(k,k) and clear Y(k) */ + Y [k] = 0.0 ; + for ( ; top < n ; top++) + { + i = Pattern [top] ; /* Pattern [top:n-1] is pattern of L(:,k) */ + yi = Y [i] ; /* get and clear Y(i) */ + Y [i] = 0.0 ; + p2 = Lp [i] + Lnz [i] ; + for (p = Lp [i] ; p < p2 ; p++) + { + Y [Li [p]] -= Lx [p] * yi ; + } + l_ki = yi / D [i] ; /* the nonzero entry L(k,i) */ + D [k] -= l_ki * yi ; + Li [p] = k ; /* store L(k,i) in column form of L */ + Lx [p] = l_ki ; + Lnz [i]++ ; /* increment count of nonzeros in col i */ + } + if (D [k] == 0.0) return (k) ; /* failure, D(k,k) is zero */ + } + return (n) ; /* success, diagonal of D is all nonzero */ + } + + +/* ========================================================================== */ +/* === ldl_lsolve: solve Lx=b ============================================== */ +/* ========================================================================== */ + + inline void + LDL_lsolve + ( + LDL_int n, /* L is n-by-n, where n >= 0 */ + double X [ ], /* size n. right-hand-side on input, soln. on output */ + LDL_int Lp [ ], /* input of size n+1, not modified */ + LDL_int Li [ ], /* input of size lnz=Lp[n], not modified */ + double Lx [ ] /* input of size lnz=Lp[n], not modified */ + ) + { + LDL_int j, p, p2 ; + for (j = 0 ; j < n ; j++) + { + p2 = Lp [j+1] ; + for (p = Lp [j] ; p < p2 ; p++) + { + X [Li [p]] -= Lx [p] * X [j] ; + } + } + } + + +/* ========================================================================== */ +/* === ldl_dsolve: solve Dx=b ============================================== */ +/* ========================================================================== */ + + inline void + LDL_dsolve + ( + LDL_int n, /* D is n-by-n, where n >= 0 */ + double X [ ], /* size n. right-hand-side on input, soln. on output */ + double D [ ] /* input of size n, not modified */ + ) + { + LDL_int j ; + for (j = 0 ; j < n ; j++) + { + X [j] /= D [j] ; + } + } + + +/* ========================================================================== */ +/* === ldl_ltsolve: solve L'x=b ============================================ */ +/* ========================================================================== */ + + inline void + LDL_ltsolve + ( + LDL_int n, /* L is n-by-n, where n >= 0 */ + double X [ ], /* size n. right-hand-side on input, soln. on output */ + LDL_int Lp [ ], /* input of size n+1, not modified */ + LDL_int Li [ ], /* input of size lnz=Lp[n], not modified */ + double Lx [ ] /* input of size lnz=Lp[n], not modified */ + ) + { + int j, p, p2 ; + for (j = n-1 ; j >= 0 ; j--) + { + p2 = Lp [j+1] ; + for (p = Lp [j] ; p < p2 ; p++) + { + X [j] -= Lx [p] * X [Li [p]] ; + } + } + } + + +/* ========================================================================== */ +/* === ldl_perm: permute a vector, x=Pb ===================================== */ +/* ========================================================================== */ + + inline void + LDL_perm + ( + LDL_int n, /* size of X, B, and P */ + double X [ ], /* output of size n. */ + double B [ ], /* input of size n. */ + LDL_int P [ ] /* input permutation array of size n. */ + ) + { + LDL_int j ; + for (j = 0 ; j < n ; j++) + { + X [j] = B [P [j]] ; + } + } + + +/* ========================================================================== */ +/* === ldl_permt: permute a vector, x=P'b =================================== */ +/* ========================================================================== */ + + inline void + LDL_permt + ( + LDL_int n, /* size of X, B, and P */ + double X [ ], /* output of size n. */ + double B [ ], /* input of size n. */ + LDL_int P [ ] /* input permutation array of size n. */ + ) + { + LDL_int j ; + for (j = 0 ; j < n ; j++) + { + X [P [j]] = B [j] ; + } + } + +} // end namespace <> + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_linear.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_linear.h new file mode 100644 index 0000000..b359624 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_linear.h @@ -0,0 +1,733 @@ +// -*- C++ -*- + +#ifndef V3D_LINEAR_H +#define V3D_LINEAR_H + +#include "Math/v3d_linearbase.h" + +#include + +namespace V3D +{ + + template + struct InlineVector : public InlineVectorBase + { + InlineVector() { } + + template + explicit InlineVector(InlineVector const& a) + { + for(int i=0; i_vec[i] = (Elem2)a[i]; + } + + template + InlineVector slice( int first ) const + { + InlineVector vec; + copyVectorSlice(*this,first,size,vec,0); + return vec; + } + }; // end struct InlineVector + + template + struct InlineVector : public InlineVectorBase + { + InlineVector() { } + + InlineVector(Elem a, Elem b) + { + InlineVectorBase::_vec[0] = a; + InlineVectorBase::_vec[1] = b; + } + + template + explicit InlineVector(InlineVector const& a) + { + for(int i=0; i<2; i++) + this->_vec[i] = (Elem2)a[i]; + } + + template + InlineVector slice( int first ) const + { + InlineVector vec; + copyVectorSlice(*this,first,size,vec,0); + return vec; + } + }; // end struct InlineVector + + template + struct InlineVector : public InlineVectorBase + { + InlineVector() { } + + InlineVector(Elem a, Elem b, Elem c) + { + InlineVectorBase::_vec[0] = a; + InlineVectorBase::_vec[1] = b; + InlineVectorBase::_vec[2] = c; + } + + InlineVector(InlineVector const &vec, Elem c) + { + InlineVectorBase::_vec[0] = vec[0]; + InlineVectorBase::_vec[1] = vec[1]; + InlineVectorBase::_vec[2] = c; + } + + template + explicit InlineVector(InlineVector const& a) + { + for(int i=0; i<3; i++) + this->_vec[i] = (Elem2)a[i]; + } + + template + InlineVector slice( int first ) const + { + InlineVector vec; + copyVectorSlice(*this,first,size,vec,0); + return vec; + } + }; // end struct InlineVector + + template + struct InlineVector : public InlineVectorBase + { + InlineVector() { } + + InlineVector(Elem a, Elem b, Elem c, Elem d) + { + InlineVectorBase::_vec[0] = a; + InlineVectorBase::_vec[1] = b; + InlineVectorBase::_vec[2] = c; + InlineVectorBase::_vec[3] = d; + } + + InlineVector( InlineVector const& v, Elem d ) + { + InlineVectorBase::_vec[0] = v[0]; + InlineVectorBase::_vec[1] = v[1]; + InlineVectorBase::_vec[2] = v[2]; + InlineVectorBase::_vec[3] = d; + } + + template + explicit InlineVector(InlineVector const& a) + { + for(int i=0; i<4; i++) + this->_vec[i] = (Elem2)a[i]; + } + + template + InlineVector slice( int first ) const + { + InlineVector vec; + copyVectorSlice(*this,first,size,vec,0); + return vec; + } + }; // end struct InlineVector + + template + struct Vector : public VectorBase + { + Vector() + : VectorBase() + { } + + Vector(unsigned int size) + : VectorBase(size) + { } + + Vector(unsigned int size, Elem * values) + : VectorBase(size, values) + { } + + Vector(Vector const& a) + : VectorBase(a) + { } + + Vector& operator=(Vector const& a) + { + (VectorBase::operator=)(a); + return *this; + } + + Vector& operator+=(Vector const& rhs) + { + addVectorsIP(rhs, *this); + return *this; + } + + Vector& operator*=(Elem scale) + { + scaleVectorIP(scale, *this); + return *this; + } + + Vector operator+(Vector const& rhs) const + { + Vector res(this->size()); + addVectors(*this, rhs, res); + return res; + } + + Vector operator-(Vector const& rhs) const + { + Vector res(this->size()); + subtractVectors(*this, rhs, res); + return res; + } + + Elem operator*(Vector const& rhs) const + { + return innerProduct(*this, rhs); + } + + }; // end struct Vector + + template + struct InlineMatrix : public InlineMatrixBase + { + template + InlineMatrix slice( int row, int col ) const + { + InlineMatrix mat; + copyMatrixSlice(*this,row,col,numRows,numCols,mat,0,0); + return mat; + } + + InlineVector row( int row ) const + { + InlineVector vec; + this->getRowSlice(row,0,Cols,vec); + return vec; + } + + InlineVector col( int col ) const + { + InlineVector vec; + this->getColumnSlice(0,Rows,col,vec); + return vec; + } + + InlineMatrix transposed() const + { + return transposedMatrix(*this); + } + }; // end struct InlineMatrix + + template + struct Matrix : public MatrixBase + { + Matrix() + : MatrixBase() + { } + + Matrix(unsigned int rows, unsigned int cols) + : MatrixBase(rows, cols) + { } + + Matrix(unsigned int rows, unsigned int cols, Elem value) + : MatrixBase(rows, cols) + { + fillMatrix(*this, value); + } + + Matrix(unsigned int rows, unsigned int cols, Elem * values) + : MatrixBase(rows, cols, values) + { } + + Matrix(Matrix const& a) + : MatrixBase(a) + { } + + Matrix& operator=(Matrix const& a) + { + (MatrixBase::operator=)(a); + return *this; + } + + Matrix& operator+=(Matrix const& rhs) + { + addMatricesIP(rhs, *this); + return *this; + } + + Matrix& operator*=(Elem scale) + { + scaleMatrixIP(scale, *this); + return *this; + } + + Matrix operator+(Matrix const& rhs) const + { + Matrix res(this->num_rows(), this->num_cols()); + addMatrices(*this, rhs, res); + return res; + } + + Matrix operator-(Matrix const& rhs) const + { + Matrix res(this->num_rows(), this->num_cols()); + subtractMatrices(*this, rhs, res); + return res; + } + + }; // end struct Matrix + +//---------------------------------------------------------------------- + + typedef InlineVector Vector2f; + typedef InlineVector Vector2d; + typedef InlineVector Vector3f; + typedef InlineVector Vector3d; + typedef InlineVector Vector4f; + typedef InlineVector Vector4d; + + typedef InlineVector Vector3b; // For color specifications e.g. + + typedef InlineMatrix Matrix2x2f; + typedef InlineMatrix Matrix2x2d; + typedef InlineMatrix Matrix3x3f; + typedef InlineMatrix Matrix3x3d; + typedef InlineMatrix Matrix4x4f; + typedef InlineMatrix Matrix4x4d; + + typedef InlineMatrix Matrix2x3f; + typedef InlineMatrix Matrix2x3d; + typedef InlineMatrix Matrix3x4f; + typedef InlineMatrix Matrix3x4d; + + template + struct VectorArray + { + VectorArray(unsigned count, unsigned size) + : _count(count), _size(size), _values(0), _vectors(0) + { + unsigned const nTotal = _count * _size; + if (count > 0) _vectors = new Vector[count]; + if (nTotal > 0) _values = new Elem[nTotal]; + for (unsigned i = 0; i < _count; ++i) new (&_vectors[i]) Vector(_size, _values + i*_size); + } + + VectorArray(unsigned count, unsigned size, Elem initVal) + : _count(count), _size(size), _values(0), _vectors(0) + { + unsigned const nTotal = _count * _size; + if (count > 0) _vectors = new Vector[count]; + if (nTotal > 0) _values = new Elem[nTotal]; + for (unsigned i = 0; i < _count; ++i) new (&_vectors[i]) Vector(_size, _values + i*_size); + std::fill(_values, _values + nTotal, initVal); + } + + ~VectorArray() + { + delete [] _values; + delete [] _vectors; + } + + unsigned count() const { return _count; } + unsigned size() const { return _size; } + + //! Get the submatrix at position ix + Vector const& operator[](unsigned ix) const + { + return _vectors[ix]; + } + + //! Get the submatrix at position ix + Vector& operator[](unsigned ix) + { + return _vectors[ix]; + } + + protected: + unsigned _count, _size; + Elem * _values; + Vector * _vectors; + + private: + VectorArray(VectorArray const&); + void operator=(VectorArray const&); + }; + + // Make a flat array look like a VectorArray, i.e. an array of vectors + template + struct VectorArrayAdapter + { + VectorArrayAdapter(unsigned count, unsigned size, Elem * values) + : _count(count), _size(size), _vectors(0) + { + if (count > 0) _vectors = new Vector[count]; + for (unsigned i = 0; i < _count; ++i) new (&_vectors[i]) Vector(_size, values + i*_size); + } + + ~VectorArrayAdapter() { delete [] _vectors; } + + unsigned count() const { return _count; } + unsigned size() const { return _size; } + + //! Get the vector at position ix + Vector const& operator[](unsigned ix) const { return _vectors[ix]; } + Vector& operator[](unsigned ix) { return _vectors[ix]; } + + protected: + unsigned _count, _size; + Vector * _vectors; + + private: + VectorArrayAdapter(VectorArrayAdapter const&); + void operator=(VectorArrayAdapter const&); + }; + + template + struct MatrixArray + { + MatrixArray(unsigned count, unsigned nRows, unsigned nCols) + : _count(count), _rows(nRows), _columns(nCols), _values(0), _matrices(0) + { + unsigned const nTotal = _count * _rows * _columns; + if (count > 0) _matrices = new Matrix[count]; + if (nTotal > 0) _values = new Elem[nTotal]; + for (unsigned i = 0; i < _count; ++i) + new (&_matrices[i]) Matrix(_rows, _columns, _values + i*(_rows*_columns)); + } + + ~MatrixArray() + { + delete [] _matrices; + delete [] _values; + } + + //! Get the submatrix at position ix + Matrix const& operator[](unsigned ix) const + { + return _matrices[ix]; + } + + //! Get the submatrix at position ix + Matrix& operator[](unsigned ix) + { + return _matrices[ix]; + } + + unsigned count() const { return _count; } + unsigned num_rows() const { return _rows; } + unsigned num_cols() const { return _columns; } + + protected: + unsigned _count, _rows, _columns; + Elem * _values; + Matrix * _matrices; + + private: + MatrixArray(MatrixArray const&); + void operator=(MatrixArray const&); + }; + +//---------------------------------------------------------------------- + + template + inline InlineVector + homogenizeVector(InlineVector const& v) + { + InlineVector res; + copyVectorSlice(v, 0, Size, res, 0); + res[Size] = 1; + return res; + } + + template + inline InlineVector + operator-(InlineVector const& v) + { + InlineVector res; + scaleVector(-1, v, res); + return res; + } + + template + inline InlineVector + operator+(InlineVector const& v, InlineVector const& w) + { + InlineVector res; + addVectors(v, w, res); + return res; + } + + template + inline InlineVector + operator-(InlineVector const& v, InlineVector const& w) + { + InlineVector res; + subtractVectors(v, w, res); + return res; + } + + template + inline InlineMatrix + operator-(InlineMatrix const& A, InlineMatrix const& B) + { + InlineMatrix res; + subtractMatrices(A, B, res); + return res; + } + + template + inline InlineMatrix + operator+(InlineMatrix const& A, InlineMatrix const& B) + { + InlineMatrix res; + addMatrices(A, B, res); + return res; + } + + template + inline InlineVector + operator*(Elem scale, InlineVector const& v) + { + InlineVector res; + scaleVector(scale, v, res); + return res; + } + + template + inline InlineMatrix + operator*(Elem scale, InlineMatrix const& A) + { + InlineMatrix res; + scaleMatrix(A, scale, res); + return res; + } + + template + inline InlineVector + operator*(InlineMatrix const& A, InlineVector const& v) + { + InlineVector res; + multiply_A_v(A, v, res); + return res; + } + + template + inline InlineVector + operator*(InlineVector const& v, InlineMatrix const& A) + { + InlineVector res; + multiply_At_v(A, v, res); + return res; + } + + template + inline InlineMatrix + operator*(InlineMatrix const& A, InlineMatrix const& B) + { + InlineMatrix res; + multiply_A_B(A, B, res); + return res; + } + + template + inline InlineMatrix + transposedMatrix(InlineMatrix const& A) + { + InlineMatrix At; + makeTransposedMatrix(A, At); + return At; + } + + template + inline InlineMatrix + invertedMatrix(InlineMatrix const& A) + { + Elem a = A[0][0], b = A[0][1], c = A[1][0], d = A[1][1]; + + Elem const det = a*d - b*c; + + InlineMatrix res; + res[0][0] = d; res[0][1] = -b; + res[1][0] = -c; res[1][1] = a; + + scaleMatrixIP(1.0/det, res); + return res; + } + + template + inline InlineMatrix + invertedMatrix(InlineMatrix const& A) + { + Elem a = A[0][0], b = A[0][1], c = A[0][2]; + Elem d = A[1][0], e = A[1][1], f = A[1][2]; + Elem g = A[2][0], h = A[2][1], i = A[2][2]; + + Elem const det = a*e*i + b*f*g + c*d*h - c*e*g - b*d*i - a*f*h; + + InlineMatrix res; + res[0][0] = e*i-f*h; res[0][1] = c*h-b*i; res[0][2] = b*f-c*e; + res[1][0] = f*g-d*i; res[1][1] = a*i-c*g; res[1][2] = c*d-a*f; + res[2][0] = d*h-e*g; res[2][1] = b*g-a*h; res[2][2] = a*e-b*d; + + scaleMatrixIP(1.0/det, res); + return res; + } + + template + inline InlineMatrix + outerProductMatrix(InlineVector const& u, InlineVector const& v) + { + InlineMatrix mat; + makeOuterProductMatrix(u,v,mat); + return mat; + } + + template + inline InlineMatrix + crossProductMatrix(InlineVector const& v) + { + InlineMatrix res; + makeCrossProductMatrix(v, res); + return res; + } + + template + inline InlineVector + crossProduct(InlineVector const& u, InlineVector const& v) + { + InlineVector res; + makeCrossProductVector(u,v,res); + return res; + } + + template + inline InlineVector + makeVector2(Elem a, Elem b) + { + InlineVector res; + res[0] = a; res[1] = b; + return res; + } + + template + inline InlineVector + makeVector3(Elem a, Elem b, Elem c) + { + InlineVector res; + res[0] = a; res[1] = b; res[2] = c; + return res; + } + + template + inline InlineVector + makeVector4(Elem a, Elem b, Elem c, Elem d) + { + InlineVector res; + res[0] = a; res[1] = b; res[2] = c; res[3] = d; + return res; + } + +//====================================================================== + + template + inline Vector + operator*(Matrix const& A, Vector const& v) + { + Vector res(A.num_rows()); + multiply_A_v(A, v, res); + return res; + } + +//====================================================================== + + template + inline void + displayVector(Vec const& v) + { + using namespace std; + + cout << "[ "; + for (int r = 0; r < v.size(); ++r) + cout << v[r] << " "; + cout << "]" << endl; + } + + template + inline void + displayMatrix(Mat const& A) + { + using namespace std; + + cout << "[ "; + for (int r = 0; r < A.num_rows(); ++r) + { + for (int c = 0; c < A.num_cols(); ++c) + cout << A[r][c] << " "; + if (r < A.num_rows()-1) + cout << endl; + else + cout << "]" << endl; + } + } + + template + inline void + displaySparseMatrix(SparseMat const& A) + { + int const n = A.num_rows(); + int const m = A.num_cols(); + + typedef typename SparseMat::value_type Elem; + + Matrix denseA(n, m); + makeZeroMatrix(denseA); + + std::vector rows(n); + std::vector vals(n); + + for (int j = 0; j < m; ++j) + { + int const nnz = A.getColumnNonzeroCount(j); + A.getSparseColumn(j, rows, vals); + + for (int k = 0; k < nnz; ++k) + { + int const i = rows[k]; + denseA[i][j] = vals[k]; + } + } // end for (j) + displayMatrix(denseA); + } // end displaySparseMatrix() + + template + inline void + showSparseMatrixInfo(CCS_Matrix const& A) + { + int const nCols = A.num_cols(); + int const nnz = A.getNonzeroCount(); + int const * colStarts = A.getColumnStarts(); + int const * rowIdxs = A.getRowIndices(); + int const * destIdxs = A.getDestIndices(); + Elem const * values = A.getValues(); + + cout << "colStarts = "; + for (int k = 0; k <= nCols; ++k) cout << colStarts[k] << " "; + cout << endl; + + cout << "rowIdxs = "; + for (int k = 0; k < nnz; ++k) cout << rowIdxs[k] << " "; + cout << endl; + + cout << "destIdxs = "; + for (int k = 0; k < nnz; ++k) cout << destIdxs[k] << " "; + cout << endl; + + cout << "values = "; + for (int k = 0; k < nnz; ++k) cout << values[k] << " "; + cout << endl; + } // end showSparseMatrixInfo() + +} // end namespace V3D + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_linear_ldlt.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_linear_ldlt.h new file mode 100644 index 0000000..b59eb20 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_linear_ldlt.h @@ -0,0 +1,112 @@ +// -*- C++ -*- +// This is a simple implementation of the LDL^T decomposition for PSD matrices + +#ifndef V3D_LINEAR_LDLT_H +#define V3D_LINEAR_LDLT_H + +#include "Math/v3d_linear.h" + +namespace V3D +{ + + template + struct LDLt + { + LDLt(Matrix const& A) + : _L(A.num_rows(), A.num_rows()), _D(A.num_rows()) + { + if (A.num_rows() != A.num_cols()) throwV3DErrorHere("LDLt::LDLt(): matrix A is not square."); + + int const N = A.num_rows(); + + makeIdentityMatrix(_L); + + for (int j = 0; j < N; ++j) + { + // First, compute the current element of D + Real Dj = 0; + for (int k = 0; k < j; ++k) Dj += _L[j][k]*_L[j][k]*_D[k]; + Dj = A[j][j] - Dj; + _D[j] = Dj; + + // Update L below position (j,j) + for (int i = j+1; i < N; ++i) + { + Real Lij = 0; + for (int k = 0; k < j; ++k) + Lij += _L[i][k]*_L[j][k]*_D[k]; + Lij = A[i][j] - Lij; + Lij /= Dj; + _L[i][j] = Lij; + } // end for (i) + } // end for (j) + } // end LDLt() + + Matrix const& getL() const { return _L; } + Vector const& getD() const { return _D; } + + template + Vector solveVec(Vec const& b) const + { + int const N = _L.num_rows(); + if (b.size() != N) throwV3DErrorHere("LDLt::solve(): size of vector b does not match."); + + Vector x(N); + copyVector(b, x); + + // Solve L*y = b; + for (int k = 0; k < N; ++k) + { + for (int i = 0; i < k; ++i) x[k] -= x[i]*_L[k][i]; + } + + for (int k = 0; k < N; ++k) x[k] /= _D[k]; + + // Solve L'*X = Y; + for (int k = N-1; k >= 0; --k) + { + for (int i = k+1; i < N; ++i) x[k] -= x[i]*_L[i][k]; + } + + return x; + } // end solveVec() + + template + Matrix solveMat(Mat const& B) const + { + int const N = _L.num_rows(); + if (B.num_rows() != N) throwV3DErrorHere("LDLt::solve(): size of matrix B does not match."); + + int const K = B.num_cols(); + + Matrix X(N, K); + copyMatrix(B, X); + + for (int j = 0; j < K; ++j) + { + // Solve L*y = b; + for (int k = 0; k < N; ++k) + { + for (int i = 0; i < k; ++i) X[k][j] -= X[i][j]*_L[k][i]; + } + + for (int k = 0; k < N; ++k) X[k][j] /= _D[k]; + + // Solve L'*X = Y; + for (int k = N-1; k >= 0; --k) + { + for (int i = k+1; i < N; ++i) X[k][j] -= X[i][j]*_L[i][k]; + } + } // end for (j) + + return X; + } // end solveMat() + + private: + Matrix _L; + Vector _D; + }; // end struct LDLt + +} // end namespace V3D + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_linear_lu.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_linear_lu.h new file mode 100644 index 0000000..8f76004 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_linear_lu.h @@ -0,0 +1,331 @@ +// -*- C++ -*- + +#ifndef V3D_LINEAR_LU_H +#define V3D_LINEAR_LU_H + +// This is adapted from the TNT matrix and linear algebra library, which is in the public domain. +// This file contains only the LU decomposition code. + +#include "Math/v3d_linear.h" +#include "Base/v3d_exception.h" + +namespace V3D +{ + + + + /** LU Decomposition. +

+ For an m-by-n matrix A with m >= n, the LU decomposition is an m-by-n + unit lower triangular matrix L, an n-by-n upper triangular matrix U, + and a permutation vector piv of length m so that A(piv,:) = L*U. + If m < n, then L is m-by-m and U is m-by-n. +

+ The LU decompostion with pivoting always exists, even if the matrix is + singular, so the constructor will never fail. The primary use of the + LU decomposition is in the solution of square systems of simultaneous + linear equations. This will fail if isNonsingular() returns false. + */ + template + class LU + { + + private: + + /* Array for internal storage of decomposition. */ + Matrix LU_; + int m, n, pivsign; + Vector piv; + + + Matrix permute_copy( const Matrix &A, + const Vector &piv, int j0, int j1) + { + int piv_length = piv.size(); + + Matrix X(piv_length, j1-j0+1); + + + for (int i = 0; i < piv_length; i++) + for (int j = j0; j <= j1; j++) + X[i][j-j0] = A[piv[i]][j]; + + return X; + } + + Vector permute_copy( const Vector &A, + const Vector &piv) + { + int piv_length = piv.size(); + if (piv_length != A.size()) + return Vector(); + + Vector x(piv_length); + + + for (int i = 0; i < piv_length; i++) + x[i] = A[piv[i]]; + + return x; + } + + + public : + + /** LU Decomposition + @param A Rectangular matrix + @return LU Decomposition object to access L, U and piv. + */ + + LU (const Matrix &A) : LU_(A), m(A.num_rows()), n(A.num_cols()), + piv(A.num_rows()) + + { + + // Use a "left-looking", dot-product, Crout/Doolittle algorithm. + + + for (int i = 0; i < m; i++) { + piv[i] = i; + } + pivsign = 1; + Real *LUrowi = 0;; + Vector LUcolj(m); + + // Outer loop. + + for (int j = 0; j < n; j++) { + + // Make a copy of the j-th column to localize references. + + for (int i = 0; i < m; i++) { + LUcolj[i] = LU_[i][j]; + } + + // Apply previous transformations. + + for (int i = 0; i < m; i++) { + LUrowi = LU_[i]; + + // Most of the time is spent in the following dot product. + + int kmax = min(i,j); + double s = Real(0.0); + for (int k = 0; k < kmax; k++) { + s += LUrowi[k]*LUcolj[k]; + } + + LUrowi[j] = LUcolj[i] -= s; + } + + // Find pivot and exchange if necessary. + + int p = j; + for (int i = j+1; i < m; i++) { + if (abs(LUcolj[i]) > abs(LUcolj[p])) { + p = i; + } + } + if (p != j) { + int k=0; + for (k = 0; k < n; k++) { + double t = LU_[p][k]; + LU_[p][k] = LU_[j][k]; + LU_[j][k] = t; + } + k = piv[p]; + piv[p] = piv[j]; + piv[j] = k; + pivsign = -pivsign; + } + + // Compute multipliers. + + if ((j < m) && (LU_[j][j] != Real(0.0))) { + for (int i = j+1; i < m; i++) { + LU_[i][j] /= LU_[j][j]; + } + } + } + } + + + /** Is the matrix nonsingular? + @return 1 (true) if upper triangular factor U (and hence A) + is nonsingular, 0 otherwise. + */ + + int isNonsingular () { + for (int j = 0; j < n; j++) { + if (LU_[j][j] == 0) + return 0; + } + return 1; + } + + /** Return lower triangular factor + @return L + */ + + Matrix getL () { + Matrix L_(m,n); + for (int i = 0; i < m; i++) { + for (int j = 0; j < n; j++) { + if (i > j) { + L_[i][j] = LU_[i][j]; + } else if (i == j) { + L_[i][j] = Real(1.0); + } else { + L_[i][j] = Real(0.0); + } + } + } + return L_; + } + + /** Return upper triangular factor + @return U portion of LU factorization. + */ + + Matrix getU () { + Matrix U_(n,n); + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) { + if (i <= j) { + U_[i][j] = LU_[i][j]; + } else { + U_[i][j] = Real(0.0); + } + } + } + return U_; + } + + /** Return pivot permutation vector + @return piv + */ + + Vector getPivot () { + return piv; + } + + + /** Compute determinant using LU factors. + @return determinant of A, or 0 if A is not square. + */ + + Real det () { + if (m != n) { + return Real(0); + } + Real d = Real(pivsign); + for (int j = 0; j < n; j++) { + d *= LU_[j][j]; + } + return d; + } + + /** Solve A*X = B + @param B A Matrix with as many rows as A and any number of columns. + @return X so that L*U*X = B(piv,:), if B is nonconformant, returns + Real(0.0) (null) array. + */ + + Matrix solve (const Matrix &B) + { + + /* Dimensions: A is mxn, X is nxk, B is mxk */ + + if (B.num_rows() != m) { + return Matrix(); + } + if (!isNonsingular()) { + return Matrix(); + } + + // Copy right hand side with pivoting + int nx = B.num_cols(); + + + Matrix X = permute_copy(B, piv, 0, nx-1); + + // Solve L*Y = B(piv,:) + for (int k = 0; k < n; k++) { + for (int i = k+1; i < n; i++) { + for (int j = 0; j < nx; j++) { + X[i][j] -= X[k][j]*LU_[i][k]; + } + } + } + // Solve U*X = Y; + for (int k = n-1; k >= 0; k--) { + for (int j = 0; j < nx; j++) { + X[k][j] /= LU_[k][k]; + } + for (int i = 0; i < k; i++) { + for (int j = 0; j < nx; j++) { + X[i][j] -= X[k][j]*LU_[i][k]; + } + } + } + return X; + } + + + /** Solve A*x = b, where x and b are vectors of length equal + to the number of rows in A. + + @param b a vector (Vector> of length equal to the first dimension + of A. + @return x a vector (Vector> so that L*U*x = b(piv), if B is nonconformant, + returns Real(0.0) (null) array. + */ + + Vector solve (const Vector &b) + { + + /* Dimensions: A is mxn, X is nxk, B is mxk */ + + if (b.size() != m) { + return Vector(); + } + if (!isNonsingular()) { + return Vector(); + } + + + Vector x = permute_copy(b, piv); + + // Solve L*Y = B(piv) + for (int k = 0; k < n; k++) { + for (int i = k+1; i < n; i++) { + x[i] -= x[k]*LU_[i][k]; + } + } + + // Solve U*X = Y; + for (int k = n-1; k >= 0; k--) { + x[k] /= LU_[k][k]; + for (int i = 0; i < k; i++) + x[i] -= x[k]*LU_[i][k]; + } + + + return x; + } + + }; /* class LU */ + + inline void + invertMatrix(Matrix const& A, Matrix& Ainv) + { + int const N = A.num_rows(); + Matrix I(N, N, 0.0); + for (int i = 0; i < N; ++i) I[i][i] = 1.0; + LU lu(A); + Ainv = lu.solve(I); + } + +} // end namespace V3D + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_linear_tnt.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_linear_tnt.h new file mode 100644 index 0000000..064ce7b --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_linear_tnt.h @@ -0,0 +1,2387 @@ +// -*- C++ -*- + +#ifndef V3D_LINEAR_TNT_H +#define V3D_LINEAR_TNT_H + +// This is adapted from the TNT matrix and linear algebra library, which is in the public domain. + +#include "Math/v3d_linear.h" +#include "Base/v3d_exception.h" + +namespace V3D +{ + + /** +

+ For a symmetric, positive definite matrix A, this function + computes the Cholesky factorization, i.e. it computes a lower + triangular matrix L such that A = L*L'. + If the matrix is not symmetric or positive definite, the function + computes only a partial decomposition. This can be tested with + the is_spd() flag. + +

Typical usage looks like: + 

+       Matrix A(n,n);
+       Matrix L;
+
+       ... 
+
+       Cholesky chol(A);
+
+       if (chol.is_spd())
+       L = chol.getL();
+		
+       else
+       cout << "factorization was not complete.\n";
+
+
+ + +

+ (Adapted from JAMA, a Java Matrix Library, developed by jointly + by the Mathworks and NIST; see http://math.nist.gov/javanumerics/jama). + + */ + + template + struct Cholesky + { + private: + Matrix L_; // lower triangular factor + int isspd; // 1 if matrix to be factored was SPD + + public: + Cholesky(); + Cholesky(const Matrix &A); + Matrix const& getL() const; + Vector solve(const Vector &B); + Matrix solve(const Matrix &B); + int is_spd() const; + + }; + + template + Cholesky::Cholesky() : L_(Real(0.0)), isspd(0) {} + +/** + @return 1, if original matrix to be factored was symmetric + positive-definite (SPD). +*/ + template + int Cholesky::is_spd() const + { + return isspd; + } + +/** + @return the lower triangular factor, L, such that L*L'=A. +*/ + template + Matrix const& Cholesky::getL() const + { + return L_; + } + +/** + Constructs a lower triangular matrix L, such that L*L'= A. + If A is not symmetric positive-definite (SPD), only a + partial factorization is performed. If is_spd() + evalutate true (1) then the factorizaiton was successful. +*/ + template + Cholesky::Cholesky(const Matrix &A) + { + int m = A.num_rows(); + int n = A.num_cols(); + + isspd = (m == n); + + if (m != n) + { + return; + } + + L_ = Matrix(n,n); + + + // Main loop. + for (int j = 0; j < n; j++) + { + Real d = Real(0.0); + for (int k = 0; k < j; k++) + { + Real s = Real(0.0); + for (int i = 0; i < k; i++) + { + s += L_[k][i]*L_[j][i]; + } + L_[j][k] = s = (A[j][k] - s)/L_[k][k]; + d = d + s*s; + isspd = isspd && (A[k][j] == A[j][k]); + } + d = A[j][j] - d; + isspd = isspd && (d > Real(0.0)); + L_[j][j] = sqrt(d > Real(0.0) ? d : Real(0.0)); + for (int k = j+1; k < n; k++) + { + L_[j][k] = Real(0.0); + } + } + } + +/** + +Solve a linear system A*x = b, using the previously computed +cholesky factorization of A: L*L'. + +@param b matrix with as many rows as A and any number of columns. +@return x so that L*L'*x = b. If b is nonconformat, or if A +was not symmetric posidtive definite, a null (Real(0.0)) +array is returned. +*/ + template + Vector Cholesky::solve(const Vector &b) + { + int n = L_.num_rows(); + //if (b.dim() != n) return Vector(); + if (int(b.size()) != n) return Vector(); + + + Vector x = b; + + + // Solve L*y = b; + for (int k = 0; k < n; k++) + { + for (int i = 0; i < k; i++) + x[k] -= x[i]*L_[k][i]; + x[k] /= L_[k][k]; + + } + + // Solve L'*X = Y; + for (int k = n-1; k >= 0; k--) + { + for (int i = k+1; i < n; i++) + x[k] -= x[i]*L_[i][k]; + x[k] /= L_[k][k]; + } + + return x; + } + + +/** + +Solve a linear system A*X = B, using the previously computed +cholesky factorization of A: L*L'. + +@param B A Matrix with as many rows as A and any number of columns. +@return X so that L*L'*X = B. If B is nonconformat, or if A +was not symmetric posidtive definite, a null (Real(0.0)) +array is returned. +*/ + template + Matrix Cholesky::solve(const Matrix &B) + { + int n = L_.num_rows(); + if (B.num_rows() != n) + return Matrix(); + + + Matrix X = B; + int nx = B.num_cols(); + + // Solve L*y = b; + for (int j=0; j< nx; j++) + { + for (int k = 0; k < n; k++) + { + for (int i = 0; i < k; i++) + X[k][j] -= X[i][j]*L_[k][i]; + X[k][j] /= L_[k][k]; + } + } + + // Solve L'*X = Y; + for (int j=0; j= 0; k--) + { + for (int i = k+1; i < n; i++) + X[k][j] -= X[i][j]*L_[i][k]; + X[k][j] /= L_[k][k]; + } + } + + return X; + } + +//---------------------------------------------------------------------- + + /** + + Computes eigenvalues and eigenvectors of a real (non-complex) + matrix. +

+ If A is symmetric, then A = V*D*V' where the eigenvalue matrix D is + diagonal and the eigenvector matrix V is orthogonal. That is, + the diagonal values of D are the eigenvalues, and + V*V' = I, where I is the identity matrix. The columns of V + represent the eigenvectors in the sense that A*V = V*D. + +

+ If A is not symmetric, then the eigenvalue matrix D is block diagonal + with the real eigenvalues in 1-by-1 blocks and any complex eigenvalues, + a + i*b, in 2-by-2 blocks, [a, b; -b, a]. That is, if the complex + eigenvalues look like + 

+
+   u + iv     .        .          .      .    .
+   .      u - iv     .          .      .    .
+   .        .      a + ib       .      .    .
+   .        .        .        a - ib   .    .
+   .        .        .          .      x    .
+   .        .        .          .      .    y
+
+ then D looks like + 
+
+   u        v        .          .      .    .
+   -v        u        .          .      .    . 
+   .        .        a          b      .    .
+   .        .       -b          a      .    .
+   .        .        .          .      x    .
+   .        .        .          .      .    y
+
+ This keeps V a real matrix in both symmetric and non-symmetric + cases, and A*V = V*D. + + + +

+ The matrix V may be badly + conditioned, or even singular, so the validity of the equation + A = V*D*inverse(V) depends upon the condition number of V. + +

+ (Adapted from JAMA, a Java Matrix Library, developed by jointly + by the Mathworks and NIST (see http://math.nist.gov/javanumerics/jama), + which in turn, were based on original EISPACK routines. + **/ + + template + class Eigenvalue + { + + + /** Row and column dimension (square matrix). */ + int n; + + int issymmetric; /* boolean*/ + + /** Arrays for internal storage of eigenvalues. */ + + Vector d; /* real part */ + Vector e; /* img part */ + + /** Array for internal storage of eigenvectors. */ + Matrix V; + + /* Array for internal storage of nonsymmetric Hessenberg form. + @serial internal storage of nonsymmetric Hessenberg form. + */ + Matrix H; + + + /* Working storage for nonsymmetric algorithm. + @serial working storage for nonsymmetric algorithm. + */ + Vector ort; + + + // Symmetric Householder reduction to tridiagonal form. + + void tred2() { + + // This is derived from the Algol procedures tred2 by + // Bowdler, Martin, Reinsch, and Wilkinson, Handbook for + // Auto. Comp., Vol.ii-Linear Algebra, and the corresponding + // Fortran subroutine in EISPACK. + + for (int j = 0; j < n; j++) { + d[j] = V[n-1][j]; + } + + // Householder reduction to tridiagonal form. + + for (int i = n-1; i > 0; i--) { + + // Scale to avoid under/overflow. + + Real scale = Real(0.0); + Real h = Real(0.0); + for (int k = 0; k < i; k++) { + scale = scale + abs(d[k]); + } + if (scale == Real(0.0)) { + e[i] = d[i-1]; + for (int j = 0; j < i; j++) { + d[j] = V[i-1][j]; + V[i][j] = Real(0.0); + V[j][i] = Real(0.0); + } + } else { + + // Generate Householder vector. + + for (int k = 0; k < i; k++) { + d[k] /= scale; + h += d[k] * d[k]; + } + Real f = d[i-1]; + Real g = sqrt(h); + if (f > 0) { + g = -g; + } + e[i] = scale * g; + h = h - f * g; + d[i-1] = f - g; + for (int j = 0; j < i; j++) { + e[j] = Real(0.0); + } + + // Apply similarity transformation to remaining columns. + + for (int j = 0; j < i; j++) { + f = d[j]; + V[j][i] = f; + g = e[j] + V[j][j] * f; + for (int k = j+1; k <= i-1; k++) { + g += V[k][j] * d[k]; + e[k] += V[k][j] * f; + } + e[j] = g; + } + f = Real(0.0); + for (int j = 0; j < i; j++) { + e[j] /= h; + f += e[j] * d[j]; + } + Real hh = f / (h + h); + for (int j = 0; j < i; j++) { + e[j] -= hh * d[j]; + } + for (int j = 0; j < i; j++) { + f = d[j]; + g = e[j]; + for (int k = j; k <= i-1; k++) { + V[k][j] -= (f * e[k] + g * d[k]); + } + d[j] = V[i-1][j]; + V[i][j] = Real(0.0); + } + } + d[i] = h; + } + + // Accumulate transformations. + + for (int i = 0; i < n-1; i++) { + V[n-1][i] = V[i][i]; + V[i][i] = Real(1.0); + Real h = d[i+1]; + if (h != Real(0.0)) { + for (int k = 0; k <= i; k++) { + d[k] = V[k][i+1] / h; + } + for (int j = 0; j <= i; j++) { + Real g = Real(0.0); + for (int k = 0; k <= i; k++) { + g += V[k][i+1] * V[k][j]; + } + for (int k = 0; k <= i; k++) { + V[k][j] -= g * d[k]; + } + } + } + for (int k = 0; k <= i; k++) { + V[k][i+1] = Real(0.0); + } + } + for (int j = 0; j < n; j++) { + d[j] = V[n-1][j]; + V[n-1][j] = Real(0.0); + } + V[n-1][n-1] = Real(1.0); + e[0] = Real(0.0); + } + + // Symmetric tridiagonal QL algorithm. + + void tql2 () { + + // This is derived from the Algol procedures tql2, by + // Bowdler, Martin, Reinsch, and Wilkinson, Handbook for + // Auto. Comp., Vol.ii-Linear Algebra, and the corresponding + // Fortran subroutine in EISPACK. + + for (int i = 1; i < n; i++) { + e[i-1] = e[i]; + } + e[n-1] = Real(0.0); + + Real f = Real(0.0); + Real tst1 = Real(0.0); + Real eps = pow(2.0,-52.0); + for (int l = 0; l < n; l++) { + + // Find small subdiagonal element + + tst1 = max(tst1,abs(d[l]) + abs(e[l])); + int m = l; + + // Original while-loop from Java code + while (m < n) { + if (abs(e[m]) <= eps*tst1) { + break; + } + m++; + } + + + // If m == l, d[l] is an eigenvalue, + // otherwise, iterate. + + if (m > l) { + int iter = 0; + do { + iter = iter + 1; // (Could check iteration count here.) + + // Compute implicit shift + + Real g = d[l]; + Real p = (d[l+1] - g) / (2.0 * e[l]); + Real r = hypot(p, static_cast(Real(1.0))); + if (p < 0) { + r = -r; + } + d[l] = e[l] / (p + r); + d[l+1] = e[l] * (p + r); + Real dl1 = d[l+1]; + Real h = g - d[l]; + for (int i = l+2; i < n; i++) { + d[i] -= h; + } + f = f + h; + + // Implicit QL transformation. + + p = d[m]; + Real c = Real(1.0); + Real c2 = c; + Real c3 = c; + Real el1 = e[l+1]; + Real s = Real(0.0); + Real s2 = Real(0.0); + for (int i = m-1; i >= l; i--) { + c3 = c2; + c2 = c; + s2 = s; + g = c * e[i]; + h = c * p; + r = hypot(p,e[i]); + e[i+1] = s * r; + s = e[i] / r; + c = p / r; + p = c * d[i] - s * g; + d[i+1] = h + s * (c * g + s * d[i]); + + // Accumulate transformation. + + for (int k = 0; k < n; k++) { + h = V[k][i+1]; + V[k][i+1] = s * V[k][i] + c * h; + V[k][i] = c * V[k][i] - s * h; + } + } + p = -s * s2 * c3 * el1 * e[l] / dl1; + e[l] = s * p; + d[l] = c * p; + + // Check for convergence. + + } while (abs(e[l]) > eps*tst1); + } + d[l] = d[l] + f; + e[l] = Real(0.0); + } + + // Sort eigenvalues and corresponding vectors. + + for (int i = 0; i < n-1; i++) { + int k = i; + Real p = d[i]; + for (int j = i+1; j < n; j++) { + if (d[j] < p) { + k = j; + p = d[j]; + } + } + if (k != i) { + d[k] = d[i]; + d[i] = p; + for (int j = 0; j < n; j++) { + p = V[j][i]; + V[j][i] = V[j][k]; + V[j][k] = p; + } + } + } + } + + // Nonsymmetric reduction to Hessenberg form. + + void orthes () { + + // This is derived from the Algol procedures orthes and ortran, + // by Martin and Wilkinson, Handbook for Auto. Comp., + // Vol.ii-Linear Algebra, and the corresponding + // Fortran subroutines in EISPACK. + + int low = 0; + int high = n-1; + + for (int m = low+1; m <= high-1; m++) { + + // Scale column. + + Real scale = Real(0.0); + for (int i = m; i <= high; i++) { + scale = scale + abs(H[i][m-1]); + } + if (scale != Real(0.0)) { + + // Compute Householder transformation. + + Real h = Real(0.0); + for (int i = high; i >= m; i--) { + ort[i] = H[i][m-1]/scale; + h += ort[i] * ort[i]; + } + Real g = sqrt(h); + if (ort[m] > 0) { + g = -g; + } + h = h - ort[m] * g; + ort[m] = ort[m] - g; + + // Apply Householder similarity transformation + // H = (I-u*u'/h)*H*(I-u*u')/h) + + for (int j = m; j < n; j++) { + Real f = Real(0.0); + for (int i = high; i >= m; i--) { + f += ort[i]*H[i][j]; + } + f = f/h; + for (int i = m; i <= high; i++) { + H[i][j] -= f*ort[i]; + } + } + + for (int i = 0; i <= high; i++) { + Real f = Real(0.0); + for (int j = high; j >= m; j--) { + f += ort[j]*H[i][j]; + } + f = f/h; + for (int j = m; j <= high; j++) { + H[i][j] -= f*ort[j]; + } + } + ort[m] = scale*ort[m]; + H[m][m-1] = scale*g; + } + } + + // Accumulate transformations (Algol's ortran). + + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) { + V[i][j] = (i == j ? Real(1.0) : Real(0.0)); + } + } + + for (int m = high-1; m >= low+1; m--) { + if (H[m][m-1] != Real(0.0)) { + for (int i = m+1; i <= high; i++) { + ort[i] = H[i][m-1]; + } + for (int j = m; j <= high; j++) { + Real g = Real(0.0); + for (int i = m; i <= high; i++) { + g += ort[i] * V[i][j]; + } + // Double division avoids possible underflow + g = (g / ort[m]) / H[m][m-1]; + for (int i = m; i <= high; i++) { + V[i][j] += g * ort[i]; + } + } + } + } + } + + + // Complex scalar division. + + Real cdivr, cdivi; + void cdiv(Real xr, Real xi, Real yr, Real yi) { + Real r,d; + if (abs(yr) > abs(yi)) { + r = yi/yr; + d = yr + r*yi; + cdivr = (xr + r*xi)/d; + cdivi = (xi - r*xr)/d; + } else { + r = yr/yi; + d = yi + r*yr; + cdivr = (r*xr + xi)/d; + cdivi = (r*xi - xr)/d; + } + } + + + // Nonsymmetric reduction from Hessenberg to real Schur form. + + void hqr2 () { + + // This is derived from the Algol procedure hqr2, + // by Martin and Wilkinson, Handbook for Auto. Comp., + // Vol.ii-Linear Algebra, and the corresponding + // Fortran subroutine in EISPACK. + + // Initialize + + int nn = this->n; + int n = nn-1; + int low = 0; + int high = nn-1; + Real eps = pow(2.0,-52.0); + Real exshift = Real(0.0); + Real p=0,q=0,r=0,s=0,z=0,t,w,x,y; + + // Store roots isolated by balanc and compute matrix norm + + Real norm = Real(0.0); + for (int i = 0; i < nn; i++) { + if ((i < low) || (i > high)) { + d[i] = H[i][i]; + e[i] = Real(0.0); + } + for (int j = max(i-1,0); j < nn; j++) { + norm = norm + abs(H[i][j]); + } + } + + // Outer loop over eigenvalue index + + int iter = 0; + while (n >= low) { + + // Look for single small sub-diagonal element + + int l = n; + while (l > low) { + s = abs(H[l-1][l-1]) + abs(H[l][l]); + if (s == Real(0.0)) { + s = norm; + } + if (abs(H[l][l-1]) < eps * s) { + break; + } + l--; + } + + // Check for convergence + // One root found + + if (l == n) { + H[n][n] = H[n][n] + exshift; + d[n] = H[n][n]; + e[n] = Real(0.0); + n--; + iter = 0; + + // Two roots found + + } else if (l == n-1) { + w = H[n][n-1] * H[n-1][n]; + p = (H[n-1][n-1] - H[n][n]) / 2.0; + q = p * p + w; + z = sqrt(abs(q)); + H[n][n] = H[n][n] + exshift; + H[n-1][n-1] = H[n-1][n-1] + exshift; + x = H[n][n]; + + // Real pair + + if (q >= 0) { + if (p >= 0) { + z = p + z; + } else { + z = p - z; + } + d[n-1] = x + z; + d[n] = d[n-1]; + if (z != Real(0.0)) { + d[n] = x - w / z; + } + e[n-1] = Real(0.0); + e[n] = Real(0.0); + x = H[n][n-1]; + s = abs(x) + abs(z); + p = x / s; + q = z / s; + r = sqrt(p * p+q * q); + p = p / r; + q = q / r; + + // Row modification + + for (int j = n-1; j < nn; j++) { + z = H[n-1][j]; + H[n-1][j] = q * z + p * H[n][j]; + H[n][j] = q * H[n][j] - p * z; + } + + // Column modification + + for (int i = 0; i <= n; i++) { + z = H[i][n-1]; + H[i][n-1] = q * z + p * H[i][n]; + H[i][n] = q * H[i][n] - p * z; + } + + // Accumulate transformations + + for (int i = low; i <= high; i++) { + z = V[i][n-1]; + V[i][n-1] = q * z + p * V[i][n]; + V[i][n] = q * V[i][n] - p * z; + } + + // Complex pair + + } else { + d[n-1] = x + p; + d[n] = x + p; + e[n-1] = z; + e[n] = -z; + } + n = n - 2; + iter = 0; + + // No convergence yet + + } else { + + // Form shift + + x = H[n][n]; + y = Real(0.0); + w = Real(0.0); + if (l < n) { + y = H[n-1][n-1]; + w = H[n][n-1] * H[n-1][n]; + } + + // Wilkinson's original ad hoc shift + + if (iter == 10) { + exshift += x; + for (int i = low; i <= n; i++) { + H[i][i] -= x; + } + s = abs(H[n][n-1]) + abs(H[n-1][n-2]); + x = y = 0.75 * s; + w = -0.4375 * s * s; + } + + // MATLAB's new ad hoc shift + + if (iter == 30) { + s = (y - x) / 2.0; + s = s * s + w; + if (s > 0) { + s = sqrt(s); + if (y < x) { + s = -s; + } + s = x - w / ((y - x) / 2.0 + s); + for (int i = low; i <= n; i++) { + H[i][i] -= s; + } + exshift += s; + x = y = w = 0.964; + } + } + + iter = iter + 1; // (Could check iteration count here.) + + // Look for two consecutive small sub-diagonal elements + + int m = n-2; + while (m >= l) { + z = H[m][m]; + r = x - z; + s = y - z; + p = (r * s - w) / H[m+1][m] + H[m][m+1]; + q = H[m+1][m+1] - z - r - s; + r = H[m+2][m+1]; + s = abs(p) + abs(q) + abs(r); + p = p / s; + q = q / s; + r = r / s; + if (m == l) { + break; + } + if (abs(H[m][m-1]) * (abs(q) + abs(r)) < + eps * (abs(p) * (abs(H[m-1][m-1]) + abs(z) + + abs(H[m+1][m+1])))) { + break; + } + m--; + } + + for (int i = m+2; i <= n; i++) { + H[i][i-2] = Real(0.0); + if (i > m+2) { + H[i][i-3] = Real(0.0); + } + } + + // Double QR step involving rows l:n and columns m:n + + for (int k = m; k <= n-1; k++) { + int notlast = (k != n-1); + if (k != m) { + p = H[k][k-1]; + q = H[k+1][k-1]; + r = (notlast ? H[k+2][k-1] : Real(0.0)); + x = abs(p) + abs(q) + abs(r); + if (x != Real(0.0)) { + p = p / x; + q = q / x; + r = r / x; + } + } + if (x == Real(0.0)) { + break; + } + s = sqrt(p * p + q * q + r * r); + if (p < 0) { + s = -s; + } + if (s != 0) { + if (k != m) { + H[k][k-1] = -s * x; + } else if (l != m) { + H[k][k-1] = -H[k][k-1]; + } + p = p + s; + x = p / s; + y = q / s; + z = r / s; + q = q / p; + r = r / p; + + // Row modification + + for (int j = k; j < nn; j++) { + p = H[k][j] + q * H[k+1][j]; + if (notlast) { + p = p + r * H[k+2][j]; + H[k+2][j] = H[k+2][j] - p * z; + } + H[k][j] = H[k][j] - p * x; + H[k+1][j] = H[k+1][j] - p * y; + } + + // Column modification + + for (int i = 0; i <= min(n,k+3); i++) { + p = x * H[i][k] + y * H[i][k+1]; + if (notlast) { + p = p + z * H[i][k+2]; + H[i][k+2] = H[i][k+2] - p * r; + } + H[i][k] = H[i][k] - p; + H[i][k+1] = H[i][k+1] - p * q; + } + + // Accumulate transformations + + for (int i = low; i <= high; i++) { + p = x * V[i][k] + y * V[i][k+1]; + if (notlast) { + p = p + z * V[i][k+2]; + V[i][k+2] = V[i][k+2] - p * r; + } + V[i][k] = V[i][k] - p; + V[i][k+1] = V[i][k+1] - p * q; + } + } // (s != 0) + } // k loop + } // check convergence + } // while (n >= low) + + // Backsubstitute to find vectors of upper triangular form + + if (norm == Real(0.0)) { + return; + } + + for (n = nn-1; n >= 0; n--) { + p = d[n]; + q = e[n]; + + // Real vector + + if (q == 0) { + int l = n; + H[n][n] = Real(1.0); + for (int i = n-1; i >= 0; i--) { + w = H[i][i] - p; + r = Real(0.0); + for (int j = l; j <= n; j++) { + r = r + H[i][j] * H[j][n]; + } + if (e[i] < Real(0.0)) { + z = w; + s = r; + } else { + l = i; + if (e[i] == Real(0.0)) { + if (w != Real(0.0)) { + H[i][n] = -r / w; + } else { + H[i][n] = -r / (eps * norm); + } + + // Solve real equations + + } else { + x = H[i][i+1]; + y = H[i+1][i]; + q = (d[i] - p) * (d[i] - p) + e[i] * e[i]; + t = (x * s - z * r) / q; + H[i][n] = t; + if (abs(x) > abs(z)) { + H[i+1][n] = (-r - w * t) / x; + } else { + H[i+1][n] = (-s - y * t) / z; + } + } + + // Overflow control + + t = abs(H[i][n]); + if ((eps * t) * t > 1) { + for (int j = i; j <= n; j++) { + H[j][n] = H[j][n] / t; + } + } + } + } + + // Complex vector + + } else if (q < 0) { + int l = n-1; + + // Last vector component imaginary so matrix is triangular + + if (abs(H[n][n-1]) > abs(H[n-1][n])) { + H[n-1][n-1] = q / H[n][n-1]; + H[n-1][n] = -(H[n][n] - p) / H[n][n-1]; + } else { + cdiv(Real(0.0),-H[n-1][n],H[n-1][n-1]-p,q); + H[n-1][n-1] = cdivr; + H[n-1][n] = cdivi; + } + H[n][n-1] = Real(0.0); + H[n][n] = Real(1.0); + for (int i = n-2; i >= 0; i--) { + Real ra,sa,vr,vi; + ra = Real(0.0); + sa = Real(0.0); + for (int j = l; j <= n; j++) { + ra = ra + H[i][j] * H[j][n-1]; + sa = sa + H[i][j] * H[j][n]; + } + w = H[i][i] - p; + + if (e[i] < Real(0.0)) { + z = w; + r = ra; + s = sa; + } else { + l = i; + if (e[i] == 0) { + cdiv(-ra,-sa,w,q); + H[i][n-1] = cdivr; + H[i][n] = cdivi; + } else { + + // Solve complex equations + + x = H[i][i+1]; + y = H[i+1][i]; + vr = (d[i] - p) * (d[i] - p) + e[i] * e[i] - q * q; + vi = (d[i] - p) * 2.0 * q; + if ((vr == Real(0.0)) && (vi == Real(0.0))) { + vr = eps * norm * (abs(w) + abs(q) + + abs(x) + abs(y) + abs(z)); + } + cdiv(x*r-z*ra+q*sa,x*s-z*sa-q*ra,vr,vi); + H[i][n-1] = cdivr; + H[i][n] = cdivi; + if (abs(x) > (abs(z) + abs(q))) { + H[i+1][n-1] = (-ra - w * H[i][n-1] + q * H[i][n]) / x; + H[i+1][n] = (-sa - w * H[i][n] - q * H[i][n-1]) / x; + } else { + cdiv(-r-y*H[i][n-1],-s-y*H[i][n],z,q); + H[i+1][n-1] = cdivr; + H[i+1][n] = cdivi; + } + } + + // Overflow control + + t = max(abs(H[i][n-1]),abs(H[i][n])); + if ((eps * t) * t > 1) { + for (int j = i; j <= n; j++) { + H[j][n-1] = H[j][n-1] / t; + H[j][n] = H[j][n] / t; + } + } + } + } + } + } + + // Vectors of isolated roots + + for (int i = 0; i < nn; i++) { + if (i < low || i > high) { + for (int j = i; j < nn; j++) { + V[i][j] = H[i][j]; + } + } + } + + // Back transformation to get eigenvectors of original matrix + + for (int j = nn-1; j >= low; j--) { + for (int i = low; i <= high; i++) { + z = Real(0.0); + for (int k = low; k <= min(j,high); k++) { + z = z + V[i][k] * H[k][j]; + } + V[i][j] = z; + } + } + } + + public: + + + /** Check for symmetry, then construct the eigenvalue decomposition + @param A Square real (non-complex) matrix + */ + + Eigenvalue(const Matrix &A) { + n = A.num_cols(); + V = Matrix(n,n); + d = Vector(n); + e = Vector(n); + + issymmetric = 1; + for (int j = 0; (j < n) && issymmetric; j++) { + for (int i = 0; (i < n) && issymmetric; i++) { + issymmetric = (A[i][j] == A[j][i]); + } + } + + if (issymmetric) { + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) { + V[i][j] = A[i][j]; + } + } + + // Tridiagonalize. + tred2(); + + // Diagonalize. + tql2(); + + } else { + H = Matrix(n,n); + ort = Vector(n); + + for (int j = 0; j < n; j++) { + for (int i = 0; i < n; i++) { + H[i][j] = A[i][j]; + } + } + + // Reduce to Hessenberg form. + orthes(); + + // Reduce Hessenberg to real Schur form. + hqr2(); + } + } + + + /** Return the eigenvector matrix + @return V + */ + + Matrix const& getV() const { return this->V; } + + void getV (Matrix &V_) { + V_ = V; + return; + } + + /** Return the real parts of the eigenvalues + @return real(diag(D)) + */ + + void getRealEigenvalues (Vector &d_) { + d_ = d; + return ; + } + + /** Return the imaginary parts of the eigenvalues + in parameter e_. + + @param e_: new matrix with imaginary parts of the eigenvalues. + */ + void getImagEigenvalues (Vector &e_) { + e_ = e; + return; + } + + +/** + Computes the block diagonal eigenvalue matrix. + If the original matrix A is not symmetric, then the eigenvalue + matrix D is block diagonal with the real eigenvalues in 1-by-1 + blocks and any complex eigenvalues, + a + i*b, in 2-by-2 blocks, [a, b; -b, a]. That is, if the complex + eigenvalues look like + 

+
+    u + iv     .        .          .      .    .
+    .      u - iv     .          .      .    .
+    .        .      a + ib       .      .    .
+    .        .        .        a - ib   .    .
+    .        .        .          .      x    .
+    .        .        .          .      .    y
+
+ then D looks like + 
+
+    u        v        .          .      .    .
+    -v        u        .          .      .    . 
+    .        .        a          b      .    .
+    .        .       -b          a      .    .
+    .        .        .          .      x    .
+    .        .        .          .      .    y
+
+ This keeps V a real matrix in both symmetric and non-symmetric + cases, and A*V = V*D. + + @param D: upon return, the matrix is filled with the block diagonal + eigenvalue matrix. + +*/ + void getD (Matrix &D) { + D = Matrix(n,n); + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) { + D[i][j] = Real(0.0); + } + D[i][i] = d[i]; + if (e[i] > 0) { + D[i][i+1] = e[i]; + } else if (e[i] < 0) { + D[i][i-1] = e[i]; + } + } + } + }; // end struct Eigenvalue + +//---------------------------------------------------------------------- + +// /** LU Decomposition. +//

+// For an m-by-n matrix A with m >= n, the LU decomposition is an m-by-n +// unit lower triangular matrix L, an n-by-n upper triangular matrix U, +// and a permutation vector piv of length m so that A(piv,:) = L*U. +// If m < n, then L is m-by-m and U is m-by-n. +//

+// The LU decompostion with pivoting always exists, even if the matrix is +// singular, so the constructor will never fail. The primary use of the +// LU decomposition is in the solution of square systems of simultaneous +// linear equations. This will fail if isNonsingular() returns false. +// */ +// template +// class LU +// { + +// private: + +// /* Array for internal storage of decomposition. */ +// Matrix LU_; +// int m, n, pivsign; +// Vector piv; + + +// Matrix permute_copy( const Matrix &A, +// const Vector &piv, int j0, int j1) +// { +// int piv_length = piv.size(); + +// Matrix X(piv_length, j1-j0+1); + + +// for (int i = 0; i < piv_length; i++) +// for (int j = j0; j <= j1; j++) +// X[i][j-j0] = A[piv[i]][j]; + +// return X; +// } + +// Vector permute_copy( const Vector &A, +// const Vector &piv) +// { +// int piv_length = piv.size(); +// if (piv_length != A.dim()) +// return Vector(); + +// Vector x(piv_length); + + +// for (int i = 0; i < piv_length; i++) +// x[i] = A[piv[i]]; + +// return x; +// } + + +// public : + +// /** LU Decomposition +// @param A Rectangular matrix +// @return LU Decomposition object to access L, U and piv. +// */ + +// LU (const Matrix &A) : LU_(A), m(A.num_rows()), n(A.num_cols()), +// piv(A.num_rows()) + +// { + +// // Use a "left-looking", dot-product, Crout/Doolittle algorithm. + + +// for (int i = 0; i < m; i++) { +// piv[i] = i; +// } +// pivsign = 1; +// Real *LUrowi = 0;; +// Vector LUcolj(m); + +// // Outer loop. + +// for (int j = 0; j < n; j++) { + +// // Make a copy of the j-th column to localize references. + +// for (int i = 0; i < m; i++) { +// LUcolj[i] = LU_[i][j]; +// } + +// // Apply previous transformations. + +// for (int i = 0; i < m; i++) { +// LUrowi = LU_[i]; + +// // Most of the time is spent in the following dot product. + +// int kmax = min(i,j); +// double s = Real(0.0); +// for (int k = 0; k < kmax; k++) { +// s += LUrowi[k]*LUcolj[k]; +// } + +// LUrowi[j] = LUcolj[i] -= s; +// } + +// // Find pivot and exchange if necessary. + +// int p = j; +// for (int i = j+1; i < m; i++) { +// if (abs(LUcolj[i]) > abs(LUcolj[p])) { +// p = i; +// } +// } +// if (p != j) { +// int k=0; +// for (k = 0; k < n; k++) { +// double t = LU_[p][k]; +// LU_[p][k] = LU_[j][k]; +// LU_[j][k] = t; +// } +// k = piv[p]; +// piv[p] = piv[j]; +// piv[j] = k; +// pivsign = -pivsign; +// } + +// // Compute multipliers. + +// if ((j < m) && (LU_[j][j] != Real(0.0))) { +// for (int i = j+1; i < m; i++) { +// LU_[i][j] /= LU_[j][j]; +// } +// } +// } +// } + + +// /** Is the matrix nonsingular? +// @return 1 (true) if upper triangular factor U (and hence A) +// is nonsingular, 0 otherwise. +// */ + +// int isNonsingular () { +// for (int j = 0; j < n; j++) { +// if (LU_[j][j] == 0) +// return 0; +// } +// return 1; +// } + +// /** Return lower triangular factor +// @return L +// */ + +// Matrix getL () { +// Matrix L_(m,n); +// for (int i = 0; i < m; i++) { +// for (int j = 0; j < n; j++) { +// if (i > j) { +// L_[i][j] = LU_[i][j]; +// } else if (i == j) { +// L_[i][j] = Real(1.0); +// } else { +// L_[i][j] = Real(0.0); +// } +// } +// } +// return L_; +// } + +// /** Return upper triangular factor +// @return U portion of LU factorization. +// */ + +// Matrix getU () { +// Matrix U_(n,n); +// for (int i = 0; i < n; i++) { +// for (int j = 0; j < n; j++) { +// if (i <= j) { +// U_[i][j] = LU_[i][j]; +// } else { +// U_[i][j] = Real(0.0); +// } +// } +// } +// return U_; +// } + +// /** Return pivot permutation vector +// @return piv +// */ + +// Vector getPivot () { +// return piv; +// } + + +// /** Compute determinant using LU factors. +// @return determinant of A, or 0 if A is not square. +// */ + +// Real det () { +// if (m != n) { +// return Real(0); +// } +// Real d = Real(pivsign); +// for (int j = 0; j < n; j++) { +// d *= LU_[j][j]; +// } +// return d; +// } + +// /** Solve A*X = B +// @param B A Matrix with as many rows as A and any number of columns. +// @return X so that L*U*X = B(piv,:), if B is nonconformant, returns +// Real(0.0) (null) array. +// */ + +// Matrix solve (const Matrix &B) +// { + +// /* Dimensions: A is mxn, X is nxk, B is mxk */ + +// if (B.num_rows() != m) { +// return Matrix(); +// } +// if (!isNonsingular()) { +// return Matrix(); +// } + +// // Copy right hand side with pivoting +// int nx = B.num_cols(); + + +// Matrix X = permute_copy(B, piv, 0, nx-1); + +// // Solve L*Y = B(piv,:) +// for (int k = 0; k < n; k++) { +// for (int i = k+1; i < n; i++) { +// for (int j = 0; j < nx; j++) { +// X[i][j] -= X[k][j]*LU_[i][k]; +// } +// } +// } +// // Solve U*X = Y; +// for (int k = n-1; k >= 0; k--) { +// for (int j = 0; j < nx; j++) { +// X[k][j] /= LU_[k][k]; +// } +// for (int i = 0; i < k; i++) { +// for (int j = 0; j < nx; j++) { +// X[i][j] -= X[k][j]*LU_[i][k]; +// } +// } +// } +// return X; +// } + + +// /** Solve A*x = b, where x and b are vectors of length equal +// to the number of rows in A. + +// @param b a vector (Vector> of length equal to the first dimension +// of A. +// @return x a vector (Vector> so that L*U*x = b(piv), if B is nonconformant, +// returns Real(0.0) (null) array. +// */ + +// Vector solve (const Vector &b) +// { + +// /* Dimensions: A is mxn, X is nxk, B is mxk */ + +// if (b.dim() != m) { +// return Vector(); +// } +// if (!isNonsingular()) { +// return Vector(); +// } + + +// Vector x = permute_copy(b, piv); + +// // Solve L*Y = B(piv) +// for (int k = 0; k < n; k++) { +// for (int i = k+1; i < n; i++) { +// x[i] -= x[k]*LU_[i][k]; +// } +// } + +// // Solve U*X = Y; +// for (int k = n-1; k >= 0; k--) { +// x[k] /= LU_[k][k]; +// for (int i = 0; i < k; i++) +// x[i] -= x[k]*LU_[i][k]; +// } + + +// return x; +// } + +// }; /* class LU */ + +//---------------------------------------------------------------------- + + /** +

+ Classical QR Decompisition: + for an m-by-n matrix A with m >= n, the QR decomposition is an m-by-n + orthogonal matrix Q and an n-by-n upper triangular matrix R so that + A = Q*R. +

+ The QR decompostion always exists, even if the matrix does not have + full rank, so the constructor will never fail. The primary use of the + QR decomposition is in the least squares solution of nonsquare systems + of simultaneous linear equations. This will fail if isFullRank() + returns 0 (false). + +

+ The Q and R factors can be retrived via the getQ() and getR() + methods. Furthermore, a solve() method is provided to find the + least squares solution of Ax=b using the QR factors. + +

+ (Adapted from JAMA, a Java Matrix Library, developed by jointly + by the Mathworks and NIST; see http://math.nist.gov/javanumerics/jama). + */ + + template + class QR { + + + /* Array for internal storage of decomposition. + @serial internal array storage. + */ + + Matrix QR_; + + /* Row and column dimensions. + @serial column dimension. + @serial row dimension. + */ + int m, n; + + /* Array for internal storage of diagonal of R. + @serial diagonal of R. + */ + Vector Rdiag; + + + public: + + /** + Create a QR factorization object for A. + + @param A rectangular (m>=n) matrix. + */ + QR(const Matrix &A) /* constructor */ + { + QR_ = A; + m = A.num_rows(); + n = A.num_cols(); + Rdiag = Vector(n); + int i=0, j=0, k=0; + + // Main loop. + for (k = 0; k < n; k++) { + // Compute 2-norm of k-th column without under/overflow. + Real nrm = 0; + for (i = k; i < m; i++) { + nrm = hypot(nrm,QR_[i][k]); + } + + if (nrm != Real(0.0)) { + // Form k-th Householder vector. + if (QR_[k][k] < 0) { + nrm = -nrm; + } + for (i = k; i < m; i++) { + QR_[i][k] /= nrm; + } + QR_[k][k] += Real(1.0); + + // Apply transformation to remaining columns. + for (j = k+1; j < n; j++) { + Real s = Real(0.0); + for (i = k; i < m; i++) { + s += QR_[i][k]*QR_[i][j]; + } + s = -s/QR_[k][k]; + for (i = k; i < m; i++) { + QR_[i][j] += s*QR_[i][k]; + } + } + } + Rdiag[k] = -nrm; + } + } + + + /** + Flag to denote the matrix is of full rank. + + @return 1 if matrix is full rank, 0 otherwise. + */ + int isFullRank() const + { + for (int j = 0; j < n; j++) + { + if (Rdiag[j] == 0) + return 0; + } + return 1; + } + + /** + Retreive the Householder vectors from QR factorization + + @returns lower trapezoidal matrix whose columns define the reflections + */ + + Matrix getHouseholder (void) const + { + Matrix H(m,n); + + /* note: H is completely filled in by algorithm, so + initializaiton of H is not necessary. + */ + for (int i = 0; i < m; i++) + { + for (int j = 0; j < n; j++) + { + if (i >= j) { + H[i][j] = QR_[i][j]; + } else { + H[i][j] = Real(0.0); + } + } + } + return H; + } + + + + /** Return the upper triangular factor, R, of the QR factorization + @return R + */ + + Matrix getR() const + { + Matrix R(n,n); + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) { + if (i < j) { + R[i][j] = QR_[i][j]; + } else if (i == j) { + R[i][j] = Rdiag[i]; + } else { + R[i][j] = Real(0.0); + } + } + } + return R; + } + + /** + @return Q the (ecnomy-sized) orthogonal factor (Q*R=A). + */ + + Matrix getQ() const + { + int i=0, j=0, k=0; + + Matrix Q(m,n); + for (k = n-1; k >= 0; k--) { + for (i = 0; i < m; i++) { + Q[i][k] = Real(0.0); + } + Q[k][k] = Real(1.0); + for (j = k; j < n; j++) { + if (QR_[k][k] != 0) { + Real s = Real(0.0); + for (i = k; i < m; i++) { + s += QR_[i][k]*Q[i][j]; + } + s = -s/QR_[k][k]; + for (i = k; i < m; i++) { + Q[i][j] += s*QR_[i][k]; + } + } + } + } + return Q; + } + + + /** Least squares solution of A*x = b + @param b right hand side (m-length vector). + @return x n-length vector that minimizes the two norm of Q*R*X-B. + If B is non-conformant, or if QR.isFullRank() is false, + the routine returns a null (0-length) vector. + */ + + Vector solve(const Vector &b) const + { +// if (b.dim() != m) /* arrays must be conformant */ +// return Vector(); + if (b.size() != m) /* arrays must be conformant */ + return Vector(); + + if ( !isFullRank() ) /* matrix is rank deficient */ + { + return Vector(); + } + + Vector x = b; + + // Compute Y = transpose(Q)*b + for (int k = 0; k < n; k++) + { + Real s = Real(0.0); + for (int i = k; i < m; i++) + { + s += QR_[i][k]*x[i]; + } + s = -s/QR_[k][k]; + for (int i = k; i < m; i++) + { + x[i] += s*QR_[i][k]; + } + } + // Solve R*X = Y; + for (int k = n-1; k >= 0; k--) + { + x[k] /= Rdiag[k]; + for (int i = 0; i < k; i++) { + x[i] -= x[k]*QR_[i][k]; + } + } + + + /* return n x nx portion of X */ + Vector x_(n); + for (int i=0; i solve(const Matrix &B) const + { + if (B.num_rows() != m) /* arrays must be conformant */ + return Matrix(Real(0.0)); + + if ( !isFullRank() ) /* matrix is rank deficient */ + { + return Matrix(Real(0.0)); + } + + int nx = B.num_cols(); + Matrix X = B; + int i=0, j=0, k=0; + + // Compute Y = transpose(Q)*B + for (k = 0; k < n; k++) { + for (j = 0; j < nx; j++) { + Real s = Real(0.0); + for (i = k; i < m; i++) { + s += QR_[i][k]*X[i][j]; + } + s = -s/QR_[k][k]; + for (i = k; i < m; i++) { + X[i][j] += s*QR_[i][k]; + } + } + } + // Solve R*X = Y; + for (k = n-1; k >= 0; k--) { + for (j = 0; j < nx; j++) { + X[k][j] /= Rdiag[k]; + } + for (i = 0; i < k; i++) { + for (j = 0; j < nx; j++) { + X[i][j] -= X[k][j]*QR_[i][k]; + } + } + } + + + /* return n x nx portion of X */ + Matrix X_(n,nx); + for (i=0; i + For an m-by-n matrix A with m >= n, the singular value decomposition is + an m-by-n orthogonal matrix U, an n-by-n diagonal matrix S, and + an n-by-n orthogonal matrix V so that A = U*S*V'. +

+ The singular values, sigma[k] = S[k][k], are ordered so that + sigma[0] >= sigma[1] >= ... >= sigma[n-1]. +

+ The singular value decompostion always exists, so the constructor will + never fail. The matrix condition number and the effective numerical + rank can be computed from this decomposition. + +

+ (Adapted from JAMA, a Java Matrix Library, developed by jointly + by the Mathworks and NIST; see http://math.nist.gov/javanumerics/jama). + */ + template + class SVD + { + Matrix U, V; + Vector s; + int m, n; + + public: + + + SVD (const Matrix &Arg) + { + m = Arg.num_rows(); + n = Arg.num_cols(); + + if (n > m) throw V3D::Exception("SVD: #columns > #rows for the provided matrix. Please apply SVD on A^T."); + + int nu = min(m,n); + s = Vector(min(m+1,n)); + U = Matrix(m, nu, Real(0)); + V = Matrix(n,n); + Vector e(n); + Vector work(m); + Matrix A(Arg); + int wantu = 1; /* boolean */ + int wantv = 1; /* boolean */ + int i=0, j=0, k=0; + + // Reduce A to bidiagonal form, storing the diagonal elements + // in s and the super-diagonal elements in e. + + int nct = min(m-1,n); + int nrt = max(0,min(n-2,m)); + for (k = 0; k < max(nct,nrt); k++) { + if (k < nct) { + + // Compute the transformation for the k-th column and + // place the k-th diagonal in s[k]. + // Compute 2-norm of k-th column without under/overflow. + s[k] = 0; + for (i = k; i < m; i++) { + s[k] = hypot(s[k],A[i][k]); + } + if (s[k] != Real(0.0)) { + if (A[k][k] < Real(0.0)) { + s[k] = -s[k]; + } + for (i = k; i < m; i++) { + A[i][k] /= s[k]; + } + A[k][k] += Real(1.0); + } + s[k] = -s[k]; + } + for (j = k+1; j < n; j++) { + if ((k < nct) && (s[k] != Real(0.0))) { + + // Apply the transformation. + + double t = 0; + for (i = k; i < m; i++) { + t += A[i][k]*A[i][j]; + } + t = -t/A[k][k]; + for (i = k; i < m; i++) { + A[i][j] += t*A[i][k]; + } + } + + // Place the k-th row of A into e for the + // subsequent calculation of the row transformation. + + e[j] = A[k][j]; + } + if (wantu & (k < nct)) { + + // Place the transformation in U for subsequent back + // multiplication. + + for (i = k; i < m; i++) { + U[i][k] = A[i][k]; + } + } + if (k < nrt) { + + // Compute the k-th row transformation and place the + // k-th super-diagonal in e[k]. + // Compute 2-norm without under/overflow. + e[k] = 0; + for (i = k+1; i < n; i++) { + e[k] = hypot(e[k],e[i]); + } + if (e[k] != Real(0.0)) { + if (e[k+1] < Real(0.0)) { + e[k] = -e[k]; + } + for (i = k+1; i < n; i++) { + e[i] /= e[k]; + } + e[k+1] += Real(1.0); + } + e[k] = -e[k]; + if ((k+1 < m) & (e[k] != Real(0.0))) { + + // Apply the transformation. + + for (i = k+1; i < m; i++) { + work[i] = Real(0.0); + } + for (j = k+1; j < n; j++) { + for (i = k+1; i < m; i++) { + work[i] += e[j]*A[i][j]; + } + } + for (j = k+1; j < n; j++) { + double t = -e[j]/e[k+1]; + for (i = k+1; i < m; i++) { + A[i][j] += t*work[i]; + } + } + } + if (wantv) { + + // Place the transformation in V for subsequent + // back multiplication. + + for (i = k+1; i < n; i++) { + V[i][k] = e[i]; + } + } + } + } + + // Set up the final bidiagonal matrix or order p. + + int p = min(n,m+1); + if (nct < n) { + s[nct] = A[nct][nct]; + } + if (m < p) { + s[p-1] = Real(0.0); + } + if (nrt+1 < p) { + e[nrt] = A[nrt][p-1]; + } + e[p-1] = Real(0.0); + + // If required, generate U. + + if (wantu) { + for (j = nct; j < nu; j++) { + for (i = 0; i < m; i++) { + U[i][j] = Real(0.0); + } + U[j][j] = Real(1.0); + } + for (k = nct-1; k >= 0; k--) { + if (s[k] != Real(0.0)) { + for (j = k+1; j < nu; j++) { + double t = 0; + for (i = k; i < m; i++) { + t += U[i][k]*U[i][j]; + } + t = -t/U[k][k]; + for (i = k; i < m; i++) { + U[i][j] += t*U[i][k]; + } + } + for (i = k; i < m; i++ ) { + U[i][k] = -U[i][k]; + } + U[k][k] = Real(1.0) + U[k][k]; + for (i = 0; i < k-1; i++) { + U[i][k] = Real(0.0); + } + } else { + for (i = 0; i < m; i++) { + U[i][k] = Real(0.0); + } + U[k][k] = Real(1.0); + } + } + } + + // If required, generate V. + + if (wantv) { + for (k = n-1; k >= 0; k--) { + if ((k < nrt) & (e[k] != Real(0.0))) { + for (j = k+1; j < nu; j++) { + double t = 0; + for (i = k+1; i < n; i++) { + t += V[i][k]*V[i][j]; + } + t = -t/V[k+1][k]; + for (i = k+1; i < n; i++) { + V[i][j] += t*V[i][k]; + } + } + } + for (i = 0; i < n; i++) { + V[i][k] = Real(0.0); + } + V[k][k] = Real(1.0); + } + } + + // Main iteration loop for the singular values. + + int pp = p-1; + int iter = 0; + double eps = pow(2.0,-52.0); + while (p > 0) { + int k=0; + int kase=0; + + // Here is where a test for too many iterations would go. + + // This section of the program inspects for + // negligible elements in the s and e arrays. On + // completion the variables kase and k are set as follows. + + // kase = 1 if s(p) and e[k-1] are negligible and k

= -1; k--) { + if (k == -1) { + break; + } + if (abs(e[k]) <= eps*(abs(s[k]) + abs(s[k+1]))) { + e[k] = Real(0.0); + break; + } + } + if (k == p-2) { + kase = 4; + } else { + int ks; + for (ks = p-1; ks >= k; ks--) { + if (ks == k) { + break; + } + double t = (ks != p ? abs(e[ks]) : 0.) + + (ks != k+1 ? abs(e[ks-1]) : 0.); + if (abs(s[ks]) <= eps*t) { + s[ks] = Real(0.0); + break; + } + } + if (ks == k) { + kase = 3; + } else if (ks == p-1) { + kase = 1; + } else { + kase = 2; + k = ks; + } + } + k++; + + // Perform the task indicated by kase. + + switch (kase) { + + // Deflate negligible s(p). + + case 1: { + double f = e[p-2]; + e[p-2] = Real(0.0); + for (j = p-2; j >= k; j--) { + double t = hypot(s[j],f); + double cs = s[j]/t; + double sn = f/t; + s[j] = t; + if (j != k) { + f = -sn*e[j-1]; + e[j-1] = cs*e[j-1]; + } + if (wantv) { + for (i = 0; i < n; i++) { + t = cs*V[i][j] + sn*V[i][p-1]; + V[i][p-1] = -sn*V[i][j] + cs*V[i][p-1]; + V[i][j] = t; + } + } + } + } + break; + + // Split at negligible s(k). + + case 2: { + double f = e[k-1]; + e[k-1] = Real(0.0); + for (j = k; j < p; j++) { + double t = hypot(s[j],f); + double cs = s[j]/t; + double sn = f/t; + s[j] = t; + f = -sn*e[j]; + e[j] = cs*e[j]; + if (wantu) { + for (i = 0; i < m; i++) { + t = cs*U[i][j] + sn*U[i][k-1]; + U[i][k-1] = -sn*U[i][j] + cs*U[i][k-1]; + U[i][j] = t; + } + } + } + } + break; + + // Perform one qr step. + + case 3: { + + // Calculate the shift. + + double scale = max(max(max(max( + abs(s[p-1]),abs(s[p-2])),abs(e[p-2])), + abs(s[k])),abs(e[k])); + double sp = s[p-1]/scale; + double spm1 = s[p-2]/scale; + double epm1 = e[p-2]/scale; + double sk = s[k]/scale; + double ek = e[k]/scale; + double b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)/2.0; + double c = (sp*epm1)*(sp*epm1); + double shift = Real(0.0); + if ((b != Real(0.0)) || (c != Real(0.0))) { + shift = sqrt(b*b + c); + if (b < Real(0.0)) { + shift = -shift; + } + shift = c/(b + shift); + } + double f = (sk + sp)*(sk - sp) + shift; + double g = sk*ek; + + // Chase zeros. + + for (j = k; j < p-1; j++) { + double t = hypot(f,g); + double cs = f/t; + double sn = g/t; + if (j != k) { + e[j-1] = t; + } + f = cs*s[j] + sn*e[j]; + e[j] = cs*e[j] - sn*s[j]; + g = sn*s[j+1]; + s[j+1] = cs*s[j+1]; + if (wantv) { + for (i = 0; i < n; i++) { + t = cs*V[i][j] + sn*V[i][j+1]; + V[i][j+1] = -sn*V[i][j] + cs*V[i][j+1]; + V[i][j] = t; + } + } + t = hypot(f,g); + cs = f/t; + sn = g/t; + s[j] = t; + f = cs*e[j] + sn*s[j+1]; + s[j+1] = -sn*e[j] + cs*s[j+1]; + g = sn*e[j+1]; + e[j+1] = cs*e[j+1]; + if (wantu && (j < m-1)) { + for (i = 0; i < m; i++) { + t = cs*U[i][j] + sn*U[i][j+1]; + U[i][j+1] = -sn*U[i][j] + cs*U[i][j+1]; + U[i][j] = t; + } + } + } + e[p-2] = f; + iter = iter + 1; + } + break; + + // Convergence. + + case 4: { + + // Make the singular values positive. + + if (s[k] <= Real(0.0)) { + s[k] = (s[k] < Real(0.0) ? -s[k] : Real(0.0)); + if (wantv) { + for (i = 0; i <= pp; i++) { + V[i][k] = -V[i][k]; + } + } + } + + // Order the singular values. + + while (k < pp) { + if (s[k] >= s[k+1]) { + break; + } + double t = s[k]; + s[k] = s[k+1]; + s[k+1] = t; + if (wantv && (k < n-1)) { + for (i = 0; i < n; i++) { + t = V[i][k+1]; V[i][k+1] = V[i][k]; V[i][k] = t; + } + } + if (wantu && (k < m-1)) { + for (i = 0; i < m; i++) { + t = U[i][k+1]; U[i][k+1] = U[i][k]; U[i][k] = t; + } + } + k++; + } + iter = 0; + p--; + } + break; + } + } + } + + Matrix getU() const + { + int minm = min(m+1,n); + + Matrix res(m, minm); + + for (int i=0; i const& getV() const { return V; } + + Matrix getS() const + { + + Matrix res(m, n); + makeZeroMatrix(res); + + for (int i = 0; i < n; i++) + res[i][i] = s[i]; + + return res; + } + + void getU (Matrix &A) + { + int minm = min(m+1,n); + + A = Matrix(m, minm); + + for (int i=0; i &A) + { + A = V; + } + + /** Return the one-dimensional array of singular values */ + + void getSingularValues (Vector &x) + { + x = s; + } + + Vector const & getSingularValues() const + { + return this->s; + } + + /** Return the diagonal matrix of singular values + @return S + */ + + void getS (Matrix &A) { + A = Matrix(n,n); + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) { + A[i][j] = Real(0.0); + } + A[i][i] = s[i]; + } + } + + /** Two norm (max(S)) */ + + double norm2 () { + return s[0]; + } + + /** Two norm of condition number (max(S)/min(S)) */ + + double cond () { + return s[0]/s[min(m,n)-1]; + } + + /** Effective numerical matrix rank + @return Number of nonnegligible singular values. + */ + + int rank () + { + double eps = pow(2.0,-52.0); + double tol = max(m,n)*s[0]*eps; + int r = 0; + for (int i = 0; i < s.dim(); i++) { + if (s[i] > tol) { + r++; + } + } + return r; + } + }; // end struct SVD + +} // end namespace V3D + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_linearbase.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_linearbase.h new file mode 100644 index 0000000..840a34c --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_linearbase.h @@ -0,0 +1,1638 @@ +// -*- C++ -*- + +#ifndef V3D_LINEAR_BASE_H +#define V3D_LINEAR_BASE_H + +#include +#include +#include +#include + +#include "Base/v3d_serialization.h" + +namespace V3D +{ + using namespace std; + + //! Unboxed vector type + template + struct InlineVectorBase + { + typedef Elem value_type; + typedef Elem element_type; + + typedef Elem const * const_iterator; + typedef Elem * iterator; + + static unsigned int size() { return Size; } + + Elem& operator[](unsigned int i) { return _vec[i]; } + Elem operator[](unsigned int i) const { return _vec[i]; } + + Elem& operator()(unsigned int i) { return _vec[i-1]; } + Elem operator()(unsigned int i) const { return _vec[i-1]; } + + const_iterator begin() const { return _vec; } + iterator begin() { return _vec; } + const_iterator end() const { return _vec + Size; } + iterator end() { return _vec + Size; } + + void newsize(unsigned int sz) const + { + assert(sz == Size); + } + + template void serialize(Archive& ar) + { + SerializationScope scope(ar); + int sz = Size; + ar & sz; + if (ar.isLoading()) this->newsize(sz); + for (int i = 0; i < sz; ++i) ar & _vec[i]; + } + + V3D_DEFINE_LOAD_SAVE(InlineVectorBase) + + protected: + Elem _vec[Size]; + }; + + //V3D_DEFINE_TEMPLATE_IOSTREAM_OPS(InlineVectorBase) + + //! Boxed (heap allocated) vector. + template + struct VectorBase + { + typedef Elem value_type; + typedef Elem element_type; + + typedef Elem const * const_iterator; + typedef Elem * iterator; + + VectorBase() + : _size(0), _ownsVec(true), _vec(0) + { } + + VectorBase(unsigned int size) + : _size(size), _ownsVec(true), _vec(0) + { + if (size > 0) _vec = new Elem[size]; + } + + VectorBase(unsigned int size, Elem * values) + : _size(size), _ownsVec(false), _vec(values) + { } + + VectorBase(VectorBase const& a) + : _size(0), _ownsVec(true), _vec(0) + { + _size = a._size; + if (_size == 0) return; + _vec = new Elem[_size]; + std::copy(a._vec, a._vec + _size, _vec); + } + + ~VectorBase() { if (_ownsVec && _vec != 0) delete [] _vec; } + + VectorBase& operator=(VectorBase const& a) + { + if (this == &a) return *this; + + this->newsize(a._size); + std::copy(a._vec, a._vec + _size, _vec); + return *this; + } + + unsigned int size() const { return _size; } + + VectorBase& newsize(unsigned int sz) + { + if (sz == _size) return *this; + assert(_ownsVec); + + __destroy(); + _size = sz; + if (_size > 0) _vec = new Elem[_size]; + + return *this; + } + + Elem& operator[](unsigned int i) { return _vec[i]; } + Elem operator[](unsigned int i) const { return _vec[i]; } + + Elem& operator()(unsigned int i) { return _vec[i-1]; } + Elem operator()(unsigned int i) const { return _vec[i-1]; } + + const_iterator begin() const { return _vec; } + iterator begin() { return _vec; } + const_iterator end() const { return _vec + _size; } + iterator end() { return _vec + _size; } + + template void serialize(Archive& ar) + { + SerializationScope scope(ar); + int sz = _size; + ar & sz; + if (ar.isLoading()) this->newsize(sz); + for (int i = 0; i < sz; ++i) ar & _vec[i]; + } + + V3D_DEFINE_LOAD_SAVE(VectorBase) + + protected: + void __destroy() + { + assert(_ownsVec); + + if (_vec != 0) delete [] _vec; + _size = 0; + _vec = 0; + } + + unsigned int _size; + bool _ownsVec; + Elem * _vec; + }; + + //V3D_DEFINE_TEMPLATE_IOSTREAM_OPS(VectorBase) + + template + struct InlineMatrixBase + { + typedef Elem value_type; + typedef Elem element_type; + + typedef Elem * iterator; + typedef Elem const * const_iterator; + + static unsigned int num_rows() { return Rows; } + static unsigned int num_cols() { return Cols; } + + Elem * operator[](unsigned int row) { return _m[row]; } + Elem const * operator[](unsigned int row) const { return _m[row]; } + + Elem& operator()(unsigned int row, unsigned int col) { return _m[row-1][col-1]; } + Elem operator()(unsigned int row, unsigned int col) const { return _m[row-1][col-1]; } + + template + void getRowSlice(unsigned int row, unsigned int first, unsigned int len, Vec& dst) const + { + for (unsigned int c = 0; c < len; ++c) dst[c] = _m[row][c+first]; + } + + template + void getColumnSlice(unsigned int first, unsigned int len, unsigned int col, Vec& dst) const + { + for (unsigned int r = 0; r < len; ++r) dst[r] = _m[r+first][col]; + } + + template + void setRowSlice(unsigned int row, unsigned int first, unsigned int len, const Vec& src) + { + for (unsigned int c = 0; c < len; ++c) _m[row][c+first] = src[c]; + } + + template + void setColumnSlice(unsigned int first, unsigned int len, unsigned int col, const Vec& src) + { + for (unsigned int r = 0; r < len; ++r) _m[r+first][col] = src[r]; + } + + void newsize(unsigned int rows, unsigned int cols) const + { + assert(rows == Rows && cols == Cols); + } + template + bool operator ==(InlineMatrixBase const & m2) const + { + assert(Rows2 == Rows && Cols2 == Cols); + int n = Rows, m = Cols; + for (int r = 0; r < n; ++r) + for (int c = 0; c < m; ++c) + if(m2._m[r][c]!=_m[r][c]) + return false; + return true; + } + + const_iterator begin() const { return &_m[0][0]; } + iterator begin() { return &_m[0][0]; } + const_iterator end() const { return &_m[0][0] + Rows*Cols; } + iterator end() { return &_m[0][0] + Rows*Cols; } + + template void serialize(Archive& ar) + { + SerializationScope scope(ar); + int n = Rows, m = Cols; + ar & n & m; + if (ar.isLoading()) this->newsize(n, m); + for (int r = 0; r < n; ++r) + for (int c = 0; c < m; ++c) + ar & _m[r][c]; + } + + V3D_DEFINE_LOAD_SAVE(InlineMatrixBase) + + protected: + Elem _m[Rows][Cols]; + }; + + //V3D_DEFINE_TEMPLATE_IOSTREAM_OPS(InlineMatrixBase) + + template + struct MatrixBase + { + typedef Elem value_type; + typedef Elem element_type; + + typedef Elem const * const_iterator; + typedef Elem * iterator; + + MatrixBase() + : _rows(0), _cols(0), _ownsData(true), _m(0) + { } + + MatrixBase(unsigned int rows, unsigned int cols) + : _rows(rows), _cols(cols), _ownsData(true), _m(0) + { + if (_rows * _cols == 0) return; + _m = new Elem[rows*cols]; + } + + MatrixBase(unsigned int rows, unsigned int cols, Elem * values) + : _rows(rows), _cols(cols), _ownsData(false), _m(values) + { } + + MatrixBase(MatrixBase const& a) + : _ownsData(true), _m(0) + { + _rows = a._rows; _cols = a._cols; + if (_rows * _cols == 0) return; + _m = new Elem[_rows*_cols]; + std::copy(a._m, a._m+_rows*_cols, _m); + } + + ~MatrixBase() + { + if (_ownsData && _m != 0) delete [] _m; + } + + MatrixBase& operator=(MatrixBase const& a) + { + if (this == &a) return *this; + + this->newsize(a.num_rows(), a.num_cols()); + + std::copy(a._m, a._m+_rows*_cols, _m); + return *this; + } + + void newsize(unsigned int rows, unsigned int cols) + { + if (rows == _rows && cols == _cols) return; + + assert(_ownsData); + + __destroy(); + + _rows = rows; + _cols = cols; + if (_rows * _cols == 0) return; + _m = new Elem[rows*cols]; + } + + unsigned int num_rows() const { return _rows; } + unsigned int num_cols() const { return _cols; } + + Elem * operator[](unsigned int row) { return _m + row*_cols; } + Elem const * operator[](unsigned int row) const { return _m + row*_cols; } + + Elem& operator()(unsigned int row, unsigned int col) { return _m[(row-1)*_cols + col-1]; } + Elem operator()(unsigned int row, unsigned int col) const { return _m[(row-1)*_cols + col-1]; } + + const_iterator begin() const { return _m; } + iterator begin() { return _m; } + const_iterator end() const { return _m + _rows*_cols; } + iterator end() { return _m + _rows*_cols; } + + template + void getRowSlice(unsigned int row, unsigned int first, unsigned int last, Vec& dst) const + { + Elem const * v = (*this)[row]; + for (unsigned int c = first; c < last; ++c) dst[c-first] = v[c]; + } + + template + void getColumnSlice(unsigned int first, unsigned int len, unsigned int col, Vec& dst) const + { + for (unsigned int r = 0; r < len; ++r) dst[r] = (*this)[r+first][col]; + } + + template + void setRowSlice(unsigned int row, unsigned int first, unsigned int len, const Vec& src) + { + Elem * v = (*this)[row]; + for (unsigned int c = 0; c < len; ++c) v[c+first] = src[c]; + } + + template + void setColumnSlice(unsigned int first, unsigned int len, unsigned int col, const Vec& src) + { + for (unsigned int r = 0; r < len; ++r) (*this)[r+first][col] = src[r]; + } + + template void serialize(Archive& ar) + { + SerializationScope scope(ar); + int n = _rows, m = _cols; + ar & n & m; + if (ar.isLoading()) this->newsize(n, m); + + for (int i = 0; i < n*m; ++i) ar & _m[i]; + } + + V3D_DEFINE_LOAD_SAVE(MatrixBase) + + protected: + void __destroy() + { + assert(_ownsData); + if (_m != 0) delete [] _m; + _m = 0; + _rows = _cols = 0; + } + + unsigned int _rows, _cols; + bool _ownsData; + Elem * _m; + }; + + //V3D_DEFINE_TEMPLATE_IOSTREAM_OPS(MatrixBase) + +//---------------------------------------------------------------------- + + template + struct CCS_Matrix + { + typedef T value_type; + typedef T element_type; + + CCS_Matrix() + : _rows(0), _cols(0) + { } + + CCS_Matrix(int const rows, int const cols, vector > const& nonZeros) + : _rows(rows), _cols(cols) + { + this->initialize(nonZeros); + } + + CCS_Matrix(int const rows, int const cols, vector > const& nonZeros, vector const& values) + : _rows(rows), _cols(cols) + { + assert(nonZeros.size() == values.size()); + + this->initialize(nonZeros); + for (size_t i = 0; i < values.size(); ++i) _values[_destIdxs[i]] = values[i]; + } + + CCS_Matrix(CCS_Matrix const& b) + : _rows(b._rows), _cols(b._cols), + _colStarts(b._colStarts), _rowIdxs(b._rowIdxs), _destIdxs(b._destIdxs), _values(b._values) + { } + + CCS_Matrix& operator=(CCS_Matrix const& b) + { + if (this == &b) return *this; + _rows = b._rows; + _cols = b._cols; + _colStarts = b._colStarts; + _rowIdxs = b._rowIdxs; + _destIdxs = b._destIdxs; + _values = b._values; + return *this; + } + + void create(int const rows, int const cols, vector > const& nonZeros) + { + _rows = rows; + _cols = cols; + this->initialize(nonZeros); + } + + void create(int const rows, int const cols, vector > const& nonZeros, vector const& values) + { + assert(nonZeros.size() == values.size()); + + _rows = rows; + _cols = cols; + this->initialize(nonZeros); + for (size_t i = 0; i < values.size(); ++i) _values[_destIdxs[i]] = values[i]; + } + + void create(int const rows, int const cols, vector > const& nonZeros, T const * values) + { + _rows = rows; + _cols = cols; + this->initialize(nonZeros); + for (size_t i = 0; i < _values.size(); ++i) _values[_destIdxs[i]] = values[i]; + } + + unsigned int num_rows() const { return _rows; } + unsigned int num_cols() const { return _cols; } + + int getNonzeroCount() const { return _values.size(); } + + T const * getValues() const { return &_values[0]; } + T * getValues() { return &_values[0]; } + + int const * getDestIndices() const { return &_destIdxs[0]; } + int const * getColumnStarts() const { return &_colStarts[0]; } + int const * getRowIndices() const { return &_rowIdxs[0]; } + + void getRowRange(unsigned int col, unsigned int& firstRow, unsigned int& lastRow) const + { + firstRow = _rowIdxs[_colStarts[col]]; + lastRow = _rowIdxs[_colStarts[col+1]-1]+1; + } + + template + void getColumnSlice(unsigned int first, unsigned int len, unsigned int col, Vec& dst) const + { + unsigned int const last = first + len; + + for (int r = 0; r < len; ++r) dst[r] = 0; // Fill vector with zeros + + int const colStart = _colStarts[col]; + int const colEnd = _colStarts[col+1]; + + int i = colStart; + int r; + // Skip rows less than the given start row + while (i < colEnd && (r = _rowIdxs[i]) < first) ++i; + + // Copy elements until the final row + while (i < colEnd && (r = _rowIdxs[i]) < last) + { + dst[r-first] = _values[i]; + ++i; + } + } // end getColumnSlice() + + int getColumnNonzeroCount(unsigned int col) const + { + int const colStart = _colStarts[col]; + int const colEnd = _colStarts[col+1]; + return colEnd - colStart; + } + + template + void getSparseColumn(unsigned int col, VecA& rows, VecB& values) const + { + int const colStart = _colStarts[col]; + int const colEnd = _colStarts[col+1]; + int const nnz = colEnd - colStart; + + for (int i = 0; i < nnz; ++i) + { + rows[i] = _rowIdxs[colStart + i]; + values[i] = _values[colStart + i]; + } + } + + void getNonzeros(std::vector >& nz) const + { + nz.clear(); + nz.reserve(_rowIdxs.size()); + for (int col = 0; col < _cols; ++col) + { + int const colStart = _colStarts[col]; + int const colEnd = _colStarts[col+1]; + int const nnz = colEnd - colStart; + + for (int i = 0; i < nnz; ++i) + nz.push_back(std::make_pair(_rowIdxs[colStart + i], col)); + } // end for (col) + } // end getNonzeros() + + protected: + struct NonzeroInfo + { + int row, col, serial; + + // Sort wrt the column first + bool operator<(NonzeroInfo const& rhs) const + { + if (col < rhs.col) return true; + if (col > rhs.col) return false; + return row < rhs.row; + } + }; + + void initialize(std::vector > const& nonZeros) + { + using namespace std; + + int const nnz = nonZeros.size(); + + _colStarts.resize(_cols + 1); + _rowIdxs.resize(nnz); + + vector nz(nnz); + for (int k = 0; k < nnz; ++k) + { + nz[k].row = nonZeros[k].first; + nz[k].col = nonZeros[k].second; + nz[k].serial = k; + } + + // Sort in column major order + std::sort(nz.begin(), nz.end()); + + for (size_t k = 0; k < nnz; ++k) _rowIdxs[k] = nz[k].row; + + int curCol = -1; + for (int k = 0; k < nnz; ++k) + { + NonzeroInfo const& el = nz[k]; + if (el.col != curCol) + { + // Update empty cols between + for (int c = curCol+1; c < el.col; ++c) _colStarts[c] = k; + + curCol = el.col; + _colStarts[curCol] = k; + } // end if + } // end for (k) + + // Update remaining columns + for (int c = curCol+1; c <= _cols; ++c) _colStarts[c] = nnz; + + _destIdxs.resize(nnz); + for (int k = 0; k < nnz; ++k) _destIdxs[nz[k].serial] = k; + + _values.resize(nnz); + } // end initialize() + + int _rows, _cols; + std::vector _colStarts; + std::vector _rowIdxs; + std::vector _destIdxs; + std::vector _values; + }; // end struct CCS_Matrix + +//---------------------------------------------------------------------- + + template + inline void + fillVector(Elem val, Vec& v) + { + // We do not use std::fill since we rely only on size() and operator[] member functions. + for (unsigned int i = 0; i < v.size(); ++i) v[i] = val; + } + + template + inline void + makeZeroVector(Vec& v) + { + fillVector(0, v); + } + + template + inline void + copyVector(VecA const& src, VecB& dst) + { + assert(src.size() == dst.size()); + // We do not use std::fill since we rely only on size() and operator[] member functions. + for (unsigned int i = 0; i < src.size(); ++i) dst[i] = src[i]; + } + + template + inline void + copyVectorSlice(VecA const& src, unsigned int srcStart, unsigned int srcLen, + VecB& dst, unsigned int dstStart) + { + unsigned int const end = std::min(srcStart + srcLen, src.size()); + unsigned int const sz = dst.size(); + unsigned int i0, i1; + for (i0 = srcStart, i1 = dstStart; i0 < end && i1 < sz; ++i0, ++i1) dst[i1] = src[i0]; + } + + template + inline typename Vec::value_type + norm_L1(Vec const& v) + { + typename Vec::value_type res(0); + for (unsigned int i = 0; i < v.size(); ++i) res += std::abs(v[i]); + return res; + } + + template + inline typename Vec::value_type + norm_Linf(Vec const& v) + { + typename Vec::value_type res(0); + for (unsigned int i = 0; i < v.size(); ++i) res = std::max(res, std::abs(v[i])); + return res; + } + + template + inline typename Vec::value_type + norm_L2(Vec const& v) + { + typename Vec::value_type res(0); + for (unsigned int i = 0; i < v.size(); ++i) res += v[i]*v[i]; + return sqrt((double)res); + } + + template + inline typename Vec::value_type + sqrNorm_L2(Vec const& v) + { + typename Vec::value_type res(0); + for (unsigned int i = 0; i < v.size(); ++i) res += v[i]*v[i]; + return res; + } + + template + inline typename VecA::value_type + distance_L2(VecA const& a, VecB const& b) + { + assert(a.size() == b.size()); + typename VecA::value_type res(0); + for (unsigned int i = 0; i < a.size(); ++i) res += (a[i]-b[i])*(a[i]-b[i]); + return sqrt(res); + } + + template + inline typename VecA::value_type + sqrDistance_L2(VecA const& a, VecB const& b) + { + assert(a.size() == b.size()); + typename VecA::value_type res(0); + for (unsigned int i = 0; i < a.size(); ++i) res += (a[i]-b[i])*(a[i]-b[i]); + return res; + } + + template + inline typename VecA::value_type + distance_Linf(VecA const& a, VecB const& b) + { + typedef typename VecA::value_type T; + assert(a.size() == b.size()); + T res(0); + for (unsigned int i = 0; i < a.size(); ++i) res = std::max(res, T(fabs(a[i] - b[i]))); + return res; + } + + template + inline void + normalizeVector(Vec& v) + { + typename Vec::value_type norm(norm_L2(v)); + for (unsigned int i = 0; i < v.size(); ++i) v[i] /= norm; + } + + template + inline typename VecA::value_type + innerProduct(VecA const& a, VecB const& b) + { + assert(a.size() == b.size()); + typename VecA::value_type res(0); + for (unsigned int i = 0; i < a.size(); ++i) res += a[i] * b[i]; + return res; + } + + template + inline void + scaleVector(Elem s, VecA const& v, VecB& dst) + { + for (unsigned int i = 0; i < v.size(); ++i) dst[i] = s * v[i]; + } + + template + inline void + scaleVectorIP(Elem s, Vec& v) + { + typedef typename Vec::value_type Elem2; + for (unsigned int i = 0; i < v.size(); ++i) + v[i] = (Elem2)(v[i] * s); + } + + template + inline void + makeCrossProductVector(VecA const& v, VecB const& w, VecC& dst) + { + assert(v.size() == 3); + assert(w.size() == 3); + assert(dst.size() == 3); + dst[0] = v[1]*w[2] - v[2]*w[1]; + dst[1] = v[2]*w[0] - v[0]*w[2]; + dst[2] = v[0]*w[1] - v[1]*w[0]; + } + + template + inline void + addVectors(VecA const& v, VecB const& w, VecC& dst) + { + assert(v.size() == w.size()); + assert(v.size() == dst.size()); + for (unsigned int i = 0; i < v.size(); ++i) dst[i] = v[i] + w[i]; + } + + template + inline void + addVectorsIP(VecA const& v, VecB& dst) + { + assert(v.size() == dst.size()); + for (unsigned int i = 0; i < v.size(); ++i) dst[i] += v[i]; + } + + template + inline void + subtractVectors(VecA const& v, VecB const& w, VecC& dst) + { + assert(v.size() == w.size()); + assert(v.size() == dst.size()); + for (unsigned int i = 0; i < v.size(); ++i) dst[i] = v[i] - w[i]; + } + + template + inline void + makeInterpolatedVector(Elem a, VecA const& v, Elem b, VecB const& w, VecC& dst) + { + assert(v.size() == w.size()); + assert(v.size() == dst.size()); + for (unsigned int i = 0; i < v.size(); ++i) dst[i] = a * v[i] + b * w[i]; + } + + template + inline typename VecA::value_type + unsignedAngleBetweenVectors(VecA const& v, VecB const& w) + { + assert(v.size() == w.size()); + typename VecA::value_type dot = innerProduct(v, w) / norm_L2(v) / norm_L2(w); + if (dot > 1.0) return 0; + if (dot < -1.0) return M_PI; + return acos(dot); + } + + template + inline void + copyMatrix(MatA const& src, MatB& dst) + { + unsigned int const rows = src.num_rows(); + unsigned int const cols = src.num_cols(); + assert(dst.num_rows() == rows); + assert(dst.num_cols() == cols); + for (unsigned int c = 0; c < cols; ++c) + for (unsigned int r = 0; r < rows; ++r) dst[r][c] = src[r][c]; + } + + template + inline void + copyMatrixSlice(MatA const& src, unsigned int rowStart, unsigned int colStart, unsigned int rowLen, unsigned int colLen, + MatB& dst, unsigned int dstRow, unsigned int dstCol) + { + unsigned int const rows = dst.num_rows(); + unsigned int const cols = dst.num_cols(); + + unsigned int const rowEnd = std::min(rowStart + rowLen, src.num_rows()); + unsigned int const colEnd = std::min(colStart + colLen, src.num_cols()); + + unsigned int c0, c1, r0, r1; + + for (c0 = colStart, c1 = dstCol; c0 < colEnd && c1 < cols; ++c0, ++c1) + for (r0 = rowStart, r1 = dstRow; r0 < rowEnd && r1 < rows; ++r0, ++r1) + dst[r1][c1] = src[r0][c0]; + } + + template + inline void + makeTransposedMatrix(MatA const& src, MatB& dst) + { + unsigned int const rows = src.num_rows(); + unsigned int const cols = src.num_cols(); + assert(dst.num_cols() == rows); + assert(dst.num_rows() == cols); + for (unsigned int c = 0; c < cols; ++c) + for (unsigned int r = 0; r < rows; ++r) dst[c][r] = src[r][c]; + } + + template + inline void + makeTransposedSparseMatrix(SparseMat const& src, SparseMat& dst) + { + unsigned int const rows = src.num_rows(); + unsigned int const cols = src.num_cols(); + + std::vector > nz; + src.getNonZeros(nz); + + for (size_t i = 0; i < nz.size(); ++i) std::swap(nz[i].first, nz[i].second); + + dst.create(cols, rows, nz, src.getValues()); + } // end makeTransposedSparseMatrix() + + + template + inline void + fillMatrix(Mat& m, typename Mat::value_type val) + { + unsigned int const rows = m.num_rows(); + unsigned int const cols = m.num_cols(); + for (unsigned int c = 0; c < cols; ++c) + for (unsigned int r = 0; r < rows; ++r) m[r][c] = val; + } + + template + inline void + makeZeroMatrix(Mat& m) + { + fillMatrix(m, 0); + } + + template + inline void + makeIdentityMatrix(Mat& m) + { + makeZeroMatrix(m); + unsigned int const rows = m.num_rows(); + unsigned int const cols = m.num_cols(); + unsigned int n = std::min(rows, cols); + for (unsigned int i = 0; i < n; ++i) + m[i][i] = 1; + } + + template + inline void + makeCrossProductMatrix(Vec const& v, Mat& m) + { + assert(v.size() == 3); + assert(m.num_rows() == 3); + assert(m.num_cols() == 3); + m[0][0] = 0; m[0][1] = -v[2]; m[0][2] = v[1]; + m[1][0] = v[2]; m[1][1] = 0; m[1][2] = -v[0]; + m[2][0] = -v[1]; m[2][1] = v[0]; m[2][2] = 0; + } + + template + inline void + makeOuterProductMatrix(Vec const& v, Mat& m) + { + assert(m.num_cols() == m.num_rows()); + assert(v.size() == m.num_cols()); + unsigned const sz = v.size(); + for (unsigned r = 0; r < sz; ++r) + for (unsigned c = 0; c < sz; ++c) m[r][c] = v[r]*v[c]; + } + + template + inline void + makeOuterProductMatrix(VecA const& u, VecB const& v, Mat& m) + { + assert(u.size() == m.num_rows()); + assert(v.size() == m.num_cols()); + unsigned const sz = u.size(); + for (unsigned r = 0; r < m.num_rows(); ++r) + for (unsigned c = 0; c < m.num_cols(); ++c) m[r][c] = u[r]*v[c]; + } + + template + inline void + makeKroneckerProductMatrix(MatA const& A, MatB const& B, MatC& dst) + { + typedef typename MatA::value_type NumA; + + int const mA = A.num_rows(); + int const nA = A.num_cols(); + int const mB = B.num_rows(); + int const nB = B.num_cols(); + + assert(dst.num_rows() == mA*mB); + assert(dst.num_cols() == nA*nB); + + for (int i1 = 0; i1 < mA; ++i1) + { + int const dstRow0 = i1*mB; + for (int j1 = 0; j1 < nA; ++j1) + { + int const dstCol0 = j1*nB; + + NumA const a = A[i1][j1]; + for (int i2 = 0; i2 < mB; ++i2) + for (int j2 = 0; j2 < nB; ++j2) + dst[dstRow0 + i2][dstCol0 + j2] = a*B[i2][j2]; + } // end for (j1) + } // end for (i1) + } // end makeKroneckerProductMatrix() + +//====================================================================== + + template + void addMatrices(MatA const& a, MatB const& b, MatC& dst) + { + assert(a.num_cols() == b.num_cols()); + assert(a.num_rows() == b.num_rows()); + assert(dst.num_cols() == a.num_cols()); + assert(dst.num_rows() == a.num_rows()); + + unsigned int const rows = a.num_rows(); + unsigned int const cols = a.num_cols(); + + for (unsigned r = 0; r < rows; ++r) + for (unsigned c = 0; c < cols; ++c) dst[r][c] = a[r][c] + b[r][c]; + } + + template + void addMatricesIP(MatA const& a, MatB& dst) + { + assert(dst.num_cols() == a.num_cols()); + assert(dst.num_rows() == a.num_rows()); + + unsigned int const rows = a.num_rows(); + unsigned int const cols = a.num_cols(); + + for (unsigned r = 0; r < rows; ++r) + for (unsigned c = 0; c < cols; ++c) dst[r][c] += a[r][c]; + } + + template + inline void + addMatricesIP_SparseDense(SparseMatA const& a, MatB& dst) + { + assert(dst.num_cols() == a.num_cols()); + assert(dst.num_rows() == a.num_rows()); + + typedef typename MatB::value_type Elem; + + std::vector rows(a.num_rows()); + std::vector vals(a.num_rows()); + + for (unsigned c = 0; c < dst.num_cols(); ++c) + { + int const nnz = a.getColumnNonzeroCount(c); + a.getSparseColumn(c, rows, vals); + for (int k = 0; k < nnz; ++k) + dst[rows[k]][c] += vals[k]; + } // end for (c) + } // end addMatricesIP_SparseDense() + + template + void subtractMatrices(MatA const& a, MatB const& b, MatC& dst) + { + assert(a.num_cols() == b.num_cols()); + assert(a.num_rows() == b.num_rows()); + assert(dst.num_cols() == a.num_cols()); + assert(dst.num_rows() == a.num_rows()); + + unsigned int const rows = a.num_rows(); + unsigned int const cols = a.num_cols(); + + for (unsigned r = 0; r < rows; ++r) + for (unsigned c = 0; c < cols; ++c) dst[r][c] = a[r][c] - b[r][c]; + } + + template + inline void + scaleMatrix(MatA const& m, Elem scale, MatB& dst) + { + unsigned int const rows = m.num_rows(); + unsigned int const cols = m.num_cols(); + for (unsigned int c = 0; c < cols; ++c) + for (unsigned int r = 0; r < rows; ++r) dst[r][c] = m[r][c] * scale; + } + + template + inline void + scaleMatrixIP(Elem scale, Mat& m) + { + unsigned int const rows = m.num_rows(); + unsigned int const cols = m.num_cols(); + for (unsigned int c = 0; c < cols; ++c) + for (unsigned int r = 0; r < rows; ++r) m[r][c] *= scale; + } + + template + inline void + multiply_A_v(Mat const& m, VecA const& in, VecB& dst) + { + unsigned int const rows = m.num_rows(); + unsigned int const cols = m.num_cols(); + assert(in.size() == cols); + assert(dst.size() == rows); + + makeZeroVector(dst); + + for (unsigned int r = 0; r < rows; ++r) + for (unsigned int c = 0; c < cols; ++c) dst[r] += m[r][c] * in[c]; + } + + template + inline void + multiply_A_v_projective(Mat const& m, VecA const& in, VecB& dst) + { + unsigned int const rows = m.num_rows(); + unsigned int const cols = m.num_cols(); + assert(in.size() == cols-1); + assert(dst.size() == rows-1); + + typename VecB::value_type w = m[rows-1][cols-1]; + unsigned int r, i; + for (i = 0; i < cols-1; ++i) w += m[rows-1][i] * in[i]; + for (r = 0; r < rows-1; ++r) dst[r] = m[r][cols-1]; + for (r = 0; r < rows-1; ++r) + for (unsigned int c = 0; c < cols-1; ++c) dst[r] += m[r][c] * in[c]; + for (i = 0; i < rows-1; ++i) dst[i] /= w; + } + + template + inline void + multiply_A_v_affine(Mat const& m, VecA const& in, VecB& dst) + { + unsigned int const rows = m.num_rows(); + unsigned int const cols = m.num_cols(); + assert(in.size() == cols-1); + assert(dst.size() == rows); + + unsigned int r; + + for (r = 0; r < rows; ++r) dst[r] = m[r][cols-1]; + for (r = 0; r < rows; ++r) + for (unsigned int c = 0; c < cols-1; ++c) dst[r] += m[r][c] * in[c]; + } + + template + inline void + multiply_At_v(Mat const& m, VecA const& in, VecB& dst) + { + unsigned int const rows = m.num_rows(); + unsigned int const cols = m.num_cols(); + assert(in.size() == rows); + assert(dst.size() == cols); + + makeZeroVector(dst); + for (unsigned int c = 0; c < cols; ++c) + for (unsigned int r = 0; r < rows; ++r) dst[c] += m[r][c] * in[r]; + } + + template + inline void + multiply_At_v_Sparse(SparseMat const& a, VecA const& in, VecB& dst) + { + assert(in.size() == a.num_rows()); + assert(dst.size() == a.num_cols()); + + typedef typename VecB::value_type Elem; + + std::vector rows(a.num_rows()); + std::vector vals(a.num_rows()); + + makeZeroVector(dst); + for (unsigned int c = 0; c < a.num_cols(); ++c) + { + int const nnz = a.getColumnNonzeroCount(c); + a.getSparseColumn(c, rows, vals); + + Elem accum = 0; + + for (int i = 0; i < nnz; ++i) + { + int const r = rows[i]; + accum += vals[i] * in[r]; + } + dst[c] = accum; + } + } // end multiply_At_v_Sparse() + + template + inline void + multiply_At_A(MatA const& a, MatB& dst) + { + assert(dst.num_rows() == a.num_cols()); + assert(dst.num_cols() == a.num_cols()); + + typedef typename MatB::value_type Elem; + + int const M = a.num_rows(); + int const N = a.num_cols(); + + Elem accum; + for (int r = 0; r < N; ++r) + for (int c = 0; c < N; ++c) + { + accum = 0; + for (int k = 0; k < M; ++k) accum += a[k][r] * a[k][c]; + dst[r][c] = accum; + } + } // end multiply_At_A + + template + inline void + multiply_A_At(MatA const& a, MatC& dst) + { + assert(dst.num_rows() == a.num_rows()); + assert(dst.num_cols() == a.num_rows()); + + typedef typename MatC::value_type Elem; + + Elem accum; + for (unsigned int r = 0; r < a.num_rows(); ++r) + for (unsigned int c = 0; c < a.num_rows(); ++c) + { + accum = 0; + for (unsigned int k = 0; k < a.num_cols(); ++k) accum += a[r][k] * a[c][k]; + dst[r][c] = accum; + } + } // end multiply_A_At() + + template + inline void + multiply_At_A_SparseDense(SparseMatA const& a, MatB& dst) + { + assert(dst.num_rows() == a.num_cols()); + assert(dst.num_cols() == a.num_cols()); + + typedef typename MatB::value_type Elem; + + makeZeroMatrix(dst); + + std::vector rows1(a.num_rows()), rows2(a.num_rows()); + std::vector vals1(a.num_rows()), vals2(a.num_rows()); + + for (unsigned int r = 0; r < dst.num_rows(); ++r) + { + int const nnz1 = a.getColumnNonzeroCount(r); + a.getSparseColumn(r, rows1, vals1); + + for (unsigned int c = 0; c <= r; ++c) + { + int const nnz2 = a.getColumnNonzeroCount(c); + a.getSparseColumn(c, rows2, vals2); + + Elem accum = 0; + + int i1 = 0, i2 = 0; + while (i1 < nnz1 && i2 < nnz2) + { + if (rows1[i1] > rows2[i2]) + ++i2; + else if (rows1[i1] < rows2[i2]) + ++i1; + else + { + accum += vals1[i1] * vals2[i2]; + ++i1; + ++i2; + } + } // end while + + dst[r][c] = accum; + dst[c][r] = accum; + } // end for (c) + } // end for (r) + } // end multiply_At_A_SparseDense() + + template + inline void + multiply_At_A_SparseSparse(SparseMatA const& a, CCS_Matrix& dst) + { + std::vector rows1(a.num_rows()), rows2(a.num_rows()); + std::vector vals1(a.num_rows()), vals2(a.num_rows()); + + vector > nzAtA; + vector valsAtA; + + for (unsigned int r = 0; r < a.num_cols(); ++r) + { + int const nnz1 = a.getColumnNonzeroCount(r); + a.getSparseColumn(r, rows1, vals1); + + for (unsigned int c = 0; c <= r; ++c) + { + int const nnz2 = a.getColumnNonzeroCount(c); + a.getSparseColumn(c, rows2, vals2); + + Elem accum = 0; + + int i1 = 0, i2 = 0; + while (i1 < nnz1 && i2 < nnz2) + { + if (rows1[i1] > rows2[i2]) + ++i2; + else if (rows1[i1] < rows2[i2]) + ++i1; + else + { + accum += vals1[i1] * vals2[i2]; + ++i1; + ++i2; + } + } // end while + + if (accum != 0) + { + if (c == r) + { + nzAtA.push_back(make_pair(r, r)); valsAtA.push_back(accum); + } + else + { + nzAtA.push_back(make_pair(r, c)); valsAtA.push_back(accum); + nzAtA.push_back(make_pair(c, r)); valsAtA.push_back(accum); + } + } // end if + } // end for (c) + } // end for (r) + dst.create(a.num_cols(), a.num_cols(), nzAtA, valsAtA); + } // multiply_At_A_SparseDense() + + template + inline void + multiply_At_B_SparseDense(SparseMatA const& a, SparseMatB const& b, MatC& dst) + { + assert(dst.num_rows() == a.num_cols()); + assert(dst.num_cols() == b.num_cols()); + + typedef typename MatC::value_type Elem; + + std::vector rows1(a.num_rows()), rows2(b.num_rows()); + std::vector vals1(a.num_rows()), vals2(b.num_rows()); + + for (unsigned int r = 0; r < dst.num_rows(); ++r) + { + int const nnz1 = a.getColumnNonzeroCount(r); + a.getSparseColumn(r, rows1, vals1); + + for (unsigned int c = 0; c < dst.num_cols(); ++c) + { + int const nnz2 = b.getColumnNonzeroCount(c); + b.getSparseColumn(c, rows2, vals2); + + Elem accum = 0; + + int i1 = 0, i2 = 0; + while (i1 < nnz1 && i2 < nnz2) + { + if (rows1[i1] > rows2[i2]) + ++i2; + else if (rows1[i1] < rows2[i2]) + ++i1; + else + { + accum += vals1[i1] * vals2[i2]; + ++i1; + ++i2; + } + } // end while + + dst[r][c] = accum; + } // end for (c) + } // end for (r) + } // end multiply_At_B_SparseDense() + + template + inline void + multiply_A_B(MatA const& a, MatB const& b, MatC& dst) + { + assert(a.num_cols() == b.num_rows()); + assert(dst.num_rows() == a.num_rows()); + assert(dst.num_cols() == b.num_cols()); + + typedef typename MatC::value_type Elem; + + Elem accum; + for (unsigned int r = 0; r < a.num_rows(); ++r) + for (unsigned int c = 0; c < b.num_cols(); ++c) + { + accum = 0; + for (unsigned int k = 0; k < a.num_cols(); ++k) accum += a[r][k] * b[k][c]; + dst[r][c] = accum; + } + } + + template + inline void + multiply_At_B(MatA const& a, MatB const& b, MatC& dst) + { + assert(a.num_rows() == b.num_rows()); + assert(dst.num_rows() == a.num_cols()); + assert(dst.num_cols() == b.num_cols()); + + typedef typename MatC::value_type Elem; + + Elem accum; + for (unsigned int r = 0; r < a.num_cols(); ++r) + for (unsigned int c = 0; c < b.num_cols(); ++c) + { + accum = 0; + for (unsigned int k = 0; k < a.num_rows(); ++k) accum += a[k][r] * b[k][c]; + dst[r][c] = accum; + } + } + + template + inline void + multiply_A_Bt(MatA const& a, MatB const& b, MatC& dst) + { + assert(a.num_cols() == b.num_cols()); + assert(dst.num_rows() == a.num_rows()); + assert(dst.num_cols() == b.num_rows()); + + typedef typename MatC::value_type Elem; + + Elem accum; + for (unsigned int r = 0; r < a.num_rows(); ++r) + for (unsigned int c = 0; c < b.num_rows(); ++c) + { + accum = 0; + for (unsigned int k = 0; k < a.num_cols(); ++k) accum += a[r][k] * b[c][k]; + dst[r][c] = accum; + } + } + +//====================================================================== + + template + inline void + vectorizeMatrix(Mat const& A, Vec& dst) + { + assert(A.num_rows() * A.num_cols() == dst.size()); + + int pos = 0; + for (int c = 0; c < A.num_cols(); ++c) + for (int r = 0; r < A.num_rows(); ++r, ++pos) + dst[pos] = A[r][c]; + } // end vectorizeMatrix() + + template + inline void + unvectorizeMatrix(Vec const& v, Mat& dst) + { + assert(dst.num_rows() * dst.num_cols() == v.size()); + + int pos = 0; + for (int c = 0; c < dst.num_cols(); ++c) + for (int r = 0; r < dst.num_rows(); ++r, ++pos) + dst[r][c] = v[pos]; + } // end vectorizeMatrix() + +//====================================================================== + + template + inline void + transposeMatrixIP(Mat& a) + { + assert(a.num_rows() == a.num_cols()); + + for (unsigned int r = 0; r < a.num_rows(); ++r) + for (unsigned int c = 0; c < r; ++c) + std::swap(a[r][c], a[c][r]); + } + + template + inline typename Mat::value_type + matrixDeterminant3x3(Mat const& A) + { + assert(A.num_rows() == 3); + assert(A.num_cols() == 3); + return (A[0][0]*A[1][1]*A[2][2] + A[0][1]*A[1][2]*A[2][0] + A[0][2]*A[1][0]*A[2][1] + -A[0][2]*A[1][1]*A[2][0] - A[0][1]*A[1][0]*A[2][2] - A[0][0]*A[1][2]*A[2][1]); + } + + template + inline double + matrixNormFrobenius(Mat const& a) + { + double accum(0.0); + for (unsigned int r = 0; r < a.num_rows(); ++r) + for (unsigned int c = 0; c < a.num_cols(); ++c) + accum += a[r][c]*a[r][c]; + + return sqrt(accum); + } + + template + inline double + sqrMatrixNormFrobenius(Mat const& a) + { + double accum(0.0); + for (unsigned int r = 0; r < a.num_rows(); ++r) + for (unsigned int c = 0; c < a.num_cols(); ++c) + accum += a[r][c]*a[r][c]; + + return accum; + } + + // Maximum absolute column sum + template + inline double + matrixNorm_L1(Mat const& a) + { + double res(0.0); + + for (unsigned int c = 0; c < a.num_cols(); ++c) + { + double sum = 0; + for (unsigned int r = 0; r < a.num_rows(); ++r) sum += fabs(a[r][c]); + res = std::max(res, sum); + } + return res; + } // end matrixNorm_L1() + + // Maximum absolute row sum + template + inline double + matrixNorm_Linf(Mat const& a) + { + double res(0.0); + + for (unsigned int r = 0; r < a.num_rows(); ++r) + { + double sum = 0; + for (unsigned int c = 0; c < a.num_cols(); ++c) sum += fabs(a[r][c]); + res = std::max(res, sum); + } + return res; + } // end matrixNorm_Linf + + template + inline double + sparseMatrixNormFrobenius(Mat const& a) + { + typedef typename Mat::value_type Elem; + + double res(0.0); + + std::vector rows(a.num_rows()); + std::vector vals(a.num_rows()); + + for (unsigned int c = 0; c < a.num_cols(); ++c) + { + int const nnz = a.getColumnNonzeroCount(c); + a.getSparseColumn(c, rows, vals); + + for (unsigned int i = 0; i < nnz; ++i) res += vals[i]*vals[i]; + } // end for (c) + + return sqrt(res); + } // end sparseMatrixNormFrobenius() + + // Maximum absolute column sum + template + inline double + sparseMatrixNorm_L1(Mat const& a) + { + typedef typename Mat::value_type Elem; + + double res(0.0); + + std::vector rows(a.num_rows()); + std::vector vals(a.num_rows()); + + for (unsigned int c = 0; c < a.num_cols(); ++c) + { + int const nnz = a.getColumnNonzeroCount(c); + a.getSparseColumn(c, rows, vals); + + double sum = 0; + for (unsigned int i = 0; i < nnz; ++i) sum += fabs(vals[i]); + res = std::max(res, sum); + } // end for (c) + + return res; + } // end sparseMatrixNorm_L1() + + // Maximum absolute row sum + template + inline double + sparseMatrixNorm_Linf(Mat const& a) + { + typedef typename Mat::value_type Elem; + + // Maintain all row sums simultaneously + std::vector rowSums(a.num_cols(), 0.0); + + std::vector rows(a.num_rows()); + std::vector vals(a.num_rows()); + + for (unsigned int c = 0; c < a.num_cols(); ++c) + { + int const nnz = a.getColumnNonzeroCount(c); + a.getSparseColumn(c, rows, vals); + + for (unsigned int i = 0; i < nnz; ++i) + rowSums[rows[i]] += fabs(vals[i]); + } // end for (c) + + return *std::max_element(rowSums.begin(), rowSums.end()); + } // end sparseMatrixNorm_Linf() + + //! Upper bound of the spectral norm + template + inline double + sparseMatrixNorm_L2_Bound(Mat const& a) + { + // |A|_2 <= |A|_F and |A|_2 <= sqrt(|A|_1 * |A|_inf) + double const normL1 = sparseMatrixNorm_L1(a); + double const normLinf = sparseMatrixNorm_Linf(a); + + return std::min(sparseMatrixNormFrobenius(a), sqrt(normL1 * normLinf)); + } + +//********************************************************************** + + //! Convert a matrix to upper triangular form, aka Gauss elimination. + template + inline void + convertToRowEchelonMatrix(Mat& A) + { + typedef typename Mat::value_type Field; + + int const n = A.num_rows(); + int const m = A.num_cols(); + + int i, j, k; + + int lead = 0; // Pivot column + + // Pass 1: Generate a upper right triangular matrix + for (i = 0; i < n && lead < m; ++i, ++lead) + { + int pivot_row = i; + Field pivot_elem(0); + + while (lead < m) + { + // Search for the largest pivot element in column lead + for (k = i; k < n; ++k) + { + Field a = std::abs(A[k][lead]); + if (a > pivot_elem) + { + pivot_elem = a; + pivot_row = k; + } + } // end for (k) + if (pivot_elem == Field(0)) + ++lead; + else + break; + } + if (lead >= m) break; + + if (i != pivot_row) + { + // Exchange row i and pivot_row + for (j = 0; j < m; ++j) + { + Field tmp = A[i][j]; + A[i][j] = A[pivot_row][j]; + A[pivot_row][j] = tmp; + } + } + + Field pivot = A[i][lead]; + Field rcpPivot = Field(1)/pivot; + + A[i][lead] = Field(1); + for (j = lead+1; j < m; ++j) A[i][j] = A[i][j] * rcpPivot; + + for (k = i+1; k < n; ++k) + { + Field q = A[k][lead]; + A[k][lead] = Field(0); + for (j = lead+1; j < m; ++j) A[k][j] = A[k][j] - q*A[i][j]; + } + } // end for (i) + } // end convertToRowEchelonMatrix() + + // Convert a row echelon matrix to a reduced one, i.e. Gauss-Jordan elimination. + template + inline void + convertToReducedRowEchelonMatrix(Mat& A) + { + typedef typename Mat::value_type Field; + + // Pass 2: Remove additional elements above the diagonal + int const n = A.num_rows(); + int const m = A.num_cols(); + + for (int i = n-1; i >= 0; --i) + { + int lead = i; + while (lead < m && A[i][lead] == Field(0)) ++lead; + + if (lead >= m) continue; + + for (int k = 0; k < i; ++k) + { + Field q = A[k][lead]; + for (int j = lead; j < m; ++j) + A[k][j] = A[k][j] - q*A[i][j]; + } // end for (k) + } // end for (i) + } // end convertToReducedRowEchelonMatrix() + +} // end namespace V3D + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_mathutilities.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_mathutilities.h new file mode 100644 index 0000000..0ba3b40 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_mathutilities.h @@ -0,0 +1,555 @@ +// -*- C++ -*- + +#ifndef V3D_MATH_UTILITIES_H +#define V3D_MATH_UTILITIES_H + +#include "Math/v3d_linear.h" +#include "Math/v3d_linear_tnt.h" + +#include + +#ifdef WIN32 +#include "win32config.h" +#endif + +namespace V3D +{ + + template + inline double + getRotationMagnitude(Mat const& R) + { + assert(R.num_rows() == 3); + assert(R.num_cols() == 3); + + double cos_theta = 0.5 * (R[0][0] + R[1][1] + R[2][2] - 1.0); + cos_theta = std::max(-1.0, std::min(1.0, cos_theta)); + return acos(cos_theta); + } + + template + inline Vector3d + getRotationAxis(Mat const& R) + { + assert(R.num_rows() == 3); + assert(R.num_cols() == 3); + + Vector3d axis(0.0, 0.0, 1.0); + + double const two_sin_theta = sqrt(sqr(R[1][0] - R[0][1]) + sqr(R[0][2] - R[2][0]) + sqr(R[2][1] - R[1][2])); + + if (fabs(two_sin_theta) > 1e-10) + { +#if 0 + axis[0] = (R[1][0] - R[0][1]) / two_sin_theta; + axis[1] = (R[0][2] - R[2][0]) / two_sin_theta; + axis[2] = (R[2][1] - R[1][2]) / two_sin_theta; +#else + // Patch by Bastien + axis[0] = (R[2][1] - R[1][2]) / two_sin_theta; + axis[1] = (R[0][2] - R[2][0]) / two_sin_theta; + axis[2] = (R[1][0] - R[0][1]) / two_sin_theta; +#endif + } + return axis; + } + +#if 0 + template + inline void + createQuaternionFromRotationMatrix(Mat const& R_, Vec4& q) + { + assert(R_.num_rows() == 3); + assert(R_.num_cols() == 3); + assert(q.size() == 4); + + Mat R; + makeTransposedMatrix(R_, R); + + const double tr = R[0][0] + R[1][1] + R[2][2]; + + if (tr >= 0.0) + { + double s = sqrt(tr + 1); + q[3] = s * 0.5; + s = 0.5 / s; + + q[0] = (R[1][2] - R[2][1]) * s; + q[1] = (R[2][0] - R[0][2]) * s; + q[2] = (R[0][1] - R[1][0]) * s; + } + else + { + int i = 0; + if (R[1][1] > R[0][0]) i = 1; + if (R[2][2] > R[i][i]) i = 2; + + if (tr > R[i][i]) + { + // Compute w first: + q[3] = sqrt(tr + 1)/2.0; + double const w4 = 4 * q[3]; + + // And compute other values: + q[0] = (R[1][2] - R[2][1]) / w4; + q[1] = (R[2][0] - R[0][2]) / w4; + q[2] = (R[0][1] - R[1][0]) / w4; + } + else + { + // Compute x, y, or z first: + int j = (i+1) % 3; + int k = (j+1) % 3; + + // Compute first value: + q[i] = sqrt(R[i][i] - R[j][j] - R[k][k] + 1) / 2; + + // And the others: + q[j] = (R[j][i] + R[i][j]) / (4*q[i]); + q[k] = (R[k][i] + R[i][k]) / (4*q[i]); + + q[3] = (R[j][k]-R[k][j])/(4*q[i]); + } // end if + } // end if (tr > 0) + } // end createQuaternionFromRotationMatrix() +#else + inline double _copysign(double x, double y) + { + if (y < 0) + return -fabs(x); + else + return fabs(x); + } + + template + inline void + createQuaternionFromRotationMatrix(Mat const& R, Vec4& q) + { + assert(R.num_rows() == 3); + assert(R.num_cols() == 3); + assert(q.size() == 4); + + double const m00 = R[0][0]; double const m01 = R[0][1]; double const m02 = R[0][2]; + double const m10 = R[1][0]; double const m11 = R[1][1]; double const m12 = R[1][2]; + double const m20 = R[2][0]; double const m21 = R[2][1]; double const m22 = R[2][2]; + + q[3] = sqrt(std::max(0.0, 1.0 + m00 + m11 + m22)) / 2; + q[0] = sqrt(std::max(0.0, 1.0 + m00 - m11 - m22)) / 2; + q[1] = sqrt(std::max(0.0, 1.0 - m00 + m11 - m22)) / 2; + q[2] = sqrt(std::max(0.0, 1.0 - m00 - m11 + m22)) / 2; + + q[0] = _copysign(q[0], m21 - m12); + q[1] = _copysign(q[1], m02 - m20); + q[2] = _copysign(q[2], m10 - m01); + } // end createQuaternionFromRotationMatrix() +#endif + + template + inline void + createRotationMatrixFromQuaternion(Vec4 const& q, Mat& R) + { + assert(R.num_rows() == 3); + assert(R.num_cols() == 3); + assert(q.size() == 4); + + double x = q[0]; + double y = q[1]; + double z = q[2]; + double w = q[3]; + + double const len = sqrt(x*x + y*y + z*z + w*w); + double const s = (len > 0.0) ? (1.0 / len) : 0.0; + + x *= s; y *= s; z *= s; w *= s; + + double const wx = 2*w*x; double const wy = 2*w*y; double const wz = 2*w*z; + double const xx = 2*x*x; double const xy = 2*x*y; double const xz = 2*x*z; + double const yy = 2*y*y; double const yz = 2*y*z; double const zz = 2*z*z; + R[0][0] = 1.0 - (yy+zz); R[0][1] = xy-wz; R[0][2] = xz+wy; + R[1][0] = xy+wz; R[1][1] = 1.0 - (xx+zz); R[1][2] = yz-wx; + R[2][0] = xz-wy; R[2][1] = yz+wx; R[2][2] = 1.0 - (xx+yy); + } // end createRotationMatrixFromQuaternion() + + template + inline void + createRotationMatrixRodrigues(Vec const& omega, Mat& R) + { + assert(omega.size() == 3); + assert(R.num_rows() == 3); + assert(R.num_cols() == 3); + + double const theta = norm_L2(omega); + makeIdentityMatrix(R); + if (fabs(theta) > 1e-6) + { + Matrix3x3d J, J2; + makeCrossProductMatrix(omega, J); + multiply_A_B(J, J, J2); + double const c1 = sin(theta)/theta; + double const c2 = (1-cos(theta))/(theta*theta); + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + R[i][j] += c1*J[i][j] + c2*J2[i][j]; + } + } // end createRotationMatrixRodrigues() + + template + inline void + createRodriguesParamFromRotationMatrix(Mat const& R, Vec& omega) + { + assert(omega.size() == 3); + assert(R.num_rows() == 3); + assert(R.num_cols() == 3); + + Vector4d q; + createQuaternionFromRotationMatrix(R, q); + omega[0] = q[0]; + omega[1] = q[1]; + omega[2] = q[2]; + normalizeVector(omega); + scaleVectorIP(2.0*acos(q[3]), omega); + } // end createRodriguesParamFromRotationMatrix() + + + template + inline void + createRotationMatrixFromModifiedRodrigues(Vec const& sigma, Mat& R) + { + assert(sigma.size() == 3); + assert(R.num_rows() == 3); + assert(R.num_cols() == 3); + + double const s2 = sqrNorm_L2(sigma); + + makeIdentityMatrix(R); + Matrix3x3d J, J2; + makeCrossProductMatrix(sigma, J); + multiply_A_B(J, J, J2); + Matrix3x3d tmp = 8.0*J2 - 4.0*(1.0 - s2)*J; + scaleMatrixIP(1/(1+s2)/(1+s2), tmp); + addMatrices(R, tmp, R); + } // end createRotationMatrixFromModifiedRodrigues() + + template + inline void + createModifiedRodriguesParamFromRotationMatrix(Mat const& R, Vec& sigma) + { + assert(sigma.size() == 3); + assert(R.num_rows() == 3); + assert(R.num_cols() == 3); + + Vector3d omega; + createRodriguesParamFromRotationMatrix(R, omega); + double w2 = sqrNorm_L2(omega); + double denom = 1.0 / (1 + sqrt(1.0 + w2)); + sigma[0] = omega[0] * denom; + sigma[1] = omega[1] * denom; + sigma[2] = omega[2] * denom; + } // end createModifiedRodriguesParamFromRotationMatrix() + + template + void + createQuaternionForVectorAlignment(Vec3 const& src_, Vec3 const& dst_, Vec4& q) + { + assert(src_.size() == 3); + assert(dst_.size() == 3); + assert(q.size() == 4); + + Vector3d src, dst, axis; + copyVector(src_, src); + copyVector(dst_, dst); + + normalizeVector(src); + normalizeVector(dst); + + double dot = innerProduct(src, dst); + makeCrossProductVector(src, dst, axis); + const double axislen2 = sqrNorm_L2(axis); + + if (axislen2 == 0.0) + { + // src and dst are parallel + // Check if they are pointing in the same direction. + if (dot > 0.0) + { + q[0] = q[1] = q[2] = 0.0; q[3] = 1.0; + } + // Ok, so they are parallel and pointing in the opposite direction + // of each other. + else + { + Vector3d t; + // Try crossing with x axis. + makeCrossProductVector(src, makeVector3(1.0, 0.0, 0.0), t); + // If not ok, cross with y axis. + if(sqrNorm_L2(t) == 0.0) makeCrossProductVector(src, makeVector3(0.0, 1.0, 0.0), t); + + normalizeVector(t); + q[0] = t[0]; q[1] = t[1]; q[2] = t[2]; q[3] = 0.0; + } // end if + } + else + { + normalizeVector(axis); + dot = std::max(-1.0, std::min(1.0, dot)); + // use half-angle formulae + // sin^2 t = ( 1 - cos (2t) ) / 2 + scaleVectorIP(sqrt((1.0 - dot) / 2), axis); + + q[0] = axis[0]; + q[1] = axis[1]; + q[2] = axis[2]; + + // cos^2 t = ( 1 + cos (2t) ) / 2 + // w part is cosine of half the rotation angle + q[3] = sqrt((1.0 + dot) / 2); + } // end if + } // end createQuaternionForVectorAlignment() + + inline void + enforceRotationMatrix(Matrix& R) + { + assert(R.num_rows() == 3); + assert(R.num_cols() == 3); + + SVD svd(R); + multiply_A_Bt(svd.getU(), svd.getV(), R); + } + + template + inline void + enforceRotationMatrix(Mat& R) + { + assert(R.num_rows() == 3); + assert(R.num_cols() == 3); + + Matrix RR(3, 3); + copyMatrix(R, RR); + enforceRotationMatrix(RR); + copyMatrix(RR, R); + } + + inline void + getEuclideanTransformation(vector const& left_, vector const& right_, + Matrix3x3d& R, Vector3d& T) + { + assert(left_.size() == right_.size()); + + size_t const n = left_.size(); + + // Determine centroids and shifted points + vector left(n); + vector right(n); + + Vector3d leftMean, rightMean; + makeZeroVector(leftMean); + makeZeroVector(rightMean); + + for (size_t i = 0; i < n; ++i) + { + leftMean = leftMean + left_[i]; + rightMean = rightMean + right_[i]; + } + scaleVectorIP(1.0 / n, leftMean); + scaleVectorIP(1.0 / n, rightMean); + + for (size_t i = 0; i < n; ++i) + { + left[i] = left_[i] - leftMean; + right[i] = right_[i] - rightMean; + } + + Matrix3x3d H, outer; + makeZeroMatrix(H); + + for (size_t i = 0; i < n; ++i) + { + makeOuterProductMatrix(right[i], left[i], outer); + addMatricesIP(outer, H); + } + + Matrix HH(3, 3); + copyMatrix(H, HH); + SVD svd(HH); + + Matrix const U = svd.getU(); + Matrix const& V = svd.getV(); + + multiply_A_Bt(U, V, R); + if (matrixDeterminant3x3(R) < 0) + { + Matrix3x3d V1; + copyMatrix(V, V1); + V1[0][2] = -V1[0][2]; + V1[1][2] = -V1[1][2]; + V1[2][2] = -V1[2][2]; + multiply_A_Bt(U, V1, R); + } + T = rightMean - R*(leftMean); + } // end getEuclideanTransformation() + + // The transformation from left to right is given by Y = scale * (R*X + T). + inline void + getSimilarityTransformation(vector const& left_, vector const& right_, + Matrix3x3d& R, Vector3d& T, double& scale) + { + assert(left_.size() == right_.size()); + + size_t const n = left_.size(); + + vector left(left_); + vector right(right_); + + Vector3d leftMean, rightMean; + makeZeroVector(leftMean); + makeZeroVector(rightMean); + + for (size_t i = 0; i < n; ++i) + { + leftMean = leftMean + left_[i]; + rightMean = rightMean + right_[i]; + } + scaleVectorIP(1.0 / n, leftMean); + scaleVectorIP(1.0 / n, rightMean); + + scale = 0.0; + for (size_t i = 0; i < n; ++i) + { + double d1 = distance_L2(left[i], leftMean); + double d2 = distance_L2(right[i], rightMean); + scale += d2/d1; + } + scale /= n; + if(isinf(scale)||isnan(scale)||scale==0.0)scale=1.0; + for (size_t i = 0; i < n; ++i) scaleVectorIP(scale, left[i]); + + getEuclideanTransformation(left, right, R, T); + scaleVectorIP(1.0 / scale, T); + } // end getSimilarityTransformation() + + inline void + makePseudoInverse(Matrix const& A, Matrix& Aplus, double tolerance = 1e-12) + { + int const M = A.num_rows(); + int const N = A.num_cols(); + + SVD svd(A); + + Matrix U = svd.getU(); + Matrix V = svd.getV(); + Vector const& S = svd.getSingularValues(); + + double eps = tolerance * S[0]; + for (int i = 0; i < N; ++i) + { + double const splus = (S[i] < eps) ? 0.0 : 1.0 / S[i]; + + for (int j = 0; j < N; ++j) V[j][i] *= splus; + } + + Aplus.newsize(N, M); + multiply_A_Bt(V, U, Aplus); + } // end makePseudoInverse() + + template + struct RootFindingParameters + { + RootFindingParameters() + : maxBracketingExponent(32), maxBisectionIterations(800), + coefficientTolerance(1e-12), rootTolerance(1e-20) + { } + + int maxBracketingExponent; //!< max power of 10 we wish to search to + int maxBisectionIterations; //!< max number of iterations in the bisection routine + Field coefficientTolerance; //!< a coefficient smaller than this is considered to be exactly zero + Field rootTolerance; //!< smallest relative error we want + }; // end struct RootFindingParameters + + template + Field evalPolynomial(int order, Field const * coeffs, Field z); + + int getRealRootsOfQuadraticPolynomial(double a, double b, double c, double roots[2]); + + int getRealRootsOfCubicPolynomial(double a, double b, double c, double d, double roots[3]); + + int getRealRootsOfQuarticPolynomial(double a, double b, double c, double d, double e, double roots[4]); + + //! \brief Compute all real roots of the given polynomial first by + //! bracketing them and then by applying the regula-falsi method. + /*! For some degenerate cases the root bracketing/counting will fail, the returned result + * is false is those cases. + */ + template + bool computeRealRootsOfPolynomial(int order, Field const * coeffs, std::vector& roots, + RootFindingParameters const& params = RootFindingParameters()); + + template inline T sqr(T x) { return x*x; } + +//********************************************************************** + + struct SimilarityTransform + { + double scale; + Matrix3x3d R; + Vector3d T; + + SimilarityTransform() + : scale(1.0) + { + makeIdentityMatrix(R); + makeZeroVector(T); + } + + SimilarityTransform(double scale_, Matrix3x3d const& R_, Vector3d const& T_) + : scale(scale_) + { + copyMatrix(R_, R); + copyVector(T_, T); + } + + operator Matrix4x4d() const + { + Matrix4x4d res; + copyMatrixSlice(R, 0, 0, 3, 3, res, 0, 0); + res[0][3] = T[0]; res[1][3] = T[1]; res[2][3] = T[2]; + scaleMatrixIP(scale, res); + res[3][0] = res[3][1] = res[3][2] = 0.0; res[3][3] = 1; + return res; + } + + Vector3d apply(Vector3d const& X) const + { + return scale * (R*X + T); + } + + SimilarityTransform inverse() const + { + return SimilarityTransform(1.0/scale, R.transposed(), -scale*(R.transposed()*T)); + } + template void serialize(Archive& ar) + { + V3D::SerializationScope scope(ar, "SimilarityTransform"); + ar & scale & T & R; + } + V3D_DEFINE_LOAD_SAVE(SimilarityTransform) + }; // end struct SimilarityTransform + + // Composition of transforms + inline SimilarityTransform operator*(SimilarityTransform const& T2, SimilarityTransform const& T1) + { + return SimilarityTransform(T2.scale*T1.scale, T2.R * T1.R, T2.R*T1.T + (1.0/T1.scale)*T2.T); + } + + inline void + displayTransform(SimilarityTransform const& xform) + { + using namespace std; + + cout << "scale = " << xform.scale << endl; + cout << "R = "; displayMatrix(xform.R); + cout << "T = "; displayVector(xform.T); + } + +} // namespace V3D + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_nonlinlsq.cpp b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_nonlinlsq.cpp new file mode 100644 index 0000000..79f13ca --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_nonlinlsq.cpp @@ -0,0 +1,736 @@ +#include "Math/v3d_nonlinlsq.h" + +#include + +#if defined(V3DLIB_ENABLE_SUITESPARSE) +# include "colamd.h" +# include "Math/v3d_ldl_private.h" + +using namespace std; +using namespace V3D; + +#define ONLY_UPPER_TRIANGULAR_HESSIAN 1 + +namespace V3D +{ + + void + NLSQ_CostFunction::fillAllJacobians(Vector const& weights, vector * >& Js) const + { + for (int i = 0; i < _usedParamTypes.size(); ++i) + { + MatrixArray& J = *Js[i]; + for (int k = 0; k < _nMeasurements; ++k) + { + int const paramIx = _correspondingParams[k][i]; + this->fillJacobian(i, paramIx, k, J[k]); + scaleMatrixIP(weights[k], J[k]); + } // end for (k) + } // end for (i) + } // end NLSQ_CostFunction::fillAllJacobians() + +//====================================================================== + + NLSQ_LM_Optimizer::NLSQ_LM_Optimizer(NLSQ_ParamDesc const& paramDesc, + std::vector const& costFunctions) + : LevenbergOptimizerCommon(), + _paramDesc(paramDesc), _costFunctions(costFunctions), + _hessianIndices(costFunctions.size()), _residuals(costFunctions.size()) + { + // First, compute the mappings between (paramType, id) pairs and global continuous ids. + _paramTypeStartID[0] = 0; + for (int paramType = 0; paramType < paramDesc.nParamTypes; ++paramType) + _paramTypeStartID[paramType+1] = _paramTypeStartID[paramType] + paramDesc.count[paramType]; + + _totalParamCount = _paramTypeStartID[paramDesc.nParamTypes]; + + _paramIdInverseMap.resize(_totalParamCount); + for (int paramType = 0; paramType < paramDesc.nParamTypes; ++paramType) + for (int ix = 0; ix < paramDesc.count[paramType]; ++ix) + { + int const id = this->getParamId(paramType, ix); + _paramIdInverseMap[id].first = paramType; + _paramIdInverseMap[id].second = ix; + } + + // This does the hard work of setting up the sparse data structures. + this->setupSparseJtJ(); + } // end NLSQ_LM_Optimizer() + + NLSQ_LM_Optimizer::~NLSQ_LM_Optimizer() + { + _hessian.deallocateMatrices(); + + for (int obj = 0; obj < _costFunctions.size(); ++obj) + { + delete _hessianIndices[obj]; + delete _residuals[obj]; + } + } // end NLSQ_LM_Optimizer::~NLSQ_LM_Optimizer() + + void + NLSQ_LM_Optimizer::setupSparseJtJ() + { + int const nObjs = _costFunctions.size(); + + // Note: for logical parameter ids i and j, J^T J has a non-zero block at (i,j) iff + // there exists at least one measurement depending on i and j. + + typedef map, int > NonzeroPosMap; + + Matrix nonzeroPosMaps(NLSQ_MAX_PARAM_TYPES, NLSQ_MAX_PARAM_TYPES); + int nNonzeroBlocks = 0; + + // Found out which non-zero blocks exist in the Hessian. + for (int obj = 0; obj < nObjs; ++obj) + { + NLSQ_CostFunction& costFun = *_costFunctions[obj]; + + int const nParamTypes = costFun._usedParamTypes.size(); + int const nMeasurements = costFun._nMeasurements; + + _hessianIndices[obj] = new MatrixArray(nMeasurements, nParamTypes, nParamTypes); + MatrixArray& index = *_hessianIndices[obj]; + + // Also allocate the residual object here + _residuals[obj] = new NLSQ_Residuals(costFun._usedParamTypes, nMeasurements, + costFun._measurementDimension, _paramDesc); + + for (int k = 0; k < nMeasurements; ++k) + { + for (int i1 = 0; i1 < nParamTypes; ++i1) + { + int const t1 = costFun._usedParamTypes[i1]; + int const ix1 = costFun._correspondingParams[k][i1]; + int const id1 = this->getParamId(t1, ix1); + + for (int i2 = 0; i2 < nParamTypes; ++i2) + { + int const t2 = costFun._usedParamTypes[i2]; + int const ix2 = costFun._correspondingParams[k][i2]; + int const id2 = this->getParamId(t2, ix2); + +#if defined(ONLY_UPPER_TRIANGULAR_HESSIAN) + if (id1 > id2) continue; // only store the upper diagonal blocks +#endif + + NonzeroPosMap& nzPosMap = nonzeroPosMaps[t1][t2]; + NonzeroPosMap::const_iterator p = nzPosMap.find(make_pair(ix1, ix2)); + + if (p == nzPosMap.end()) + { + // We have a new non-zero block that needs to be stored in the Hessian. + int const curPos = nzPosMap.size(); + index[k][i1][i2] = curPos; + nzPosMap.insert(make_pair(make_pair(ix1, ix2), curPos)); + _hessian.nonzeroPairs[t1][t2].push_back(make_pair(ix1, ix2)); + ++nNonzeroBlocks; + } + else + { + index[k][i1][i2] = p->second; + } // end if + } // end for (i2) + } // end for (i1) + } // end for (k) + } // end for (obj) + + _hessian.allocateMatrices(_paramDesc); + if (0) { + cout << "nNonzeroBlocks = " << nNonzeroBlocks << endl; + int const nParamTypes = _paramDesc.nParamTypes; + for (int t1 = 0; t1 < nParamTypes; ++t1) + for (int t2 = 0; t2 < nParamTypes; ++t2) + { + cout << "Hs[" << t1 << "][" << t2 << "] = " << _hessian.Hs[t1][t2] << endl; + if (_hessian.Hs[t1][t2]) + cout << "Hs.size() = " << _hessian.Hs[t1][t2]->count() << endl; + cout << "_hessian.nonzeroPairs[t1][t2].size() = " << _hessian.nonzeroPairs[t1][t2].size() << endl; + vector > const& pairs = _hessian.nonzeroPairs[t1][t2]; + //for (int k = 0; k < pairs.size(); ++k) cout << " " << pairs[k].first << " <-> " << pairs[k].second << endl; + } + } + + // At thos point we know the (block) nonzeros of J^T J and we have allocated the needed memory. + // Now for the column reordering + + // For the column reordering we treat the columns belonging to one set + // of parameters as one (logical) column. + + // Determine non-zeros of JtJ (we forget about the non-zero diagonal for now) + // Also ignore the upper triangular part of JtJ + // Only consider nonzeros of Ai^t * Bj induced by the measurements. + vector > nz_blockJtJ; + nz_blockJtJ.reserve(nNonzeroBlocks); + for (int t1 = 0; t1 < _paramDesc.nParamTypes; ++t1) + for (int t2 = 0; t2 < _paramDesc.nParamTypes; ++t2) + { + vector > const& nz = _hessian.nonzeroPairs[t1][t2]; + for (int k = 0; k < nz.size(); ++k) + { + int const ix1 = nz[k].first; + int const ix2 = nz[k].second; + int const id1 = this->getParamId(t1, ix1); + int const id2 = this->getParamId(t2, ix2); + // Note: id1 < id2 (lower triangular part), but for symamd one + // needs only the strictly lower triangular part. + if (id1 != id2) nz_blockJtJ.push_back(make_pair(id2, id1)); + } // end for (t1) + } // end for (t2) + + int const nnzBlock = nz_blockJtJ.size(); + + vector permBlockJtJ(_totalParamCount + 1); + + if (nnzBlock > 0) + { + CCS_Matrix blockJtJ(_totalParamCount, _totalParamCount, nz_blockJtJ); + + int * colStarts = (int *)blockJtJ.getColumnStarts(); + int * rowIdxs = (int *)blockJtJ.getRowIndices(); + + int stats[COLAMD_STATS]; + symamd(_totalParamCount, rowIdxs, colStarts, &permBlockJtJ[0], (double *) NULL, stats, &calloc, &free); + if (optimizerVerbosenessLevel >= 2) symamd_report(stats); + } + else + for (size_t k = 0; k < permBlockJtJ.size(); ++k) permBlockJtJ[k] = k; + + //cout << "permBlockJtJ = "; displayVector(permBlockJtJ); + + // The number of columns/rows of the scalar JtJ + int JtJ_size = 0; + for (int t = 0; t < _paramDesc.nParamTypes; ++t) + { + _paramTypeRowStart[t] = JtJ_size; + JtJ_size += _paramDesc.dimension[t]*_paramDesc.count[t]; + } + _paramTypeRowStart[_paramDesc.nParamTypes] = JtJ_size; + + // From the determined symbolic permutation with logical (block) variables, determine the actual ordering for all the scalar unknowns + _perm_JtJ.resize(JtJ_size + 1); + + int curDstCol = 0; + for (size_t k = 0; k < permBlockJtJ.size()-1; ++k) + { + int const srcCol = permBlockJtJ[k]; + + int const paramType = _paramIdInverseMap[srcCol].first; + int const paramIx = _paramIdInverseMap[srcCol].second; + int const dimension = _paramDesc.dimension[paramType]; + + int const colStart = _paramTypeRowStart[paramType] + paramIx*dimension; + + for (int n = 0; n < dimension; ++n) + _perm_JtJ[curDstCol + n] = colStart + n; + + curDstCol += dimension; + } // end for (k) + _perm_JtJ.back() = _perm_JtJ.size() - 1; + //cout << "_perm_JtJ = "; displayVector(_perm_JtJ); + + // Finally, compute the inverse of the full permutation. + _invPerm_JtJ.resize(_perm_JtJ.size()); + for (size_t k = 0; k < _perm_JtJ.size(); ++k) _invPerm_JtJ[_perm_JtJ[k]] = k; + //cout << "_invPerm_JtJ = "; displayVector(_invPerm_JtJ); + + // Now determine all non-zeros of JtJ (the upper triangular part) + vector > nz_JtJ; + for (int t1 = 0; t1 < _paramDesc.nParamTypes; ++t1) + for (int t2 = 0; t2 < _paramDesc.nParamTypes; ++t2) + { + if (!_hessian.Hs[t1][t2]) continue; + + vector > const& nz = _hessian.nonzeroPairs[t1][t2]; + for (int k = 0; k < nz.size(); ++k) + { + int const ix1 = nz[k].first; + int const ix2 = nz[k].second; + + int const dim1 = _paramDesc.dimension[t1]; + int const dim2 = _paramDesc.dimension[t2]; + + int const r0 = _paramTypeRowStart[t1] + ix1 * dim1; + int const c0 = _paramTypeRowStart[t2] + ix2 * dim2; + + for (int r = 0; r < dim1; ++r) + for (int c = 0; c < dim2; ++c) + { +#if defined(ONLY_UPPER_TRIANGULAR_HESSIAN) + if (r0 + r <= c0 + c) nz_JtJ.push_back(make_pair(r0 + r, c0 + c)); +#else + nz_JtJ.push_back(make_pair(r0 + r, c0 + c)); +#endif + } + } // end for (t1) + } // end for (t2) + //cout << "nz_JtJ.size() = " << nz_JtJ.size() << endl; + + // Reorder columns + for (size_t k = 0; k < nz_JtJ.size(); ++k) + { + int const i = nz_JtJ[k].first; + int const j = nz_JtJ[k].second; + + int pi = _invPerm_JtJ[i]; + int pj = _invPerm_JtJ[j]; +#if defined(ONLY_UPPER_TRIANGULAR_HESSIAN) + // Swap values if in lower triangular part + if (pi > pj) std::swap(pi, pj); +#endif + nz_JtJ[k].first = pi; + nz_JtJ[k].second = pj; + //cout << "(" << i << ", " << j << ") -> (" << pi << ", " << pj << ")" << endl; + } + + _JtJ.create(JtJ_size, JtJ_size, nz_JtJ); + + vector workFlags(JtJ_size); + + _JtJ_Lp.resize(JtJ_size+1); + _JtJ_Parent.resize(JtJ_size); + _JtJ_Lnz.resize(JtJ_size); + + LDL_symbolic(JtJ_size, (int *)_JtJ.getColumnStarts(), (int *)_JtJ.getRowIndices(), + &_JtJ_Lp[0], &_JtJ_Parent[0], &_JtJ_Lnz[0], &workFlags[0]); + + if (optimizerVerbosenessLevel >= 1) + cout << "NLSQ_LM_Optimizer: Nonzeros in LDL decomposition: " << _JtJ_Lp[JtJ_size] << endl; + } // end NLSQ_LM_Optimizer::setupSparseJtJ() + + void + NLSQ_LM_Optimizer::fillJtJ() + { + // The following has to replicate the procedure as in serializeNonZerosJtJ() + int serial = 0; + + double * values = _JtJ.getValues(); + int const * destIdxs = _JtJ.getDestIndices(); + + for (int t1 = 0; t1 < _paramDesc.nParamTypes; ++t1) + for (int t2 = 0; t2 < _paramDesc.nParamTypes; ++t2) + { + if (!_hessian.Hs[t1][t2]) continue; + + vector > const& nz = _hessian.nonzeroPairs[t1][t2]; + MatrixArray const& Hs = *_hessian.Hs[t1][t2]; + + for (int k = 0; k < nz.size(); ++k) + { + int const ix1 = nz[k].first; + int const ix2 = nz[k].second; + + int const dim1 = _paramDesc.dimension[t1]; + int const dim2 = _paramDesc.dimension[t2]; + + int const r0 = _paramTypeRowStart[t1] + ix1 * dim1; + int const c0 = _paramTypeRowStart[t2] + ix2 * dim2; + + for (int r = 0; r < dim1; ++r) + for (int c = 0; c < dim2; ++c) + { +#if defined(ONLY_UPPER_TRIANGULAR_HESSIAN) + if (r0 + r <= c0 + c) + { + values[destIdxs[serial]] = Hs[k][r][c]; + ++serial; + } +#else + values[destIdxs[serial]] = Hs[k][r][c]; + ++serial; +#endif + } // end for (c) + } // end for (t1) + } // end for (t2) + + //cout << "serial = " << serial << endl; + //displaySparseMatrix(_JtJ); + } // end NLSQ_LM_Optimizer::fillJtJ() + + void + NLSQ_LM_Optimizer::fillJacobians() + { + int const nObjs = _costFunctions.size(); + for (int obj = 0; obj < nObjs; ++obj) + { + NLSQ_CostFunction& costFun = *_costFunctions[obj]; + NLSQ_Residuals& residuals = *_residuals[obj]; + costFun.initializeJacobian(); + costFun.fillAllJacobians(residuals._weights, residuals._Js); + } // end for (obj) + } // end NLSQ_LM_Optimizer::fillJacobians() + + void + NLSQ_LM_Optimizer::fillHessian() + { + // Set Hessian to zero + _hessian.setZero(); + + int const nObjs = _costFunctions.size(); + for (int obj = 0; obj < nObjs; ++obj) + { + NLSQ_CostFunction& costFun = *_costFunctions[obj]; + + int const nParamTypes = costFun._usedParamTypes.size(); + int const nMeasurements = costFun._nMeasurements; + + MatrixArray const& index = *_hessianIndices[obj]; + NLSQ_Residuals const& residuals = *_residuals[obj]; + + vector const& usedParamTypes = costFun._usedParamTypes; + + for (int i1 = 0; i1 < usedParamTypes.size(); ++i1) + { + int const t1 = usedParamTypes[i1]; + int const dim1 = _paramDesc.dimension[t1]; + + MatrixArray const& Js1 = *residuals._Js[i1]; + + for (int i2 = 0; i2 < usedParamTypes.size(); ++i2) + { + int const t2 = usedParamTypes[i2]; + int const dim2 = _paramDesc.dimension[t2]; + + MatrixArray const& Js2 = *residuals._Js[i2]; + + Matrix J1tJ2(dim1, dim2); + + // Ignore non-existent Hessians lying in the lower triangular part. + if (!_hessian.Hs[t1][t2]) continue; + + MatrixArray& Hs = *_hessian.Hs[t1][t2]; + + for (int k = 0; k < nMeasurements; ++k) + { + int const ix1 = costFun._correspondingParams[k][i1]; + int const id1 = this->getParamId(t1, ix1); + int const ix2 = costFun._correspondingParams[k][i2]; + int const id2 = this->getParamId(t2, ix2); + +#if defined(ONLY_UPPER_TRIANGULAR_HESSIAN) + if (id1 > id2) continue; // only store the upper diagonal blocks +#endif + + int const n = index[k][i1][i2]; + // cout << "obj = " << obj << " i1 = " << i1 << " i2 = " << i2 << " t1 = " << t1 << " t2 = " << t2; + // cout << " n = " << n << " Hs.count = " << Hs.count() << endl; + assert(n < Hs.count()); + + multiply_At_B(Js1[k], Js2[k], J1tJ2); + addMatricesIP(J1tJ2, Hs[n]); + } // end for (k) + } // end for (i2) + } // end for (i1) + } // end for (obj) + } // end NLSQ_LM_Optimizer::fillHessian() + + void + NLSQ_LM_Optimizer::evalJt_e(Vector& Jt_e) + { + makeZeroVector(Jt_e); + + int const nObjs = _costFunctions.size(); + for (int obj = 0; obj < nObjs; ++obj) + { + NLSQ_CostFunction& costFun = *_costFunctions[obj]; + NLSQ_Residuals& residuals = *_residuals[obj]; + + int const nParamTypes = costFun._usedParamTypes.size(); + int const nMeasurements = costFun._nMeasurements; + + for (int i = 0; i < nParamTypes; ++i) + { + int const paramType = costFun._usedParamTypes[i]; + int const paramDim = _paramDesc.dimension[paramType]; + + MatrixArray const& J = *residuals._Js[i]; + + Vector Jkt_e(paramDim); + + for (int k = 0; k < nMeasurements; ++k) + { + int const id = costFun._correspondingParams[k][i]; + int const dstRow = _paramTypeRowStart[paramType] + id*paramDim;; + + multiply_At_v(J[k], residuals._residuals[k], Jkt_e); + for (int l = 0; l < paramDim; ++l) Jt_e[dstRow + l] += Jkt_e[l]; + } // end for (k) + } // end for (i) + } // end for (obj) + } // end NLSQ_LM_Optimizer::evalJt_e() + + void + NLSQ_LM_Optimizer::minimize() + { + status = LEVENBERG_OPTIMIZER_TIMEOUT; + bool computeDerivatives = true; + + if (_totalParamCount == 0) + { + // No degrees of freedom, nothing to optimize. + status = LEVENBERG_OPTIMIZER_CONVERGED; + return; + } + + int const totalParamDimension = _JtJ.num_cols(); + + Vector Jt_e(totalParamDimension); + Vector delta(totalParamDimension); + Vector deltaPerm(totalParamDimension); + + double err = 0.0; + + int const nObjs = _costFunctions.size(); + + for (currentIteration = 0; currentIteration < maxIterations; ++currentIteration) + { + if (optimizerVerbosenessLevel >= 2) + cout << "ExtSparseLevenbergOptimizer: currentIteration: " << currentIteration << endl; + if (computeDerivatives) + { + vector errors(nObjs); + err = 0.0; + for (int obj = 0; obj < nObjs; ++obj) + { + NLSQ_CostFunction& costFun = *_costFunctions[obj]; + NLSQ_Residuals& residuals = *_residuals[obj]; + + costFun.evalAllResiduals(residuals._residuals); + costFun.fillAllWeights(residuals._residuals, residuals._weights); + + for (int k = 0; k < costFun._nMeasurements; ++k) + scaleVectorIP(-residuals._weights[k], residuals._residuals[k]); + + errors[obj] = squaredResidual(residuals._residuals); + err += errors[obj]; + } // end for (obj) + + if (optimizerVerbosenessLevel >= 1) cout << "NLSQ_LM_Optimizer: |residual|^2 = " << err << endl; + if (optimizerVerbosenessLevel >= 2) cout << "NLSQ_LM_Optimizer: residuals (itemized) = "; displayVector(errors); + if (optimizerVerbosenessLevel >= 2) cout << "NLSQ_LM_Optimizer: lambda = " << lambda << endl; + + this->fillJacobians(); + this->evalJt_e(Jt_e); + + if (this->applyGradientStoppingCriteria(norm_Linf(Jt_e))) + { + status = LEVENBERG_OPTIMIZER_CONVERGED; + goto end; + } + + this->fillHessian(); + + if (currentIteration == 0) + { + // Initialize lambda as tau*max(JtJ[i][i]) + double maxEl = -1e30; + if (optimizerVerbosenessLevel < 2) + { + for (int paramType = 0; paramType < _paramDesc.nParamTypes; ++paramType) + { + MatrixArray const& Hs = *_hessian.Hs[paramType][paramType]; + int const dim = Hs.num_cols(); + int const count = Hs.count(); + for (int n = 0; n < count; ++n) + for (int l = 0; l < dim; ++l) maxEl = std::max(maxEl, Hs[n][l][l]); + } // end for (paramType) + } + else + { + int maxParamType = -1; + int maxElIndex = -1; + int maxElRow = -1; + + for (int paramType = 0; paramType < _paramDesc.nParamTypes; ++paramType) + { + MatrixArray const& Hs = *_hessian.Hs[paramType][paramType]; + int const dim = Hs.num_cols(); + int const count = Hs.count(); + for (int n = 0; n < count; ++n) + for (int l = 0; l < dim; ++l) + { + double const el = Hs[n][l][l]; + if (el > maxEl) + { + maxEl = el; + maxParamType = paramType; + maxElIndex = n; + maxElRow = l; + } // end if + } // end for (l) + } // end for (paramType) + + cout << "NLSQ_LM_Optimizer: initial lambda = " << tau*maxEl << "; max diagonal element found at " + << maxParamType << "[" << maxElIndex << "][" << maxElRow << "]" << endl; + } // end if + + lambda = tau * maxEl; + } // end if (currentIteration == 0) + } // end if (computeDerivatives) + + // Augment the diagonals + for (int paramType = 0; paramType < _paramDesc.nParamTypes; ++paramType) + { + MatrixArray& Hs = *_hessian.Hs[paramType][paramType]; + vector > const& nzPairs = _hessian.nonzeroPairs[paramType][paramType]; + int const dim = Hs.num_cols(); + int const count = Hs.count(); + + // Only augment those with the same parameter id + for (int n = 0; n < count; ++n) + { + if (nzPairs[n].first != nzPairs[n].second) continue; + for (int l = 0; l < dim; ++l) + Hs[n][l][l] += lambda; + } + } // end for (paramType) + + this->fillJtJ(); + + bool success = true; + double rho = 0.0; + { + int const nCols = _JtJ_Parent.size(); + //int const nnz = _JtJ.getNonzeroCount(); + int const lnz = _JtJ_Lp.back(); + + vector Li(lnz); + vector Lx(lnz); + vector D(nCols), Y(nCols); + vector workPattern(nCols), workFlag(nCols); + + int * colStarts = (int *)_JtJ.getColumnStarts(); + int * rowIdxs = (int *)_JtJ.getRowIndices(); + double * values = _JtJ.getValues(); + + int const d = LDL_numeric(nCols, colStarts, rowIdxs, values, + &_JtJ_Lp[0], &_JtJ_Parent[0], &_JtJ_Lnz[0], + &Li[0], &Lx[0], &D[0], + &Y[0], &workPattern[0], &workFlag[0]); + + if (d == nCols) + { + LDL_perm(nCols, &deltaPerm[0], &Jt_e[0], &_perm_JtJ[0]); + LDL_lsolve(nCols, &deltaPerm[0], &_JtJ_Lp[0], &Li[0], &Lx[0]); + LDL_dsolve(nCols, &deltaPerm[0], &D[0]); + LDL_ltsolve(nCols, &deltaPerm[0], &_JtJ_Lp[0], &Li[0], &Lx[0]); + LDL_permt(nCols, &delta[0], &deltaPerm[0], &_perm_JtJ[0]); + } + else + { + if (optimizerVerbosenessLevel >= 2) + cout << "NLSQ_LM_Optimizer: LDL decomposition failed with d = " << d << ". Increasing lambda." << endl; + success = false; + } + } + + double deltaError = 0; + + if (success) + { + double const deltaSqrLength = sqrNorm_L2(delta); + double const paramLength = this->getParameterLength(); + + if (optimizerVerbosenessLevel >= 2) + cout << "NLSQ_LM_Optimizer: ||delta|| = " << sqrt(deltaSqrLength) << " ||paramLength|| = " << paramLength << endl; + + if (this->applyUpdateStoppingCriteria(paramLength, sqrt(deltaSqrLength))) + { + status = LEVENBERG_OPTIMIZER_SMALL_UPDATE; + goto end; + } + + saveAllParameters(); + + for (int paramType = 0; paramType < _paramDesc.nParamTypes; ++paramType) + { + int const paramDim = _paramDesc.dimension[paramType]; + int const count = _paramDesc.count[paramType]; + int const rowStart = _paramTypeRowStart[paramType]; + + VectorArrayAdapter deltaParam(count, paramDim, &delta[0] + rowStart); + this->updateParameters(paramType, deltaParam); + } // end for (paramType) + + double newErr = 0.0; + vector errors(nObjs); + for (int obj = 0; obj < nObjs; ++obj) + { + NLSQ_CostFunction& costFun = *_costFunctions[obj]; + NLSQ_Residuals& residuals = *_residuals[obj]; + + costFun.evalAllResiduals(residuals._residuals); + costFun.fillAllWeights(residuals._residuals, residuals._weights); + for (int k = 0; k < costFun._nMeasurements; ++k) + scaleVectorIP(residuals._weights[k], residuals._residuals[k]); + errors[obj] = squaredResidual(residuals._residuals); + newErr += errors[obj]; + } // end for (obj) + + deltaError = newErr - err; + + rho = err - newErr; + if (optimizerVerbosenessLevel >= 2) + { + cout << "NLSQ_LM_Optimizer: |new residual|^2 = " << newErr << " rho = " << rho << endl; + cout << "NLSQ_LM_Optimizer: new residuals (itemized) = "; displayVector(errors); + } + + double const denom1 = lambda * deltaSqrLength; + double const denom2 = innerProduct(delta, Jt_e); + rho = rho / (denom1 + denom2); + if (optimizerVerbosenessLevel >= 2) + cout << "NLSQ_LM_Optimizer: rho = " << rho + << " denom1 = " << denom1 << " denom2 = " << denom2 << endl; + } // end if (success) + + if (success && deltaError < 0) + { + if (this->applyImprovementStoppingCriteria(deltaError, err)) + { + if (optimizerVerbosenessLevel >= 2) + cout << "NLSQ_LM_Optimizer: too small improvement in cost function, exiting." << endl; + goto end; + } + } + + if (success && rho > 0) + { + if (optimizerVerbosenessLevel >= 2) + cout << "NLSQ_LM_Optimizer: Improved solution - decreasing lambda." << endl; + // Improvement in the new solution + decreaseLambda(rho); + if (optimizerVerbosenessLevel >= 2) cout << "NLSQ_LM_Optimizer: new lambda = " << lambda << endl; + computeDerivatives = true; + } + else + { + if (optimizerVerbosenessLevel >= 2) + cout << "NLSQ_LM_Optimizer: Inferior solution - increasing lambda." << endl; + restoreAllParameters(); + + // Undo the augmentation of the diagonals + for (int paramType = 0; paramType < _paramDesc.nParamTypes; ++paramType) + { + MatrixArray& Hs = *_hessian.Hs[paramType][paramType]; + int const dim = Hs.num_cols(); + int const count = Hs.count(); + for (int n = 0; n < count; ++n) + for (int l = 0; l < dim; ++l) + Hs[n][l][l] -= lambda; + } // end for (paramType) + + increaseLambda(); + if (optimizerVerbosenessLevel >= 2) cout << "NLSQ_LM_Optimizer: new lambda = " << lambda << endl; + computeDerivatives = false; + } // end if + } // end for (currentIteration) + + end:; + if (optimizerVerbosenessLevel >= 2) + cout << "Leaving NLSQ_LM_Optimizer::minimize()." << endl; + } // end NLSQ_LM_Optimizer::minimize() + +} // end namespace V3D + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_nonlinlsq.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_nonlinlsq.h new file mode 100644 index 0000000..3ec737f --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_nonlinlsq.h @@ -0,0 +1,197 @@ +// -*- C++ -*- +#ifndef V3D_NONLINEAR_LSQR_H +#define V3D_NONLINEAR_LSQR_H + +#include "Math/v3d_linear.h" +#include "Math/v3d_linear_tnt.h" +#include "Math/v3d_mathutilities.h" +#include "Math/v3d_optimization.h" + +#include +#include + +namespace V3D +{ + +#if defined(V3DLIB_ENABLE_SUITESPARSE) + +#define NLSQ_MAX_PARAM_TYPES 32 + + struct NLSQ_ParamDesc + { + int nParamTypes; //!< How many different kinds of parameters exist (2 for std. BA, cameras and 3D points) + int dimension[NLSQ_MAX_PARAM_TYPES]; //!< What is the dimension of each parameter kind + int count[NLSQ_MAX_PARAM_TYPES]; //!< How many unknowns are there per parameter type + }; // end struct NLSQ_ParamDesc + + //! This structure holds the residuals, weights and Jacobian for a particular cost function. + struct NLSQ_Residuals + { + NLSQ_Residuals(vector const& usedParamTypes, int const nMeasurements, + int const measurementDimension, NLSQ_ParamDesc const& paramDesc) + : _residuals(nMeasurements, measurementDimension), + _weights(nMeasurements), + _Js(usedParamTypes.size()) + { + for (int k = 0; k < usedParamTypes.size(); ++k) + { + int const paramType = usedParamTypes[k]; + int const paramDimension = paramDesc.dimension[paramType]; + _Js[k] = new MatrixArray(nMeasurements, measurementDimension, paramDimension); + } // end for (k) + } // end NLSQ_Residuals() + + ~NLSQ_Residuals() + { + for (int i = 0; i < _Js.size(); ++i) delete _Js[i]; + } + + VectorArray _residuals; + Vector _weights; + + vector * > _Js; + }; // end struct NLSQ_Residuals + + struct NLSQ_LM_BlockHessian + { + NLSQ_LM_BlockHessian() + { + for (int t1 = 0; t1 < NLSQ_MAX_PARAM_TYPES; ++t1) + for (int t2 = 0; t2 < NLSQ_MAX_PARAM_TYPES; ++t2) + Hs[t1][t2] = 0; + } + + void allocateMatrices(NLSQ_ParamDesc const& paramDesc) + { + int const nParamTypes = paramDesc.nParamTypes; + + for (int t1 = 0; t1 < nParamTypes; ++t1) + for (int t2 = 0; t2 < nParamTypes; ++t2) + { + int const nz = nonzeroPairs[t1][t2].size(); + if (nz > 0) + { + int const rows = paramDesc.dimension[t1]; + int const cols = paramDesc.dimension[t2]; + Hs[t1][t2] = new MatrixArray(nz, rows, cols); + } + else + Hs[t1][t2] = 0; + } // end for (t2) + } // end allocateMatrices() + + void deallocateMatrices() + { + for (int t1 = 0; t1 < NLSQ_MAX_PARAM_TYPES; ++t1) + for (int t2 = 0; t2 < NLSQ_MAX_PARAM_TYPES; ++t2) + if (Hs[t1][t2]) delete Hs[t1][t2]; + } + + void setZero() + { + for (int t1 = 0; t1 < NLSQ_MAX_PARAM_TYPES; ++t1) + for (int t2 = 0; t2 < NLSQ_MAX_PARAM_TYPES; ++t2) + if (Hs[t1][t2]) + { + MatrixArray& H = *Hs[t1][t2]; + for (int n = 0; n < H.count(); ++n) makeZeroMatrix(H[n]); + } + } // end setZero() + + vector > nonzeroPairs[NLSQ_MAX_PARAM_TYPES][NLSQ_MAX_PARAM_TYPES]; + MatrixArray * Hs[NLSQ_MAX_PARAM_TYPES][NLSQ_MAX_PARAM_TYPES]; + }; // end struct NLSQ_LM_BlockHessian + + struct NLSQ_CostFunction + { + NLSQ_CostFunction(std::vector const& usedParamTypes, + Matrix const& correspondingParams, + int const measurementDimension) + : _usedParamTypes(usedParamTypes), + _nMeasurements(correspondingParams.num_rows()), + _measurementDimension(measurementDimension), + _correspondingParams(correspondingParams) + { + assert(usedParamTypes.size() == correspondingParams.num_cols()); + } + + virtual double getWeight(Vector const& residual) const { return 1.0; } + virtual void evalResidual(int const k, Vector& residual) const = 0; + + virtual void initializeJacobian() { } + virtual void fillJacobian(int const whichParam, int const paramIx, int const k, Matrix& J) const = 0; + + protected: + void evalAllResiduals(VectorArray& residuals) const + { + for (int k = 0; k < _nMeasurements; ++k) this->evalResidual(k, residuals[k]); + } // end evalAllResiduals() + + void fillAllWeights(VectorArray const& residuals, Vector& w) const + { + for (int k = 0; k < _nMeasurements; ++k) w[k] = this->getWeight(residuals[k]); + } + + void fillAllJacobians(Vector const& weights, vector * >& Js) const; + + std::vector const& _usedParamTypes; + int const _nMeasurements; + int const _measurementDimension; + Matrix const& _correspondingParams; + + friend struct NLSQ_LM_Optimizer; + }; // end struct NLSQ_CostFunction + + struct NLSQ_LM_Optimizer : public LevenbergOptimizerCommon + { + NLSQ_LM_Optimizer(NLSQ_ParamDesc const& paramDesc, + std::vector const& costFunctions); + + ~NLSQ_LM_Optimizer(); + + void minimize(); + + virtual double getParameterLength() const = 0; + + virtual void updateParameters(int const paramType, VectorArrayAdapter const& delta) = 0; + virtual void saveAllParameters() = 0; + virtual void restoreAllParameters() = 0; + + CCS_Matrix const& JtJ() const { return _JtJ; } + + protected: + void setupSparseJtJ(); + void fillJtJ(); + void fillJacobians(); + void fillHessian(); + void evalJt_e(Vector& Jt_e); + + //! Map (paramType,ix) pairs to a continuous range of global ids. + int getParamId(int const paramType, int const paramIx) const { return _paramTypeStartID[paramType] + paramIx; } + + NLSQ_ParamDesc const& _paramDesc; + std::vector const& _costFunctions; + + //! Mapping of parameter types to their first id in the continuous range. + int _paramTypeStartID[NLSQ_MAX_PARAM_TYPES + 1]; + int _totalParamCount; + vector > _paramIdInverseMap; //!< the reverse mapping from global ids to (paramType,id) pairs. + + vector _residuals; // one per cost function + //!< _hessianIndices establishes the link between the local hessian in a cost function to the global non-zero block. + vector *> _hessianIndices; // one per cost function + NLSQ_LM_BlockHessian _hessian; + + // Used for sparse Cholesky + std::vector _JtJ_Lp, _JtJ_Parent, _JtJ_Lnz; + std::vector _perm_JtJ, _invPerm_JtJ; + + int _paramTypeRowStart[NLSQ_MAX_PARAM_TYPES + 1]; + CCS_Matrix _JtJ; + }; // end struct NLSQ_LM_Optimizer + +#endif // defined(V3DLIB_ENABLE_SUITESPARSE) + +} // end namespace V3D + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_optimization.cpp b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_optimization.cpp new file mode 100644 index 0000000..192fc2d --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_optimization.cpp @@ -0,0 +1,988 @@ +#include "Math/v3d_optimization.h" + +#if defined(V3DLIB_ENABLE_SUITESPARSE) +//# include "COLAMD/Include/colamd.h" +# include "colamd.h" +# if 0 +extern "C" +{ +//# include "LDL/Include/ldl.h" +# include "ldl.h" +} +# else +# include "Math/v3d_ldl_private.h" +# endif +#endif + +#define USE_BLOCK_REORDERING 1 +//#define USE_MULTIPLICATIVE_UPDATE 1 + +#include +#include + +using namespace std; + +namespace V3D +{ + + int optimizerVerbosenessLevel = 0; + +#if defined(V3DLIB_ENABLE_SUITESPARSE) + + void + SparseLevenbergOptimizer::setupSparseJtJ() + { + int const nVaryingA = _nParametersA - _nNonvaryingA; + int const nVaryingB = _nParametersB - _nNonvaryingB; + int const nVaryingC = _paramDimensionC - _nNonvaryingC; + + int const bColumnStart = nVaryingA*_paramDimensionA; + int const cColumnStart = bColumnStart + nVaryingB*_paramDimensionB; + int const nColumns = cColumnStart + nVaryingC; + + _jointNonzerosW.clear(); + _jointIndexW.resize(_nMeasurements); +#if 1 + { + map, int> jointNonzeroMap; + for (int k = 0; k < _nMeasurements; ++k) + { + int const i = _correspondingParamA[k] - _nNonvaryingA; + int const j = _correspondingParamB[k] - _nNonvaryingB; + if (i >= 0 && j >= 0) + { + map, int>::const_iterator p = jointNonzeroMap.find(make_pair(i, j)); + if (p == jointNonzeroMap.end()) + { + jointNonzeroMap.insert(make_pair(make_pair(i, j), _jointNonzerosW.size())); + _jointIndexW[k] = _jointNonzerosW.size(); + _jointNonzerosW.push_back(make_pair(i, j)); + } + else + { + _jointIndexW[k] = (*p).second; + } // end if + } // end if + } // end for (k) + } // end scope +#else + for (size_t k = 0; k < _nMeasurements; ++k) + { + int const i = _correspondingParamA[k] - _nNonvaryingA; + int const j = _correspondingParamB[k] - _nNonvaryingB; + if (i >= 0 && j >= 0) + { + _jointIndexW[k] = _jointNonzerosW.size(); + _jointNonzerosW.push_back(make_pair(i, j)); + } + } // end for (k) +#endif + +#if defined(USE_BLOCK_REORDERING) + int const bBlockColumnStart = nVaryingA; + int const cBlockColumnStart = bBlockColumnStart + nVaryingB; + + int const nBlockColumns = cBlockColumnStart + ((nVaryingC > 0) ? 1 : 0); + + //cout << "nBlockColumns = " << nBlockColumns << endl; + + // For the column reordering we treat the columns belonging to one set + // of parameters as one (logical) column. + + // Determine non-zeros of JtJ (we forget about the non-zero diagonal for now) + // Only consider nonzeros of Ai^t * Bj induced by the measurements. + vector > nz_blockJtJ(_jointNonzerosW.size()); + for (size_t k = 0; k < _jointNonzerosW.size(); ++k) + { + nz_blockJtJ[k].first = _jointNonzerosW[k].second + bBlockColumnStart; + nz_blockJtJ[k].second = _jointNonzerosW[k].first; + } + + if (nVaryingC > 0) + { + // We assume, that the global unknowns are linked to every other variable. + for (int i = 0; i < nVaryingA; ++i) + nz_blockJtJ.push_back(make_pair(cBlockColumnStart, i)); + for (int j = 0; j < nVaryingB; ++j) + nz_blockJtJ.push_back(make_pair(cBlockColumnStart, j + bBlockColumnStart)); + } // end if + + int const nnzBlock = nz_blockJtJ.size(); + + vector permBlockJtJ(nBlockColumns + 1); + + if (nnzBlock > 0) + { +// cout << "nnzBlock = " << nnzBlock << endl; + + CCS_Matrix blockJtJ(nBlockColumns, nBlockColumns, nz_blockJtJ); + +// cout << " nz_blockJtJ: " << endl; +// for (size_t k = 0; k < nz_blockJtJ.size(); ++k) +// cout << " " << nz_blockJtJ[k].first << ":" << nz_blockJtJ[k].second << endl; +// cout << endl; + + int * colStarts = (int *)blockJtJ.getColumnStarts(); + int * rowIdxs = (int *)blockJtJ.getRowIndices(); + +// cout << "blockJtJ_colStarts = "; +// for (int k = 0; k <= nBlockColumns; ++k) cout << colStarts[k] << " "; +// cout << endl; + +// cout << "blockJtJ_rowIdxs = "; +// for (int k = 0; k < nnzBlock; ++k) cout << rowIdxs[k] << " "; +// cout << endl; + + int stats[COLAMD_STATS]; + symamd(nBlockColumns, rowIdxs, colStarts, &permBlockJtJ[0], (double *) NULL, stats, &calloc, &free); + if (optimizerVerbosenessLevel >= 2) symamd_report(stats); + } + else + { + for (size_t k = 0; k < permBlockJtJ.size(); ++k) permBlockJtJ[k] = k; + } // end if + +// cout << "permBlockJtJ = "; +// for (int k = 0; k < permBlockJtJ.size(); ++k) +// cout << permBlockJtJ[k] << " "; +// cout << endl; + + // From the determined symbolic permutation with logical variables, determine the actual ordering + _perm_JtJ.resize(nVaryingA*_paramDimensionA + nVaryingB*_paramDimensionB + nVaryingC + 1); + + int curDstCol = 0; + for (size_t k = 0; k < permBlockJtJ.size()-1; ++k) + { + int const srcCol = permBlockJtJ[k]; + if (srcCol < nVaryingA) + { + for (int n = 0; n < _paramDimensionA; ++n) + _perm_JtJ[curDstCol + n] = srcCol*_paramDimensionA + n; + curDstCol += _paramDimensionA; + } + else if (srcCol >= bBlockColumnStart && srcCol < cBlockColumnStart) + { + int const bStart = nVaryingA*_paramDimensionA; + int const j = srcCol - bBlockColumnStart; + + for (int n = 0; n < _paramDimensionB; ++n) + _perm_JtJ[curDstCol + n] = bStart + j*_paramDimensionB + n; + curDstCol += _paramDimensionB; + } + else if (srcCol == cBlockColumnStart) + { + int const cStart = nVaryingA*_paramDimensionA + nVaryingB*_paramDimensionB; + + for (int n = 0; n < nVaryingC; ++n) + _perm_JtJ[curDstCol + n] = cStart + n; + curDstCol += nVaryingC; + } + else + { + cerr << "Should not reach " << __LINE__ << ":" << __LINE__ << "!" << endl; + assert(false); + } + } +#else + vector > nz, nzL; + this->serializeNonZerosJtJ(nz); + + for (int k = 0; k < nz.size(); ++k) + { + // Swap rows and columns, since serializeNonZerosJtJ() generates the + // upper triangular part but symamd wants the lower triangle. + nzL.push_back(make_pair(nz[k].second, nz[k].first)); + } + + _perm_JtJ.resize(nColumns+1); + + if (nzL.size() > 0) + { + CCS_Matrix symbJtJ(nColumns, nColumns, nzL); + + int * colStarts = (int *)symbJtJ.getColumnStarts(); + int * rowIdxs = (int *)symbJtJ.getRowIndices(); + +// cout << "symbJtJ_colStarts = "; +// for (int k = 0; k <= nColumns; ++k) cout << colStarts[k] << " "; +// cout << endl; + +// cout << "symbJtJ_rowIdxs = "; +// for (int k = 0; k < nzL.size(); ++k) cout << rowIdxs[k] << " "; +// cout << endl; + + int stats[COLAMD_STATS]; + symamd(nColumns, rowIdxs, colStarts, &_perm_JtJ[0], (double *) NULL, stats, &calloc, &free); + if (optimizerVerbosenessLevel >= 2) symamd_report(stats); + } + else + { + for (int k = 0; k < _perm_JtJ.size(); ++k) _perm_JtJ[k] = k; + } //// end if +#endif + _perm_JtJ.back() = _perm_JtJ.size() - 1; + +// cout << "_perm_JtJ = "; +// for (int k = 0; k < _perm_JtJ.size(); ++k) cout << _perm_JtJ[k] << " "; +// cout << endl; + + // Finally, compute the inverse of the full permutation. + _invPerm_JtJ.resize(_perm_JtJ.size()); + for (size_t k = 0; k < _perm_JtJ.size(); ++k) + _invPerm_JtJ[_perm_JtJ[k]] = k; + + vector > nz_JtJ; + this->serializeNonZerosJtJ(nz_JtJ); + + for (size_t k = 0; k < nz_JtJ.size(); ++k) + { + int const i = nz_JtJ[k].first; + int const j = nz_JtJ[k].second; + + int pi = _invPerm_JtJ[i]; + int pj = _invPerm_JtJ[j]; + // Swap values if in lower triangular part + if (pi > pj) std::swap(pi, pj); + nz_JtJ[k].first = pi; + nz_JtJ[k].second = pj; + } + + //int const nnz = nz_JtJ.size(); + +// cout << "nz_JtJ = "; +// for (int k = 0; k < nnz; ++k) cout << nz_JtJ[k].first << ":" << nz_JtJ[k].second << " "; +// cout << endl; + + _JtJ.create(nColumns, nColumns, nz_JtJ); + +// cout << "_colStart_JtJ = "; +// for (int k = 0; k < _JtJ.num_cols(); ++k) cout << _JtJ.getColumnStarts()[k] << " "; +// cout << endl; + +// cout << "_rowIdxs_JtJ = "; +// for (int k = 0; k < nnz; ++k) cout << _JtJ.getRowIndices()[k] << " "; +// cout << endl; + + vector workFlags(nColumns); + + _JtJ_Lp.resize(nColumns+1); + _JtJ_Parent.resize(nColumns); + _JtJ_Lnz.resize(nColumns); + +#if 0 + ldl_symbolic(nColumns, (int *)_JtJ.getColumnStarts(), (int *)_JtJ.getRowIndices(), + &_JtJ_Lp[0], &_JtJ_Parent[0], &_JtJ_Lnz[0], + &workFlags[0], NULL, NULL); +#else + LDL_symbolic(nColumns, (int *)_JtJ.getColumnStarts(), (int *)_JtJ.getRowIndices(), + &_JtJ_Lp[0], &_JtJ_Parent[0], &_JtJ_Lnz[0], + &workFlags[0]); +#endif + + if (optimizerVerbosenessLevel >= 1) + cout << "SparseLevenbergOptimizer: Nonzeros in LDL decomposition: " << _JtJ_Lp[nColumns] << endl; + + } // end SparseLevenbergOptimizer::setupSparseJtJ() + + void + SparseLevenbergOptimizer::serializeNonZerosJtJ(vector >& dst) const + { + int const nVaryingA = _nParametersA - _nNonvaryingA; + int const nVaryingB = _nParametersB - _nNonvaryingB; + int const nVaryingC = _paramDimensionC - _nNonvaryingC; + + int const bColumnStart = nVaryingA*_paramDimensionA; + int const cColumnStart = bColumnStart + nVaryingB*_paramDimensionB; + + dst.clear(); + + // Add the diagonal block matrices (only the upper triangular part). + + // Ui submatrices of JtJ + for (int i = 0; i < nVaryingA; ++i) + { + int const i0 = i * _paramDimensionA; + + for (int c = 0; c < _paramDimensionA; ++c) + for (int r = 0; r <= c; ++r) + dst.push_back(make_pair(i0 + r, i0 + c)); + } + + // Vj submatrices of JtJ + for (int j = 0; j < nVaryingB; ++j) + { + int const j0 = j*_paramDimensionB + bColumnStart; + + for (int c = 0; c < _paramDimensionB; ++c) + for (int r = 0; r <= c; ++r) + dst.push_back(make_pair(j0 + r, j0 + c)); + } + + // Z submatrix of JtJ + for (int c = 0; c < nVaryingC; ++c) + for (int r = 0; r <= c; ++r) + dst.push_back(make_pair(cColumnStart + r, cColumnStart + c)); + + // Add the elements i and j linked by an observation k + // W submatrix of JtJ + for (size_t n = 0; n < _jointNonzerosW.size(); ++n) + { + int const i0 = _jointNonzerosW[n].first; + int const j0 = _jointNonzerosW[n].second; + int const r0 = i0*_paramDimensionA; + int const c0 = j0*_paramDimensionB + bColumnStart; + + for (int r = 0; r < _paramDimensionA; ++r) + for (int c = 0; c < _paramDimensionB; ++c) + dst.push_back(make_pair(r0 + r, c0 + c)); + } // end for (n) + + if (nVaryingC > 0) + { + // Finally, add the dense columns linking i (resp. j) with the global parameters. + // X submatrix of JtJ + for (int i = 0; i < nVaryingA; ++i) + { + int const i0 = i*_paramDimensionA; + + for (int r = 0; r < _paramDimensionA; ++r) + for (int c = 0; c < nVaryingC; ++c) + dst.push_back(make_pair(i0 + r, cColumnStart + c)); + } + + // Y submatrix of JtJ + for (int j = 0; j < nVaryingB; ++j) + { + int const j0 = j*_paramDimensionB + bColumnStart; + + for (int r = 0; r < _paramDimensionB; ++r) + for (int c = 0; c < nVaryingC; ++c) + dst.push_back(make_pair(j0 + r, cColumnStart + c)); + } + } // end if + } // end SparseLevenbergOptimizer::serializeNonZerosJtJ() + + void + SparseLevenbergOptimizer::fillSparseJtJ(MatrixArray const& Ui, + MatrixArray const& Vj, + MatrixArray const& Wn, + Matrix const& Z, + Matrix const& X, + Matrix const& Y) + { + int const nVaryingA = _nParametersA - _nNonvaryingA; + int const nVaryingB = _nParametersB - _nNonvaryingB; + int const nVaryingC = _paramDimensionC - _nNonvaryingC; + + int const bColumnStart = nVaryingA*_paramDimensionA; + //int const cColumnStart = bColumnStart + nVaryingB*_paramDimensionB; + + //int const nCols = _JtJ.num_cols(); + //int const nnz = _JtJ.getNonzeroCount(); + + // The following has to replicate the procedure as in serializeNonZerosJtJ() + + int serial = 0; + + double * values = _JtJ.getValues(); + int const * destIdxs = _JtJ.getDestIndices(); + + // Add the diagonal block matrices (only the upper triangular part). + + // Ui submatrices of JtJ + for (int i = 0; i < nVaryingA; ++i) + { + //int const i0 = i * _paramDimensionA; + + for (int c = 0; c < _paramDimensionA; ++c) + for (int r = 0; r <= c; ++r, ++serial) + values[destIdxs[serial]] = Ui[i][r][c]; + } + + // Vj submatrices of JtJ + for (int j = 0; j < nVaryingB; ++j) + { + //int const j0 = j*_paramDimensionB + bColumnStart; + + for (int c = 0; c < _paramDimensionB; ++c) + for (int r = 0; r <= c; ++r, ++serial) + values[destIdxs[serial]] = Vj[j][r][c]; + } + + // Z submatrix of JtJ + for (int c = 0; c < nVaryingC; ++c) + for (int r = 0; r <= c; ++r, ++serial) + values[destIdxs[serial]] = Z[r][c]; + + // Add the elements i and j linked by an observation k + // W submatrix of JtJ + for (size_t n = 0; n < _jointNonzerosW.size(); ++n) + { + for (int r = 0; r < _paramDimensionA; ++r) + for (int c = 0; c < _paramDimensionB; ++c, ++serial) + values[destIdxs[serial]] = Wn[n][r][c]; + } // end for (k) + + if (nVaryingC > 0) + { + // Finally, add the dense columns linking i (resp. j) with the global parameters. + // X submatrix of JtJ + for (int i = 0; i < nVaryingA; ++i) + { + int const r0 = i * _paramDimensionA; + for (int r = 0; r < _paramDimensionA; ++r) + for (int c = 0; c < nVaryingC; ++c, ++serial) + values[destIdxs[serial]] = X[r0+r][c]; + } + + // Y submatrix of JtJ + for (int j = 0; j < nVaryingB; ++j) + { + int const r0 = j * _paramDimensionB; + for (int r = 0; r < _paramDimensionB; ++r) + for (int c = 0; c < nVaryingC; ++c, ++serial) + values[destIdxs[serial]] = Y[r0+r][c]; + } + } // end if + } // end SparseLevenbergOptimizer::fillSparseJtJ() + + void + SparseLevenbergOptimizer::minimize() + { + status = LEVENBERG_OPTIMIZER_TIMEOUT; + bool computeDerivatives = true; + + int const nVaryingA = _nParametersA - _nNonvaryingA; + int const nVaryingB = _nParametersB - _nNonvaryingB; + int const nVaryingC = _paramDimensionC - _nNonvaryingC; + + if (nVaryingA == 0 && nVaryingB == 0 && nVaryingC == 0) + { + // No degrees of freedom, nothing to optimize. + status = LEVENBERG_OPTIMIZER_CONVERGED; + return; + } + + this->setupSparseJtJ(); + + Vector weights(_nMeasurements); + + MatrixArray Ak(_nMeasurements, _measurementDimension, _paramDimensionA); + MatrixArray Bk(_nMeasurements, _measurementDimension, _paramDimensionB); + MatrixArray Ck(_nMeasurements, _measurementDimension, _paramDimensionC); + + MatrixArray Ui(nVaryingA, _paramDimensionA, _paramDimensionA); + MatrixArray Vj(nVaryingB, _paramDimensionB, _paramDimensionB); + + // Wn = Ak^t*Bk + MatrixArray Wn(_jointNonzerosW.size(), _paramDimensionA, _paramDimensionB); + + Matrix Z(nVaryingC, nVaryingC); + + // X = A^t*C + Matrix X(nVaryingA*_paramDimensionA, nVaryingC); + // Y = B^t*C + Matrix Y(nVaryingB*_paramDimensionB, nVaryingC); + + VectorArray residuals(_nMeasurements, _measurementDimension); + + VectorArray diagUi(nVaryingA, _paramDimensionA); + VectorArray diagVj(nVaryingB, _paramDimensionB); + Vector diagZ(nVaryingC); + + VectorArray At_e(nVaryingA, _paramDimensionA); + VectorArray Bt_e(nVaryingB, _paramDimensionB); + Vector Ct_e(nVaryingC); + + Vector Jt_e(nVaryingA*_paramDimensionA + nVaryingB*_paramDimensionB + nVaryingC); + + Vector delta(nVaryingA*_paramDimensionA + nVaryingB*_paramDimensionB + nVaryingC); + Vector deltaPerm(nVaryingA*_paramDimensionA + nVaryingB*_paramDimensionB + nVaryingC); + + VectorArray deltaAi(_nParametersA, _paramDimensionA); + VectorArray deltaBj(_nParametersB, _paramDimensionB); + Vector deltaC(_paramDimensionC); + + double err = 0.0; + + for (currentIteration = 0; currentIteration < maxIterations; ++currentIteration) + { + if (optimizerVerbosenessLevel >= 2) + cout << "SparseLevenbergOptimizer: currentIteration: " << currentIteration << endl; + if (computeDerivatives) + { + this->evalResidual(residuals); + this->fillWeights(residuals, weights); + for (int k = 0; k < _nMeasurements; ++k) + scaleVectorIP(weights[k], residuals[k]); + + err = squaredResidual(residuals); + + if (optimizerVerbosenessLevel >= 1) cout << "SparseLevenbergOptimizer: |residual|^2 = " << err << endl; + if (optimizerVerbosenessLevel >= 2) cout << "SparseLevenbergOptimizer: lambda = " << lambda << endl; + + for (size_t k = 0; k < residuals.count(); ++k) scaleVectorIP(-1.0, residuals[k]); + + this->setupJacobianGathering(); + this->fillAllJacobians(weights, Ak, Bk, Ck); + + // Compute the different parts of J^t*e + if (nVaryingA > 0) + { + for (int i = 0; i < nVaryingA; ++i) makeZeroVector(At_e[i]); + + Vector tmp(_paramDimensionA); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const i = _correspondingParamA[k] - _nNonvaryingA; + if (i < 0) continue; + multiply_At_v(Ak[k], residuals[k], tmp); + addVectors(tmp, At_e[i], At_e[i]); + } // end for (k) + } // end if + + if (nVaryingB > 0) + { + for (int j = 0; j < nVaryingB; ++j) makeZeroVector(Bt_e[j]); + + Vector tmp(_paramDimensionB); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const j = _correspondingParamB[k] - _nNonvaryingB; + if (j < 0) continue; + multiply_At_v(Bk[k], residuals[k], tmp); + addVectors(tmp, Bt_e[j], Bt_e[j]); + } // end for (k) + } // end if + + if (nVaryingC > 0) + { + makeZeroVector(Ct_e); + + Vector tmp(_paramDimensionC); + + for (int k = 0; k < _nMeasurements; ++k) + { + multiply_At_v(Ck[k], residuals[k], tmp); + for (int l = 0; l < nVaryingC; ++l) Ct_e[l] += tmp[_nNonvaryingC + l]; + } + } // end if + + int pos = 0; + for (int i = 0; i < nVaryingA; ++i) + for (int l = 0; l < _paramDimensionA; ++l, ++pos) + Jt_e[pos] = At_e[i][l]; + for (int j = 0; j < nVaryingB; ++j) + for (int l = 0; l < _paramDimensionB; ++l, ++pos) + Jt_e[pos] = Bt_e[j][l]; + for (int l = 0; l < nVaryingC; ++l, ++pos) + Jt_e[pos] = Ct_e[l]; + +// cout << "Jt_e = "; +// for (int k = 0; k < Jt_e.size(); ++k) cout << Jt_e[k] << " "; +// cout << endl; + + if (this->applyGradientStoppingCriteria(norm_Linf(Jt_e))) + { + status = LEVENBERG_OPTIMIZER_CONVERGED; + goto end; + } + + // The lhs J^t*J consists of several parts: + // [ U W X ] + // J^t*J = [ W^t V Y ] + // [ X^t Y^t Z ], + // where U, V and W are block-sparse matrices (due to the sparsity of A and B). + // X, Y and Z contain only a few columns (the number of global parameters). + + if (nVaryingA > 0) + { + // Compute Ui + Matrix U(_paramDimensionA, _paramDimensionA); + + for (int i = 0; i < nVaryingA; ++i) makeZeroMatrix(Ui[i]); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const i = _correspondingParamA[k] - _nNonvaryingA; + if (i < 0) continue; + multiply_At_A(Ak[k], U); + addMatricesIP(U, Ui[i]); + } // end for (k) + } // end if + + if (nVaryingB > 0) + { + // Compute Vj + Matrix V(_paramDimensionB, _paramDimensionB); + + for (int j = 0; j < nVaryingB; ++j) makeZeroMatrix(Vj[j]); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const j = _correspondingParamB[k] - _nNonvaryingB; + if (j < 0) continue; + multiply_At_A(Bk[k], V); + addMatricesIP(V, Vj[j]); + } // end for (k) + } // end if + + if (nVaryingC > 0) + { + Matrix ZZ(_paramDimensionC, _paramDimensionC); + Matrix Zsum(_paramDimensionC, _paramDimensionC); + + makeZeroMatrix(Zsum); + + for (int k = 0; k < _nMeasurements; ++k) + { + multiply_At_A(Ck[k], ZZ); + addMatricesIP(ZZ, Zsum); + } // end for (k) + + // Ignore the non-varying parameters + for (int i = 0; i < nVaryingC; ++i) + for (int j = 0; j < nVaryingC; ++j) + Z[i][j] = Zsum[i+_nNonvaryingC][j+_nNonvaryingC]; + } // end if + + if (nVaryingA > 0 && nVaryingB > 0) + { + for (size_t n = 0; n < Wn.count(); ++n) makeZeroMatrix(Wn[n]); + + Matrix W(_paramDimensionA, _paramDimensionB); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const n = _jointIndexW[k]; + if (n >= 0) + { + //int const i0 = _jointNonzerosW[n].first; + //int const j0 = _jointNonzerosW[n].second; + + multiply_At_B(Ak[k], Bk[k], W); + addMatricesIP(W, Wn[n]); + } // end if + } // end for (k) + } // end if + + if (nVaryingA > 0 && nVaryingC > 0) + { + Matrix XX(_paramDimensionA, _paramDimensionC); + + makeZeroMatrix(X); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const i = _correspondingParamA[k] - _nNonvaryingA; + // Ignore the non-varying parameters + if (i < 0) continue; + + multiply_At_B(Ak[k], Ck[k], XX); + + for (int r = 0; r < _paramDimensionA; ++r) + for (int c = 0; c < nVaryingC; ++c) + X[r+i*_paramDimensionA][c] += XX[r][c+_nNonvaryingC]; + } // end for (k) + } // end if + + if (nVaryingB > 0 && nVaryingC > 0) + { + Matrix YY(_paramDimensionB, _paramDimensionC); + + makeZeroMatrix(Y); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const j = _correspondingParamB[k] - _nNonvaryingB; + // Ignore the non-varying parameters + if (j < 0) continue; + + multiply_At_B(Bk[k], Ck[k], YY); + + for (int r = 0; r < _paramDimensionB; ++r) + for (int c = 0; c < nVaryingC; ++c) + Y[r+j*_paramDimensionB][c] += YY[r][c+_nNonvaryingC]; + } // end for (k) + } // end if + + if (currentIteration == 0) + { + // Initialize lambda as tau*max(JtJ[i][i]) + double maxEl = -1e30; + if (optimizerVerbosenessLevel < 2) + { + if (nVaryingA > 0) + { + for (int i = 0; i < nVaryingA; ++i) + for (int l = 0; l < _paramDimensionA; ++l) + maxEl = std::max(maxEl, Ui[i][l][l]); + } + if (nVaryingB > 0) + { + for (int j = 0; j < nVaryingB; ++j) + for (int l = 0; l < _paramDimensionB; ++l) + maxEl = std::max(maxEl, Vj[j][l][l]); + } + if (nVaryingC > 0) + { + for (int l = 0; l < nVaryingC; ++l) + maxEl = std::max(maxEl, Z[l][l]); + } + } + else + { + char maxElSection = 'X'; + int maxElIndex = -1; + int maxElRow = -1; + + if (nVaryingA > 0) + { + for (int i = 0; i < nVaryingA; ++i) + for (int l = 0; l < _paramDimensionA; ++l) + { + if (Ui[i][l][l] > maxEl) + { + maxEl = Ui[i][l][l]; + maxElSection = 'U'; maxElIndex = i; maxElRow = l; + } + } + } + if (nVaryingB > 0) + { + for (int j = 0; j < nVaryingB; ++j) + for (int l = 0; l < _paramDimensionB; ++l) + { + if (Vj[j][l][l] > maxEl) + { + maxEl = Vj[j][l][l]; + maxElSection = 'V'; maxElIndex = j; maxElRow = l; + } + } + } + if (nVaryingC > 0) + { + for (int l = 0; l < nVaryingC; ++l) + { + if (Z[l][l] > maxEl) + { + maxEl = Z[l][l]; + maxElSection = 'Z'; maxElIndex = 0; maxElRow = l; + } + } + } + cout << "SparseLevenbergOptimizer: initial lambda = " << tau*maxEl + << "; max diagonal element found at " << maxElSection << "[" << maxElIndex << "][" + << maxElRow << "]" << endl; + } // end if + + lambda = tau * maxEl; + } // end if (currentIteration == 0) + } // end if (computeDerivatives) + + for (int i = 0; i < nVaryingA; ++i) + { + for (int l = 0; l < _paramDimensionA; ++l) diagUi[i][l] = Ui[i][l][l]; + } // end for (i) + + for (int j = 0; j < nVaryingB; ++j) + { + for (int l = 0; l < _paramDimensionB; ++l) diagVj[j][l] = Vj[j][l][l]; + } // end for (j) + + for (int l = 0; l < nVaryingC; ++l) diagZ[l] = Z[l][l]; + + // Augment the diagonals with lambda (either by the standard additive update or by multiplication). +#if !defined(USE_MULTIPLICATIVE_UPDATE) + for (int i = 0; i < nVaryingA; ++i) + for (int l = 0; l < _paramDimensionA; ++l) + Ui[i][l][l] += lambda; + + for (int j = 0; j < nVaryingB; ++j) + for (int l = 0; l < _paramDimensionB; ++l) + Vj[j][l][l] += lambda; + + for (int l = 0; l < nVaryingC; ++l) + Z[l][l] += lambda; +#else + for (int i = 0; i < nVaryingA; ++i) + for (unsigned l = 0; l < _paramDimensionA; ++l) + Ui[i][l][l] = std::max(Ui[i][l][l] * (1.0 + lambda), 1e-15); + + for (int j = 0; j < nVaryingB; ++j) + for (unsigned l = 0; l < _paramDimensionB; ++l) + Vj[j][l][l] = std::max(Vj[j][l][l] * (1.0 + lambda), 1e-15); + + for (unsigned l = 0; l < nVaryingC; ++l) + Z[l][l] = std::max(Z[l][l] * (1.0 + lambda), 1e-15); +#endif + + this->fillSparseJtJ(Ui, Vj, Wn, Z, X, Y); + + bool success = true; + double rho = 0.0; + { + int const nCols = _JtJ_Parent.size(); + //int const nnz = _JtJ.getNonzeroCount(); + int const lnz = _JtJ_Lp.back(); + + vector Li(lnz); + vector Lx(lnz); + vector D(nCols), Y(nCols); + vector workPattern(nCols), workFlag(nCols); + + int * colStarts = (int *)_JtJ.getColumnStarts(); + int * rowIdxs = (int *)_JtJ.getRowIndices(); + double * values = _JtJ.getValues(); + +#if 0 + int const d = ldl_numeric(nCols, colStarts, rowIdxs, values, + &_JtJ_Lp[0], &_JtJ_Parent[0], &_JtJ_Lnz[0], + &Li[0], &Lx[0], &D[0], + &Y[0], &workPattern[0], &workFlag[0], + NULL, NULL); +#else + int const d = LDL_numeric(nCols, colStarts, rowIdxs, values, + &_JtJ_Lp[0], &_JtJ_Parent[0], &_JtJ_Lnz[0], + &Li[0], &Lx[0], &D[0], + &Y[0], &workPattern[0], &workFlag[0]); +#endif + + if (d == nCols) + { +#if 0 + ldl_perm(nCols, &deltaPerm[0], &Jt_e[0], &_perm_JtJ[0]); + ldl_lsolve(nCols, &deltaPerm[0], &_JtJ_Lp[0], &Li[0], &Lx[0]); + ldl_dsolve(nCols, &deltaPerm[0], &D[0]); + ldl_ltsolve(nCols, &deltaPerm[0], &_JtJ_Lp[0], &Li[0], &Lx[0]); + ldl_permt(nCols, &delta[0], &deltaPerm[0], &_perm_JtJ[0]); +#else + LDL_perm(nCols, &deltaPerm[0], &Jt_e[0], &_perm_JtJ[0]); + LDL_lsolve(nCols, &deltaPerm[0], &_JtJ_Lp[0], &Li[0], &Lx[0]); + LDL_dsolve(nCols, &deltaPerm[0], &D[0]); + LDL_ltsolve(nCols, &deltaPerm[0], &_JtJ_Lp[0], &Li[0], &Lx[0]); + LDL_permt(nCols, &delta[0], &deltaPerm[0], &_perm_JtJ[0]); +#endif + } + else + { + if (optimizerVerbosenessLevel >= 2) + cout << "SparseLevenbergOptimizer: LDL decomposition failed. Increasing lambda." << endl; + success = false; + } + } + + if (success) + { + double const deltaSqrLength = sqrNorm_L2(delta); + double const paramLength = this->getParameterLength(); + + if (optimizerVerbosenessLevel >= 2) + cout << "ExtSparseLevenbergOptimizer: ||delta|| = " << sqrt(deltaSqrLength) << " ||paramLength|| = " << paramLength << endl; + + if (this->applyUpdateStoppingCriteria(paramLength, sqrt(deltaSqrLength))) + { + status = LEVENBERG_OPTIMIZER_SMALL_UPDATE; + goto end; + } + + // Copy the updates from delta to the respective arrays + int pos = 0; + + for (int i = 0; i < _nNonvaryingA; ++i) makeZeroVector(deltaAi[i]); + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + for (int l = 0; l < _paramDimensionA; ++l, ++pos) + deltaAi[i][l] = delta[pos]; + + for (int j = 0; j < _nNonvaryingB; ++j) makeZeroVector(deltaBj[j]); + for (int j = _nNonvaryingB; j < _nParametersB; ++j) + for (int l = 0; l < _paramDimensionB; ++l, ++pos) + deltaBj[j][l] = delta[pos]; + + makeZeroVector(deltaC); + for (int l = _nNonvaryingC; l < _paramDimensionC; ++l, ++pos) + deltaC[l] = delta[pos]; + + saveAllParameters(); + if (nVaryingA > 0) updateParametersA(deltaAi); + if (nVaryingB > 0) updateParametersB(deltaBj); + if (nVaryingC > 0) updateParametersC(deltaC); + + // if (optimizerVerbosenessLevel >= 2) + // cout << "SparseLevenbergOptimizer: |deltaAi|^2 = " << squaredResidual(deltaAi) << " |deltaBj|^2 = " << squaredResidual(deltaBj) << endl; + + this->evalResidual(residuals); + this->fillWeights(residuals, weights); + for (int k = 0; k < _nMeasurements; ++k) + scaleVectorIP(weights[k], residuals[k]); + + double const newErr = squaredResidual(residuals); + rho = err - newErr; + if (optimizerVerbosenessLevel >= 2) + cout << "SparseLevenbergOptimizer: |new residual|^2 = " << newErr << " rho = " << rho << endl; + +#if !defined(USE_MULTIPLICATIVE_UPDATE) + double const denom1 = lambda * deltaSqrLength; +#else + double denom1 = 0.0f; + for (int i = _nNonvaryingA; i < _nParametersA; ++i) + for (int l = 0; l < _paramDimensionA; ++l) + denom1 += deltaAi[i][l] * deltaAi[i][l] * diagUi[i-_nNonvaryingA][l]; + + for (int j = _nNonvaryingB; j < _nParametersB; ++j) + for (int l = 0; l < _paramDimensionB; ++l) + denom1 += deltaBj[j][l] * deltaBj[j][l] * diagVj[j-_nNonvaryingB][l]; + + for (int l = _nNonvaryingC; l < _paramDimensionC; ++l) + denom1 += deltaC[l] * deltaC[l] * diagZ[l-_nNonvaryingC]; + + denom1 *= lambda; +#endif + double const denom2 = innerProduct(delta, Jt_e); + rho = rho / (denom1 + denom2); + if (optimizerVerbosenessLevel >= 2) + cout << "SparseLevenbergOptimizer: rho = " << rho + << " denom1 = " << denom1 << " denom2 = " << denom2 << endl; + } // end if (success) + + if (success && rho > 0) + { + if (optimizerVerbosenessLevel >= 2) + cout << "SparseLevenbergOptimizer: Improved solution - decreasing lambda." << endl; + // Improvement in the new solution + decreaseLambda(rho); + computeDerivatives = true; + } + else + { + if (optimizerVerbosenessLevel >= 2) + cout << "SparseLevenbergOptimizer: Inferior solution - increasing lambda." << endl; + restoreAllParameters(); + increaseLambda(); + computeDerivatives = false; + + // Restore diagonal elements in Ui, Vj and Z. + for (int i = 0; i < nVaryingA; ++i) + { + for (int l = 0; l < _paramDimensionA; ++l) Ui[i][l][l] = diagUi[i][l]; + } // end for (i) + + for (int j = 0; j < nVaryingB; ++j) + { + for (int l = 0; l < _paramDimensionB; ++l) Vj[j][l][l] = diagVj[j][l]; + } // end for (j) + + for (int l = 0; l < nVaryingC; ++l) Z[l][l] = diagZ[l]; + } // end if + } // end for + + end:; + if (optimizerVerbosenessLevel >= 2) + cout << "Leaving SparseLevenbergOptimizer::minimize()." << endl; + } // end SparseLevenbergOptimizer::minimize() + +#endif // defined(V3DLIB_ENABLE_SUITESPARSE) + +} // end namespace V3D diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_optimization.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_optimization.h new file mode 100644 index 0000000..c189eb4 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_optimization.h @@ -0,0 +1,661 @@ +// -*- C++ -*- + +#ifndef V3D_OPTIMIZATION_H +#define V3D_OPTIMIZATION_H + +#include "Math/v3d_linear.h" +#include "Math/v3d_linear_tnt.h" +#include "Math/v3d_mathutilities.h" + +#include +#include + +namespace V3D +{ + + enum + { + LEVENBERG_OPTIMIZER_TIMEOUT = 0, + LEVENBERG_OPTIMIZER_SMALL_UPDATE = 1, + LEVENBERG_OPTIMIZER_CONVERGED = 2 + }; + + extern int optimizerVerbosenessLevel; + + struct LevenbergOptimizerCommon + { + LevenbergOptimizerCommon() + : status(LEVENBERG_OPTIMIZER_TIMEOUT), currentIteration(0), maxIterations(50), minIterations(10), + tau(1e-3), lambda(1e-3), gradientThreshold(1e-8), updateThreshold(1e-8), improvementThreshold(1e-8), + _nu(2.0) + { } + + virtual ~LevenbergOptimizerCommon() + { } + +#if 0 + // See Madsen et al., "Methods for non-linear least squares problems." + virtual void increaseLambda() + { + lambda *= _nu; _nu *= 2.0; + } + + virtual void decreaseLambda(double const rho) + { + double const r = 2*rho - 1.0; + lambda *= std::max(1.0/3.0, 1 - r*r*r); + if (lambda < 1e-10) lambda = 1e-10; + _nu = 2; + } +#else + virtual void increaseLambda() { lambda *= 10.0; } + virtual void decreaseLambda(double const rho) { lambda = std::max(1e-10, lambda * 0.1); } +#endif + + bool applyGradientStoppingCriteria(double maxGradient) const + { + return maxGradient < gradientThreshold; + } + + bool applyUpdateStoppingCriteria(double paramLength, double updateLength) const + { + if (currentIteration < minIterations) return false; + return updateLength < updateThreshold * (paramLength + updateThreshold); + } + + bool applyImprovementStoppingCriteria(double const deltaError, double const oldError) const + { + double const relImprovement = fabs(deltaError / oldError); + return relImprovement < improvementThreshold; + } + + int status; + int currentIteration, maxIterations, minIterations; + double tau, lambda; + double gradientThreshold, updateThreshold, improvementThreshold; + + protected: + double _nu; + }; // end struct LevenbergOptimizerCommon + + //! Dense (and simple) Levenberg-Marquardt optimizer. + struct SimpleLevenbergOptimizer : public LevenbergOptimizerCommon + { + SimpleLevenbergOptimizer(unsigned nMeasurements_, unsigned nParameters_) + : LevenbergOptimizerCommon(), + nMeasurements(nMeasurements_), nParameters(nParameters_), + numDiffDelta(nParameters_, 1e-6), + observation(nMeasurements), + currentParameters(nParameters_), savedParameters(nParameters_) + { + makeZeroVector(observation); + } + + virtual ~SimpleLevenbergOptimizer() { } + + //! Minimize the objective function starting from the current parameters. + void minimize() + { + using namespace std; + + status = LEVENBERG_OPTIMIZER_TIMEOUT; + bool computeDerivatives = true; + + Matrix J(nMeasurements, nParameters); + Matrix JtJ(nParameters, nParameters); + Matrix N(nParameters, nParameters); + Vector e(nMeasurements), e_new(nMeasurements), Jt_e(nParameters); + Vector delta(nParameters); + Vector weights(nMeasurements); + fillVector(1.0, weights); + + for (currentIteration = 0; currentIteration < maxIterations; ++currentIteration) + { + if (optimizerVerbosenessLevel >= 2) cout << "currentIteration: " << currentIteration << endl; + + this->fillWeights(weights); + this->evalWeightedResidual(weights, e); + double const err = sqrNorm_L2(e); + + if (optimizerVerbosenessLevel >= 1) cout << "residual = " << err << endl; + if (optimizerVerbosenessLevel >= 2) cout << "lambda = " << lambda << endl; + + if (computeDerivatives) + { + fillJacobian(J); + multiply_At_A(J, JtJ); + multiply_At_v(J, e, Jt_e); + } + + if (this->applyGradientStoppingCriteria(norm_Linf(Jt_e))) + { + status = LEVENBERG_OPTIMIZER_CONVERGED; + return; + } + + if (currentIteration == 0) + { + // Initialize lambda as tau*max(JtJ(i, i)) + // Note: operator()(i, j) is 1-based. + double maxEl = -1e30; + for (unsigned l = 0; l < nParameters; ++l) + maxEl = std::max(maxEl, JtJ[l][l]); + + lambda = tau * maxEl; + if (optimizerVerbosenessLevel >= 2) cout << "initial lambda = " << lambda << endl; + } // end if (currentIteration == 0) + + N = JtJ; + for (unsigned k = 1; k <= N.num_rows(); ++k) + this->augmentDiagonal(N(k, k)); + + Cholesky chol(N); + delta = chol.solve(Jt_e); + + if (optimizerVerbosenessLevel >= 2) cout << " deltaSqrLength = " << sqrNorm_L2(delta) << endl; + + double const paramLength = this->getParameterLength(); + if (this->applyUpdateStoppingCriteria(paramLength, norm_L2(delta))) + { + status = LEVENBERG_OPTIMIZER_SMALL_UPDATE; + return; + } + + this->saveCurrentParameters(); + updateCurrentParameters(delta); + + evalWeightedResidual(weights, e_new); + double const err_new = sqrNorm_L2(e_new); + double rho = err - err_new; + + if (optimizerVerbosenessLevel >= 2) cout << "new residual = " << err_new << " rho = " << rho << endl; + + { + double denom1 = sqrNorm_L2(delta); + double denom2 = innerProduct(delta, Jt_e); + + denom1 *= lambda; + rho = rho / (denom1 + denom2); + if (optimizerVerbosenessLevel >= 2) cout << " rho = " << rho << endl; + } + + if (rho > 0) + { + // Improvement in the new solution + decreaseLambda(rho); + computeDerivatives = true; + } + else + { + restoreCurrentParameters(); + increaseLambda(); + computeDerivatives = false; + } // end if (rho > 0) + } // end for + } // end minimize() + + //! Minimize the objective function using from the given parameters. + void minimize(Vector const& startingParameters) + { + currentParameters = startingParameters; + minimize(); + } + + //virtual void augmentDiagonal(double& v) const { v *= (1.0 + lambda); } + virtual void augmentDiagonal(double& v) const { v += lambda; } + + virtual double getParameterLength() const + { + return norm_L2(currentParameters); + } + + virtual void updateCurrentParameters(Vector const& delta) + { + currentParameters += delta; + } + + virtual void saveCurrentParameters() + { + savedParameters = currentParameters; + } + + virtual void restoreCurrentParameters() + { + currentParameters = savedParameters; + } + + //! residual = observation - f(currentParameters) + virtual void evalWeightedResidual(Vector const& w, Vector& res) + { + evalFunction(res); + for (unsigned i = 0; i < res.size(); ++i) + res[i] = w[i] * (observation[i] - res[i]); + } + + virtual void evalFunction(Vector& res) = 0; + virtual void fillJacobian(Matrix& J) { this->approximateJacobian(J); } + + virtual void fillWeights(Vector& w) + { + std::fill(w.begin(), w.end(), 1.0); + } + + //! Approximates the Jacobian using a 2-sided difference scheme. + void approximateJacobian(Matrix& J) + { + Vector update(nParameters); + makeZeroVector(update); + Vector y1(nMeasurements), y2(nMeasurements); + + for (unsigned j = 0; j < J.num_cols(); ++j) + { + saveCurrentParameters(); + + update[j] = -numDiffDelta[j]; + updateCurrentParameters(update); + evalFunction(y1); + restoreCurrentParameters(); + + update[j] = numDiffDelta[j]; + updateCurrentParameters(update); + evalFunction(y2); + + // Note: operator()(i, j) is 1-based. + for (unsigned i = 0; i < J.num_rows(); ++i) + J(1+i, 1+j) = (y2[i] - y1[i])/2/numDiffDelta[j]; + + update[j] = 0; + restoreCurrentParameters(); + } // end for (j) + } + + unsigned nMeasurements, nParameters; + std::vector numDiffDelta; + + Vector observation; + Vector currentParameters, savedParameters; + }; // end struct SimpleLevenbergOptimizer + +//---------------------------------------------------------------------- + +# if defined(V3DLIB_ENABLE_SUITESPARSE) + + struct SparseLevenbergOptimizer : public LevenbergOptimizerCommon + { + SparseLevenbergOptimizer(int measurementDimension, + int nParametersA, int paramDimensionA, + int nParametersB, int paramDimensionB, + int paramDimensionC, + std::vector const& correspondingParamA, + std::vector const& correspondingParamB) + : LevenbergOptimizerCommon(), + _nMeasurements(correspondingParamA.size()), + _measurementDimension(measurementDimension), + _nParametersA(nParametersA), _paramDimensionA(paramDimensionA), + _nParametersB(nParametersB), _paramDimensionB(paramDimensionB), + _paramDimensionC(paramDimensionC), + _nNonvaryingA(0), _nNonvaryingB(0), _nNonvaryingC(0), + _correspondingParamA(correspondingParamA), + _correspondingParamB(correspondingParamB) + { + assert(correspondingParamA.size() == correspondingParamB.size()); + } + + ~SparseLevenbergOptimizer() { } + + void setNonvaryingCounts(int nNonvaryingA, int nNonvaryingB, int nNonvaryingC) + { + _nNonvaryingA = nNonvaryingA; + _nNonvaryingB = nNonvaryingB; + _nNonvaryingC = nNonvaryingC; + } + + void getNonvaryingCounts(int& nNonvaryingA, int& nNonvaryingB, int& nNonvaryingC) const + { + nNonvaryingA = _nNonvaryingA; + nNonvaryingB = _nNonvaryingB; + nNonvaryingC = _nNonvaryingC; + } + + void minimize(); + + virtual void evalResidual(VectorArray& residuals) = 0; + + virtual void fillWeights(VectorArray const& residuals, Vector& w) + { + std::fill(w.begin(), w.end(), 1.0); + } + + void fillAllJacobians(Vector const& w, + MatrixArray& Ak, + MatrixArray& Bk, + MatrixArray& Ck) + { + int const nVaryingA = _nParametersA - _nNonvaryingA; + int const nVaryingB = _nParametersB - _nNonvaryingB; + int const nVaryingC = _paramDimensionC - _nNonvaryingC; + + for (int k = 0; k < _nMeasurements; ++k) + { + int const i = _correspondingParamA[k]; + int const j = _correspondingParamB[k]; + + if (i < _nNonvaryingA && j < _nNonvaryingB) continue; + + fillJacobians(Ak[k], Bk[k], Ck[k], i, j, k); + + // Clear the Jacobians for locked parameters. + if (i < _nNonvaryingA) makeZeroMatrix(Ak[k]); + if (j < _nNonvaryingB) makeZeroMatrix(Bk[k]); + + if (_nNonvaryingC > 0) + { + for (int r = 0; r < _measurementDimension; ++r) + for (int c = 0; c < _nNonvaryingC; ++c) Ck[k][r][c] = 0.0; + } + } // end for (k) + + if (nVaryingA > 0) + { + for (int k = 0; k < _nMeasurements; ++k) + scaleMatrixIP(w[k], Ak[k]); + } + if (nVaryingB > 0) + { + for (int k = 0; k < _nMeasurements; ++k) + scaleMatrixIP(w[k], Bk[k]); + } + if (nVaryingC > 0) + { + for (int k = 0; k < _nMeasurements; ++k) + scaleMatrixIP(w[k], Ck[k]); + } + } // end fillAllJacobians() + + virtual void setupJacobianGathering() { } + + virtual void fillJacobians(Matrix& Ak, Matrix& Bk, Matrix& Ck, + int i, int j, int k) = 0; + + virtual double getParameterLength() const = 0; + + virtual void updateParametersA(VectorArray const& deltaAi) = 0; + virtual void updateParametersB(VectorArray const& deltaBj) = 0; + virtual void updateParametersC(Vector const& deltaC) = 0; + virtual void saveAllParameters() = 0; + virtual void restoreAllParameters() = 0; + + CCS_Matrix const& JtJ() const + { + return _JtJ; + } + + protected: + void serializeNonZerosJtJ(std::vector >& dst) const; + void setupSparseJtJ(); + void fillSparseJtJ(MatrixArray const& Ui, MatrixArray const& Vj, MatrixArray const& Wk, + Matrix const& Z, Matrix const& X, Matrix const& Y); + + int const _nMeasurements, _measurementDimension; + int const _nParametersA, _paramDimensionA; + int const _nParametersB, _paramDimensionB; + int const _paramDimensionC; + + int _nNonvaryingA, _nNonvaryingB, _nNonvaryingC; + + std::vector const& _correspondingParamA; + std::vector const& _correspondingParamB; + + std::vector > _jointNonzerosW; + std::vector _jointIndexW; + + std::vector _JtJ_Lp, _JtJ_Parent, _JtJ_Lnz; + std::vector _perm_JtJ, _invPerm_JtJ; + + CCS_Matrix _JtJ; + }; // end struct SparseLevenbergOptimizer + +//---------------------------------------------------------------------- + + struct StdSparseLevenbergOptimizer : public SparseLevenbergOptimizer + { + StdSparseLevenbergOptimizer(int measurementDimension, + int nParametersA, int paramDimensionA, + int nParametersB, int paramDimensionB, + int paramDimensionC, + std::vector const& correspondingParamA, + std::vector const& correspondingParamB) + : SparseLevenbergOptimizer(measurementDimension, nParametersA, paramDimensionA, + nParametersB, paramDimensionB, paramDimensionC, + correspondingParamA, correspondingParamB), + curParametersA(nParametersA, paramDimensionA), savedParametersA(nParametersA, paramDimensionA), + curParametersB(nParametersB, paramDimensionB), savedParametersB(nParametersB, paramDimensionB), + curParametersC(paramDimensionC), savedParametersC(paramDimensionC) + { } + + virtual double getParameterLength() const + { + double res = 0.0; + for (int i = 0; i < _nParametersA; ++i) res += sqrNorm_L2(curParametersA[i]); + for (int j = 0; j < _nParametersB; ++j) res += sqrNorm_L2(curParametersB[j]); + res += sqrNorm_L2(curParametersC); + return sqrt(res); + } + + virtual void updateParametersA(VectorArray const& deltaAi) + { + for (int i = 0; i < _nParametersA; ++i) addVectors(deltaAi[i], curParametersA[i], curParametersA[i]); + } + + virtual void updateParametersB(VectorArray const& deltaBj) + { + for (int j = 0; j < _nParametersB; ++j) addVectors(deltaBj[j], curParametersB[j], curParametersB[j]); + } + + virtual void updateParametersC(Vector const& deltaC) + { + addVectors(deltaC, curParametersC, curParametersC); + } + + virtual void saveAllParameters() + { + for (int i = 0; i < _nParametersA; ++i) savedParametersA[i] = curParametersA[i]; + for (int j = 0; j < _nParametersB; ++j) savedParametersB[j] = curParametersB[j]; + savedParametersC = curParametersC; + } + + virtual void restoreAllParameters() + { + for (int i = 0; i < _nParametersA; ++i) curParametersA[i] = savedParametersA[i]; + for (int j = 0; j < _nParametersB; ++j) curParametersB[j] = savedParametersB[j]; + curParametersC = savedParametersC; + } + + VectorArray curParametersA, savedParametersA; + VectorArray curParametersB, savedParametersB; + Vector curParametersC, savedParametersC; + }; // end struct StdSparseLevenbergOptimizer + +//---------------------------------------------------------------------- + + struct SparseLM_ParameterInfo + { + SparseLM_ParameterInfo(int nParametersA_, int paramDimensionA_, + int nParametersB_, int paramDimensionB_, + int paramDimensionC_) + : nParametersA(nParametersA_), paramDimensionA(paramDimensionA_), + nParametersB(nParametersB_), paramDimensionB(paramDimensionB_), + paramDimensionC(paramDimensionC_), + nNonvaryingA(0), nNonvaryingB(0), nNonvaryingC(0) + { + nVaryingA = nParametersA - nNonvaryingA; + nVaryingB = nParametersB - nNonvaryingB; + nVaryingC = paramDimensionC - nNonvaryingC; + } + + void setNonvaryingCounts(int const nNonvaryingA_, int const nNonvaryingB_, int const nNonvaryingC_) + { + nNonvaryingA = nNonvaryingA_; + nNonvaryingB = nNonvaryingB_; + nNonvaryingC = nNonvaryingC_; + nVaryingA = nParametersA - nNonvaryingA; + nVaryingB = nParametersB - nNonvaryingB; + nVaryingC = paramDimensionC - nNonvaryingC; + } + + void getNonvaryingCounts(int& nNonvaryingA_, int& nNonvaryingB_, int& nNonvaryingC_) const + { + nNonvaryingA_ = nNonvaryingA; + nNonvaryingB_ = nNonvaryingB; + nNonvaryingC_ = nNonvaryingC; + } + + int const nParametersA, paramDimensionA; + int const nParametersB, paramDimensionB; + int const paramDimensionC; + + int nNonvaryingA, nNonvaryingB, nNonvaryingC; + int nVaryingA, nVaryingB, nVaryingC; + }; // end struct SparseLM_ParameterInfo + + struct SparseLM_CostFunction + { + SparseLM_CostFunction(SparseLM_ParameterInfo const& paramInfo, + int const measurementDimension, + std::vector const& correspondingParamA, + std::vector const& correspondingParamB) + : _paramInfo(paramInfo), + _nMeasurements(correspondingParamA.size()), + _measurementDimension(measurementDimension), + _correspondingParamA(correspondingParamA), + _correspondingParamB(correspondingParamB), + _residuals(_nMeasurements, measurementDimension), + _weights(_nMeasurements), + _Ak(_nMeasurements, measurementDimension, paramInfo.paramDimensionA), + _Bk(_nMeasurements, measurementDimension, paramInfo.paramDimensionB), + _Ck(_nMeasurements, measurementDimension, paramInfo.paramDimensionC) + { + assert(correspondingParamA.size() == correspondingParamB.size()); + } + + ~SparseLM_CostFunction() { } + + // Domain specific code to be defined in derived classes + virtual void fillWeights(VectorArray const& residuals, Vector& w) const + { + std::fill(w.begin(), w.end(), 1.0); + } + virtual void evalResidual(VectorArray& residuals) const = 0; + virtual void setupJacobianGathering() { } + virtual void fillJacobians(Matrix& Ak, Matrix& Bk, Matrix& Ck, + int i, int j, int k) const = 0; + // End of domain specific code + + protected: + void fillAllJacobians(); + void fillHessian(std::vector const& jointIndexW, + MatrixArray& Ui, MatrixArray& Vj, + MatrixArray& Wn, + Matrix& X, Matrix& Y, Matrix& Z); + void evalJt_e(VectorArray& At_e, VectorArray& Bt_e, Vector& Ct_e); + + SparseLM_ParameterInfo const _paramInfo; + int const _nMeasurements, _measurementDimension; + + std::vector const& _correspondingParamA; + std::vector const& _correspondingParamB; + + VectorArray _residuals; + Vector _weights; + MatrixArray _Ak, _Bk, _Ck; + + friend struct ExtSparseLevenbergOptimizer; + }; // end struct SparseLM_CostFunction + + struct ExtSparseLevenbergOptimizer : public LevenbergOptimizerCommon + { + ExtSparseLevenbergOptimizer(SparseLM_ParameterInfo const& paramInfo, + std::vector const& costFunctions) + : LevenbergOptimizerCommon(), + _paramInfo(paramInfo), _costFunctions(costFunctions), + _Ui(paramInfo.nVaryingA, paramInfo.paramDimensionA, paramInfo.paramDimensionA), + _Vj(paramInfo.nVaryingB, paramInfo.paramDimensionB, paramInfo.paramDimensionB), + _X(paramInfo.nVaryingA*paramInfo.paramDimensionA, paramInfo.nVaryingC), + _Y(paramInfo.nVaryingB*paramInfo.paramDimensionB, paramInfo.nVaryingC), + _Z(paramInfo.nVaryingC, paramInfo.nVaryingC) + { } + + ~ExtSparseLevenbergOptimizer() { } + + void minimize(); + + virtual double getParameterLength() const = 0; + + virtual void updateParametersA(VectorArray const& deltaAi) = 0; + virtual void updateParametersB(VectorArray const& deltaBj) = 0; + virtual void updateParametersC(Vector const& deltaC) = 0; + virtual void saveAllParameters() = 0; + virtual void restoreAllParameters() = 0; + + CCS_Matrix const& JtJ() const { return _JtJ; } + + protected: + void setupSparseJtJ(); + void serializeNonZerosJtJ(std::vector >& dst) const; + void fillJtJ(MatrixArray const& Ui, MatrixArray const& Vj, MatrixArray const& Wn, + Matrix const& Z, Matrix const& X, Matrix const& Y); + + SparseLM_ParameterInfo const& _paramInfo; + std::vector const& _costFunctions; + + std::vector > _jointNonzerosW; // vector of all pairs (i,j) generated by all measurements + std::vector > _jointIndexW; // maps measurements wrt. the cost function to the position of Wij + + std::vector _JtJ_Lp, _JtJ_Parent, _JtJ_Lnz; + std::vector _perm_JtJ, _invPerm_JtJ; + + CCS_Matrix _JtJ; + MatrixArray _Ui, _Vj; + Matrix _X, _Y, _Z; // X = A^t*C, Y = B^t*C, Z = C^t*C + }; // end struct ExtSparseLevenbergOptimizer + +# endif // defined(V3DLIB_ENABLE_SUITESPARSE) + +//********************************************************************** + + enum ConstraintType + { + LP_EQUAL, + LP_LESS_EQUAL, + LP_GREATER_EQUAL + }; + + enum LP_SolverType + { + LP_LPSOLVE55, + LP_DSDP + }; + + struct LP_Configuration + { + LP_Configuration() + : solverType(LP_LPSOLVE55), + verbose(false), maximize(false), useInitialValue(false) + { } + + LP_SolverType solverType; + bool verbose, maximize, useInitialValue; + }; // end struct LP_Configuration + + bool solveLinearProgram(std::vector const& costs, + CCS_Matrix const& A, + std::vector const& constraintTypes, + std::vector const& b, + std::vector > const& lowerBounds, + std::vector > const& upperBounds, + std::vector const& nonNegativeVariables, + std::vector& result, LP_Configuration const& conf = LP_Configuration()); + +} // end namespace V3D + +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_optimization_lm.cpp b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_optimization_lm.cpp new file mode 100644 index 0000000..bfdb7d8 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/v3d_optimization_lm.cpp @@ -0,0 +1,1021 @@ +#include "Math/v3d_optimization.h" + +#if defined(V3DLIB_ENABLE_SUITESPARSE) +//# include "COLAMD/Include/colamd.h" +# include "colamd.h" +# if 0 +extern "C" +{ +//# include "LDL/Include/ldl.h" +# include "ldl.h" +} +# else +# include "Math/v3d_ldl_private.h" +# endif +#endif + +#include +#include + +//#define USE_MULTIPLICATIVE_UPDATE 1 + +using namespace std; + + +namespace V3D +{ + +#if defined(V3DLIB_ENABLE_SUITESPARSE) + + void + SparseLM_CostFunction::fillAllJacobians() + { + int const nVaryingA = _paramInfo.nVaryingA; + int const nVaryingB = _paramInfo.nVaryingB; + int const nVaryingC = _paramInfo.nVaryingC; + + for (int k = 0; k < _nMeasurements; ++k) + { + int const i = _correspondingParamA[k]; + int const j = _correspondingParamB[k]; + + if (i < _paramInfo.nNonvaryingA && j < _paramInfo.nNonvaryingB) continue; + + fillJacobians(_Ak[k], _Bk[k], _Ck[k], i, j, k); + + // Clear the Jacobians for locked parameters. + if (i < _paramInfo.nNonvaryingA) makeZeroMatrix(_Ak[k]); + if (j < _paramInfo.nNonvaryingB) makeZeroMatrix(_Bk[k]); + + if (_paramInfo.nNonvaryingC > 0) + { + for (int r = 0; r < _measurementDimension; ++r) + for (int c = 0; c < _paramInfo.nNonvaryingC; ++c) _Ck[k][r][c] = 0.0; + } + } // end for (k) + + if (nVaryingA > 0) + { + for (int k = 0; k < _nMeasurements; ++k) + scaleMatrixIP(_weights[k], _Ak[k]); + } + if (nVaryingB > 0) + { + for (int k = 0; k < _nMeasurements; ++k) + scaleMatrixIP(_weights[k], _Bk[k]); + } + if (nVaryingC > 0) + { + for (int k = 0; k < _nMeasurements; ++k) + scaleMatrixIP(_weights[k], _Ck[k]); + } + } // end SparseLM_CostFunction::fillAllJacobians() + + void + SparseLM_CostFunction::fillHessian(std::vector const& jointIndexW, + MatrixArray& Ui, MatrixArray& Vj, + MatrixArray& Wn, + Matrix& X, Matrix& Y, Matrix& Z) + { + // The lhs J^t*J consists of several parts: + // [ U W X ] + // J^t*J = [ W^t V Y ] + // [ X^t Y^t Z ], + // where U, V and W are block-sparse matrices (due to the sparsity of A and B). + // X, Y and Z contain only a few columns (the number of global parameters). + + int const nParametersA = _paramInfo.nParametersA; + int const nParametersB = _paramInfo.nParametersB; + int const nNonvaryingA = _paramInfo.nNonvaryingA; + int const nNonvaryingB = _paramInfo.nNonvaryingB; + int const nNonvaryingC = _paramInfo.nNonvaryingC; + int const paramDimensionA = _paramInfo.paramDimensionA; + int const paramDimensionB = _paramInfo.paramDimensionB; + int const paramDimensionC = _paramInfo.paramDimensionC; + int const nVaryingA = nParametersA - nNonvaryingA; + int const nVaryingB = nParametersB - nNonvaryingB; + int const nVaryingC = paramDimensionC - nNonvaryingC; + + if (nVaryingA > 0) + { + // Compute Ui + Matrix U(paramDimensionA, paramDimensionA); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const i = _correspondingParamA[k] - nNonvaryingA; + if (i < 0) continue; + multiply_At_A(_Ak[k], U); + addMatricesIP(U, Ui[i]); + } // end for (k) + } // end if + + if (nVaryingB > 0) + { + // Compute Vj + Matrix V(paramDimensionB, paramDimensionB); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const j = _correspondingParamB[k] - nNonvaryingB; + if (j < 0) continue; + multiply_At_A(_Bk[k], V); + addMatricesIP(V, Vj[j]); + } // end for (k) + } // end if + + if (nVaryingC > 0) + { + Matrix ZZ(paramDimensionC, paramDimensionC); + Matrix Zsum(paramDimensionC, paramDimensionC); + + makeZeroMatrix(Zsum); + + for (int k = 0; k < _nMeasurements; ++k) + { + multiply_At_A(_Ck[k], ZZ); + addMatricesIP(ZZ, Zsum); + } // end for (k) + + // Ignore the non-varying parameters + for (int i = 0; i < nVaryingC; ++i) + for (int j = 0; j < nVaryingC; ++j) + Z[i][j] += Zsum[i+nNonvaryingC][j+nNonvaryingC]; + } // end if + + if (nVaryingA > 0 && nVaryingB > 0) + { + Matrix W(paramDimensionA, paramDimensionB); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const n = jointIndexW[k]; + if (n >= 0) + { + multiply_At_B(_Ak[k], _Bk[k], W); + addMatricesIP(W, Wn[n]); + } // end if + } // end for (k) + } // end if + + if (nVaryingA > 0 && nVaryingC > 0) + { + Matrix XX(paramDimensionA, paramDimensionC); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const i = _correspondingParamA[k] - nNonvaryingA; + // Ignore the non-varying parameters + if (i < 0) continue; + + multiply_At_B(_Ak[k], _Ck[k], XX); + + for (int r = 0; r < paramDimensionA; ++r) + for (int c = 0; c < nVaryingC; ++c) + X[r+i*paramDimensionA][c] += XX[r][c+nNonvaryingC]; + } // end for (k) + } // end if + + if (nVaryingB > 0 && nVaryingC > 0) + { + Matrix YY(paramDimensionB, paramDimensionC); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const j = _correspondingParamB[k] - nNonvaryingB; + // Ignore the non-varying parameters + if (j < 0) continue; + + multiply_At_B(_Bk[k], _Ck[k], YY); + + for (int r = 0; r < paramDimensionB; ++r) + for (int c = 0; c < nVaryingC; ++c) + Y[r+j*paramDimensionB][c] += YY[r][c+nNonvaryingC]; + } // end for (k) + } // end if + } // end SparseLM_CostFunction::fillHessian() + + void + SparseLM_CostFunction::evalJt_e(VectorArray& At_e, VectorArray& Bt_e, Vector& Ct_e) + { + int const nParametersA = _paramInfo.nParametersA; + int const nParametersB = _paramInfo.nParametersB; + int const nNonvaryingA = _paramInfo.nNonvaryingA; + int const nNonvaryingB = _paramInfo.nNonvaryingB; + int const nNonvaryingC = _paramInfo.nNonvaryingC; + int const paramDimensionA = _paramInfo.paramDimensionA; + int const paramDimensionB = _paramInfo.paramDimensionB; + int const paramDimensionC = _paramInfo.paramDimensionC; + int const nVaryingA = nParametersA - nNonvaryingA; + int const nVaryingB = nParametersB - nNonvaryingB; + int const nVaryingC = paramDimensionC - nNonvaryingC; + + // Compute the different parts of J^t*e + if (nVaryingA > 0) + { + Vector tmp(paramDimensionA); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const i = _correspondingParamA[k] - nNonvaryingA; + if (i < 0) continue; + multiply_At_v(_Ak[k], _residuals[k], tmp); + addVectors(tmp, At_e[i], At_e[i]); + } // end for (k) + } // end if + + if (nVaryingB > 0) + { + Vector tmp(paramDimensionB); + + for (int k = 0; k < _nMeasurements; ++k) + { + int const j = _correspondingParamB[k] - nNonvaryingB; + if (j < 0) continue; + multiply_At_v(_Bk[k], _residuals[k], tmp); + addVectors(tmp, Bt_e[j], Bt_e[j]); + } // end for (k) + } // end if + + if (nVaryingC > 0) + { + Vector tmp(paramDimensionC); + + for (int k = 0; k < _nMeasurements; ++k) + { + multiply_At_v(_Ck[k], _residuals[k], tmp); + for (int l = 0; l < nVaryingC; ++l) Ct_e[l] += tmp[nNonvaryingC + l]; + } + } // end if + } // end SparseLM_CostFunction::evalJt_e() + +//====================================================================== + + void + ExtSparseLevenbergOptimizer::setupSparseJtJ() + { + int const nParametersA = _paramInfo.nParametersA; + int const nParametersB = _paramInfo.nParametersB; + int const nNonvaryingA = _paramInfo.nNonvaryingA; + int const nNonvaryingB = _paramInfo.nNonvaryingB; + int const nNonvaryingC = _paramInfo.nNonvaryingC; + int const paramDimensionA = _paramInfo.paramDimensionA; + int const paramDimensionB = _paramInfo.paramDimensionB; + int const paramDimensionC = _paramInfo.paramDimensionC; + int const nVaryingA = nParametersA - nNonvaryingA; + int const nVaryingB = nParametersB - nNonvaryingB; + int const nVaryingC = paramDimensionC - nNonvaryingC; + + int const bColumnStart = nVaryingA*paramDimensionA; + int const cColumnStart = bColumnStart + nVaryingB*paramDimensionB; + int const nColumns = cColumnStart + nVaryingC; + + int const nObjs = _costFunctions.size(); + + _jointIndexW.resize(nObjs); + _jointNonzerosW.clear(); + + map, int> jointNonzeroMap; + + for (int obj = 0; obj < nObjs; ++obj) + { + SparseLM_CostFunction& costFun = *_costFunctions[obj]; + vector& jointIndexW = _jointIndexW[obj]; + jointIndexW.resize(costFun._nMeasurements); + + for (int k = 0; k < costFun._nMeasurements; ++k) + { + int const i = costFun._correspondingParamA[k] - nNonvaryingA; + int const j = costFun._correspondingParamB[k] - nNonvaryingB; + if (i >= 0 && j >= 0) + { + map, int>::const_iterator p = jointNonzeroMap.find(make_pair(i, j)); + if (p == jointNonzeroMap.end()) + { + jointNonzeroMap.insert(make_pair(make_pair(i, j), _jointNonzerosW.size())); + jointIndexW[k] = _jointNonzerosW.size(); + _jointNonzerosW.push_back(make_pair(i, j)); + } + else + jointIndexW[k] = (*p).second; + } // end if + } // end for (k) + } // end for (obj) + + int const bBlockColumnStart = nVaryingA; + int const cBlockColumnStart = bBlockColumnStart + nVaryingB; + + // If there are C parameters to optimize over, this counts as one column in the block representation + int const nBlockColumns = cBlockColumnStart + ((nVaryingC > 0) ? 1 : 0); + //cout << "nBlockColumns = " << nBlockColumns << endl; + + // For the column reordering we treat the columns belonging to one set + // of parameters as one (logical) column. + + // Determine non-zeros of JtJ (we forget about the non-zero diagonal for now) + // Only consider nonzeros of Ai^t * Bj induced by the measurements. + vector > nz_blockJtJ(_jointNonzerosW.size()); + for (size_t k = 0; k < _jointNonzerosW.size(); ++k) + { + nz_blockJtJ[k].first = _jointNonzerosW[k].second + bBlockColumnStart; + nz_blockJtJ[k].second = _jointNonzerosW[k].first; + } + + if (nVaryingC > 0) + { + // We assume, that the global unknowns are linked to every other variable. + for (int i = 0; i < nVaryingA; ++i) + nz_blockJtJ.push_back(make_pair(cBlockColumnStart, i)); + for (int j = 0; j < nVaryingB; ++j) + nz_blockJtJ.push_back(make_pair(cBlockColumnStart, j + bBlockColumnStart)); + } // end if + + int const nnzBlock = nz_blockJtJ.size(); + + vector permBlockJtJ(nBlockColumns + 1); + + if (nnzBlock > 0) + { +// cout << "nnzBlock = " << nnzBlock << endl; + + CCS_Matrix blockJtJ(nBlockColumns, nBlockColumns, nz_blockJtJ); + +// cout << " nz_blockJtJ: " << endl; +// for (size_t k = 0; k < nz_blockJtJ.size(); ++k) +// cout << " " << nz_blockJtJ[k].first << ":" << nz_blockJtJ[k].second << endl; +// cout << endl; + + int * colStarts = (int *)blockJtJ.getColumnStarts(); + int * rowIdxs = (int *)blockJtJ.getRowIndices(); + +// cout << "blockJtJ_colStarts = "; +// for (int k = 0; k <= nBlockColumns; ++k) cout << colStarts[k] << " "; +// cout << endl; + +// cout << "blockJtJ_rowIdxs = "; +// for (int k = 0; k < nnzBlock; ++k) cout << rowIdxs[k] << " "; +// cout << endl; + + int stats[COLAMD_STATS]; + symamd(nBlockColumns, rowIdxs, colStarts, &permBlockJtJ[0], (double *) NULL, stats, &calloc, &free); + if (optimizerVerbosenessLevel >= 2) symamd_report(stats); + } + else + for (size_t k = 0; k < permBlockJtJ.size(); ++k) permBlockJtJ[k] = k; + +// cout << "permBlockJtJ = "; +// for (int k = 0; k < permBlockJtJ.size(); ++k) +// cout << permBlockJtJ[k] << " "; +// cout << endl; + + // From the determined symbolic permutation with logical variables, determine the actual ordering + _perm_JtJ.resize(nVaryingA*paramDimensionA + nVaryingB*paramDimensionB + nVaryingC + 1); + + int curDstCol = 0; + for (size_t k = 0; k < permBlockJtJ.size()-1; ++k) + { + int const srcCol = permBlockJtJ[k]; + if (srcCol < nVaryingA) + { + for (int n = 0; n < paramDimensionA; ++n) + _perm_JtJ[curDstCol + n] = srcCol*paramDimensionA + n; + curDstCol += paramDimensionA; + } + else if (srcCol >= bBlockColumnStart && srcCol < cBlockColumnStart) + { + int const bStart = nVaryingA*paramDimensionA; + int const j = srcCol - bBlockColumnStart; + + for (int n = 0; n < paramDimensionB; ++n) + _perm_JtJ[curDstCol + n] = bStart + j*paramDimensionB + n; + curDstCol += paramDimensionB; + } + else if (srcCol == cBlockColumnStart) + { + int const cStart = nVaryingA*paramDimensionA + nVaryingB*paramDimensionB; + + for (int n = 0; n < nVaryingC; ++n) + _perm_JtJ[curDstCol + n] = cStart + n; + curDstCol += nVaryingC; + } + else + throwV3DErrorHere("setupJtJ(): column out of bounds"); + } // end for (k) + _perm_JtJ.back() = _perm_JtJ.size() - 1; + +// cout << "_perm_JtJ = "; +// for (int k = 0; k < _perm_JtJ.size(); ++k) cout << _perm_JtJ[k] << " "; +// cout << endl; + + // Finally, compute the inverse of the full permutation. + _invPerm_JtJ.resize(_perm_JtJ.size()); + for (size_t k = 0; k < _perm_JtJ.size(); ++k) _invPerm_JtJ[_perm_JtJ[k]] = k; + + vector > nz_JtJ; + this->serializeNonZerosJtJ(nz_JtJ); + + for (size_t k = 0; k < nz_JtJ.size(); ++k) + { + int const i = nz_JtJ[k].first; + int const j = nz_JtJ[k].second; + + int pi = _invPerm_JtJ[i]; + int pj = _invPerm_JtJ[j]; + // Swap values if in lower triangular part + if (pi > pj) std::swap(pi, pj); + nz_JtJ[k].first = pi; + nz_JtJ[k].second = pj; + } + + _JtJ.create(nColumns, nColumns, nz_JtJ); + + vector workFlags(nColumns); + + _JtJ_Lp.resize(nColumns+1); + _JtJ_Parent.resize(nColumns); + _JtJ_Lnz.resize(nColumns); + + LDL_symbolic(nColumns, (int *)_JtJ.getColumnStarts(), (int *)_JtJ.getRowIndices(), + &_JtJ_Lp[0], &_JtJ_Parent[0], &_JtJ_Lnz[0], + &workFlags[0]); + + if (optimizerVerbosenessLevel >= 1) + cout << "ExtSparseLevenbergOptimizer: Nonzeros in LDL decomposition: " << _JtJ_Lp[nColumns] << endl; + } // end ExtSparseLevenbergOptimizer::setupSparseJtJ() + + void + ExtSparseLevenbergOptimizer::serializeNonZerosJtJ(vector >& dst) const + { + int const nParametersA = _paramInfo.nParametersA; + int const nParametersB = _paramInfo.nParametersB; + int const nNonvaryingA = _paramInfo.nNonvaryingA; + int const nNonvaryingB = _paramInfo.nNonvaryingB; + int const nNonvaryingC = _paramInfo.nNonvaryingC; + int const paramDimensionA = _paramInfo.paramDimensionA; + int const paramDimensionB = _paramInfo.paramDimensionB; + int const paramDimensionC = _paramInfo.paramDimensionC; + int const nVaryingA = nParametersA - nNonvaryingA; + int const nVaryingB = nParametersB - nNonvaryingB; + int const nVaryingC = paramDimensionC - nNonvaryingC; + + int const bColumnStart = nVaryingA*paramDimensionA; + int const cColumnStart = bColumnStart + nVaryingB*paramDimensionB; + + dst.clear(); + + // Add the diagonal block matrices (only the upper triangular part). + + // Ui submatrices of JtJ + for (int i = 0; i < nVaryingA; ++i) + { + int const i0 = i * paramDimensionA; + + for (int c = 0; c < paramDimensionA; ++c) + for (int r = 0; r <= c; ++r) + dst.push_back(make_pair(i0 + r, i0 + c)); + } + + // Vj submatrices of JtJ + for (int j = 0; j < nVaryingB; ++j) + { + int const j0 = j*paramDimensionB + bColumnStart; + + for (int c = 0; c < paramDimensionB; ++c) + for (int r = 0; r <= c; ++r) + dst.push_back(make_pair(j0 + r, j0 + c)); + } + + // Z submatrix of JtJ + for (int c = 0; c < nVaryingC; ++c) + for (int r = 0; r <= c; ++r) + dst.push_back(make_pair(cColumnStart + r, cColumnStart + c)); + + // Add the elements i and j linked by an observation k + // W submatrix of JtJ + for (size_t n = 0; n < _jointNonzerosW.size(); ++n) + { + int const i0 = _jointNonzerosW[n].first; + int const j0 = _jointNonzerosW[n].second; + int const r0 = i0*paramDimensionA; + int const c0 = j0*paramDimensionB + bColumnStart; + + for (int r = 0; r < paramDimensionA; ++r) + for (int c = 0; c < paramDimensionB; ++c) + dst.push_back(make_pair(r0 + r, c0 + c)); + } // end for (n) + + if (nVaryingC > 0) + { + // Finally, add the dense columns linking i (resp. j) with the global parameters. + // X submatrix of JtJ + for (int i = 0; i < nVaryingA; ++i) + { + int const i0 = i*paramDimensionA; + + for (int r = 0; r < paramDimensionA; ++r) + for (int c = 0; c < nVaryingC; ++c) + dst.push_back(make_pair(i0 + r, cColumnStart + c)); + } + + // Y submatrix of JtJ + for (int j = 0; j < nVaryingB; ++j) + { + int const j0 = j*paramDimensionB + bColumnStart; + + for (int r = 0; r < paramDimensionB; ++r) + for (int c = 0; c < nVaryingC; ++c) + dst.push_back(make_pair(j0 + r, cColumnStart + c)); + } + } // end if + } // end ExtSparseLevenbergOptimizer::serializeNonZerosJtJ() + + void + ExtSparseLevenbergOptimizer::fillJtJ(MatrixArray const& Ui, + MatrixArray const& Vj, + MatrixArray const& Wn, + Matrix const& Z, + Matrix const& X, + Matrix const& Y) + { + int const nParametersA = _paramInfo.nParametersA; + int const nParametersB = _paramInfo.nParametersB; + int const nNonvaryingA = _paramInfo.nNonvaryingA; + int const nNonvaryingB = _paramInfo.nNonvaryingB; + int const nNonvaryingC = _paramInfo.nNonvaryingC; + int const paramDimensionA = _paramInfo.paramDimensionA; + int const paramDimensionB = _paramInfo.paramDimensionB; + int const paramDimensionC = _paramInfo.paramDimensionC; + int const nVaryingA = nParametersA - nNonvaryingA; + int const nVaryingB = nParametersB - nNonvaryingB; + int const nVaryingC = paramDimensionC - nNonvaryingC; + + int const bColumnStart = nVaryingA*paramDimensionA; + //int const cColumnStart = bColumnStart + nVaryingB*_paramDimensionB; + + //int const nCols = _JtJ.num_cols(); + //int const nnz = _JtJ.getNonzeroCount(); + + // The following has to replicate the procedure as in serializeNonZerosJtJ() + + int serial = 0; + + double * values = _JtJ.getValues(); + int const * destIdxs = _JtJ.getDestIndices(); + + // Add the diagonal block matrices (only the upper triangular part). + + // Ui submatrices of JtJ + for (int i = 0; i < nVaryingA; ++i) + { + //int const i0 = i * _paramDimensionA; + + for (int c = 0; c < paramDimensionA; ++c) + for (int r = 0; r <= c; ++r, ++serial) + values[destIdxs[serial]] = Ui[i][r][c]; + } + + // Vj submatrices of JtJ + for (int j = 0; j < nVaryingB; ++j) + { + //int const j0 = j*_paramDimensionB + bColumnStart; + + for (int c = 0; c < paramDimensionB; ++c) + for (int r = 0; r <= c; ++r, ++serial) + values[destIdxs[serial]] = Vj[j][r][c]; + } + + // Z submatrix of JtJ + for (int c = 0; c < nVaryingC; ++c) + for (int r = 0; r <= c; ++r, ++serial) + values[destIdxs[serial]] = Z[r][c]; + + // Add the elements i and j linked by an observation k + // W submatrix of JtJ + for (size_t n = 0; n < _jointNonzerosW.size(); ++n) + { + for (int r = 0; r < paramDimensionA; ++r) + for (int c = 0; c < paramDimensionB; ++c, ++serial) + values[destIdxs[serial]] = Wn[n][r][c]; + } // end for (k) + + if (nVaryingC > 0) + { + // Finally, add the dense columns linking i (resp. j) with the global parameters. + // X submatrix of JtJ + for (int i = 0; i < nVaryingA; ++i) + { + int const r0 = i * paramDimensionA; + for (int r = 0; r < paramDimensionA; ++r) + for (int c = 0; c < nVaryingC; ++c, ++serial) + values[destIdxs[serial]] = X[r0+r][c]; + } + + // Y submatrix of JtJ + for (int j = 0; j < nVaryingB; ++j) + { + int const r0 = j * paramDimensionB; + for (int r = 0; r < paramDimensionB; ++r) + for (int c = 0; c < nVaryingC; ++c, ++serial) + values[destIdxs[serial]] = Y[r0+r][c]; + } + } // end if + } // end ExtSparseLevenbergOptimizer::fillJtJ() + + void + ExtSparseLevenbergOptimizer::minimize() + { + status = LEVENBERG_OPTIMIZER_TIMEOUT; + bool computeDerivatives = true; + + int const nParametersA = _paramInfo.nParametersA; + int const nParametersB = _paramInfo.nParametersB; + int const nNonvaryingA = _paramInfo.nNonvaryingA; + int const nNonvaryingB = _paramInfo.nNonvaryingB; + int const nNonvaryingC = _paramInfo.nNonvaryingC; + int const paramDimensionA = _paramInfo.paramDimensionA; + int const paramDimensionB = _paramInfo.paramDimensionB; + int const paramDimensionC = _paramInfo.paramDimensionC; + int const nVaryingA = nParametersA - nNonvaryingA; + int const nVaryingB = nParametersB - nNonvaryingB; + int const nVaryingC = paramDimensionC - nNonvaryingC; + + if (nVaryingA == 0 && nVaryingB == 0 && nVaryingC == 0) + { + // No degrees of freedom, nothing to optimize. + status = LEVENBERG_OPTIMIZER_CONVERGED; + return; + } + + this->setupSparseJtJ(); + + // Wn = Ak^t*Bk + MatrixArray Wn(_jointNonzerosW.size(), paramDimensionA, paramDimensionB); + + //VectorArray residuals2(_nMeasurements, _measurementDimension); + + VectorArray diagUi(nVaryingA, paramDimensionA); + VectorArray diagVj(nVaryingB, paramDimensionB); + Vector diagZ(nVaryingC); + + VectorArray At_e(nVaryingA, paramDimensionA); + VectorArray Bt_e(nVaryingB, paramDimensionB); + Vector Ct_e(nVaryingC); + + Vector Jt_e(nVaryingA*paramDimensionA + nVaryingB*paramDimensionB + nVaryingC); + + Vector delta(nVaryingA*paramDimensionA + nVaryingB*paramDimensionB + nVaryingC); + Vector deltaPerm(nVaryingA*paramDimensionA + nVaryingB*paramDimensionB + nVaryingC); + + VectorArray deltaAi(nParametersA, paramDimensionA); + VectorArray deltaBj(nParametersB, paramDimensionB); + Vector deltaC(paramDimensionC); + + double err = 0.0; + + int const nObjs = _costFunctions.size(); + + for (currentIteration = 0; currentIteration < maxIterations; ++currentIteration) + { + if (optimizerVerbosenessLevel >= 2) + cout << "ExtSparseLevenbergOptimizer: currentIteration: " << currentIteration << endl; + if (computeDerivatives) + { + err = 0.0; + for (int obj = 0; obj < nObjs; ++obj) + { + SparseLM_CostFunction& costFun = *_costFunctions[obj]; + costFun.evalResidual(costFun._residuals); + costFun.fillWeights(costFun._residuals, costFun._weights); + for (int k = 0; k < costFun._nMeasurements; ++k) + scaleVectorIP(-costFun._weights[k], costFun._residuals[k]); + + err += squaredResidual(costFun._residuals); + } // end for (obj) + + if (optimizerVerbosenessLevel >= 1) cout << "ExtSparseLevenbergOptimizer: |residual|^2 = " << err << endl; + if (optimizerVerbosenessLevel >= 2) cout << "ExtSparseLevenbergOptimizer: lambda = " << lambda << endl; + + //for (size_t k = 0; k < residuals.count(); ++k) scaleVectorIP(-1.0, residuals[k]); + + for (int obj = 0; obj < nObjs; ++obj) + { + SparseLM_CostFunction& costFun = *_costFunctions[obj]; + costFun.setupJacobianGathering(); + costFun.fillAllJacobians(); + } // end for (obj) + + for (int i = 0; i < nVaryingA; ++i) makeZeroVector(At_e[i]); + for (int j = 0; j < nVaryingB; ++j) makeZeroVector(Bt_e[j]); + makeZeroVector(Ct_e); + + for (int obj = 0; obj < nObjs; ++obj) + _costFunctions[obj]->evalJt_e(At_e, Bt_e, Ct_e); + + int pos = 0; + for (int i = 0; i < nVaryingA; ++i) + for (int l = 0; l < paramDimensionA; ++l, ++pos) + Jt_e[pos] = At_e[i][l]; + for (int j = 0; j < nVaryingB; ++j) + for (int l = 0; l < paramDimensionB; ++l, ++pos) + Jt_e[pos] = Bt_e[j][l]; + for (int l = 0; l < nVaryingC; ++l, ++pos) + Jt_e[pos] = Ct_e[l]; + +// cout << "Jt_e = "; +// for (int k = 0; k < Jt_e.size(); ++k) cout << Jt_e[k] << " "; +// cout << endl; + + if (this->applyGradientStoppingCriteria(norm_Linf(Jt_e))) + { + status = LEVENBERG_OPTIMIZER_CONVERGED; + goto end; + } + + // The lhs J^t*J consists of several parts: + // [ U W X ] + // J^t*J = [ W^t V Y ] + // [ X^t Y^t Z ], + // where U, V and W are block-sparse matrices (due to the sparsity of A and B). + // X, Y and Z contain only a few columns (the number of global parameters). + for (int i = 0; i < nVaryingA; ++i) makeZeroMatrix(_Ui[i]); + for (int j = 0; j < nVaryingB; ++j) makeZeroMatrix(_Vj[j]); + for (size_t n = 0; n < Wn.count(); ++n) makeZeroMatrix(Wn[n]); + makeZeroMatrix(_X); + makeZeroMatrix(_Y); + makeZeroMatrix(_Z); + + for (int obj = 0; obj < nObjs; ++obj) + _costFunctions[obj]->fillHessian(_jointIndexW[obj], _Ui, _Vj, Wn, _X, _Y, _Z); + + if (currentIteration == 0) + { + // Initialize lambda as tau*max(JtJ[i][i]) + double maxEl = -1e30; + if (optimizerVerbosenessLevel < 2) + { + if (nVaryingA > 0) + { + for (int i = 0; i < nVaryingA; ++i) + for (int l = 0; l < paramDimensionA; ++l) + maxEl = std::max(maxEl, _Ui[i][l][l]); + } + if (nVaryingB > 0) + { + for (int j = 0; j < nVaryingB; ++j) + for (int l = 0; l < paramDimensionB; ++l) + maxEl = std::max(maxEl, _Vj[j][l][l]); + } + if (nVaryingC > 0) + { + for (int l = 0; l < nVaryingC; ++l) + maxEl = std::max(maxEl, _Z[l][l]); + } + } + else + { + char maxElSection = 'X'; + int maxElIndex = -1; + int maxElRow = -1; + + if (nVaryingA > 0) + { + for (int i = 0; i < nVaryingA; ++i) + for (int l = 0; l < paramDimensionA; ++l) + { + if (_Ui[i][l][l] > maxEl) + { + maxEl = _Ui[i][l][l]; + maxElSection = 'U'; maxElIndex = i; maxElRow = l; + } + } + } + if (nVaryingB > 0) + { + for (int j = 0; j < nVaryingB; ++j) + for (int l = 0; l < paramDimensionB; ++l) + { + if (_Vj[j][l][l] > maxEl) + { + maxEl = _Vj[j][l][l]; + maxElSection = 'V'; maxElIndex = j; maxElRow = l; + } + } + } + if (nVaryingC > 0) + { + for (int l = 0; l < nVaryingC; ++l) + { + if (_Z[l][l] > maxEl) + { + maxEl = _Z[l][l]; + maxElSection = 'Z'; maxElIndex = 0; maxElRow = l; + } + } + } + cout << "ExtSparseLevenbergOptimizer: initial lambda = " << tau*maxEl + << "; max diagonal element found at " << maxElSection << "[" << maxElIndex << "][" + << maxElRow << "]" << endl; + } // end if + + lambda = tau * maxEl; + } // end if (currentIteration == 0) + } // end if (computeDerivatives) + + for (int i = 0; i < nVaryingA; ++i) + { + for (int l = 0; l < paramDimensionA; ++l) diagUi[i][l] = _Ui[i][l][l]; + } // end for (i) + + for (int j = 0; j < nVaryingB; ++j) + { + for (int l = 0; l < paramDimensionB; ++l) diagVj[j][l] = _Vj[j][l][l]; + } // end for (j) + + for (int l = 0; l < nVaryingC; ++l) diagZ[l] = _Z[l][l]; + + // Augment the diagonals with lambda (either by the standard additive update or by multiplication). +#if !defined(USE_MULTIPLICATIVE_UPDATE) + for (int i = 0; i < nVaryingA; ++i) + for (int l = 0; l < paramDimensionA; ++l) + _Ui[i][l][l] += lambda; + + for (int j = 0; j < nVaryingB; ++j) + for (int l = 0; l < paramDimensionB; ++l) + _Vj[j][l][l] += lambda; + + for (int l = 0; l < nVaryingC; ++l) + _Z[l][l] += lambda; +#else + for (int i = 0; i < nVaryingA; ++i) + for (unsigned l = 0; l < paramDimensionA; ++l) + _Ui[i][l][l] = std::max(_Ui[i][l][l] * (1.0 + lambda), 1e-15); + + for (int j = 0; j < nVaryingB; ++j) + for (unsigned l = 0; l < paramDimensionB; ++l) + _Vj[j][l][l] = std::max(_Vj[j][l][l] * (1.0 + lambda), 1e-15); + + for (unsigned l = 0; l < nVaryingC; ++l) + _Z[l][l] = std::max(_Z[l][l] * (1.0 + lambda), 1e-15); +#endif + + this->fillJtJ(_Ui, _Vj, Wn, _Z, _X, _Y); + + bool success = true; + double rho = 0.0; + { + int const nCols = _JtJ_Parent.size(); + //int const nnz = _JtJ.getNonzeroCount(); + int const lnz = _JtJ_Lp.back(); + + vector Li(lnz); + vector Lx(lnz); + vector D(nCols), Y(nCols); + vector workPattern(nCols), workFlag(nCols); + + int * colStarts = (int *)_JtJ.getColumnStarts(); + int * rowIdxs = (int *)_JtJ.getRowIndices(); + double * values = _JtJ.getValues(); + + int const d = LDL_numeric(nCols, colStarts, rowIdxs, values, + &_JtJ_Lp[0], &_JtJ_Parent[0], &_JtJ_Lnz[0], + &Li[0], &Lx[0], &D[0], + &Y[0], &workPattern[0], &workFlag[0]); + + if (d == nCols) + { + LDL_perm(nCols, &deltaPerm[0], &Jt_e[0], &_perm_JtJ[0]); + LDL_lsolve(nCols, &deltaPerm[0], &_JtJ_Lp[0], &Li[0], &Lx[0]); + LDL_dsolve(nCols, &deltaPerm[0], &D[0]); + LDL_ltsolve(nCols, &deltaPerm[0], &_JtJ_Lp[0], &Li[0], &Lx[0]); + LDL_permt(nCols, &delta[0], &deltaPerm[0], &_perm_JtJ[0]); + } + else + { + if (optimizerVerbosenessLevel >= 2) + cout << "ExtSparseLevenbergOptimizer: LDL decomposition failed. Increasing lambda." << endl; + success = false; + } + } + + if (success) + { + double const deltaSqrLength = sqrNorm_L2(delta); + double const paramLength = this->getParameterLength(); + + if (optimizerVerbosenessLevel >= 2) + cout << "ExtSparseLevenbergOptimizer: ||delta|| = " << sqrt(deltaSqrLength) << " ||paramLength|| = " << paramLength << endl; + + if (this->applyUpdateStoppingCriteria(paramLength, sqrt(deltaSqrLength))) + { + status = LEVENBERG_OPTIMIZER_SMALL_UPDATE; + goto end; + } + + // Copy the updates from delta to the respective arrays + int pos = 0; + + for (int i = 0; i < nNonvaryingA; ++i) makeZeroVector(deltaAi[i]); + for (int i = nNonvaryingA; i < nParametersA; ++i) + for (int l = 0; l < paramDimensionA; ++l, ++pos) + deltaAi[i][l] = delta[pos]; + + for (int j = 0; j < nNonvaryingB; ++j) makeZeroVector(deltaBj[j]); + for (int j = nNonvaryingB; j < nParametersB; ++j) + for (int l = 0; l < paramDimensionB; ++l, ++pos) + deltaBj[j][l] = delta[pos]; + + makeZeroVector(deltaC); + for (int l = nNonvaryingC; l < paramDimensionC; ++l, ++pos) + deltaC[l] = delta[pos]; + + saveAllParameters(); + if (nVaryingA > 0) updateParametersA(deltaAi); + if (nVaryingB > 0) updateParametersB(deltaBj); + if (nVaryingC > 0) updateParametersC(deltaC); + + if (optimizerVerbosenessLevel >= 2) + cout << "ExtSparseLevenbergOptimizer: |deltaAi|^2 = " << squaredResidual(deltaAi) << " |deltaBj|^2 = " << squaredResidual(deltaBj) << endl; + + // cout << "deltaAi = "; + // for (int i = 0; i < nParametersA; ++i) + // displayVector(deltaAi[i]); + + double newErr = 0.0; + for (int obj = 0; obj < nObjs; ++obj) + { + SparseLM_CostFunction& costFun = *_costFunctions[obj]; + costFun.evalResidual(costFun._residuals); + costFun.fillWeights(costFun._residuals, costFun._weights); + for (int k = 0; k < costFun._nMeasurements; ++k) + scaleVectorIP(costFun._weights[k], costFun._residuals[k]); + + newErr += squaredResidual(costFun._residuals); + } // end for (obj) + + rho = err - newErr; + if (optimizerVerbosenessLevel >= 2) + cout << "ExtSparseLevenbergOptimizer: |new residual|^2 = " << newErr << " rho = " << rho << endl; + +#if !defined(USE_MULTIPLICATIVE_UPDATE) + double const denom1 = lambda * deltaSqrLength; +#else + double denom1 = 0.0f; + for (int i = nNonvaryingA; i < nParametersA; ++i) + for (int l = 0; l < paramDimensionA; ++l) + denom1 += deltaAi[i][l] * deltaAi[i][l] * diagUi[i-nNonvaryingA][l]; + + for (int j = nNonvaryingB; j < nParametersB; ++j) + for (int l = 0; l < paramDimensionB; ++l) + denom1 += deltaBj[j][l] * deltaBj[j][l] * diagVj[j-nNonvaryingB][l]; + + for (int l = nNonvaryingC; l < paramDimensionC; ++l) + denom1 += deltaC[l] * deltaC[l] * diagZ[l-nNonvaryingC]; + + denom1 *= lambda; +#endif + double const denom2 = innerProduct(delta, Jt_e); + rho = rho / (denom1 + denom2); + if (optimizerVerbosenessLevel >= 2) + cout << "ExtSparseLevenbergOptimizer: rho = " << rho + << " denom1 = " << denom1 << " denom2 = " << denom2 << endl; + } // end if (success) + + if (success && rho > 0) + { + if (optimizerVerbosenessLevel >= 2) + cout << "ExtSparseLevenbergOptimizer: Improved solution - decreasing lambda." << endl; + // Improvement in the new solution + decreaseLambda(rho); + computeDerivatives = true; + } + else + { + if (optimizerVerbosenessLevel >= 2) + cout << "ExtSparseLevenbergOptimizer: Inferior solution - increasing lambda." << endl; + restoreAllParameters(); + increaseLambda(); + computeDerivatives = false; + + // Restore diagonal elements in Ui, Vj and Z. + for (int i = 0; i < nVaryingA; ++i) + { + for (int l = 0; l < paramDimensionA; ++l) _Ui[i][l][l] = diagUi[i][l]; + } // end for (i) + + for (int j = 0; j < nVaryingB; ++j) + { + for (int l = 0; l < paramDimensionB; ++l) _Vj[j][l][l] = diagVj[j][l]; + } // end for (j) + + for (int l = 0; l < nVaryingC; ++l) _Z[l][l] = diagZ[l]; + } // end if + } // end for (currentIteration) + + end:; + if (optimizerVerbosenessLevel >= 2) + cout << "Leaving ExtSparseLevenbergOptimizer::minimize()." << endl; + } // end ExtSparseLevenbergOptimizer::minimize() + +#endif // defined(V3DLIB_ENABLE_SUITESPARSE) + +} // end namespace V3D diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/win32config.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/win32config.h new file mode 100644 index 0000000..e0c05ef --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/Math/win32config.h @@ -0,0 +1,108 @@ +#ifndef __LIBXML_WIN32_CONFIG__ +#define __LIBXML_WIN32_CONFIG__ + +#define HAVE_CTYPE_H +#define HAVE_STDARG_H +#define HAVE_MALLOC_H +#define HAVE_ERRNO_H + +#ifdef _WIN32_WCE +#undef HAVE_ERRNO_H +#include +#include "wincecompat.h" +#else +#define HAVE_SYS_STAT_H +#define HAVE__STAT +#define HAVE_STAT +#define HAVE_STDLIB_H +#define HAVE_TIME_H +#define HAVE_FCNTL_H +#include +#include +#endif + +#ifdef NEED_SOCKETS +#include +#endif + +#define HAVE_ISINF +#define HAVE_ISNAN +#include +#ifdef _MSC_VER +/* MS C-runtime has functions which can be used in order to determine if + a given floating-point variable contains NaN, (+-)INF. These are + preferred, because floating-point technology is considered propriatary + by MS and we can assume that their functions know more about their + oddities than we do. */ +#include +/* Bjorn Reese figured a quite nice construct for isinf() using the _fpclass + function. */ +#ifndef isinf +#define isinf(d) ((_fpclass(d) == _FPCLASS_PINF) ? 1 \ + : ((_fpclass(d) == _FPCLASS_NINF) ? -1 : 0)) +#endif +/* _isnan(x) returns nonzero if (x == NaN) and zero otherwise. */ +#ifndef isnan +#define isnan(d) (_isnan(d)) +#endif +#else /* _MSC_VER */ +#ifndef isinf +static int isinf (double d) { + int expon = 0; + double val = frexp (d, &expon); + if (expon == 1025) { + if (val == 0.5) { + return 1; + } else if (val == -0.5) { + return -1; + } else { + return 0; + } + } else { + return 0; + } +} +#endif +#ifndef isnan +static int isnan (double d) { + int expon = 0; + double val = frexp (d, &expon); + if (expon == 1025) { + if (val == 0.5) { + return 0; + } else if (val == -0.5) { + return 0; + } else { + return 1; + } + } else { + return 0; + } +} +#endif +#endif /* _MSC_VER */ + +#if defined(_MSC_VER) || defined(__MINGW32__) +#define mkdir(p,m) _mkdir(p) +#define snprintf _snprintf +#define vsnprintf(b,c,f,a) _vsnprintf(b,c,f,a) +#endif + +/* Threading API to use should be specified here for compatibility reasons. + This is however best specified on the compiler's command-line. */ +#if defined(LIBXML_THREAD_ENABLED) +#if !defined(HAVE_PTHREAD_H) && !defined(HAVE_WIN32_THREADS) +#define HAVE_WIN32_THREADS +#endif +#endif + +/* Some third-party libraries far from our control assume the following + is defined, which it is not if we don't include windows.h. */ +#if !defined(FALSE) +#define FALSE 0 +#endif +#if !defined(TRUE) +#define TRUE (!(FALSE)) +#endif + +#endif /* __LIBXML_WIN32_CONFIG__ */ diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/README.TXT b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/README.TXT new file mode 100644 index 0000000..569b7cb --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/README.TXT @@ -0,0 +1,108 @@ +Description + +This is an implementation of a sparse Levenberg-Marquardt optimization +procedure and several bundle adjustment modules based on it. There are three +versions of bundle adjustment: +1) Pure metric adjustment. Camera poses have 6 dof and 3D points have 3 dof. +2) Common, but adjustable intrinsic and distortion parameters. This is useful, + if the set of images are taken with the same camera under constant zoom + settings. +3) Variable intrinsics and distortion parameters for each view. This addresses + the "community photo collection" setting, where each image is captured with + a different camera and/or with varying zoom setting. + +There are two demo applications in the Apps directory, bundle_common and +bundle_varying, which correspond to item 2) and 3) above. + +The input data file for both applications is a text file with the following +numerical values: + +First, the number of 3D points, views and 2D measurements: + +Then, the values of the intrinsic matrix + [ fx skew cx ] +K = [ 0 fy cy ] + [ 0 0 1 ], +and the distortion parameters according to the convention of the Bouget +toolbox: + + + +For the bundle_varying application this is given times, one for each +camera/view. +Then the 3D point positions are given: + + + +Note: the point-ids need not to be exactly from 0 to M-1, any (unique) ids +will do. +The camera poses are given subsequently: + + <12 entries of the RT matrix> + +There is a lot of confusion how to specify the orientation of cameras. We use +projection matrix notation, i.e. P = K [R|T], and a 3D point X in world +coordinates is transformed into the camera coordinate system by XX=R*X+T. + +Finally, the 2d image measurements (given in pixels) are provided: + + 1 + +See the example in the Dataset folder. + + +Performance + +This software is able to perform successful loop closing for a video sequence +containing 1745 views, 37920 3D points and 627228 image measurements in about +16min on a 2.2 GHz Core 2. The footprint in memory was <700MB. + + +Requirements + +Solving the augmented normal equation in the LM optimizer is done with LDL, a +Cholsky like decomposition method for sparse matrices (see +http://www.cise.ufl.edu/research/sparse/ldl). The appropriate column +reordering is done with COLAMD (see +http://www.cise.ufl.edu/research/sparse/colamd). Both packages are licensed +under the GNU LGPL. + +This software was developed under Linux, but should compile equally well on +other operating systems. + +-Christopher Zach (chzach@inf.ethz.ch) + + +News for SSBA 2.0 + +* Added a sparse LM implementation (struct ExtSparseLevenbergOptimizer) + handling several least-squares terms in the cost function. This is useful + when several types of measurements (e.g. image feature locations and GPS + positions) are available. See Apps/bundle_ext_LM.cpp for a simple demo. + +* Changed the default update rule for the damping parameter lambda to a + simpler one (multiply and divide lambda by 10, depending on the cost + function improvement). This seems to work better that the more complicated + rule used before. + +* Fixed a trivial, but important bug in cost evaluation after the parameter + update. + +/* +Copyright (c) 2011 Christopher Zach, Computer Vision and Geometry Group, ETH Zurich + +This file is part of SSBA-2.0 (Simple Sparse Bundle Adjustment). + +SSBA is free software: you can redistribute it and/or modify it under the +terms of the GNU Lesser General Public License as published by the Free +Software Foundation, either version 3 of the License, or (at your option) any +later version. + +SSBA is distributed in the hope that it will be useful, but WITHOUT ANY +WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR +A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more +details. + +You should have received a copy of the GNU Lesser General Public License along +with SSBA. If not, see . +*/ diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/CMakeLists.txt b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/CMakeLists.txt new file mode 100644 index 0000000..2af03f4 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/CMakeLists.txt @@ -0,0 +1,10 @@ +project( SuiteSparse_config ) +cmake_minimum_required(VERSION 2.8) + +file(GLOB SRCS + *.h + *.c) + +add_library( SuiteSparse_config ${SRCS} ) + +set( SuiteSparse_config_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR} ) \ No newline at end of file diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/Makefile b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/Makefile new file mode 100644 index 0000000..6e7b35d --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/Makefile @@ -0,0 +1,43 @@ +#------------------------------------------------------------------------------- +# SuiteSparse_config Makefile +#------------------------------------------------------------------------------- + +VERSION = 4.0.0 + +default: ccode + +include SuiteSparse_config.mk + +ccode: libsuitesparseconfig.a + +all: libsuitesparseconfig.a + +library: libsuitesparseconfig.a + +libsuitesparseconfig.a: SuiteSparse_config.c SuiteSparse_config.h + $(CC) $(CF) -c SuiteSparse_config.c + $(ARCHIVE) libsuitesparseconfig.a SuiteSparse_config.o + $(RANLIB) libsuitesparseconfig.a + - $(RM) SuiteSparse_config.o + +distclean: purge + +purge: clean + - $(RM) *.o *.a + +clean: + - $(RM) -r $(CLEAN) + +# install SuiteSparse_config +install: + $(CP) libsuitesparseconfig.a $(INSTALL_LIB)/libsuitesparseconfig.$(VERSION).a + ( cd $(INSTALL_LIB) ; ln -sf libsuitesparseconfig.$(VERSION).a libsuitesparseconfig.a ) + $(CP) SuiteSparse_config.h $(INSTALL_INCLUDE) + chmod 644 $(INSTALL_LIB)/libsuitesparseconfig*.a + chmod 644 $(INSTALL_INCLUDE)/SuiteSparse_config.h + +# uninstall SuiteSparse_config +uninstall: + $(RM) $(INSTALL_LIB)/libsuitesparseconfig*.a + $(RM) $(INSTALL_INCLUDE)/SuiteSparse_config.h + diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/README.txt b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/README.txt new file mode 100644 index 0000000..2dbaacd --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/README.txt @@ -0,0 +1,44 @@ +SuiteSparse_config, 2012, Timothy A. Davis, http://www.suitesparse.com +(formerly the UFconfig package) + +SuiteSparse_config contains configuration settings for all many of the software +packages that I develop or co-author. Note that older versions of some of +these packages do not require SuiteSparse_config. + + Package Description + ------- ----------- + AMD approximate minimum degree ordering + CAMD constrained AMD + COLAMD column approximate minimum degree ordering + CCOLAMD constrained approximate minimum degree ordering + UMFPACK sparse LU factorization, with the BLAS + CXSparse int/long/real/complex version of CSparse + CHOLMOD sparse Cholesky factorization, update/downdate + KLU sparse LU factorization, BLAS-free + BTF permutation to block triangular form + LDL concise sparse LDL' + LPDASA LP Dual Active Set Algorithm + RBio read/write files in Rutherford/Boeing format + SPQR sparse QR factorization (full name: SuiteSparseQR) + +SuiteSparse_config is not required by these packages: + + CSparse a Concise Sparse matrix package + MATLAB_Tools toolboxes for use in MATLAB + +In addition, the xerbla/ directory contains Fortan and C versions of the +BLAS/LAPACK xerbla routine, which is called when an invalid input is passed to +the BLAS or LAPACK. The xerbla provided here does not print any message, so +the entire Fortran I/O library does not need to be linked into a C application. +Most versions of the BLAS contain xerbla, but those from K. Goto do not. Use +this if you need too. + +-------------------------------------------------------------------------------- +A note on the update to SuiteSparse Version 4.0.0: The SuiteSparse_long macro +defines an integer that is 64-bits in size on 64-bit platforms, and 32-bits on +32-bit platforms. It was formerly called UF_long, but UF_long has been removed +because of potential name conflicts. UF_long is still available to user codes, +but it can now be safely #undef'd in case of name conflicts in user code. +Future codes should use SuiteSparse_long in place of UF_long. +-------------------------------------------------------------------------------- + diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/SuiteSparse_config.c b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/SuiteSparse_config.c new file mode 100644 index 0000000..3a6d731 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/SuiteSparse_config.c @@ -0,0 +1,173 @@ +/* ========================================================================== */ +/* === SuiteSparse_config =================================================== */ +/* ========================================================================== */ + +/* Copyright (c) 2012, Timothy A. Davis. No licensing restrictions + * apply to this file or to the SuiteSparse_config directory. + * Author: Timothy A. Davis. + */ + +#include "SuiteSparse_config.h" + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_malloc: malloc wrapper */ +/* -------------------------------------------------------------------------- */ + +void *SuiteSparse_malloc /* pointer to allocated block of memory */ +( + size_t nitems, /* number of items to malloc (>=1 is enforced) */ + size_t size_of_item, /* sizeof each item */ + int *ok, /* TRUE if successful, FALSE otherwise */ + SuiteSparse_config *config /* SuiteSparse-wide configuration */ +) +{ + void *p ; + if (nitems < 1) nitems = 1 ; + if (nitems * size_of_item != ((double) nitems) * size_of_item) + { + /* Int overflow */ + *ok = 0 ; + return (NULL) ; + } + if (!config || config->malloc_memory == NULL) + { + /* use malloc by default */ + p = (void *) malloc (nitems * size_of_item) ; + } + else + { + /* use the pointer to malloc in the config */ + p = (void *) (config->malloc_memory) (nitems * size_of_item) ; + } + *ok = (p != NULL) ; + return (p) ; +} + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_free: free wrapper */ +/* -------------------------------------------------------------------------- */ + +void *SuiteSparse_free /* always returns NULL */ +( + void *p, /* block to free */ + SuiteSparse_config *config /* SuiteSparse-wide configuration */ +) +{ + if (p) + { + if (!config || config->free_memory == NULL) + { + /* use free by default */ + free (p) ; + } + else + { + /* use the pointer to free in the config */ + (config->free_memory) (p) ; + } + } + return (NULL) ; +} + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_tic: return current wall clock time */ +/* -------------------------------------------------------------------------- */ + +/* Returns the number of seconds (tic [0]) and nanoseconds (tic [1]) since some + * unspecified but fixed time in the past. If no timer is installed, zero is + * returned. A scalar double precision value for 'tic' could be used, but this + * might cause loss of precision because clock_getttime returns the time from + * some distant time in the past. Thus, an array of size 2 is used. + * + * The timer is enabled by default. To disable the timer, compile with + * -DNTIMER. If enabled on a POSIX C 1993 system, the timer requires linking + * with the -lrt library. + * + * example: + * + * double tic [2], r, s, t ; + * SuiteSparse_tic (tic) ; // start the timer + * // do some work A + * t = SuiteSparse_toc (tic) ; // t is time for work A, in seconds + * // do some work B + * s = SuiteSparse_toc (tic) ; // s is time for work A and B, in seconds + * SuiteSparse_tic (tic) ; // restart the timer + * // do some work C + * r = SuiteSparse_toc (tic) ; // s is time for work C, in seconds + * + * A double array of size 2 is used so that this routine can be more easily + * ported to non-POSIX systems. The caller does not rely on the POSIX + * include file. + */ + +#ifdef SUITESPARSE_TIMER_ENABLED + +#include + +void SuiteSparse_tic +( + double tic [2] /* output, contents undefined on input */ +) +{ + /* POSIX C 1993 timer, requires -librt */ + struct timespec t ; + clock_gettime (CLOCK_MONOTONIC, &t) ; + tic [0] = (double) (t.tv_sec) ; + tic [1] = (double) (t.tv_nsec) ; +} + +#else + +void SuiteSparse_tic +( + double tic [2] /* output, contents undefined on input */ +) +{ + /* no timer installed */ + tic [0] = 0 ; + tic [1] = 0 ; +} + +#endif + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_toc: return time since last tic */ +/* -------------------------------------------------------------------------- */ + +/* Assuming SuiteSparse_tic is accurate to the nanosecond, this function is + * accurate down to the nanosecond for 2^53 nanoseconds since the last call to + * SuiteSparse_tic, which is sufficient for SuiteSparse (about 104 days). If + * additional accuracy is required, the caller can use two calls to + * SuiteSparse_tic and do the calculations differently. + */ + +double SuiteSparse_toc /* returns time in seconds since last tic */ +( + double tic [2] /* input, not modified from last call to SuiteSparse_tic */ +) +{ + double toc [2] ; + SuiteSparse_tic (toc) ; + return ((toc [0] - tic [0]) + 1e-9 * (toc [1] - tic [1])) ; +} + + +/* -------------------------------------------------------------------------- */ +/* SuiteSparse_time: return current wallclock time in seconds */ +/* -------------------------------------------------------------------------- */ + +/* This function might not be accurate down to the nanosecond. */ + +double SuiteSparse_time /* returns current wall clock time in seconds */ +( + void +) +{ + double toc [2] ; + SuiteSparse_tic (toc) ; + return (toc [0] + 1e-9 * toc [1]) ; +} + diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/SuiteSparse_config.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/SuiteSparse_config.h new file mode 100644 index 0000000..26dd5a0 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/SuiteSparse_config.h @@ -0,0 +1,179 @@ +/* ========================================================================== */ +/* === SuiteSparse_config =================================================== */ +/* ========================================================================== */ + +/* Configuration file for SuiteSparse: a Suite of Sparse matrix packages + * (AMD, COLAMD, CCOLAMD, CAMD, CHOLMOD, UMFPACK, CXSparse, and others). + * + * SuiteSparse_config.h provides the definition of the long integer. On most + * systems, a C program can be compiled in LP64 mode, in which long's and + * pointers are both 64-bits, and int's are 32-bits. Windows 64, however, uses + * the LLP64 model, in which int's and long's are 32-bits, and long long's and + * pointers are 64-bits. + * + * SuiteSparse packages that include long integer versions are + * intended for the LP64 mode. However, as a workaround for Windows 64 + * (and perhaps other systems), the long integer can be redefined. + * + * If _WIN64 is defined, then the __int64 type is used instead of long. + * + * The long integer can also be defined at compile time. For example, this + * could be added to SuiteSparse_config.mk: + * + * CFLAGS = -O -D'SuiteSparse_long=long long' \ + * -D'SuiteSparse_long_max=9223372036854775801' -D'SuiteSparse_long_idd="lld"' + * + * This file defines SuiteSparse_long as either long (on all but _WIN64) or + * __int64 on Windows 64. The intent is that a SuiteSparse_long is always a + * 64-bit integer in a 64-bit code. ptrdiff_t might be a better choice than + * long; it is always the same size as a pointer. + * + * This file also defines the SUITESPARSE_VERSION and related definitions. + * + * Copyright (c) 2012, Timothy A. Davis. No licensing restrictions apply + * to this file or to the SuiteSparse_config directory. + * Author: Timothy A. Davis. + */ + +#ifndef _SUITESPARSECONFIG_H +#define _SUITESPARSECONFIG_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +/* ========================================================================== */ +/* === SuiteSparse_long ===================================================== */ +/* ========================================================================== */ + +#ifndef SuiteSparse_long + +#ifdef _WIN64 + +#define SuiteSparse_long __int64 +#define SuiteSparse_long_max _I64_MAX +#define SuiteSparse_long_idd "I64d" + +#else + +#define SuiteSparse_long long +#define SuiteSparse_long_max LONG_MAX +#define SuiteSparse_long_idd "ld" + +#endif +#define SuiteSparse_long_id "%" SuiteSparse_long_idd +#endif + +/* For backward compatibility with prior versions of SuiteSparse. The UF_* + * macros are deprecated and will be removed in a future version. */ +#ifndef UF_long +#define UF_long SuiteSparse_long +#define UF_long_max SuiteSparse_long_max +#define UF_long_idd SuiteSparse_long_idd +#define UF_long_id SuiteSparse_long_id +#endif + +/* ========================================================================== */ +/* === SuiteSparse_config parameters and functions ========================== */ +/* ========================================================================== */ + +/* SuiteSparse-wide parameters will be placed in this struct. */ + +typedef struct SuiteSparse_config_struct +{ + void *(*malloc_memory) (size_t) ; /* pointer to malloc */ + void *(*realloc_memory) (void *, size_t) ; /* pointer to realloc */ + void (*free_memory) (void *) ; /* pointer to free */ + void *(*calloc_memory) (size_t, size_t) ; /* pointer to calloc */ + +} SuiteSparse_config ; + +void *SuiteSparse_malloc /* pointer to allocated block of memory */ +( + size_t nitems, /* number of items to malloc (>=1 is enforced) */ + size_t size_of_item, /* sizeof each item */ + int *ok, /* TRUE if successful, FALSE otherwise */ + SuiteSparse_config *config /* SuiteSparse-wide configuration */ +) ; + +void *SuiteSparse_free /* always returns NULL */ +( + void *p, /* block to free */ + SuiteSparse_config *config /* SuiteSparse-wide configuration */ +) ; + +void SuiteSparse_tic /* start the timer */ +( + double tic [2] /* output, contents undefined on input */ +) ; + +double SuiteSparse_toc /* return time in seconds since last tic */ +( + double tic [2] /* input: from last call to SuiteSparse_tic */ +) ; + +double SuiteSparse_time /* returns current wall clock time in seconds */ +( + void +) ; + +/* determine which timer to use, if any */ +#ifndef NTIMER +#ifdef _POSIX_C_SOURCE +#if _POSIX_C_SOURCE >= 199309L +#define SUITESPARSE_TIMER_ENABLED +#endif +#endif +#endif + +/* ========================================================================== */ +/* === SuiteSparse version ================================================== */ +/* ========================================================================== */ + +/* SuiteSparse is not a package itself, but a collection of packages, some of + * which must be used together (UMFPACK requires AMD, CHOLMOD requires AMD, + * COLAMD, CAMD, and CCOLAMD, etc). A version number is provided here for the + * collection itself. The versions of packages within each version of + * SuiteSparse are meant to work together. Combining one packge from one + * version of SuiteSparse, with another package from another version of + * SuiteSparse, may or may not work. + * + * SuiteSparse contains the following packages: + * + * SuiteSparse_config version 4.0.0 (version always the same as SuiteSparse) + * AMD version 2.3.0 + * BTF version 1.2.0 + * CAMD version 2.3.0 + * CCOLAMD version 2.8.0 + * CHOLMOD version 2.0.0 + * COLAMD version 2.8.0 + * CSparse version 3.1.0 + * CXSparse version 3.1.0 + * KLU version 1.2.0 + * LDL version 2.1.0 + * RBio version 2.1.0 + * SPQR version 1.3.0 (full name is SuiteSparseQR) + * UMFPACK version 5.6.0 + * MATLAB_Tools various packages & M-files + * + * Other package dependencies: + * BLAS required by CHOLMOD and UMFPACK + * LAPACK required by CHOLMOD + * METIS 4.0.1 required by CHOLMOD (optional) and KLU (optional) + */ + +#define SUITESPARSE_DATE "Jun 1, 2012" +#define SUITESPARSE_VER_CODE(main,sub) ((main) * 1000 + (sub)) +#define SUITESPARSE_MAIN_VERSION 4 +#define SUITESPARSE_SUB_VERSION 0 +#define SUITESPARSE_SUBSUB_VERSION 0 +#define SUITESPARSE_VERSION \ + SUITESPARSE_VER_CODE(SUITESPARSE_MAIN_VERSION,SUITESPARSE_SUB_VERSION) + +#ifdef __cplusplus +} +#endif +#endif diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/SuiteSparse_config.mk b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/SuiteSparse_config.mk new file mode 100644 index 0000000..8fd72af --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/SuiteSparse_config.mk @@ -0,0 +1,392 @@ +#=============================================================================== +# SuiteSparse_config.mk: common configuration file for the SuiteSparse +#=============================================================================== + +# This file contains all configuration settings for all packages authored or +# co-authored by Tim Davis: +# +# Package Version Description +# ------- ------- ----------- +# AMD 1.2 or later approximate minimum degree ordering +# COLAMD 2.4 or later column approximate minimum degree ordering +# CCOLAMD 1.0 or later constrained column approximate minimum degree ordering +# CAMD any constrained approximate minimum degree ordering +# UMFPACK 4.5 or later sparse LU factorization, with the BLAS +# CHOLMOD any sparse Cholesky factorization, update/downdate +# KLU 0.8 or later sparse LU factorization, BLAS-free +# BTF 0.8 or later permutation to block triangular form +# LDL 1.2 or later concise sparse LDL' +# CXSparse any extended version of CSparse (int/long, real/complex) +# SuiteSparseQR any sparse QR factorization +# RBio 2.0 or later read/write sparse matrices in Rutherford-Boeing format +# +# By design, this file is NOT included in the CSparse makefile. +# That package is fully stand-alone. CSparse is primarily for teaching; +# production code should use CXSparse. +# +# The SuiteSparse_config directory and the above packages should all appear in +# a single directory, in order for the Makefile's within each package to find +# this file. +# +# To enable an option of the form "# OPTION = ...", edit this file and +# delete the "#" in the first column of the option you wish to use. +# +# The use of METIS 4.0.1 is optional. To exclude METIS, you must compile with +# CHOLMOD_CONFIG set to -DNPARTITION. See below for details. However, if you +# do not have a metis-4.0 directory inside the SuiteSparse directory, the +# */Makefile's that optionally rely on METIS will automatically detect this +# and compile without METIS. + +#------------------------------------------------------------------------------ +# Generic configuration +#------------------------------------------------------------------------------ + +# Using standard definitions from the make environment, typically: +# +# CC cc C compiler +# CXX g++ C++ compiler +# CFLAGS [ ] flags for C and C++ compiler +# CPPFLAGS [ ] flags for C and C++ compiler +# TARGET_ARCH [ ] target architecture +# FFLAGS [ ] flags for Fortran compiler +# RM rm -f delete a file +# AR ar create a static *.a library archive +# ARFLAGS rv flags for ar +# MAKE make make itself (sometimes called gmake) +# +# You can redefine them here, but by default they are used from the +# default make environment. + +# C and C++ compiler flags. The first three are standard for *.c and *.cpp +# Add -DNTIMER if you do use any timing routines (otherwise -lrt is required). +# CF = $(CFLAGS) $(CPPFLAGS) $(TARGET_ARCH) -O3 -fexceptions -fPIC -DNTIMER + CF = $(CFLAGS) $(CPPFLAGS) $(TARGET_ARCH) -O3 -fexceptions -fPIC + +# ranlib, and ar, for generating libraries. If you don't need ranlib, +# just change it to RANLAB = echo +RANLIB = ranlib +ARCHIVE = $(AR) $(ARFLAGS) + +# copy and delete a file +CP = cp -f +MV = mv -f + +# Fortran compiler (not required for 'make' or 'make library') +F77 = gfortran +F77FLAGS = $(FFLAGS) -O +F77LIB = + +# C and Fortran libraries. Remove -lrt if you don't have it. + LIB = -lm -lrt +# Using the following requires CF = ... -DNTIMER on POSIX C systems. +# LIB = -lm + +# For "make install" +INSTALL_LIB = /usr/local/lib +INSTALL_INCLUDE = /usr/local/include + +# Which version of MAKE you are using (default is "make") +# MAKE = make +# MAKE = gmake + +#------------------------------------------------------------------------------ +# BLAS and LAPACK configuration: +#------------------------------------------------------------------------------ + +# UMFPACK and CHOLMOD both require the BLAS. CHOLMOD also requires LAPACK. +# See Kazushige Goto's BLAS at http://www.cs.utexas.edu/users/flame/goto/ or +# http://www.tacc.utexas.edu/~kgoto/ for the best BLAS to use with CHOLMOD. +# LAPACK is at http://www.netlib.org/lapack/ . You can use the standard +# Fortran LAPACK along with Goto's BLAS to obtain very good performance. +# CHOLMOD gets a peak numeric factorization rate of 3.6 Gflops on a 3.2 GHz +# Pentium 4 (512K cache, 4GB main memory) with the Goto BLAS, and 6 Gflops +# on a 2.5Ghz dual-core AMD Opteron. + +# These settings will probably not work, since there is no fixed convention for +# naming the BLAS and LAPACK library (*.a or *.so) files. + +# This is probably slow ... it might connect to the Standard Reference BLAS: +BLAS = -lblas -lgfortran +LAPACK = -llapack + +# NOTE: this next option for the "Goto BLAS" has nothing to do with a "goto" +# statement. Rather, the Goto BLAS is written by Dr. Kazushige Goto. +# Using the Goto BLAS: +# BLAS = -lgoto -lgfortran -lgfortranbegin +# BLAS = -lgoto2 -lgfortran -lgfortranbegin -lpthread + +# Using non-optimized versions: +# BLAS = -lblas_plain -lgfortran -lgfortranbegin +# LAPACK = -llapack_plain + +# BLAS = -lblas_plain -lgfortran -lgfortranbegin +# LAPACK = -llapack + +# The BLAS might not contain xerbla, an error-handling routine for LAPACK and +# the BLAS. Also, the standard xerbla requires the Fortran I/O library, and +# stops the application program if an error occurs. A C version of xerbla +# distributed with this software (SuiteSparse_config/xerbla/libcerbla.a) +# includes a Fortran-callable xerbla routine that prints nothing and does not +# stop the application program. This is optional. + +# XERBLA = ../../SuiteSparse_config/xerbla/libcerbla.a + +# If you wish to use the XERBLA in LAPACK and/or the BLAS instead, +# use this option: +XERBLA = + +# If you wish to use the Fortran SuiteSparse_config/xerbla/xerbla.f instead, +# use this: + +# XERBLA = ../../SuiteSparse_config/xerbla/libxerbla.a + +#------------------------------------------------------------------------------ +# GPU configuration for CHOLMOD, using the CUDA BLAS +#------------------------------------------------------------------------------ + +# no cuda +GPU_BLAS_PATH = +GPU_CONFIG = + +# with cuda BLAS acceleration for CHOLMOD +# GPU_BLAS_PATH=/usr/local/cuda +# GPU_CONFIG=-DGPU_BLAS -I$(GPU_BLAS_PATH)/include + +#------------------------------------------------------------------------------ +# METIS, optionally used by CHOLMOD +#------------------------------------------------------------------------------ + +# If you do not have METIS, or do not wish to use it in CHOLMOD, you must +# compile CHOLMOD with the -DNPARTITION flag. + +# The path is relative to where it is used, in CHOLMOD/Lib, CHOLMOD/MATLAB, etc. +# You may wish to use an absolute path. METIS is optional. Compile +# CHOLMOD with -DNPARTITION if you do not wish to use METIS. +METIS_PATH = ../../metis-4.0 +METIS = ../../metis-4.0/libmetis.a + +#------------------------------------------------------------------------------ +# UMFPACK configuration: +#------------------------------------------------------------------------------ + +# Configuration flags for UMFPACK. See UMFPACK/Source/umf_config.h for details. +# +# -DNBLAS do not use the BLAS. UMFPACK will be very slow. +# -D'LONGBLAS=long' or -DLONGBLAS='long long' defines the integers used by +# LAPACK and the BLAS (defaults to 'int') +# -DNSUNPERF do not use the Sun Perf. Library (default is use it on Solaris) +# -DNRECIPROCAL do not multiply by the reciprocal +# -DNO_DIVIDE_BY_ZERO do not divide by zero +# -DNCHOLMOD do not use CHOLMOD as a ordering method. If -DNCHOLMOD is +# included in UMFPACK_CONFIG, then UMFPACK does not rely on +# CHOLMOD, CAMD, CCOLAMD, COLAMD, and METIS. + +UMFPACK_CONFIG = + +# uncomment this line to compile UMFPACK without CHOLMOD: +# UMFPACK_CONFIG = -DNCHOLMOD + +#------------------------------------------------------------------------------ +# CHOLMOD configuration +#------------------------------------------------------------------------------ + +# CHOLMOD Library Modules, which appear in libcholmod.a: +# Core requires: none +# Check requires: Core +# Cholesky requires: Core, AMD, COLAMD. optional: Partition, Supernodal +# MatrixOps requires: Core +# Modify requires: Core +# Partition requires: Core, CCOLAMD, METIS. optional: Cholesky +# Supernodal requires: Core, BLAS, LAPACK +# +# CHOLMOD test/demo Modules (all are GNU GPL, do not appear in libcholmod.a): +# Tcov requires: Core, Check, Cholesky, MatrixOps, Modify, Supernodal +# optional: Partition +# Valgrind same as Tcov +# Demo requires: Core, Check, Cholesky, MatrixOps, Supernodal +# optional: Partition +# +# Configuration flags: +# -DNCHECK do not include the Check module. License GNU LGPL +# -DNCHOLESKY do not include the Cholesky module. License GNU LGPL +# -DNPARTITION do not include the Partition module. License GNU LGPL +# also do not include METIS. +# -DNGPL do not include any GNU GPL Modules in the CHOLMOD library: +# -DNMATRIXOPS do not include the MatrixOps module. License GNU GPL +# -DNMODIFY do not include the Modify module. License GNU GPL +# -DNSUPERNODAL do not include the Supernodal module. License GNU GPL +# +# -DNPRINT do not print anything. +# -D'LONGBLAS=long' or -DLONGBLAS='long long' defines the integers used by +# LAPACK and the BLAS (defaults to 'int') +# -DNSUNPERF for Solaris only. If defined, do not use the Sun +# Performance Library + +CHOLMOD_CONFIG = $(GPU_CONFIG) + +# uncomment this line to compile CHOLMOD without METIS: +# CHOLMOD_CONFIG = -DNPARTITION + +#------------------------------------------------------------------------------ +# SuiteSparseQR configuration: +#------------------------------------------------------------------------------ + +# The SuiteSparseQR library can be compiled with the following options: +# +# -DNPARTITION do not include the CHOLMOD partition module +# -DNEXPERT do not include the functions in SuiteSparseQR_expert.cpp +# -DHAVE_TBB enable the use of Intel's Threading Building Blocks (TBB) + +# default, without timing, without TBB: +SPQR_CONFIG = +# with TBB: +# SPQR_CONFIG = -DHAVE_TBB + +# This is needed for IBM AIX: (but not for and C codes, just C++) +# SPQR_CONFIG = -DBLAS_NO_UNDERSCORE + +# with TBB, you must select this: +# TBB = -ltbb +# without TBB: +TBB = + +#------------------------------------------------------------------------------ +# Linux +#------------------------------------------------------------------------------ + +# Using default compilers: +# CC = gcc +# CF = $(CFLAGS) -O3 -fexceptions + +# alternatives: +# CF = $(CFLAGS) -g -fexceptions \ + -Wall -W -Wshadow -Wmissing-prototypes -Wstrict-prototypes \ + -Wredundant-decls -Wnested-externs -Wdisabled-optimization -ansi \ + -funit-at-a-time +# CF = $(CFLAGS) -O3 -fexceptions \ + -Wall -W -Werror -Wshadow -Wmissing-prototypes -Wstrict-prototypes \ + -Wredundant-decls -Wnested-externs -Wdisabled-optimization -ansi +# CF = $(CFLAGS) -O3 -fexceptions -D_FILE_OFFSET_BITS=64 -D_LARGEFILE64_SOURCE +# CF = $(CFLAGS) -O3 +# CF = $(CFLAGS) -O3 -g -fexceptions +# CF = $(CFLAGS) -g -fexceptions \ + -Wall -W -Wshadow \ + -Wredundant-decls -Wdisabled-optimization -ansi + +# consider: +# -fforce-addr -fmove-all-movables -freduce-all-givs -ftsp-ordering +# -frename-registers -ffast-math -funroll-loops + +# Using the Goto BLAS: +# BLAS = -lgoto -lfrtbegin -lg2c $(XERBLA) -lpthread + +# Using Intel's icc and ifort compilers: +# (does not work for mexFunctions unless you add a mexopts.sh file) +# F77 = ifort +# CC = icc +# CF = $(CFLAGS) -O3 -xN -vec_report=0 +# CF = $(CFLAGS) -g + +# 64bit: +# F77FLAGS = -O -m64 +# CF = $(CFLAGS) -O3 -fexceptions -m64 +# BLAS = -lgoto64 -lfrtbegin -lg2c -lpthread $(XERBLA) +# LAPACK = -llapack64 + +# SUSE Linux 10.1, AMD Opteron, with GOTO Blas +# F77 = gfortran +# BLAS = -lgoto_opteron64 -lgfortran + +# SUSE Linux 10.1, Intel Pentium, with GOTO Blas +# F77 = gfortran +# BLAS = -lgoto -lgfortran + +#------------------------------------------------------------------------------ +# Mac +#------------------------------------------------------------------------------ + +# As recommended by macports, http://suitesparse.darwinports.com/ +# I've tested them myself on Mac OSX 10.6.1 and 10.6.8 (Snow Leopard), +# on my MacBook Air, and they work fine. + +# F77 = gfortran +# CF = $(CFLAGS) -O3 -fno-common -fexceptions -DNTIMER +# BLAS = -framework Accelerate +# LAPACK = -framework Accelerate +# LIB = -lm + +#------------------------------------------------------------------------------ +# Solaris +#------------------------------------------------------------------------------ + +# 32-bit +# CF = $(CFLAGS) -KPIC -dalign -xc99=%none -Xc -xlibmieee -xO5 -xlibmil -m32 + +# 64-bit +# CF = $(CFLAGS) -fast -KPIC -xc99=%none -xlibmieee -xlibmil -m64 -Xc + +# FFLAGS = -fast -KPIC -dalign -xlibmil -m64 + +# The Sun Performance Library includes both LAPACK and the BLAS: +# BLAS = -xlic_lib=sunperf +# LAPACK = + + +#------------------------------------------------------------------------------ +# Compaq Alpha +#------------------------------------------------------------------------------ + +# 64-bit mode only +# CF = $(CFLAGS) -O2 -std1 +# BLAS = -ldxml +# LAPACK = + +#------------------------------------------------------------------------------ +# IBM RS 6000 +#------------------------------------------------------------------------------ + +# BLAS = -lessl +# LAPACK = + +# 32-bit mode: +# CF = $(CFLAGS) -O4 -qipa -qmaxmem=16384 -qproto +# F77FLAGS = -O4 -qipa -qmaxmem=16384 + +# 64-bit mode: +# CF = $(CFLAGS) -O4 -qipa -qmaxmem=16384 -q64 -qproto +# F77FLAGS = -O4 -qipa -qmaxmem=16384 -q64 + +#------------------------------------------------------------------------------ +# SGI IRIX +#------------------------------------------------------------------------------ + +# BLAS = -lscsl +# LAPACK = + +# 32-bit mode +# CF = $(CFLAGS) -O + +# 64-bit mode (32 bit int's and 64-bit long's): +# CF = $(CFLAGS) -64 +# F77FLAGS = -64 + +# SGI doesn't have ranlib +# RANLIB = echo + +#------------------------------------------------------------------------------ +# AMD Opteron (64 bit) +#------------------------------------------------------------------------------ + +# BLAS = -lgoto_opteron64 -lg2c +# LAPACK = -llapack_opteron64 + +# SUSE Linux 10.1, AMD Opteron +# F77 = gfortran +# BLAS = -lgoto_opteron64 -lgfortran +# LAPACK = -llapack_opteron64 + +#------------------------------------------------------------------------------ +# remove object files and profile output +#------------------------------------------------------------------------------ + +CLEAN = *.o *.obj *.ln *.bb *.bbg *.da *.tcov *.gcov gmon.out *.bak *.d *.gcda *.gcno diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/SuiteSparse_config_Mac.mk b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/SuiteSparse_config_Mac.mk new file mode 100644 index 0000000..4cc8699 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/SuiteSparse_config_Mac.mk @@ -0,0 +1,394 @@ +#=============================================================================== +# SuiteSparse_config_Mac.mk: Mac configuration file for the SuiteSparse +# To use this configuration, delete the SuiteSparse_config.mk file that +# comes with SuiteSparse and rename this file as SuiteSparse_config.mk . +#=============================================================================== + +# This file contains all configuration settings for all packages authored or +# co-authored by Tim Davis: +# +# Package Version Description +# ------- ------- ----------- +# AMD 1.2 or later approximate minimum degree ordering +# COLAMD 2.4 or later column approximate minimum degree ordering +# CCOLAMD 1.0 or later constrained column approximate minimum degree ordering +# CAMD any constrained approximate minimum degree ordering +# UMFPACK 4.5 or later sparse LU factorization, with the BLAS +# CHOLMOD any sparse Cholesky factorization, update/downdate +# KLU 0.8 or later sparse LU factorization, BLAS-free +# BTF 0.8 or later permutation to block triangular form +# LDL 1.2 or later concise sparse LDL' +# CXSparse any extended version of CSparse (int/long, real/complex) +# SuiteSparseQR any sparse QR factorization +# RBio 2.0 or later read/write sparse matrices in Rutherford-Boeing format +# +# By design, this file is NOT included in the CSparse makefile. +# That package is fully stand-alone. CSparse is primarily for teaching; +# production code should use CXSparse. +# +# The SuiteSparse_config directory and the above packages should all appear in +# a single directory, in order for the Makefile's within each package to find +# this file. +# +# To enable an option of the form "# OPTION = ...", edit this file and +# delete the "#" in the first column of the option you wish to use. +# +# The use of METIS 4.0.1 is optional. To exclude METIS, you must compile with +# CHOLMOD_CONFIG set to -DNPARTITION. See below for details. However, if you +# do not have a metis-4.0 directory inside the SuiteSparse directory, the +# */Makefile's that optionally rely on METIS will automatically detect this +# and compile without METIS. + +#------------------------------------------------------------------------------ +# Generic configuration +#------------------------------------------------------------------------------ + +# Using standard definitions from the make environment, typically: +# +# CC cc C compiler +# CXX g++ C++ compiler +# CFLAGS [ ] flags for C and C++ compiler +# CPPFLAGS [ ] flags for C and C++ compiler +# TARGET_ARCH [ ] target architecture +# FFLAGS [ ] flags for Fortran compiler +# RM rm -f delete a file +# AR ar create a static *.a library archive +# ARFLAGS rv flags for ar +# MAKE make make itself (sometimes called gmake) +# +# You can redefine them here, but by default they are used from the +# default make environment. + +# C and C++ compiler flags. The first three are standard for *.c and *.cpp +# Add -DNTIMER if you do use any timing routines (otherwise -lrt is required). +# CF = $(CFLAGS) $(CPPFLAGS) $(TARGET_ARCH) -O3 -fexceptions -fPIC -DNTIMER + CF = $(CFLAGS) $(CPPFLAGS) $(TARGET_ARCH) -O3 -fexceptions -fPIC + +# ranlib, and ar, for generating libraries. If you don't need ranlib, +# just change it to RANLAB = echo +RANLIB = ranlib +ARCHIVE = $(AR) $(ARFLAGS) + +# copy and delete a file +CP = cp -f +MV = mv -f + +# Fortran compiler (not required for 'make' or 'make library') +F77 = gfortran +F77FLAGS = $(FFLAGS) -O +F77LIB = + +# C and Fortran libraries. Remove -lrt if you don't have it. + LIB = -lm -lrt +# Using the following requires CF = ... -DNTIMER on POSIX C systems. +# LIB = -lm + +# For "make install" +INSTALL_LIB = /usr/local/lib +INSTALL_INCLUDE = /usr/local/include + +# Which version of MAKE you are using (default is "make") +# MAKE = make +# MAKE = gmake + +#------------------------------------------------------------------------------ +# BLAS and LAPACK configuration: +#------------------------------------------------------------------------------ + +# UMFPACK and CHOLMOD both require the BLAS. CHOLMOD also requires LAPACK. +# See Kazushige Goto's BLAS at http://www.cs.utexas.edu/users/flame/goto/ or +# http://www.tacc.utexas.edu/~kgoto/ for the best BLAS to use with CHOLMOD. +# LAPACK is at http://www.netlib.org/lapack/ . You can use the standard +# Fortran LAPACK along with Goto's BLAS to obtain very good performance. +# CHOLMOD gets a peak numeric factorization rate of 3.6 Gflops on a 3.2 GHz +# Pentium 4 (512K cache, 4GB main memory) with the Goto BLAS, and 6 Gflops +# on a 2.5Ghz dual-core AMD Opteron. + +# These settings will probably not work, since there is no fixed convention for +# naming the BLAS and LAPACK library (*.a or *.so) files. + +# This is probably slow ... it might connect to the Standard Reference BLAS: +BLAS = -lblas -lgfortran +LAPACK = -llapack + +# NOTE: this next option for the "Goto BLAS" has nothing to do with a "goto" +# statement. Rather, the Goto BLAS is written by Dr. Kazushige Goto. +# Using the Goto BLAS: +# BLAS = -lgoto -lgfortran -lgfortranbegin +# BLAS = -lgoto2 -lgfortran -lgfortranbegin -lpthread + +# Using non-optimized versions: +# BLAS = -lblas_plain -lgfortran -lgfortranbegin +# LAPACK = -llapack_plain + +# BLAS = -lblas_plain -lgfortran -lgfortranbegin +# LAPACK = -llapack + +# The BLAS might not contain xerbla, an error-handling routine for LAPACK and +# the BLAS. Also, the standard xerbla requires the Fortran I/O library, and +# stops the application program if an error occurs. A C version of xerbla +# distributed with this software (SuiteSparse_config/xerbla/libcerbla.a) +# includes a Fortran-callable xerbla routine that prints nothing and does not +# stop the application program. This is optional. + +# XERBLA = ../../SuiteSparse_config/xerbla/libcerbla.a + +# If you wish to use the XERBLA in LAPACK and/or the BLAS instead, +# use this option: +XERBLA = + +# If you wish to use the Fortran SuiteSparse_config/xerbla/xerbla.f instead, +# use this: + +# XERBLA = ../../SuiteSparse_config/xerbla/libxerbla.a + +#------------------------------------------------------------------------------ +# GPU configuration for CHOLMOD, using the CUDA BLAS +#------------------------------------------------------------------------------ + +# no cuda +GPU_BLAS_PATH = +GPU_CONFIG = + +# with cuda BLAS acceleration for CHOLMOD +# GPU_BLAS_PATH=/usr/local/cuda +# GPU_CONFIG=-DGPU_BLAS -I$(GPU_BLAS_PATH)/include + +#------------------------------------------------------------------------------ +# METIS, optionally used by CHOLMOD +#------------------------------------------------------------------------------ + +# If you do not have METIS, or do not wish to use it in CHOLMOD, you must +# compile CHOLMOD with the -DNPARTITION flag. + +# The path is relative to where it is used, in CHOLMOD/Lib, CHOLMOD/MATLAB, etc. +# You may wish to use an absolute path. METIS is optional. Compile +# CHOLMOD with -DNPARTITION if you do not wish to use METIS. +METIS_PATH = ../../metis-4.0 +METIS = ../../metis-4.0/libmetis.a + +#------------------------------------------------------------------------------ +# UMFPACK configuration: +#------------------------------------------------------------------------------ + +# Configuration flags for UMFPACK. See UMFPACK/Source/umf_config.h for details. +# +# -DNBLAS do not use the BLAS. UMFPACK will be very slow. +# -D'LONGBLAS=long' or -DLONGBLAS='long long' defines the integers used by +# LAPACK and the BLAS (defaults to 'int') +# -DNSUNPERF do not use the Sun Perf. Library (default is use it on Solaris) +# -DNRECIPROCAL do not multiply by the reciprocal +# -DNO_DIVIDE_BY_ZERO do not divide by zero +# -DNCHOLMOD do not use CHOLMOD as a ordering method. If -DNCHOLMOD is +# included in UMFPACK_CONFIG, then UMFPACK does not rely on +# CHOLMOD, CAMD, CCOLAMD, COLAMD, and METIS. + +UMFPACK_CONFIG = + +# uncomment this line to compile UMFPACK without CHOLMOD: +# UMFPACK_CONFIG = -DNCHOLMOD + +#------------------------------------------------------------------------------ +# CHOLMOD configuration +#------------------------------------------------------------------------------ + +# CHOLMOD Library Modules, which appear in libcholmod.a: +# Core requires: none +# Check requires: Core +# Cholesky requires: Core, AMD, COLAMD. optional: Partition, Supernodal +# MatrixOps requires: Core +# Modify requires: Core +# Partition requires: Core, CCOLAMD, METIS. optional: Cholesky +# Supernodal requires: Core, BLAS, LAPACK +# +# CHOLMOD test/demo Modules (all are GNU GPL, do not appear in libcholmod.a): +# Tcov requires: Core, Check, Cholesky, MatrixOps, Modify, Supernodal +# optional: Partition +# Valgrind same as Tcov +# Demo requires: Core, Check, Cholesky, MatrixOps, Supernodal +# optional: Partition +# +# Configuration flags: +# -DNCHECK do not include the Check module. License GNU LGPL +# -DNCHOLESKY do not include the Cholesky module. License GNU LGPL +# -DNPARTITION do not include the Partition module. License GNU LGPL +# also do not include METIS. +# -DNGPL do not include any GNU GPL Modules in the CHOLMOD library: +# -DNMATRIXOPS do not include the MatrixOps module. License GNU GPL +# -DNMODIFY do not include the Modify module. License GNU GPL +# -DNSUPERNODAL do not include the Supernodal module. License GNU GPL +# +# -DNPRINT do not print anything. +# -D'LONGBLAS=long' or -DLONGBLAS='long long' defines the integers used by +# LAPACK and the BLAS (defaults to 'int') +# -DNSUNPERF for Solaris only. If defined, do not use the Sun +# Performance Library + +CHOLMOD_CONFIG = $(GPU_CONFIG) + +# uncomment this line to compile CHOLMOD without METIS: +# CHOLMOD_CONFIG = -DNPARTITION + +#------------------------------------------------------------------------------ +# SuiteSparseQR configuration: +#------------------------------------------------------------------------------ + +# The SuiteSparseQR library can be compiled with the following options: +# +# -DNPARTITION do not include the CHOLMOD partition module +# -DNEXPERT do not include the functions in SuiteSparseQR_expert.cpp +# -DHAVE_TBB enable the use of Intel's Threading Building Blocks (TBB) + +# default, without timing, without TBB: +SPQR_CONFIG = +# with TBB: +# SPQR_CONFIG = -DHAVE_TBB + +# This is needed for IBM AIX: (but not for and C codes, just C++) +# SPQR_CONFIG = -DBLAS_NO_UNDERSCORE + +# with TBB, you must select this: +# TBB = -ltbb +# without TBB: +TBB = + +#------------------------------------------------------------------------------ +# Linux +#------------------------------------------------------------------------------ + +# Using default compilers: +# CC = gcc +# CF = $(CFLAGS) -O3 -fexceptions + +# alternatives: +# CF = $(CFLAGS) -g -fexceptions \ + -Wall -W -Wshadow -Wmissing-prototypes -Wstrict-prototypes \ + -Wredundant-decls -Wnested-externs -Wdisabled-optimization -ansi \ + -funit-at-a-time +# CF = $(CFLAGS) -O3 -fexceptions \ + -Wall -W -Werror -Wshadow -Wmissing-prototypes -Wstrict-prototypes \ + -Wredundant-decls -Wnested-externs -Wdisabled-optimization -ansi +# CF = $(CFLAGS) -O3 -fexceptions -D_FILE_OFFSET_BITS=64 -D_LARGEFILE64_SOURCE +# CF = $(CFLAGS) -O3 +# CF = $(CFLAGS) -O3 -g -fexceptions +# CF = $(CFLAGS) -g -fexceptions \ + -Wall -W -Wshadow \ + -Wredundant-decls -Wdisabled-optimization -ansi + +# consider: +# -fforce-addr -fmove-all-movables -freduce-all-givs -ftsp-ordering +# -frename-registers -ffast-math -funroll-loops + +# Using the Goto BLAS: +# BLAS = -lgoto -lfrtbegin -lg2c $(XERBLA) -lpthread + +# Using Intel's icc and ifort compilers: +# (does not work for mexFunctions unless you add a mexopts.sh file) +# F77 = ifort +# CC = icc +# CF = $(CFLAGS) -O3 -xN -vec_report=0 +# CF = $(CFLAGS) -g + +# 64bit: +# F77FLAGS = -O -m64 +# CF = $(CFLAGS) -O3 -fexceptions -m64 +# BLAS = -lgoto64 -lfrtbegin -lg2c -lpthread $(XERBLA) +# LAPACK = -llapack64 + +# SUSE Linux 10.1, AMD Opteron, with GOTO Blas +# F77 = gfortran +# BLAS = -lgoto_opteron64 -lgfortran + +# SUSE Linux 10.1, Intel Pentium, with GOTO Blas +# F77 = gfortran +# BLAS = -lgoto -lgfortran + +#------------------------------------------------------------------------------ +# Mac +#------------------------------------------------------------------------------ + +# As recommended by macports, http://suitesparse.darwinports.com/ +# I've tested them myself on Mac OSX 10.6.1 and 10.6.8 (Snow Leopard), +# on my MacBook Air, and they work fine. + + F77 = gfortran + CF = $(CFLAGS) -O3 -fno-common -fexceptions -DNTIMER + BLAS = -framework Accelerate + LAPACK = -framework Accelerate + LIB = -lm + +#------------------------------------------------------------------------------ +# Solaris +#------------------------------------------------------------------------------ + +# 32-bit +# CF = $(CFLAGS) -KPIC -dalign -xc99=%none -Xc -xlibmieee -xO5 -xlibmil -m32 + +# 64-bit +# CF = $(CFLAGS) -fast -KPIC -xc99=%none -xlibmieee -xlibmil -m64 -Xc + +# FFLAGS = -fast -KPIC -dalign -xlibmil -m64 + +# The Sun Performance Library includes both LAPACK and the BLAS: +# BLAS = -xlic_lib=sunperf +# LAPACK = + + +#------------------------------------------------------------------------------ +# Compaq Alpha +#------------------------------------------------------------------------------ + +# 64-bit mode only +# CF = $(CFLAGS) -O2 -std1 +# BLAS = -ldxml +# LAPACK = + +#------------------------------------------------------------------------------ +# IBM RS 6000 +#------------------------------------------------------------------------------ + +# BLAS = -lessl +# LAPACK = + +# 32-bit mode: +# CF = $(CFLAGS) -O4 -qipa -qmaxmem=16384 -qproto +# F77FLAGS = -O4 -qipa -qmaxmem=16384 + +# 64-bit mode: +# CF = $(CFLAGS) -O4 -qipa -qmaxmem=16384 -q64 -qproto +# F77FLAGS = -O4 -qipa -qmaxmem=16384 -q64 + +#------------------------------------------------------------------------------ +# SGI IRIX +#------------------------------------------------------------------------------ + +# BLAS = -lscsl +# LAPACK = + +# 32-bit mode +# CF = $(CFLAGS) -O + +# 64-bit mode (32 bit int's and 64-bit long's): +# CF = $(CFLAGS) -64 +# F77FLAGS = -64 + +# SGI doesn't have ranlib +# RANLIB = echo + +#------------------------------------------------------------------------------ +# AMD Opteron (64 bit) +#------------------------------------------------------------------------------ + +# BLAS = -lgoto_opteron64 -lg2c +# LAPACK = -llapack_opteron64 + +# SUSE Linux 10.1, AMD Opteron +# F77 = gfortran +# BLAS = -lgoto_opteron64 -lgfortran +# LAPACK = -llapack_opteron64 + +#------------------------------------------------------------------------------ +# remove object files and profile output +#------------------------------------------------------------------------------ + +CLEAN = *.o *.obj *.ln *.bb *.bbg *.da *.tcov *.gcov gmon.out *.bak *.d *.gcda *.gcno diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/xerbla/Makefile b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/xerbla/Makefile new file mode 100644 index 0000000..9de7330 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/xerbla/Makefile @@ -0,0 +1,33 @@ +# Makefile for null-output xerbla + +default: ccode + +include ../SuiteSparse_config.mk + +ccode: libcerbla.a + +fortran: libxerbla.a + +all: libxerbla.a libcerbla.a + +# Fortran version: +libxerbla.a: xerbla.f + $(F77) $(F77FLAGS) -c xerbla.f + $(ARCHIVE) libxerbla.a xerbla.o + - $(RANLIB) libxerbla.a + - $(RM) xerbla.o + +# C version: +libcerbla.a: xerbla.c xerbla.h + $(CC) $(CF) -c xerbla.c + $(ARCHIVE) libcerbla.a xerbla.o + - $(RANLIB) libcerbla.a + - $(RM) xerbla.o + +distclean: purge + +purge: clean + - $(RM) *.o *.a + +clean: + - $(RM) $(CLEAN) diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/xerbla/xerbla.c b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/xerbla/xerbla.c new file mode 100644 index 0000000..5107f03 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/xerbla/xerbla.c @@ -0,0 +1,12 @@ + +void xerbla_ (char *srname, int *info) +{ + /* do nothing */ ; +} + + +void xerbla (char *srname, int *info) +{ + /* do nothing */ ; +} + diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/xerbla/xerbla.f b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/xerbla/xerbla.f new file mode 100644 index 0000000..4272004 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/xerbla/xerbla.f @@ -0,0 +1,46 @@ + SUBROUTINE XERBLA( SRNAME, INFO ) +* +* -- LAPACK auxiliary routine (version 3.0) -- +* Univ. of Tennessee, Univ. of California Berkeley, NAG Ltd., +* Courant Institute, Argonne National Lab, and Rice University +* September 30, 1994 +* +* .. Scalar Arguments .. + CHARACTER*6 SRNAME + INTEGER INFO +* .. +* +* Purpose +* ======= +* +* XERBLA is an error handler for the LAPACK routines. +* It is called by an LAPACK routine if an input parameter has an +* invalid value. A message is printed and execution stops. +* +* Installers may consider modifying the STOP statement in order to +* call system-specific exception-handling facilities. +* +* Arguments +* ========= +* +* SRNAME (input) CHARACTER*6 +* The name of the routine which called XERBLA. +* +* INFO (input) INTEGER +* The position of the invalid parameter in the parameter list +* of the calling routine. +* +* ===================================================================== +* +* .. Executable Statements .. +* +***** WRITE( *, FMT = 9999 )SRNAME, INFO +* +***** STOP +* +***** 9999 FORMAT( ' ** On entry to ', A6, ' parameter number ', I2, ' had ', +***** $ 'an illegal value' ) +* +* End of XERBLA +* + END diff --git a/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/xerbla/xerbla.h b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/xerbla/xerbla.h new file mode 100644 index 0000000..b332eb3 --- /dev/null +++ b/Chapter4_StructureFromMotion/3rdparty/SSBA-3.0/SuiteSparse_config/xerbla/xerbla.h @@ -0,0 +1,2 @@ +void xerbla_ (char *srname, int *info) ; +void xerbla (char *srname, int *info) ; diff --git a/Chapter4_StructureFromMotion/Common.cpp b/Chapter4_StructureFromMotion/Common.cpp index cddd5c4..12530b1 100755 --- a/Chapter4_StructureFromMotion/Common.cpp +++ b/Chapter4_StructureFromMotion/Common.cpp @@ -17,6 +17,8 @@ #ifndef WIN32 #include +#else +#include #endif using namespace std; diff --git a/Chapter4_StructureFromMotion/GPUSURFFeatureMatcher.h b/Chapter4_StructureFromMotion/GPUSURFFeatureMatcher.h index a562af2..b025309 100755 --- a/Chapter4_StructureFromMotion/GPUSURFFeatureMatcher.h +++ b/Chapter4_StructureFromMotion/GPUSURFFeatureMatcher.h @@ -8,6 +8,7 @@ #include "IFeatureMatcher.h" #include +#include class GPUSURFFeatureMatcher : public IFeatureMatcher { private: