├── .dockerignore ├── .gitignore ├── resource ├── main.jpg ├── paper_preview.png ├── thirdparty │ ├── msh2shd │ └── LICENSE.txt ├── blender │ ├── studio.blend │ └── README.md └── scripts │ ├── plot_error.py │ ├── poisson_6.sh │ ├── plot_dist_weight.py │ ├── decimate_meshes.sh │ ├── gen_regions.sh │ ├── dl_tests.sh │ ├── meshlab_poisson.mlx │ ├── meshlab_poisson_6.mlx │ ├── affine_tform_meshes.sh │ ├── remesh_graph.sh │ ├── abs_tform.mlx │ ├── meshlab_scale.mlx │ ├── meshlab_isotropic_remesh.mlx │ ├── meshlab_transform.mlx │ ├── meshlab_decimate.mlx │ └── dot_similarity.m ├── meshtracker ├── include │ ├── prog_opts.h │ ├── constants.h │ ├── log.h │ ├── cpd_utils.h │ ├── tri_mesh.h │ ├── keyframer.h │ ├── nonrigid_icp.h │ ├── cpd_matrix.h │ ├── cpd_icp.h │ ├── gauss_newton_solver.h │ ├── cpd_normalization.h │ ├── cpd_gauss_transform.h │ ├── kalman_smoothing.h │ ├── cpd_nonrigid.h │ ├── matching.h │ ├── mesh_segmentation.h │ ├── deform_graph.h │ ├── cloud_processing.h │ ├── mesh_tracking.h │ ├── tests.h │ ├── cpd_transform.h │ ├── utils.h │ └── mesh_processing.h ├── CMakeLists.txt ├── solver_options.conf ├── src │ ├── cpd_gauss_transform_make_default.cpp │ ├── cpd_transform.cpp │ ├── cpd_matrix.cpp │ ├── cpd_normalization.cpp │ ├── cpd_gauss_transform.cpp │ ├── cpd_nonrigid.cpp │ ├── cpd_utils.cpp │ ├── keyframer.cpp │ ├── nonrigid_icp.cpp │ ├── cpd_icp.cpp │ ├── cloud_processing.cpp │ └── kalman_smoothing.cpp └── main.cpp ├── dockers ├── dockerfile_cgal ├── build_dockers.sh ├── dockerfile_meshlab ├── dockerfile_pcl ├── dockerfile_base └── dockerfile_build ├── CMakeLists.txt └── README.md /.dockerignore: -------------------------------------------------------------------------------- 1 | .git/** 2 | resource/tests/** -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/** 2 | mt.code-workspace 3 | .vscode 4 | resource/** -------------------------------------------------------------------------------- /resource/main.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/V-Sense/AutoMeshTracker/HEAD/resource/main.jpg -------------------------------------------------------------------------------- /resource/paper_preview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/V-Sense/AutoMeshTracker/HEAD/resource/paper_preview.png -------------------------------------------------------------------------------- /resource/thirdparty/msh2shd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/V-Sense/AutoMeshTracker/HEAD/resource/thirdparty/msh2shd -------------------------------------------------------------------------------- /resource/blender/studio.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/V-Sense/AutoMeshTracker/HEAD/resource/blender/studio.blend -------------------------------------------------------------------------------- /meshtracker/include/prog_opts.h: -------------------------------------------------------------------------------- 1 | #ifndef PROG_OPTS_H 2 | #define PROG_OPTS_H 3 | 4 | namespace icp { 5 | 6 | struct IcpOptions { 7 | 8 | bool use_adaptive; 9 | bool parallel; 10 | 11 | }; 12 | 13 | } 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /resource/blender/README.md: -------------------------------------------------------------------------------- 1 | # Studio Blender file 2 | studio.blend is a simple blender scenario used to render the results in the paper and video. 3 | You can use [stop-motion-obj](https://github.com/neverhood311/Stop-motion-OBJ) to load the output obj sequences and playback with the blender animation timeline 4 | -------------------------------------------------------------------------------- /resource/scripts/plot_error.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import sys 4 | import os 5 | 6 | fname = sys.argv[1] 7 | e = np.loadtxt(fname) 8 | 9 | plt.plot(e) 10 | plt.ylabel('Feasibility Score') 11 | plt.xlabel('Frame Number') 12 | outname = fname[0:len(fname)-4] + '.png' 13 | plt.savefig(outname) -------------------------------------------------------------------------------- /resource/scripts/poisson_6.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # set meshlabserver dir and script location 4 | MSERVER="/meshlab/distrib/meshlabserver" 5 | MLABSCRIPT_POISSON="/MeshTracker/resource/scripts/meshlab_poisson_6.mlx" 6 | 7 | # perform poisson remesh 8 | xvfb-run -a -s "-screen 0 800x600x24" $MSERVER -i $1 -o $2 -s $MLABSCRIPT_POISSON 9 | 10 | -------------------------------------------------------------------------------- /resource/scripts/plot_dist_weight.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import sys 4 | 5 | fname = sys.argv[1] 6 | e = np.loadtxt(fname) 7 | 8 | plt.plot(e) 9 | bottom, top = plt.ylim() 10 | plt.ylim(0,top) 11 | plt.ylabel('val') 12 | plt.xlabel('neighbor') 13 | outname = fname[0:len(fname)-4] + '.png' 14 | plt.savefig(outname) -------------------------------------------------------------------------------- /meshtracker/include/constants.h: -------------------------------------------------------------------------------- 1 | #ifndef CONSTANTS_H 2 | #define CONSTANTS_H 3 | 4 | // Deform Graph 5 | inline int kDG_KNN = 8; // for graph solve 6 | // inline const int kDeform_KNN = 8; // for graph deform 7 | // TODO: add to deform code 8 | 9 | // Path to solver options 10 | inline const std::string kSolverOptionsPath = 11 | "/MeshTracker/meshtracker/solver_options.conf"; 12 | 13 | #endif // CONSTANTS_H 14 | 15 | -------------------------------------------------------------------------------- /meshtracker/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(src) 2 | include_directories(include) 3 | file(GLOB HEADERS include/*.h) 4 | file(GLOB SOURCES src/*.cpp) 5 | 6 | add_executable(${PROJECTNAME} ${SOURCES} main.cpp) 7 | 8 | target_link_libraries(${PROJECTNAME} 9 | ${PCL_LIBRARIES} 10 | ${CGAL_LIBRARIES} 11 | ${OpenCV_LIBS} 12 | ${Boost_LIBRARIES} 13 | ${SuiteSparse_CHOLMOD_LIBRARY_RELEASE}) 14 | -------------------------------------------------------------------------------- /resource/scripts/decimate_meshes.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | MSERVER="/meshlab/distrib/meshlabserver" 4 | MLABSCRIPT="/MeshTracker/resource/scripts/meshlab_decimate.mlx" 5 | 6 | first_frame=00101 7 | last_frame=00886 8 | ext="obj" 9 | length=$(expr $last_frame - $first_frame) 10 | file_pref="Mesh-F" 11 | file_postf="."${ext} 12 | 13 | for frame in $(seq -w $first_frame $last_frame) 14 | do 15 | 16 | fname=${file_pref}${frame}${file_postf} 17 | xvfb-run -a -s "-screen 0 800x600x24" $MSERVER -i $fname -o $fname -s $MLABSCRIPT 18 | 19 | done 20 | -------------------------------------------------------------------------------- /dockers/dockerfile_cgal: -------------------------------------------------------------------------------- 1 | FROM mesh_tracker:pcl 2 | 3 | ARG NUM_JOBS=3 4 | RUN echo "Number of jobs to execute for make deps: " 5 | RUN echo ${NUM_JOBS} 6 | 7 | # Fixes no GMP warning 8 | RUN apt-get install -y libmpfr-dev libgmp-dev libboost-all-dev 9 | 10 | # CGAL 5.2 11 | RUN git clone https://github.com/CGAL/cgal.git \ 12 | && cd cgal \ 13 | && git checkout 485e672c26e71b2f10be2e48e5c98ed83e3762f7 \ 14 | && mkdir build \ 15 | && cd build \ 16 | && cmake -DCGAL_HEADER_ONLY=OFF -DCMAKE_BUILD_TYPE=Release .. \ 17 | && make -j${NUM_JOBS} \ 18 | && make install -------------------------------------------------------------------------------- /resource/scripts/gen_regions.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | matlab_script_path="/MeshTracker/resource/scripts" 4 | 5 | # Run MATLAB script that takes feature vectors as input and outputs keyframe indices 6 | echo "-------------------------------------------------" 7 | echo "KEYFRAME EXTRACTION VIA SHAPE SIMILARITY" 8 | echo "-------------------------------------------------" 9 | 10 | descriptors=$1 11 | feas=$2 12 | min_region=10 13 | matlab "$@" -nodesktop -nosplash -r "addpath(\"${matlab_script_path}\"); dot_similarity(\"${descriptors}\",\"${feas}\",$min_region); quit" 14 | 15 | exit; 16 | -------------------------------------------------------------------------------- /resource/scripts/dl_tests.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | gURL=1gI7_U5DuoNSRKe03PHRrLy5MzLaD7KFU 4 | 5 | # match more than 26 word characters 6 | ggID=$(echo "$gURL" | egrep -o '(\w|-){26,}') 7 | 8 | ggURL='https://drive.google.com/uc?export=download' 9 | 10 | apt-get install curl 11 | curl -sc /tmp/gcokie "${ggURL}&id=${ggID}" >/dev/null 12 | getcode="$(awk '/_warning_/ {print $NF}' /tmp/gcokie)" 13 | 14 | cmd='curl --insecure -C - -LOJb /tmp/gcokie "${ggURL}&confirm=${getcode}&id=${ggID}"' 15 | echo -e "Downloading from "$gURL"...\n" 16 | cd /MeshTracker/resource 17 | eval $cmd 18 | unzip tests.zip && rm tests.zip 19 | -------------------------------------------------------------------------------- /resource/scripts/meshlab_poisson.mlx: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /resource/scripts/meshlab_poisson_6.mlx: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /resource/scripts/affine_tform_meshes.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Perform an affine transformation to a sequence of meshes 4 | 5 | parent_path=$( cd "$(dirname "${BASH_SOURCE[0]}")" ; pwd -P ) 6 | 7 | MSERVER="/meshlab/distrib/meshlabserver" 8 | MLABSCRIPT="/MeshTracker/resource/scripts/abs_tform.mlx" 9 | 10 | first_frame=00001 11 | last_frame=00075 12 | ext="ply" 13 | length=$(expr $last_frame - $first_frame) 14 | file_pref="Frame_" 15 | file_postf="_hd_t."${ext} 16 | 17 | for frame in $(seq -w $first_frame $last_frame) 18 | do 19 | 20 | fname=${file_pref}${frame}${file_postf} 21 | xvfb-run -a -s "-screen 0 800x600x24" $MSERVER -i $fname -o $fname -s $MLABSCRIPT 22 | 23 | done 24 | -------------------------------------------------------------------------------- /dockers/build_dockers.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # enter the number of jobs to run in parallel for make commands(default=3) 4 | NUM_JOBS=$1 5 | 6 | ## Build dependencies in stages 7 | docker build --build-arg NUM_JOBS=${NUM_JOBS} -t mesh_tracker:base -f ./dockerfile_base . 8 | docker build --build-arg NUM_JOBS=${NUM_JOBS} -t mesh_tracker:meshlab -f ./dockerfile_meshlab . 9 | docker build --build-arg NUM_JOBS=${NUM_JOBS} -t mesh_tracker:pcl -f ./dockerfile_pcl . 10 | docker build --build-arg NUM_JOBS=${NUM_JOBS} -t mesh_tracker:cgal -f ./dockerfile_cgal . 11 | 12 | ## Main code and immediatey thirdparty deps contained here 13 | cd .. 14 | docker build --build-arg NUM_JOBS=${NUM_JOBS} -t mesh_tracker:latest -f ./dockers/dockerfile_build . -------------------------------------------------------------------------------- /resource/scripts/remesh_graph.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # set meshlabserver dir and script location 4 | MSERVER="/meshlab/distrib/meshlabserver" 5 | MLABSCRIPT_POISSON="/MeshTracker/resource/scripts/meshlab_poisson.mlx" 6 | MLABSCRIPT_ISOTROPIC="/MeshTracker/resource/scripts/meshlab_isotropic_remesh.mlx" 7 | 8 | # set instant meshes dir 9 | IMESH="/MeshTracker/resource/thirdparty/Instant_Meshes" 10 | 11 | # perform poisson mesh 12 | xvfb-run -a -s "-screen 0 800x600x24" $MSERVER -i $1 -o /tmp/remeshed_graph.ply -s $MLABSCRIPT_POISSON 13 | 14 | # perform uniform remesh 15 | # $MSERVER -i /tmp/remeshed_graph.ply -o $2 -s $MLABSCRIPT_ISOTROPIC 16 | $IMESH -o $2 -i -s $3 -r 6 -p 6 /tmp/remeshed_graph.ply 17 | 18 | # rm remeshed_graph.ply 19 | -------------------------------------------------------------------------------- /meshtracker/solver_options.conf: -------------------------------------------------------------------------------- 1 | // More options are available but these are most relevant 2 | // for fine tuning this application 3 | 4 | // default: 50 5 | max_num_iterations = 25 6 | 7 | // default: 1e4 8 | initial_trust_region_radius = 1e4 9 | 10 | // default: 1e16 11 | max_trust_region_radius = 1e16 12 | 13 | // default: 1e-32 14 | min_trust_region_radius = 1e-32 15 | 16 | // default: 1e-3 17 | min_relative_decrease = 1e-3 18 | 19 | // default: 1e6 20 | min_lm_diagonal = 1e6 21 | 22 | // default: 1e32 23 | max_lm_diagonal = 1e32 24 | 25 | // default: 5 26 | max_num_consecutive_invalid_steps = 5 27 | 28 | // default: 1e-6 29 | function_tolerance = 1e-5 30 | 31 | // default: 1e-10 32 | gradient_tolerance = 1e-8 33 | 34 | // default: 1e-8 35 | parameter_tolerance = 1e-7 36 | 37 | // options are DENSE_SCHUR or (default) SPARSE_NORMAL_CHOLESKY 38 | linear_solver_type = SPARSE_NORMAL_CHOLESKY 39 | 40 | // default: false 41 | minimizer_progress_to_stdout = true 42 | -------------------------------------------------------------------------------- /dockers/dockerfile_meshlab: -------------------------------------------------------------------------------- 1 | FROM mesh_tracker:base 2 | 3 | ARG NUM_JOBS 4 | 5 | RUN git clone --recursive --branch Meshlab-2020.03 https://github.com/cnr-isti-vclab/meshlab.git 6 | # WORKDIR /meshlab/ 7 | # RUN git checkout f0a3e261ea3abf5ad3067a875b8a90f0dc2d1422 8 | 9 | # Running to ovveride "sudo" in 0_setup_env_ubuntu.sh 10 | RUN apt-get update 11 | ENV DEBIAN_FRONTEND noninteractive 12 | RUN apt-get install -y qt5-default qttools5-dev-tools qtscript5-dev libqt5xmlpatterns5-dev mesa-common-dev libglu1-mesa-dev lib3ds-dev libglew-dev libeigen3-dev libopenctm-dev libgmp-dev libqhull-dev 13 | 14 | 15 | # # set meshlabserver build on by default 16 | # RUN sed -i 's/#add_subdirectory(meshlabserver)/add_subdirectory(meshlabserver)/g' /meshlab/src/CMakeLists.txt 17 | WORKDIR /meshlab/install/linux/ 18 | RUN sh linux_build.sh 19 | 20 | RUN export PATH=${PATH}:/meshlab/distrib 21 | 22 | WORKDIR / 23 | 24 | # To actually use meshlabserver you need to use a virtual framebuffer and export the display 25 | 26 | # Xvfb :100 27 | # export DISPLAY=:100.0 28 | -------------------------------------------------------------------------------- /dockers/dockerfile_pcl: -------------------------------------------------------------------------------- 1 | FROM mesh_tracker:meshlab 2 | 3 | ARG NUM_JOBS=3 4 | RUN echo "Number of jobs to execute for make deps: " 5 | RUN echo ${NUM_JOBS} 6 | 7 | # Install VTK-8.2.0 first 8 | WORKDIR / 9 | RUN wget https://www.vtk.org/files/release/8.2/VTK-8.2.0.zip && unzip VTK-8.2.0.zip && rm VTK-8.2.0.zip 10 | 11 | WORKDIR /VTK-8.2.0 12 | RUN mkdir build && cd build \ 13 | && cmake -DCMAKE_BUILD_TYPE=Release .. \ 14 | && make -j${NUM_JOBS} \ 15 | && make install 16 | 17 | WORKDIR / 18 | 19 | # PCL 1.8.1 # pull a fixed cmake file from future commit to fix lz4 issue 20 | RUN git clone https://github.com/PointCloudLibrary/pcl.git 21 | RUN cd pcl \ 22 | && git checkout f38c3cfd496c89b04858ebecf6f7afe4ad4bce50 \ 23 | # && git checkout d98313133b014553ab1b1b5b112f9aade837d55c cmake/Modules/FindFLANN.cmake \ 24 | && mkdir build \ 25 | && cd build \ 26 | && cmake -DBUILD_GPU=OFF -DBUILD_visualization=OFF -DCMAKE_BUILD_TYPE=Release .. \ 27 | && make -j${NUM_JOBS} \ 28 | && make install \ 29 | && cd ../../ \ 30 | && rm -rf pcl \ 31 | && cd 32 | -------------------------------------------------------------------------------- /resource/scripts/abs_tform.mlx: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /meshtracker/src/cpd_gauss_transform_make_default.cpp: -------------------------------------------------------------------------------- 1 | // cpd - Coherent Point Drift 2 | // Copyright (C) 2017 Pete Gadomski 3 | // 4 | // This program is free software; you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation; either version 2 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License along 15 | // with this program; if not, write to the Free Software Foundation, Inc., 16 | // 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | #include "cpd_gauss_transform.h" 19 | 20 | namespace cpd { 21 | std::unique_ptr GaussTransform::make_default() { 22 | return std::unique_ptr(new GaussTransformDirect()); 23 | } 24 | } // namespace cpd 25 | -------------------------------------------------------------------------------- /meshtracker/src/cpd_transform.cpp: -------------------------------------------------------------------------------- 1 | // cpd - Coherent Point Drift 2 | // Copyright (C) 2017 Pete Gadomski 3 | // 4 | // This program is free software; you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation; either version 2 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License along 15 | // with this program; if not, write to the Free Software Foundation, Inc., 16 | // 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | #include "cpd_transform.h" 19 | 20 | namespace cpd { 21 | 22 | void Result::denormalize(const Normalization& normalization) { 23 | points = points * normalization.fixed_scale + 24 | normalization.fixed_mean.transpose().replicate(points.rows(), 1); 25 | } 26 | } // namespace cpd 27 | -------------------------------------------------------------------------------- /meshtracker/include/log.h: -------------------------------------------------------------------------------- 1 | #ifndef LOG_H 2 | #define LOG_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace logging = boost::log; 14 | 15 | static void init_logging 16 | ( 17 | const int & verbosity, 18 | const std::string & fname 19 | ) 20 | { 21 | logging::register_simple_formatter_factory("Severity"); 22 | 23 | 24 | logging::add_file_log( 25 | boost::log::keywords::file_name = fname, 26 | boost::log::keywords::format = "[%TimeStamp%] %Message%", 27 | boost::log::keywords::auto_flush = true // potential performance increase when false 28 | ); 29 | 30 | // Severity Levels: 31 | // (0) trace, 32 | // (1) debug, 33 | // (2) info, 34 | // (3) warning, 35 | // (4) error, 36 | // (5) fatal 37 | 38 | logging::core::get()->set_filter 39 | ( 40 | logging::trivial::severity >= logging::trivial::severity_level(verbosity) 41 | ); 42 | 43 | logging::add_common_attributes(); 44 | } 45 | 46 | #endif -------------------------------------------------------------------------------- /meshtracker/src/cpd_matrix.cpp: -------------------------------------------------------------------------------- 1 | // cpd - Coherent Point Drift 2 | // Copyright (C) 2017 Pete Gadomski 3 | // 4 | // This program is free software; you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation; either version 2 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License along 15 | // with this program; if not, write to the Free Software Foundation, Inc., 16 | // 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | #include "cpd_matrix.h" 19 | 20 | namespace cpd { 21 | 22 | Matrix apply_transformation_matrix(Matrix points, const Matrix& transform) { 23 | Matrix::Index rows = points.rows(); 24 | Matrix::Index cols = points.cols() + 1; 25 | points.conservativeResize(rows, cols); 26 | points.col(cols - 1) = Vector::Ones(rows); 27 | Matrix transformed_points = points * transform.transpose(); 28 | return transformed_points.leftCols(cols - 1); 29 | } 30 | } // namespace cpd 31 | -------------------------------------------------------------------------------- /meshtracker/include/cpd_utils.h: -------------------------------------------------------------------------------- 1 | // cpd - Coherent Point Drift 2 | // Copyright (C) 2017 Pete Gadomski 3 | // 4 | // This program is free software; you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation; either version 2 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License along 15 | // with this program; if not, write to the Free Software Foundation, Inc., 16 | // 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | /// \file 19 | /// 20 | /// The always-present, always-ambiguous utils file. 21 | 22 | #pragma once 23 | 24 | #include "cpd_matrix.h" 25 | 26 | namespace cpd { 27 | 28 | /// Loads a matrix from a delimited text file. 29 | Matrix matrix_from_path(const std::string& path); 30 | 31 | /// Computes the default sigma2 for the given matrices. 32 | double default_sigma2(const Matrix& fixed, const Matrix& moving); 33 | 34 | /// Computes the affinity matrix between the two matrices. 35 | Matrix affinity(const Matrix& fixed, const Matrix& moving, double beta); 36 | } // namespace cpd 37 | -------------------------------------------------------------------------------- /dockers/dockerfile_base: -------------------------------------------------------------------------------- 1 | FROM ubuntu:20.04 2 | 3 | ARG NUM_JOBS 4 | RUN echo "Number of jobs to execute for make deps: " 5 | RUN echo ${NUM_JOBS} 6 | 7 | RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y --no-install-recommends \ 8 | build-essential \ 9 | git \ 10 | wget \ 11 | unzip \ 12 | cmake \ 13 | cmake-curses-gui \ 14 | libgtk2.0-dev \ 15 | pkg-config \ 16 | libavcodec-dev \ 17 | libavformat-dev \ 18 | python3-dev \ 19 | python3-numpy \ 20 | python3-pip \ 21 | python-setuptools \ 22 | libtbb-dev \ 23 | libjpeg-dev \ 24 | libpng-dev \ 25 | libtiff-dev \ 26 | mpi-default-dev \ 27 | openmpi-bin \ 28 | openmpi-common \ 29 | libflann-dev \ 30 | libeigen3-dev \ 31 | libboost-all-dev \ 32 | libvtk6-dev \ 33 | libqhull* \ 34 | freeglut3-dev \ 35 | libglew-dev \ 36 | pkg-config \ 37 | libproj-dev \ 38 | libatlas-base-dev \ 39 | libsuitesparse-dev \ 40 | libassimp-dev \ 41 | libglfw3-dev \ 42 | libglfw3 \ 43 | cimg-dev \ 44 | graphviz \ 45 | lcov \ 46 | ca-certificates \ 47 | software-properties-common \ 48 | g++ \ 49 | qt5-qmake \ 50 | qtscript5-dev \ 51 | libqt5xmlpatterns5-dev \ 52 | libqt5opengl5-dev \ 53 | assimp-utils \ 54 | nano \ 55 | lz4 \ 56 | xvfb \ 57 | && rm -rf /var/lib/apt/lists/* 58 | 59 | RUN cd / && wget https://github.com/Kitware/CMake/releases/download/v3.18.3/cmake-3.18.3-Linux-x86_64.tar.gz && tar -xvf *.tar.gz && rm cmake-3.18.3-Linux-x86_64.tar.gz && mv cmake-3.18.3-Linux-x86_64 cmake-3.18.3 60 | -------------------------------------------------------------------------------- /meshtracker/include/tri_mesh.h: -------------------------------------------------------------------------------- 1 | #ifndef _TRI_MESH_H_ 2 | #define _TRI_MESH_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | typedef pcl::PointCloud::Ptr CloudPtr; 17 | typedef pcl::geometry::PolygonMesh> Mesh; 18 | 19 | class TriMesh 20 | { 21 | public: 22 | struct PolyIndex 23 | { 24 | unsigned int vert_index[3]; 25 | unsigned int norm_index[3]; 26 | }; 27 | std::vector vertex_coord;// 28 | std::vector norm_coord; 29 | std::vector polyIndex; 30 | std::vector face_norm; 31 | Eigen::Vector3d BoundingBox_Min, BoundingBox_Max; //min max 32 | int vert_num; 33 | int poly_num; 34 | 35 | TriMesh() : vert_num(0), poly_num(0) {} 36 | TriMesh(const char* filename); 37 | TriMesh(pcl::PolygonMesh& pcl_mesh); 38 | CloudPtr ToPclPointCloud(); 39 | void updateNorm(); 40 | void prepareFaceNeighbours(std::vector>& neighbours); 41 | void saveOBJ(const char* filename); 42 | void savePLY(const char* filename); 43 | void getBoundingBox(Eigen::Vector3d& Min, Eigen::Vector3d& Max); 44 | private: 45 | void readPLY(const char* filename); 46 | void readOBJ(const char* filename); 47 | }; 48 | 49 | #endif/*_TRI_MESH_H_*/ 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /meshtracker/include/keyframer.h: -------------------------------------------------------------------------------- 1 | #ifndef _KEYFRAMER_H 2 | #define _KEYFRAMER_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "mesh_processing.h" 18 | 19 | typedef pcl::geometry::PolygonMesh> Mesh; 20 | 21 | 22 | class KeyFramer{ 23 | 24 | 25 | public: 26 | KeyFramer(){} 27 | ~KeyFramer(){} 28 | 29 | // Takes list of meshes as input, calculates keyframe locations and returns 30 | // kf indices for input list. 31 | std::vector GetKeyframeIndices(const std::string& _filename); 32 | 33 | // Read mesh list 34 | std::vector ReadMeshList(const std::string& _filename); 35 | 36 | // Computes the area of a surface mesh 37 | double ComputeMeshArea(const pcl::PolygonMesh& mesh); 38 | 39 | unsigned int ComputeMeshGenus(const Mesh& mesh); 40 | 41 | unsigned int ComputeMeshGenus(const pcl::PolygonMesh& mesh); 42 | 43 | void GenerateSPHDescriptors( 44 | const std::string & meshlist, 45 | const std::string & exe); 46 | 47 | // This function only exists to implement feasibility score of Collet et. al 48 | void GenerateFeasScore( 49 | std::vector meshlist, 50 | std::string outpath); 51 | 52 | void GenKeyframesAndRegions( 53 | const std::vector & meshlist); 54 | 55 | }; 56 | #endif 57 | -------------------------------------------------------------------------------- /meshtracker/src/cpd_normalization.cpp: -------------------------------------------------------------------------------- 1 | // cpd - Coherent Point Drift 2 | // Copyright (C) 2017 Pete Gadomski 3 | // 4 | // This program is free software; you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation; either version 2 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License along 15 | // with this program; if not, write to the Free Software Foundation, Inc., 16 | // 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | #include "cpd_normalization.h" 19 | 20 | namespace cpd { 21 | 22 | Normalization::Normalization(const Matrix& f, const Matrix& m, bool linked) 23 | : fixed_mean(f.colwise().mean()) 24 | , fixed(f - fixed_mean.transpose().replicate(f.rows(), 1)) 25 | , fixed_scale(std::sqrt(fixed.array().pow(2).sum() / fixed.rows())) 26 | , moving_mean(m.colwise().mean()) 27 | , moving(m - moving_mean.transpose().replicate(m.rows(), 1)) 28 | , moving_scale(std::sqrt(moving.array().pow(2).sum() / moving.rows())) { 29 | if (linked) { 30 | double scale = std::max(fixed_scale, moving_scale); 31 | fixed_scale = scale; 32 | moving_scale = scale; 33 | } 34 | fixed /= fixed_scale; 35 | moving /= moving_scale; 36 | } 37 | } // namespace cpd 38 | -------------------------------------------------------------------------------- /meshtracker/include/nonrigid_icp.h: -------------------------------------------------------------------------------- 1 | /* 2 | Modified by Matt Moynihan, https://github.com/mjkmoynihan 3 | Original Author: Yizhi Tang 4 | From: https://github.com/Tonsty/Non-Rigid-Registar 5 | Accessed September 7th 2018 6 | */ 7 | 8 | #ifndef NONRIGID_ICP_H 9 | #define NONRIGID_ICP_H 10 | 11 | #include 12 | 13 | typedef vtkSmartPointer VTKMesh; 14 | 15 | typedef VTKMesh Template; 16 | typedef VTKMesh Target; 17 | typedef std::pair Edge; 18 | typedef boost::shared_ptr< std::vector > Edges; 19 | typedef vtkSmartPointer Vertices; 20 | typedef boost::shared_ptr< std::vector > Weights; 21 | 22 | class NonRigidICP 23 | { 24 | public: 25 | NonRigidICP( 26 | Template _template, 27 | Target _target): 28 | template_(_template), 29 | target_(_target){} 30 | 31 | ~NonRigidICP(){} 32 | 33 | void init(); 34 | 35 | void initCompute(); 36 | 37 | void initCompute(const std::vector>&); 38 | 39 | void edgesInit(); 40 | 41 | void nearestSearchInit(); 42 | 43 | void verticesInit(); 44 | 45 | void correspondencesInit(); 46 | 47 | void setCorrespondences( 48 | const std::vector>& mt_matches); 49 | 50 | void weightsInit(); 51 | 52 | int compute(float alpha, float gamma); 53 | 54 | inline Template GetTemplate(){ 55 | return this->template_; 56 | } 57 | 58 | protected: 59 | Template template_; 60 | Target target_; 61 | 62 | Edges edges_; 63 | Vertices vertices_; 64 | 65 | // corr[src_id] = target_id ? 66 | Vertices correspondences_; 67 | Weights weights_; 68 | 69 | private: 70 | vtkSmartPointer cellLocator_; 71 | }; 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /resource/scripts/meshlab_scale.mlx: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /meshtracker/include/cpd_matrix.h: -------------------------------------------------------------------------------- 1 | // cpd - Coherent Point Drift 2 | // Copyright (C) 2017 Pete Gadomski 3 | // 4 | // This program is free software; you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation; either version 2 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License along 15 | // with this program; if not, write to the Free Software Foundation, Inc., 16 | // 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | /// \file 19 | /// 20 | /// Basic typedefs for our flavors of Eigen matrices. 21 | 22 | #pragma once 23 | 24 | #include 25 | 26 | 27 | namespace cpd { 28 | 29 | /// Our base matrix class. 30 | typedef Eigen::MatrixXd Matrix; 31 | 32 | /// Typedef for our specific type of vector. 33 | typedef Eigen::VectorXd Vector; 34 | 35 | /// Typedef for an index vector, used to index other matrices. 36 | typedef Eigen::Matrix IndexVector; 37 | 38 | /// Typedef for our specific type of array. 39 | typedef Eigen::ArrayXd Array; 40 | 41 | /// Apply a transformation matrix to a set of points. 42 | /// 43 | /// The transformation matrix should be one column wider than the point matrix. 44 | Matrix apply_transformation_matrix(Matrix points, const Matrix& transform); 45 | } // namespace cpd 46 | -------------------------------------------------------------------------------- /dockers/dockerfile_build: -------------------------------------------------------------------------------- 1 | FROM mesh_tracker:cgal 2 | 3 | ARG NUM_JOBS=8 4 | RUN echo "Number of jobs to execute for make deps: " 5 | RUN echo ${NUM_JOBS} 6 | 7 | # add GCC 9 and make it default compiller 8 | RUN add-apt-repository ppa:ubuntu-toolchain-r/test \ 9 | && apt-get update && apt-get install -y --no-install-recommends \ 10 | gcc-9 g++-9 \ 11 | && export CC=/usr/bin/gcc-9 \ 12 | && export CXX=/usr/bin/g++-9 \ 13 | && apt-get autoremove -y \ 14 | && apt-get clean 15 | 16 | # fix suitesparse linking error 17 | RUN sed -i 's///g' /usr/include/eigen3/Eigen/CholmodSupport 18 | 19 | RUN cd / && wget https://github.com/Kitware/CMake/releases/download/v3.18.3/cmake-3.18.3-Linux-x86_64.tar.gz && tar -xvf *.tar.gz && rm cmake-3.18.3-Linux-x86_64.tar.gz && mv cmake-3.18.3-Linux-x86_64 cmake-3.18.3 20 | 21 | WORKDIR /MeshTracker 22 | COPY /CMakeLists.txt /MeshTracker/CMakeLists.txt 23 | COPY /meshtracker /MeshTracker/meshtracker 24 | 25 | RUN mkdir -p build && cd build && /cmake-3.18.3/bin/cmake -DCMAKE_BUILD_TYPE=Release .. && make -j${NUM_JOBS} && make install 26 | # RUN sed -i.bak "/\b\(__CUDACC_VER__ is no longer supported\)\b/d" /usr/local/cuda/include/crt/common_functions.h 27 | 28 | # Sometimes need to update shared lib cache 29 | RUN ldconfig 30 | 31 | COPY /resource/ /MeshTracker/resource/ 32 | 33 | # Third-Party Resources 34 | RUN cd /MeshTracker/resource/thirdparty && wget https://instant-meshes.s3.eu-central-1.amazonaws.com/instant-meshes-linux.zip && unzip *.zip && mv 'Instant Meshes' Instant_Meshes && rm *.zip 35 | 36 | RUN mkdir -p /input 37 | RUN mkdir -p /result/smooth 38 | 39 | # Tests 40 | RUN sh /MeshTracker/resource/scripts/dl_tests.sh 41 | RUN chmod -R u=rw /MeshTracker/resource/tests/ 42 | RUN chmod -R u=rwx /MeshTracker/resource/scripts/ 43 | 44 | 45 | -------------------------------------------------------------------------------- /meshtracker/include/cpd_icp.h: -------------------------------------------------------------------------------- 1 | #ifndef CPD_ICP_H 2 | #define CPD_ICP_H 3 | 4 | using CloudPtr = pcl::PointCloud::Ptr; 5 | using MatCloudPtr = pcl::PointCloud::Ptr; 6 | 7 | struct CPD_Result{ 8 | 9 | CloudPtr aligned_cloud; 10 | pcl::PolygonMesh aligned_mesh; 11 | CloudPtr subsampled_moving; 12 | CloudPtr subsampled_fixed; 13 | CloudPtr lo_res_aligned; 14 | 15 | }; 16 | 17 | class CPDICP { 18 | 19 | public: 20 | CPDICP(){}; 21 | ~CPDICP(){}; 22 | 23 | // conversion functions 24 | cpd::Matrix PCLToCpdMat(const CloudPtr & ptcloud); 25 | CloudPtr CpdMatToPCL(const cpd::Matrix & mat); 26 | 27 | cpd::NonrigidResult NonRigidRegistrationImpl( 28 | const cpd::Matrix & fixed, 29 | const cpd::Matrix & moving, 30 | const double & tolerance); 31 | 32 | // encapsulated function call to cpd::getAffinity 33 | void getAffinity( 34 | const cpd::Matrix & fixed, 35 | const cpd::Matrix & moving, 36 | cpd::Matrix & _aff) 37 | { 38 | cpd::Nonrigid nr; 39 | nr.getAffinity(fixed,moving,_aff); 40 | } 41 | 42 | // Use cpd to align high res meshses by downsampling and applying low-res 43 | // alignment to a high res mesh. 44 | pcl::PolygonMesh AlignHighResMeshes( 45 | const pcl::PolygonMesh & _mesh_fixed, 46 | const pcl::PolygonMesh & _mesh_moving, 47 | const double & tolerance); 48 | 49 | // Overrride for CloudPtr input 50 | CPD_Result AlignHighResClouds( 51 | const CloudPtr & _cloud_fixed, 52 | const CloudPtr & _cloud_moving, 53 | const double & tolerance); 54 | 55 | inline void SetTrackedMeshNum(const int & mesh_num){ 56 | tracked_mesh_num=mesh_num; 57 | } 58 | 59 | inline int GetTrackedMeshNum() const{ 60 | return tracked_mesh_num; 61 | } 62 | 63 | 64 | 65 | private: 66 | int tracked_mesh_num = 0; 67 | const int kSamplesUpperLimit = 800; 68 | const int kSamplesLowerLimit = 50; 69 | 70 | }; 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /resource/scripts/meshlab_isotropic_remesh.mlx: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /meshtracker/include/gauss_newton_solver.h: -------------------------------------------------------------------------------- 1 | #ifndef GAUSS_NEWTON_SOLVER_H 2 | #define GAUSS_NEWTON_SOLVER_H 3 | 4 | #include "utils.h" 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | // Node idx, corrspondence pairs 20 | typedef std::vector> NodeCorrs; 22 | 23 | struct GNParams 24 | { 25 | GNParams( 26 | double alpha_rigid = 500.0, // 1000.0 27 | double alpha_smooth = 500.0, //1000.0 28 | double alpha_point = 0.1, //0.1 29 | double alpha_plane = 1.0) : //1.0 30 | m_alpha_rigid(alpha_rigid), 31 | m_alpha_smooth(alpha_smooth), 32 | m_alpha_point(alpha_point), 33 | m_alpha_plane(alpha_plane) 34 | {} 35 | 36 | // weights 37 | double m_alpha_rigid = 0.0; 38 | double m_alpha_smooth = 0.0; 39 | double m_alpha_point = 0.0; 40 | double m_alpha_plane = 0.0; 41 | 42 | void RelaxParams(void) 43 | { 44 | this->m_alpha_rigid /= 2.0; 45 | this->m_alpha_smooth /= 2.0; 46 | } 47 | }; 48 | 49 | class GaussNewtonSolver 50 | { 51 | private: 52 | GNParams params_; 53 | 54 | public: 55 | GaussNewtonSolver(){}; 56 | ~GaussNewtonSolver(){}; 57 | 58 | inline void SetParams(const GNParams p) 59 | { 60 | this->params_=p; 61 | } 62 | 63 | // Solve use Gauss-Newton Iterative Minimization. 64 | // returns residual energy 65 | double solve( 66 | const std::vector> & node_node_neigh, 67 | const std::vector> & node_node_weights, 68 | const NodeCorrs & constraints, 69 | const std::vector> & con_node_neigh, 70 | const std::vector> & con_neigh_weights, 71 | std::vector & node_pos, 72 | Eigen::VectorXd & affine_vector); 73 | }; 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /meshtracker/include/cpd_normalization.h: -------------------------------------------------------------------------------- 1 | // cpd - Coherent Point Drift 2 | // Copyright (C) 2017 Pete Gadomski 3 | // 4 | // This program is free software; you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation; either version 2 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License along 15 | // with this program; if not, write to the Free Software Foundation, Inc., 16 | // 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | /// \file 19 | /// 20 | /// Utility to scale/offset points to (roughly) conform to unit shape centered 21 | /// at zero. 22 | 23 | #pragma once 24 | 25 | #include "cpd_matrix.h" 26 | 27 | namespace cpd { 28 | 29 | /// The results of normalizing data to a unit cube (or whatever dimensionality). 30 | struct Normalization { 31 | /// The average of the fixed points, that was subtracted from those data. 32 | Vector fixed_mean; 33 | /// The fixed points. 34 | Matrix fixed; 35 | /// The scaling factor for the fixed points. 36 | double fixed_scale; 37 | /// The average of the moving points, that was subtracted from those data. 38 | Vector moving_mean; 39 | /// The moving points. 40 | Matrix moving; 41 | /// The scaling factor for the moving points. 42 | double moving_scale; 43 | 44 | /// Creates a new normalization for the provided matrices. 45 | /// 46 | /// If `linked = true`, apply the same scaling to both sets of points. This 47 | /// is recommended if you are working with data that should not be scaled, 48 | /// e.g. LiDAR data. If `linked = false`, each point set is scaled 49 | /// seperately. 50 | /// 51 | /// Myronenko's original implementation only had `linked = false` logic. 52 | Normalization(const Matrix& fixed, const Matrix& moving, 53 | bool linked = true); 54 | }; 55 | } // namespace cpd 56 | -------------------------------------------------------------------------------- /meshtracker/include/cpd_gauss_transform.h: -------------------------------------------------------------------------------- 1 | // cpd - Coherent Point Drift 2 | // Copyright (C) 2017 Pete Gadomski 3 | // 4 | // This program is free software; you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation; either version 2 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License along 15 | // with this program; if not, write to the Free Software Foundation, Inc., 16 | // 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | /// \file 19 | /// 20 | /// Basic correspondence/error calculation between two datasets, using the 21 | /// direct method of the Gauss transform. 22 | 23 | #pragma once 24 | 25 | #include "cpd_matrix.h" 26 | #include 27 | 28 | namespace cpd { 29 | 30 | /// Probability matrices produced by comparing two data sets with a 31 | /// `GaussTransform`. 32 | struct Probabilities { 33 | /// The probability matrix, multiplied by the identity matrix. 34 | Vector p1; 35 | /// The probability matrix, transposes, multiplied by the identity matrix. 36 | Vector pt1; 37 | /// The probability matrix multiplied by the fixed points. 38 | Matrix px; 39 | /// The total error. 40 | double l; 41 | /// The correspondence vector between the two datasets. 42 | IndexVector correspondence; 43 | }; 44 | 45 | /// Abstract base class for Gauss transforms. 46 | class GaussTransform { 47 | public: 48 | /// Returns the default Gauss transform as a unique ptr. 49 | static std::unique_ptr make_default(); 50 | 51 | /// Computes the Gauss transform. 52 | virtual Probabilities compute(const Matrix& fixed, const Matrix& moving, 53 | double sigma2, double outliers) const = 0; 54 | }; 55 | 56 | /// The direct Gauss transform. 57 | class GaussTransformDirect : public GaussTransform { 58 | public: 59 | Probabilities compute(const Matrix& fixed, const Matrix& moving, 60 | double sigma2, double outliers) const; 61 | }; 62 | } // namespace cpd 63 | -------------------------------------------------------------------------------- /resource/thirdparty/LICENSE.txt: -------------------------------------------------------------------------------- 1 | ----- Spherical Harmonics Copyright ---- 2 | Spherical Harmonic Transform Kit 2.5 3 | 4 | Sean Moore, Dennis Healy, Dan Rockmore, Peter Kostelec 5 | smoore@bbn.com, {healy,rockmore,geelong}@cs.dartmouth.edu 6 | 7 | Contact: Peter Kostelec 8 | geelong@cs.dartmouth.edu 9 | 10 | 11 | Copyright 1997, 1998 Sean Moore, Dennis Healy, 12 | Dan Rockmore, Peter Kostelec 13 | 14 | 15 | This program is free software; you can redistribute it and/or modify 16 | it under the terms of the GNU General Public License as published by 17 | the Free Software Foundation; either version 2 of the License, or 18 | (at your option) any later version. 19 | 20 | This program is distributed in the hope that it will be useful, 21 | but WITHOUT ANY WARRANTY; without even the implied warranty of 22 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 23 | GNU General Public License for more details. 24 | 25 | You should have received a copy of the GNU General Public License 26 | along with this program; if not, write to the Free Software 27 | Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. 28 | 29 | 30 | Commercial use is absolutely prohibited. 31 | 32 | 33 | ---- GAPS copyright ----- 34 | Copyright (c) 2007 Thomas Funkhouser 35 | 36 | Permission is hereby granted, free of charge, to any person obtaining a copy 37 | of this software and associated documentation files (the "Software"), to deal 38 | in the Software without restriction, including without limitation the rights 39 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 40 | copies of the Software, and to permit persons to whom the Software is 41 | furnished to do so, subject to the following conditions: 42 | 43 | The above copyright notice and this permission notice shall be included in 44 | all copies or substantial portions of the Software. 45 | 46 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 47 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 48 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 49 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 50 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 51 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 52 | THE SOFTWARE. 53 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | CMAKE_MINIMUM_REQUIRED(VERSION 3.4.1) 2 | 3 | 4 | SET(PROJECTNAME "MeshTracker") 5 | PROJECT(${PROJECTNAME} C CXX) 6 | 7 | set (CMAKE_BUILD_TYPE "Release") 8 | 9 | set (CMAKE_CXX_STANDARD 17) 10 | 11 | include_directories( ${CMAKE_CURRENT_BINARY_DIR} ) 12 | 13 | # Add CGAL 14 | FIND_PACKAGE(CGAL REQUIRED) 15 | IF (CGAL_FOUND) 16 | message(CGAL_FOUND) 17 | include_directories(${CGAL_INCLUDE_DIRS}) 18 | link_directories(${CGAL_LIBRARY_DIRS}) 19 | include(${CGAL_USE_FILE}) 20 | ENDIF (CGAL_FOUND) 21 | 22 | # Add Eigen3 23 | FIND_PACKAGE(Eigen3 REQUIRED) 24 | IF (EIGEN3_FOUND) 25 | message(EIGEN3_FOUND) 26 | INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR}) 27 | ENDIF (EIGEN3_FOUND) 28 | 29 | # Add SuiteSparse 30 | find_package(SuiteSparse REQUIRED COMPONENTS CHOLMOD) 31 | if (SUITESPARSE_FOUND) 32 | message(SUITESPARSE_FOUND) 33 | add_library (SuiteSparse::Cholmod INTERFACE IMPORTED) 34 | target_link_libraries (SuiteSparse::Cholmod INTERFACE ${SuiteSparse_CHOLMOD_LIBRARY_RELEASE}) 35 | target_include_directories (SuiteSparse::Cholmod INTERFACE ${SuiteSparse_INCLUDE_DIRS}) 36 | ENDIF(SUITESPARSE_FOUND) 37 | 38 | # Add OpenCV 39 | find_package(OpenCV REQUIRED) 40 | if (OPENCV_FOUND) 41 | message(OPENCV_FOUND) 42 | INCLUDE_DIRECTORIES(${OPENCV_INCLUDE_DIRS}) 43 | ENDIF (OPENCV_FOUND) 44 | 45 | # Add Point Cloud Library 46 | find_package(PCL 1.8 REQUIRED) 47 | if (PCL_FOUND) 48 | message(PCL_FOUND) 49 | list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") 50 | add_definitions(-DPCL_NEW=1) 51 | include_directories(${PCL_INCLUDE_DIRS}) 52 | link_directories(${PCL_LIBRARY_DIRS}) 53 | add_definitions(${PCL_DEFINITIONS}) 54 | endif (PCL_FOUND) 55 | 56 | # Add Boost 57 | FIND_PACKAGE(Boost COMPONENTS program_options log REQUIRED) 58 | IF (Boost_FOUND) 59 | message(BOOST_FOUND) 60 | ADD_DEFINITIONS(-DBOOST_LOG_DYN_LINK) 61 | INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS}) 62 | ENDIF (Boost_FOUND) 63 | 64 | #Add VTK 65 | FIND_PACKAGE(VTK REQUIRED) 66 | IF (VTK_FOUND) 67 | message(VTK_FOUND) 68 | INCLUDE (${VTK_USE_FILE}) 69 | ENDIF (VTK_FOUND) 70 | 71 | find_package(OpenMP) 72 | if (OPENMP_FOUND) 73 | message(OPENMP_FOUND) 74 | set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 75 | set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 76 | set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 77 | endif() 78 | 79 | add_subdirectory(meshtracker) 80 | 81 | install(TARGETS ${PROJECTNAME} DESTINATION bin) -------------------------------------------------------------------------------- /meshtracker/include/kalman_smoothing.h: -------------------------------------------------------------------------------- 1 | #ifndef KALMAN_SMOOTHING_H 2 | #define KALMAN_SMOOTHING_H 3 | 4 | class KalmanSmoother 5 | { 6 | 7 | public: 8 | KalmanSmoother(){}; 9 | ~KalmanSmoother(){}; 10 | 11 | inline void setMeshNames(const std::vector & _meshNames) 12 | { 13 | meshNames_ = _meshNames; 14 | } 15 | 16 | inline const std::vector & getMeshNames() const 17 | { 18 | return meshNames_; 19 | } 20 | 21 | inline void setMeshes(const std::vector & _meshes) 22 | { 23 | meshes_ = _meshes; 24 | } 25 | 26 | inline const std::vector & getMeshes() const 27 | { 28 | return meshes_; 29 | } 30 | 31 | inline void setClouds(const std::vector::Ptr> & _clouds) 32 | { 33 | clouds_ = _clouds; 34 | } 35 | 36 | inline const std::vector::Ptr> & getClouds() const 37 | { 38 | return clouds_; 39 | } 40 | 41 | inline void setKeyFrameIndices(const std::vector & _keyFrameIndices) 42 | { 43 | keyFrameIndices_ = _keyFrameIndices; 44 | } 45 | 46 | inline const std::vector &getKeyFrameIndices() const 47 | { 48 | return keyFrameIndices_; 49 | } 50 | 51 | // Read the mesh sequence and keyframes from a text file 52 | int readMeshes(const std::string & _fileName); 53 | 54 | // Finds the keyframe indices in the mesh sequence 55 | int ReadRegions(const std::string & _fileName); 56 | 57 | // Smooths the mesh sequence keyframe to keyframe 58 | // It is simpler but not the rigth way 59 | void smoothLinearly(); 60 | 61 | // 62 | void smoothInRegions(); 63 | 64 | // 3D Kalman filter to smooth the sequence 65 | void filterKalman( 66 | const std::vector & _input, 67 | std::vector & _output) const; 68 | 69 | // Takes the smooth clouds and builds meshes 70 | std::vector setupSmoothMeshes(bool _export = true) const; 71 | 72 | private: 73 | // 74 | void setupClouds(); 75 | 76 | std::vector meshNames_; 77 | std::vector meshes_; 78 | std::vector::Ptr> clouds_; 79 | std::vector keyFrameIndices_; 80 | std::vector> regions_; 81 | std::vector::Ptr> smoothClouds_; 82 | }; 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /meshtracker/src/cpd_gauss_transform.cpp: -------------------------------------------------------------------------------- 1 | // cpd - Coherent Point Drift 2 | // Copyright (C) 2017 Pete Gadomski 3 | // 4 | // This program is free software; you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation; either version 2 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License along 15 | // with this program; if not, write to the Free Software Foundation, Inc., 16 | // 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | #define _USE_MATH_DEFINES 19 | 20 | #include "cpd_gauss_transform.h" 21 | #include 22 | #include 23 | 24 | namespace cpd { 25 | Probabilities GaussTransformDirect::compute(const Matrix& fixed, 26 | const Matrix& moving, double sigma2, 27 | double outliers) const { 28 | double ksig = -2.0 * sigma2; 29 | size_t cols = fixed.cols(); 30 | double outlier_tmp = 31 | (outliers * moving.rows() * std::pow(-ksig * M_PI, 0.5 * cols)) / 32 | ((1 - outliers) * fixed.rows()); 33 | Vector p = Vector::Zero(moving.rows()); 34 | Vector p1 = Vector::Zero(moving.rows()); 35 | Vector p1_max = Vector::Zero(moving.rows()); 36 | Vector pt1 = Vector::Zero(fixed.rows()); 37 | Matrix px = Matrix::Zero(moving.rows(), cols); 38 | IndexVector correspondence = IndexVector::Zero(moving.rows()); 39 | double l = 0.0; 40 | 41 | for (Matrix::Index i = 0; i < fixed.rows(); ++i) { 42 | double sp = 0; 43 | for (Matrix::Index j = 0; j < moving.rows(); ++j) { 44 | double razn = (fixed.row(i) - moving.row(j)).array().pow(2).sum(); 45 | p(j) = std::exp(razn / ksig); 46 | sp += p(j); 47 | } 48 | sp += outlier_tmp; 49 | pt1(i) = 1 - outlier_tmp / sp; 50 | for (Matrix::Index j = 0; j < moving.rows(); ++j) { 51 | p1(j) += p(j) / sp; 52 | px.row(j) += fixed.row(i) * p(j) / sp; 53 | if (p(j) / sp > p1_max(j)) { 54 | correspondence(j) = i; 55 | p1_max(j) = p(j) / sp; 56 | } 57 | } 58 | l += -std::log(sp); 59 | } 60 | l += cols * fixed.rows() * std::log(sigma2) / 2; 61 | return { p1, pt1, px, l, correspondence }; 62 | } 63 | } // namespace cpd 64 | -------------------------------------------------------------------------------- /meshtracker/src/cpd_nonrigid.cpp: -------------------------------------------------------------------------------- 1 | // cpd - Coherent Point Drift 2 | // Copyright (C) 2017 Pete Gadomski 3 | // 4 | // This program is free software; you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation; either version 2 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License along 15 | // with this program; if not, write to the Free Software Foundation, Inc., 16 | // 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | #include "cpd_nonrigid.h" 19 | 20 | namespace cpd { 21 | 22 | void Nonrigid::init(const Matrix& fixed, const Matrix& moving) { 23 | 24 | m_g = affinity(moving, moving, m_beta); 25 | m_w = Matrix::Zero(moving.rows(), moving.cols()); 26 | m_w = Matrix::Ones(moving.rows(), moving.cols()); 27 | // kIteration=0; 28 | } 29 | 30 | void Nonrigid::getAffinity(const Matrix& fixed, const Matrix& moving, Matrix& _aff){ 31 | _aff = affinity(fixed, moving, m_beta); 32 | } 33 | 34 | void Nonrigid::modify_probabilities(Probabilities& probabilities) const { 35 | probabilities.l += m_lambda / 2.0 * (m_w.transpose() * m_g * m_w).trace(); 36 | } 37 | 38 | NonrigidResult Nonrigid::compute_one(const Matrix& fixed, const Matrix& moving, 39 | const Probabilities& probabilities, 40 | double sigma2) const { 41 | size_t cols = fixed.cols(); 42 | auto dp = probabilities.p1.asDiagonal(); 43 | 44 | 45 | Matrix to_solve =dp * m_g + m_lambda * sigma2 * 46 | Matrix::Identity(moving.rows(), moving.rows()); 47 | Matrix w = (to_solve).colPivHouseholderQr().solve(probabilities.px - dp * moving); 48 | NonrigidResult result; 49 | result.points = moving + m_g * w; 50 | result.tform = w; 51 | double np = probabilities.p1.sum(); 52 | result.sigma2 = std::abs( 53 | ((fixed.array().pow(2) * probabilities.pt1.replicate(1, cols).array()) 54 | .sum() + 55 | (result.points.array().pow(2) * 56 | probabilities.p1.replicate(1, cols).array()) 57 | .sum() - 58 | 2 * (probabilities.px.transpose() * result.points).trace()) / 59 | (np * cols)); 60 | return result; 61 | } 62 | 63 | NonrigidResult nonrigid(const Matrix& fixed, const Matrix& moving) { 64 | Nonrigid nonrigid; 65 | return nonrigid.run(fixed, moving); 66 | } 67 | } // namespace cpd 68 | -------------------------------------------------------------------------------- /resource/scripts/meshlab_transform.mlx: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /meshtracker/include/cpd_nonrigid.h: -------------------------------------------------------------------------------- 1 | // cpd - Coherent Point Drift 2 | // Copyright (C) 2017 Pete Gadomski 3 | // 4 | // This program is free software; you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation; either version 2 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License along 15 | // with this program; if not, write to the Free Software Foundation, Inc., 16 | // 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | /// \file 19 | /// 20 | /// Nonrigid coherent point drift transform. 21 | 22 | #pragma once 23 | 24 | #include "cpd_transform.h" 25 | 26 | namespace cpd { 27 | 28 | /// Default value for beta. 29 | const double DEFAULT_BETA = 5.0; //default 3 30 | /// Default value for lambda. 31 | const double DEFAULT_LAMBDA = 5.0; //default 3 32 | 33 | /// The result of a nonrigid coherent point drift run. 34 | struct NonrigidResult : public Result {}; 35 | 36 | /// Nonrigid coherent point drift. 37 | class Nonrigid : public Transform { 38 | public: 39 | Nonrigid() 40 | : Transform() 41 | , m_lambda(DEFAULT_LAMBDA) 42 | , m_beta(DEFAULT_BETA) 43 | , m_linked(DEFAULT_LINKED) {} 44 | 45 | /// Initialize this transform for the provided matrices. 46 | void init(const Matrix& fixed, const Matrix& moving); 47 | 48 | void getAffinity(const Matrix& fixed, const Matrix& moving, Matrix& _aff); 49 | 50 | /// Modifies the probabilities with some affinity and weight information. 51 | void modify_probabilities(Probabilities& probabilities) const; 52 | 53 | /// Sets the beta. 54 | Nonrigid& beta(double beta) { 55 | m_beta = beta; 56 | return *this; 57 | } 58 | 59 | /// Sets the lambda. 60 | Nonrigid& lambda(double lambda) { 61 | m_lambda = lambda; 62 | return *this; 63 | } 64 | 65 | /// Computes one iteration of the nonrigid transformation. 66 | NonrigidResult compute_one(const Matrix& fixed, const Matrix& moving, 67 | const Probabilities& probabilities, 68 | double sigma2) const; 69 | 70 | /// Sets whether the scalings of the two datasets are linked. 71 | Nonrigid& linked(bool linked) { 72 | m_linked = linked; 73 | return *this; 74 | } 75 | 76 | virtual bool linked() const { return m_linked; } 77 | 78 | private: 79 | Matrix m_g; 80 | Matrix m_w; 81 | double m_lambda; 82 | double m_beta; 83 | bool m_linked; 84 | }; 85 | 86 | /// Runs a nonrigid registration on two matrices. 87 | NonrigidResult nonrigid(const Matrix& fixed, const Matrix& moving); 88 | } // namespace cpd 89 | -------------------------------------------------------------------------------- /meshtracker/src/cpd_utils.cpp: -------------------------------------------------------------------------------- 1 | // cpd - Coherent Point Drift 2 | // Copyright (C) 2017 Pete Gadomski 3 | // 4 | // This program is free software; you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation; either version 2 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License along 15 | // with this program; if not, write to the Free Software Foundation, Inc., 16 | // 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include "cpd_matrix.h" 23 | #include "cpd_utils.h" 24 | 25 | namespace cpd { 26 | 27 | Matrix matrix_from_path(const std::string& path) { 28 | std::ifstream file(path); 29 | if (!file.is_open()) { 30 | std::stringstream msg; 31 | msg << "Unable to open file for reading: " << path; 32 | throw std::runtime_error(msg.str()); 33 | } 34 | std::string line; 35 | std::vector> rows; 36 | while (std::getline(file, line)) { 37 | std::vector row; 38 | std::stringstream ss(line); 39 | double n; 40 | while (ss >> n) { 41 | row.push_back(n); 42 | // TODO support other delimiters than commas 43 | if (ss.peek() == ',') { 44 | ss.ignore(); 45 | } 46 | } 47 | if (!rows.empty() && rows.back().size() != row.size()) { 48 | std::stringstream msg; 49 | msg << "Irregular number of rows: " << rows.back().size() << ", " 50 | << row.size(); 51 | throw std::runtime_error(msg.str()); 52 | } 53 | rows.push_back(row); 54 | } 55 | if (rows.empty()) { 56 | return Matrix(0, 0); 57 | } 58 | size_t nrows = rows.size(); 59 | size_t ncols = rows[0].size(); 60 | Matrix matrix(nrows, ncols); 61 | for (size_t i = 0; i < nrows; ++i) { 62 | for (size_t j = 0; j < ncols; ++j) { 63 | matrix(i, j) = rows[i][j]; 64 | } 65 | } 66 | return matrix; 67 | } 68 | 69 | double default_sigma2(const Matrix& fixed, const Matrix& moving) { 70 | return ((moving.rows() * (fixed.transpose() * fixed).trace()) + 71 | (fixed.rows() * (moving.transpose() * moving).trace()) - 72 | 2 * fixed.colwise().sum() * moving.colwise().sum().transpose()) / 73 | (fixed.rows() * moving.rows() * fixed.cols()); 74 | } 75 | 76 | Matrix affinity(const Matrix& x, const Matrix& y, double beta) { 77 | double k = -2.0 * beta * beta; 78 | double k_rcp = 1.0/k; 79 | size_t x_rows = x.rows(); 80 | size_t y_rows = y.rows(); 81 | Matrix g; 82 | try { 83 | g = Matrix(x_rows, y_rows); 84 | } catch (const std::bad_alloc& err) { 85 | std::stringstream msg; 86 | msg << "Unable to allocate " << x_rows << " by " << y_rows 87 | << " affinity matrix, try again with fewer points"; 88 | throw std::runtime_error(msg.str()); 89 | } 90 | for (size_t i = 0; i < y_rows; ++i) { 91 | g.col(i) = ((x.array() - y.row(i).replicate(x_rows, 1).array()) 92 | .pow(2) 93 | .rowwise() 94 | .sum() / 95 | k) 96 | .exp(); 97 | } 98 | return g; 99 | } 100 | } // namespace cpd 101 | -------------------------------------------------------------------------------- /resource/scripts/meshlab_decimate.mlx: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /meshtracker/include/matching.h: -------------------------------------------------------------------------------- 1 | #ifndef MATCHING_H 2 | #define MATCHING_H 3 | 4 | #include "cloud_processing.h" 5 | #include "mesh_processing.h" 6 | #include "log.h" 7 | 8 | // 9 | typedef std::pair match_conf; 10 | typedef pcl::PointCloud::Ptr CloudPtr; 11 | 12 | // Use to find correspondence pairs between point clouds or meshes. 13 | // Functions can return confidences or use KNN to assume matches where 14 | // none are found. 15 | class Matcher 16 | { 17 | private: 18 | // TODO: Getter setter for source target with template func 19 | // to accept either mesh or pt cloud 20 | 21 | public: 22 | Matcher(){}; 23 | ~Matcher(){}; 24 | 25 | // Simple pcl nearest neighbour search that will match 1-to-1 for each input 26 | std::vector GetKNNMatches( 27 | const CloudPtr & source, 28 | const CloudPtr & query, 29 | const int & search_length = 10); 30 | 31 | // Simple pcl nearest neighbour search that will match 1-to-1 for each input 32 | std::vector GetKNNMatches( 33 | const pcl::PolygonMesh & source, 34 | const pcl::PolygonMesh & query, 35 | const int & search_length = 10); 36 | 37 | // Simple pcl nearest neighbour search that will match 1-to-1 for each input 38 | std::vector GetKNNMatches( 39 | const pcl::PointXYZRGBNormal & source, 40 | const CloudPtr & query, 41 | const int & tolerance, 42 | int search_length = 10); 43 | 44 | // Returns only the point correspondences which conform to strict matching rules. 45 | // i.e. sparse matching only 46 | std::vector GetConstrainedMatches( 47 | const pcl::PolygonMesh & source, 48 | const pcl::PolygonMesh & query, 49 | double align_tolerance = 0.25); 50 | 51 | // Returns only the point correspondences which conform to strict matching rules. 52 | // i.e. sparse matching only 53 | std::vector GetConstrainedMatches( 54 | const CloudPtr & source, 55 | const CloudPtr & query, 56 | double align_tolerance = 0.25, 57 | float radius = 0); // flag to auto calculate max_radius 58 | 59 | // Returns only the point correspondences which conform to strict matching rules. 60 | // i.e. sparse matching only 61 | std::vector GetConstrainedMatches( 62 | const pcl::PointXYZRGBNormal & source, 63 | const CloudPtr & query, 64 | const double & tolerance, 65 | int search_length = 10); 66 | 67 | // Performs KNN search from source to query, returns exact 1-to-1 matches as well 68 | // as confidence between [0, 1.0] based on normal alignment 69 | std::vector GetMatchesAndConfidence( 70 | CloudPtr & source, 71 | CloudPtr & query, 72 | const float normal_tol = 0.5, 73 | const float max_ray_dist = 2.0); 74 | 75 | // // Debug method outputs a colorized cloud which illustrates matching 76 | // void VisualizeMatches( 77 | // const std::vector& matches, 78 | // const CloudPtr& source, 79 | // const CloudPtr& query, 80 | // CloudPtr& _colored_source, 81 | // CloudPtr& _colored_query); 82 | 83 | // override for polygonmesh input 84 | void VisualizeMatches( 85 | const std::vector & matches, 86 | const pcl::PolygonMesh & source, 87 | const pcl::PolygonMesh & query, 88 | pcl::PolygonMesh & _colored_source, 89 | pcl::PolygonMesh & _colored_query); 90 | 91 | // Override for basic KNNMatches 92 | void VisualizeMatches( 93 | const std::vector & matches, 94 | const CloudPtr & source, 95 | const CloudPtr & query, 96 | CloudPtr & _colored_source, 97 | CloudPtr & _colored_query); 98 | 99 | // Override for basic KNNMatches 100 | void VisualizeMatches( 101 | const std::vector & matches, 102 | const CloudPtr & source, 103 | const CloudPtr & query, 104 | CloudPtr & _colored_source, 105 | CloudPtr & _colored_query); 106 | }; 107 | 108 | 109 | 110 | #endif -------------------------------------------------------------------------------- /resource/scripts/dot_similarity.m: -------------------------------------------------------------------------------- 1 | 2 | function keyframe_indices = dot_similarity(descriptor_path,feas_path,max_region_length) 3 | 4 | % grab the output path 5 | [filepath] = fileparts(char(descriptor_path)); 6 | 7 | descriptors = load(descriptor_path); 8 | 9 | for row_idx = 1:size(descriptors,1) 10 | for row_idx_2 = 1:size(descriptors,1) 11 | 12 | %Generate similarity using simple dot product of features 13 | similarity(row_idx,row_idx_2)=dot(descriptors(row_idx,:),descriptors(row_idx_2,:)); 14 | 15 | end 16 | end 17 | 18 | % normalize and plot 19 | sim_norm=normalize(similarity,'range'); 20 | heatmap(sim_norm); 21 | 22 | % get average sim score per frame 23 | averaged_signal = mean(sim_norm,2); 24 | 25 | % load feas score 26 | feas = load(feas_path); 27 | 28 | % filter data with moving average 29 | % define coeffs 30 | a=1; 31 | % b=repmat(1/4,1,window_size); 32 | window_size = max_region_length/2; %ceil(seq_length * 0.025); 33 | b=(1/window_size)*ones(1,window_size); 34 | 35 | filtered_signal=filter(b,a,averaged_signal); 36 | 37 | plot(filtered_signal,'b','LineWidth',1); 38 | set(get(gca, 'XLabel'), 'String', 'Frame number'); 39 | set(get(gca, 'YLabel'), 'String', 'Score'); 40 | hold on; 41 | 42 | % define region locations about least similar points 43 | %local_minima=islocalmin(filtered_signal,'MinProminence',min_prominence); 44 | local_minima=islocalmin(filtered_signal,'MinSeparation',ceil(max_region_length/2),'SamplePoints',1:length(filtered_signal)); 45 | region_stops=find(local_minima); 46 | region_stops(end+1) = length(filtered_signal); 47 | 48 | 49 | % push out last region stop if it's close enough to the end of the 50 | % sequence 51 | if region_stops(end) - region_stops(end-1) < max_region_length 52 | region_stops(end - 1) = region_stops(end); 53 | region_stops(end) = []; 54 | end 55 | 56 | msg=sprintf('Found %d regions in %d input frames.\n',length(region_stops), size(descriptors,1)); 57 | disp(msg); 58 | 59 | % integrate boundary proximity score 60 | start_r = 1; 61 | region_starts = [1]; 62 | for region_id = 1:length(region_stops) 63 | end_r = region_stops(region_id); 64 | feas(start_r) = 0; 65 | feas(start_r+1) = feas(start_r+1) * 0.5; 66 | feas(end_r) = 0; 67 | feas(end_r-1) = feas(end_r-1) * 0.5; 68 | start_r = end_r; 69 | end 70 | 71 | % for each region, set keyframe at most similar frame 72 | start_r = 1; 73 | keyframe_indices = int32.empty; 74 | region_starts = [1]; 75 | for region_id = 1:length(region_stops) 76 | end_r = region_stops(region_id); 77 | region = feas(start_r:end_r,:); 78 | [val,idx] = max(region); 79 | keyframe_indices(end+1) = start_r + idx - 1; 80 | region_starts(end+1) = end_r+1; 81 | start_r = end_r; 82 | 83 | plot(keyframe_indices(end),val,'r.','MarkerSize',40); 84 | line_h = filtered_signal(start_r); 85 | line([start_r start_r], [0 line_h],'Color','magenta','LineStyle','--','LineWidth',1); 86 | end 87 | 88 | plot(feas,'r','LineWidth',1); 89 | 90 | % save output to intermediary file 91 | idcs = strfind(filepath,'/'); 92 | outdir = filepath(1:idcs(end-1)-1); 93 | fileID=fopen(outdir +"/input/regions.txt",'w'); 94 | for i = 1:length(keyframe_indices) 95 | fprintf(fileID,'%d,%d,%d\n',region_starts(i)-1,region_stops(i)-1,keyframe_indices(i)-1); 96 | end 97 | fclose(fileID); 98 | 99 | leg=legend('Similarity Score','Keyframe Indices','Region Boundaries'); 100 | leg.FontSize = 20; 101 | ax=gca; 102 | ax.YAxis.FontSize = 20; 103 | ax.XAxis.FontSize = 20; 104 | set(gcf, 'Units', 'Normalized', 'OuterPosition', [0, 0.04, 0.5, 0.75]); 105 | title('Keyframe Indices and Region Boundaries'); 106 | ax.TitleFontSizeMultiplier = 2; 107 | saveas(gcf,outdir+"/input/sim_score_filtered.png"); 108 | % 109 | % hold off 110 | % plot(filtered_signal,'b','LineWidth',2); 111 | % set(get(gca, 'XLabel'), 'String', 'Frame number'); 112 | % set(get(gca, 'YLabel'), 'String', 'Score'); 113 | % hold on 114 | % feas = load('feas.txt'); 115 | % feas_n = normalize(feas,'range') 116 | % plot(feas_n,'r','LineWidth',2); 117 | % leg=legend('Similarity Score','Feasibility Score'); 118 | % leg.FontSize = 20; 119 | % ax=gca; 120 | % ax.YAxis.FontSize = 20; 121 | % ax.XAxis.FontSize = 20; 122 | % set(gcf, 'Units', 'Normalized', 'OuterPosition', [0, 0.04, 0.5, 0.75]); 123 | % title('Similarity Vs Feasibility') 124 | % ax.TitleFontSizeMultiplier = 2; 125 | % saveas(gcf,"sim_feas.png"); 126 | 127 | 128 | 129 | 130 | end 131 | -------------------------------------------------------------------------------- /meshtracker/include/mesh_segmentation.h: -------------------------------------------------------------------------------- 1 | #ifndef MESH_SEGMENTATION_H 2 | #define MESH_SEGMENTATION_H 3 | 4 | // vertex_id , segment_id 5 | typedef std::map VidSidMap; 6 | typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel; 7 | typedef CGAL::Surface_mesh SM; 8 | 9 | class MeshSegmentation 10 | { 11 | 12 | private: 13 | // default paramters seem good for 25K meshes of typical human body poses 14 | // generally aim to create as many segments as possible and then fuse the small ones 15 | 16 | int m_num_clusters = 20; // number of segment levels about large sdf values, recc = 20 17 | double m_lambda = 0.1; // smoothness param (0.0 - 1.0), recc = 0.1 18 | size_t m_num_segments = 0; //init member variable 19 | int m_seg_size_threshold = 100; // minimum allowed number of vertices for a segment 20 | bool m_seg_size_passed = false; 21 | 22 | public: 23 | MeshSegmentation(){}; 24 | ~MeshSegmentation(){}; 25 | 26 | // Takes a CGAL::Surface_mesh, performs segmentation based 27 | // on Shape diameter function. Outputs a vector of vert_id->seg_id pairs 28 | void Segment( 29 | const pcl::PolygonMesh & mesh, 30 | VidSidMap & output); 31 | 32 | // Looks for segments smaller than m_seg_size_threshold and fuse with nearest 33 | // connected neighbour 34 | VidSidMap FuseSmallSegments( 35 | const VidSidMap & vs_map, 36 | const pcl::PolygonMesh & mesh, 37 | const int & number_of_segments); 38 | 39 | // Finds connected segments and merges indices with the smallest neighbor. 40 | // Returns the index of the fused segment 41 | int FuseWithSmallestConnectedSegment( 42 | VidSidMap & vs_map_, 43 | const pcl::PolygonMesh & mesh, 44 | const size_t & ctr_sid); 45 | 46 | // Circulates each vertex in mesh to find a connected vertex with different 47 | // seg_id. Result is first unique segment. Returns -1 if none found 48 | int FindNeighbourSegment( 49 | const VidSidMap & vs_map, 50 | const int & seg_id, 51 | const pcl::PolygonMesh & _mesh) const; 52 | 53 | inline void SetNumClusters(const int & nc) 54 | { 55 | m_num_clusters = nc; 56 | } 57 | 58 | inline void SetLambda(const double & l) 59 | { 60 | m_lambda = l; 61 | } 62 | 63 | inline size_t GetNumSegments() const 64 | { 65 | return m_num_segments; 66 | } 67 | 68 | inline void SetNumSegments(const size_t & ns) 69 | { 70 | m_num_segments = ns; 71 | } 72 | 73 | inline void SetSizeCheckPass(const bool & state) 74 | { 75 | m_seg_size_passed = state; 76 | } 77 | 78 | // Returns a list of all indices for given segment id 79 | std::vector GetSegIndices( 80 | const VidSidMap & map, 81 | const int & seg_id); 82 | 83 | // Returns a vector of connected seg ids 84 | std::vector GetSegmentConnectivity( 85 | const int & seg_id, 86 | const VidSidMap & vs_map, 87 | const pcl::PolygonMesh & _mesh) const; 88 | 89 | // Reorders the seg_ids such that 0 is the most-connected descending 90 | void ReOrderSegMapByConnectivity( 91 | VidSidMap & vs_map, 92 | const pcl::PolygonMesh & mesh, 93 | const int & num_segs); 94 | 95 | // returns vertex indices of self and k-connected segments 96 | std::vector GetSelfAndConnectedSegIndices( 97 | const VidSidMap & vs_map, 98 | const pcl::PolygonMesh & mesh, 99 | const int & seg_id, 100 | const int & k = 2); 101 | 102 | // returns the seg_ids for input and connected segments 103 | std::vector GetSelfAndConnectedSegIds( 104 | const VidSidMap & vs_map, 105 | const pcl::PolygonMesh & mesh, 106 | const int & seg_id); 107 | 108 | // Given a VidSidMap vertex seg_id map, returns a polygonmesh for the given 109 | // seg_id consisting of all the faces around the input vertices. No conisderation 110 | // is given for overlapping faces. 111 | // NOTE: Ideally we would just add the required faces by index but for some 112 | // reason we get a seg fault from PCL when attempting to manually triangulate 113 | // input vertices. Hence, the implementation below takes the less efficient route 114 | // of adding every face and then slectively removing faces not associated with 115 | // the input vertex set. 116 | pcl::PolygonMesh MeshSegFromVertexIds( 117 | const pcl::PolygonMesh & global_mesh, 118 | const VidSidMap & vs_map, 119 | const int & seg_id); 120 | 121 | // Prints the distribution of points among discovered segments, can be returned 122 | std::map GetSegSizes( 123 | const VidSidMap & map, 124 | const bool & print); 125 | 126 | // Save segments to file for debug 127 | void ExportSegments( 128 | const VidSidMap & vs_map, 129 | const pcl::PolygonMesh & mesh, 130 | const std::string & out_path) const; 131 | }; 132 | 133 | #endif 134 | -------------------------------------------------------------------------------- /meshtracker/include/deform_graph.h: -------------------------------------------------------------------------------- 1 | #ifndef DEFORM_GRAPH_H 2 | #define DEFORM_GRAPH_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "tri_mesh.h" 21 | #include "mesh_processing.h" 22 | #include "matching.h" 23 | #include "gauss_newton_solver.h" 24 | #include "utils.h" 25 | 26 | typedef pcl::PointCloud::Ptr CloudPtr; 27 | 28 | // Node idx, corrspondence pairs 29 | typedef std::vector> NodeCorrs; 31 | 32 | class DeformGraph 33 | { 34 | 35 | private: 36 | pcl::PolygonMesh graph_structure_; 37 | double node_sample_radius_ = 1.0; // init value only 38 | double dmax_scaler_ = 1.5; // Scales the inclusion radius during vert-node weight assignment 39 | size_t debug_vertex_ = 2000; 40 | 41 | public: 42 | 43 | std::vector node_norm; 44 | std::vector node_rots; 45 | std::vector node_rots_mat; 46 | std::vector node_trans; 47 | std::vector node_pos; 48 | std::vector> node_neigh; 49 | std::vector> node_node_weights; 50 | std::vector> vert_node_weights; 51 | 52 | double max_dist_neigh; // used for building neighbours 53 | int k_neigh; 54 | 55 | DeformGraph() 56 | { 57 | k_neigh = kDG_KNN; 58 | }; 59 | 60 | ~DeformGraph(){}; 61 | 62 | void BuildGraph( 63 | const TriMesh & mesh, 64 | pcl::PolygonMesh & in_graph, 65 | int k_nn = kDG_KNN); 66 | 67 | inline void SetMGraphStructure(pcl::PolygonMesh & mesh) 68 | { 69 | this->graph_structure_ = mesh; 70 | } 71 | 72 | inline void SetSampleRadius(const double & radius) 73 | { 74 | this->node_sample_radius_ = radius; 75 | } 76 | 77 | void BuildNeighbours(); 78 | 79 | void CalculateNodeNodeWeights(); 80 | 81 | std::vector> CalculateVertNodeWeights( 82 | const std::vector & verts, 83 | const std::vector> & vert_neigh); 84 | 85 | std::vector> CalculateVertNodeWeights( 86 | const pcl::PolygonMesh & in_mesh, 87 | const std::vector> & vert_neigh); 88 | 89 | void UpdateGraph(const Eigen::VectorXd & X); 90 | 91 | // Per-vertex get the knn nodes of influence for input mesh 92 | // in_graph is used as it will provide better correspondence 93 | std::vector> GetVertNodeNeighbours( 94 | pcl::PolygonMesh & in_mesh); 95 | 96 | // Get vert-node matches using graph connectivity and sampling radius 97 | // constraints. Requires cloud so that we don't have to 98 | // calculate norms erry time 99 | std::vector GetVertNodeMatches( 100 | const pcl::PointXYZRGBNormal & source, 101 | const pcl::PolygonMesh & query, 102 | const CloudPtr & query_cloud); 103 | 104 | std::vector> GetConNodeNeighbours( 105 | const std::vector & verts, 106 | int search_size = 12); 107 | 108 | // Smooth out the deform space to prevent high frequency noise 109 | void NormalizeTforms(); 110 | 111 | void Deform( 112 | const std::vector> & node_influence_list, 113 | const std::vector> & vert_node_weights, 114 | const pcl::PolygonMesh & input_graph, 115 | TriMesh & d_mesh); 116 | 117 | // Initialize the vector of stacked affine transforms 118 | // 12 * num_nodes, [R|t] : 9 R, 3 t 119 | Eigen::VectorXd InitAffineVector(size_t nodeSize) const; 120 | 121 | //constraints_index is in respect to graph nodes' index 122 | //TODO: we could probably make a pair for index-weight access 123 | double OptimizeOnce( 124 | DeformGraph & graph, 125 | GNParams & params, 126 | std::vector> & con_node_neigh, 127 | std::vector> & con_node_weights, 128 | const NodeCorrs & cons); 129 | 130 | void DebugRandomVertNodeDeformInfluence( 131 | std::vector> node_influence_list, 132 | std::vector> vert_node_weights, 133 | CloudPtr moving_cloud, 134 | pcl::PolygonMesh graph_mesh, 135 | std::string out_path); 136 | 137 | }; 138 | 139 | #endif 140 | //////////////////////////////////////////////////////////////////////////////// 141 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MeshTracker 2 | 3 | A segmentation-based tracking algorithm for registering volumetric video meshes (ply/obj) in C++. 4 | This is the official implementation of the paper: 5 | >Moynihan, M., Ruano, S., Pagés, R. and Smolic, A., 2021. Autonomous Tracking For Volumetric Video Sequences. In Proceedings of the IEEE/CVF Winter Conference on Applications of Computer Vision (pp. 1660-1669). 6 | 7 | ## Video 8 | [![Watch the video](https://img.youtube.com/vi/JwO2obk0tJM/maxresdefault.jpg)](https://youtu.be/JwO2obk0tJM) 9 | 10 | ## Getting Started 11 | 12 | * Clone the repository into your workspace and see instructions below for install: 13 | 14 | ### Installing 15 | 16 | 1. Install Docker 17 | 18 | 1. Download this repo and run the following from the same location as Dockerfile. 19 | NUM_JOBS is just an integer which will tell Make how many threads to use (Default=3). 20 | 21 | ``` 22 | cd dockers && sh ./build_dockers.sh NUM_JOBS 23 | ``` 24 | 25 | ** Use as many jobs as your cpu/ram will allow as the docker building is quite slow! ** 26 | 27 | ### Usage 28 | 29 | To run the code on your own input simply run the following command 30 | ``` 31 | docker run --rm -v :/input -v :/result --name meshtracker_instance mesh_tracker:latest MeshTracker -r 32 | ``` 33 | N.B! The code currently does not support keyframe detection yet. For custom data you must provide the 34 | regions.txt files in the same format as we have provided in the sample data 35 | 36 | ### Configuration 37 | 38 | In lieu of a configuration file (coming soon), the following list of parameters can be tweaked to 39 | accommodate various topology characteristics: 40 | In general most default parameters will be in the header files. Some important ones... 41 | * constants.h: kDG_KNN, amount of graph nodes which influence a given point on the mesh. Higher is more stable but less able to model small scale deformations 42 | * cpd_icp.h: kSamplesUpperLimit, kSamplesLowerLimit: Bounds for segment size and high-res alignment trigger. 43 | * gauss_newton_solver.h: Most of the parameters for the minimization criteria of the GN solver e.g. rigidity/smoothness 44 | * mesh_segmentation.h 45 | * mesh_tracking.h 46 | * mesh_tracking.cpp:344, set this to true if your input meshes are truly awful. It'll remesh them and hopefully add some stability to processing 47 | mesh_tracking.cpp:461, by default the detail synthesis is disabled, see [this issue](https://github.com/V-Sense/AutoMeshTracker/issues/2#issue-875273954). Set true to enable and tweak parames in mesh_trackpering.cpp:GetDetailLayer() to suit the input. 48 | 49 | ### Testing 50 | 51 | We provide two sequences to test the system, Floss and Matt_Hello. 52 | In particular Matt_Hello demonstrates a tracked edit on frame 30 where the 53 | missing hand details in the original have been added back in. 54 | 55 | You can run the test code with the following command 56 | ``` 57 | docker run --rm --name meshtracker_instance mesh_tracker:latest MeshTracker -t 58 | ``` 59 | 60 | ## Limitations 61 | 62 | * Automatic keyframe selection needs to be ported from it's original MATLAB implementation and 63 | is not yet provided as part of the public release. Keyframes must be provided manually. 64 | * Will likely fail on any self-intersecting meshes 65 | * Can track fast motion but should be avoided. Ideally there should be a good 66 | amount of overlap between meshes 67 | * Per the usual apology, this is research code and as such it features some coffee-fuelled, late night 68 | hacks and quick fixes. We'll be supporting it over time but PR's and patience are welcome :) 69 | 70 | ## Preparing keyframes for tracking user-edits 71 | 72 | * In blender, use boolean modifier with union to merge the edited objects into one mesh 73 | * Then apply the remesh modifier adjusting the octree depth and smoothness parameters to best suit your output 74 | * Export the result, then load it in meshlab 75 | * Perform Quadratic edge collapse. Set desired faces to be 2x the final output 76 | * Clean up non-manifold edges and vertices 77 | * Perfom isometric remeshing, 1 iteration, adaptive, world unit 0.5% 78 | * Load up the original unedited frame and use Meshlab's align tool to realing it 79 | * Save with same name as original keyframe 80 | 81 | ## Authors 82 | 83 | * **Matt Moynihan** - [GitHub](https://github.com/mjkmoynihan), [LinkedIn](https://www.linkedin.com/in/mjkmoynihan/) 84 | 85 | * **Susana Ruano** - [ResearchGate](https://www.researchgate.net/profile/Susana-Ruano-2) 86 | 87 | * **Rafael Pages** - [Github](https://github.com/rafapages),[web](https://www.rafapages.com/) 88 | 89 | * **Aljosa Smolic** - [Academic Page](https://www.tcd.ie/research/profiles/?profile=smolica) 90 | 91 | ## Third Party Code 92 | 93 | * **Peter Gadomski** - [Coherent Point Drift Implementation (CPD-ICP)](https://github.com/gadomski/cpd) 94 | 95 | * **Yizhi Tang** - [Optimal Step Non-Rigid Registration Implementation](https://github.com/Tonsty/Non-Rigid-Registar) 96 | 97 | * **Kaiwen Guo** - [Robust non-rigid motion tracking and surface reconstruction using l0 regularization](https://www.guokaiwen.com/svr.html) 98 | 99 | ## Bibtex 100 | 101 | If you find our code useful in your work please consider citing: 102 | ``` 103 | @InProceedings{Moynihan_2021_WACV, 104 | author = {Moynihan, Matthew and Ruano, Susana and Pages, Rafael and Smolic, Aljosa}, 105 | title = {Autonomous Tracking for Volumetric Video Sequences}, 106 | booktitle = {Proceedings of the IEEE/CVF Winter Conference on Applications of Computer Vision (WACV)}, 107 | month = {January}, 108 | year = {2021}, 109 | pages = {1660-1669} 110 | } 111 | ``` 112 | 113 | ## Paper Errata 114 | 115 | Figure 9 (a)-(d) is missing the following reference to the source of the mesh used. 116 | ``` 117 | @inproceedings{casas20144d, 118 | title={4d video textures for interactive character appearance}, 119 | author={Casas, Dan and Volino, Marco and Collomosse, John and Hilton, Adrian}, 120 | booktitle={Computer Graphics Forum}, 121 | volume={33}, 122 | number={2}, 123 | pages={371--380}, 124 | year={2014}, 125 | organization={Wiley Online Library} 126 | } 127 | ``` 128 | -------------------------------------------------------------------------------- /meshtracker/main.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "utils.h" 10 | #include "constants.h" 11 | #include "keyframer.h" 12 | #include "mesh_tracking.h" 13 | #include "kalman_smoothing.h" 14 | #include "mesh_segmentation.h" 15 | #include "prog_opts.h" 16 | #include "log.h" 17 | #include "tests.h" 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | int main(int argc, char *argv[]) 25 | { 26 | std::cout << "Cxx: " << __cplusplus << std::endl; 27 | std::cout<<"Eigen version: " 28 | <(&verbosity_level)->default_value(2),"set verbosity level: (5) fatal, (4) error, (3) warning, (2) info, (1) debug, (0) trace"); 51 | 52 | prog_opts::variables_map var_map; 53 | prog_opts::store(prog_opts::parse_command_line(argc, argv, desc), var_map); 54 | 55 | if (var_map.count("help") || argc == 1) 56 | { 57 | std::cerr << desc << '\n'; 58 | return 0; 59 | } 60 | 61 | if (var_map.count("testing")) 62 | { 63 | 64 | try 65 | { 66 | TESTING::SequenceTestingMattHello(); 67 | // TESTING::SequenceTestingFloss(); 68 | } 69 | catch(const std::exception& e) 70 | { 71 | std::cerr << e.what() << '\n'; 72 | return 0; 73 | } 74 | 75 | return 0; 76 | } 77 | prog_opts::notify(var_map); 78 | } 79 | 80 | catch (std::exception &e) 81 | { 82 | const std::string err = e.what(); 83 | BOOST_LOG_TRIVIAL(error) << err; 84 | std::cerr << "Run: " << argv[0] << " -h for usage" << std::endl; 85 | return 1; 86 | } 87 | 88 | catch (...) 89 | { 90 | std::string e = "Unknown Exception encountered!"; 91 | BOOST_LOG_TRIVIAL(error) << e; 92 | std::cerr << e << std::endl; 93 | } 94 | 95 | // Mesh tracking from user input 96 | try 97 | { 98 | 99 | // start timer 100 | time_t tic, toc; 101 | tic = time(0); 102 | 103 | MeshTracking mt; 104 | KeyFramer kf; 105 | 106 | // create result directory 107 | std::string res_path = "/result/"; 108 | if (mkdir(res_path.c_str(), 0777) == -1) 109 | { 110 | std::cerr 111 | << "Couldn't create result directory at:\n" 112 | << res_path 113 | << "\n path may already exist..." 114 | << std::endl; 115 | } 116 | 117 | // Setup logs 118 | std::string f_log = res_path + "output.log"; 119 | init_logging(0,f_log); //log everything 120 | BOOST_LOG_TRIVIAL(debug) << "Tracking from user input...\n"; 121 | 122 | // Create meshlist 123 | // std::string cmd = "ls /input/*{.obj,.ply} > /input/meshlist.txt"; 124 | std::string cmd = "ls /input/*.ply /input/*.obj > /input/meshlist.txt"; 125 | system(cmd.c_str()); 126 | // Read meshlist and store 127 | mt.ReadMeshList("/input/meshlist.txt"); 128 | 129 | // Generate Abs layers from meshlist and save in result folder 130 | // mt.GenAbslayersFromList("/result/"); 131 | 132 | // Generate Spherical Harmonics descriptor for each abs layers using the 133 | // third party exe 134 | // std::string msh2shd = "/MeshTracker/resource/thirdparty/msh2shd"; 135 | // kf.GenerateSPHDescriptors("/result/abs/meshlist.txt",msh2shd); 136 | 137 | // Generate feasibility scores from abs layers 138 | // kf.GenerateFeasScore(kf.ReadMeshList("/result/abs/meshlist.txt"),path); 139 | // kf.GenerateFeasScore(kf.ReadMeshList("/result/abs/meshlist.txt"),""); 140 | 141 | // Execute matlab script to find and plot eligible keyframes and tracking 142 | // regions 143 | // std::string descriptors_p = "/result/abs/descriptors.csv"; 144 | // std::string feas_p = "/result/abs/feas.txt"; 145 | // cmd = "sh /MeshTracker/resource/scripts/gen_regions.sh "+descriptors_p+" "+feas_p; 146 | // system(cmd.c_str()); 147 | 148 | // set variables and pass object to mt 149 | icp::IcpOptions icp_ops; 150 | icp_ops.use_adaptive = true; 151 | icp_ops.parallel = true; 152 | std::string regions_file = "/input/regions.txt"; 153 | std::string tracking_out = "/result/"; 154 | 155 | // perform tracking 156 | mt.ReadMeshList("/input/meshlist.txt"); 157 | mt.ReadRegions(regions_file); 158 | mt.TrackMeshSequence(icp_ops,tracking_out); 159 | 160 | // Create meshlist for smoothing 161 | cmd = "ls /result/*.obj > /result/smooth/meshlist.txt"; 162 | system(cmd.c_str()); 163 | KalmanSmoother ks; 164 | ks.readMeshes("/result/smooth/meshlist.txt"); 165 | ks.ReadRegions(regions_file); 166 | ks.smoothInRegions(); 167 | ks.setupSmoothMeshes(true); 168 | 169 | toc = time(0); 170 | cout << "Elapsed: "<< difftime(toc, tic) <<" second(s)."<< endl; 171 | 172 | // Calculate and export hdorf metrics 173 | std::vector errors = TESTING::GetSequenceError("/input/meshlist.txt",tracking_out); 174 | std::string err_path = tracking_out + "error.txt"; 175 | TESTING::SaveSequenceError(errors,err_path); 176 | } 177 | catch (std::exception& e) 178 | { 179 | std::string dump = 180 | std::string(e.what()) + "\n\tCheck output.log in project folder for details.\n"; 181 | std::cerr< 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | #include "log.h" 22 | 23 | typedef pcl::PointCloud::Ptr CloudPtr; 24 | 25 | 26 | namespace cloud_processing 27 | { 28 | struct BBox 29 | { 30 | 31 | double min_x = DBL_MAX; 32 | double min_y = DBL_MAX; 33 | double min_z = DBL_MAX; 34 | double max_x = -DBL_MAX; 35 | double max_y = -DBL_MAX; 36 | double max_z = -DBL_MAX; 37 | double cx = 0; 38 | double cy = 0; 39 | double cz = 0; 40 | double l_diag_sq = 0; 41 | 42 | BBox(const pcl::PolygonMesh& mesh){ this->get(mesh);}; 43 | BBox(const CloudPtr& cloud){ this->get(cloud);}; 44 | 45 | void get(const pcl::PolygonMesh& mesh) 46 | { 47 | CloudPtr cloud(new pcl::PointCloud); 48 | pcl::fromPCLPointCloud2(mesh.cloud,*cloud); 49 | 50 | for (size_t pt = 0; pt < cloud->points.size(); pt++) 51 | { 52 | pcl::PointXYZRGBNormal p = cloud->points[pt]; 53 | 54 | if (p.x < this->min_x) this->min_x = p.x; 55 | if (p.x > this->max_x) this->max_x = p.x; 56 | if (p.y < this->min_y) this->min_y = p.y; 57 | if (p.y > this->max_y) this->max_y = p.y; 58 | if (p.z < this->min_z) this->min_z = p.z; 59 | if (p.z > this->max_z) this->max_z = p.z; 60 | } 61 | 62 | this->cx = this->min_x + (this->max_x - this->min_x)*0.5; 63 | this->cy = this->min_y + (this->max_y - this->min_y)*0.5; 64 | this->cz = this->min_z + (this->max_z - this->min_z)*0.5; 65 | 66 | this->l_diag_sq = (max_x - min_x) + (max_y - min_y); 67 | }; 68 | 69 | void get(const CloudPtr& cloud) 70 | { 71 | for (size_t pt = 0; pt < cloud->points.size(); pt++) 72 | { 73 | pcl::PointXYZRGBNormal p = cloud->points[pt]; 74 | 75 | if (p.x < this->min_x) this->min_x = p.x; 76 | if (p.x > this->max_x) this->max_x = p.x; 77 | if (p.y < this->min_y) this->min_y = p.y; 78 | if (p.y > this->max_y) this->max_y = p.y; 79 | if (p.z < this->min_z) this->min_z = p.z; 80 | if (p.z > this->max_z) this->max_z = p.z; 81 | } 82 | 83 | this->cx = this->min_x + (this->max_x - this->min_x)*0.5; 84 | this->cy = this->min_y + (this->max_y - this->min_y)*0.5; 85 | this->cz = this->min_z + (this->max_z - this->min_z)*0.5; 86 | 87 | this->l_diag_sq = (max_x - min_x) + (max_y - min_y); 88 | }; 89 | 90 | void print() 91 | { 92 | std::cout<<"Bbox:"<min_x<<", "<max_x<<"] l:"<max_x - this->min_x<min_y<<", "<max_y<<"] l:"<max_y - this->min_y<min_z<<", "<max_z<<"] l:"<max_z - this->min_z<cx<<", "<cy<<", "<cz<<"]"< FindOutliers( 131 | const CloudPtr & cloud); 132 | 133 | // Calculates hdorf from a to b and returns avg dist per point 134 | double ComputeHausdorffDistance( 135 | const CloudPtr & cloud_a, 136 | const CloudPtr & cloud_b); 137 | 138 | // Calculates distance from point to cloud 139 | double ComputeHausdorffDistance( 140 | const pcl::PointXYZRGBNormal & pt, 141 | const CloudPtr & cloud); 142 | 143 | // Implements pcl VoxelGrid resampling 144 | CloudPtr ResamplePointCloud( 145 | CloudPtr & cloud); 146 | 147 | // Implements pcl VoxelGrid uniform sampling 148 | CloudPtr UniformsamplePointCloud( 149 | const CloudPtr & cloud, 150 | std::optional target_size); 151 | 152 | // Implements Random subsampling 153 | CloudPtr RandomsamplePointCloud( 154 | CloudPtr & cloud, 155 | const unsigned int & samples); 156 | 157 | // Translate moving cloud such that center of bbox aligns with 158 | // centre of target bbox 159 | CloudPtr AlignCentres( 160 | const CloudPtr & moving, 161 | const CloudPtr & fixed); 162 | 163 | // transform cloud to origin 164 | CloudPtr TransformToOrigin( 165 | const CloudPtr & cloud); 166 | 167 | // Perform rigid ICP and return aligned cloud 168 | CloudPtr RigidICP( 169 | const CloudPtr & moving, 170 | const CloudPtr & fixed, 171 | const double & max_pair_dist = 1.0, 172 | const int & max_iters = 50, 173 | const double & tform_epsilon = 1e-8, 174 | const double & fitness_epsilon = 0.05); 175 | 176 | inline int GetSamplesUpperLimit() const 177 | { 178 | return kSamplesUpperLimit; 179 | } 180 | 181 | inline int GetSamplesLowerLimit() const 182 | { 183 | return kSamplesLowerLimit; 184 | } 185 | 186 | void ColorizeCloud( 187 | CloudPtr & cloud, 188 | std::array & rgb); 189 | }; 190 | 191 | 192 | #endif -------------------------------------------------------------------------------- /meshtracker/src/keyframer.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "keyframer.h" 3 | #include "log.h" 4 | 5 | //////////////////////////////////////////////////////////////////////////////// 6 | // Determine keyframe locations. 7 | 8 | std::vector KeyFramer::GetKeyframeIndices(const std::string& _filename){ 9 | 10 | // Read mesh list 11 | std::vector mesh_names = ReadMeshList(_filename); 12 | 13 | std::vector kf_indices; 14 | 15 | return kf_indices; 16 | } 17 | 18 | //////////////////////////////////////////////////////////////////////////////// 19 | 20 | std::vector KeyFramer::ReadMeshList(const std::string& _filename){ 21 | 22 | std::vector mesh_names_; 23 | 24 | std::ifstream inputfile(_filename.c_str()); 25 | 26 | if (inputfile.is_open()){ 27 | std::string line; 28 | do { 29 | std::getline(inputfile,line); 30 | if (line.empty()) continue; 31 | // std::cerr << line << std::endl; 32 | mesh_names_.push_back(line); 33 | 34 | } while (!inputfile.eof()); 35 | 36 | } else { 37 | std::string e = "\t In KeyFramer::ReadMeshList: couldn't open input file"; 38 | throw std::runtime_error(e); 39 | } 40 | 41 | BOOST_LOG_TRIVIAL(info) << "Number of meshes in list: " << mesh_names_.size(); 42 | return mesh_names_; 43 | } 44 | 45 | //////////////////////////////////////////////////////////////////////////////// 46 | 47 | double KeyFramer::ComputeMeshArea(const pcl::PolygonMesh& mesh) { 48 | 49 | double totalarea = 0.0; 50 | 51 | pcl::PointCloud mesh_cloud; 52 | pcl::fromPCLPointCloud2 (mesh.cloud, mesh_cloud); 53 | 54 | // #pragma omp parallel for 55 | for (int i = 0; i < mesh.polygons.size(); i++){ 56 | const pcl::PointXYZRGBNormal pclv0 = mesh_cloud.points[mesh.polygons[i].vertices[0]]; 57 | const pcl::PointXYZRGBNormal pclv1 = mesh_cloud.points[mesh.polygons[i].vertices[1]]; 58 | const pcl::PointXYZRGBNormal pclv2 = mesh_cloud.points[mesh.polygons[i].vertices[2]]; 59 | const Eigen::Vector3f v0 = pclv0.getVector3fMap(); 60 | const Eigen::Vector3f v1 = pclv1.getVector3fMap(); 61 | const Eigen::Vector3f v2 = pclv2.getVector3fMap(); 62 | Eigen::Vector3f A, B; 63 | A = v1 - v0; 64 | B = v2 - v0; 65 | Eigen::Vector3f eignormal = A.cross(B); 66 | totalarea += 0.5 * eignormal.norm(); 67 | } 68 | 69 | return totalarea; 70 | } 71 | 72 | //////////////////////////////////////////////////////////////////////////////// 73 | 74 | unsigned int KeyFramer::ComputeMeshGenus(const Mesh& mesh) { 75 | 76 | const int v = mesh.sizeVertices(); 77 | const int f = mesh.sizeFaces(); 78 | const int e = mesh.sizeEdges(); 79 | 80 | // return 1 - 0.5*(v-e+f); 81 | return 0.5*(e-v-f+2); 82 | // return v + f - e + 2; 83 | } 84 | 85 | //////////////////////////////////////////////////////////////////////////////// 86 | 87 | unsigned int KeyFramer::ComputeMeshGenus(const pcl::PolygonMesh& mesh) { 88 | 89 | Mesh he_mesh; 90 | pcl::geometry::toHalfEdgeMesh(mesh, he_mesh); 91 | return ComputeMeshGenus(he_mesh); 92 | 93 | } 94 | 95 | //////////////////////////////////////////////////////////////////////////////// 96 | 97 | void KeyFramer::GenerateSPHDescriptors 98 | ( 99 | const std::string & meshlist, 100 | const std::string & exe 101 | ) 102 | { 103 | 104 | BOOST_LOG_TRIVIAL(info) << "Generating SPH descriptors"; 105 | std::vector meshnames = ReadMeshList(meshlist); 106 | if (meshnames.empty()) 107 | { 108 | std::string e 109 | = "In KeyFramer::GenerateSPHDescriptors: couldn't load meshlist"; 110 | BOOST_LOG_TRIVIAL(error) << e; 111 | throw std::runtime_error(e); 112 | } 113 | 114 | // get save location 115 | size_t path_end = meshnames[0].find_last_of("/\\"); 116 | std::string outpath = meshnames[0].substr(0, path_end); 117 | 118 | // remove any existing file 119 | std::string cmd = "rm "+outpath+"descriptors.csv"; 120 | system(cmd.c_str()); 121 | 122 | for (size_t mesh_idx = 0; mesh_idx < meshnames.size(); mesh_idx++) 123 | { 124 | // generate descriptor 125 | cmd = exe+" "+meshnames[mesh_idx]+" tmp_out.txt"; 126 | system(cmd.c_str()); 127 | 128 | // cleanup and append to csv file 129 | cmd = "echo $( sed -n '2p' tmp_out.txt) >> "+ outpath + "descriptors.csv"; 130 | system(cmd.c_str()); 131 | } 132 | 133 | // remove temp file 134 | cmd = "rm tmp_out.txt"; 135 | system(cmd.c_str()); 136 | } 137 | 138 | //////////////////////////////////////////////////////////////////////////////// 139 | 140 | void KeyFramer::GenerateFeasScore( 141 | std::vector meshlist, 142 | std::string outpath) 143 | { 144 | 145 | std::vector genii; 146 | std::vector areas; 147 | int max_genus = 0; 148 | double max_area = 0.0; 149 | 150 | std::ofstream ofs_debug; 151 | std::string debug_path = outpath + "/result/abs/debug_feas.txt"; 152 | ofs_debug.open(debug_path); 153 | 154 | MeshProcessing mp; 155 | 156 | // get params 157 | for (size_t mesh_id = 0; mesh_id < meshlist.size(); mesh_id++) 158 | { 159 | pcl::PolygonMesh mesh; 160 | pcl::io::loadPolygonFilePLY(meshlist[mesh_id],mesh); 161 | 162 | // pre-clean 163 | mesh = mp.RemoveSmallComponents(mesh); 164 | mesh = mp.FixManifoldness(mesh); 165 | 166 | int genus = ComputeMeshGenus(mesh); 167 | double area = ComputeMeshArea(mesh); 168 | genii.push_back(genus); 169 | areas.push_back(area); 170 | ofs_debug<<"i: "< max_genus ? genus : max_genus; 175 | max_area = area > max_area ? area : max_area; 176 | 177 | } 178 | ofs_debug.close(); 179 | 180 | // get score 181 | std::vector scores; 182 | for (size_t i = 0; i < meshlist.size(); i++) 183 | { 184 | double score = 1 + (max_genus - genii[i]) + (areas[i]/(max_area + 1) ); 185 | 186 | scores.push_back(score); 187 | 188 | } 189 | 190 | // normalize score and save 191 | std::string feas_path = outpath + "/result/abs/feas.txt"; 192 | std::ofstream ofs; 193 | ofs.open(feas_path); 194 | double max_feas = *std::max_element(scores.begin(), scores.end()); 195 | double min_feas = *std::min_element(scores.begin(), scores.end()); 196 | for (size_t i = 0; i < meshlist.size(); i++) 197 | { 198 | double score = (scores[i] - min_feas)/(max_feas - min_feas); 199 | ofs< & meshlist 209 | ) 210 | { 211 | size_t num_meshes = meshlist.size(); 212 | 213 | size_t path_end = meshlist[0].find_last_of("/\\"); 214 | const std::string meshpath = meshlist[0].substr(0,path_end); 215 | 216 | // for each frame in meshlist 217 | for (size_t mesh_idx = 0; mesh_idx < num_meshes; mesh_idx++) 218 | { 219 | // calculate shd 220 | 221 | 222 | } 223 | 224 | // execute matlab script 225 | 226 | } -------------------------------------------------------------------------------- /meshtracker/src/nonrigid_icp.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Modified by Matt Moynihan, https://github.com/mjkmoynihan 3 | Original Author: Yizhi Tang 4 | From: https://github.com/Tonsty/Non-Rigid-Registar 5 | Accessed September 7th 2018 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | #include 21 | 22 | #include "nonrigid_icp.h" 23 | #include "log.h" 24 | 25 | void NonRigidICP::init() 26 | { 27 | edgesInit(); 28 | verticesInit(); 29 | nearestSearchInit(); 30 | } 31 | 32 | void NonRigidICP::initCompute() 33 | { 34 | correspondencesInit(); 35 | weightsInit(); 36 | } 37 | 38 | void NonRigidICP::initCompute 39 | ( 40 | const std::vector>& mt_matches 41 | ) 42 | { 43 | setCorrespondences(mt_matches); 44 | weightsInit(); 45 | } 46 | 47 | void NonRigidICP::edgesInit() 48 | { 49 | if (edges_ == NULL) 50 | { 51 | edges_.reset(new std::vector); 52 | } 53 | 54 | for (int i = 0; i < template_->GetNumberOfCells(); ++i) 55 | { 56 | vtkCell* cell = template_->GetCell(i); 57 | int a = cell->GetPointId(0); 58 | int b = cell->GetPointId(1); 59 | int c = cell->GetPointId(2); 60 | 61 | edges_->push_back(Edge(a,b)); 62 | edges_->push_back(Edge(b,c)); 63 | edges_->push_back(Edge(c,a)); 64 | } 65 | } 66 | 67 | void NonRigidICP::nearestSearchInit() 68 | { 69 | cellLocator_ = vtkSmartPointer::New(); 70 | cellLocator_->SetDataSet(target_); 71 | cellLocator_->BuildLocator(); 72 | } 73 | 74 | void NonRigidICP::verticesInit() 75 | { 76 | vertices_ = template_->GetPoints(); 77 | } 78 | 79 | void NonRigidICP::correspondencesInit() 80 | { 81 | 82 | 83 | // TODO: Check this doesn't screw up the intermediate step processes 84 | if (correspondences_ != NULL) 85 | { 86 | return; 87 | } 88 | 89 | correspondences_ = Vertices::New(); 90 | 91 | correspondences_->SetNumberOfPoints(vertices_->GetNumberOfPoints()); 92 | for (int i = 0; i < vertices_->GetNumberOfPoints(); i++) 93 | { 94 | double testPoint[3]; 95 | vertices_->GetPoint(i, testPoint); 96 | double closestPoint[3]; 97 | double closestPointDist2; 98 | vtkIdType cellId; 99 | int subId; 100 | cellLocator_->FindClosestPoint(testPoint, closestPoint, cellId, subId, closestPointDist2); 101 | correspondences_->SetPoint(i, closestPoint); 102 | } 103 | } 104 | 105 | void NonRigidICP::setCorrespondences 106 | ( 107 | const std::vector>& mt_matches 108 | ) 109 | { 110 | 111 | if (mt_matches.size() != vertices_->GetNumberOfPoints()) 112 | { 113 | std::string e = 114 | "In NonRigidICP::setCorrespondences. Matches size different to mesh size."; 115 | BOOST_LOG_TRIVIAL(error) << e; 116 | throw std::runtime_error(e); 117 | } 118 | 119 | correspondences_ = Vertices::New(); 120 | correspondences_->SetNumberOfPoints(vertices_->GetNumberOfPoints()); 121 | for (size_t pt = 0; pt < mt_matches.size(); pt++) 122 | { 123 | std::array matchPoint = mt_matches[pt]; 124 | //TODO: replace this 125 | double point[3]; 126 | point[0] = matchPoint[0]; 127 | point[1] = matchPoint[1]; 128 | point[2] = matchPoint[2]; 129 | correspondences_->SetPoint(pt,point); 130 | } 131 | } 132 | 133 | // initialize all weights to 1 134 | // Should modify this: wrt the paper, weights may refer to the correspondence reliability 135 | // in which case a value of 1 assumes perfect 1-to-1 matches. 136 | // TODO: add a flag function to SetCorrespondences which can be read to set the 137 | // relevant weight indices. i.e. weight could be some function of the arg between 138 | // the matched point dist and normal 139 | void NonRigidICP::weightsInit() 140 | { 141 | if (weights_ == NULL) 142 | { 143 | weights_.reset(new std::vector()); 144 | } 145 | 146 | weights_->resize(vertices_->GetNumberOfPoints()); 147 | 148 | for (int i = 0; i < weights_->size(); ++i) 149 | { 150 | (*weights_)[i] = 1.0f; 151 | } 152 | } 153 | 154 | 155 | // alpha: stiffness, gamma: weight diff between rot/skew agains trans 156 | int NonRigidICP::compute( 157 | float _alpha, 158 | float _gamma 159 | ) 160 | { 161 | int n = vertices_->GetNumberOfPoints(); 162 | int m = edges_->size(); 163 | 164 | Eigen::SparseMatrix A(4*m + n, 4*n); 165 | 166 | // Global Cost Function : E(X) = || [alpha M x G, WD] X - [0, WU] || ^2 167 | // = || AX - B || ^ 2 (linearized) 168 | 169 | 170 | // Set up alpha M x G 171 | std::vector< Eigen::Triplet > alpha_M_G; 172 | for (int i = 0; i < m; ++i){ 173 | Edge edge = (*edges_)[i]; 174 | int a = edge.first; 175 | int b = edge.second; 176 | 177 | for (int j = 0; j < 3; j++) 178 | { 179 | alpha_M_G.push_back(Eigen::Triplet(i*4 + j, a*4 + j, _alpha)); 180 | } 181 | 182 | // apply gamma 183 | alpha_M_G.push_back(Eigen::Triplet(i*4 + 3, a*4 + 3, _alpha * _gamma)); 184 | 185 | for (int j = 0; j < 3; j++) 186 | { 187 | alpha_M_G.push_back(Eigen::Triplet(i*4 + j, b*4 + j, -_alpha)); 188 | } 189 | 190 | // apply gamma 191 | alpha_M_G.push_back(Eigen::Triplet(i*4 + 3, b*4 + 3, -_alpha * _gamma)); 192 | } 193 | 194 | // set up W D 195 | std::vector< Eigen::Triplet > W_D; 196 | for (int i = 0; i < n; ++i) 197 | { 198 | double xyz[3]; 199 | vertices_->GetPoint(i, xyz); 200 | 201 | float weight = (*weights_)[i]; 202 | 203 | for (int j = 0; j < 3; ++j) 204 | { 205 | W_D.push_back(Eigen::Triplet(4*m + i, i*4 + j, weight * xyz[j])); 206 | } 207 | 208 | W_D.push_back(Eigen::Triplet(4*m + i, i*4 + 3, weight)); 209 | } 210 | 211 | 212 | // We ignore the landmark term, beta D_L as we're not using tailored correspondences 213 | 214 | std::vector< Eigen::Triplet > _A = alpha_M_G; 215 | _A.insert(_A.end(), W_D.begin(), W_D.end()); 216 | 217 | A.setFromTriplets(_A.begin(), _A.end()); 218 | 219 | // Set up last term of cost function, B = [0,WU] 220 | Eigen::MatrixX3f B = Eigen::MatrixX3f::Zero(4*m + n, 3); 221 | for (int i = 0; i < n; ++i) 222 | { 223 | double xyz[3]; 224 | correspondences_->GetPoint(i, xyz); 225 | 226 | float weight = (*weights_)[i]; 227 | 228 | for (int j = 0; j < 3; j++) 229 | { 230 | B(4*m + i, j) = weight * xyz[j]; 231 | } 232 | } 233 | 234 | // E(X) is minimised for X = (A^TA)^-1 A^TB 235 | 236 | Eigen::SparseMatrix ATA = Eigen::SparseMatrix(A.transpose()) * A; 237 | Eigen::MatrixX3f ATB = Eigen::SparseMatrix(A.transpose()) * B; 238 | 239 | // Eigen::ConjugateGradient< Eigen::SparseMatrix > solver; 240 | Eigen::ConjugateGradient< Eigen::SparseMatrix, Eigen::Lower|Eigen::Upper > solver; 241 | 242 | solver.compute(ATA); 243 | 244 | if (solver.info()!=Eigen::Success) 245 | { 246 | BOOST_LOG_TRIVIAL(error) << "Decomposition failed"; 247 | return 1; 248 | } 249 | 250 | Eigen::MatrixX3f X = solver.solve(ATB); 251 | 252 | Eigen::Matrix3Xf XT = X.transpose(); 253 | 254 | for (int i = 0; i < n; ++i) 255 | { 256 | double xyz[3]; 257 | vertices_->GetPoint(i, xyz); 258 | Eigen::Vector4f point(xyz[0], xyz[1], xyz[2], 1.0f); 259 | Eigen::Vector3f point_transformed = XT.block<3, 4>(0, 4*i) * point; 260 | vertices_->SetPoint(i, point_transformed[0], point_transformed[1], point_transformed[2]); 261 | 262 | double txyz[3]; 263 | correspondences_->GetPoint(i, txyz); 264 | } 265 | 266 | return 0; 267 | } 268 | -------------------------------------------------------------------------------- /meshtracker/src/cpd_icp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "cpd_nonrigid.h" 11 | 12 | #include "utils.h" 13 | #include "cloud_processing.h" 14 | #include "cpd_icp.h" 15 | 16 | //////////////////////////////////////////////////////////////////////////////// 17 | 18 | // Utility function for converting PCL point cloud ptrs to eigen matrices for 19 | // use with cpd-ICP library 20 | cpd::Matrix CPDICP::PCLToCpdMat(const CloudPtr &ptcloud){ 21 | 22 | cpd::Matrix mat(ptcloud->size(),3); 23 | 24 | for (size_t pt = 0; pt < ptcloud->size(); pt++) { 25 | mat(pt,0)=ptcloud->points[pt].x; 26 | mat(pt,1)=ptcloud->points[pt].y; 27 | mat(pt,2)=ptcloud->points[pt].z; 28 | } 29 | 30 | return mat; 31 | } 32 | 33 | //////////////////////////////////////////////////////////////////////////////// 34 | 35 | // Utility function for converting eigen matrices back to pcl point clouds 36 | CloudPtr CPDICP::CpdMatToPCL(const cpd::Matrix &mat){ 37 | 38 | CloudPtr ptcloud (new pcl::PointCloud); 39 | 40 | for (size_t pt = 0; pt < mat.rows(); pt++) { 41 | 42 | pcl::PointXYZRGBNormal pcl_pt; 43 | pcl_pt.x = (float) mat(pt,0); 44 | pcl_pt.y = (float) mat(pt,1); 45 | pcl_pt.z = (float) mat(pt,2); 46 | ptcloud->push_back(pcl_pt); 47 | } 48 | 49 | return ptcloud; 50 | } 51 | 52 | //////////////////////////////////////////////////////////////////////////////// 53 | 54 | // Nonrigid ICP registration of two eigen matrix point sets. Returns a results 55 | // object which contains the output point set among other paramters 56 | cpd::NonrigidResult CPDICP::NonRigidRegistrationImpl( 57 | const cpd::Matrix &fixed, 58 | const cpd::Matrix &moving, 59 | const double& tolerance){ 60 | 61 | // Matrices must be equal size 62 | if (moving.rows() != fixed.rows()) { 63 | std::string e = "Size error In CPDICP::NonRigidRegistrationImpl, input matrices rows must match size\n"; 64 | e += "\tfixed: "+std::to_string(fixed.rows())+","+std::to_string(fixed.cols())+"\n"; 65 | e += "\tmoving: "+std::to_string(moving.rows())+","+std::to_string(moving.cols())+"\n"; 66 | BOOST_LOG_TRIVIAL(error) << e; 67 | throw std::runtime_error(e); 68 | } 69 | 70 | cpd::Nonrigid nonrigid; 71 | nonrigid.tolerance(tolerance); //Essential! 72 | 73 | return nonrigid.run(fixed,moving); 74 | } 75 | 76 | //////////////////////////////////////////////////////////////////////////////// 77 | // Similar to AlignHigherResClouds except it uses affinity to estimate the 78 | // relationship between the lo-res tform and hi-res. 79 | 80 | pcl::PolygonMesh CPDICP::AlignHighResMeshes( 81 | const pcl::PolygonMesh& _mesh_fixed, 82 | const pcl::PolygonMesh& _mesh_moving, 83 | const double& tolerance){ 84 | 85 | // Create PCL Pointers 86 | CloudPtr cloud_moving (new pcl::PointCloud); 87 | CloudPtr cloud_fixed (new pcl::PointCloud); 88 | 89 | // extract cloud data from meshes 90 | pcl::fromPCLPointCloud2(_mesh_moving.cloud, *cloud_moving); 91 | pcl::fromPCLPointCloud2(_mesh_fixed.cloud, *cloud_fixed); 92 | 93 | // Copy our source before downsampling 94 | CloudPtr cloud_moving_hi (new pcl::PointCloud(*cloud_moving)); 95 | CloudPtr cloud_moving_lo (new pcl::PointCloud); 96 | 97 | // Don't bother sampling if it's a small enough segment 98 | if (cloud_moving->size()kSamplesUpperLimit) { 99 | 100 | cloud_moving_lo = cloud_moving; 101 | 102 | } else { 103 | 104 | CloudProcessing cp; 105 | 106 | // Approximate uniform sampling 107 | std::optional target_samples; // uses default if not init 108 | cloud_moving_lo = cp.UniformsamplePointCloud( 109 | cloud_moving,target_samples); 110 | 111 | // if uniform sample fails, default to random sample 112 | if (cloud_moving_lo->size() < this->kSamplesLowerLimit) { 113 | cloud_moving_lo = cp.RandomsamplePointCloud(cloud_moving,(int)this->kSamplesUpperLimit); 114 | } 115 | } 116 | 117 | // Create and populate downsampled fixed cloud 118 | CloudPtr cloud_fixed_lo (new pcl::PointCloud); 119 | 120 | if (cloud_fixed->size()kSamplesUpperLimit) { 121 | 122 | cloud_fixed_lo = cloud_fixed; 123 | 124 | } else { 125 | 126 | CloudProcessing cp; 127 | 128 | // Approximate uniform sampling 129 | std::optional target_samples; 130 | cloud_fixed_lo = cp.UniformsamplePointCloud(cloud_fixed,target_samples); 131 | 132 | // if uniform sample fails, default to random sample 133 | if (cloud_fixed_lo->size() < this->kSamplesLowerLimit) { 134 | cloud_fixed_lo = cp.RandomsamplePointCloud(cloud_fixed,(int)this->kSamplesUpperLimit); 135 | } 136 | } 137 | 138 | // Convert from ptcloud to Matrix 139 | cpd::Matrix mat_moving_hi = PCLToCpdMat(cloud_moving_hi); 140 | cpd::Matrix mat_moving_lo = PCLToCpdMat(cloud_moving_lo); 141 | cpd::Matrix mat_fixed_lo = PCLToCpdMat(cloud_fixed_lo); 142 | 143 | // Matrices must be equal size 144 | if (mat_moving_lo.rows() > mat_fixed_lo.rows()) { 145 | mat_moving_lo.resize(mat_moving_lo.rows(),mat_fixed_lo.cols()); 146 | } else { 147 | mat_fixed_lo.resize(mat_fixed_lo.rows(),mat_moving_lo.cols()); 148 | } 149 | 150 | // perform lo-res alignment 151 | cpd::NonrigidResult nr_result = NonRigidRegistrationImpl( 152 | mat_fixed_lo, 153 | mat_moving_lo, 154 | tolerance); 155 | 156 | Eigen::MatrixXd weights; 157 | weights = nr_result.tform; 158 | 159 | Eigen::MatrixXd m_g; 160 | 161 | getAffinity(mat_moving_hi,mat_moving_lo,m_g); 162 | 163 | // apply new transformation 164 | mat_moving_hi = mat_moving_hi + m_g*weights; 165 | 166 | // Convert new points to cloud 167 | CloudPtr cloud_out (new pcl::PointCloud); 168 | cloud_out = CpdMatToPCL(mat_moving_hi); 169 | 170 | // apply new points to input mesh 171 | pcl::PolygonMesh mesh_out; 172 | pcl::PCLPointCloud2 cloud_aligned; 173 | pcl::toPCLPointCloud2(*cloud_out, cloud_aligned); 174 | mesh_out = _mesh_moving; 175 | mesh_out.cloud = cloud_aligned; 176 | 177 | return mesh_out; 178 | } 179 | 180 | //////////////////////////////////////////////////////////////////////////////// 181 | 182 | CPD_Result CPDICP::AlignHighResClouds( 183 | const CloudPtr& in_cloud_fixed, 184 | const CloudPtr& in_cloud_moving, 185 | const double& tolerance){ 186 | 187 | // Create PCL Pointers, hard copy instances 188 | CloudPtr cloud_moving (new pcl::PointCloud(*in_cloud_moving)); 189 | CloudPtr cloud_fixed (new pcl::PointCloud(*in_cloud_fixed)); 190 | 191 | // Copy our source before downsampling 192 | CloudPtr cloud_moving_hi (new pcl::PointCloud(*cloud_moving)); 193 | CloudPtr cloud_moving_lo (new pcl::PointCloud); 194 | 195 | bool was_downsampled = false; 196 | // Don't bother sampling if it's a small enough segment 197 | if (cloud_moving->size() <= this->kSamplesUpperLimit) { 198 | cloud_moving_lo = cloud_moving; 199 | 200 | } else { 201 | 202 | CloudProcessing cp; 203 | // Approximate uniform sampling 204 | std::optional target_samples; 205 | // cloud_moving_lo = cp.UniformsamplePointCloud( 206 | // cloud_moving,target_samples); 207 | cloud_moving_lo = cp.RandomsamplePointCloud( 208 | cloud_moving,this->kSamplesUpperLimit-10); 209 | was_downsampled = true; 210 | } 211 | 212 | // Create and populate downsampled fixed cloud 213 | CloudPtr cloud_fixed_lo (new pcl::PointCloud); 214 | 215 | if (cloud_fixed->size() <= this->kSamplesUpperLimit) { 216 | cloud_fixed_lo = cloud_fixed; 217 | 218 | } else { 219 | CloudProcessing cp; 220 | // Approximate uniform sampling 221 | std::optional target_samples; 222 | // cloud_fixed_lo = cp.UniformsamplePointCloud( 223 | // cloud_fixed, target_samples); 224 | cloud_fixed_lo = cp.RandomsamplePointCloud( 225 | cloud_fixed, this->kSamplesUpperLimit-10); 226 | was_downsampled = true; 227 | } 228 | 229 | if (cloud_moving_lo->size() == 0) 230 | { 231 | std::string e = "In CPDICP::AlignHighResClouds: failed to resample input moving"; 232 | BOOST_LOG_TRIVIAL(error) << e; 233 | throw std::runtime_error(e); 234 | } 235 | 236 | if (cloud_fixed_lo->size() == 0) 237 | { 238 | std::string e = "In CPDICP::AlignHighResClouds: failed to resample input fixed"; 239 | BOOST_LOG_TRIVIAL(error) << e; 240 | throw std::runtime_error(e); 241 | } 242 | CPD_Result result; 243 | result.subsampled_moving = cloud_moving_lo; 244 | result.subsampled_fixed = cloud_fixed_lo; 245 | 246 | // Convert from ptcloud to Matrix 247 | cpd::Matrix mat_moving_hi = PCLToCpdMat(cloud_moving); 248 | cpd::Matrix mat_moving = PCLToCpdMat(cloud_moving_lo); 249 | cpd::Matrix mat_fixed = PCLToCpdMat(cloud_fixed_lo); 250 | 251 | cpd::Matrix test(mat_moving); 252 | 253 | // Matrices must be equal size, subsampling has a margin of error so we resize to smallest 254 | if (mat_moving.rows() != mat_fixed.rows()) { 255 | // std::string w = "\tIn CPDICP::AlignHighResClouds: correcting downsample mismatch"; 256 | // BOOST_LOG_TRIVIAL(debug) << w; 257 | if (mat_moving.rows() > mat_fixed.rows()) { 258 | Eigen::MatrixXd nu_moving = mat_moving.topRows(mat_fixed.rows()); 259 | mat_moving = nu_moving; 260 | } else { 261 | Eigen::MatrixXd nu_fixed = mat_fixed.topRows(mat_moving.rows()); 262 | mat_fixed = nu_fixed; 263 | } 264 | } 265 | 266 | // perform lo-res alignment 267 | cpd::NonrigidResult nr_result = NonRigidRegistrationImpl( 268 | mat_fixed, 269 | mat_moving, 270 | tolerance); 271 | 272 | // convert back to pcl format 273 | result.lo_res_aligned = CpdMatToPCL(nr_result.points); 274 | 275 | // Only apply this if the input was downsampled 276 | if (was_downsampled) 277 | { 278 | Eigen::MatrixXd weights; 279 | weights = nr_result.tform; 280 | 281 | Eigen::MatrixXd m_g; 282 | getAffinity(mat_moving_hi,test,m_g); 283 | 284 | // apply new transformation 285 | mat_moving_hi = mat_moving_hi + m_g*weights; 286 | 287 | // Convert new points to cloud 288 | CloudPtr cloud_out (new pcl::PointCloud); 289 | cloud_out = CpdMatToPCL(mat_moving_hi); 290 | result.aligned_cloud = cloud_out; 291 | } else 292 | { 293 | result.aligned_cloud = result.lo_res_aligned; 294 | result.subsampled_moving = cloud_moving; 295 | } 296 | return result; 297 | } 298 | 299 | //////////////////////////////////////////////////////////////////////////////// 300 | -------------------------------------------------------------------------------- /meshtracker/include/mesh_tracking.h: -------------------------------------------------------------------------------- 1 | #ifndef _MESH_TRACKING_ 2 | #define _MESH_TRACKING_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "cpd_nonrigid.h" 24 | 25 | #include "mesh_segmentation.h" 26 | #include "mesh_processing.h" 27 | #include "cloud_processing.h" 28 | #include "tri_mesh.h" 29 | #include "deform_graph.h" 30 | #include "cpd_icp.h" 31 | #include "nonrigid_icp.h" 32 | #include "prog_opts.h" 33 | #include "constants.h" 34 | #include "matching.h" 35 | #include "log.h" 36 | 37 | typedef pcl::PointCloud::Ptr CloudPtr; 38 | typedef std::pair> GroupIdxs; 39 | 40 | typedef pcl::geometry::PolygonMesh< 41 | pcl::geometry::DefaultMeshTraits> Mesh; 42 | 43 | typedef CGAL::Exact_predicates_inexact_constructions_kernel EPKernel; 44 | typedef CGAL::Surface_mesh SM; 45 | 46 | // Vertex_id , Segment_id mapping 47 | typedef std::map VidSidMap; 48 | 49 | // Segment_id, centroid mapping 50 | typedef std::map CtrSidMap; 51 | 52 | // corrspondence pairs 53 | typedef std::vector< 54 | std::pair> NodeCorrs; 55 | 56 | 57 | class MeshTracking{ 58 | 59 | public: 60 | 61 | MeshTracking(){}; 62 | ~MeshTracking(){}; 63 | 64 | // Avoid unless you know what you're doing! 65 | inline void SetCurrSegs(const int& num_segs) 66 | { 67 | this->curr_num_segs_ = num_segs; 68 | } 69 | 70 | inline void SetInitNumSegs( const int& num_segs) 71 | { 72 | this->init_num_segs_ = num_segs; 73 | this->curr_num_segs_ = num_segs; 74 | } 75 | 76 | inline void setMeshNames( 77 | const std::vector& _meshNames) 78 | { 79 | meshNames_ = _meshNames; 80 | } 81 | inline const std::vector& getMeshNames() const 82 | { 83 | return meshNames_; 84 | } 85 | 86 | inline void setClouds( 87 | const std::vector& _clouds) 88 | { 89 | clouds_ = _clouds; 90 | } 91 | 92 | inline const std::vector& getClouds() const 93 | { 94 | return clouds_; 95 | } 96 | 97 | inline void setKeyFrameIndices( 98 | const std::vector& _keyframeIndices) 99 | { 100 | keyframeIndices_ = _keyframeIndices; 101 | } 102 | 103 | inline const std::vector& getKeyFrameIndices() const 104 | { 105 | return keyframeIndices_; 106 | } 107 | 108 | inline void IncrementIter() 109 | { 110 | this->curr_iter_++; 111 | } 112 | 113 | inline void Reset() 114 | { 115 | this->curr_iter_ = 0; 116 | this->curr_energy_ = 0.0; 117 | this->prev_energy_ = 0.0; 118 | } 119 | 120 | inline int GetCurrentIter(){ return this->curr_iter_; } 121 | 122 | // Read mesh list 123 | int ReadMeshList( 124 | const std::string & _fileName); 125 | 126 | // Read keyframes and regions from file 127 | int ReadRegions( 128 | const std::string & file_name); 129 | 130 | // Return a mesh from input filename. Throws error if unable 131 | pcl::PolygonMesh LoadMeshFromFile( 132 | const std::string & filename); 133 | 134 | // Generates abs layers from input meshlist and saves them to a subfolder 135 | // in the out_path. Also generates a new meshlist in the out_path folder 136 | void GenAbslayersFromList( 137 | const std::string & out_path); 138 | 139 | // This approach tracks a keyframe back and forward through a region 140 | // Boolean flag switches on adaptive tracking usage 141 | void TrackMeshSequence( 142 | icp::IcpOptions icp_ops, 143 | std::string out_path); 144 | 145 | // Performs global tracking using the VTK tracker. 146 | // Call the optimal_nonrigid VTKMeshTracker for global tracking. 147 | pcl::PolygonMesh DoSurfaceAlignment( 148 | const pcl::PolygonMesh & moving, 149 | const pcl::PolygonMesh & fixed); 150 | 151 | // Function used to call segmentation-based tracking functions, perform 152 | // iterative fusion and seam blending. 153 | pcl::PolygonMesh DoAdaptiveTracking( 154 | const pcl::PolygonMesh & _moving_mesh, 155 | const pcl::PolygonMesh & _fixed_mesh); 156 | 157 | // Returns an aligned deformation graph corresponding to the constraints 158 | // which aligned moving to fixed 159 | void GetDenseCorrespondences( 160 | pcl::PolygonMesh & moving_mesh, 161 | pcl::PolygonMesh & fixed_mesh, 162 | pcl::PolygonMesh & aligned_graph_, 163 | pcl::PolygonMesh & in_graph_, 164 | const int & align_layer_size = 1750); //default 2500 165 | 166 | // Uses a list of dense vertex correspondences to match segmentation 167 | // information between meshes 168 | VidSidMap SegmentationFromMatches( 169 | const pcl::PolygonMesh & fixed, 170 | const VidSidMap input_vs_map, 171 | const std::vector & matches) const; 172 | 173 | // Calculates a map of segment centroids and corresponding ids 174 | CtrSidMap GetSegmentCentroids( 175 | const VidSidMap & vs_map, 176 | const pcl::PolygonMesh & mesh) const; 177 | 178 | // Returns the index of the first segment pair which shows 179 | // a disparity equal or greater than member threshold. 180 | // Returns -1 if all segments are coherent 181 | int SegmentProjectionCoherenceTest( 182 | VidSidMap & moving_vs_map, 183 | VidSidMap & fixed_vs_map); 184 | 185 | // Exports segments for a given vs_map. Used for vizualisation and debug. 186 | // Currently only exports points 187 | void ExportSegments( 188 | const VidSidMap & vs_map, 189 | const pcl::PolygonMesh & mesh, 190 | const std::string & out_path) const; 191 | 192 | // Given seg_maps for two meshes, deforms the 193 | CloudPtr AlignSegment( 194 | CloudPtr & moving, 195 | CloudPtr & fixed, 196 | const VidSidMap & moving_vs_map, 197 | const VidSidMap & fixed_vs_map, 198 | const int & seg_id); 199 | 200 | // Rigid alignment of two input meshes, sometimes useful for initialization 201 | pcl::PolygonMesh AlignRigid( 202 | pcl::PolygonMesh & moving, 203 | pcl::PolygonMesh & fixed); 204 | 205 | // Get correspondence constraints for solver 206 | NodeCorrs GetCorresConstraints( 207 | const DeformGraph & graph, 208 | const CloudPtr & aligned_graph_cloud, 209 | const CloudPtr & input_graph_cloud, 210 | const CloudPtr & fixed_cloud); 211 | 212 | // Given two polygon meshes, performs surface-based non-rigid alignment 213 | pcl::PolygonMesh TrackMesh( 214 | const pcl::PolygonMesh & _moving_mesh, 215 | const pcl::PolygonMesh & _fixed_mesh, 216 | const float & _alpha_max = 15.0, 217 | const float & _alpha_min = 5.0, 218 | const int & _iter_num = 5, 219 | const float & _gamma = 1.0) const; 220 | 221 | // Perform actual deformation 222 | pcl::PolygonMesh DeformMesh( 223 | const pcl::PolygonMesh & in_mesh, 224 | const pcl::PolygonMesh & d_graph, 225 | const std::vector> & node_influence_list); 226 | 227 | // Returns tform between two meshes 228 | std::vector> GetTform( 229 | const pcl::PolygonMesh & source, 230 | const pcl::PolygonMesh & target); 231 | 232 | // Apply input tform to input mesh and return result 233 | pcl::PolygonMesh ApplyTform( 234 | const pcl::PolygonMesh & in_mesh, 235 | const std::vector> & tform, 236 | const bool & invert = false); 237 | 238 | // save the per-vertex transformation between soure and target 239 | bool StoreTform( 240 | const pcl::PolygonMesh & source, 241 | const pcl::PolygonMesh & target, 242 | const std::string & path) const; 243 | 244 | // Perform dense wrapping of source to target to simulate details. 245 | // Source and target should be aligned as closely as possible. 246 | pcl::PolygonMesh GetDetailLayer( 247 | const pcl::PolygonMesh & source, 248 | const pcl::PolygonMesh & target, 249 | const bool & strict_matches = true); 250 | 251 | // save debug files, default is false 252 | void SaveDebugOutput( 253 | bool & state, 254 | std::string & path) 255 | { 256 | this->save_debug_ = state; 257 | this->debug_path_ = path; 258 | } 259 | 260 | private: 261 | 262 | std::vector meshNames_; 263 | std::vector::Ptr> clouds_; 264 | std::vector keyframeIndices_; 265 | std::vector > regions_; 266 | 267 | // Only counts segment which fail matching coherence i.e, 268 | // does not track segs which were fused due to small size 269 | std::vector fused_segments_; 270 | 271 | int curr_num_segs_ = 0; 272 | int init_num_segs_ = 0; 273 | int curr_iter_ = 0; 274 | int max_iters_ = 0; 275 | double curr_energy_ = 0.0; 276 | double prev_energy_ = 0.0; 277 | 278 | // TODO: rename to max_num_vert_node_neighbors_ 279 | // This is not a target, but more of an upper limit. 280 | // The real influence is determined by the the scaler and sampling radius 281 | // in deform graph members 282 | int num_vert_node_neighbors_ = 25; //10 283 | const double default_CPD_tolerance_ = 1e-8;//1e-8; 284 | double m_segment_size_match_threshold = 1.00; // % disparity in nummber of points //0.6 285 | double tracking_error_thresh_ = 1e-5; //1e-5; 286 | double GN_solver_relax_thresh_ = 0.65; // 0.05 287 | double GN_solver_rigidity_w_min = 100.0; //100.0 288 | 289 | bool save_debug_ = false; 290 | std::string debug_path_ = "/MeshTracker/resource/tests/oneshot_test/debug/"; 291 | }; 292 | 293 | #endif -------------------------------------------------------------------------------- /meshtracker/include/tests.h: -------------------------------------------------------------------------------- 1 | #ifndef _TESTING_H 2 | #define _TESTING_H 3 | 4 | #include 5 | 6 | #include "mesh_tracking.h" 7 | #include "kalman_smoothing.h" 8 | #include "mesh_processing.h" 9 | #include "cloud_processing.h" 10 | #include "log.h" 11 | 12 | #include "keyframer.h" 13 | 14 | namespace TESTING 15 | { 16 | //////////////////////////////////////////////////////////////////////////////// 17 | // Fundamental tracking test for pairwise alignment 18 | 19 | static void OneShotTracking 20 | ( 21 | const std::string & f_moving, 22 | const std::string & f_fixed, 23 | const std::string & f_result, 24 | std::string debug_path = "/MeshTracker/resource/tests/oneshot_test/debug/", 25 | bool save_debug = true 26 | ) 27 | { 28 | // start timer 29 | time_t tic, toc; 30 | tic = time(0); 31 | 32 | MeshTracking mt; 33 | 34 | MeshProcessing mp; 35 | CloudProcessing cp; 36 | pcl::PolygonMesh moving, fixed, result; 37 | 38 | // setup debug output 39 | mt.SaveDebugOutput(save_debug,debug_path); 40 | 41 | BOOST_LOG_TRIVIAL(debug) << "loading input..."; 42 | moving = mt.LoadMeshFromFile(f_moving); 43 | fixed = mt.LoadMeshFromFile(f_fixed); 44 | 45 | // Clean up input 46 | moving = mp.RemoveSmallComponents(moving,0.01); 47 | fixed = mp.RemoveSmallComponents(fixed,0.01); 48 | moving = mp.FixManifoldness(moving); 49 | fixed = mp. FixManifoldness(fixed); 50 | 51 | result = mt.DoAdaptiveTracking(moving,fixed); 52 | 53 | if(pcl::io::saveOBJFile(f_result, result)<0) 54 | { 55 | std::string e = "\tIn 1Shot Test: Couldn't save output"; 56 | BOOST_LOG_TRIVIAL(error) << e; 57 | throw std::runtime_error(e); 58 | } 59 | 60 | toc = time(0); 61 | cout << "Elapsed: "<< difftime(toc, tic) <<" second(s)."<< endl; 62 | 63 | // Get error 64 | CloudPtr res_cloud(new pcl::PointCloud); 65 | CloudPtr gt_cloud(new pcl::PointCloud); 66 | pcl::fromPCLPointCloud2(result.cloud,*res_cloud); 67 | pcl::fromPCLPointCloud2(fixed.cloud,*gt_cloud); 68 | const double err = cp.ComputeHausdorffDistance(res_cloud,gt_cloud); 69 | BOOST_LOG_TRIVIAL(info) << "Error: "< GetSequenceError( 105 | const std::string meshlist, 106 | const std::string res_path) 107 | { 108 | CloudProcessing cp; 109 | std::vector errors; 110 | 111 | std::string line; 112 | ifstream myfile (meshlist); 113 | if (myfile.is_open()) 114 | { 115 | while ( getline (myfile,line) ) 116 | { 117 | pcl::PolygonMesh res_mesh, gt_mesh; 118 | int path_end = line.find_last_of("/\\")+1; 119 | std::string fend = line.substr(path_end,line.size()-path_end-4); 120 | std::string res_fname = res_path + fend + "_t.obj"; 121 | 122 | // load result mesh 123 | pcl::io::loadPolygonFile(res_fname, res_mesh); 124 | 125 | // load target as ground truth 126 | pcl::io::loadPolygonFile(line, gt_mesh); 127 | 128 | CloudPtr res_cloud(new pcl::PointCloud); 129 | CloudPtr gt_cloud(new pcl::PointCloud); 130 | pcl::fromPCLPointCloud2(res_mesh.cloud,*res_cloud); 131 | pcl::fromPCLPointCloud2(gt_mesh.cloud,*gt_cloud); 132 | 133 | errors.push_back(cp.ComputeHausdorffDistance(gt_cloud,res_cloud)); 134 | } 135 | myfile.close(); 136 | 137 | 138 | } else 139 | { 140 | cout << "Unable to open file"; 141 | } 142 | return errors; 143 | } 144 | 145 | /////////////////////////////////////////////////////////////////////////////// 146 | 147 | static void SaveSequenceError( 148 | const std::vector errors, 149 | const std::string& fname 150 | ) 151 | { 152 | // max error 153 | double max_e = *std::max_element(errors.begin(), errors.end()); 154 | 155 | // median error 156 | double median_e = 0; 157 | 158 | std::vector sorted_errors(errors); 159 | std::sort(sorted_errors.begin(),sorted_errors.end()); 160 | 161 | // check for even case 162 | if (errors.size() % 2 != 0) 163 | { 164 | median_e = (double)sorted_errors[errors.size()/2]; 165 | } else 166 | { 167 | median_e = 168 | (double)(sorted_errors[(errors.size()-1)/2] + 169 | sorted_errors[errors.size()/2])/2.0; 170 | } 171 | 172 | ofstream myfile(fname); 173 | if (myfile.is_open()) 174 | { 175 | myfile << std::to_string(max_e) << " " ; 176 | myfile << std::to_string(median_e) << "\n"; 177 | for (size_t e = 0; e < errors.size(); e++) 178 | { 179 | myfile << std::to_string(errors[e]) << "\n"; 180 | } 181 | myfile.close(); 182 | } 183 | else cout << "Unable to open file: "< "+path+"input/meshlist.txt"; 230 | system(cmd.c_str()); 231 | 232 | // Read meshlist and store 233 | mt.ReadMeshList(path+"input/meshlist.txt"); 234 | 235 | // Generate Abs layers from meshlist and save in result folder 236 | mt.GenAbslayersFromList(path+"result/"); 237 | 238 | // Generate Spherical Harmonics descriptor for each abs layers using the 239 | // third party exe 240 | std::string msh2shd = "/MeshTracker/resource/thirdparty/msh2shd"; 241 | kf.GenerateSPHDescriptors(path+"result/abs/meshlist.txt",msh2shd); 242 | 243 | // Generate feasibility scores from abs layers 244 | // kf.GenerateFeasScore(kf.ReadMeshList(path+"result/abs/meshlist.txt"),path); 245 | kf.GenerateFeasScore(kf.ReadMeshList(path+"result/abs/meshlist.txt"),path); 246 | 247 | // Execute matlab script to find and plot eligible keyframes and tracking 248 | // regions 249 | std::string descriptors_p = path+"result/abs/descriptors.csv"; 250 | std::string feas_p = path+"result/abs/feas.txt"; 251 | cmd = "sh /MeshTracker/resource/scripts/gen_regions.sh "+descriptors_p+" "+feas_p; 252 | system(cmd.c_str()); 253 | exit(1); 254 | } 255 | #endif 256 | 257 | // set variables and pass object to mt 258 | icp::IcpOptions icp_ops; 259 | icp_ops.use_adaptive = true; 260 | icp_ops.parallel = true; 261 | std::string regions_file = path+"input/regions.txt"; 262 | std::string tracking_out = path+"result/"; 263 | 264 | // perform tracking and detail synth 265 | mt.ReadMeshList(path+"input/meshlist.txt"); 266 | mt.ReadRegions(regions_file); 267 | mt.TrackMeshSequence(icp_ops,tracking_out); 268 | 269 | // Kalman smoothing 270 | // Create meshlist 271 | // std::string cmd = "ls -d $PWD/"+path+"result/*_t.obj > "+path+"result/meshlist.txt"; 272 | // system(cmd.c_str()); 273 | // KalmanSmoother ks; 274 | // ks.readMeshes(path+"result/smooth/meshlist.txt"); 275 | // ks.ReadRegions(regions_file); 276 | // ks.smoothInRegions(); 277 | // std::cerr<<"done smoothing, saving"<<"\n"; 278 | // ks.setupSmoothMeshes(true); 279 | 280 | toc = time(0); 281 | cout << "Elapsed: "<< difftime(toc, tic) <<" second(s)."<< endl; 282 | 283 | // Calculate and export hdorf metrics 284 | // std::vector errors = GetSequenceError(path+"input/meshlist.txt",tracking_out); 285 | // std::string err_path = tracking_out + "error.txt"; 286 | // SaveSequenceError(errors,err_path); 287 | } 288 | 289 | static void SequenceTestingMattHello() 290 | { 291 | std::string path = "/MeshTracker/resource/tests/sequence_test/matt_hello/"; 292 | SequenceTesting(path); 293 | } 294 | 295 | static void SequenceTestingFloss() 296 | { 297 | std::string path = "/MeshTracker/resource/tests/sequence_test/floss/"; 298 | SequenceTesting(path); 299 | } 300 | 301 | } // end namespace 302 | #endif -------------------------------------------------------------------------------- /meshtracker/include/cpd_transform.h: -------------------------------------------------------------------------------- 1 | // cpd - Coherent Point Drift 2 | // Copyright (C) 2017 Pete Gadomski 3 | // 4 | // This program is free software; you can redistribute it and/or modify 5 | // it under the terms of the GNU General Public License as published by 6 | // the Free Software Foundation; either version 2 of the License, or 7 | // (at your option) any later version. 8 | // 9 | // This program is distributed in the hope that it will be useful, 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | // GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License along 15 | // with this program; if not, write to the Free Software Foundation, Inc., 16 | // 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | /// \file 19 | /// 20 | /// Generic coherent point drift transform. 21 | /// 22 | /// Downstreams shouldn't need to include this file directly, use a realization 23 | /// of a transform (e.g. `Rigid`) instead. 24 | 25 | #pragma once 26 | 27 | #include 28 | #include "cpd_gauss_transform.h" 29 | #include "cpd_matrix.h" 30 | #include "cpd_normalization.h" 31 | #include "cpd_utils.h" 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | namespace cpd { 38 | 39 | /// The default number of iterations allowed. 40 | const size_t DEFAULT_MAX_ITERATIONS = 150; 41 | /// Whether points should be normalized by default. 42 | const bool DEFAULT_NORMALIZE = false; 43 | /// The default outlier weight. 44 | const double DEFAULT_OUTLIERS = 0.1; 45 | /// The default tolerance. 46 | const double DEFAULT_TOLERANCE = 1e-8; 47 | /// The default sigma2. 48 | const double DEFAULT_SIGMA2 = 0.0; 49 | /// Whether correspondence vector should be computed by default. 50 | const bool DEFAULT_CORRESPONDENCE = false; 51 | /// Are the scalings of the two datasets linked by default? 52 | const bool DEFAULT_LINKED = true; 53 | 54 | /// The result of a generic transform run. 55 | struct Result { 56 | /// The final moved points. 57 | Matrix points; 58 | /// The final sigma2 value. 59 | double sigma2; 60 | /// The correspondence vector. 61 | IndexVector correspondence; 62 | /// The runtime. 63 | std::chrono::microseconds runtime; 64 | /// The number of iterations. 65 | size_t iterations; 66 | 67 | // The transformation used to produce result 68 | Matrix tform; 69 | 70 | /// De-normalize this result. 71 | /// 72 | /// Generally, this scales the points back, and sometimes adjust transforms 73 | /// or shifts or the like. 74 | virtual void denormalize(const Normalization& normalization); 75 | }; 76 | 77 | /// Generic coherent point drift transform. 78 | /// 79 | /// An abstract base class for real transforms, e.g. `Rigid` or `Nonrigid`. 80 | template 81 | class Transform { 82 | public: 83 | Transform() 84 | : m_correspondence(DEFAULT_CORRESPONDENCE) 85 | , m_gauss_transform(GaussTransform::make_default()) 86 | , m_max_iterations(DEFAULT_MAX_ITERATIONS) 87 | , m_normalize(DEFAULT_NORMALIZE) 88 | , m_outliers(DEFAULT_OUTLIERS) 89 | , m_sigma2(DEFAULT_SIGMA2) 90 | , m_tolerance(DEFAULT_TOLERANCE) {} 91 | 92 | inline void set_mesh_num(const int& mesh_num){ 93 | m_mesh_num=mesh_num; 94 | } 95 | 96 | /// Sets whether the correspondence vector will be computed. 97 | Transform& correspondence(bool correspondence) { 98 | m_correspondence = correspondence; 99 | return *this; 100 | } 101 | 102 | /// Sets the gauss transform. 103 | Transform& gauss_transform( 104 | std::unique_ptr gauss_transform) { 105 | m_gauss_transform = std::move(gauss_transform); 106 | return *this; 107 | } 108 | 109 | /// Sets the max iterations for this transform. 110 | Transform& max_iterations(double max_iterations) { 111 | m_max_iterations = max_iterations; 112 | return *this; 113 | } 114 | 115 | /// Sets whether to normalize the points before running cpd. 116 | Transform& normalize(bool normalize) { 117 | m_normalize = normalize; 118 | return *this; 119 | } 120 | 121 | /// Sets the outlier tolerance. 122 | Transform& outliers(double outliers) { 123 | m_outliers = outliers; 124 | return *this; 125 | } 126 | 127 | /// Sets the sigma2 value for this transform. 128 | Transform& sigma2(double sigma2) { 129 | m_sigma2 = sigma2; 130 | return *this; 131 | } 132 | 133 | /// Sets the final tolerance. 134 | Transform& tolerance(double tolerance) { 135 | m_tolerance = tolerance; 136 | return *this; 137 | } 138 | 139 | /// Sets the tracked mesh number. 140 | Transform& mesh_num(int mesh_num) { 141 | m_mesh_num = mesh_num; 142 | return *this; 143 | } 144 | 145 | /// Runs this transform for the provided matrices. 146 | Result run(Matrix fixed, Matrix moving) { 147 | auto tic = std::chrono::high_resolution_clock::now(); 148 | Normalization normalization(fixed, moving, linked()); 149 | if (m_normalize) { 150 | fixed = normalization.fixed; 151 | moving = normalization.moving; 152 | } 153 | this->init(fixed, moving); 154 | Result result; 155 | result.points = moving; 156 | 157 | // Sigma2 : Isotropic Covariance Matrix 158 | if (m_sigma2 == 0.0) { 159 | result.sigma2 = cpd::default_sigma2(fixed, moving); 160 | } else if (m_normalize) { 161 | result.sigma2 = m_sigma2 / normalization.fixed_scale; 162 | } else { 163 | result.sigma2 = m_sigma2; 164 | } 165 | 166 | int iter = 0; 167 | double ntol = m_tolerance + 10.0; 168 | double l = 0.; 169 | 170 | while (iter < m_max_iterations && ntol > m_tolerance && 171 | // while (iter < 1 && ntol > m_tolerance && 172 | result.sigma2 > 10 * std::numeric_limits::epsilon()) { 173 | Probabilities probabilities = m_gauss_transform->compute( 174 | fixed, result.points, result.sigma2, m_outliers); 175 | 176 | visualize_probabilities(probabilities,moving,m_mesh_num,iter); 177 | this->modify_probabilities(probabilities); 178 | 179 | ntol = std::abs((probabilities.l - l) / probabilities.l); 180 | l = probabilities.l; 181 | result = 182 | this->compute_one(fixed, moving, probabilities, result.sigma2); 183 | ++iter; 184 | } 185 | 186 | if (m_normalize) { 187 | result.denormalize(normalization); 188 | } 189 | 190 | if (m_correspondence) { 191 | GaussTransformDirect direct; 192 | Probabilities probabilities = direct.compute( 193 | fixed, result.points, result.sigma2, m_outliers); 194 | result.correspondence = probabilities.correspondence; 195 | assert(result.correspondence.rows() > 0); 196 | } 197 | 198 | auto toc = std::chrono::high_resolution_clock::now(); 199 | result.runtime = 200 | std::chrono::duration_cast(toc - tic); 201 | result.iterations = iter; 202 | 203 | return result; 204 | } 205 | 206 | /// Initialize this transform for the provided matrices. 207 | /// 208 | /// This is called before beginning each run, but after normalization. In 209 | /// general, transforms shouldn't need to be initialized, but the nonrigid 210 | /// does. 211 | virtual void init(const Matrix& fixed, const Matrix& moving) { 212 | } 213 | 214 | /// Modifies `Probabilities` in some way. 215 | /// 216 | /// Some types of transform need to tweak the probabilities before moving on 217 | /// with an interation. The default behavior is to do nothing. 218 | virtual void modify_probabilities(Probabilities& probabilities) const {} 219 | 220 | /// Computes one iteration of the transform. 221 | virtual Result compute_one(const Matrix& fixed, const Matrix& moving, 222 | const Probabilities& probabilities, 223 | double sigma2) const = 0; 224 | 225 | /// Returns true if the normalization should be linked. 226 | /// 227 | /// No effect if no normalization is applied. 228 | virtual bool linked() const = 0; 229 | 230 | // used in probability visualization 231 | static void val2rgb(float min, float max, float val, int (&rgb)[3]){ 232 | float ratio = 2* (val-min)/(max-min); 233 | rgb[0] = int(std::max(0.0f,255*(1-ratio))); 234 | rgb[2] = int(std::max(0.0f,255*(ratio-1))); 235 | rgb[1] = 255 - rgb[2] - rgb[0]; 236 | } 237 | 238 | // TODO: Modify to accept only prob matrix that we're interested in and 239 | // perform a check to make sure fixed.size == prob_mat.size 240 | static void visualize_probabilities(const Probabilities& probabilities, 241 | const Matrix& fixed, const int& mesh_num, const int& iteration){ 242 | 243 | float p1_max =probabilities.p1(0); 244 | float p1_min =probabilities.p1(0); 245 | for (size_t index = 0; index < probabilities.p1.rows(); index++) { 246 | if(probabilities.p1(index)>p1_max) p1_max = probabilities.p1(index); 247 | if(probabilities.p1(index) m_gauss_transform; 285 | size_t m_max_iterations; 286 | bool m_normalize; 287 | double m_outliers; 288 | double m_sigma2; 289 | double m_tolerance; 290 | int m_mesh_num=0; 291 | }; 292 | } // namespace cpd 293 | -------------------------------------------------------------------------------- /meshtracker/include/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef _UTILS_H 2 | #define _UTILS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "log.h" 31 | 32 | typedef std::chrono::high_resolution_clock c_clock; 33 | 34 | typedef pcl::PointCloud::Ptr CloudPtr; 35 | typedef pcl::geometry::PolygonMesh> Mesh; 36 | 37 | typedef Mesh::VertexIndex VertexIndex; 38 | typedef Mesh::VertexIndices VertexIndices; 39 | typedef Mesh::FaceAroundVertexCirculator FAVC; 40 | typedef Mesh::VertexAroundFaceCirculator VAFC; 41 | 42 | // Vertex_id , Segment_id mapping 43 | typedef std::map VidSidMap; 44 | 45 | 46 | namespace utils 47 | { 48 | 49 | static auto square = [](const float argu) { return argu * argu; }; 50 | static auto cube = [](const float argu) { return argu * argu * argu; }; 51 | static auto max = [](const float lhs, const float rhs) { return lhs > rhs ? lhs : rhs; }; 52 | 53 | static bool PairSortPredicate(const std::pair& lhs, const std::pair& rhs) 54 | { 55 | return (lhs.first == rhs.first && lhs.second == rhs.second); 56 | } 57 | 58 | //////////////////////////////////////////////////////////////////////////////// 59 | 60 | // Return time difference in s using std::chrono::high_resolution_clock points 61 | static double time_elapsed(c_clock::time_point& t1, c_clock::time_point& t2) 62 | { 63 | return std::chrono::duration_cast>(t2 - t1).count(); 64 | } 65 | 66 | //////////////////////////////////////////////////////////////////////////////// 67 | 68 | // Given PolygonMesh, write to stringstream for CGAL. More versatile to provide 69 | // stream rather than a specific type as cgal can automatically assign type. 70 | static std::stringstream PclMesh2CgalStream(const pcl::PolygonMesh &mesh) 71 | { 72 | 73 | const unsigned int num_points = mesh.cloud.height * mesh.cloud.width; 74 | const unsigned int num_polygons = mesh.polygons.size(); 75 | 76 | if (num_points == 0 || num_polygons == 0) 77 | { 78 | std::string e = "In PclMesh2CgalStream: Invalid input mesh"; 79 | BOOST_LOG_TRIVIAL(error) << e; 80 | throw std::runtime_error(e); 81 | } 82 | 83 | // convert to more manageable format 84 | CloudPtr cloud(new pcl::PointCloud); 85 | pcl::fromPCLPointCloud2(mesh.cloud, *cloud); 86 | 87 | std::stringstream ss; 88 | 89 | // write header 90 | ss << "OFF\n"; 91 | ss << num_points << " " << num_polygons << " 0" 92 | << "\n"; 93 | 94 | // write points 95 | for (size_t pt = 0; pt < num_points; pt++) 96 | { 97 | double x = cloud->points[pt].x; 98 | double y = cloud->points[pt].y; 99 | double z = cloud->points[pt].z; 100 | ss << x << " " << y << " " << z << "\n"; 101 | } 102 | 103 | // write polygons 104 | for (size_t poly = 0; poly < num_polygons; poly++) 105 | { 106 | int idx1 = mesh.polygons[poly].vertices[0]; 107 | int idx2 = mesh.polygons[poly].vertices[1]; 108 | int idx3 = mesh.polygons[poly].vertices[2]; 109 | ss << "3 " << idx1 << " " << idx2 << " " << idx3 << "\n"; 110 | } 111 | 112 | return ss; 113 | } 114 | 115 | //////////////////////////////////////////////////////////////////////////////// 116 | 117 | // Currently only accept triangle polygon objects 118 | // TODO: Update for robustness, headers can vary 119 | static pcl::PolygonMesh CgalStream2PclMesh(std::stringstream &ss) 120 | { 121 | 122 | // create mesh and cloud objects 123 | pcl::PolygonMesh mesh; 124 | CloudPtr cloud(new pcl::PointCloud); 125 | 126 | std::string line; 127 | unsigned int line_num = 0; 128 | unsigned int num_verts, num_faces; 129 | 130 | while (std::getline(ss, line)) 131 | { 132 | 133 | if (line.empty()) 134 | { 135 | ++line_num; 136 | continue; 137 | } 138 | 139 | // fancy way of reading whitespace delimited data 140 | std::istringstream iss(line); 141 | std::vector tokens; 142 | 143 | copy(std::istream_iterator(iss), 144 | std::istream_iterator(), 145 | std::back_inserter(tokens)); 146 | 147 | // extract header info 148 | if (line_num == 1) 149 | { 150 | num_verts = std::stoi(tokens[0]); 151 | num_faces = std::stoi(tokens[1]); 152 | ++line_num; 153 | continue; 154 | } 155 | 156 | if (tokens[0].compare("OFF") == 0) 157 | { 158 | ++line_num; 159 | continue; 160 | } 161 | 162 | // get polygon tri 163 | if (tokens[0].compare("3") == 0) 164 | { 165 | unsigned int v1, v2, v3; 166 | v1 = std::stoi(tokens[1]); 167 | v2 = std::stoi(tokens[2]); 168 | v3 = std::stoi(tokens[3]); 169 | 170 | pcl::Vertices verts; 171 | verts.vertices.push_back(v1); 172 | verts.vertices.push_back(v2); 173 | verts.vertices.push_back(v3); 174 | mesh.polygons.push_back(verts); 175 | } 176 | else 177 | { 178 | 179 | //get vertex 180 | pcl::PointXYZRGBNormal pt; 181 | pt.x = std::stof(tokens[0]); 182 | pt.y = std::stof(tokens[1]); 183 | pt.z = std::stof(tokens[2]); 184 | 185 | cloud->push_back(pt); 186 | } 187 | ++line_num; 188 | } // end lines 189 | 190 | // attach cloud to ouput mesh 191 | pcl::toPCLPointCloud2(*cloud, mesh.cloud); 192 | 193 | return mesh; 194 | } 195 | 196 | //////////////////////////////////////////////////////////////////////////////// 197 | 198 | // Checks if vectors are roughly aligned, 199 | static bool areAligned( 200 | Eigen::Vector3d a, 201 | Eigen::Vector3d b, 202 | double tolerance) 203 | { 204 | 205 | // normalize 206 | a.normalize(); 207 | b.normalize(); 208 | 209 | bool ans; 210 | a.dot(b) > tolerance ? ans = true : ans = false; 211 | 212 | return ans; 213 | } 214 | 215 | //////////////////////////////////////////////////////////////////////////////// 216 | 217 | // Simple Linear interpolation, step normalized [0,1] 218 | static double LERP( 219 | double min, 220 | double max, 221 | double step) 222 | { 223 | return min + (max - min) * step; 224 | } 225 | 226 | //////////////////////////////////////////////////////////////////////////////// 227 | 228 | // double* Mat2Quat(const Eigen::MatrixXd& mat) 229 | // { 230 | 231 | // // TODO: check that matrix is orothogonal and special orthogonal 232 | // float tr = mat(0,0) + mat(1,1) + mat(2,2); 233 | // double* q[4]; 234 | 235 | // if (tr > 0) { 236 | // float S = sqrt(tr+1.0) * 2; // S=4*qw 237 | // *q[0] = 0.25 * S; 238 | // *q[1] = (mat(2,1) - mat(1,2)) / S; 239 | // *q[2] = (mat(0,2) - mat(2,0)) / S; 240 | // *q[3] = (mat(1,0) - mat(0,1)) / S; 241 | // } else if ((mat(0,0) > mat(1,1))&(mat(0,0) > mat(2,2))) { 242 | // float S = sqrt(1.0 + mat(0,0) - mat(1,1) - mat(2,2)) * 2; // S=4*qx 243 | // *q[0] = (mat(2,1) - mat(1,2)) / S; 244 | // *q[1] = 0.25 * S; 245 | // *q[2] = (mat(0,1) + mat(1,0)) / S; 246 | // *q[3] = (mat(0,2) + mat(2,0)) / S; 247 | // } else if (mat(1,1) > mat(2,2)) { 248 | // float S = sqrt(1.0 + mat(1,1) - mat(0,0) - mat(2,2)) * 2; // S=4*qy 249 | // *q[0] = (mat(0,2) - mat(2,0)) / S; 250 | // *q[1] = (mat(0,1) + mat(1,0)) / S; 251 | // *q[2] = 0.25 * S; 252 | // *q[3] = (mat(1,2) + mat(2,1)) / S; 253 | // } else { 254 | // float S = sqrt(1.0 + mat(2,2) - mat(0,0) - mat(1,1)) * 2; // S=4*qz 255 | // *q[0] = (mat(1,0) - mat(0,1)) / S; 256 | // *q[1] = (mat(0,2) + mat(2,0)) / S; 257 | // *q[2] = (mat(1,2) + mat(2,1)) / S; 258 | // *q[3] = 0.25 * S; 259 | // } 260 | 261 | // return *q; 262 | // } 263 | 264 | //////////////////////////////////////////////////////////////////////////////// 265 | 266 | // Eigen::MatrixXd Quat2Mat(const double* q) 267 | // { 268 | // double sqw = q[0]*q[0]; 269 | // double sqx = q[1]*q[1]; 270 | // double sqy = q[2]*q[2]; 271 | // double sqz = q[3]*q[3]; 272 | 273 | // Eigen::MatrixXd mat; 274 | // // invs (inverse square length) 275 | // // is only required if quaternion is not already normalised 276 | // double invs = 1 / (sqx + sqy + sqz + sqw); 277 | // mat(0,0) = ( sqx - sqy - sqz + sqw)*invs ; // since sqw + sqx + sqy + sqz =1/invs*invs 278 | // mat(1,1) = (-sqx + sqy - sqz + sqw)*invs ; 279 | // mat(2,2) = (-sqx - sqy + sqz + sqw)*invs ; 280 | 281 | // double tmp1 = q[1]*q[2]; 282 | // double tmp2 = q[3]*q[0]; 283 | // mat(1,0) = 2.0 * (tmp1 + tmp2)*invs ; 284 | // mat(0,1) = 2.0 * (tmp1 - tmp2)*invs ; 285 | 286 | // tmp1 = q[1]*q[3]; 287 | // tmp2 = q[2]*q[0]; 288 | // mat(2,0) = 2.0 * (tmp1 - tmp2)*invs ; 289 | // mat(0,2) = 2.0 * (tmp1 + tmp2)*invs ; 290 | // tmp1 = q[2]*q[3]; 291 | // tmp2 = q[1]*q[0]; 292 | // mat(2,1) = 2.0 * (tmp1 + tmp2)*invs ; 293 | // mat(1,2) = 2.0 * (tmp1 - tmp2)*invs ; 294 | 295 | // return mat; 296 | // } 297 | 298 | //////////////////////////////////////////////////////////////////////////////// 299 | 300 | // used to sort nested vectors based on first index 301 | // TODO: replace with a lambda 302 | static bool colsort( const std::vector& v1, 303 | const std::vector& v2 ) { 304 | return v1[0] < v2[0]; 305 | } 306 | 307 | //////////////////////////////////////////////////////////////////////////////// 308 | 309 | static int RandInt(const int& min = 0, const int& max = 100) 310 | { 311 | std::random_device rand_dev; 312 | std::mt19937 generator(rand_dev()); 313 | std::uniform_int_distribution distr(min, max); 314 | return distr(generator); 315 | } 316 | 317 | //////////////////////////////////////////////////////////////////////////////// 318 | 319 | // Remove duplicates from unsorted list object 320 | template 321 | static ForwardIterator remove_duplicates( 322 | ForwardIterator first, 323 | ForwardIterator last ) 324 | { 325 | auto new_last = first; 326 | 327 | for ( auto current = first; current != last; ++current ) 328 | { 329 | if ( std::find( first, new_last, *current ) == new_last ) 330 | { 331 | if ( new_last != current ) *new_last = *current; 332 | ++new_last; 333 | } 334 | } 335 | 336 | return new_last; 337 | } 338 | 339 | //////////////////////////////////////////////////////////////////////////////// 340 | 341 | // from Evan Teran on SO: 342 | // http://stackoverflow.com/questions/216823/whats-the-best-way-to-trim-stdstring 343 | 344 | // trim from start 345 | static inline std::string <rim(std::string &s) 346 | { 347 | s.erase( 348 | s.begin(), 349 | std::find_if(s.begin(), 350 | s.end(), 351 | std::not1(std::ptr_fun(std::isspace)))); 352 | return s; 353 | } 354 | 355 | // trim from end 356 | static inline std::string &rtrim(std::string &s) 357 | { 358 | s.erase( 359 | std::find_if(s.rbegin(), 360 | s.rend(), 361 | std::not1(std::ptr_fun(std::isspace))).base(), s.end()); 362 | return s; 363 | } 364 | 365 | // trim from both ends 366 | static inline std::string &trim(std::string &s) 367 | { 368 | return ltrim(rtrim(s)); 369 | } 370 | 371 | 372 | //////////////////////////////////////////////////////////////////////////////// 373 | 374 | } // namespace utils 375 | #endif 376 | 377 | -------------------------------------------------------------------------------- /meshtracker/include/mesh_processing.h: -------------------------------------------------------------------------------- 1 | #ifndef _MESH_PROCESSING_H 2 | #define _MESH_PROCESSING_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | // #include "cloud_processing.h" 36 | # include "constants.h" 37 | #include "utils.h" 38 | #include "log.h" 39 | 40 | typedef pcl::PointCloud::Ptr CloudPtr; 41 | typedef pcl::geometry::PolygonMesh< 42 | pcl::geometry::DefaultMeshTraits> Mesh; 43 | 44 | typedef CGAL::Exact_predicates_inexact_constructions_kernel K; 45 | typedef K::Point_3 Point; 46 | typedef K::Vector_3 Vector; 47 | typedef CGAL::Surface_mesh Surface_mesh; 48 | typedef CGAL::Surface_mesh Surface_Mesh_3; 49 | 50 | typedef boost::graph_traits::vertex_descriptor vertex_descriptor; 51 | typedef boost::graph_traits::face_descriptor face_descriptor; 52 | typedef boost::graph_traits< 53 | Surface_Mesh_3>::vertex_descriptor sm3_vertex_descriptor; 54 | typedef boost::graph_traits< 55 | Surface_Mesh_3>::face_descriptor sm3_face_descriptor; 56 | typedef boost::graph_traits< 57 | Surface_Mesh_3>::halfedge_descriptor sm3_halfedge_descriptor; 58 | typedef boost::graph_traits< 59 | Surface_Mesh_3>::edge_descriptor sm3_edge_descriptor; 60 | 61 | namespace PMP = CGAL::Polygon_mesh_processing; 62 | 63 | struct halfedge2edge 64 | { 65 | const Surface_Mesh_3 & m_mesh; 66 | std::vector & m_edges; 67 | 68 | halfedge2edge( 69 | const Surface_Mesh_3 & m, 70 | std::vector & edges): 71 | m_mesh(m), 72 | m_edges(edges) 73 | {} 74 | 75 | void operator()( 76 | const sm3_halfedge_descriptor & h) const 77 | { 78 | m_edges.push_back(edge(h, m_mesh)); 79 | } 80 | }; 81 | 82 | 83 | namespace mesh_processing 84 | { 85 | 86 | struct BBox 87 | { 88 | 89 | double min_x = DBL_MAX; 90 | double min_y = DBL_MAX; 91 | double min_z = DBL_MAX; 92 | double max_x = -DBL_MAX; 93 | double max_y = -DBL_MAX; 94 | double max_z = -DBL_MAX; 95 | double cx = 0; 96 | double cy = 0; 97 | double cz = 0; 98 | double l_diag_sq = 0; 99 | 100 | BBox(const pcl::PolygonMesh & mesh){ this->get(mesh);}; 101 | BBox(const CloudPtr & cloud){ this->get(cloud);}; 102 | 103 | void get(const pcl::PolygonMesh & mesh) 104 | { 105 | CloudPtr cloud(new pcl::PointCloud); 106 | pcl::fromPCLPointCloud2(mesh.cloud,*cloud); 107 | 108 | for (size_t pt = 0; pt < cloud->points.size(); pt++) 109 | { 110 | pcl::PointXYZRGBNormal p = cloud->points[pt]; 111 | 112 | if (p.x < this->min_x) this->min_x = p.x; 113 | if (p.x > this->max_x) this->max_x = p.x; 114 | if (p.y < this->min_y) this->min_y = p.y; 115 | if (p.y > this->max_y) this->max_y = p.y; 116 | if (p.z < this->min_z) this->min_z = p.z; 117 | if (p.z > this->max_z) this->max_z = p.z; 118 | } 119 | 120 | this->cx = this->min_x + (this->max_x - this->min_x)*0.5; 121 | this->cy = this->min_y + (this->max_y - this->min_y)*0.5; 122 | this->cz = this->min_z + (this->max_z - this->min_z)*0.5; 123 | 124 | this->l_diag_sq = (max_x - min_x) + (max_y - min_y); 125 | }; 126 | 127 | void get(const CloudPtr & cloud) 128 | { 129 | for (size_t pt = 0; pt < cloud->points.size(); pt++) 130 | { 131 | pcl::PointXYZRGBNormal p = cloud->points[pt]; 132 | 133 | if (p.x < this->min_x) this->min_x = p.x; 134 | if (p.x > this->max_x) this->max_x = p.x; 135 | if (p.y < this->min_y) this->min_y = p.y; 136 | if (p.y > this->max_y) this->max_y = p.y; 137 | if (p.z < this->min_z) this->min_z = p.z; 138 | if (p.z > this->max_z) this->max_z = p.z; 139 | } 140 | 141 | this->cx = this->min_x + (this->max_x - this->min_x)*0.5; 142 | this->cy = this->min_y + (this->max_y - this->min_y)*0.5; 143 | this->cz = this->min_z + (this->max_z - this->min_z)*0.5; 144 | 145 | this->l_diag_sq = (max_x - min_x) + (max_y - min_y); 146 | }; 147 | 148 | void print() 149 | { 150 | std::cout<<"Bbox:"<min_x<<", "<max_x<<"] l:"<max_x - this->min_x<min_y<<", "<max_y<<"] l:"<max_y - this->min_y<min_z<<", "<max_z<<"] l:"<max_z - this->min_z<cx<<", "<cy<<", "<cz<<"]"< & offset); 250 | 251 | // Fix any non-manifold faces in polygon mesh 252 | pcl::PolygonMesh FixManifoldness( 253 | const pcl::PolygonMesh & mesh); 254 | 255 | // Calculates the centroid of all 1-ring neighbours for a given point 256 | pcl::PointXYZRGBNormal GetOneRingCentroid( 257 | const pcl::PolygonMesh & mesh, 258 | const int & init_pt); 259 | 260 | // Returns list of indices for 1-ring connected vertices for a 261 | // given vertex index 262 | std::vector GetOneRingNeighbors( 263 | const pcl::PolygonMesh & mesh, 264 | const int & init_pt); 265 | 266 | // Returns list of indices for 1-ring connected vertices for a 267 | // given vertex index 268 | std::vector GetTwoRingNeighbors( 269 | const pcl::PolygonMesh & mesh, 270 | const int & init_pt); 271 | 272 | // Returns list of all k-ring connected vertices 273 | // WARNING: Scales terribly 274 | std::vector GetKRingNeighbors( 275 | const pcl::PolygonMesh & mesh, 276 | const int & vid, 277 | const int & k); 278 | 279 | // Checks the first point to see if xyz normals are non-zero 280 | bool NormalsExist( 281 | const pcl::PolygonMesh & mesh); 282 | 283 | // Takes a polygon mesh object and calculates vertex normals, 284 | // option to return normals separately 285 | pcl::PolygonMesh CalculateVertexNormals( 286 | const pcl::PolygonMesh & mesh); 287 | 288 | // Takes a PolygonMesh and returns a point cloud with normals 289 | CloudPtr GetCloudFromPolygonMesh( 290 | pcl::PolygonMesh & mesh); 291 | 292 | // Check for NaN points in vertex data 293 | bool HasNanData( 294 | const pcl::PolygonMesh & mesh); 295 | 296 | // Performs LERP between source and target mesh. Meshes must have the same 297 | // number of vertices and topology. Step is expressed as between [0,1] 298 | pcl::PolygonMesh LERPMesh( 299 | const pcl::PolygonMesh source, 300 | const pcl::PolygonMesh target, 301 | const float step = 0.5); 302 | 303 | // Fix any nan points by using connectivity info to replace nan data 304 | // with the centroid of connecting points 305 | pcl::PolygonMesh FixNanPoints( 306 | const pcl::PolygonMesh & mesh_in); 307 | 308 | // // Given a mesh, a source and a vector of points, returns the input 309 | // // vector sorted by descrete geodesic distance wrt input point 310 | // std::vector SortByGeodesicDistance( 311 | // const pcl::PolygonMesh& mesh, 312 | // const int& src_pt_idx, 313 | // const std::vector& pts_indxs); 314 | 315 | // // Get descrete geodesic distance between src and dst vertex along mesh 316 | // int GetGeodesicDistance( 317 | // const pcl::PolygonMesh& mesh, 318 | // const int& src_pt, 319 | // const int& dst_pt); 320 | }; 321 | 322 | namespace MeshProcDefs 323 | { 324 | const int PQEC = 0; 325 | const int VG = 1; 326 | } 327 | 328 | 329 | 330 | 331 | #endif -------------------------------------------------------------------------------- /meshtracker/src/cloud_processing.cpp: -------------------------------------------------------------------------------- 1 | #include "cloud_processing.h" 2 | 3 | //////////////////////////////////////////////////////////////////////////////// 4 | 5 | CloudPtr CloudProcessing::SmoothCloudMLS( 6 | const CloudPtr &cloud) 7 | { 8 | 9 | CloudPtr out_cloud(new pcl::PointCloud(*cloud)); 10 | pcl::PointCloud::Ptr xyz_cloud(new pcl::PointCloud); 11 | 12 | pcl::copyPointCloud(*out_cloud, *xyz_cloud); 13 | 14 | pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); 15 | 16 | // Output has the PointNormal type in order to store the normals calculated by MLS 17 | pcl::PointCloud mls_points; 18 | 19 | // Init object (second point type is for the normals, even if unused) 20 | pcl::MovingLeastSquares mls; 21 | 22 | mls.setComputeNormals(true); 23 | 24 | // Set parameters 25 | mls.setInputCloud(xyz_cloud); 26 | mls.setPolynomialFit(true); 27 | mls.setSearchMethod(tree); 28 | mls.setSearchRadius(10); //TODO Define radius 29 | 30 | // Reconstruct 31 | mls.process(mls_points); 32 | 33 | pcl::copyPointCloud(*xyz_cloud, *out_cloud); 34 | 35 | return out_cloud; 36 | } 37 | 38 | //////////////////////////////////////////////////////////////////////////////// 39 | 40 | double CloudProcessing::ComputeCloudResolution( 41 | const CloudPtr &cloud) 42 | { 43 | 44 | double res = 0.0; 45 | int n_points = 0; 46 | int nres; 47 | std::vector indices(2); 48 | std::vector sqr_distances(2); 49 | pcl::search::KdTree tree; 50 | 51 | tree.setInputCloud(cloud); 52 | 53 | for (size_t i = 1; i < cloud->size(); ++i) 54 | { 55 | if (!pcl_isfinite((*cloud)[i].x)) 56 | { 57 | continue; 58 | } 59 | //Considering the second neighbor since the first is the point itself. 60 | nres = tree.nearestKSearch(i, 2, indices, sqr_distances); 61 | if (nres == 2) 62 | { 63 | res += sqrt(sqr_distances[1]); 64 | ++n_points; 65 | } 66 | } 67 | 68 | if (n_points != 0) 69 | { 70 | res /= n_points; 71 | } 72 | 73 | return res; 74 | } 75 | 76 | //////////////////////////////////////////////////////////////////////////////// 77 | 78 | bool CloudProcessing::NormalsExist( 79 | const CloudPtr &cloud) 80 | { 81 | 82 | if (cloud->points[0].normal_x == 0 && 83 | cloud->points[0].normal_y == 0 && 84 | cloud->points[0].normal_z == 0) 85 | { 86 | return false; 87 | } 88 | return true; 89 | } 90 | 91 | //////////////////////////////////////////////////////////////////////////////// 92 | 93 | void CloudProcessing::CalculatePointCloudNormals( 94 | CloudPtr& cloud) 95 | { 96 | pcl::NormalEstimationOMP ne; 97 | ne.setInputCloud (cloud); 98 | 99 | pcl::search::KdTree::Ptr tree 100 | (new pcl::search::KdTree ()); 101 | ne.setSearchMethod (tree); 102 | 103 | double res = ComputeCloudResolution(cloud); 104 | ne.setRadiusSearch (res * 3); 105 | 106 | ne.compute (*cloud); 107 | } 108 | 109 | //////////////////////////////////////////////////////////////////////////////// 110 | 111 | std::vector CloudProcessing::FindOutliers( 112 | const CloudPtr &cloud) 113 | { 114 | 115 | // Radius Outlier removal removes all points that have less than the specified 116 | // number of neighbours within a given radius 117 | pcl::RadiusOutlierRemoval outrem(true); 118 | 119 | // build the filter 120 | outrem.setInputCloud(cloud); 121 | 122 | // use 2x density to specify radius 123 | double radius = ComputeCloudResolution(cloud); 124 | outrem.setRadiusSearch(radius * 4); 125 | outrem.setMinNeighborsInRadius(1); 126 | outrem.setNegative(true); // set true so that output returns the removed points 127 | 128 | // apply filter 129 | std::vector filtered_indices; 130 | outrem.filter(filtered_indices); 131 | 132 | return filtered_indices; 133 | } 134 | 135 | //////////////////////////////////////////////////////////////////////////////// 136 | 137 | double CloudProcessing::ComputeHausdorffDistance( 138 | const CloudPtr& cloud_a, 139 | const CloudPtr& cloud_b) 140 | { 141 | 142 | // compare A to B 143 | pcl::search::KdTree tree_b; 144 | tree_b.setInputCloud(cloud_b); 145 | double max_dist_a = -std::numeric_limits::max (); 146 | 147 | for (const auto &point : cloud_a->points) 148 | { 149 | std::vector indices (1); 150 | std::vector sqr_distances (1); 151 | 152 | tree_b.nearestKSearch (point, 1, indices, sqr_distances); 153 | if (sqr_distances[0] > max_dist_a) 154 | max_dist_a = sqr_distances[0]; 155 | } 156 | 157 | // compare B to A 158 | pcl::search::KdTree tree_a; 159 | tree_a.setInputCloud(cloud_a); 160 | double max_dist_b = -std::numeric_limits::max (); 161 | 162 | for (const auto &point : cloud_b->points) 163 | { 164 | std::vector indices (1); 165 | std::vector sqr_distances (1); 166 | 167 | tree_a.nearestKSearch (point, 1, indices, sqr_distances); 168 | if (sqr_distances[0] > max_dist_b) 169 | max_dist_b = sqr_distances[0]; 170 | } 171 | 172 | double max_dist = max_dist_a > max_dist_b ? max_dist_a : max_dist_b; 173 | return max_dist; 174 | } 175 | 176 | //////////////////////////////////////////////////////////////////////////////// 177 | 178 | double CloudProcessing::ComputeHausdorffDistance( 179 | const pcl::PointXYZRGBNormal& pt, 180 | const CloudPtr& cloud) 181 | { 182 | 183 | pcl::search::KdTree tree; 184 | tree.setInputCloud(cloud); 185 | 186 | std::vector indices (1); 187 | std::vector sqr_distances (1); 188 | 189 | tree.nearestKSearch(pt, 1, indices, sqr_distances); 190 | 191 | return std::sqrt(sqr_distances[0]); 192 | } 193 | 194 | //////////////////////////////////////////////////////////////////////////////// 195 | // Uses a voxel grid to approximate a subsampled surface. Can give errors for 196 | // thin objects when the voxel size is larger than the object as the approximation 197 | // will be a point lying in between 198 | 199 | CloudPtr CloudProcessing::ResamplePointCloud(CloudPtr& cloud){ 200 | 201 | CloudProcessing cp; 202 | 203 | // Create the filtering object 204 | CloudPtr out_cloud(new pcl::PointCloud(*cloud)); 205 | pcl::VoxelGrid sor; 206 | sor.setInputCloud (out_cloud); 207 | 208 | // set voxel grid size as small as possible. 1.5 has a high chance of being 209 | // large enough without badly approximating thin surfaces 210 | double leaf_size = cp.ComputeCloudResolution(cloud); 211 | sor.setLeafSize (leaf_size, leaf_size, leaf_size); 212 | sor.filter (*out_cloud); 213 | 214 | return out_cloud; 215 | } 216 | 217 | 218 | //////////////////////////////////////////////////////////////////////////////// 219 | 220 | CloudPtr CloudProcessing::UniformsamplePointCloud( 221 | const CloudPtr& cloud, 222 | std::optional max_size) 223 | { 224 | if (!max_size.has_value()) 225 | { 226 | max_size = this->kSamplesUpperLimit; 227 | } 228 | 229 | CloudPtr res_cloud(new pcl::PointCloud(*cloud)); 230 | 231 | // Create the filtering object 232 | pcl::VoxelGrid sor; 233 | sor.setInputCloud(res_cloud); 234 | 235 | int cloud_size = cloud->size(); 236 | double reduction_factor = 0.005; 237 | 238 | CloudPtr out_cloud(new pcl::PointCloud); 239 | 240 | // iteratively approach the ideal cloud size 241 | while (cloud_size > max_size.value()) { 242 | 243 | // get downsampled cloud 244 | // (this is quite uniform but it creates new points instead of actually sampling) 245 | double leaf_size = (reduction_factor); 246 | sor.setLeafSize (leaf_size, leaf_size, leaf_size); 247 | sor.filter (*res_cloud); 248 | 249 | // perform knn search for each resampled point toward input cloud 250 | pcl::KdTreeFLANN kdtree; 251 | kdtree.setInputCloud(cloud); 252 | 253 | const int search_length=2; // just has to be > 1 254 | std::vector visited_indices;// store visited points to avoid returning duplicates 255 | for (size_t point = 0; point < res_cloud->size(); point++) { 256 | 257 | std::vector pointIdxNKNSearch(search_length); 258 | std::vector pointNKNSquaredDistance(search_length); 259 | 260 | pcl::PointXYZRGBNormal search_pt = res_cloud->points[point]; 261 | 262 | int nres = kdtree.nearestKSearch(search_pt, search_length, 263 | pointIdxNKNSearch, pointNKNSquaredDistance); 264 | 265 | if (std::count(visited_indices.begin(),visited_indices.end(),pointIdxNKNSearch[0])==0) { 266 | out_cloud->push_back(cloud->points[pointIdxNKNSearch[0]]); 267 | visited_indices.push_back(pointIdxNKNSearch[0]); 268 | } 269 | } 270 | 271 | cloud_size = out_cloud->size(); 272 | if (cloud_size > this->kSamplesUpperLimit) { 273 | out_cloud->clear(); 274 | reduction_factor+=0.005; 275 | // BOOST_LOG_TRIVIAL(debug) 276 | // << "In CloudProcessing::UniformsamplePointCloud: increasing reduction factor to " 277 | // <(*cloud)); 291 | pcl::RandomSample sor; 292 | sor.setInputCloud(out_cloud); 293 | sor.setSample(samples); 294 | sor.setSeed(rand()); 295 | sor.filter (*out_cloud); 296 | 297 | return out_cloud; 298 | } 299 | 300 | //////////////////////////////////////////////////////////////////////////////// 301 | 302 | CloudPtr CloudProcessing::AlignCentres( 303 | const CloudPtr& moving, 304 | const CloudPtr& fixed) 305 | { 306 | 307 | CloudPtr out_cloud(new pcl::PointCloud(*moving)); 308 | 309 | cloud_processing::BBox moving_bb(moving); 310 | cloud_processing::BBox fixed_bb(fixed); 311 | 312 | Eigen::Vector3f tform( 313 | fixed_bb.cx - moving_bb.cx, 314 | fixed_bb.cy - moving_bb.cy, 315 | fixed_bb.cz - moving_bb.cz); 316 | 317 | for (size_t pt = 0; pt < out_cloud->size(); pt++) 318 | { 319 | out_cloud->points[pt].x += tform[0]; 320 | out_cloud->points[pt].y += tform[1]; 321 | out_cloud->points[pt].z += tform[2]; 322 | } 323 | 324 | return out_cloud; 325 | } 326 | 327 | //////////////////////////////////////////////////////////////////////////////// 328 | 329 | CloudPtr CloudProcessing::TransformToOrigin( 330 | const CloudPtr& cloud) 331 | { 332 | CloudPtr out(new pcl::PointCloud(*cloud)); 333 | 334 | cloud_processing::BBox bb(out); 335 | double tform[3] = {-bb.cx,-bb.cy,-bb.cz}; 336 | 337 | for (size_t pt = 0; pt < out->size(); pt++) 338 | { 339 | out->points[pt].x += tform[0]; 340 | out->points[pt].y += tform[1]; 341 | out->points[pt].z += tform[2]; 342 | } 343 | 344 | return out; 345 | } 346 | 347 | //////////////////////////////////////////////////////////////////////////////// 348 | 349 | CloudPtr CloudProcessing::RigidICP( 350 | const CloudPtr& moving, 351 | const CloudPtr& fixed, 352 | const double& max_pair_dist, 353 | const int& max_iters, 354 | const double& tform_epsilon, 355 | const double& fitness_epsilon) 356 | { 357 | pcl::IterativeClosestPoint icp; 358 | 359 | // Set the input source and target 360 | icp.setInputSource(moving); 361 | icp.setInputTarget(fixed); 362 | 363 | // Set the max correspondence distance (e.g., correspondences with higher distances will be ignored) 364 | icp.setMaxCorrespondenceDistance(max_pair_dist); 365 | 366 | // Set the maximum number of iterations 367 | icp.setMaximumIterations(max_iters); 368 | 369 | // Set the transformation epsilon 370 | icp.setTransformationEpsilon(tform_epsilon); 371 | 372 | // Set the euclidean distance difference epsilon 373 | icp.setEuclideanFitnessEpsilon(fitness_epsilon); 374 | 375 | // Perform the alignment 376 | CloudPtr aligned_cloud(new pcl::PointCloud); 377 | icp.align(*aligned_cloud); 378 | 379 | return aligned_cloud; 380 | 381 | } 382 | 383 | //////////////////////////////////////////////////////////////////////////////// 384 | 385 | void CloudProcessing::ColorizeCloud( 386 | CloudPtr& cloud, 387 | std::array& rgb) 388 | { 389 | 390 | if (rgb.size() != 3 || rgb[0]<0 || rgb[1]<0 || rgb[2]<0) 391 | { 392 | BOOST_LOG_TRIVIAL(warning) 393 | << "In ColorizeCloud: Please supply a valid color"; 394 | return; 395 | } 396 | 397 | for (size_t pt = 0; pt < cloud->size(); pt++) 398 | { 399 | cloud->points[pt].r = uint8_t(rgb[0]); 400 | cloud->points[pt].g = uint8_t(rgb[1]); 401 | cloud->points[pt].b = uint8_t(rgb[2]); 402 | } 403 | } 404 | 405 | //////////////////////////////////////////////////////////////////////////////// -------------------------------------------------------------------------------- /meshtracker/src/kalman_smoothing.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include "kalman_smoothing.h" 12 | 13 | int KalmanSmoother::readMeshes(const std::string &_fileName) 14 | { 15 | 16 | std::ifstream inputfile(_fileName.c_str()); 17 | 18 | if (inputfile.is_open()) 19 | { 20 | std::string line; 21 | do 22 | { 23 | std::getline(inputfile, line); 24 | if (line.empty()) 25 | continue; 26 | 27 | meshNames_.push_back(line); 28 | // std::cerr<setupClouds(); 49 | std::cerr << "Number of clouds loaded: " << clouds_.size() << std::endl; 50 | 51 | return 0; 52 | } 53 | 54 | int KalmanSmoother::ReadRegions(const std::string &file_name) 55 | { 56 | if (meshNames_.empty()) 57 | { 58 | std::cerr << "Load mesh sequence before finding keyframes!"<first != 0) 106 | { 107 | std::cerr << "Warning: Check regions.txt.\n" 108 | << "\tFirst index must always be 0\n"; 109 | regions_.begin()->first = 0; 110 | } 111 | 112 | if(regions_.back().second + 1 < meshNames_.size()) 113 | { 114 | std::cerr 115 | << "Warning: End of specified regions is less than input meshes!" 116 | <<"["<second = meshNames_.size() - 1; 119 | } 120 | } 121 | else 122 | { 123 | 124 | std::cerr << "Keyframe reader: couldn't open file " << file_name<<"\n"; 125 | return -1; 126 | } 127 | 128 | 129 | return 1; 130 | } 131 | 132 | void KalmanSmoother::setupClouds() 133 | { 134 | 135 | for (unsigned int i = 0; i < meshes_.size(); i++) 136 | { 137 | 138 | pcl::PolygonMesh mesh = meshes_[i]; 139 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 140 | pcl::fromPCLPointCloud2(mesh.cloud, *cloud); 141 | 142 | clouds_.push_back(cloud); 143 | } 144 | } 145 | 146 | void KalmanSmoother::smoothLinearly() 147 | { 148 | 149 | // Allocate space for the smooth clouds 150 | for (unsigned int i = 0; i < meshes_.size(); i++) 151 | { 152 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 153 | smoothClouds_.push_back(cloud); 154 | } 155 | 156 | // First find the mesh interval 157 | std::vector::iterator kfit = keyFrameIndices_.begin(); 158 | for (; kfit != keyFrameIndices_.end(); ++kfit) 159 | { 160 | 161 | // First and last indices in the tracked interval 162 | unsigned int first = *kfit; 163 | unsigned int last; 164 | 165 | if (kfit + 1 != keyFrameIndices_.end()) 166 | { 167 | last = *(kfit + 1) - 1; // The one previous to the next keyframe 168 | } 169 | else 170 | { 171 | last = meshes_.size() - 1; // The last one 172 | } 173 | 174 | std::cerr << first << "/" << last << std::endl; 175 | 176 | // We iterate through the meshes in this specific range 177 | pcl::PointCloud::Ptr keycloud = clouds_[first]; 178 | unsigned int nvtx = keycloud->height * keycloud->width; 179 | 180 | // Store the keycloud in the smoothclouds vector, as this one does not change 181 | smoothClouds_[first] = keycloud; 182 | 183 | // This vector will store all equivalent vertices in the mesh sequence 184 | std::vector vertexseq(last - first + 1); 185 | std::vector smoothvertexseq(last - first + 1); 186 | 187 | for (unsigned int i = 0; i < nvtx; i++) 188 | { 189 | 190 | unsigned int k = 0; 191 | for (unsigned int j = first; j <= last; j++, k++) 192 | { 193 | 194 | const pcl::PointCloud::Ptr &cloud = clouds_[j]; 195 | const pcl::PointXYZRGBNormal &point = cloud->points[i]; 196 | vertexseq[k] = point; 197 | 198 | } // end per cloud 199 | 200 | // vertexseq is already filled here 201 | // We use a 3D Kalman filter to smooth the positions 202 | this->filterKalman(vertexseq, smoothvertexseq); 203 | 204 | // Assign the new vertex to the whole sequence 205 | for (unsigned int j = first; j <= last; j++) 206 | { 207 | pcl::PointCloud::Ptr &cloud = smoothClouds_[j]; 208 | cloud->push_back(smoothvertexseq[j - first]); 209 | 210 | } // end per cloud 211 | 212 | } // end per vertex 213 | } 214 | } 215 | 216 | void KalmanSmoother::smoothInRegions() 217 | { 218 | 219 | // Allocate space for the smooth clouds 220 | for (unsigned int i = 0; i < meshes_.size(); i++) 221 | { 222 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 223 | smoothClouds_.push_back(cloud); 224 | } 225 | 226 | // First find the mesh interval 227 | for (unsigned int r = 0; r < regions_.size(); r++) 228 | { 229 | // We do two passes: 230 | // KF -> Rinit 231 | // KF -> Rend 232 | unsigned int first = regions_[r].first; 233 | unsigned int last = regions_[r].second; 234 | 235 | std::cerr << first << "/" << last << std::endl; 236 | 237 | // We iterate through the meshes in this specific range 238 | pcl::PointCloud::Ptr keycloud = clouds_[first]; 239 | unsigned int nvtx = keycloud->height * keycloud->width; 240 | 241 | // Store the keycloud in the smoothclouds vector, as this one does not change 242 | smoothClouds_[first] = keycloud; 243 | 244 | // This vector will store all equivalent vertices in the mesh sequence 245 | std::vector vertexseq(last - first + 1); 246 | std::vector smoothvertexseq(last - first + 1); 247 | 248 | for (unsigned int i = 0; i < nvtx; i++) 249 | { 250 | 251 | unsigned int k = 0; 252 | for (unsigned int j = first; j <= last; j++, k++) 253 | { 254 | 255 | const pcl::PointCloud::Ptr &cloud = clouds_[j]; 256 | const pcl::PointXYZRGBNormal &point = cloud->points[i]; 257 | vertexseq[k] = point; 258 | 259 | } // end per cloud 260 | 261 | // vertexseq is already filled here 262 | // We use a 3D Kalman filter to smooth the positions 263 | this->filterKalman(vertexseq, smoothvertexseq); 264 | 265 | // Assign the new vertex to the whole sequence 266 | for (unsigned int j = first + 1; j <= last; j++) 267 | { 268 | pcl::PointCloud::Ptr &cloud = smoothClouds_[j]; 269 | cloud->push_back(smoothvertexseq[j - first]); 270 | 271 | } // end per cloud 272 | } // end per vertex 273 | } 274 | } 275 | 276 | void KalmanSmoother::filterKalman(const std::vector &_input, std::vector &_output) const 277 | { 278 | 279 | // Initial values 280 | 281 | double dt = 0.04; // time between measurements (1/FPS = 1/25) 282 | 283 | cv::KalmanFilter KF(9, 3, 0, CV_64F); 284 | 285 | double proc_nc_max = 1.0; 286 | double proc_nc_min = 1e-5; 287 | 288 | double max_v = DBL_MIN; 289 | double min_v = DBL_MAX; 290 | 291 | std::vector v_normd; 292 | 293 | // analyze displacement magnitude 294 | for (unsigned int i = 1; i < _input.size(); i++) 295 | { 296 | double v = 0; 297 | const pcl::PointXYZRGBNormal &pclinitial = _input[i]; 298 | const pcl::PointXYZRGBNormal &pclprev = _input[i-1]; 299 | 300 | v += std::abs(double(pclinitial.x - pclprev.x)); 301 | v += std::abs(double(pclinitial.y - pclprev.y)); 302 | v += std::abs(double(pclinitial.z - pclprev.z)); 303 | 304 | max_v = v > max_v ? v : max_v; 305 | min_v = v < min_v ? v : min_v; 306 | 307 | v_normd.push_back(v); 308 | } 309 | 310 | // normalize displacements between [0, nc_max - nc_min] 311 | for (size_t i = 0; i < v_normd.size(); i++) 312 | { 313 | v_normd[i] = ((v_normd[i] - min_v)*(proc_nc_max-proc_nc_min))/(max_v-min_v); 314 | } 315 | // zero pad the front 316 | v_normd.insert(v_normd.begin(), 0); 317 | 318 | cv::setIdentity(KF.measurementNoiseCov, cv::Scalar::all(1e-3)); // set measurement noise 319 | 320 | cv::setIdentity(KF.errorCovPost, cv::Scalar::all(1)); // error covariance 321 | 322 | KF.transitionMatrix.at(0, 3) = dt; 323 | KF.transitionMatrix.at(1, 4) = dt; 324 | KF.transitionMatrix.at(2, 5) = dt; 325 | KF.transitionMatrix.at(3, 6) = dt; 326 | KF.transitionMatrix.at(4, 7) = dt; 327 | KF.transitionMatrix.at(5, 8) = dt; 328 | KF.transitionMatrix.at(0, 6) = 0.5 * dt * dt; 329 | KF.transitionMatrix.at(1, 7) = 0.5 * dt * dt; 330 | KF.transitionMatrix.at(2, 8) = 0.5 * dt * dt; 331 | 332 | KF.measurementMatrix.at(0, 0) = 1; // x 333 | KF.measurementMatrix.at(1, 1) = 1; // y 334 | KF.measurementMatrix.at(2, 2) = 1; // z 335 | 336 | for (unsigned int i = 0; i < _input.size(); i++) 337 | { 338 | 339 | 340 | cv::setIdentity(KF.processNoiseCov, cv::Scalar::all(proc_nc_min + v_normd[i])); // set process noise 341 | 342 | cv::Mat measurement = cv::Mat::zeros(3, 1, CV_64F); 343 | 344 | const pcl::PointXYZRGBNormal &pclinitial = _input[i]; 345 | cv::Point3f initial(pclinitial.x, pclinitial.y, pclinitial.z); 346 | 347 | measurement.at(0) = initial.x; // Initial X observation 348 | measurement.at(1) = initial.y; // Initial Y observation 349 | measurement.at(2) = initial.z; // Initial Z observation 350 | 351 | // First predict, to update the internal statePre variable 352 | cv::Mat prediction = KF.predict(); 353 | // The "correct" phase that is going to use the predicted value and our measurement 354 | cv::Mat estimated = KF.correct(measurement); 355 | // Estimated translation 356 | cv::Mat finalE(3, 1, CV_64F); 357 | finalE.at(0) = estimated.at(0); 358 | finalE.at(1) = estimated.at(1); 359 | finalE.at(2) = estimated.at(2); 360 | 361 | pcl::PointXYZRGBNormal finalP; 362 | finalP.x = finalE.at(0); 363 | finalP.y = finalE.at(1); 364 | finalP.z = finalE.at(2); 365 | 366 | _output[i] = finalP; 367 | } 368 | } 369 | 370 | std::vector KalmanSmoother::setupSmoothMeshes(bool _export) const 371 | { 372 | 373 | std::vector smoothmeshes; 374 | 375 | for (unsigned int i = 0; i < meshes_.size(); i++) 376 | { 377 | 378 | pcl::PCLPointCloud2 nucloud; 379 | pcl::toPCLPointCloud2(*smoothClouds_[i], nucloud); 380 | 381 | pcl::PolygonMesh mesh; 382 | 383 | mesh.cloud = nucloud; 384 | mesh.polygons = meshes_[i].polygons; 385 | 386 | if (_export) 387 | { 388 | std::string outname = meshNames_[i].substr(0, meshNames_[i].size() - 4); 389 | pcl::io::saveOBJFile(outname + "_s.obj", mesh); 390 | } 391 | 392 | smoothmeshes.push_back(mesh); 393 | } 394 | 395 | return smoothmeshes; 396 | } 397 | --------------------------------------------------------------------------------