diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..be3df2d --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +build*/ +.vscode/* diff --git a/docker/.dockerignore b/docker/.dockerignore new file mode 100644 index 0000000..0076530 --- /dev/null +++ b/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 diff --git a/docker/Dockerfile b/docker/Dockerfile new file mode 100644 index 0000000..467747e --- /dev/null +++ b/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 diff --git a/docker/build_docker.sh b/docker/build_docker.sh new file mode 100755 index 0000000..c63ec1d --- /dev/null +++ b/docker/build_docker.sh @@ -0,0 +1 @@ +docker build -t ransaclib . diff --git a/docker/run_docker.sh b/docker/run_docker.sh new file mode 100755 index 0000000..e80e6dd --- /dev/null +++ b/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" diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index b01a083..9723889 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -4,7 +4,7 @@ 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) @@ -12,6 +12,8 @@ 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 diff --git a/examples/CameraPoseRANSAC_Mex.cpp b/examples/CameraPoseRANSAC_Mex.cpp new file mode 100644 index 0000000..b7d75ab --- /dev/null +++ b/examples/CameraPoseRANSAC_Mex.cpp @@ -0,0 +1,68 @@ +#include +#include +#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 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(numInliers); +} diff --git a/examples/calibrated_absolute_pose_estimator.cc b/examples/calibrated_absolute_pose_estimator.cc index 7c43fb7..6321119 100644 --- a/examples/calibrated_absolute_pose_estimator.cc +++ b/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); diff --git a/examples/localize.cc b/examples/localize.cc new file mode 100644 index 0000000..599d92e --- /dev/null +++ b/examples/localize.cc @@ -0,0 +1,105 @@ +#include +#include +#include "calibrated_absolute_pose_estimator.h" +#include "localize.h" + +#define NPTS 100 +bool test_ransac() +{ + int n = 1000; + Eigen::Matrix X = Eigen::MatrixXd::Random(3,NPTS)*10.0; + for (int k=0; k 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 aa(vaa.norm(), vaa / vaa.norm()); + Eigen::Matrix P_gt = Eigen::MatrixXd::Random(3,4); + P_gt.block(0,0,3,3) = aa.toRotationMatrix(); + Eigen::Matrix x = P_gt*U; + Eigen::Matrix 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 &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 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; +} diff --git a/examples/localize.h b/examples/localize.h new file mode 100644 index 0000000..9ecd618 --- /dev/null +++ b/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 &cameraMatrix, + int &oNumInliers);