Mex library and docker stuff

.gitignore

@@ -0,0 +1,2 @@
+build*/
+.vscode/*

docker/.dockerignore

@@ -0,0 +1,26 @@
+**/.classpath
+**/.dockerignore
+**/.env
+**/.git
+**/.gitignore
+**/.project
+**/.settings
+**/.toolstarget
+**/.vs
+**/.vscode
+**/*.*proj.user
+**/*.dbmdl
+**/*.jfm
+**/azds.yaml
+**/bin
+**/charts
+**/docker-compose*
+**/Dockerfile*
+**/node_modules
+**/npm-debug.log
+**/obj
+**/secrets.dev.yaml
+**/values.dev.yaml
+run_docker.sh
+build_docker.sh
+README.md

docker/Dockerfile

@@ -0,0 +1,42 @@
+FROM ubuntu:18.04
+
+ARG DEBIAN_FRONTEND=noninteractive
+
+# Install apt-getable dependencies
+RUN apt-get update \
+    && apt-get install -y \
+        build-essential \
+        cmake \
+        git \
+        libatlas-base-dev \
+        libeigen3-dev \
+        libgoogle-glog-dev \
+        libopencv-dev \
+        libsuitesparse-dev \
+        curl \
+        g++ \
+        gdb \
+    && apt-get clean \
+    && rm -rf /var/lib/apt/lists/* /tmp/* /var/tmp/*
+
+
+# opengv
+RUN \
+    mkdir -p /source && cd /source && \
+    git clone https://github.com/paulinus/opengv.git && \
+    cd /source/opengv && \
+    git submodule update --init --recursive && \
+    mkdir -p build && cd build && \
+    cmake .. -DBUILD_TESTS=OFF && \
+    make install && \
+    cd / && rm -rf /source/opengv
+
+# ceres
+RUN \
+    mkdir -p /source && cd /source && \
+    curl -L http://ceres-solver.org/ceres-solver-1.14.0.tar.gz | tar xz && \
+    cd /source/ceres-solver-1.14.0 && \
+    mkdir -p build && cd build && \
+    cmake .. -DOPENMP=OFF -DCXSPARSE=OFF -DMINIGLOG=ON -DSUITESPARSE=OFF -DGFLAGS=OFF -DSCHUR_SPECIALIZATIONS=OFF -DCUSTOM_BLAS=OFF -DLAPACK=OFF -DBUILD_BENCHMARKS=OFF -DCMAKE_C_FLAGS=-fPIC -DCMAKE_CXX_FLAGS=-fPIC -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF && \
+    make -j4 install && \
+    cd / && rm -rf /source/ceres-solver-1.14.0

docker/build_docker.sh

@@ -0,0 +1 @@
+docker build -t ransaclib .

docker/run_docker.sh

@@ -0,0 +1,64 @@
+#!/bin/bash
+DOCKER_TAG="ransaclib"
+DOCKER_NAME="ransaclib"
+DOCKER_ENV=""
+read -r -a DOCKER_RUN_ARGS <<< "$DOCKER_ENV"
+
+DOCKER_RUN_ARGS+=("--name" "${DOCKER_NAME}"
+                  "--rm"
+                  "--tty"
+                  "--init"
+                  "--interactive"
+                  "--privileged"
+                  "--net=host"
+                  "--gpus" "all"
+                  "--hostname" "$DOCKER_NAME"
+                  "-e" "DISPLAY"
+                  "-e" "HOME"
+                  "-e" "LANG=C.UTF-8"
+                  "-e" "TERM=xterm-256color"
+                  "-v" "/tmp/.X11-unix:/tmp/.X11-unix"
+                  "-v" "$HOME:$HOME"
+                  "-w" "$PWD")
+
+MATLAB_BASE_PATH="/usr/local/MATLAB"
+
+
+if [ -d "${MATLAB_BASE_PATH}" ]; then
+    MATLAB_VER=$( ls ${MATLAB_BASE_PATH} | tail -1 )
+    MATLAB_PATH="${MATLAB_BASE_PATH}/${MATLAB_VER}"
+    DOCKER_RUN_ARGS+=("-v" "${MATLAB_PATH}:${MATLAB_PATH}")
+    PATH="${PATH}:${MATLAB_PATH}/bin"
+fi
+
+
+# The following is a hack that creates a linux group and user with the same
+# uid:gid as the host. It then logs in this user so that all files created have
+# the host user as owner and group.
+PATH_ENV="$PATH"
+CONTAINER_USERNAME=$(whoami)
+CONTAINER_GROUPNAME=$(id -gn)
+USER_ID=$(id -u)
+GROUP_ID=$(id -g)
+read -r -d '' CREATE_USER_COMMAND << EOF || true
+groupadd --force --gid "$GROUP_ID" "$CONTAINER_GROUPNAME" &&
+useradd --no-create-home \
+        --uid "$USER_ID" \
+        --gid "$GROUP_ID" \
+        --groups sudo \
+        --home-dir "$HOME" \
+        --shell /bin/bash \
+        "$CONTAINER_USERNAME" &&
+echo "root:root" | chpasswd &&
+echo "$CONTAINER_USERNAME:$CONTAINER_USERNAME" | chpasswd &&
+# Set PATH for both root and local user (since su does not preserve PATH)
+echo PATH="$PATH_ENV" | tee /etc/profile /etc/environment &&
+su --login --preserve-environment "$CONTAINER_USERNAME"
+EOF
+
+#If commands are given on the command line, then run them and exit, otherwise just start a terminal
+if [ "$#" -gt 0 ]; then
+    CREATE_USER_COMMAND="${CREATE_USER_COMMAND} -c \"$@\""
+fi
+
+docker run "${DOCKER_RUN_ARGS[@]}" "${DOCKER_TAG}" bash -ci "$CREATE_USER_COMMAND"

examples/CMakeLists.txt

@@ -4,14 +4,16 @@ if (EXISTS "${CMAKE_SOURCE_DIR}/cmake")
   set (CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_SOURCE_DIR}/cmake)
 endif ()
 
-#SET(CMAKE_CXX_FLAGS "-std=gnu++17")
+set (CMAKE_POSITION_INDEPENDENT_CODE ON)
 
 find_package (Eigen3 REQUIRED)
 
 find_package (opengv REQUIRED)
 
 find_package (Ceres REQUIRED)
 
+find_package (Matlab)
+
 add_definitions (-march=native)
 
 include_directories (
@@ -28,11 +30,23 @@ add_executable (camera_pose_estimation camera_pose_estimation.cc calibrated_abso
 target_link_libraries (camera_pose_estimation opengv ${CERES_LIBRARIES})
 
 add_executable (localization localization.cc calibrated_absolute_pose_estimator.cc calibrated_absolute_pose_estimator.h)
-target_link_libraries (localization opengv
-                                    ${CERES_LIBRARIES})
+target_link_libraries (localization opengv ${CERES_LIBRARIES})
+
+add_library (localize STATIC localize.cc calibrated_absolute_pose_estimator.cc calibrated_absolute_pose_estimator.h)
+target_link_libraries (localize opengv ${CERES_LIBRARIES})
 
-add_executable (localization_with_gt localization_with_gt.cc calibrated_absolute_pose_estimator.cc calibrated_absolute_pose_estimator.h)
-target_link_libraries (localization_with_gt opengv ${CERES_LIBRARIES})
+#add_executable (localization_with_gt localization_with_gt.cc calibrated_absolute_pose_estimator.cc calibrated_absolute_pose_estimator.h)
+#target_link_libraries (localization_with_gt opengv ${CERES_LIBRARIES})
 
 #add_executable (localization_gc localization_gc.cc #calibrated_absolute_pose_estimator.cc calibrated_absolute_pose_estimator.h)
 #target_link_libraries (localization opengv)
+
+IF (Matlab_FOUND)
+  matlab_add_mex (NAME CameraPoseRANSAC_Mex SRC CameraPoseRANSAC_Mex.cpp)
+  target_link_libraries (CameraPoseRANSAC_Mex localize opengv ${CERES_LIBRARIES} ${MATLAB_LIBRARIES})
+  set_target_properties (CameraPoseRANSAC_Mex PROPERTIES COMPILE_FLAGS "-fvisibility=default")
+ENDIF()
+
+#This is the "Matlab way" to compile a mex library. However, it crashes when trying to run the localization.
+#cd build
+#mex -g -I../examples -I/usr/include/eigen3 -L./examples -llocalize -L/usr/local/lib -lopengv -L/usr/local/lib -lceres ../examples/CameraPoseRANSAC_Mex.cpp

examples/CameraPoseRANSAC_Mex.cpp

@@ -0,0 +1,68 @@
+#include <mex.h>
+#include <Eigen/Eigen>
+#include "localize.h"
+
+// Takes an mxArray and returns its contents as an eigen matrix by reference
+void input_matrix(const mxArray* p, Eigen::MatrixXd& mat) {
+    int m = mxGetM(p);
+    int n = mxGetN(p);
+
+    mat.resize(m,n);
+
+    double *data = mxGetPr(p);
+
+    for(int j = 0; j < n; j++) {
+        for(int i = 0; i < m; i++) {
+            mat(i,j) = data[m*j + i];
+        }
+    }
+}
+
+// Should be called as:
+// [P, inliers] = CameraPoseRANSAC_Mex(X,x,tol,nIters), where X is a 3xN matrix containing the 3D points, and
+// x is a 3xN matrix containing the image points as unit vectors and tol is the largest allowed
+// angular deviation (in radians) for a correspondence to be considered an inlier, and nIters is the max number of iterations.
+// The output P is the calculated camera matrix as well as an integer designating the number of inliers.
+void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
+{
+    // Validate the input
+    if(nrhs != 4)
+        mexErrMsgTxt("Invalid input! The function takes three inputs: \n\t3d structure X as a 3xN matrix, \n\ta 3xN matrix representing image points as unit vectors, \n\tthe maximum allowed angle deviation for a correspondence to be considered an inlier,\n\tnumber of RANSAC iterations.");
+    if(nlhs != 2)
+        mexErrMsgTxt("Invalid output! The function returns two outputs: P representing the triangulated camera matrix and an integer designating the number of inliers for P. ");
+
+    Eigen::MatrixXd x;
+    Eigen::MatrixXd X;
+    double tol = mxGetScalar(prhs[2]);
+    int nIter = int(mxGetScalar(prhs[3]));
+
+    input_matrix(prhs[0],X);
+    input_matrix(prhs[1],x);
+
+    if(x.rows() != 3 || X.rows() != 3 || x.cols() != X.cols())
+        mexErrMsgTxt("Input matrices must both have three rows and the same number of columns!");
+
+    // Inputs and outputs are ok! Calculate camera pose!
+    int numInliers = 0;
+    Eigen::Matrix<double,3,4> P = Eigen::MatrixXd::Zero(3,4);
+    bool success = cameraPoseRANSAC(x,X,nIter,tol,P,numInliers);
+    if (!success)
+    {
+        numInliers = 0;
+    }
+
+    // Create output
+    plhs[0] = mxCreateDoubleMatrix(P.rows(),4,mxREAL);
+    double* outputMatrix = mxGetPr(plhs[0]);
+    int index = 0;
+    for (int j = 0; j < P.cols(); j++) {
+        for (int i = 0; i < P.rows(); i++) {
+            outputMatrix[index] = P(i,j);
+            index++;
+        }
+    }
+
+    plhs[1] = mxCreateDoubleMatrix(1,1,mxREAL);
+    double* output = mxGetPr(plhs[1]);
+    output[0] = static_cast<double>(numInliers);
+}

examples/calibrated_absolute_pose_estimator.cc

@@ -189,6 +189,7 @@ void CalibratedAbsolutePoseEstimator::LeastSquares(
   ceres::Solver::Options options;
   options.linear_solver_type = ceres::DENSE_QR;
   options.minimizer_progress_to_stdout = false;
+  options.logging_type = ceres::SILENT;
   //  options.function_tolerance = 0.000001;
   ceres::Solver::Summary summary;
   ceres::Solve(options, &refinement_problem, &summary);

examples/localize.cc

@@ -0,0 +1,105 @@
+#include <iostream>
+#include <RansacLib/ransac.h>
+#include "calibrated_absolute_pose_estimator.h"
+#include "localize.h"
+
+#define NPTS 100
+bool test_ransac()
+{
+    int n = 1000;
+    Eigen::Matrix<double,3,NPTS> X = Eigen::MatrixXd::Random(3,NPTS)*10.0;
+    for (int k=0; k<NPTS; ++k)
+    {
+        X(2,k) += 25.0;
+    }
+    Eigen::Matrix<double,4,NPTS> U = Eigen::MatrixXd::Ones(4,NPTS);
+    U.block(0,0,3,NPTS) = X;
+    Eigen::Vector3d vaa = Eigen::Vector3d::Random(3,1)*0.2;
+    Eigen::AngleAxis<double> aa(vaa.norm(), vaa / vaa.norm());
+    Eigen::Matrix<double,3,4> P_gt = Eigen::MatrixXd::Random(3,4);
+    P_gt.block(0,0,3,3) = aa.toRotationMatrix();
+    Eigen::Matrix<double,3,NPTS> x = P_gt*U;
+    Eigen::Matrix<double,3,4> P = Eigen::MatrixXd::Zero(3,4);
+
+    bool res = cameraPoseRANSAC(x, X, 1000, 0.008, P, n);
+    return (res && n==NPTS && (P-P_gt).norm()<1e-8);
+}
+
+bool cameraPoseRANSAC(const Eigen::MatrixXd &impoints, const Eigen::MatrixXd &spacePoints, int numIterations, double tol, Eigen::Matrix<double,3,4> &cameraMatrixOut, int &numInliersOut)
+{
+    numInliersOut = 0;
+
+    using ransac_lib::LocallyOptimizedMSAC;
+    using ransac_lib::calibrated_absolute_pose::CalibratedAbsolutePoseEstimator;
+    using ransac_lib::calibrated_absolute_pose::CameraPose;
+    using ransac_lib::calibrated_absolute_pose::CameraPoses;
+    using ransac_lib::calibrated_absolute_pose::Points2D;
+    using ransac_lib::calibrated_absolute_pose::Points3D;
+
+    const int kNumMatches = impoints.cols();
+    if (kNumMatches <= 3)
+    {
+        return false;
+    }
+    if (spacePoints.cols() != kNumMatches)
+    {
+        return false;
+    }
+    if (impoints.rows() != 3)
+    {
+        return false;
+    }
+    if (spacePoints.rows() != 3)
+    {
+        return false;
+    }
+
+    Points2D points2D;
+    Points3D points3D;
+
+    for (size_t k = 0; k < kNumMatches; ++k)
+    {
+        points2D.push_back(impoints.block(0,k,2,1)/impoints(2,k));
+        points3D.push_back(spacePoints.block(0,k,3,1));
+    }
+
+    opengv::bearingVectors_t rays;
+
+    double focal_x = 1.0, focal_y = 1.0; //Normalized cameras as input
+    //Could read this directly from the input instead...
+    CalibratedAbsolutePoseEstimator::PixelsToViewingRays(
+        focal_x, focal_y, points2D, &rays);
+
+    ransac_lib::LORansacOptions options;
+    options.min_num_iterations_ = 100u;
+    options.max_num_iterations_ = numIterations;
+    options.min_sample_multiplicator_ = 7;
+    options.num_lsq_iterations_ = 4;
+    options.num_lo_steps_ = 10;
+    options.lo_starting_iterations_ = 20;
+    options.final_least_squares_ = true;
+
+    std::random_device rand_dev;
+    options.random_seed_ = rand_dev();
+
+    options.squared_inlier_threshold_ = tol*tol;
+
+    CalibratedAbsolutePoseEstimator solver(
+        focal_x, focal_y, options.squared_inlier_threshold_,
+        points2D, rays, points3D);
+
+    LocallyOptimizedMSAC<CameraPose, CameraPoses,
+                         CalibratedAbsolutePoseEstimator> lomsac;
+
+    ransac_lib::RansacStatistics ransac_stats;
+    CameraPose best_model = Eigen::MatrixXd::Zero(3,4);
+    best_model(0,0) = 1.0;
+    best_model(1,1) = 1.0;
+    best_model(2,2) = 1.0;
+
+    //This line causes a crash in Matlab...
+    numInliersOut = lomsac.EstimateModel(options, solver, &best_model, &ransac_stats);
+    best_model.block(0,3,3,1) = -best_model.block(0,0,3,3)*best_model.block(0,3,3,1);
+    cameraMatrixOut = best_model;
+    return numInliersOut >= 4;
+}

examples/localize.h

@@ -0,0 +1,9 @@
+#include "Eigen/Eigen"
+
+bool cameraPoseRANSAC(
+    const Eigen::MatrixXd &impoints,
+    const Eigen::MatrixXd &spacePoints,
+    int numIterations,
+    double tol,
+    Eigen::Matrix<double, 3, 4> &cameraMatrix,
+    int &oNumInliers);