├── test ├── data │ ├── RasterizationTest_DrawShadedTriangle1.png │ ├── RasterizationTest_DrawShadedTriangle2.png │ ├── RasterizationTest_DrawShadedTriangleTop.png │ ├── RasterizationTest_DrawLineInterpolatedTest.png │ ├── RasterizationTest_DrawShadedTriangleBottom.png │ ├── RasterizationTest_DrawTopFlatShadedTriangle.png │ ├── RasterizationTest_DrawBottomFlatShadedTriangle.png │ ├── RasterizationTest_DrawShadedTriangleOutOfOrder1.png │ ├── RasterizationTest_DrawShadedTriangleOutOfOrder2.png │ ├── RasterizationTest_DrawShadedTriangleBarycentric1.png │ └── RasterizationTest_DrawShadedTriangleBarycentric2.png ├── CMakeLists.txt └── utils │ ├── load_tracker_test.cc │ ├── visualization_test.cc │ └── stats_tracker_test.cc ├── cmake ├── templates │ ├── flameConfigVersion.cmake.in │ └── flameConfig.cmake.in ├── pkg_config.cmake └── setup.cmake ├── scripts ├── env.sh ├── sophus.sh └── eigen.sh ├── .gitignore ├── .circleci └── config.yml ├── src ├── CMakeLists.txt └── flame │ ├── utils │ ├── triangulator.h │ ├── frame.h │ ├── pyramids.cc │ ├── delaunay.h │ ├── frame.cc │ ├── keyframe_selector.h │ ├── pyramids.h │ ├── delaunay.cc │ ├── assert.h │ ├── stats_tracker.h │ ├── rasterization.h │ ├── rasterization.cc │ ├── keyframe_selector.cc │ ├── visualization.h │ ├── load_tracker.h │ └── image_utils.cc │ ├── stereo │ ├── inverse_depth_filter.h │ ├── inverse_depth_meas_model.h │ ├── inverse_depth_meas_model.cc │ ├── inverse_depth_filter.cc │ ├── line_stereo.h │ └── epipolar_geometry.h │ ├── types.h │ ├── params.h │ └── optimizers │ ├── nltgv2_l1_graph_regularizer.cc │ └── nltgv2_l1_graph_regularizer.h ├── package.xml ├── README.md └── CMakeLists.txt /test/data/RasterizationTest_DrawShadedTriangle1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robustrobotics/flame/HEAD/test/data/RasterizationTest_DrawShadedTriangle1.png -------------------------------------------------------------------------------- /test/data/RasterizationTest_DrawShadedTriangle2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robustrobotics/flame/HEAD/test/data/RasterizationTest_DrawShadedTriangle2.png -------------------------------------------------------------------------------- /test/data/RasterizationTest_DrawShadedTriangleTop.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robustrobotics/flame/HEAD/test/data/RasterizationTest_DrawShadedTriangleTop.png -------------------------------------------------------------------------------- /test/data/RasterizationTest_DrawLineInterpolatedTest.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robustrobotics/flame/HEAD/test/data/RasterizationTest_DrawLineInterpolatedTest.png -------------------------------------------------------------------------------- /test/data/RasterizationTest_DrawShadedTriangleBottom.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robustrobotics/flame/HEAD/test/data/RasterizationTest_DrawShadedTriangleBottom.png -------------------------------------------------------------------------------- /test/data/RasterizationTest_DrawTopFlatShadedTriangle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robustrobotics/flame/HEAD/test/data/RasterizationTest_DrawTopFlatShadedTriangle.png -------------------------------------------------------------------------------- /test/data/RasterizationTest_DrawBottomFlatShadedTriangle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robustrobotics/flame/HEAD/test/data/RasterizationTest_DrawBottomFlatShadedTriangle.png -------------------------------------------------------------------------------- /test/data/RasterizationTest_DrawShadedTriangleOutOfOrder1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robustrobotics/flame/HEAD/test/data/RasterizationTest_DrawShadedTriangleOutOfOrder1.png -------------------------------------------------------------------------------- /test/data/RasterizationTest_DrawShadedTriangleOutOfOrder2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robustrobotics/flame/HEAD/test/data/RasterizationTest_DrawShadedTriangleOutOfOrder2.png -------------------------------------------------------------------------------- /test/data/RasterizationTest_DrawShadedTriangleBarycentric1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robustrobotics/flame/HEAD/test/data/RasterizationTest_DrawShadedTriangleBarycentric1.png -------------------------------------------------------------------------------- /test/data/RasterizationTest_DrawShadedTriangleBarycentric2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robustrobotics/flame/HEAD/test/data/RasterizationTest_DrawShadedTriangleBarycentric2.png -------------------------------------------------------------------------------- /cmake/templates/flameConfigVersion.cmake.in: -------------------------------------------------------------------------------- 1 | set(PACKAGE_VERSION @@PACKAGE_NAME@_VERSION@) 2 | 3 | set(PACKAGE_VERSION_EXACT False) 4 | set(PACKAGE_VERSION_COMPATIBLE False) 5 | 6 | if(PACKAGE_FIND_VERSION VERSION_EQUAL PACKAGE_VERSION) 7 | set(PACKAGE_VERSION_EXACT True) 8 | set(PACKAGE_VERSION_COMPATIBLE True) 9 | endif() 10 | 11 | if(PACKAGE_FIND_VERSION VERSION_LESS PACKAGE_VERSION) 12 | set(PACKAGE_VERSION_COMPATIBLE True) 13 | endif() 14 | -------------------------------------------------------------------------------- /scripts/env.sh: -------------------------------------------------------------------------------- 1 | # WARNING: AUTOMATICALLY GENERATED FILE! DO NOT MODIFY! 2 | # This file appends the current directory to the user's environment variables. 3 | 4 | if [ -n "$ZSH_VERSION" ]; then 5 | WS="$( cd "$( dirname "$0" )" && pwd )" 6 | else 7 | WS="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" 8 | fi 9 | export PATH=${WS}/bin:${PATH} 10 | export LD_LIBRARY_PATH=${WS}/lib:${LD_LIBRARY_PATH} 11 | export PKG_CONFIG_PATH=${WS}/lib/pkgconfig:${WS}/share/pkgconfig:${PKG_CONFIG_PATH} 12 | export PYTHONPATH=${WS}/lib/python2.7/dist-packages:${WS}/lib/python2.7/site-packages:${PYTHONPATH} 13 | export CLASSPATH=${WS}/share/java:${CLASSPATH} 14 | export CMAKE_PREFIX_PATH=${WS}:${CMAKE_PREFIX_PATH} 15 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | enable_testing() 2 | 3 | file(GLOB TEST_SOURCES "test/*.cc") 4 | 5 | set(TEST_SOURCES 6 | stereo/epipolar_geometry_test.cc 7 | utils/image_utils_test.cc 8 | utils/load_tracker_test.cc 9 | utils/rasterization_test.cc 10 | utils/stats_tracker_test.cc 11 | utils/visualization_test.cc) 12 | 13 | add_executable(${PROJECT_NAME}_test ${TEST_SOURCES} ./gtest/gtest-all.cc) 14 | target_link_libraries(${PROJECT_NAME}_test pthread ${PROJECT_NAME}) 15 | set_target_properties(${PROJECT_NAME}_test PROPERTIES COMPILE_FLAGS "-std=c++11 -Wno-deprecated-register") 16 | 17 | add_test(NAME ${PROJECT_NAME}_test COMMAND ${PROJECT_NAME}_test --gtest_color=yes) 18 | 19 | # Copy test data. 20 | file(COPY ./data DESTINATION ${CMAKE_BINARY_DIR}) 21 | -------------------------------------------------------------------------------- /scripts/sophus.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # This script will checkout, build, and install Sophus. 4 | # 5 | # Usage: 6 | # 7 | # >> ./sophus.sh 8 | 9 | # Terminate if any line returns non-zero exit code. Beware of gotchas: 10 | # http://mywiki.wooledge.org/BashFAQ/105 11 | set -e 12 | 13 | # Save current working directory. 14 | WD=${PWD} 15 | 16 | # Number of jobs to launch. 17 | NUM_JOBS=$((`getconf _NPROCESSORS_ONLN` - 1)) 18 | 19 | rm -rf ${1}/Sophus 20 | git clone https://github.com/stevenlovegrove/Sophus.git ${1}/Sophus 21 | cd ${1}/Sophus 22 | 23 | git checkout b474f05f839c0f63c281aa4e7ece03145729a2cd 24 | 25 | mkdir build 26 | cd build 27 | 28 | cmake -DCMAKE_INSTALL_PREFIX=${2} .. 29 | make install -j ${NUM_JOBS} 30 | 31 | cd ${WD} 32 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | id_rsa 2 | cpplint.txt 3 | cppcheck.xml 4 | 5 | # Compiled Object files 6 | *.slo 7 | *.lo 8 | *.o 9 | *.obj 10 | 11 | # Precompiled Headers 12 | *.gch 13 | *.pch 14 | 15 | # Compiled Dynamic libraries 16 | *.so 17 | *.dylib 18 | *.dll 19 | 20 | # Fortran module files 21 | *.mod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | # Temporary files. 35 | *~ 36 | 37 | # Build artifacts. 38 | build 39 | devel 40 | install 41 | logs 42 | dependencies 43 | .catkin_workspace 44 | .catkin_tools 45 | src/drivers/realsense 46 | 47 | # lcmgen puts auto-generated messages source files here. 48 | src/utilities/common_lcmtypes/src 49 | 50 | # eclipse settings 51 | *.cproject 52 | *.project 53 | *.settings 54 | -------------------------------------------------------------------------------- /scripts/eigen.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # This script will checkout, build, and install Eigen. 4 | # 5 | # Usage: 6 | # 7 | # >> ./eigen.sh 8 | 9 | # Terminate if any line returns non-zero exit code. Beware of gotchas: 10 | # http://mywiki.wooledge.org/BashFAQ/105 11 | set -e 12 | 13 | # Save current working directory. 14 | WD=${PWD} 15 | 16 | # Number of jobs to launch. 17 | NUM_JOBS=$((`getconf _NPROCESSORS_ONLN` - 1)) 18 | 19 | # Install from source: 20 | # Create pkgconfig folder, otherwise eigen will not install its *.pc file. 21 | mkdir -p ${2}/lib/pkgconfig 22 | 23 | rm -rf ${1}/eigen 24 | hg clone https://bitbucket.org/eigen/eigen ${1}/eigen 25 | cd ${1}/eigen 26 | hg checkout 3.2.10 27 | 28 | mkdir build 29 | cd build 30 | 31 | cmake -DCMAKE_INSTALL_PREFIX=${2} .. 32 | make install -j ${NUM_JOBS} 33 | 34 | cd ${WD} 35 | -------------------------------------------------------------------------------- /cmake/templates/flameConfig.cmake.in: -------------------------------------------------------------------------------- 1 | # Config file for the module example 2 | # It defines the following variables: 3 | # ${PROJECT_NAME}_INCLUDE_DIR - Location of header files 4 | # ${PROJECT_NAME}_INCLUDE_DIRS - All include directories needed to use ${PROJECT_NAME} 5 | # ${PROJECT_NAME}_LIBRARY - ${PROJECT_NAME} library 6 | # ${PROJECT_NAME}_LIBRARIES - ${PROJECT_NAME} library and all dependent libraries 7 | # ${PROJECT_NAME}_DEFINITIONS - Compiler definitions as semicolon separated list 8 | find_library(@PROJECT_NAME@_LIBRARY flame 9 | PATHS @CMAKE_INSTALL_PREFIX@/lib 10 | NO_DEFAULT_PATH 11 | ) 12 | set(@PROJECT_NAME@_LIBRARIES 13 | ${@PROJECT_NAME@_LIBRARY} 14 | ${OpenCV_LIBS} 15 | ${Boost_LIBRARIES}) 16 | 17 | find_path(@PROJECT_NAME@_INCLUDE_DIR flame/utils/assert.h 18 | PATHS @CMAKE_INSTALL_PREFIX@/include 19 | NO_DEFAULT_PATH 20 | ) 21 | 22 | set(@PROJECT_NAME@_INCLUDE_DIRS 23 | ${@PROJECT_NAME@_INCLUDE_DIR} 24 | ${OpenCV_INCLUDE_DIRS} 25 | ${Boost_INCLUDE_DIRS} 26 | ${EIGEN3_INCLUDE_DIRS} 27 | ${Sophus_INCLUDE_DIRS}) 28 | 29 | set(@PROJECT_NAME@_DEFINITIONS "-std=c++11") 30 | -------------------------------------------------------------------------------- /.circleci/config.yml: -------------------------------------------------------------------------------- 1 | # Configuration file for CircleCI 2.0. 2 | version: 2 3 | jobs: 4 | # Main build job. 5 | build: 6 | docker: 7 | # Only need kinetic image so that we can easily use OpenCV 3.3. 8 | - image: osrf/ros:kinetic-desktop-full 9 | steps: 10 | # Commands to checkout/build repo and run unit tests. 11 | - checkout 12 | - run: apt-get install libboost-all-dev 13 | - run: mkdir -p ./dependencies/src 14 | - run: ./scripts/eigen.sh ./dependencies/src ./dependencies 15 | - run: ./scripts/sophus.sh ./dependencies/src ./dependencies 16 | - run: cp ./scripts/env.sh ./dependencies 17 | - run: mkdir ./build && mkdir ./install 18 | - run: source /opt/ros/kinetic/setup.sh && source ./dependencies/env.sh && cd ./build && cmake -D CMAKE_INSTALL_PREFIX=../install .. 19 | - run: make -C build install 20 | - run: ./build/bin/flame_test 21 | 22 | # Two main workflows: One run per commit, one to run master nightly. 23 | workflows: 24 | version: 2 25 | commit-workflow: 26 | jobs: 27 | - build 28 | nightly-workflow: 29 | triggers: 30 | - schedule: 31 | cron: "0 1 * * *" 32 | filters: 33 | branches: 34 | only: master 35 | jobs: 36 | - build 37 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Build main library. 2 | set(FLAME_SRCS 3 | flame/flame.h 4 | flame/flame.cc 5 | 6 | flame/params.h 7 | flame/types.h 8 | 9 | flame/external/triangle/triangle.h 10 | flame/external/triangle/triangle.cpp 11 | 12 | flame/optimizers/nltgv2_l1_graph_regularizer.h 13 | flame/optimizers/nltgv2_l1_graph_regularizer.cc 14 | 15 | flame/stereo/epipolar_geometry.h 16 | flame/stereo/inverse_depth_filter.h 17 | flame/stereo/inverse_depth_filter.cc 18 | flame/stereo/inverse_depth_meas_model.h 19 | flame/stereo/inverse_depth_meas_model.cc 20 | flame/stereo/line_stereo.h 21 | 22 | flame/utils/assert.h 23 | flame/utils/delaunay.h 24 | flame/utils/delaunay.cc 25 | flame/utils/frame.h 26 | flame/utils/frame.cc 27 | flame/utils/image_utils.h 28 | flame/utils/image_utils.cc 29 | flame/utils/keyframe_selector.h 30 | flame/utils/keyframe_selector.cc 31 | flame/utils/load_tracker.h 32 | flame/utils/pyramids.h 33 | flame/utils/pyramids.cc 34 | flame/utils/rasterization.h 35 | flame/utils/rasterization.cc 36 | flame/utils/stats_tracker.h 37 | flame/utils/triangulator.h 38 | flame/utils/visualization.h) 39 | 40 | add_library(${PROJECT_NAME} SHARED ${FLAME_SRCS}) 41 | 42 | target_link_libraries(${PROJECT_NAME} 43 | ${OpenCV_LIBS} 44 | ${Boost_LIBRARIES}) 45 | 46 | set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) 47 | 48 | install(TARGETS ${PROJECT_NAME} 49 | ARCHIVE DESTINATION lib 50 | LIBRARY DESTINATION lib 51 | RUNTIME DESTINATION bin) 52 | 53 | install(DIRECTORY ${PROJECT_NAME} DESTINATION include 54 | FILES_MATCHING PATTERN "*.h") 55 | -------------------------------------------------------------------------------- /src/flame/utils/triangulator.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file triangulator.h 19 | * @author W. Nicholas Greene 20 | * @date 2017-08-18 18:57:10 (Fri) 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | #include 28 | 29 | namespace flame { 30 | 31 | using Vertex = cv::Point2f; 32 | using Triangle = cv::Vec3i; 33 | using Edge = cv::Vec2i; 34 | 35 | namespace utils { 36 | 37 | /** 38 | * \brief CRTP interface for 2D triangulation. 39 | */ 40 | template 41 | class Triangulator { 42 | public: 43 | /** 44 | * \brief Triangulate a set of 2D vertices. 45 | * 46 | * @param[in] vertices 2D vertices to triangulate. 47 | * @param[in] triangles Output triangles. 48 | */ 49 | void triangulate(const std::vector& vertices, 50 | std::vector* triangles) { 51 | static_cast(this)->triangulate(vertices, triangles); 52 | } 53 | }; 54 | 55 | } // namespace utils 56 | 57 | } // namespace flame 58 | -------------------------------------------------------------------------------- /cmake/pkg_config.cmake: -------------------------------------------------------------------------------- 1 | function(install_pkg_config_file) 2 | list(GET ARGV 0 pc_name) 3 | # TODO error check 4 | 5 | set(pc_version 0.0.1) 6 | set(pc_description ${pc_name}) 7 | set(pc_requires "") 8 | set(pc_libs "") 9 | set(pc_cflags "") 10 | set(pc_fname "${PKG_CONFIG_OUTPUT_PATH}/${pc_name}.pc") 11 | 12 | set(modewords LIBS CFLAGS REQUIRES VERSION DESCRIPTION) 13 | set(curmode "") 14 | 15 | # parse function arguments and populate pkg-config parameters 16 | list(REMOVE_AT ARGV 0) 17 | foreach(word ${ARGV}) 18 | list(FIND modewords ${word} mode_index) 19 | if(${mode_index} GREATER -1) 20 | set(curmode ${word}) 21 | elseif(curmode STREQUAL LIBS) 22 | set(pc_libs "${pc_libs} ${word}") 23 | elseif(curmode STREQUAL CFLAGS) 24 | set(pc_cflags "${pc_cflags} ${word}") 25 | elseif(curmode STREQUAL REQUIRES) 26 | set(pc_requires "${pc_requires} ${word}") 27 | elseif(curmode STREQUAL VERSION) 28 | set(pc_version ${word}) 29 | set(curmode "") 30 | elseif(curmode STREQUAL DESCRIPTION) 31 | set(pc_description "${word}") 32 | set(curmode "") 33 | else(${mode_index} GREATER -1) 34 | message("WARNING incorrect use of pods_add_pkg_config (${word})") 35 | break() 36 | endif(${mode_index} GREATER -1) 37 | endforeach(word) 38 | 39 | # write the .pc file out 40 | file(WRITE ${pc_fname} 41 | "prefix=${CMAKE_INSTALL_PREFIX}\n" 42 | "exec_prefix=\${prefix}\n" 43 | "libdir=\${exec_prefix}/lib\n" 44 | "includedir=\${prefix}/include\n" 45 | "\n" 46 | "Name: ${pc_name}\n" 47 | "Description: ${pc_description}\n" 48 | "Requires: ${pc_requires}\n" 49 | "Version: ${pc_version}\n" 50 | "Libs: -L\${libdir} ${pc_libs}\n" 51 | "Cflags: -I\${includedir} ${pc_cflags}\n") 52 | 53 | # mark the .pc file for installation to the lib/pkgconfig directory 54 | install(FILES ${pc_fname} DESTINATION lib/pkgconfig) 55 | 56 | endfunction(install_pkg_config_file) 57 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | flame 4 | 1.0.0 5 | A lightweight method for dense online monocular depth estimation. 6 | 7 | 8 | 9 | 10 | W. Nicholas Greene 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | 20 | 21 | 22 | https://github.com/robustrobotics/flame 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | cmake 43 | 44 | boost 45 | boost 46 | 47 | 48 | 49 | 50 | cmake 51 | 52 | 53 | -------------------------------------------------------------------------------- /test/utils/load_tracker_test.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file load_tracker_test.cc 19 | * @author W. Nicholas Greene 20 | * @date 2017-02-04 19:56:23 (Sat) 21 | */ 22 | 23 | #include "flame/utils/load_tracker.h" 24 | 25 | #include "gtest/gtest.h" 26 | 27 | namespace flame { 28 | 29 | namespace utils { 30 | 31 | /** 32 | * @brief Query load periodically while sleeping. 33 | * 34 | * Not sure how to effectively test. What I did during development is pick a PID 35 | * and compare output with htop. 36 | */ 37 | TEST(LoadTrackerTest, IdleTest) { 38 | LoadTracker load_tracker(getpid()); 39 | 40 | int dt_ms = 500; 41 | int num_iters = 5; 42 | for (int ii = 0; ii < num_iters; ++ii) { 43 | Load max_load, sys_load, pid_load; 44 | 45 | load_tracker.get(&max_load, &sys_load, &pid_load); 46 | 47 | printf("max_load.cpu = %f\n", max_load.cpu); 48 | printf("max_load.mem = %lu\n", max_load.mem); 49 | printf("max_load.swap = %lu\n", max_load.swap); 50 | 51 | printf("sys_load.cpu = %f\n", sys_load.cpu); 52 | printf("sys_load.mem = %lu\n", sys_load.mem); 53 | printf("sys_load.swap = %lu\n", sys_load.swap); 54 | 55 | printf("pid_load.cpu = %f\n", pid_load.cpu); 56 | printf("pid_load.mem = %lu\n", pid_load.mem); 57 | printf("pid_load.swap = %lu\n", pid_load.swap); 58 | 59 | std::this_thread::sleep_for(std::chrono::milliseconds(dt_ms)); 60 | } 61 | 62 | return; 63 | } 64 | 65 | } // namespace utils 66 | 67 | } // namespace flame 68 | -------------------------------------------------------------------------------- /src/flame/utils/frame.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file frame.h 19 | * @author W. Nicholas Greene 20 | * @date 2017-03-30 19:35:44 (Thu) 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | #include "flame/types.h" 29 | #include "flame/utils/triangulator.h" 30 | 31 | namespace flame { 32 | 33 | namespace utils { 34 | 35 | /** 36 | * @brief Struct to hold various image data. 37 | */ 38 | struct Frame { 39 | using Ptr = std::shared_ptr; 40 | using ConstPtr = std::shared_ptr; 41 | 42 | explicit Frame(int num_lvls) : 43 | id(0), pose(), 44 | img(num_lvls), gradx(num_lvls), grady(num_lvls), 45 | img_pad(num_lvls), gradx_pad(num_lvls), grady_pad(num_lvls) {} 46 | 47 | // Create a frame pointer object from a pose and raw image. 48 | static Frame::Ptr create(const Sophus::SE3f& pose, const cv::Mat1b& img, 49 | int id, int num_levels, int border); 50 | 51 | uint32_t id; // Image number/ID. 52 | SE3f pose; // Pose of this image. 53 | ImagePyramidb img; // Image pyramid. 54 | ImagePyramidf gradx; // Horizontal gradient pyramid. 55 | ImagePyramidf grady; // Vertical gradient pyramid. 56 | ImagePyramidb img_pad; // Padded pyramids for LK. 57 | ImagePyramidf gradx_pad; 58 | ImagePyramidf grady_pad; 59 | ImagePyramidf idepthmap; // Dense inverse depthmap. 60 | 61 | // Mesh stuff. 62 | std::vector tris; 63 | std::vector edges; 64 | std::vector vtx; 65 | std::vector vtx_idepths; 66 | std::vector vtx_w1; 67 | std::vector vtx_w2; 68 | }; 69 | 70 | } // namespace utils 71 | 72 | } // namespace flame 73 | -------------------------------------------------------------------------------- /src/flame/utils/pyramids.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file pyramids.cc 19 | * @author W. Nicholas Greene 20 | * @date 2017-08-18 20:09:30 (Fri) 21 | */ 22 | 23 | #include "flame/utils/pyramids.h" 24 | 25 | namespace flame { 26 | 27 | namespace utils { 28 | 29 | void getPyramidDisplay(const ImagePyramid& pyr, cv::Mat* out) { 30 | int rows = pyr[0].rows; 31 | int cols = pyr[0].cols; 32 | 33 | if (out->empty()) { 34 | out->create(rows, cols*1.5, pyr[0].type()); 35 | (*out) = cv::Scalar(0); 36 | } 37 | 38 | cv::Rect curr_roi(0, 0, cols, rows); 39 | for (int ii = 0; ii < pyr.size(); ii++) { 40 | pyr[ii].copyTo((*out)(curr_roi)); 41 | 42 | if (ii % 4 == 0) { 43 | /* Place new image on the right/top. */ 44 | curr_roi.x += pyr[ii].cols; 45 | curr_roi.width = pyr[ii].cols/2; 46 | curr_roi.height = pyr[ii].rows/2; 47 | } else if (ii % 4 == 1) { 48 | /* Place new image on the bottom/right. */ 49 | curr_roi.x += pyr[ii].cols/2; 50 | curr_roi.y += pyr[ii].rows; 51 | curr_roi.width = pyr[ii].cols/2; 52 | curr_roi.height = pyr[ii].rows/2; 53 | 54 | } else if (ii % 4 == 2) { 55 | /* Place new image on the left/bottom. */ 56 | curr_roi.x -= pyr[ii].cols/2; 57 | curr_roi.y += pyr[ii].rows/2; 58 | curr_roi.width = pyr[ii].cols/2; 59 | curr_roi.height = pyr[ii].rows/2; 60 | 61 | } else if (ii % 4 == 3) { 62 | /* Place new image on the top/left. */ 63 | curr_roi.y -= pyr[ii].rows/2; 64 | curr_roi.width = pyr[ii].cols/2; 65 | curr_roi.height = pyr[ii].rows/2; 66 | } 67 | } 68 | 69 | return; 70 | } 71 | 72 | } // namespace utils 73 | 74 | } // namespace flame 75 | -------------------------------------------------------------------------------- /src/flame/utils/delaunay.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file delaunay.h 19 | * @author W. Nicholas Greene 20 | * @date 2017-08-18 20:33:42 (Fri) 21 | */ 22 | 23 | // Adapted from cv_utils.cpp from fast-stereo (Sudeep Pillai). 24 | 25 | #pragma once 26 | 27 | #include 28 | 29 | // Delaunay triangulation 30 | #include 31 | 32 | #include 33 | 34 | namespace flame { 35 | 36 | namespace utils { 37 | 38 | /** 39 | * \brief Class that implements Delaunay triangulation. 40 | */ 41 | class Delaunay final : public Triangulator { 42 | public: 43 | Delaunay() = default; 44 | ~Delaunay() = default; 45 | 46 | Delaunay(const Delaunay& rhs) = delete; 47 | Delaunay& operator=(const Delaunay& rhs) = delete; 48 | 49 | Delaunay(Delaunay&& rhs) = default; 50 | Delaunay& operator=(Delaunay&& rhs) = default; 51 | 52 | /** 53 | * \brief Triangulate a set of 2D vertices. 54 | * 55 | * @param[in] vertices 2D vertices to triangulate. 56 | * @param[in] triangles Output triangles. 57 | */ 58 | void triangulate(const std::vector& vertices, 59 | std::vector* triangles); 60 | 61 | /** 62 | * @brief Triangulate a set of 2D vertices. 63 | * 64 | * @param[in] vertices 2D vertices to triangulate. 65 | */ 66 | void triangulate(const std::vector& vertices); 67 | 68 | // Accessors. 69 | const std::vector& triangles() const { return triangles_; } 70 | const std::vector& edges() const { return edges_; } 71 | const std::vector& neighbors() const { return neighbors_; } 72 | 73 | private: 74 | void cleanup(); 75 | void getTriangles(std::vector* triangles); 76 | void getNeighbors(); 77 | void getEdges(); 78 | 79 | struct triangulateio out_; 80 | 81 | std::vector triangles_; 82 | std::vector neighbors_; 83 | std::vector edges_; 84 | }; 85 | 86 | } // namespace utils 87 | 88 | } // namespace flame 89 | -------------------------------------------------------------------------------- /src/flame/utils/frame.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file frame.cc 19 | * @author W. Nicholas Greene 20 | * @date 2017-03-30 19:37:04 (Thu) 21 | */ 22 | 23 | #include "flame/utils/frame.h" 24 | 25 | #include 26 | 27 | #include "flame/utils/pyramids.h" 28 | 29 | namespace flame { 30 | 31 | namespace utils { 32 | 33 | Frame::Ptr Frame::create(const SE3f& pose, const Image1b& img, 34 | int id, int num_levels, int border) { 35 | Frame::Ptr frame = std::make_shared(num_levels); 36 | 37 | frame->id = id; 38 | frame->pose = pose; 39 | 40 | // Create image pyramid. 41 | ImagePyramid img_pyr_tmp(utils::getGaussianPyramid(img, num_levels)); 42 | // Need to cast down pyramid. 43 | frame->img = ImagePyramidb(img_pyr_tmp.begin(), img_pyr_tmp.end()); 44 | 45 | // Create gradient pyramid. 46 | utils::getGradientPyramid(frame->img, &(frame->gradx), &(frame->grady)); 47 | 48 | frame->idepthmap.resize(num_levels); 49 | 50 | for (int lvl = 0; lvl < num_levels; ++lvl) { 51 | // Create padded versions. 52 | int rlvl = frame->img[lvl].rows; 53 | int clvl = frame->img[lvl].cols; 54 | frame->img_pad[lvl].create(rlvl + 2 * border, clvl + 2 * border); 55 | frame->gradx_pad[lvl].create(rlvl + 2 * border, clvl + 2 * border); 56 | frame->grady_pad[lvl].create(rlvl + 2 * border, clvl + 2 * border); 57 | 58 | cv::copyMakeBorder(frame->img[lvl], frame->img_pad[lvl], 59 | border, border, border, border, cv::BORDER_REFLECT_101); 60 | cv::copyMakeBorder(frame->gradx[lvl], frame->gradx_pad[lvl], 61 | border, border, border, border, cv::BORDER_CONSTANT); 62 | cv::copyMakeBorder(frame->grady[lvl], frame->grady_pad[lvl], 63 | border, border, border, border, cv::BORDER_CONSTANT); 64 | 65 | // Create idepthmap. 66 | frame->idepthmap[lvl] = Image1f(rlvl, clvl, 67 | std::numeric_limits::quiet_NaN()); 68 | } 69 | 70 | return frame; 71 | } 72 | 73 | } // namespace utils 74 | 75 | } // namespace flame 76 | -------------------------------------------------------------------------------- /cmake/setup.cmake: -------------------------------------------------------------------------------- 1 | #------------# 2 | # Set custom options 3 | #------------# 4 | if (CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT) 5 | message(STATUS "Installing to ${CMAKE_SOURCE_DIR}/install") 6 | message(WARNING "CMake defaults to installing to ${CMAKE_INSTALL_PREFIX}. This is probably not what you want, and so this build system installs locally, to ${CMAKE_SOURCE_DIR}/install. To override this behavior, set CMAKE_INSTALL_PREFIX manually.") 7 | set(CMAKE_INSTALL_PREFIX ${CMAKE_SOURCE_DIR}/install CACHE PATH "${PROJECT_NAME} install prefix" FORCE) 8 | else() 9 | message(STATUS "Installing to ${CMAKE_INSTALL_PREFIX}") 10 | endif (CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT) 11 | 12 | if (NOT CMAKE_BUILD_TYPE) 13 | set(CMAKE_BUILD_TYPE "Release" CACHE STRING 14 | "Choose the type of build: Debug|Release|RelWithDebInfo|MinSizeRel." FORCE) 15 | endif() 16 | 17 | set(CMAKE_EXPORT_COMPILE_COMMANDS 1) 18 | 19 | #------------# 20 | # Set up paths 21 | #------------# 22 | # set cmake module path 23 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_INSTALL_PREFIX}/lib/cmake) 24 | set(CMAKE_PREFIX_PATH ${CMAKE_MODULE_PATH} ${CMAKE_INSTALL_PREFIX}/lib/cmake) 25 | 26 | # set where files should be output locally 27 | set(LIBRARY_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib) 28 | set(EXECUTABLE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/bin) 29 | set(INCLUDE_OUTPUT_PATH ${CMAKE_BINARY_DIR}/include) 30 | set(PKG_CONFIG_OUTPUT_PATH ${CMAKE_BINARY_DIR}/lib/pkgconfig) 31 | 32 | # set where files should be installed to 33 | set(LIBRARY_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/lib) 34 | set(EXECUTABLE_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/bin) 35 | set(INCLUDE_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/include) 36 | set(PKG_CONFIG_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/lib/pkgconfig) 37 | 38 | # add build/lib/pkgconfig to the pkg-config search path 39 | # wrvb 2014-11-02: CMake shouldn't touch environment variables unless it has to 40 | # set(ENV{PKG_CONFIG_PATH} ${PKG_CONFIG_INSTALL_PATH}:$ENV{PKG_CONFIG_PATH}) 41 | # set(ENV{PKG_CONFIG_PATH} ${PKG_CONFIG_OUTPUT_PATH}:$ENV{PKG_CONFIG_PATH}) 42 | # set(ENV{PKG_CONFIG_PATH} ${CMAKE_PREFIX_PATH}/lib/pkgconfig:$ENV{PKG_CONFIG_PATH}) 43 | 44 | # add include directories to the compiler include path 45 | # include_directories(include) 46 | # include_directories(BEFORE ${INCLUDE_OUTPUT_PATH}) 47 | # include_directories(${INCLUDE_INSTALL_PATH}) 48 | 49 | # add build/lib to the link path 50 | link_directories(${LIBRARY_OUTPUT_PATH}) 51 | link_directories(${LIBRARY_INSTALL_PATH}) 52 | 53 | # abuse RPATH 54 | if(${CMAKE_INSTALL_RPATH}) 55 | set(CMAKE_INSTALL_RPATH ${LIBRARY_INSTALL_PATH}:${CMAKE_INSTALL_RPATH}) 56 | else(${CMAKE_INSTALL_RPATH}) 57 | set(CMAKE_INSTALL_RPATH ${LIBRARY_INSTALL_PATH}) 58 | endif(${CMAKE_INSTALL_RPATH}) 59 | 60 | # for osx, which uses "install name" path rather than rpath 61 | #set(CMAKE_INSTALL_NAME_DIR ${LIBRARY_OUTPUT_PATH}) 62 | set(CMAKE_INSTALL_NAME_DIR ${CMAKE_INSTALL_RPATH}) 63 | 64 | # hack to force cmake always create install and clean targets 65 | install(FILES DESTINATION) 66 | # wrvb 2014-11-02: I don't think these are necessary to generate a clean 67 | # target, the above line alone does it for me. 68 | # string(RANDOM LENGTH 32 __rand_target__name__) 69 | # add_custom_target(${__rand_target__name__}) 70 | # unset(__rand_target__name__) 71 | 72 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![CircleCI](https://circleci.com/gh/robustrobotics/flame/tree/master.svg?style=shield)](https://circleci.com/gh/robustrobotics/flame/tree/master) 2 | 3 | # flame 4 | **FLaME** (Fast Lightweight Mesh Estimation) is a lightweight, CPU-only method 5 | for dense online monocular depth estimation. Given a sequence of camera images 6 | with known poses, **FLaME** is able to reconstruct dense 3D meshes of the 7 | environment by posing the depth estimation problem as a variational optimization 8 | over a Delaunay graph that can be solved at framerate, even on computationally 9 | constrained platforms. 10 | 11 | The `flame` repository contains the source code for the core algorithm. It 12 | should be input/output agnostic, so feel free to write an appropriate frontend 13 | for your data. ROS bindings are available with the 14 | associated [flame_ros](https://github.com/robustrobotics/flame_ros.git) 15 | repository, which also includes examples for running `flame` on offline data. 16 | 17 |

18 | 19 | FLaME 20 | 21 |

22 | 23 | ### Related Publications: 24 | * [**FLaME: Fast Lightweight Mesh Estimation using Variational Smoothing on Delaunay Graphs**](https://groups.csail.mit.edu/rrg/papers/greene_iccv17.pdf), 25 | *W. Nicholas Greene and Nicholas Roy*, ICCV 2017. 26 | 27 | ## Author 28 | - W. Nicholas Greene (wng@csail.mit.edu) 29 | 30 | ## Dependencies 31 | - Ubuntu 16.04 32 | - Boost 1.58 33 | - OpenCV 3.2 34 | - Eigen 3.2 35 | - Sophus (SHA: b474f05f839c0f63c281aa4e7ece03145729a2cd) 36 | 37 | ## Installation 38 | **NOTE:** These instructions assume you are running Ubuntu 16.04 and are 39 | interested in installing `flame` only. See the installation instructions for 40 | `flame_ros` if you also wish to build the ROS bindings as the process can be 41 | streamlined using `catkin_tools`. 42 | 43 | 1. Install `apt` dependencies: 44 | ```bash 45 | sudo apt-get install libboost-all-dev 46 | ``` 47 | 48 | 2. Install OpenCV 3.2: 49 | 50 | Unfortunately OpenCV 3.2 is not available through `apt` on Ubuntu 16.04. If you 51 | have ROS Kinetic installed on your system, you can simply source your ROS 52 | installation as this version of OpenCV is packaged with ROS Kinetic. If you 53 | don't have ROS Kinetic installed, then you will need to install from 54 | source. Please consult 55 | the 56 | [OpenCV docs](http://docs.opencv.org/3.0-beta/doc/tutorials/introduction/linux_install/linux_install.html) for 57 | instructions. 58 | 59 | 3. Install Eigen 3.2 and Sophus using the provided scripts: 60 | ```bash 61 | cd flame 62 | 63 | # Create a dependencies folder. 64 | mkdir -p dependencies/src 65 | 66 | # Checkout Eigen and Sophus into ./dependencies/src and install into ./dependencies. 67 | ./scripts/eigen.sh ./dependencies/src ./dependencies 68 | ./scripts/sophus.sh ./dependencies/src ./dependencies 69 | 70 | # Copy and source environment variable script: 71 | cp ./scripts/env.sh ./dependencies/ 72 | source ./dependencies/env.sh 73 | ``` 74 | 75 | 4. Install `flame`: 76 | ```bash 77 | cd flame 78 | mkdir build 79 | cd build 80 | cmake -D CMAKE_INSTALL_PREFIX=path/to/install/directory .. 81 | make install 82 | ``` 83 | 84 | ## Usage 85 | See `flame_ros` for ROS bindings and example usage. 86 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | # Configure CCache if available 4 | find_program(CCACHE_FOUND ccache) 5 | if (CCACHE_FOUND) 6 | set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE ccache) 7 | set_property(GLOBAL PROPERTY RULE_LAUNCH_LINK ccache) 8 | endif (CCACHE_FOUND) 9 | 10 | project(flame) 11 | set(${PROJECT_NAME}_VERSION 1.0.0) 12 | 13 | # Options 14 | option(BUILD_DOCUMENTATION "Use Doxygen to create the HTML based API documentation" OFF) 15 | option(BUILD_TESTS "Use GTest to build and test libraries" ON) 16 | 17 | option(WITH_COVERAGE "Compile with code coverage" OFF) 18 | if (WITH_COVERAGE) 19 | set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -g -O0 -fprofile-arcs -ftest-coverage") 20 | set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -g -O0 -fprofile-arcs -ftest-coverage") 21 | set(CMAKE_EXE_LINKER_FLAGS_DEBUG "${CMAKE_EXE_LINKER_FLAGS_DEBUG} -fprofile-arcs -ftest-coverage") 22 | endif () 23 | 24 | add_definitions("-std=c++11") 25 | 26 | # Add package setup file. 27 | include(cmake/setup.cmake) 28 | 29 | # Locate external dependencies 30 | find_package(OpenCV REQUIRED COMPONENTS core highgui imgproc imgcodecs) 31 | find_package(Boost 1.54 REQUIRED COMPONENTS system filesystem graph) 32 | find_package(Sophus REQUIRED) 33 | 34 | find_package(PkgConfig REQUIRED) # For projects with pkg-config files. 35 | pkg_check_modules(EIGEN3 REQUIRED eigen3>=3.2) 36 | 37 | # Project includes 38 | include_directories(SYSTEM src 39 | ${OpenCV_INCLUDE_DIRS} 40 | ${Boost_INCLUDE_DIRS} 41 | ${EIGEN3_INCLUDE_DIRS} 42 | ${Sophus_INCLUDE_DIRS}) 43 | 44 | # Build the library 45 | add_subdirectory(src) 46 | 47 | # Configure the library 48 | # Configuration method 1: pkg_config 49 | include(cmake/pkg_config.cmake) 50 | install_pkg_config_file(${PROJECT_NAME} 51 | DESCRIPTION "A lightweight method for dense online monocular depth estimation." 52 | LIBS ${PROJECT_NAME} 53 | VERSION ${${PROJECT_NAME}_VERSION} 54 | REQUIRES opencv eigen3>=3.2 55 | CFLAGS "-std=c++11") 56 | 57 | # Configuration method 2: pure cmake, no import/export 58 | configure_file(${PROJECT_SOURCE_DIR}/cmake/templates/${PROJECT_NAME}Config.cmake.in 59 | "${PROJECT_BINARY_DIR}/lib/cmake/${PROJECT_NAME}/${PROJECT_NAME}Config.cmake" @ONLY) 60 | configure_file(${PROJECT_SOURCE_DIR}/cmake/templates/${PROJECT_NAME}ConfigVersion.cmake.in 61 | "${PROJECT_BINARY_DIR}/lib/cmake/${PROJECT_NAME}/${PROJECT_NAME}ConfigVersion.cmake" @ONLY) 62 | install(FILES 63 | ${PROJECT_BINARY_DIR}/lib/cmake/${PROJECT_NAME}/${PROJECT_NAME}Config.cmake 64 | ${PROJECT_BINARY_DIR}/lib/cmake/${PROJECT_NAME}/${PROJECT_NAME}ConfigVersion.cmake 65 | DESTINATION ${CMAKE_INSTALL_PREFIX}/lib/cmake/${PROJECT_NAME}) 66 | 67 | # If the BUILD_TESTS option is set, a Google test binary will be compiled. 68 | if (BUILD_TESTS) 69 | add_subdirectory(./test) 70 | endif() 71 | 72 | # Build the doxygen documentation. 73 | if (BUILD_DOCUMENTATION) 74 | find_package(Doxygen) 75 | 76 | if (NOT DOXYGEN_FOUND) 77 | message(FATAL_ERROR 78 | "Doxygen is needed to build the documentation.") 79 | endif() 80 | 81 | configure_file(${CMAKE_CURRENT_SOURCE_DIR}/doc/Doxyfile.in 82 | ${CMAKE_CURRENT_BINARY_DIR}/doc/Doxyfile @ONLY) 83 | add_custom_target(doc ALL 84 | ${DOXYGEN_EXECUTABLE} ${CMAKE_CURRENT_BINARY_DIR}/doc/Doxyfile 85 | WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/doc 86 | COMMENT "Generating API documentation with Doxygen" VERBATIM) 87 | 88 | install(DIRECTORY ${CMAKE_BINARY_DIR}/doc/html DESTINATION doc/${PROJECT_NAME}) 89 | install(DIRECTORY ${CMAKE_BINARY_DIR}/doc/latex DESTINATION doc/${PROJECT_NAME}) 90 | endif() 91 | 92 | -------------------------------------------------------------------------------- /src/flame/utils/keyframe_selector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file keyframe_selector.h 19 | * @author W. Nicholas Greene 20 | * @date 2017-08-18 20:35:30 (Fri) 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | #include "flame/types.h" 29 | 30 | namespace flame { 31 | 32 | namespace utils { 33 | 34 | /** 35 | * \brief Selects a comparison image from a pool of keyframes. 36 | */ 37 | class KeyFrameSelector final { 38 | public: 39 | /** 40 | * \brief Constructor. 41 | * 42 | * @param[in] max_keyframes Max num keyframes to maintain in pool. 43 | * @param[in] new_keyframe_thresh Threshold to create new keyframe. 44 | */ 45 | KeyFrameSelector(const Matrix3f& K, int max_kfs, float new_kf_thresh); 46 | 47 | ~KeyFrameSelector() = default; 48 | 49 | KeyFrameSelector() = delete; 50 | KeyFrameSelector(const KeyFrameSelector& rhs) = delete; 51 | KeyFrameSelector& operator=(const KeyFrameSelector& rhs) = delete; 52 | 53 | KeyFrameSelector(KeyFrameSelector&& rhs) = delete; 54 | KeyFrameSelector& operator=(KeyFrameSelector&& rhs) = delete; 55 | 56 | /** 57 | * \brief Select a keyframe index for this new image. 58 | * 59 | * Will add new image to keyframe pool if it exceeds threshold. 60 | * 61 | * If no suitable keyframe found (e.g. no keyframes in pool yet, not enough 62 | * baseline yet, etc.), will return -1. 63 | * 64 | * @return Index of keyframe in pool. 65 | */ 66 | int select(double new_time, const Image1b& new_img, 67 | const SE3f& new_pose); 68 | 69 | /** 70 | * \brief Get keyframe corresponding to idx. 71 | */ 72 | void getKeyFrame(int idx, double* time, Image1b* img, SE3f* pose); 73 | 74 | /** 75 | * \brief Return number of keyframes in pool. 76 | */ 77 | int size() { return num_kfs_; } 78 | 79 | /** 80 | * \brief Get the score associated with using ref_img as the keyframe. 81 | */ 82 | static float score(int width, int height, 83 | const Matrix3f& K, const Matrix3f& Kinv, 84 | const SE3f& new_to_ref, 85 | float min_depth = 1.0f, float max_depth = 10.0f, 86 | float max_disparity = 128.0f); 87 | 88 | private: 89 | // Store the keyframe pool in circular buffers. 90 | std::deque kf_times_; 91 | std::deque kf_imgs_; 92 | std::deque kf_poses_; 93 | 94 | // Camera matrix. 95 | Matrix3f K_; 96 | Matrix3f Kinv_; 97 | 98 | // Number of keyframes currently in pool. 99 | int num_kfs_; 100 | 101 | // Maximum number of keyframes to maintain in pool. 102 | int max_kfs_; 103 | 104 | // Threshold to create new keyframe. 105 | float new_kf_thresh_; 106 | }; 107 | 108 | } // namespace utils 109 | 110 | } // namespace flame 111 | -------------------------------------------------------------------------------- /src/flame/stereo/inverse_depth_filter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file inverse_depth_filter.h 19 | * @author W. Nicholas Greene 20 | * @date 2016-09-17 00:23:50 (Sat) 21 | */ 22 | 23 | #pragma once 24 | 25 | #include "flame/stereo/line_stereo.h" 26 | #include "flame/stereo/epipolar_geometry.h" 27 | 28 | namespace flame { 29 | 30 | namespace stereo { 31 | 32 | /** 33 | * @brief Namespace for functions that perform LSD-SLAM style inverse depth 34 | * filtering. 35 | */ 36 | namespace inverse_depth_filter { 37 | 38 | enum Status { 39 | SUCCESS = 0, 40 | FAIL_REF_PATCH_GRADIENT = 1, 41 | FAIL_AMBIGUOUS_MATCH = 2, 42 | FAIL_MAX_COST = 3, 43 | }; 44 | 45 | /** 46 | * @brief Parameter struct for inverse_depth_filter. 47 | */ 48 | struct Params { 49 | Params() {} 50 | 51 | int win_size = 5; // Size of cost window. 52 | float search_sigma = 2.0f; // Size of search region (+/- search_sigma * sigma). 53 | 54 | float min_grad_mag = 5.0f; 55 | 56 | // Bounds for idepth search region. 57 | float idepth_min = 1e-3; 58 | float idepth_max = 2.0f; 59 | 60 | // Bounds for epipolar search region. 61 | float epilength_min = 3.0f; 62 | float epilength_max = 32.0f; 63 | 64 | float process_var_factor = 1.01f; // IDepth variance is multiplied by this each timestep. 65 | float process_fail_var_factor = 1.1f; // IDepth variance is multiplied by this if search fails. 66 | 67 | stereo::line_stereo::Params sparams; 68 | }; 69 | 70 | /** 71 | * @brief Predict step. 72 | */ 73 | bool predict(const stereo::EpipolarGeometry& epigeo, 74 | float process_var_factor, 75 | const cv::Point2f& u_ref, float mu, float var, 76 | cv::Point2f* u_cmp, float* mu_pred, float* var_pred); 77 | 78 | /** 79 | * @brief Compute search region based on idepth moments. 80 | */ 81 | bool getSearchRegion(const Params& params, 82 | const stereo::EpipolarGeometry& epigeo, 83 | int width, int height, 84 | const cv::Point2f& u_ref, 85 | float mu_prior, float var_prior, 86 | cv::Point2f* start, cv::Point2f* end, cv::Point2f* epi); 87 | 88 | /** 89 | * @brief Search for matching pixel in search region. 90 | */ 91 | Status search(const Params& params, 92 | const stereo::EpipolarGeometry& epigeo, 93 | float rescale_factor, 94 | const cv::Mat1b& img_ref, 95 | const cv::Mat1b& img_cmp, 96 | const cv::Point2f& u_ref, 97 | const cv::Point2f& u_start, 98 | const cv::Point2f& u_end, 99 | cv::Point2f* u_cmp); 100 | 101 | /** 102 | * @brief Fuse measurement with pred. 103 | */ 104 | bool update(float mu_pred, float var_pred, 105 | float mu_meas, float var_meas, 106 | float* mu_post, float* var_post, 107 | float outlier_sigma_thresh = 2.0f); 108 | 109 | } // namespace inverse_depth_filter 110 | 111 | } // namespace stereo 112 | 113 | } // namespace flame 114 | -------------------------------------------------------------------------------- /src/flame/types.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file types.h 19 | * @author W. Nicholas Greene 20 | * @date 2017-06-16 16:49:05 (Fri) 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | namespace flame { 36 | 37 | /*==================== Image types ====================*/ 38 | // Point2. 39 | template 40 | using Point2 = cv::Point_; 41 | 42 | using Point2i = Point2; 43 | using Point2f = Point2; 44 | using Point2d = Point2; 45 | 46 | // Vec3b/Vec4b. 47 | using Vec3b = cv::Vec3b; 48 | using Vec4b = cv::Vec4b; 49 | 50 | // Image. 51 | template 52 | using Image = cv::Mat_; 53 | 54 | using Image1b = Image; 55 | using Image3b = Image; 56 | using Image4b = Image; 57 | using Image1f = Image; 58 | 59 | // Pyramids. 60 | using ImagePyramid = std::vector; 61 | 62 | template 63 | using ImagePyramid_ = std::vector >; 64 | 65 | using ImagePyramidb = ImagePyramid_; 66 | using ImagePyramid3b = ImagePyramid_; 67 | using ImagePyramidf = ImagePyramid_; 68 | using ImagePyramidd = ImagePyramid_; 69 | 70 | /*==================== Matrix/Vector types ====================*/ 71 | template 72 | using Matrix = Eigen::Matrix; 73 | 74 | template 75 | using Matrix2 = Matrix; 76 | 77 | template 78 | using Matrix3 = Matrix; 79 | 80 | template 81 | using Matrix4 = Matrix; 82 | 83 | template 84 | using Vector = Matrix; 85 | 86 | template 87 | // cppcheck-suppress constStatement 88 | using Vector2 = Vector; 89 | 90 | template 91 | // cppcheck-suppress constStatement 92 | using Vector3 = Vector; 93 | 94 | template 95 | // cppcheck-suppress constStatement 96 | using Vector4 = Vector; 97 | 98 | using Matrix2f = Matrix2; 99 | using Matrix3f = Matrix3; 100 | using Matrix4f = Matrix4; 101 | using Matrix2d = Matrix2; 102 | using Matrix3d = Matrix3; 103 | using Matrix4d = Matrix4; 104 | 105 | using Vector2f = Vector2; 106 | using Vector3f = Vector3; 107 | using Vector4f = Vector4; 108 | using Vector2d = Vector2; 109 | using Vector3d = Vector3; 110 | using Vector4d = Vector4; 111 | 112 | /*==================== Quaternion types. ====================*/ 113 | template 114 | using Quaternion = Eigen::Quaternion; 115 | 116 | using Quaternionf = Quaternion; 117 | using Quaterniond = Quaternion; 118 | 119 | /*==================== AngleAxis types ====================*/ 120 | template 121 | using AngleAxis = Eigen::AngleAxis; 122 | 123 | using AngleAxisf = AngleAxis; 124 | using AngleAxisd = AngleAxis; 125 | 126 | /*==================== Lie group types. ====================*/ 127 | template 128 | using SE3 = Sophus::SE3Group; 129 | 130 | using SE3f = SE3; 131 | using SE3d = SE3; 132 | 133 | } // namespace flame 134 | -------------------------------------------------------------------------------- /test/utils/visualization_test.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file visualization_test.cc 19 | * @author W. Nicholas Greene 20 | * @date 2017-08-18 19:15:38 (Fri) 21 | */ 22 | 23 | #include "gtest/gtest.h" 24 | 25 | #include "flame/utils/visualization.h" 26 | 27 | namespace flame { 28 | 29 | namespace utils { 30 | 31 | TEST(VisualizationTest, blendColorTest1) { 32 | cv::Vec3b red(0, 0, 255); 33 | cv::Vec3b green(0, 255, 0); 34 | cv::Vec3b blend = blendColor(red, green, -1.0f, 0.0f, 1.0f); 35 | 36 | EXPECT_EQ(red[0], blend[0]); 37 | EXPECT_EQ(red[1], blend[1]); 38 | EXPECT_EQ(red[2], blend[2]); 39 | } 40 | 41 | TEST(VisualizationTest, blendColorTest2) { 42 | cv::Vec3b red(0, 0, 255); 43 | cv::Vec3b green(0, 255, 0); 44 | cv::Vec3b blend = blendColor(red, green, 2.0f, 0.0f, 1.0f); 45 | 46 | EXPECT_EQ(green[0], blend[0]); 47 | EXPECT_EQ(green[1], blend[1]); 48 | EXPECT_EQ(green[2], blend[2]); 49 | } 50 | 51 | TEST(VisualizationTest, blendColorTest3) { 52 | cv::Vec3b red(0, 0, 255); 53 | cv::Vec3b green(0, 255, 0); 54 | cv::Vec3b blend = blendColor(red, green, 0.5f, 0.0f, 1.0f); 55 | 56 | EXPECT_EQ(0, blend[0]); 57 | EXPECT_EQ(127, blend[1]); 58 | EXPECT_EQ(127, blend[2]); 59 | } 60 | 61 | TEST(VisualizationTest, censusToGrayTest1) { 62 | cv::Vec3b ret = censusToGray(0, 0, 100); 63 | EXPECT_EQ(0, ret[0]); 64 | EXPECT_EQ(0, ret[1]); 65 | EXPECT_EQ(0, ret[2]); 66 | } 67 | 68 | TEST(VisualizationTest, censusToGrayTest2) { 69 | cv::Vec3b ret = censusToGray(100, 0, 100); 70 | EXPECT_EQ(255, ret[0]); 71 | EXPECT_EQ(255, ret[1]); 72 | EXPECT_EQ(255, ret[2]); 73 | } 74 | 75 | TEST(VisualizationTest, censusToGrayTest3) { 76 | cv::Vec3b ret = censusToGray(50, 0, 100); 77 | EXPECT_EQ(127, ret[0]); 78 | EXPECT_EQ(127, ret[1]); 79 | EXPECT_EQ(127, ret[2]); 80 | } 81 | 82 | // TEST(VisualizationTest, censusToRGBTest1) { 83 | // cv::Mat_ img(3, 3, static_cast(0)); 84 | // // For some reason operator() is giving weird results. just work on raw data. 85 | // // img(1, 0) = 10000; 86 | // // img(0, 1) = 20000; 87 | 88 | // uint32_t* img_ptr = reinterpret_cast(img.data); 89 | // img_ptr[0 * img.cols + 1] = 10000; 90 | // img_ptr[1 * img.cols + 0] = 20000; 91 | 92 | // cv::Mat3b out(3, 3); 93 | // cv::Vec3b* out_ptr = reinterpret_cast(out.data); 94 | 95 | // censusToRGB(img, &out); 96 | 97 | // EXPECT_EQ(img.rows, out.rows); 98 | // EXPECT_EQ(img.cols, out.cols); 99 | // for (int ii = 0; ii < img.rows; ++ii) { 100 | // for (int jj = 0; jj < img.cols; ++jj) { 101 | // if ((ii == 0) && (jj == 1)) { 102 | // EXPECT_EQ(127, out_ptr[ii * img.cols + jj][0]); 103 | // EXPECT_EQ(127, out_ptr[ii * img.cols + jj][1]); 104 | // EXPECT_EQ(127, out_ptr[ii * img.cols + jj][2]); 105 | // } else if ((ii == 1) && (jj == 0)) { 106 | // EXPECT_EQ(255, out_ptr[ii * img.cols + jj][0]); 107 | // EXPECT_EQ(255, out_ptr[ii * img.cols + jj][1]); 108 | // EXPECT_EQ(255, out_ptr[ii * img.cols + jj][2]); 109 | // } else { 110 | // EXPECT_EQ(0, out_ptr[ii * img.cols + jj][0]); 111 | // EXPECT_EQ(0, out_ptr[ii * img.cols + jj][1]); 112 | // EXPECT_EQ(0, out_ptr[ii * img.cols + jj][2]); 113 | // } 114 | // } 115 | // } 116 | // } 117 | 118 | } // namespace utils 119 | 120 | } // namespace flame 121 | -------------------------------------------------------------------------------- /src/flame/utils/pyramids.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file pyramids.h 19 | * @author W. Nicholas Greene 20 | * @date 2017-08-18 20:07:46 (Fri) 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | #include "flame/utils/image_utils.h" 28 | 29 | namespace flame { 30 | 31 | namespace utils { 32 | 33 | /** 34 | * \brief Create a Gaussian pyramid. 35 | * 36 | * The input image is placed into the lowest level. 37 | * 38 | * @param[in] img Base level image. 39 | * @param[in] num_lvl Number of levels in pyramid. 40 | * @return Image pyramid. 41 | */ 42 | inline ImagePyramid getGaussianPyramid(const cv::Mat& img, int num_lvl = 5) { 43 | ImagePyramid ret(num_lvl); 44 | 45 | ret[0] = img; 46 | for (int lvl = 1; lvl < num_lvl; ++lvl) { 47 | cv::pyrDown(ret[lvl - 1], ret[lvl]); 48 | } 49 | 50 | return ret; 51 | } 52 | 53 | /** 54 | * \brief Arrange an image pyramid for display in a single image. 55 | * 56 | * Arranges the levels of the image pyramid next to each other for display. 57 | * 58 | * @param[in] pyr Input pyramid. 59 | * @param[out] out Single output image. 60 | */ 61 | void getPyramidDisplay(const ImagePyramid& pyr, cv::Mat* out); 62 | 63 | /** 64 | * \brief Create a pyramid of image gradients. 65 | * 66 | * @param[in] pyr Image pyramid with 0 being the lowest level. 67 | * @param[out] gradx Horiztonal gradient. 68 | * @param[out] grady Vertical gradient. 69 | * @return Gradient pyramid. 70 | */ 71 | template 72 | inline void getGradientPyramid(const ImagePyramid_& pyr, 73 | ImagePyramid_* gradx, 74 | ImagePyramid_* grady) { 75 | FLAME_ASSERT(pyr.size() > 0); 76 | 77 | if (gradx->empty()) { 78 | gradx->resize(pyr.size()); 79 | } 80 | if (grady->empty()) { 81 | grady->resize(pyr.size()); 82 | } 83 | 84 | for (int lvl = 0; lvl < pyr.size(); ++lvl) { 85 | getCentralGradient(pyr[lvl], &((*gradx)[lvl]), 86 | &((*grady)[lvl])); 87 | } 88 | 89 | return; 90 | } 91 | 92 | /** 93 | * \brief Return the magnitude of the image gradients. 94 | * @param[in] gradx Horizontal gradient. 95 | * @param[in] grady Vertical gradient. 96 | * @param[out] mag Magnitude. 97 | */ 98 | template 99 | inline void getGradientMagPyramid(const ImagePyramid_& gradx, 100 | const ImagePyramid_& grady, 101 | ImagePyramid_* mag) { 102 | FLAME_ASSERT(!gradx.empty()); 103 | FLAME_ASSERT(!grady.empty()); 104 | FLAME_ASSERT(gradx.size() == grady.size()); 105 | 106 | if (mag->empty()) { 107 | mag->resize(gradx.size()); 108 | } 109 | 110 | for (int lvl = 0; lvl < gradx.size(); ++lvl) { 111 | getGradientMag(gradx[lvl], grady[lvl], &((*mag)[lvl])); 112 | } 113 | 114 | return; 115 | } 116 | 117 | /** 118 | * \brief Apply 3x3 max filter to a pyramid. 119 | * 120 | * @param[in] pyr Image pyramid. 121 | */ 122 | template 123 | void applyMaxFilter3Pyramid(ImagePyramid_* pyr) { 124 | for (int lvl = 0; lvl < pyr->size(); ++lvl) { 125 | applyMaxFilter3(&((*pyr)[lvl])); 126 | } 127 | } 128 | 129 | } // namespace utils 130 | 131 | } // namespace flame 132 | -------------------------------------------------------------------------------- /src/flame/utils/delaunay.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file delaunay.cc 19 | * @author W. Nicholas Greene 20 | * @date 2017-08-18 20:48:05 (Fri) 21 | */ 22 | 23 | // Adapted from cv_utils.cpp from fast-stereo (Sudeep Pillai). 24 | 25 | #include "flame/utils/delaunay.h" 26 | 27 | namespace flame { 28 | 29 | namespace utils { 30 | 31 | void Delaunay::triangulate(const std::vector& support, 32 | std::vector* triangles) { 33 | 34 | // input/output structure for triangulation 35 | struct triangulateio in; 36 | int32_t k; 37 | 38 | // inputs 39 | in.numberofpoints = support.size(); 40 | in.pointlist = (float*)malloc(in.numberofpoints*2*sizeof(float)); // NOLINT 41 | k = 0; 42 | for (int32_t i = 0; i < support.size(); i++) { 43 | in.pointlist[k++] = support[i].x; 44 | in.pointlist[k++] = support[i].y; 45 | } 46 | in.numberofpointattributes = 0; 47 | in.pointattributelist = NULL; 48 | in.pointmarkerlist = NULL; 49 | in.numberofsegments = 0; 50 | in.numberofholes = 0; 51 | in.numberofregions = 0; 52 | in.regionlist = NULL; 53 | 54 | // outputs 55 | out_.pointlist = NULL; 56 | out_.pointattributelist = NULL; 57 | out_.pointmarkerlist = NULL; 58 | out_.trianglelist = NULL; 59 | out_.triangleattributelist = NULL; 60 | out_.neighborlist = NULL; 61 | out_.segmentlist = NULL; 62 | out_.segmentmarkerlist = NULL; 63 | out_.edgelist = NULL; 64 | out_.edgemarkerlist = NULL; 65 | 66 | // do triangulation (z=zero-based, n=neighbors, Q=quiet, B=no boundary markers) 67 | char parameters[] = "zneQB"; 68 | ::triangulate(parameters, &in, &out_, NULL); 69 | free(in.pointlist); 70 | 71 | getTriangles(triangles); 72 | getNeighbors(); 73 | getEdges(); 74 | cleanup(); 75 | 76 | return; 77 | } 78 | 79 | void Delaunay::triangulate(const std::vector& vertices) { 80 | triangulate(vertices, &triangles_); 81 | return; 82 | } 83 | 84 | void Delaunay::cleanup() { 85 | // free memory used for triangulation 86 | free(out_.pointlist); 87 | free(out_.trianglelist); 88 | free(out_.edgelist); 89 | free(out_.neighborlist); 90 | 91 | out_.pointlist = NULL; 92 | out_.trianglelist = NULL; 93 | out_.edgelist = NULL; 94 | out_.neighborlist = NULL; 95 | 96 | return; 97 | } 98 | 99 | void Delaunay::getTriangles(std::vector* triangles) { 100 | // put resulting triangles into vector tri 101 | triangles->resize(out_.numberoftriangles); 102 | int k = 0; 103 | for (int32_t i = 0; i < out_.numberoftriangles; i++) { 104 | (*triangles)[i] = Triangle(out_.trianglelist[k], 105 | out_.trianglelist[k+1], 106 | out_.trianglelist[k+2]); 107 | k+=3; 108 | } 109 | return; 110 | } 111 | 112 | void Delaunay::getNeighbors() { 113 | // put neighboring triangles into vector tri 114 | neighbors_.resize(out_.numberoftriangles); 115 | int k = 0; 116 | for (int32_t i = 0; i < out_.numberoftriangles; i++) { 117 | neighbors_[i] = Triangle(out_.neighborlist[k], 118 | out_.neighborlist[k+1], 119 | out_.neighborlist[k+2]); 120 | k+=3; 121 | } 122 | return; 123 | } 124 | 125 | void Delaunay::getEdges() { 126 | // put resulting edges into vector 127 | edges_.resize(out_.numberofedges); 128 | int k = 0; 129 | for (int32_t i = 0; i < out_.numberofedges; i++) { 130 | edges_[i] = Edge(out_.edgelist[k], out_.edgelist[k+1]); 131 | k+=2; 132 | } 133 | return; 134 | } 135 | 136 | } // namespace utils 137 | 138 | } // namespace flame 139 | -------------------------------------------------------------------------------- /src/flame/utils/assert.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file assert.h 19 | * @author W. Nicholas Greene 20 | * @date 2017-08-18 18:22:29 (Fri) 21 | */ 22 | 23 | #pragma once 24 | 25 | #ifdef FLAME_NO_ASSERT 26 | #define FLAME_ASSERT(x) 27 | #else 28 | #include 29 | #include 30 | #include // for stack trace 31 | #include // for abort 32 | 33 | #define FLAME_COLOR_RESET "\033[0m" 34 | #define FLAME_COLOR_RED "\033[31m" 35 | #define FLAME_COLOR_GREEN "\033[32m" 36 | 37 | namespace flame { 38 | 39 | namespace utils { 40 | 41 | inline void assert_fail(const char *condition, const char *function, 42 | const char *file, int line) { 43 | fprintf(stderr, FLAME_COLOR_RED "FLAME_ASSERT failed: %s in function %s at %s: %i\n" FLAME_COLOR_RESET, 44 | condition, function, file, line); 45 | 46 | fprintf(stderr, FLAME_COLOR_RED "Stacktrace:\n" FLAME_COLOR_RESET); 47 | 48 | // Get and print stack trace with demangled names. Taken from: 49 | // https://panthema.net/2008/0901-stacktrace-demangled 50 | constexpr int max_frames = 16; 51 | void* stack_frames[max_frames]; 52 | int num_frames = backtrace(stack_frames, max_frames); // Get stack addresses. 53 | 54 | // Get strings of trace. 55 | char** symbols = backtrace_symbols(stack_frames, num_frames); 56 | 57 | // Allocate string which will be filled with the demangled function name. 58 | // allocate string which will be filled with the demangled function name 59 | size_t funcnamesize = 256; 60 | char* funcname = static_cast(malloc(funcnamesize)); 61 | 62 | // iterate over the returned symbol lines. skip the first, it is the 63 | // address of this function. 64 | for (int i = 1; i < num_frames; i++) { 65 | char *begin_name = 0, *begin_offset = 0, *end_offset = 0; 66 | 67 | // find parentheses and +address offset surrounding the mangled name: 68 | // ./module(function+0x15c) [0x8048a6d] 69 | for (char* p = symbols[i]; *p; ++p) { 70 | if (*p == '(') { 71 | begin_name = p; 72 | } else if (*p == '+') { 73 | begin_offset = p; 74 | } else if (*p == ')' && begin_offset) { 75 | end_offset = p; 76 | break; 77 | } 78 | } 79 | 80 | if (begin_name && begin_offset && end_offset && begin_name < begin_offset) { 81 | *begin_name++ = '\0'; 82 | *begin_offset++ = '\0'; 83 | *end_offset = '\0'; 84 | 85 | // mangled name is now in [begin_name, begin_offset) and caller 86 | // offset in [begin_offset, end_offset). now apply 87 | // __cxa_demangle(): 88 | 89 | int status; 90 | char* ret = abi::__cxa_demangle(begin_name, funcname, &funcnamesize, 91 | &status); 92 | if (status == 0) { 93 | funcname = ret; // use possibly realloc()-ed string 94 | fprintf(stderr, FLAME_COLOR_RED " %s : %s+%s\n" FLAME_COLOR_RESET, 95 | symbols[i], funcname, begin_offset); 96 | } else { 97 | // demangling failed. Output function name as a C function with 98 | // no arguments. 99 | fprintf(stderr, FLAME_COLOR_RED " %s : %s()+%s\n" FLAME_COLOR_RESET, 100 | symbols[i], begin_name, begin_offset); 101 | } 102 | } else { 103 | // couldn't parse the line? print the whole line. 104 | fprintf(stderr, FLAME_COLOR_RED " %s\n" FLAME_COLOR_RESET, symbols[i]); 105 | } 106 | } 107 | 108 | free(funcname); 109 | free(symbols); 110 | 111 | exit(1); 112 | // abort(); 113 | } 114 | 115 | } // namespace utils 116 | 117 | } // namespace flame 118 | 119 | #define FLAME_ASSERT(condition) \ 120 | do { \ 121 | if (!(condition)) \ 122 | flame::utils::assert_fail(#condition, __PRETTY_FUNCTION__, __FILE__, __LINE__); \ 123 | } while (false) 124 | 125 | #endif 126 | -------------------------------------------------------------------------------- /src/flame/utils/stats_tracker.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file stats_tracker.h 19 | * @author W. Nicholas Greene 20 | * @date 2017-08-18 19:43:19 (Fri) 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace flame { 32 | 33 | namespace utils { 34 | 35 | typedef std::chrono::high_resolution_clock clock; 36 | typedef std::chrono::duration msec; 37 | 38 | class StatsTracker final { 39 | public: 40 | /** 41 | * \brief Constructor. 42 | * 43 | * @param[in] prefix Prefix to all keys. 44 | */ 45 | explicit StatsTracker(const std::string& prefix = "") : 46 | prefix_(prefix), 47 | start_times_(), 48 | timings_(), 49 | stats_() {} 50 | 51 | ~StatsTracker() = default; 52 | 53 | StatsTracker(const StatsTracker& rhs) = delete; 54 | StatsTracker& operator=(const StatsTracker& rhs) = delete; 55 | 56 | StatsTracker(const StatsTracker&& rhs) = delete; 57 | StatsTracker& operator=(const StatsTracker&& rhs) = delete; 58 | 59 | /** 60 | * \brief Return copy of internal timing stats map. 61 | * 62 | * Return a copy of the data, otherwise you would need to lock the mutex 63 | * before getting a reference. 64 | */ 65 | std::unordered_map timings() { 66 | std::lock_guard lock(mtx_); 67 | return timings_; 68 | } 69 | 70 | /** 71 | * \brief Return a copy of internal stats map. 72 | * 73 | * Return a copy of the data, otherwise you would need to lock the mutex 74 | * before getting a reference. 75 | */ 76 | std::unordered_map stats() { 77 | std::lock_guard lock(mtx_); 78 | return stats_; 79 | } 80 | 81 | /** 82 | * \brief Return current timing for key. 83 | */ 84 | double timings(const std::string& key) { 85 | std::lock_guard lock(mtx_); 86 | return timings_[prefix_ + key]; 87 | } 88 | 89 | /** 90 | * \brief Return current stat for key. 91 | */ 92 | double stats(const std::string& key) { 93 | std::lock_guard lock(mtx_); 94 | return stats_[prefix_ + key]; 95 | } 96 | 97 | /** 98 | * \brief Start timer for key. 99 | */ 100 | void tick(const std::string& key) { 101 | std::lock_guard lock(mtx_); 102 | start_times_[prefix_ + key] = clock::now(); 103 | return; 104 | } 105 | 106 | /** 107 | * \brief Update timing for key. 108 | * 109 | * Updates timing with latest time since last call to tick(key). 110 | */ 111 | double tock(const std::string& key) { 112 | std::lock_guard lock(mtx_); 113 | auto search = start_times_.find(prefix_ + key); 114 | if (search != start_times_.end()) { 115 | msec ms = clock::now() - search->second; 116 | timings_[prefix_ + key] = ms.count(); 117 | return ms.count(); 118 | } 119 | 120 | return std::numeric_limits::quiet_NaN(); 121 | } 122 | 123 | /** 124 | * \brief Set stat to x. 125 | */ 126 | void set(const std::string& key, double x = 0) { 127 | std::lock_guard lock(mtx_); 128 | stats_[prefix_ + key] = x; 129 | return; 130 | } 131 | 132 | /** 133 | * \brief Add x to stat. 134 | */ 135 | double add(const std::string& key, double x) { 136 | std::lock_guard lock(mtx_); 137 | stats_[prefix_ + key] += x; 138 | return stats_[prefix_ + key]; 139 | } 140 | 141 | /** 142 | * \brief Return a handle to the underlying mutex. 143 | */ 144 | std::mutex& mutex() { return mtx_; } 145 | 146 | /** 147 | * \brief Return a copy of the prefix string. 148 | */ 149 | std::string prefix() { return prefix_; } 150 | 151 | void reset() { 152 | start_times_.clear(); 153 | timings_.clear(); 154 | stats_.clear(); 155 | return; 156 | } 157 | 158 | private: 159 | std::string prefix_; 160 | std::unordered_map > start_times_; 161 | std::unordered_map timings_; 162 | std::unordered_map stats_; 163 | std::mutex mtx_; 164 | }; 165 | 166 | } // namespace utils 167 | 168 | } // namespace flame 169 | -------------------------------------------------------------------------------- /src/flame/utils/rasterization.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file rasterization.h 19 | * @author W. Nicholas Greene 20 | * @date 2017-08-18 19:05:08 (Fri) 21 | */ 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | namespace flame { 29 | 30 | namespace utils { 31 | 32 | /** 33 | * \brief Sort indices by increasing y-coord. 34 | */ 35 | inline void SortVertices(cv::Point* p1, cv::Point* p2, cv::Point* p3) { 36 | cv::Point tmp; 37 | if (p1->y > p2->y) { 38 | tmp = *p1; 39 | *p1 = *p2; 40 | *p2 = tmp; 41 | } 42 | 43 | if (p1->y > p3->y) { 44 | tmp = *p1; 45 | *p1 = *p3; 46 | *p3 = tmp; 47 | } 48 | 49 | if (p2->y > p3->y) { 50 | tmp = *p2; 51 | *p2 = *p3; 52 | *p3 = tmp; 53 | } 54 | 55 | return; 56 | } 57 | 58 | /** 59 | * \brief Draw an line of interpolated values. 60 | * 61 | * Draw a line between p1 and p2, interpolating values linearly from v1 to v2. 62 | * Assumes img is a 32-bit float image. 63 | */ 64 | void DrawLineInterpolated(cv::Point p1, cv::Point p2, 65 | float v1, float v2, cv::Mat* img); 66 | 67 | /** 68 | * \brief Draw a shaded triangle. 69 | * 70 | * Assumes the vertices are sorted in increasing y order. 71 | * Assumes the top of the triangle is flat such that p1.y = p2.y and that p1.x < p2.x. 72 | * 73 | * Based on: 74 | * http://www.sunshine2k.de/coding/java/TriangleRasterization/TriangleRasterization.html 75 | */ 76 | void DrawTopFlatShadedTriangle(cv::Point p1, cv::Point p2, cv::Point p3, 77 | float v1, float v2, float v3, cv::Mat* img); 78 | 79 | /** 80 | * \brief Draw a shaded triangle. 81 | * 82 | * Assumes the vertices are sorted in increasing y order. 83 | * Assumes the bottom of the triangle is flat such that p2.y = p3.y and that p2.x < p3.x. 84 | * 85 | * Based on: 86 | * http://www.sunshine2k.de/coding/java/TriangleRasterization/TriangleRasterization.html 87 | */ 88 | void DrawBottomFlatShadedTriangle(cv::Point p1, cv::Point p2, cv::Point p3, 89 | float v1, float v2, float v3, cv::Mat* img); 90 | 91 | /** 92 | * \brief Draw a shaded triangle. 93 | */ 94 | void DrawShadedTriangle(cv::Point p1, cv::Point p2, cv::Point p3, 95 | float v1, float v2, float v3, cv::Mat* img); 96 | 97 | /** 98 | * \brief Draw a shaded triangle. 99 | * 100 | * Assumes points are in counter-clockwise order. 101 | * 102 | * Uses the barycentric approach described here: 103 | * https://fgiesen.wordpress.com/2013/02/06/the-barycentric-conspirac/ 104 | */ 105 | void DrawShadedTriangleBarycentric(cv::Point p1, cv::Point p2, cv::Point p3, 106 | float v1, float v2, float v3, cv::Mat* img); 107 | 108 | inline int orient2d(const cv::Point& a, const cv::Point& b, const cv::Point& c) { 109 | return (b.y - a.y)*c.x - (b.x - a.x)*c.y + (b.x*a.y - a.x*b.y); 110 | } 111 | 112 | inline int min3(int x, int y, int z) { 113 | return x < y ? (x < z ? x : z) : (y < z ? y : z); 114 | } 115 | 116 | inline int max3(int x, int y, int z) { 117 | return x > y ? (x > z ? x : z) : (y > z ? y : z); 118 | } 119 | 120 | struct Edge { 121 | static const int stepXSize = 4; 122 | static const int stepYSize = 1; 123 | 124 | // __m128 is the SSE 128-bit packed float type (4 floats). 125 | __m128 oneStepX; 126 | __m128 oneStepY; 127 | 128 | __m128 init(const cv::Point& v0, const cv::Point& v1, 129 | const cv::Point& origin) { 130 | // Edge setup 131 | float A = v1.y - v0.y; 132 | float B = v0.x - v1.x; 133 | float C = v1.x*v0.y - v0.x*v1.y; 134 | 135 | // Step deltas 136 | // __m128i y = _mm_set1_ps(x) sets y[0..3] = x. 137 | oneStepX = _mm_set1_ps(A*stepXSize); 138 | oneStepY = _mm_set1_ps(B*stepYSize); 139 | 140 | // x/y values for initial pixel block 141 | // NOTE: Set operations have arguments in reverse order! 142 | // __m128 y = _mm_set_epi32(x3, x2, x1, x0) sets y0 = x0, etc. 143 | __m128 x = _mm_set_ps(origin.x + 3, origin.x + 2, origin.x + 1, origin.x); 144 | __m128 y = _mm_set1_ps(origin.y); 145 | 146 | // Edge function values at origin 147 | // A*x + B*y + C. 148 | __m128 A4 = _mm_set1_ps(A); 149 | __m128 B4 = _mm_set1_ps(B); 150 | __m128 C4 = _mm_set1_ps(C); 151 | 152 | return _mm_add_ps(_mm_add_ps(_mm_mul_ps(A4, x), _mm_mul_ps(B4, y)), C4); 153 | } 154 | }; 155 | 156 | } // namespace utils 157 | 158 | } // namespace flame 159 | -------------------------------------------------------------------------------- /src/flame/stereo/inverse_depth_meas_model.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file inverse_depth_noise_model.h 19 | * @author W. Nicholas Greene 20 | * @date 2016-07-19 20:19:52 (Tue) 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | 31 | #include "flame/stereo/epipolar_geometry.h" 32 | 33 | namespace flame { 34 | 35 | namespace stereo { 36 | 37 | /** 38 | * @brief Class to compute noisy inverse depth measurements using the LSD-SLAM 39 | * noise model. 40 | */ 41 | class InverseDepthMeasModel final { 42 | public: 43 | // Noise parameters. 44 | struct Params { 45 | Params() {} 46 | 47 | int win_size = 5; // Window size for padding. 48 | float pixel_var = 16.0f; 49 | float epipolar_line_var = 1.0f; 50 | 51 | bool verbose = false; // Print verbose errors. 52 | }; 53 | 54 | InverseDepthMeasModel(const Eigen::Matrix3f& K, 55 | const Eigen::Matrix3f& Kinv, 56 | const Params& params = Params()); 57 | ~InverseDepthMeasModel() = default; 58 | 59 | InverseDepthMeasModel(const InverseDepthMeasModel& rhs) = default; 60 | InverseDepthMeasModel& operator=(const InverseDepthMeasModel& rhs) = default; 61 | 62 | InverseDepthMeasModel(InverseDepthMeasModel&& rhs) = default; 63 | InverseDepthMeasModel& operator=(InverseDepthMeasModel&& rhs) = default; 64 | 65 | /** 66 | * @brief Load geometry data. 67 | */ 68 | void loadGeometry(const Sophus::SE3f& T_ref, const Sophus::SE3f& T_cmp) { 69 | T_ref_to_cmp_ = T_cmp.inverse() * T_ref; 70 | epigeo_.loadGeometry(T_ref_to_cmp_.unit_quaternion(), 71 | T_ref_to_cmp_.translation()); 72 | inited_geo_ = true; 73 | return; 74 | } 75 | 76 | /** 77 | * @brief Load non-padded image data. 78 | */ 79 | void loadImages(const cv::Mat& img_ref, const cv::Mat& img_cmp, 80 | const cv::Mat1f& gradx_ref, const cv::Mat1f& grady_ref, 81 | const cv::Mat1f& gradx_cmp, const cv::Mat1f& grady_cmp) { 82 | FLAME_ASSERT(img_ref.isContinuous()); 83 | FLAME_ASSERT(img_cmp.isContinuous()); 84 | FLAME_ASSERT(gradx_ref.isContinuous()); 85 | FLAME_ASSERT(grady_ref.isContinuous()); 86 | FLAME_ASSERT(gradx_cmp.isContinuous()); 87 | FLAME_ASSERT(grady_cmp.isContinuous()); 88 | 89 | // Allocate memory. 90 | int border = params_.win_size / 2 + 1; 91 | img_ref_.create(img_ref.rows + 2 * border, img_ref.cols + 2 * border, 0); 92 | img_cmp_.create(img_ref.rows + 2 * border, img_ref.cols + 2 * border, 0); 93 | gradx_ref_.create(img_ref.rows + 2 * border, img_ref.cols + 2 * border); 94 | grady_ref_.create(img_ref.rows + 2 * border, img_ref.cols + 2 * border); 95 | gradx_cmp_.create(img_cmp.rows + 2 * border, img_cmp.cols + 2 * border); 96 | grady_cmp_.create(img_cmp.rows + 2 * border, img_cmp.cols + 2 * border); 97 | 98 | // Pad images. 99 | cv::copyMakeBorder(img_ref, img_ref_, border, border, border, border, 100 | cv::BORDER_REFLECT_101); 101 | cv::copyMakeBorder(img_cmp, img_cmp_, border, border, border, border, 102 | cv::BORDER_REFLECT_101); 103 | cv::copyMakeBorder(gradx_ref, gradx_ref_, border, border, border, border, 104 | cv::BORDER_CONSTANT); 105 | cv::copyMakeBorder(grady_ref, grady_ref_, border, border, border, border, 106 | cv::BORDER_CONSTANT); 107 | cv::copyMakeBorder(gradx_cmp, gradx_cmp_, border, border, border, border, 108 | cv::BORDER_CONSTANT); 109 | cv::copyMakeBorder(grady_cmp, grady_cmp_, border, border, border, border, 110 | cv::BORDER_CONSTANT); 111 | 112 | inited_imgs_ = true; 113 | return; 114 | } 115 | 116 | /** 117 | * @brief Load padded image data. 118 | */ 119 | void loadPaddedImages(const cv::Mat& img_ref, const cv::Mat& img_cmp, 120 | const cv::Mat1f& gradx_ref, const cv::Mat1f& grady_ref, 121 | const cv::Mat1f& gradx_cmp, const cv::Mat1f& grady_cmp) { 122 | FLAME_ASSERT(img_ref.isContinuous()); 123 | FLAME_ASSERT(img_cmp.isContinuous()); 124 | FLAME_ASSERT(gradx_ref.isContinuous()); 125 | FLAME_ASSERT(grady_ref.isContinuous()); 126 | FLAME_ASSERT(gradx_cmp.isContinuous()); 127 | FLAME_ASSERT(grady_cmp.isContinuous()); 128 | 129 | img_ref_ = img_ref; 130 | img_cmp_ = img_cmp; 131 | gradx_ref_ = gradx_ref; 132 | grady_ref_ = grady_ref; 133 | gradx_cmp_ = gradx_cmp; 134 | grady_cmp_ = grady_cmp; 135 | 136 | inited_imgs_ = true; 137 | return; 138 | } 139 | 140 | /** 141 | * @brief Return inverse depth mean and variance. 142 | */ 143 | bool idepth(const cv::Point2f& u_ref, const cv::Point2f& u_cmp, 144 | float* mu, float* var) const; 145 | 146 | private: 147 | bool inited_geo_; 148 | bool inited_imgs_; 149 | Params params_; 150 | 151 | cv::Mat img_ref_; 152 | cv::Mat img_cmp_; 153 | cv::Mat1f gradx_ref_; 154 | cv::Mat1f grady_ref_; 155 | cv::Mat1f gradx_cmp_; 156 | cv::Mat1f grady_cmp_; 157 | 158 | Sophus::SE3f T_ref_to_cmp_; 159 | stereo::EpipolarGeometry epigeo_; 160 | }; 161 | 162 | } // namespace stereo 163 | 164 | } // namespace flame 165 | -------------------------------------------------------------------------------- /src/flame/stereo/inverse_depth_meas_model.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file inverse_depth_meas_model.cc 19 | * @author W. Nicholas Greene 20 | * @date 2016-07-19 21:03:02 (Tue) 21 | */ 22 | 23 | #include "flame/stereo/inverse_depth_meas_model.h" 24 | 25 | #include 26 | 27 | #include "flame/utils/assert.h" 28 | #include "flame/utils/image_utils.h" 29 | 30 | namespace flame { 31 | 32 | namespace stereo { 33 | 34 | InverseDepthMeasModel::InverseDepthMeasModel(const Eigen::Matrix3f& K, 35 | const Eigen::Matrix3f& Kinv, 36 | const Params& params) : 37 | inited_geo_(false), 38 | inited_imgs_(false), 39 | params_(params), 40 | img_ref_(), 41 | img_cmp_(), 42 | gradx_ref_(), 43 | grady_ref_(), 44 | gradx_cmp_(), 45 | grady_cmp_(), 46 | T_ref_to_cmp_(), 47 | epigeo_(K, Kinv) {} 48 | 49 | bool InverseDepthMeasModel::idepth(const cv::Point2f& u_ref, 50 | const cv::Point2f& u_cmp, 51 | float* mu, float* var) const { 52 | FLAME_ASSERT(inited_geo_ && inited_imgs_); 53 | 54 | // Compute the inverse depth mean. 55 | cv::Point2f u_inf, epi; 56 | float disp = epigeo_.disparity(u_ref, u_cmp, &u_inf, &epi); 57 | 58 | // printf("epi = %f, %f\n", epi.x, epi.y); 59 | // printf("disp = %f\n", disp); 60 | 61 | if (disp < 1e-3) { 62 | if (params_.verbose) { 63 | fprintf(stderr, "IDepthMeasModel[ERROR]: NegativeDisparity: u_ref = (%f, %f), u_cmp = (%f, %f), u_inf = (%f, %f), disp = %f\n", 64 | u_ref.x, u_ref.y, u_cmp.x, u_cmp.y, u_inf.x, u_inf.y, disp); 65 | } 66 | 67 | // No dispariy = idepth information. 68 | *mu = 0.0f; 69 | *var = 1e10; 70 | return false; 71 | } 72 | 73 | *mu = epigeo_.disparityToInverseDepth(u_ref, u_inf, epi, disp); 74 | if (*mu < 0.0f) { 75 | if (params_.verbose) { 76 | fprintf(stderr, "IDepthMeasModel[ERROR]: NegativeIDepth: u_ref = (%f, %f), u_cmp = (%f, %f), u_inf = (%f, %f), disp = %f, idepth = %f\n", 77 | u_ref.x, u_ref.y, u_cmp.x, u_cmp.y, u_inf.x, u_inf.y, disp, *mu); 78 | } 79 | 80 | // No depth information. 81 | *mu = 0.0f; 82 | *var = 1e10; 83 | return false; 84 | } 85 | 86 | // Get the image gradient. 87 | cv::Point2f offset(params_.win_size/2+1, params_.win_size/2+1); 88 | float gx = utils::bilinearInterp(gradx_cmp_, 89 | u_cmp.x + offset.x, 90 | u_cmp.y + offset.y); 91 | float gy = utils::bilinearInterp(grady_cmp_, 92 | u_cmp.x + offset.x, 93 | u_cmp.y + offset.y); 94 | 95 | float gnorm = sqrt(gx * gx + gy * gy); 96 | if (gnorm < 1e-3) { 97 | if (params_.verbose) { 98 | fprintf(stderr, "IDepthMeasModel[ERROR]: NoGradient: u_ref = (%f, %f), u_cmp = (%f, %f), u_inf = (%f, %f), disp = %f, idepth = %f, grad = (%f, %f)\n", 99 | u_ref.x, u_ref.y, u_cmp.x, u_cmp.y, u_inf.x, u_inf.y, disp, *mu, gx, gy); 100 | } 101 | 102 | // No depth information. 103 | *mu = 0.0f; 104 | *var = 1e10; 105 | return false; 106 | } 107 | 108 | float ngx = gx / gnorm; 109 | float ngy = gy / gnorm; 110 | 111 | // printf("grad = %f, %f\n", gx, gy); 112 | 113 | // Compute geometry disparity variance. 114 | float epi_dot_ngrad = ngx * epi.x + ngy * epi.y; 115 | float geo_var = params_.epipolar_line_var / (epi_dot_ngrad * epi_dot_ngrad); 116 | 117 | if (utils::fast_abs(epi_dot_ngrad) < 1e-3) { 118 | if (params_.verbose) { 119 | fprintf(stderr, "IDepthMeasModel[ERROR]: NoEpiDotGradient: u_ref = (%f, %f), u_cmp = (%f, %f), u_inf = (%f, %f), disp = %f, idepth = %f, grad = (%f, %f), epi = (%f, %f)\n", 120 | u_ref.x, u_ref.y, u_cmp.x, u_cmp.y, u_inf.x, u_inf.y, disp, *mu, gx, gy, epi.x, epi.y); 121 | } 122 | 123 | // No depth information. 124 | *mu = 0.0f; 125 | *var = 1e10; 126 | return false; 127 | } 128 | 129 | // Compute photometric disparity variance. 130 | float epi_dot_grad = gx * epi.x + gy * epi.y; 131 | float photo_var = 2 * params_.pixel_var / (epi_dot_grad * epi_dot_grad); 132 | 133 | // Compute disparity to inverse depth scaling factor. 134 | float disp_min = disp - disp / 10; 135 | float disp_max = disp + disp / 10; 136 | float idepth_min = epigeo_.disparityToInverseDepth(u_ref, u_inf, epi, disp_min); 137 | float idepth_max = epigeo_.disparityToInverseDepth(u_ref, u_inf, epi, disp_max); 138 | 139 | float alpha = (idepth_max - idepth_min) / (disp_max - disp_min); 140 | 141 | float meas_var = alpha * alpha * (geo_var + photo_var); 142 | 143 | FLAME_ASSERT(!std::isnan(meas_var)); 144 | FLAME_ASSERT(!std::isinf(meas_var)); 145 | 146 | // printf("var1 = %f\n", meas_var); 147 | 148 | // float angle = Eigen::AngleAxisf(T_ref_to_cmp_.unit_quaternion()).angle(); 149 | // float baseline = T_ref_to_cmp_.translation().squaredNorm(); 150 | // meas_var = 0.000001f / baseline + 0.0001f / cos(angle); 151 | *var = meas_var; 152 | 153 | // printf("var2 = %f\n", meas_var); 154 | 155 | return true; 156 | } 157 | 158 | } // namespace stereo 159 | 160 | } // namespace flame 161 | -------------------------------------------------------------------------------- /src/flame/params.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file params.h 19 | * @author W. Nicholas Greene 20 | * @date 2017-03-30 19:20:57 (Thu) 21 | */ 22 | 23 | #pragma once 24 | 25 | #include "flame/stereo/inverse_depth_filter.h" 26 | #include "flame/stereo/inverse_depth_meas_model.h" 27 | #include "flame/stereo/line_stereo.h" 28 | 29 | #include "flame/optimizers/nltgv2_l1_graph_regularizer.h" 30 | 31 | namespace flame { 32 | 33 | /** 34 | * @brief Parameter struct. 35 | */ 36 | struct Params { 37 | Params() {} 38 | // Threshold on gradient magnitude. 39 | float min_grad_mag = 5.0f; 40 | 41 | // Screen out low-gradient points after projecting to new image. 42 | bool do_grad_check_after_projection = false; 43 | 44 | int num_levels = 5; // Number of pyramid levels. 45 | 46 | // Feature Detection params. 47 | bool continuous_detection = true; // Continually detect features. 48 | int detection_win_size = 16; // Features are detected on grid with cells of size (win_size x win_size). 49 | bool do_letterbox = false; // Only process middle rows of image. 50 | float min_error = 100.0f; 51 | 52 | int photo_error_num_pfs = 30; // Number of pfs to consider for photo error computation. 53 | 54 | // Measurement model params. 55 | stereo::InverseDepthMeasModel::Params zparams; 56 | 57 | // IDepth filter params. 58 | float rescale_factor_min = 0.7f; 59 | float rescale_factor_max = 1.4f; 60 | float idepth_init = 0.01f; 61 | float idepth_var_init = 0.5f * 0.5f; 62 | float idepth_var_max = 0.5f * 0.5f; // Vertex is removed if variance exceeds this. 63 | int max_dropouts = 5; // Maximum number of failed tracking attempts. 64 | float outlier_sigma_thresh = 3.0; // Measurements >= this are rejected. 65 | float min_baseline = 0.01f; // Min baseline for idepth update. 66 | bool do_meas_fusion = true; // If false, measurements will not be fused. 67 | stereo::inverse_depth_filter::Params fparams; 68 | 69 | // Filter oblique triangles (only for display purposes). 70 | bool do_oblique_triangle_filter = true; 71 | float oblique_normal_thresh = 1.39626; // 80 degrees. Max angle between 72 | // surface normal and viewing ray. 73 | float oblique_idepth_diff_factor = 0.35f; // Filter triangle if (max_idepth - 74 | // min_idepth)/max_idepth is greater 75 | // than thresh. 76 | float oblique_idepth_diff_abs = 0.1f; // Filter triangle if (max_idepth - 77 | // min_idepth) is greater than thresh. 78 | 79 | // Filter triangles that have very long edges (only for display purposes). 80 | bool do_edge_length_filter = true; 81 | float edge_length_thresh = 0.333f; // As fraction of image width. 82 | 83 | // Filter triangles that are far away (only for display purposes). 84 | bool do_idepth_triangle_filter = true; 85 | float min_triangle_idepth = 0.01f; 86 | 87 | // Regularizer params. 88 | float min_height = 0.1f; 89 | float max_height = 4; 90 | float idepth_var_max_graph = 1e-2f; // Max var of feat to add to graph. 91 | bool adaptive_data_weights = false; // Set data weights to 1/var instead of 1. 92 | bool init_with_prediction = false; // Initialize idepth using predicted value. 93 | bool rescale_data = false; 94 | bool check_sticky_obstacles = false; // Check if idepths are being sucked towards the camera because of sticky obstacles. 95 | bool do_nltgv2 = true; 96 | optimizers::nltgv2_l1_graph_regularizer::Params rparams; 97 | 98 | // OpenMP params. 99 | int omp_num_threads = 4; 100 | int omp_chunk_size = 256; 101 | 102 | // Stereo params. 103 | stereo::line_stereo::Params sparams; 104 | 105 | // Used to scale idepths before they are colormapped. For example, if 106 | // average scene depth is >> 1m, then coloring by idepth will not provide 107 | // much dynamic range. Coloring by scene_color_scale * idepth (if 108 | // scene_color_scale > 1) is much more informative. 109 | float scene_color_scale = 1.0f; 110 | 111 | // Debugging parameters. 112 | bool debug_quiet = false; 113 | bool debug_print_timing_update = true; 114 | bool debug_print_timing_update_locking = true; 115 | bool debug_print_timing_frame_creation = true; 116 | bool debug_print_timing_gradients = true; 117 | bool debug_print_timing_poseframe = true; 118 | bool debug_print_timing_detection = true; 119 | bool debug_print_timing_detection_loop = true; 120 | bool debug_print_timing_graph_sync_loop = true; 121 | bool debug_print_timing_update_idepths = true; 122 | bool debug_print_timing_project_features = true; 123 | bool debug_print_timing_project_graph = true; 124 | bool debug_print_timing_sync_graph = true; 125 | bool debug_print_timing_triangulate = true; 126 | bool debug_print_timing_normals = true; 127 | bool debug_print_timing_interpolate = true; 128 | bool debug_print_timing_median_filter = true; 129 | bool debug_print_timing_lowpass_filter = true; 130 | bool debug_print_timing_oblique_triangle_filter = true; 131 | bool debug_print_timing_edge_length_filter = true; 132 | bool debug_print_timing_idepth_triangle_filter = true; 133 | bool debug_print_verbose_errors = false; 134 | bool debug_draw_detections = false; 135 | bool debug_draw_wireframe = false; 136 | bool debug_draw_features = false; 137 | bool debug_draw_matches = false; 138 | bool debug_draw_photo_error = false; 139 | bool debug_draw_normals = false; 140 | bool debug_draw_idepthmap = false; 141 | bool debug_draw_text_overlay = true; 142 | bool debug_flip_images = false; 143 | }; 144 | 145 | } // namespace flame 146 | -------------------------------------------------------------------------------- /src/flame/optimizers/nltgv2_l1_graph_regularizer.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file nltgv2_l1_graph_regularizer.cc 19 | * @author W. Nicholas Greene 20 | * @date 2016-10-06 20:43:34 (Thu) 21 | */ 22 | 23 | #include "flame/optimizers/nltgv2_l1_graph_regularizer.h" 24 | 25 | #include 26 | 27 | namespace flame { 28 | 29 | namespace optimizers { 30 | 31 | namespace nltgv2_l1_graph_regularizer { 32 | 33 | void step(const Params& params, Graph* graph) { 34 | // Save previous solution. 35 | Graph::vertex_iterator vit, end; 36 | boost::tie(vit, end) = boost::vertices(*graph); 37 | for ( ; vit != end; ++vit) { 38 | VertexData& vtx = (*graph)[*vit]; 39 | vtx.x_prev = vtx.x; 40 | vtx.w1_prev = vtx.w1; 41 | vtx.w2_prev = vtx.w2; 42 | } 43 | 44 | internal::dualStep(params, graph); 45 | internal::primalStep(params, graph); 46 | internal::extraGradientStep(params, graph); 47 | 48 | return; 49 | } 50 | 51 | float smoothnessCost(const Params& params, const Graph& graph) { 52 | float cost = 0.0f; 53 | 54 | auto fast_abs = [](float a) { return (a >= 0) ? a : -a; }; 55 | 56 | Graph::edge_iterator eit, end; 57 | boost::tie(eit, end) = boost::edges(graph); 58 | for ( ; eit != end; ++eit) { 59 | const EdgeData& edge = graph[*eit]; 60 | const VertexData& vtx_ii = graph[boost::source(*eit, graph)]; 61 | const VertexData& vtx_jj = graph[boost::target(*eit, graph)]; 62 | 63 | cv::Point2f xy_diff = vtx_ii.pos - vtx_jj.pos; 64 | cost += edge.alpha * fast_abs(vtx_ii.x - vtx_jj.x - 65 | vtx_ii.w1 * xy_diff.x - vtx_ii.w2 * xy_diff.y); 66 | cost += edge.beta * fast_abs(vtx_ii.w1 - vtx_jj.w1) + 67 | edge.beta * fast_abs(vtx_ii.w2 - vtx_jj.w2); 68 | } 69 | 70 | return params.data_factor * cost; 71 | } 72 | 73 | float dataCost(const Params& params, const Graph& graph) { 74 | float cost = 0.0f; 75 | Graph::vertex_iterator vit, end; 76 | boost::tie(vit, end) = boost::vertices(graph); 77 | for ( ; vit != end; ++vit) { 78 | const VertexData& vtx = graph[*vit]; 79 | float diff = (vtx.x - vtx.data_term) * vtx.data_weight; 80 | diff = (diff > 0) ? diff : -diff; 81 | cost += diff; 82 | } 83 | 84 | return cost; 85 | } 86 | 87 | namespace internal { 88 | 89 | void dualStep(const Params& params, Graph* graph) { 90 | // Iterate over edges. 91 | Graph::edge_iterator eit, end; 92 | boost::tie(eit, end) = boost::edges(*graph); 93 | for ( ; eit != end; ++eit) { 94 | EdgeData& edge = (*graph)[*eit]; 95 | VertexData& vtx_ii = (*graph)[boost::source(*eit, *graph)]; 96 | VertexData& vtx_jj = (*graph)[boost::target(*eit, *graph)]; 97 | 98 | // Update q1. 99 | float K1x = edge.alpha * (vtx_ii.x_bar - vtx_jj.x_bar); 100 | K1x -= edge.alpha * (vtx_ii.pos.x - vtx_jj.pos.x) * vtx_ii.w1_bar; 101 | K1x -= edge.alpha * (vtx_ii.pos.y - vtx_jj.pos.y) * vtx_ii.w2_bar; 102 | edge.q1 = proxNLTGV2Conj(params.step_q, edge.q1 + params.step_q * K1x); 103 | 104 | // Update q2. 105 | float K2x = edge.beta * (vtx_ii.w1_bar - vtx_jj.w1_bar); 106 | edge.q2 = proxNLTGV2Conj(params.step_q, edge.q2 + params.step_q * K2x); 107 | 108 | // Update q3. 109 | float K3x = edge.beta * (vtx_ii.w2_bar - vtx_jj.w2_bar); 110 | edge.q3 = proxNLTGV2Conj(params.step_q, edge.q3 + params.step_q * K3x); 111 | } 112 | 113 | return; 114 | } 115 | 116 | void primalStep(const Params& params, Graph* graph) { 117 | // Iterate over edges. 118 | Graph::edge_iterator eit, end; 119 | boost::tie(eit, end) = boost::edges(*graph); 120 | for ( ; eit != end; ++eit) { 121 | EdgeData& edge = (*graph)[*eit]; 122 | VertexData& vtx_ii = (*graph)[boost::source(*eit, *graph)]; 123 | VertexData& vtx_jj = (*graph)[boost::target(*eit, *graph)]; 124 | 125 | // Apply updates that require q1. 126 | vtx_ii.x -= edge.q1 * params.step_x * edge.alpha; 127 | vtx_jj.x += edge.q1 * params.step_x * edge.alpha; 128 | 129 | vtx_ii.w1 += edge.q1 * params.step_x * edge.alpha * 130 | (vtx_ii.pos.x - vtx_jj.pos.x); 131 | 132 | vtx_ii.w2 += edge.q1 * params.step_x * edge.alpha * 133 | (vtx_ii.pos.y - vtx_jj.pos.y); 134 | 135 | // Apply updates that require q2. 136 | vtx_ii.w1 -= edge.q2 * params.step_x * edge.beta; 137 | vtx_jj.w1 += edge.q2 * params.step_x * edge.beta; 138 | 139 | // Apply updates that require q3. 140 | vtx_ii.w2 -= edge.q3 * params.step_x * edge.beta; 141 | vtx_jj.w2 += edge.q3 * params.step_x * edge.beta; 142 | } 143 | 144 | // Apply proximal operator to each vertex. 145 | Graph::vertex_iterator vit, vend; 146 | boost::tie(vit, vend) = boost::vertices(*graph); 147 | for ( ; vit != vend; ++vit) { 148 | VertexData& vtx = (*graph)[*vit]; 149 | vtx.x = proxL1(params.x_min, params.x_max, params.step_x, 150 | params.data_factor * vtx.data_weight, vtx.x, vtx.data_term); 151 | } 152 | 153 | return; 154 | } 155 | 156 | void extraGradientStep(const Params& params, Graph* graph) { 157 | // Iteratate over vertices. 158 | Graph::vertex_iterator vit, end; 159 | boost::tie(vit, end) = boost::vertices(*graph); 160 | for ( ; vit != end; ++vit) { 161 | VertexData& vtx = (*graph)[*vit]; 162 | float new_x_bar = vtx.x + params.theta * (vtx.x - vtx.x_prev); 163 | 164 | // Project back onto the feasible set. 165 | new_x_bar = (new_x_bar < params.x_min) ? params.x_min : new_x_bar; 166 | new_x_bar = (new_x_bar > params.x_max) ? params.x_max : new_x_bar; 167 | vtx.x_bar = new_x_bar; 168 | 169 | vtx.w1_bar = vtx.w1 + params.theta * (vtx.w1 - vtx.w1_prev); 170 | vtx.w2_bar = vtx.w2 + params.theta * (vtx.w2 - vtx.w2_prev); 171 | } 172 | 173 | return; 174 | } 175 | 176 | } // namespace internal 177 | 178 | } // namespace nltgv2_l1_graph_regularizer 179 | 180 | } // namespace optimizers 181 | 182 | } // namespace flame 183 | -------------------------------------------------------------------------------- /test/utils/stats_tracker_test.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file stats_tracker_test.cc 19 | * @author W. Nicholas Greene 20 | * @date 2017-08-18 19:45:15 (Fri) 21 | */ 22 | 23 | #include 24 | 25 | #include "flame/utils/stats_tracker.h" 26 | 27 | #include "gtest/gtest.h" 28 | 29 | namespace flame { 30 | 31 | namespace utils { 32 | 33 | TEST(StatsTrackerTest, ConstructorTest) { 34 | StatsTracker stats; 35 | return; 36 | } 37 | 38 | TEST(StatsTrackerTest, SetTest) { 39 | StatsTracker stats; 40 | 41 | stats.set("num_a"); 42 | EXPECT_EQ(0, stats.stats("num_a")); 43 | 44 | return; 45 | } 46 | 47 | TEST(StatsTrackerTest, AddTest1) { 48 | StatsTracker stats; 49 | 50 | stats.add("num_a", 9); 51 | EXPECT_EQ(9, stats.stats("num_a")); 52 | 53 | return; 54 | } 55 | 56 | TEST(StatsTrackerTest, AddTest2) { 57 | StatsTracker stats; 58 | 59 | stats.add("num_a", 9); 60 | stats.add("num_a", 9); 61 | stats.add("num_a", 9); 62 | 63 | EXPECT_EQ(27, stats.stats("num_a")); 64 | 65 | return; 66 | } 67 | 68 | TEST(StatsTrackerTest, TickTockTest1) { 69 | StatsTracker stats; 70 | 71 | float sleep_length = 1.0f; 72 | 73 | stats.tick("t1"); 74 | sleep(sleep_length); 75 | float ret = stats.tock("t1"); 76 | 77 | float tol = 10.0f; 78 | EXPECT_NEAR(sleep_length * 1000, ret, tol); 79 | 80 | return; 81 | } 82 | 83 | TEST(StatsTrackerTest, TickTockTest2) { 84 | StatsTracker stats; 85 | 86 | float sleep_length = 1.0f; 87 | 88 | stats.tick("t1"); 89 | sleep(sleep_length); 90 | stats.tock("t1"); 91 | 92 | float tol = 10.0f; 93 | EXPECT_NEAR(sleep_length * 1000, stats.timings("t1"), tol); 94 | 95 | return; 96 | } 97 | 98 | TEST(StatsTrackerTest, SetPrefixTest) { 99 | StatsTracker stats("a/"); 100 | 101 | stats.set("num_a"); 102 | EXPECT_EQ(0, stats.stats("num_a")); 103 | 104 | return; 105 | } 106 | 107 | TEST(StatsTrackerTest, AddPrefixTest1) { 108 | StatsTracker stats("a/"); 109 | 110 | stats.add("num_a", 9); 111 | 112 | EXPECT_EQ(9, stats.stats("num_a")); 113 | 114 | return; 115 | } 116 | 117 | TEST(StatsTrackerTest, AddPrefixTest2) { 118 | StatsTracker stats("a/"); 119 | 120 | stats.add("num_a", 9); 121 | stats.add("num_a", 9); 122 | stats.add("num_a", 9); 123 | 124 | EXPECT_EQ(27, stats.stats("num_a")); 125 | 126 | return; 127 | } 128 | 129 | TEST(StatsTrackerTest, TickTockPrefixTest1) { 130 | StatsTracker stats("a/"); 131 | 132 | float sleep_length = 1.0f; 133 | 134 | stats.tick("t1"); 135 | sleep(sleep_length); 136 | float ret = stats.tock("t1"); 137 | 138 | float tol = 10.0f; 139 | EXPECT_NEAR(sleep_length * 1000, ret, tol); 140 | 141 | return; 142 | } 143 | 144 | TEST(StatsTrackerTest, TickTockPrefixTest2) { 145 | StatsTracker stats("a/"); 146 | 147 | float sleep_length = 1.0f; 148 | 149 | stats.tick("t1"); 150 | sleep(sleep_length); 151 | stats.tock("t1"); 152 | 153 | float tol = 10.0f; 154 | EXPECT_NEAR(sleep_length * 1000, stats.timings("t1"), tol); 155 | 156 | return; 157 | } 158 | 159 | TEST(StatsTrackerTest, AddThreadTest1) { 160 | StatsTracker stats; 161 | 162 | std::thread t1([&stats]() { 163 | stats.add("a", 1); 164 | }); 165 | 166 | stats.add("a", 1); 167 | 168 | t1.join(); 169 | 170 | EXPECT_EQ(2, stats.stats("a")); 171 | 172 | return; 173 | } 174 | 175 | TEST(StatsTrackerTest, AddThreadTest2) { 176 | StatsTracker stats; 177 | 178 | std::thread t1([&stats]() { 179 | stats.add("a", 1); 180 | }); 181 | 182 | stats.add("b", 1); 183 | 184 | t1.join(); 185 | 186 | EXPECT_EQ(1, stats.stats("a")); 187 | EXPECT_EQ(1, stats.stats("b")); 188 | 189 | return; 190 | } 191 | 192 | TEST(StatsTrackerTest, TickTockThreadTest1) { 193 | StatsTracker stats; 194 | 195 | float sleep_length = 1.0f; 196 | 197 | std::thread t1([&stats, sleep_length]() { 198 | stats.tick("a"); 199 | sleep(sleep_length); 200 | stats.tock("a"); 201 | }); 202 | 203 | stats.tick("a"); 204 | sleep(sleep_length); 205 | stats.tock("a"); 206 | 207 | t1.join(); 208 | 209 | float tol = 10.0f; 210 | EXPECT_NEAR(sleep_length * 1000, stats.timings("a"), tol); 211 | 212 | return; 213 | } 214 | 215 | TEST(StatsTrackerTest, TickTockThreadTest2) { 216 | StatsTracker stats; 217 | 218 | float sleep_length = 1.0f; 219 | 220 | std::thread t1([&stats, sleep_length]() { 221 | stats.tick("a"); 222 | sleep(sleep_length); 223 | stats.tock("a"); 224 | }); 225 | 226 | stats.tick("b"); 227 | sleep(sleep_length); 228 | stats.tock("b"); 229 | 230 | t1.join(); 231 | 232 | float tol = 10.0f; 233 | EXPECT_NEAR(sleep_length * 1000, stats.timings("a"), tol); 234 | EXPECT_NEAR(sleep_length * 1000, stats.timings("b"), tol); 235 | 236 | return; 237 | } 238 | 239 | TEST(StatsTrackerTest, StatsCopyTest1) { 240 | StatsTracker stats; 241 | 242 | stats.add("a", 10); 243 | 244 | auto sstats = stats.stats(); 245 | 246 | stats.add("a", 10); 247 | 248 | EXPECT_EQ(10, sstats["a"]); 249 | EXPECT_EQ(20, stats.stats("a")); 250 | 251 | return; 252 | } 253 | 254 | TEST(StatsTrackerTest, StatsCopyTest2) { 255 | StatsTracker stats; 256 | 257 | stats.add("a", 10); 258 | 259 | auto sstats = stats.stats(); 260 | 261 | stats.add("b", 9); 262 | 263 | EXPECT_EQ(10, sstats["a"]); 264 | EXPECT_EQ(9, stats.stats("b")); 265 | EXPECT_EQ(1, sstats.size()); 266 | 267 | return; 268 | } 269 | 270 | TEST(StatsTrackerTest, TimingsCopyTest1) { 271 | StatsTracker stats; 272 | 273 | float sleep_length = 1.0f; 274 | 275 | stats.tick("a"); 276 | sleep(sleep_length); 277 | stats.tock("a"); 278 | 279 | auto timings = stats.timings(); 280 | 281 | stats.tick("a"); 282 | sleep(2*sleep_length); 283 | stats.tock("a"); 284 | 285 | float tol = 10.0f; 286 | EXPECT_NEAR(sleep_length * 1000, timings["a"], tol); 287 | EXPECT_NEAR(2 * sleep_length * 1000, stats.timings("a"), tol); 288 | 289 | return; 290 | } 291 | 292 | } // namespace utils 293 | 294 | } // namespace flame 295 | -------------------------------------------------------------------------------- /src/flame/optimizers/nltgv2_l1_graph_regularizer.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file nltgv2_l1_graph_regularizer.h 19 | * @author W. Nicholas Greene 20 | * @date 2016-10-06 20:43:29 (Thu) 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | #include "flame/utils/image_utils.h" 31 | 32 | namespace flame { 33 | 34 | namespace optimizers { 35 | 36 | /** 37 | * @namespace This namespace includes types and functions to minimize a 38 | * NLTGV2-L1 problem defined on a Boost graph/mesh. 39 | * 40 | * An NLTGV2-L1 problem is a cost functional of the following form: 41 | * 42 | * min_x F(Kx) + G(x) 43 | * 44 | * x = Primal variable 45 | * q = Dual variable 46 | * K = Continuous, linear operator between X and Q 47 | * G = Convex lower-semicontinuous (lsc) function 48 | * F = Convex lsc function. 49 | * F* = Convex conjugate of F (also convex lsc function) 50 | * 51 | * Typically the F(Kx) is smoothness term and the G term is a data fitting term. 52 | * 53 | * Here the F(Kx) function is NLTGV2, which stands for Non-Local Total 54 | * Generalized Variation^2, which is a generalized version of the Total 55 | * Variation semi-norm. It promotes solutions that are polynomials of degree < 2 56 | * (i.e. affine or planar solutions). 57 | * 58 | * The G(x) function is (weighted) L1, which measures the L1 norm between the 59 | * primal variable x and observed data. 60 | * 61 | * The optimization is carried out using the Chambolle-Pock primal-dual 62 | * algorithm (2010). See Bredies 2010, Ranftl 2014, Pinies 2015 for more 63 | * information. 64 | * 65 | * The Boost Graph Library is used to set up the problem. To use, create a Graph 66 | * object and add vertices and edges as needed. Run the step(...) function to 67 | * perform an optimization step. 68 | */ 69 | namespace nltgv2_l1_graph_regularizer { 70 | 71 | /** 72 | * @brief Struct that defines a vertex in the graph. 73 | */ 74 | struct VertexData { 75 | cv::Point2f pos; // Position. 76 | float x = 0.0f; // Main primal variable. 77 | float w1 = 0.0f; // Plane parameters. 78 | float w2 = 0.0f;; 79 | 80 | float x_bar = 0.0f; // Extragradient varaibles. 81 | float w1_bar = 0.0f; 82 | float w2_bar = 0.0f; 83 | 84 | float x_prev = 0.0f; // Previous x value. 85 | float w1_prev = 0.0f; 86 | float w2_prev = 0.0f; 87 | 88 | float data_term = 0.0f; // Data term. 89 | float data_weight = 1.0f; // Weight on data term. 90 | }; 91 | 92 | /** 93 | * @brief Struct that defines an edge in the graph. 94 | */ 95 | struct EdgeData { 96 | float alpha = 1.0f; // Edge weights. 97 | float beta = 1.0f; 98 | float q1 = 0.0f; // Dual variables. 99 | float q2 = 0.0f; 100 | float q3 = 0.0f; 101 | bool valid = true; // False if this edge should be removed. 102 | }; 103 | 104 | /** 105 | * @brief Graph representation using the Boost Graph Library. 106 | */ 107 | using Graph = 108 | boost::adjacency_list; // Data stored at each edge 113 | 114 | // These descriptors are essentially handles to the vertices and edges. 115 | using VertexHandle = boost::graph_traits::vertex_descriptor; 116 | using EdgeHandle = boost::graph_traits::edge_descriptor; 117 | 118 | /** 119 | * @brief Parameter struct. 120 | */ 121 | struct Params { 122 | float data_factor = 0.1f; // lambda in the TV literature. 123 | float step_x = 0.001f; // Primal step size. 124 | float step_q = 125.0f; // Dual step size. 125 | float theta = 0.25f; // Extra gradient step size. 126 | 127 | float x_min = 0.0f; // Feasible set. 128 | float x_max = 10.0f; 129 | }; 130 | 131 | /** 132 | * @brief Performs a full optimization step. 133 | */ 134 | void step(const Params& params, Graph* graph); 135 | 136 | /** 137 | * @brief Return the smoothness cost (i.e. the NLTGV2 part). 138 | */ 139 | float smoothnessCost(const Params& params, const Graph& graph); 140 | 141 | /** 142 | * @brief Return the data cost (i.e. the weighted L1 part). 143 | */ 144 | float dataCost(const Params& params, const Graph& graph); 145 | 146 | /** 147 | * @brief Return the total cost of the current solution. 148 | */ 149 | inline float cost(const Params& params, const Graph& graph) { 150 | return smoothnessCost(params, graph) + dataCost(params, graph); 151 | } 152 | 153 | namespace internal { 154 | 155 | /** 156 | * @brief Perform a gradient ascent step in the dual. 157 | */ 158 | void dualStep(const Params& params, Graph* graph); 159 | 160 | /** 161 | * @brief Perform a gradient descent step in the primal. 162 | */ 163 | void primalStep(const Params& params, Graph* graph); 164 | 165 | /** 166 | * @brief Perform an extra-gradient step in the primal. 167 | */ 168 | void extraGradientStep(const Params& params, Graph* graph); 169 | 170 | // Proximal operator for convex conjugate of NLTGV2 regularizer. 171 | inline float proxNLTGV2Conj(float step, float q) { 172 | float absq = utils::fast_abs(q); 173 | float new_q = q / (absq > 1 ? absq : 1); 174 | FLAME_ASSERT(!std::isnan(new_q)); 175 | return new_q; 176 | } 177 | 178 | // Proximal operator for L2 data term. 179 | inline float proxL1(float x_min, float x_max, float step_x, float data_weight, 180 | float x, float data) { 181 | float diff = x - data; 182 | float thresh = step_x * data_weight; 183 | 184 | float new_x = 0.0f; 185 | if (diff > thresh) { 186 | new_x = x - thresh; 187 | } else if (diff < -thresh) { 188 | new_x = x + thresh; 189 | } else { 190 | new_x = data; 191 | } 192 | 193 | // Project back onto the feasible set. 194 | new_x = (new_x < x_min) ? x_min : new_x; 195 | new_x = (new_x > x_max) ? x_max : new_x; 196 | return new_x; 197 | } 198 | 199 | } // namespace internal 200 | 201 | } // namespace nltgv2_l1_graph_regularizer 202 | 203 | } // namespace optimizers 204 | 205 | } // namespace flame 206 | -------------------------------------------------------------------------------- /src/flame/utils/rasterization.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file rasterization.cc 19 | * @author W. Nicholas Greene 20 | * @date 2017-08-18 19:05:24 (Fri) 21 | */ 22 | 23 | #include 24 | #include 25 | 26 | #include "flame/utils/rasterization.h" 27 | 28 | namespace flame { 29 | 30 | namespace utils { 31 | 32 | void DrawLineInterpolated(cv::Point p1, cv::Point p2, 33 | float v1, float v2, cv::Mat* img) { 34 | float length = sqrt((p2.x - p1.x)*(p2.x - p1.x) + (p2.y - p1.y)*(p2.y - p1.y)); 35 | float slope = (v2 - v1)/length; 36 | 37 | float val = v1; 38 | cv::LineIterator it(*img, p1, p2); 39 | int count = it.count; 40 | 41 | for (int ii = 0; ii < count; ++ii) { 42 | float* ptr = reinterpret_cast(*it); 43 | *ptr = val; 44 | val += slope; 45 | it++; 46 | } 47 | 48 | return; 49 | } 50 | 51 | void DrawTopFlatShadedTriangle(cv::Point p1, cv::Point p2, cv::Point p3, 52 | float v1, float v2, float v3, cv::Mat*img) { 53 | // Check for special case (three vertices are adjacent, don't waste time 54 | // interpolating). 55 | if ((abs(p2.x - p1.x) == 1) && (abs(p3.y - p2.y) == 1)) { 56 | img->at(p1.y, p1.x) = v1; 57 | img->at(p2.y, p2.x) = v2; 58 | img->at(p3.y, p3.x) = v3; 59 | return; 60 | } 61 | 62 | cv::LineIterator it1(*img, p1, p3); 63 | cv::LineIterator it2(*img, p2, p3); 64 | 65 | float slope1 = (v3 - v1)/(p3.y - p1.y); 66 | float slope2 = (v3 - v2)/(p3.y - p2.y); 67 | 68 | float val1 = v1; 69 | float val2 = v2; 70 | 71 | for (int y = p1.y; y <= p3.y; ++y) { 72 | // Walk along the line from p1-p2 until we get to y. 73 | while (it1.pos().y < y) { 74 | it1++; 75 | } 76 | 77 | // Walk along the line from p2-p3 until we get to y. 78 | while (it2.pos().y < y) { 79 | it2++; 80 | } 81 | 82 | // Fill this row of the triangle. 83 | DrawLineInterpolated(it1.pos(), it2.pos(), val1, val2, img); 84 | 85 | // Increment values. 86 | val1 += slope1; 87 | val2 += slope2; 88 | } 89 | 90 | return; 91 | } 92 | 93 | void DrawBottomFlatShadedTriangle(cv::Point p1, cv::Point p2, cv::Point p3, 94 | float v1, float v2, float v3, cv::Mat*img) { 95 | // Check for special case (three vertices are adjacent, don't waste time 96 | // interpolating). 97 | if ((abs(p2.y - p1.y) == 1) && (abs(p3.x - p2.x) == 1)) { 98 | img->at(p1.y, p1.x) = v1; 99 | img->at(p2.y, p2.x) = v2; 100 | img->at(p3.y, p3.x) = v3; 101 | return; 102 | } 103 | 104 | cv::LineIterator it1(*img, p1, p2); 105 | cv::LineIterator it2(*img, p1, p3); 106 | 107 | float slope1 = (v2 - v1)/(p2.y - p1.y); 108 | float slope2 = (v3 - v1)/(p3.y - p1.y); 109 | 110 | float val1 = v1; 111 | float val2 = v1; 112 | 113 | for (int y = p1.y; y <= p3.y; ++y) { 114 | // Walk along the line from p1-p2 until we get to y. 115 | while (it1.pos().y < y) { 116 | it1++; 117 | } 118 | 119 | // Walk along the line from p2-p3 until we get to y. 120 | while (it2.pos().y < y) { 121 | it2++; 122 | } 123 | 124 | // Fill this row of the triangle. 125 | DrawLineInterpolated(it1.pos(), it2.pos(), val1, val2, img); 126 | 127 | // Increment values. 128 | val1 += slope1; 129 | val2 += slope2; 130 | } 131 | 132 | return; 133 | } 134 | 135 | void DrawShadedTriangle(cv::Point p1, cv::Point p2, cv::Point p3, 136 | float v1, float v2, float v3, cv::Mat* img) { 137 | SortVertices(&p1, &p2, &p3); 138 | 139 | if (p1.y == p2.y) { 140 | DrawTopFlatShadedTriangle(p1, p2, p3, v1, v2, v3, img); 141 | } else if (p2.y == p3.y) { 142 | DrawBottomFlatShadedTriangle(p1, p2, p3, v1, v2, v3, img); 143 | } else { 144 | // Split triangle and draw the top and bottom halves. 145 | float point_slope = static_cast(p3.x - p1.x) / (p3.y - p1.y); 146 | int x4 = static_cast(p1.x + point_slope * (p2.y - p1.y)); 147 | cv::Point p4(x4, p2.y); 148 | 149 | float length = sqrt((p3.x - p1.x)*(p3.x - p1.x) + (p3.y - p1.y)*(p3.y - p1.y)); 150 | float val_slope = (v3 - v1) / length; 151 | float l = sqrt((p3.x - p4.x)*(p3.x - p4.x) + (p3.y - p4.y)*(p3.y - p4.y)); 152 | float v4 = v1 + val_slope * l; 153 | 154 | // printf("v4 = %f\n", v4); 155 | // printf("val_slope = %f, l = %f, length = %f\n", val_slope, l, length); 156 | 157 | DrawBottomFlatShadedTriangle(p1, p2, p4, v1, v2, v4, img); 158 | DrawTopFlatShadedTriangle(p2, p4, p3, v2, v4, v3, img); 159 | } 160 | 161 | return; 162 | } 163 | 164 | void DrawShadedTriangleBarycentric(cv::Point p1, cv::Point p2, cv::Point p3, 165 | float v1, float v2, float v3, cv::Mat* img) { 166 | // Compute triangle bounding box 167 | int xmin = min3(p1.x, p2.x, p3.x); 168 | int ymin = min3(p1.y, p2.y, p3.y); 169 | int xmax = max3(p1.x, p2.x, p3.x); 170 | int ymax = max3(p1.y, p2.y, p3.y); 171 | 172 | cv::Point p(xmin, ymin); 173 | Edge e12, e23, e31; 174 | 175 | // __m128 is the SSE 128-bit packed float type (4 floats). 176 | __m128 w1_row = e23.init(p2, p3, p); 177 | __m128 w2_row = e31.init(p3, p1, p); 178 | __m128 w3_row = e12.init(p1, p2, p); 179 | 180 | // Values as 4 packed floats. 181 | __m128 v14 = _mm_set1_ps(v1); 182 | __m128 v24 = _mm_set1_ps(v2); 183 | __m128 v34 = _mm_set1_ps(v3); 184 | 185 | // Rasterize 186 | for (p.y = ymin; p.y <= ymax; p.y += Edge::stepYSize) { 187 | // Determine barycentric coordinates 188 | __m128 w1 = w1_row; 189 | __m128 w2 = w2_row; 190 | __m128 w3 = w3_row; 191 | 192 | for (p.x = xmin; p.x <= xmax; p.x += Edge::stepXSize) { 193 | // If p is on or inside all edges, render pixel. 194 | __m128 zero = _mm_set1_ps(0.0f); 195 | 196 | // (w1 >= 0) && (w2 >= 0) && (w3 >= 0) 197 | // mask tells whether we should set the pixel. 198 | __m128 mask = _mm_and_ps(_mm_cmpge_ps(w1, zero), 199 | _mm_and_ps(_mm_cmpge_ps(w2, zero), 200 | _mm_cmpge_ps(w3, zero))); 201 | 202 | // w1 + w2 + w3 203 | __m128 norm = _mm_add_ps(w1, _mm_add_ps(w2, w3)); 204 | 205 | // v1*w1 + v2*w2 + v3*w3 / norm 206 | __m128 vals = _mm_div_ps(_mm_add_ps(_mm_mul_ps(v14, w1), 207 | _mm_add_ps(_mm_mul_ps(v24, w2), 208 | _mm_mul_ps(v34, w3))), norm); 209 | 210 | // Grab original data. We need to use different store/load functions if 211 | // the address is not aligned to 16-bytes. 212 | uint32_t addr = sizeof(float)*(p.y*img->cols + p.x); 213 | if (addr % 16 == 0) { 214 | float* img_ptr = reinterpret_cast(&(img->data[addr])); 215 | __m128 data = _mm_load_ps(img_ptr); 216 | 217 | // Set values using mask. 218 | // If mask is true, use vals, otherwise use data. 219 | __m128 res = _mm_or_ps(_mm_and_ps(mask, vals), _mm_andnot_ps(mask, data)); 220 | _mm_store_ps(img_ptr, res); 221 | } else { 222 | // Address is not 16-byte aligned. Need to use special functions to load/store. 223 | float* img_ptr = reinterpret_cast(&(img->data[addr])); 224 | __m128 data = _mm_loadu_ps(img_ptr); 225 | 226 | // Set values using mask. 227 | // If mask is true, use vals, otherwise use data. 228 | __m128 res = _mm_or_ps(_mm_and_ps(mask, vals), _mm_andnot_ps(mask, data)); 229 | _mm_storeu_ps(img_ptr, res); 230 | } 231 | 232 | // One step to the right. 233 | w1 = _mm_add_ps(w1, e23.oneStepX); 234 | w2 = _mm_add_ps(w2, e31.oneStepX); 235 | w3 = _mm_add_ps(w3, e12.oneStepX); 236 | } 237 | 238 | // Row step. 239 | w1_row = _mm_add_ps(w1_row, e23.oneStepY); 240 | w2_row = _mm_add_ps(w2_row, e31.oneStepY); 241 | w3_row = _mm_add_ps(w3_row, e12.oneStepY); 242 | } 243 | 244 | return; 245 | } 246 | 247 | } // namespace utils 248 | 249 | } // namespace flame 250 | -------------------------------------------------------------------------------- /src/flame/utils/keyframe_selector.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file keyframe_selector.cc 19 | * @author W. Nicholas Greene 20 | * @date 2017-08-18 20:36:40 (Fri) 21 | */ 22 | 23 | #include "flame/utils/keyframe_selector.h" 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include "flame/utils/assert.h" 37 | 38 | // Some typedefs 39 | namespace bgeo = boost::geometry; 40 | typedef bgeo::model::d2::point_xy BPoint; 41 | typedef bgeo::model::polygon BPolygon; 42 | 43 | namespace { 44 | 45 | /** 46 | * \brief Fast round to int. 47 | * 48 | * std::round() is notoriously slow (I think because it needs to handle all 49 | * edge cases). This is a faster implementation I found here: 50 | * https://stackoverflow.com/questions/485525/round-for-float-in-c 51 | */ 52 | int fast_roundf(float r) { 53 | return (r > 0.0f) ? (r + 0.5f) : (r - 0.5f); 54 | } 55 | 56 | } // namespace 57 | 58 | namespace flame { 59 | 60 | namespace utils { 61 | 62 | KeyFrameSelector::KeyFrameSelector(const Matrix3f& K, int max_kfs, 63 | float new_kf_thresh) : 64 | kf_times_(), 65 | kf_imgs_(), 66 | kf_poses_(), 67 | K_(K), 68 | Kinv_(K.inverse()), 69 | num_kfs_(0), 70 | max_kfs_(max_kfs), 71 | new_kf_thresh_(new_kf_thresh) {} 72 | 73 | int KeyFrameSelector::select(double new_time, const Image1b& new_img, 74 | const SE3f& new_pose) { 75 | int best_idx = -1; 76 | float best_score = std::numeric_limits::lowest(); 77 | if (num_kfs_ > 0) { 78 | // Walk through keyframes and select best one. 79 | for (int ii = 0; ii < num_kfs_; ++ii) { 80 | float score_ii = score(new_img.cols, new_img.rows, 81 | K_, Kinv_, kf_poses_[ii].inverse() * new_pose); 82 | 83 | bool debug_print_kf_score_info = false; // TODO(wng): Make this dynamic. 84 | if (debug_print_kf_score_info) { 85 | printf("kf score(%i) = %f, num_kfs = %i, %i, baseline = %f\n", 86 | ii, score_ii, num_kfs_, kf_times_.size(), 87 | (new_pose.translation() - kf_poses_[ii].translation()).norm()); 88 | } 89 | 90 | if (score_ii > best_score) { 91 | best_score = score_ii; 92 | best_idx = ii; 93 | } 94 | } 95 | } 96 | 97 | bool debug_print_best_kfs_idx = false; // TODO(wng): Make this dynamic. 98 | if (debug_print_best_kfs_idx) { 99 | printf("best_idx = %i, score = %f\n", best_idx, best_score); 100 | } 101 | 102 | // Pick a new keyframe based on distance from last keyframe. 103 | if ((num_kfs_ == 0) || 104 | ((new_pose.translation() - kf_poses_.back().translation()).norm() > 105 | new_kf_thresh_)) { 106 | // Add keyframe to pool. 107 | kf_times_.push_back(new_time); 108 | kf_imgs_.push_back(new_img); 109 | kf_poses_.push_back(new_pose); 110 | 111 | if (kf_times_.size() > max_kfs_) { 112 | kf_times_.pop_front(); 113 | kf_imgs_.pop_front(); 114 | kf_poses_.pop_front(); 115 | } 116 | 117 | num_kfs_ = kf_times_.size(); 118 | best_idx--; 119 | } 120 | 121 | return best_idx; 122 | } 123 | 124 | void KeyFrameSelector::getKeyFrame(int idx, double* time, Image1b* img, 125 | SE3f* pose) { 126 | FLAME_ASSERT(idx >= 0); 127 | FLAME_ASSERT(idx < max_kfs_); 128 | 129 | *time = kf_times_[idx]; 130 | *img = kf_imgs_[idx]; 131 | *pose = kf_poses_[idx]; 132 | 133 | return; 134 | } 135 | 136 | /** 137 | * Adapted from MobileFusion - Ondruska et al. 138 | */ 139 | float KeyFrameSelector::score(int width, int height, 140 | const Matrix3f& K, const Matrix3f& Kinv, 141 | const SE3f& new_to_ref, 142 | float min_depth, float max_depth, 143 | float max_disparity) { 144 | /*==================== Compute orientation score ====================*/ 145 | // Sparse features are sensitive to rotation. Try to find a keyframe that has 146 | // the same orientation. 147 | float angle = AngleAxisf(new_to_ref.unit_quaternion()).angle(); 148 | float S_orientation = 0.5 * (cos(angle) + 1); // Use cos to get around wrapping to (-pi, pi). 149 | 150 | // Apply hard check for vastly different orientations. 151 | float cos_angle_thresh = 0.5 * (cos(60.0f * M_PI / 180.0f) + 1); 152 | if (S_orientation < cos_angle_thresh) { 153 | // printf("Inward normal does not align with viewing direction from reference image!\n"); 154 | return std::numeric_limits::lowest(); 155 | } 156 | 157 | /*==================== Copmute overlap score ====================*/ 158 | // Assume structure at max_depth relative to new_img. Project into ref_img and 159 | // compute the overlap. 160 | Vector3f u_new_corners[4]; 161 | u_new_corners[0] << 0.0f, 0.0f, 1.0f; 162 | u_new_corners[1] << 0.0f, height-1, 1.0f; 163 | u_new_corners[2] << width-1, height-1, 1.0f; 164 | u_new_corners[3] << width-1, 0.0f, 1.0f; 165 | 166 | Vector3f u_ref_corners[4]; 167 | for (int ii = 0; ii < 4; ++ii) { 168 | u_ref_corners[ii] = (K * (new_to_ref.unit_quaternion() * 169 | (max_depth * (Kinv * u_new_corners[ii])) + 170 | new_to_ref.translation())); 171 | u_ref_corners[ii] /= u_ref_corners[ii](2); 172 | } 173 | 174 | // Compute area of intersection using boost. 175 | // Extra point is needed to close polygon. 176 | std::vector new_pts(5); 177 | std::vector ref_pts(5); 178 | for (int ii = 0; ii < 4; ++ii) { 179 | new_pts[ii] = BPoint(u_new_corners[ii](0), u_new_corners[ii](1)); 180 | ref_pts[ii] = BPoint(u_ref_corners[ii](0), u_ref_corners[ii](1)); 181 | } 182 | new_pts[4] = new_pts[0]; 183 | ref_pts[4] = ref_pts[0]; 184 | 185 | BPolygon new_poly; 186 | bgeo::assign_points(new_poly, new_pts); 187 | BPolygon ref_poly; 188 | bgeo::assign_points(ref_poly, ref_pts); 189 | 190 | float S_overlap = 0.0f; 191 | 192 | // Check if reference polygon (u_ref_corners) intersects itself. 193 | bool ref_poly_self_intersection = bgeo::intersects(ref_poly); 194 | if (ref_poly_self_intersection) { 195 | fprintf(stderr, "KeyFrameSelector: Reference polygon self-intersection detected!\n"); 196 | return std::numeric_limits::lowest(); 197 | } 198 | 199 | // Check if the reference polygon and new polygon do not intersect. 200 | bool new_ref_intersection = bgeo::intersects(new_poly, ref_poly); 201 | if (!new_ref_intersection) { 202 | fprintf(stderr, "KeyFrameSelector: No intersection detected!\n"); 203 | return std::numeric_limits::lowest(); 204 | } 205 | 206 | // Use try-catch block. Sometimes boost throws a 207 | // boost::geometry::overlay_invalid_input_exception exception for some reason. 208 | float area = 0.0f; 209 | try { 210 | std::deque intersection_poly; 211 | bgeo::intersection(new_poly, ref_poly, intersection_poly); 212 | 213 | // Check if intersection...intersects itself. 214 | bool self_intersects = bgeo::intersects(intersection_poly[0]); 215 | if (self_intersects) { 216 | fprintf(stderr, "KeyFrameSelector: Self intersection detected2!\n"); 217 | return std::numeric_limits::lowest(); 218 | } 219 | 220 | area = bgeo::area(intersection_poly[0]); 221 | } catch (...) { 222 | fprintf(stderr, "WARNING: Caught exception from boost::intersection!\n"); 223 | return std::numeric_limits::lowest(); 224 | } 225 | 226 | S_overlap = area / ((width-1) * (height-1)); 227 | 228 | /*==================== Compute disparity score ====================*/ 229 | // Assume structure at min_depth relative to new_img. This will generate 230 | // maximum disparity. Compare maximum disparity to allowable maximum 231 | // disparity (better if they're about the same). 232 | Vector3f u_new_test(width / 4, height / 4, 1.0f); // Pick point not on optical axis. 233 | 234 | // Point at infinite depth. 235 | Vector3f p_ref_inf = K * new_to_ref.unit_quaternion() * Kinv * u_new_test; 236 | p_ref_inf /= p_ref_inf(2); 237 | 238 | // Point at minimum depth. 239 | Vector3f p_ref_min(K * (new_to_ref.unit_quaternion() * 240 | (min_depth * (Kinv * u_new_test)) + 241 | new_to_ref.translation())); 242 | p_ref_min /= p_ref_min(2); 243 | 244 | float disparity = (p_ref_min - p_ref_inf).norm(); 245 | 246 | float S_disparity = -fabs(1.0f - disparity / max_disparity); 247 | 248 | bool debug_print_score = false; 249 | if (debug_print_score) { 250 | printf("S_orient = %f, S_overlap = %f, S_disparity = %f\n", 251 | S_orientation, S_overlap, S_disparity); 252 | } 253 | 254 | return S_orientation + S_overlap + S_disparity; 255 | } 256 | 257 | } // namespace utils 258 | 259 | } // namespace flame 260 | -------------------------------------------------------------------------------- /src/flame/stereo/inverse_depth_filter.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file inverse_depth_filter.cc 19 | * @author W. Nicholas Greene 20 | * @date 2016-09-17 00:38:42 (Sat) 21 | */ 22 | 23 | #include "flame/stereo/inverse_depth_filter.h" 24 | 25 | #include 26 | 27 | #include "flame/utils/image_utils.h" 28 | 29 | namespace flame { 30 | 31 | namespace stereo { 32 | 33 | namespace inverse_depth_filter { 34 | 35 | bool predict(const stereo::EpipolarGeometry& epigeo, 36 | float process_var_factor, 37 | const cv::Point2f& u_ref, float mu, float var, 38 | cv::Point2f* u_cmp, float* mu_pred, 39 | float* var_pred) { 40 | // Project pixel into new image. 41 | epigeo.project(u_ref, mu, u_cmp, mu_pred); 42 | if (*mu_pred < 0.0f) { 43 | // Point is behind camera. 44 | *mu_pred = 0.0f; 45 | *var_pred = 1e10; 46 | return false; 47 | } 48 | 49 | // Compute new variance using LSD-SLAM model. 50 | // Large idepth -> point is near -> large increase in variance. 51 | // Small idepth -> point is far -> small increase in variance. 52 | float var_factor4 = *mu_pred / mu; 53 | var_factor4 *= var_factor4; 54 | var_factor4 *= var_factor4; 55 | 56 | if (mu < 1e-6) { 57 | // If mu == 0, then var_factor4 is inf. 58 | var_factor4 = 1; 59 | } 60 | *var_pred = process_var_factor * var_factor4 * var; // + params_.process_var; 61 | 62 | return true; 63 | } 64 | 65 | bool getSearchRegion(const Params& params, 66 | const stereo::EpipolarGeometry& epigeo, 67 | int width, int height, 68 | const cv::Point2f& u_ref, 69 | float mu_prior, float var_prior, 70 | cv::Point2f* start, cv::Point2f* end, cv::Point2f* epi) { 71 | // Compute idepth search region. 72 | float id_min = params.idepth_min; 73 | float id_max = params.idepth_max; 74 | 75 | if (!std::isnan(mu_prior) && !std::isnan(var_prior)) { 76 | // Center around mean. 77 | float sigma = sqrt(var_prior); 78 | id_min = mu_prior - params.search_sigma * sigma; 79 | id_max = mu_prior + params.search_sigma * sigma; 80 | } 81 | 82 | // Clip to idepth bounds. 83 | id_min = (id_min < params.idepth_min) ? params.idepth_min : id_min; 84 | id_max = (id_max > params.idepth_max) ? params.idepth_max : id_max; 85 | if (id_max < id_min) { 86 | // Search region outside of bounds. 87 | return false; 88 | } 89 | 90 | // Get pixels in comparison image corresponding to search region. 91 | *start = epigeo.project(u_ref, id_min); 92 | *end = epigeo.project(u_ref, id_max); 93 | 94 | // Compute epipolar direction. 95 | cv::Point2f diff(*end - *start); 96 | float epilength = sqrt(diff.x * diff.x + diff.y * diff.y); 97 | if (epilength <= 0) { 98 | // ABORT! ABORT! ABORT! 99 | return false; 100 | } 101 | 102 | *epi = cv::Point2f(diff.x/epilength, diff.y/epilength); 103 | 104 | // Clip to valid region. 105 | // 1pixel border to be safe. 106 | cv::Rect valid_region(1, 1, width-2, height-2); 107 | 108 | FLAME_ASSERT(!std::isnan(start->x)); 109 | FLAME_ASSERT(!std::isnan(start->y)); 110 | FLAME_ASSERT(!std::isnan(end->x)); 111 | FLAME_ASSERT(!std::isnan(end->y)); 112 | 113 | float start_clipx, start_clipy; 114 | float end_clipx, end_clipy; 115 | if (!utils::clipLineLiangBarsky(valid_region.tl().x, valid_region.br().x, 116 | valid_region.tl().y, valid_region.br().y, 117 | start->x, start->y, 118 | end->x, end->y, 119 | &start_clipx, &start_clipy, 120 | &end_clipx, &end_clipy)) { 121 | // Epipolar line is entirely outside valid region. 122 | return false; 123 | } 124 | 125 | start->x = start_clipx; 126 | start->y = start_clipy; 127 | end->x = end_clipx; 128 | end->y = end_clipy; 129 | 130 | // Update epilength. 131 | diff = *end - *start; 132 | epilength = sqrt(diff.x * diff.x + diff.y * diff.y); 133 | if (epilength <= 0) { 134 | // ABORT! ABORT! ABORT! 135 | return false; 136 | } 137 | 138 | // Clip to disparity search region. 139 | if (epilength < params.epilength_min) { 140 | // Epilength is too short - pad. 141 | float pad = (params.epilength_min - epilength) / 2.0f; 142 | *start -= pad * (*epi); 143 | *end += pad * (*epi); 144 | } 145 | 146 | if (epilength > params.epilength_max) { 147 | epilength = params.epilength_max; 148 | 149 | // Center at search region midpoint. 150 | // *start = mid - (epilength / 2) * (*epi); 151 | // *end = mid + (epilength / 2) * (*epi); 152 | 153 | // Clip far end. 154 | // *start = (*end) - epilength * (*epi); 155 | 156 | // Clip near end. 157 | *end = (*start) + epilength * (*epi); 158 | } 159 | 160 | FLAME_ASSERT(!std::isnan(start->x)); 161 | FLAME_ASSERT(!std::isnan(start->y)); 162 | FLAME_ASSERT(!std::isnan(end->x)); 163 | FLAME_ASSERT(!std::isnan(end->y)); 164 | 165 | // Check clipping again. 166 | if (!utils::clipLineLiangBarsky(valid_region.tl().x, valid_region.br().x, 167 | valid_region.tl().y, valid_region.br().y, 168 | start->x, start->y, 169 | end->x, end->y, 170 | &start_clipx, &start_clipy, 171 | &end_clipx, &end_clipy)) { 172 | // Epipolar line is entirely outside valid region. 173 | return false; 174 | } 175 | 176 | start->x = start_clipx; 177 | start->y = start_clipy; 178 | end->x = end_clipx; 179 | end->y = end_clipy; 180 | 181 | return true; 182 | } 183 | 184 | Status search(const Params& params, 185 | const stereo::EpipolarGeometry& epigeo, 186 | float rescale_factor, 187 | const cv::Mat1b& img_ref, 188 | const cv::Mat1b& img_cmp, 189 | const cv::Point2f& u_ref, 190 | const cv::Point2f& u_start, 191 | const cv::Point2f& u_end, 192 | cv::Point2f* u_cmp) { 193 | // Get epilines. 194 | cv::Point2f epi = u_end - u_start; 195 | float norm = sqrt(epi.x*epi.x + epi.y*epi.y); 196 | epi.x /= norm; 197 | epi.y /= norm; 198 | 199 | cv::Point2f epi_ref; 200 | epigeo.referenceEpiline(u_ref, &epi_ref); 201 | 202 | FLAME_ASSERT(params.win_size % 2 == 1); 203 | FLAME_ASSERT(params.win_size == 5); 204 | 205 | // Check ref patch bounds. 206 | FLAME_ASSERT((u_ref.x - 2*epi_ref.x*rescale_factor) >= 0); 207 | FLAME_ASSERT((u_ref.x + 2*epi_ref.x*rescale_factor) < img_ref.cols - 1); 208 | FLAME_ASSERT((u_ref.y - 2*epi_ref.y*rescale_factor) >= 0); 209 | FLAME_ASSERT((u_ref.y + 2*epi_ref.y*rescale_factor) < img_ref.rows - 1); 210 | 211 | // Compute reference patch. 212 | cv::Mat1f ref_patch(1, 5); 213 | ref_patch(0, 0) = utils::bilinearInterp(img_ref, 214 | u_ref.x - 2*epi_ref.x*rescale_factor, 215 | u_ref.y - 2*epi_ref.y*rescale_factor); 216 | ref_patch(0, 1) = utils::bilinearInterp(img_ref, 217 | u_ref.x - epi_ref.x*rescale_factor, 218 | u_ref.y - epi_ref.y*rescale_factor); 219 | ref_patch(0, 2) = utils::bilinearInterp(img_ref, u_ref.x, u_ref.y); 220 | ref_patch(0, 3) = utils::bilinearInterp(img_ref, 221 | u_ref.x + epi_ref.x*rescale_factor, 222 | u_ref.y + epi_ref.y*rescale_factor); 223 | ref_patch(0, 4) = utils::bilinearInterp(img_ref, 224 | u_ref.x + 2*epi_ref.x*rescale_factor, 225 | u_ref.y + 2*epi_ref.y*rescale_factor); 226 | 227 | // Check gradient along this patch to make sure there's information to 228 | // constrain matching. 229 | float ref_patch_grad = 0.0f; 230 | for (int ii = 1; ii < ref_patch.cols; ++ii) { 231 | float abs_grad = utils::fast_abs(ref_patch(ii) - ref_patch(ii - 1)); 232 | if (abs_grad > ref_patch_grad) { 233 | ref_patch_grad = abs_grad; 234 | } 235 | } 236 | if (ref_patch_grad < params.min_grad_mag) { 237 | return Status::FAIL_REF_PATCH_GRADIENT; 238 | } 239 | 240 | // Do line stereo. 241 | float residual = std::numeric_limits::max(); 242 | auto result = stereo::line_stereo::match(rescale_factor, ref_patch, img_cmp, 243 | u_start, u_end, u_cmp, &residual, 244 | params.sparams); 245 | 246 | if (result != stereo::line_stereo::Status::SUCCESS) { 247 | // Stereo match not successful. 248 | bool verbose = false; 249 | if (verbose) { 250 | fprintf(stderr, "epi_ref = (%f, %f)\n", epi_ref.x, epi_ref.y); 251 | fprintf(stderr, "InverseDepthFilter[FAIL=%i]: Stereo match unsuccessful for pixel (%f, %f), start = (%f, %f), end = (%f, %f)!\n", 252 | result, u_ref.x, u_ref.y, u_start.x, u_start.y, u_end.x, u_end.y); 253 | } 254 | } 255 | 256 | if (result == stereo::line_stereo::Status::FAIL_AMBIGUOUS_MATCH) { 257 | return Status::FAIL_AMBIGUOUS_MATCH; 258 | } else if (result == stereo::line_stereo::Status::FAIL_MAX_COST) { 259 | return Status::FAIL_MAX_COST; 260 | } else if (result != stereo::line_stereo::Status::SUCCESS) { 261 | fprintf(stderr, "inverse_depth_filter::search: Unrecognized status!\n"); 262 | FLAME_ASSERT(false); 263 | } 264 | 265 | return Status::SUCCESS; 266 | } 267 | 268 | bool update(float mu_pred, float var_pred, 269 | float mu_meas, float var_meas, 270 | float* mu_post, float* var_post, 271 | float outlier_sigma_thresh) { 272 | if (!std::isnan(mu_pred) && (mu_pred > 0.0f)) { 273 | // Fu-sion...HA! 274 | float w = var_pred + var_meas; 275 | *mu_post = (var_meas * mu_pred + var_pred * mu_meas) / w; 276 | *var_post = (var_pred * var_meas) / w; 277 | } else { 278 | // First detection, set to measurement. 279 | *mu_post = mu_meas; 280 | *var_post = var_meas; 281 | } 282 | 283 | // Perform Chi-square/mahlanobis distance test. 284 | float res = (mu_meas - mu_pred); 285 | float dist = res * res / var_pred; 286 | if (dist > outlier_sigma_thresh * outlier_sigma_thresh) { 287 | // Measurement is far away from prediction. Probably an outlier. 288 | bool verbose = false; 289 | if (verbose) { 290 | fprintf(stderr, "InverseDepthFilter[ERROR]: Measurement rejected pred = (%f, %f) meas = (%f, %f) res = %f sigma_dist = %f!\n", 291 | mu_pred, var_pred, mu_meas, var_meas, res, dist); 292 | } 293 | return false; 294 | } 295 | 296 | // Make sure idepth is >= 0. 297 | *mu_post = (*mu_post <= 0) ? 0.0f : *mu_post; 298 | 299 | FLAME_ASSERT(!std::isnan(*mu_post)); 300 | FLAME_ASSERT(!std::isnan(*var_post)); 301 | FLAME_ASSERT(*mu_post >= 0); 302 | FLAME_ASSERT(*var_post >= 0); 303 | 304 | return true; 305 | } 306 | 307 | } // namespace inverse_depth_filter 308 | 309 | } // namespace stereo 310 | 311 | } // namespace flame 312 | -------------------------------------------------------------------------------- /src/flame/utils/visualization.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file visualization.h 19 | * @author W. Nicholas Greene 20 | * @date 2017-08-18 18:58:49 (Fri) 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include "flame/types.h" 31 | #include "flame/utils/assert.h" 32 | 33 | namespace flame { 34 | 35 | namespace utils { 36 | 37 | /** 38 | * @brief Draw a rectangle on an image. 39 | */ 40 | template 41 | void rectangle(const Point2i& top_left, const Point2i& bot_right, 42 | const T& color, Image* img, int thickness = 1) { 43 | int xstart = top_left.x; 44 | int xend = bot_right.x; 45 | int ystart = top_left.y; 46 | int yend = bot_right.y; 47 | 48 | // Draw horizontal lines. 49 | for (int ii = -thickness/2; ii <= thickness/2; ++ii) { 50 | for (int jj = xstart; jj <= xend; ++jj) { 51 | (*img)(ystart + ii, jj) = color; 52 | (*img)(yend + ii, jj) = color; 53 | } 54 | } 55 | 56 | // Draw vertical lines. (Switch drawing order to prefer walking over columns.) 57 | for (int ii = ystart; ii <= yend; ++ii) { 58 | for (int jj = -thickness/2; jj <= thickness/2; ++jj) { 59 | (*img)(ii, xstart + jj) = color; 60 | (*img)(ii, xend + jj) = color; 61 | } 62 | } 63 | 64 | return; 65 | } 66 | 67 | // Adapted from http://axonflux.com/handy-rgb-to-hsl-and-rgb-to-hsv-color-model-c 68 | inline float hueToRGB(float p, float q, float t) { 69 | if (t < 0) 70 | t += 1; 71 | if (t > 1) 72 | t -= 1; 73 | if (t < 1.0/6) 74 | return p + (q - p) * 6 * t; 75 | if (t < 1.0/2) 76 | return q; 77 | if (t < 2.0/3) 78 | return p + (q - p) * (2.0/3 - t) * 6; 79 | 80 | return p; 81 | } 82 | 83 | /** 84 | * Converts an HSL color value to RGB. Conversion formula 85 | * adapted from http://en.wikipedia.org/wiki/HSL_color_space. 86 | * Assumes h, s, and l are contained in the set [0, 1] and 87 | * returns r, g, and b in the set [0, 255]. 88 | * 89 | * @param Number h The hue 90 | * @param Number s The saturation 91 | * @param Number l The lightness 92 | * @return Array The RGB representation 93 | */ 94 | inline cv::Vec3b hslToRGB(float h, float s, float l) { 95 | float r, g, b; 96 | 97 | if (s == 0) { 98 | r = g = b = l; // achromatic 99 | } else { 100 | 101 | float q = l < 0.5 ? l * (1 + s) : l + s - l * s; 102 | float p = 2 * l - q; 103 | r = hueToRGB(p, q, h + 1.0/3); 104 | g = hueToRGB(p, q, h); 105 | b = hueToRGB(p, q, h - 1.0/3); 106 | } 107 | 108 | cv::Vec3b c(255, 255, 255); // white 109 | c[0] = b * 255; 110 | c[1] = g * 255; 111 | c[2] = r * 255; 112 | 113 | return c; 114 | } 115 | 116 | /** 117 | * @brief Maps an inward point normal to a color. 118 | */ 119 | inline cv::Vec3b normalMap(float nx, float ny, float nz) { 120 | cv::Vec3b color(0, 0, 0); 121 | 122 | uint8_t red = 255 * (nx + 1)/2; 123 | uint8_t green = 255 * (ny + 1)/2; 124 | uint8_t blue = 127 * nz + 127; 125 | color[0] = blue; 126 | color[1] = green; 127 | color[2] = red; 128 | 129 | return color; 130 | } 131 | 132 | /** 133 | * \brief Jet colormap. 134 | * 135 | * Matlab-like jet colormap. 136 | * 137 | * @param v[in] Value. 138 | * @param vmin[in] Minimum value limit. 139 | * @param vmax[in] Maximum value limit. 140 | * @return Color. 141 | */ 142 | inline cv::Vec3b jet(float v, float vmin, float vmax) { 143 | cv::Vec3b c(255, 255, 255); // white 144 | float dv; 145 | 146 | if (v < vmin) 147 | v = vmin; 148 | if (v > vmax) 149 | v = vmax; 150 | dv = vmax - vmin; 151 | 152 | if (v < (vmin + 0.25 * dv)) { 153 | c[2] = 0; 154 | c[1] = static_cast(255 * (4 * (v - vmin) / dv)); 155 | } else if (v < (vmin + 0.5 * dv)) { 156 | c[2] = 0; 157 | c[0] = static_cast(255 * (1 + 4 * (vmin + 0.25 * dv - v) / dv));; 158 | } else if (v < (vmin + 0.75 * dv)) { 159 | c[2] = static_cast(255 * (4 * (v - vmin - 0.5 * dv) / dv)); 160 | c[0] = 0; 161 | } else { 162 | c[1] = static_cast(255 * (1 + 4 * (vmin + 0.75 * dv - v) / dv)); 163 | c[0] = 0; 164 | } 165 | 166 | return c; 167 | } 168 | 169 | /** 170 | * \brief Maps a value linearly between two colors. 171 | */ 172 | inline cv::Vec3b blendColor(const cv::Vec3b& cmin, const cv::Vec3b& cmax, 173 | float v, float vmin, float vmax) { 174 | // Clamp value. 175 | if (v < vmin) { 176 | v = vmin; 177 | } 178 | if (v > vmax) { 179 | v = vmax; 180 | } 181 | 182 | float alpha = (v - vmin) / (vmax - vmin); 183 | float invalpha = 1.0f - alpha; 184 | 185 | return cv::Vec3b(invalpha*cmin[0] + alpha*cmax[0], 186 | invalpha*cmin[1] + alpha*cmax[1], 187 | invalpha*cmin[2] + alpha*cmax[2]); 188 | } 189 | 190 | /** 191 | * \brief Inverse depth colormap. 192 | * 193 | * Colormap for displaying inverse depth. 194 | * 195 | * @param id[in] Inverse depth. 196 | * @return Color. 197 | */ 198 | inline cv::Vec3b idepthColor(float id) { 199 | // rainbow between 0 and 4 200 | float r = (0 - id) * 255 / 1.0; 201 | r = (r < 0) ? -r : r; 202 | 203 | float g = (1-id) * 255 / 1.0; 204 | g = (g < 0) ? -g : g; 205 | 206 | float b = (2-id) * 255 / 1.0; 207 | b = (b < 0) ? -b : b; 208 | 209 | uchar rc = r < 0 ? 0 : (r > 255 ? 255 : r); 210 | uchar gc = g < 0 ? 0 : (g > 255 ? 255 : g); 211 | uchar bc = b < 0 ? 0 : (b > 255 ? 255 : b); 212 | 213 | return cv::Vec3b(255 - rc, 255 - gc, 255 - bc); 214 | } 215 | 216 | /** 217 | * \brief Convert 32-bit Census value to 8-bit gray value. 218 | * 219 | * @param[in] val Input Census value. 220 | * @param[in] min Minimum Census value. 221 | * @param[in] max Maximum Census value. 222 | * @return Gray scale value. 223 | */ 224 | inline cv::Vec3b censusToGray(const uint32_t val, const uint32_t min, 225 | const uint32_t max) { 226 | uint32_t range = max - min; 227 | range = (range == 0) ? 1 : range; 228 | uint8_t gray = 255 * (static_cast(val) - min) / range; 229 | return cv::Vec3b(gray, gray, gray); 230 | } 231 | 232 | /** 233 | * \brief Apply a colormap to a line, interpolating along the values. 234 | */ 235 | template 236 | inline void applyColorMapLine(const cv::Point2f& A, const cv::Point2f& B, 237 | float A_val, float B_val, ColorMap map, 238 | float alpha, cv::Mat* img) { 239 | FLAME_ASSERT(img->channels() == 3); 240 | FLAME_ASSERT(img->isContinuous()); 241 | #if CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >= 3 242 | FLAME_ASSERT(img->type() == cv::traits::Type::value); 243 | #else 244 | FLAME_ASSERT(img->type() == cv::DataType::type); 245 | #endif 246 | 247 | cv::LineIterator it(*img, A, B); 248 | float slope0 = (B_val - A_val) / (it.count); 249 | 250 | for (int ii = 0; ii < it.count; ++ii, ++it) { 251 | float val_ii = A_val + ii * slope0; 252 | 253 | cv::Vec3b color = map(val_ii); 254 | (*it)[0] = color[0] * alpha + (*it)[0] * (1.0f - alpha); 255 | (*it)[1] = color[1] * alpha + (*it)[1] * (1.0f - alpha); 256 | (*it)[2] = color[2] * alpha + (*it)[2] * (1.0f - alpha); 257 | } 258 | 259 | return; 260 | } 261 | 262 | /** 263 | * \brief Apply a colormap to an image. 264 | * 265 | * Long Description 266 | * 267 | * @param img[in] Input image. 268 | * @param map[in] Colormap function object/functor. 269 | * @param out[out] Colormapped image. 270 | */ 271 | template 272 | void applyColorMap(const cv::Mat& img, ColorMap map, cv::Mat3b* out) { 273 | FLAME_ASSERT(img.channels() == 1); 274 | FLAME_ASSERT(img.isContinuous()); 275 | 276 | if (out->empty()) { 277 | out->create(img.rows, img.cols); 278 | } 279 | 280 | const ChannelType* in_ptr = reinterpret_cast(img.data); 281 | cv::Vec3b* out_ptr = reinterpret_cast(out->data); 282 | for (int ii = 0; ii < img.rows * img.cols; ++ii) { 283 | out_ptr[ii] = map(in_ptr[ii], out_ptr[ii]); 284 | } 285 | 286 | return; 287 | } 288 | 289 | #if CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION <= 2 290 | // cv::Mat_ is not supported in OpenCV 3.3. 291 | 292 | /** 293 | * \brief Convert a 32-bit Census image to RGB image. 294 | * 295 | * Most of the convenient functions that make this easy in OpenCV don't work for 296 | * uint32_t. 297 | * 298 | * @param[in] img Input Census image. 299 | * @param[ou] out Output image. 300 | */ 301 | inline void censusToRGB(const cv::Mat_& img, cv::Mat3b* out, 302 | uint32_t min = 0, uint32_t max = 10000000) { 303 | FLAME_ASSERT(img.isContinuous()); 304 | 305 | if (out->empty()) { 306 | out->create(img.rows, img.cols); 307 | } 308 | 309 | // Create RGB image. 310 | for (int ii = 0; ii < img.rows; ++ii) { 311 | for (int jj = 0; jj < img.cols; ++jj) { 312 | // (*out)(ii, jj) = censusToGray(img(ii, jj), min, max); 313 | (*out)(ii, jj) = jet(img(ii, jj), min, max); 314 | } 315 | } 316 | 317 | return; 318 | } 319 | #endif 320 | 321 | // /** 322 | // * \brief Colormap for an inverse depth estimate. 323 | // * 324 | // * @param lastFrameID[in] Visualization type? 325 | // * @return RGB color. 326 | // */ 327 | // inline cv::Vec3b getVisualizationColor(const InverseFlameimate& est, int lastFrameID) { 328 | // if (params::debugDisplay == 0 || 329 | // params::debugDisplay == 1) { 330 | // float id; 331 | // if (params::debugDisplay == 0) { 332 | // id = est.idepth_smoothed; 333 | // } else {// if(params::debugDisplay == 1) 334 | // id = est.idepth; 335 | // } 336 | 337 | // if (id < 0) { 338 | // return cv::Vec3b(255, 255, 255); 339 | // } 340 | 341 | // // rainbow between 0 and 4 342 | // float r = (0 - id) * 255 / 1.0; 343 | // r = (r < 0) ? -r : r; 344 | 345 | // float g = (1-id) * 255 / 1.0; 346 | // g = (g < 0) ? -g : g; 347 | 348 | // float b = (2-id) * 255 / 1.0; 349 | // b = (b < 0) ? -b : b; 350 | 351 | // uchar rc = r < 0 ? 0 : (r > 255 ? 255 : r); 352 | // uchar gc = g < 0 ? 0 : (g > 255 ? 255 : g); 353 | // uchar bc = b < 0 ? 0 : (b > 255 ? 255 : b); 354 | 355 | // return cv::Vec3b(255 - rc, 255 - gc, 255 - bc); 356 | // } 357 | 358 | // // plot validity counter 359 | // if (params::debugDisplay == 2) { 360 | // float f = est.validity_counter * (255.0 / 361 | // (params::VALIDITY_COUNTER_MAX_VARIABLE + params::VALIDITY_COUNTER_MAX)); 362 | // uchar v = f < 0 ? 0 : (f > 255 ? 255 : f); 363 | 364 | // return cv::Vec3b(0, v, v); 365 | // } 366 | 367 | // // plot var 368 | // if (params::debugDisplay == 3 || params::debugDisplay == 4) { 369 | // float idv; 370 | // if (params::debugDisplay == 3) { 371 | // idv = est.idepth_var_smoothed; 372 | // } else { 373 | // idv = est.idepth_var; 374 | // } 375 | 376 | // float var = - 0.5 * log10(idv); 377 | 378 | // var = var * 255 * 0.333; 379 | // if (var > 255) { 380 | // var = 255; 381 | // } 382 | // if (var < 0) { 383 | // return cv::Vec3b(0, 0, 255); 384 | // } 385 | 386 | // return cv::Vec3b(255 - var, var, 0);// bw 387 | // } 388 | 389 | // // plot skip 390 | // if (params::debugDisplay == 5) { 391 | // float f = (est.next_stereo_min_frame_id - lastFrameID) * (255.0 / 100); 392 | // uchar v = f < 0 ? 0 : (f > 255 ? 255 : f); 393 | // return cv::Vec3b(v, 0, v); 394 | // } 395 | 396 | // return cv::Vec3b(255, 255, 255); 397 | // } 398 | 399 | } // namespace utils 400 | 401 | } // namespace flame 402 | -------------------------------------------------------------------------------- /src/flame/utils/load_tracker.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file load_tracker.h 19 | * @author W. Nicholas Greene 20 | * @date 2017-02-04 17:41:45 (Sat) 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | #include // For the Jiffy constant HZ. 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "flame/utils/assert.h" 36 | 37 | namespace flame { 38 | 39 | namespace utils { 40 | 41 | typedef std::chrono::high_resolution_clock clock; 42 | typedef std::chrono::duration msec; 43 | 44 | /** 45 | * @brief Struct to hold load information. 46 | */ 47 | struct Load { 48 | float cpu = 0; // CPU load between 0 and number of processors. 49 | uint64_t mem = 0; // Memory load in megabytes/mebibytes (1024^2 bytes). 50 | uint64_t swap = 0; // Swap load in megabytes/mebibytes (1024^2 bytes). 51 | }; 52 | 53 | /** 54 | * @brief Struct to hold measured process CPU times in clock ticks. 55 | * 56 | * Obtained from columns 13-16 in /proc/pid/stat. 57 | */ 58 | struct ProcessCPUMeasurement { 59 | uint64_t utime = 0; // Time spent in user mode. 60 | uint64_t stime = 0; // Time spent in kernel mode. 61 | uint64_t cutime = 0; // Time children spent in user mode. 62 | uint64_t cstime = 0; // Time children spent in kernel mode. 63 | }; 64 | 65 | /** 66 | * @brief Struct to hold measured system CPU times in jiffies (0.01 seconds). 67 | * 68 | * Obtained from columns 1-7 in /proc/stat. 69 | */ 70 | struct SystemCPUMeasurement { 71 | uint64_t user = 0; // Time processes spent in user mode. 72 | uint64_t nice = 0; // Time niced processes spent in user mode. 73 | uint64_t system = 0; // Time processes spent in kernel mode. 74 | uint64_t idle = 0; // Time spent idle. 75 | uint64_t iowait = 0; // Time spent waiting for IO to complete. 76 | uint64_t irq = 0; // Time spent servicing interrupts. 77 | uint64_t softirq = 0; // Time spent servicing softirqs. 78 | }; 79 | 80 | /** 81 | * @brief Read /proc//stat to generate CPU timing measurement. 82 | */ 83 | inline void getProcessCPUMeasurement(int pid, ProcessCPUMeasurement* meas) { 84 | std::ifstream file("/proc/" + std::to_string(pid) + "/stat", 85 | std::ifstream::in); 86 | std::string line; 87 | std::getline(file, line); 88 | std::istringstream lstream(line); 89 | 90 | // Skip the first 13 columns. 91 | for (int ii = 0; ii < 13; ++ii) { 92 | std::string word; 93 | lstream >> word; 94 | } 95 | 96 | // Read columns 14-17. 97 | lstream >> meas->utime; // Time spent in user mode. 98 | lstream >> meas->stime; // Time spent in kernel mode. 99 | lstream >> meas->cutime; // Time spent in child processes in user mode. 100 | lstream >> meas->cstime; // Time spent in child processes in kernel mode. 101 | 102 | return; 103 | } 104 | 105 | /** 106 | * @brief Read /proc/stat to generate system CPU timing measurement. 107 | */ 108 | inline void getSystemCPUMeasurement(SystemCPUMeasurement* meas) { 109 | std::ifstream file("/proc/stat", std::ifstream::in); 110 | std::string line; 111 | std::getline(file, line); 112 | std::istringstream lstream(line); 113 | 114 | // Skip first column. 115 | std::string blah; 116 | lstream >> blah; 117 | FLAME_ASSERT(blah == "cpu"); 118 | 119 | // Read next 7 columns. 120 | lstream >> meas->user; // Time processes spent in user mode. 121 | lstream >> meas->nice; // Time niced processes spent in user mode. 122 | lstream >> meas->system; // Time processes spent in kernel mode. 123 | lstream >> meas->idle; // Time spent idle. 124 | lstream >> meas->iowait; // Time spent waiting for IO to complete. 125 | lstream >> meas->irq; // Time spent servicing interrupts. 126 | lstream >> meas->softirq; // Time spent servicing softirqs. 127 | 128 | return; 129 | } 130 | 131 | /** 132 | * @brief Get the CPU load on the system given two time measurements. 133 | */ 134 | inline void getSystemCPULoad(const SystemCPUMeasurement& prev, 135 | const SystemCPUMeasurement& curr, 136 | float dt, int num_procs, 137 | Load* sys_load) { 138 | // Compute the updated system load. We take the difference of the sum of the 139 | // two measurements (except idle time) and then divide by the Jiffy constant 140 | // (HZ) and the sampling rate, which gives us the load expressed as a number 141 | // between 0 and num_procs. 142 | float cpu_sys_total = curr.user + curr.nice + curr.system + curr.iowait + 143 | curr.irq + curr.softirq; 144 | float cpu_sys_total_prev = prev.user + prev.nice + prev.system + prev.iowait + 145 | prev.irq + prev.softirq; 146 | 147 | FLAME_ASSERT(cpu_sys_total >= cpu_sys_total_prev); 148 | 149 | sys_load->cpu = (cpu_sys_total - cpu_sys_total_prev) / dt / HZ; 150 | 151 | if (sys_load->cpu < 0) { 152 | sys_load->cpu = 0; 153 | } 154 | if (sys_load->cpu > num_procs) { 155 | sys_load->cpu = num_procs; 156 | } 157 | 158 | return; 159 | } 160 | 161 | /** 162 | * @brief Get the CPU load from a single process given two time measurements. 163 | */ 164 | inline void getProcessCPULoad(const ProcessCPUMeasurement& prev, 165 | const ProcessCPUMeasurement& curr, 166 | float dt, int num_procs, 167 | int ticks_per_sec, Load* pid_load) { 168 | // Compute updated process load. We take the difference of the sum of the 169 | // two measurements and then divide by the clock speed (ticks_per_sec_) and 170 | // the sampling rate (dt_), which gives us the load expressed as a number 171 | // between 0 and num_procs_. 172 | float cpu_pid_total = curr.utime + curr.stime + curr.cutime + curr.cstime; 173 | float cpu_pid_total_prev = prev.utime + prev.stime + prev.cutime + prev.cstime; 174 | 175 | FLAME_ASSERT(cpu_pid_total >= cpu_pid_total_prev); 176 | 177 | pid_load->cpu = (cpu_pid_total - cpu_pid_total_prev) / dt / ticks_per_sec; 178 | 179 | // Check bounds. Numbers may be slightly off because of timing accuracy. 180 | if (pid_load->cpu < 0) { 181 | pid_load->cpu = 0; 182 | } 183 | if (pid_load->cpu > num_procs) { 184 | pid_load->cpu = num_procs; 185 | } 186 | 187 | return; 188 | } 189 | 190 | /** 191 | * @brief Read /proc/meminfo to compute system memory loads. 192 | */ 193 | inline void getSystemMemoryLoad(Load* max_load, Load* sys_load) { 194 | std::ifstream file("/proc/meminfo", std::ifstream::in); 195 | std::string line; 196 | 197 | // Loop through file and grab relevant fields. 198 | uint64_t mem_total_kb = 0; 199 | uint64_t mem_free_kb = 0; 200 | uint64_t buffers_kb = 0; 201 | uint64_t cached_kb = 0; 202 | uint64_t swap_free_kb = 0; 203 | uint64_t swap_total_kb = 0; 204 | int count = 0; 205 | while (std::getline(file, line)) { 206 | std::stringstream lstream(line); 207 | std::string field; 208 | lstream >> field; 209 | 210 | if (field == "MemTotal:") { 211 | lstream >> mem_total_kb; 212 | max_load->mem = mem_total_kb >> 10; // Convert from kB to MB. 213 | count++; 214 | } else if (field == "MemFree:") { 215 | lstream >> mem_free_kb; 216 | count++; 217 | } else if (field == "Buffers:") { 218 | lstream >> buffers_kb; 219 | count++; 220 | } else if (field == "Cached:") { 221 | lstream >> cached_kb; 222 | count++; 223 | } else if (field == "SwapTotal:") { 224 | lstream >> swap_total_kb; 225 | max_load->swap = swap_total_kb >> 10; // Convert from kB to MB. 226 | count++; 227 | } else if (field == "SwapFree:") { 228 | lstream >> swap_free_kb; 229 | count++; 230 | } 231 | } 232 | 233 | FLAME_ASSERT(count == 6); 234 | 235 | // Substract to get used swap. 236 | sys_load->swap = (swap_total_kb - swap_free_kb) >> 10; // Convert to MB. 237 | 238 | // Physical memory used is total - free - buffers - cached. This number 239 | // corresponds to the green bar in htop: 240 | // https://stackoverflow.com/questions/41224738/how-to-calculate-memory-usage-from-proc-meminfo-like-htop 241 | sys_load->mem = (mem_total_kb - mem_free_kb - buffers_kb - 242 | cached_kb) >> 10; // Convert to MB. 243 | 244 | return; 245 | } 246 | 247 | /** 248 | * @brief Read /proc//status to generate process memory load. 249 | */ 250 | inline void getProcessMemoryLoad(int pid, Load* load) { 251 | // Grab a process CPU load measurement. 252 | std::ifstream file("/proc/" + std::to_string(pid) + "/status", 253 | std::ifstream::in); 254 | std::string line; 255 | 256 | // Loop through file and get relevant fields. 257 | int count = 0; 258 | uint64_t mem_pid_kb = 0; 259 | uint64_t swap_pid_kb = 0; 260 | while (std::getline(file, line)) { 261 | std::stringstream lstream(line); 262 | std::string field; 263 | lstream >> field; 264 | if (field == "VmRSS:") { 265 | lstream >> mem_pid_kb; 266 | load->mem = mem_pid_kb >> 10; // Convert to MB. 267 | count++; 268 | } else if (field == "VmSwap:") { 269 | lstream >> swap_pid_kb; 270 | load->swap = swap_pid_kb >> 10; // Convert to MB. 271 | count++; 272 | } 273 | } 274 | 275 | FLAME_ASSERT(count == 2); 276 | 277 | return; 278 | } 279 | 280 | /** 281 | * @brief A class to track a process's CPU and memory load. 282 | * 283 | * We read the following files to compute load: 284 | * - /proc/stat for system CPU load 285 | * - /proc/meminfo for system memory load 286 | * - /proc//stat for CPU load for given pid. 287 | * - /proc//status for memory load for given pid. 288 | * 289 | * CPU load information in the above files is expressed in ticks or jiffies 290 | * spent executing code in either user or kernel mode. To get actual load we 291 | * need to take two measurements from the files, subtract them, and then scale 292 | * the result, which will be a number between 0 and the number of processors 293 | * (technically number of concurrent threads supported). 294 | * 295 | * Memory load is pretty much read directly from the files. 296 | * 297 | * Usage: 298 | * @code{.cpp} 299 | * #include 300 | * 301 | * LoadTracker load_tracker(pid); 302 | * 303 | * Load max_load, sys_load, pid_load; 304 | * load_tracker.get(&max_load, &sys_load, &pid_load); // Grab current load info. 305 | * 306 | * printf("system CPU load = %f / %.0f\n", sys_load.cpu, max_load.cpu); 307 | * printf("system memory load = %lu / %lu MB\n", sys_load.mem, max_load.mem); 308 | * printf("system swap load = %lu / %lu MB\n", sys_load.swap, max_load.swap); 309 | * printf("process CPU load = %f / %.0f\n", pid_load.cpu, max_load.cpu); 310 | * printf("process memory load = %lu / %lu MB\n", pid_load.mem, max_load.mem); 311 | * printf("process swap load = %lu / %lu MB\n", pid_load.swap, max_load.swap); 312 | * @endcode 313 | * 314 | * Based on: 315 | * https://stackoverflow.com/questions/63166/how-to-determine-cpu-and-memory-consumption-from-inside-a-process 316 | * https://stackoverflow.com/questions/16726779/how-do-i-get-the-total-cpu-usage-of-an-application-from-proc-pid-stat 317 | */ 318 | class LoadTracker final { 319 | public: 320 | /** 321 | * #brief Constructor. 322 | * 323 | * Tracks load for given PID. 324 | * 325 | * @param[in] pid Process ID. 326 | */ 327 | explicit LoadTracker(uint32_t pid = getpid()) : 328 | last_update_(), 329 | num_procs_(std::thread::hardware_concurrency()), 330 | ticks_per_sec_(sysconf(_SC_CLK_TCK)), 331 | pid_(pid), 332 | cpu_pid_meas_(), 333 | cpu_sys_meas_(), 334 | max_load_(), 335 | sys_load_(), 336 | pid_load_() { 337 | // Initialize CPU load measurements. 338 | getSystemCPUMeasurement(&cpu_sys_meas_); 339 | getProcessCPUMeasurement(pid_, &cpu_pid_meas_); 340 | max_load_.cpu = num_procs_; 341 | return; 342 | } 343 | 344 | ~LoadTracker() = default; 345 | 346 | LoadTracker(const LoadTracker& rhs) = delete; 347 | LoadTracker& operator=(const LoadTracker& rhs) = delete; 348 | 349 | LoadTracker(LoadTracker&& rhs) = default; 350 | LoadTracker& operator=(LoadTracker&& rhs) = default; 351 | 352 | /** 353 | * @brief Get the current load information. 354 | * 355 | * @param[out] max_load Maximum possible load for this system. 356 | * @param[out] sys_load Current system load. 357 | * @param[out] pid_load Current process load. 358 | */ 359 | void get(Load* max_load, Load* sys_load, Load* pid_load); 360 | 361 | private: 362 | clock::time_point last_update_; // Time of last update. 363 | 364 | int num_procs_; // Number of processors. 365 | uint32_t ticks_per_sec_; // Clock speed. 366 | 367 | int pid_; // Process ID. 368 | ProcessCPUMeasurement cpu_pid_meas_; // Last process CPU load measurement. 369 | SystemCPUMeasurement cpu_sys_meas_; // Last system CPU load measurement. 370 | 371 | Load max_load_; // Maximum possible load for system. 372 | Load sys_load_; // Current system load. 373 | Load pid_load_; // Current load for PID. 374 | }; 375 | 376 | inline void LoadTracker::get(Load* max_load, Load* sys_load, Load* pid_load) { 377 | // Get time since last update. 378 | msec dt_ms = clock::now() - last_update_; 379 | float dt = dt_ms.count() / 1000; // in seconds. 380 | 381 | SystemCPUMeasurement cpu_sys_meas_prev(cpu_sys_meas_); // Save old measurement. 382 | getSystemCPUMeasurement(&cpu_sys_meas_); // Get new measurement. 383 | getSystemCPULoad(cpu_sys_meas_prev, cpu_sys_meas_, dt, num_procs_, &sys_load_); 384 | 385 | ProcessCPUMeasurement cpu_pid_meas_prev(cpu_pid_meas_); // Save old measurement. 386 | getProcessCPUMeasurement(pid_, &cpu_pid_meas_); // Get new measurement. 387 | getProcessCPULoad(cpu_pid_meas_prev, cpu_pid_meas_, dt, num_procs_, 388 | ticks_per_sec_, &pid_load_); 389 | 390 | getSystemMemoryLoad(&max_load_, &sys_load_); 391 | getProcessMemoryLoad(pid_, &pid_load_); 392 | 393 | last_update_ = clock::now(); 394 | 395 | *max_load = max_load_; 396 | *sys_load = sys_load_; 397 | *pid_load = pid_load_; 398 | 399 | return; 400 | } 401 | 402 | } // namespace utils 403 | 404 | } // namespace flame 405 | -------------------------------------------------------------------------------- /src/flame/utils/image_utils.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file image_utils.cc 19 | * @author W. Nicholas Greene 20 | * @date 2017-08-18 18:53:35 (Fri) 21 | */ 22 | 23 | #include "flame/utils/image_utils.h" 24 | #include "flame/utils/rasterization.h" 25 | 26 | namespace flame { 27 | 28 | namespace utils { 29 | 30 | #ifdef __SSE__ 31 | 32 | /** 33 | * @brief SSE optimized specialization for uint8_t images. 34 | */ 35 | template <> 36 | void getCentralGradient(int width, int height, const uint8_t* img_ptr, 37 | float* gradx_ptr, float* grady_ptr) { 38 | __m128 half = _mm_set_ps(0.5f, 0.5f, 0.5f, 0.5); 39 | 40 | // Compute gradx. 41 | for (uint32_t ii = 0; ii < height; ++ii) { 42 | uint32_t jj = 0; 43 | for (jj = 1; jj < width - 4 - 1; jj+=4) { 44 | int idx = ii * width + jj; 45 | 46 | __m128 left = _mm_set_ps(img_ptr[idx + 2], img_ptr[idx + 1], 47 | img_ptr[idx], img_ptr[idx - 1]); 48 | __m128 right = _mm_set_ps(img_ptr[idx + 4], img_ptr[idx + 3], 49 | img_ptr[idx + 2], img_ptr[idx + 1]); 50 | __m128 diffx = _mm_sub_ps(right, left); 51 | __m128 gx = _mm_mul_ps(half, diffx); 52 | _mm_storeu_ps(gradx_ptr + idx, gx); 53 | } 54 | 55 | // Finish remainder of row without SSE if not divisble by 4. 56 | for (; jj < width - 1; ++jj) { 57 | int idx = ii * width + jj; 58 | float left = img_ptr[idx - 1]; 59 | float right = img_ptr[idx + 1]; 60 | gradx_ptr[idx] = 0.5 * (right - left); 61 | } 62 | } 63 | 64 | // Compute gradient for first/last columns using forward/backward differences. 65 | for (uint32_t ii = 0; ii < height; ++ii) { 66 | // First column (forward difference). 67 | float left = img_ptr[ii * width]; 68 | float right = img_ptr[ii * width + 1]; 69 | gradx_ptr[ii * width] = right - left; 70 | 71 | // Last column (backward difference). 72 | left = img_ptr[ii * width + width - 2]; 73 | right = img_ptr[ii * width + width - 1]; 74 | gradx_ptr[ii * width + width - 1] = right - left; 75 | } 76 | 77 | // Compute grady. 78 | for (uint32_t ii = 1; ii < height - 1; ++ii) { 79 | uint32_t jj = 0; 80 | for (jj = 0; jj < width - 4; jj+=4) { 81 | int idx = ii * width + jj; 82 | 83 | __m128 up = _mm_set_ps(img_ptr[idx - width + 3], img_ptr[idx - width + 2], 84 | img_ptr[idx - width + 1], img_ptr[idx - width]); 85 | __m128 down = _mm_set_ps(img_ptr[idx + width + 3], img_ptr[idx + width + 2], 86 | img_ptr[idx + width + 1], img_ptr[idx + width]); 87 | __m128 diffy = _mm_sub_ps(down, up); 88 | __m128 gy = _mm_mul_ps(half, diffy); 89 | _mm_storeu_ps(grady_ptr + idx, gy); 90 | } 91 | 92 | // Finish remainder of row without SSE if not divisble by 4. 93 | for (; jj < width; ++jj) { 94 | int idx = ii * width + jj; 95 | float up = img_ptr[idx - width]; 96 | float down = img_ptr[idx + width]; 97 | grady_ptr[idx] = 0.5 * (down - up); 98 | } 99 | } 100 | 101 | // Compute gradient for first/last rows using forward/backward differences. 102 | for (uint32_t ii = 0; ii < width; ++ii) { 103 | // First row (forward difference). 104 | float up = img_ptr[ii]; 105 | float down = img_ptr[ii + width]; 106 | grady_ptr[ii] = down - up; 107 | 108 | // Last row (backward difference). 109 | up = img_ptr[(height - 2) * width + ii]; 110 | down = img_ptr[(height - 1) * width + ii]; 111 | grady_ptr[(height - 1) * width + ii] = down - up; 112 | } 113 | 114 | return; 115 | } 116 | 117 | /** 118 | * @brief SSE optimized specialization for uint8_t images. 119 | */ 120 | template<> 121 | void getCentralGradient(const cv::Mat_& img, 122 | cv::Mat_* gradx, 123 | cv::Mat_* grady) { 124 | int width = img.cols; 125 | int height = img.rows; 126 | 127 | // Allocate outputs if needed. 128 | if (gradx->empty()) { 129 | gradx->create(height, width); 130 | } 131 | if (grady->empty()) { 132 | grady->create(height, width); 133 | } 134 | 135 | FLAME_ASSERT(img.isContinuous()); 136 | FLAME_ASSERT(gradx->isContinuous()); 137 | FLAME_ASSERT(grady->isContinuous()); 138 | 139 | // Grab raw pointers to data. 140 | const uint8_t* img_ptr = reinterpret_cast(img.data); 141 | float* gradx_ptr = reinterpret_cast(gradx->data); 142 | float* grady_ptr = reinterpret_cast(grady->data); 143 | 144 | getCentralGradient(width, height, img_ptr, gradx_ptr, grady_ptr); 145 | 146 | return; 147 | } 148 | 149 | /** 150 | * @brief SSE optimized specialization for float images. 151 | */ 152 | template <> 153 | void getCentralGradient(int width, int height, const float* img_ptr, 154 | float* gradx_ptr, float* grady_ptr) { 155 | __m128 half = _mm_set_ps(0.5f, 0.5f, 0.5f, 0.5); 156 | 157 | // Compute gradx. 158 | for (uint32_t ii = 0; ii < height; ++ii) { 159 | uint32_t jj = 0; 160 | for (jj = 1; jj < width - 4 - 1; jj+=4) { 161 | int idx = ii * width + jj; 162 | 163 | __m128 left = _mm_loadu_ps(img_ptr + idx - 1); 164 | __m128 right = _mm_loadu_ps(img_ptr + idx + 1); 165 | __m128 diffx = _mm_sub_ps(right, left); 166 | __m128 gx = _mm_mul_ps(half, diffx); 167 | _mm_storeu_ps(gradx_ptr + idx, gx); 168 | } 169 | 170 | // Finish remainder of row without SSE if not divisble by 4. 171 | for (; jj < width - 1; ++jj) { 172 | int idx = ii * width + jj; 173 | float left = img_ptr[idx - 1]; 174 | float right = img_ptr[idx + 1]; 175 | gradx_ptr[idx] = 0.5 * (right - left); 176 | } 177 | } 178 | 179 | // Compute gradient for first/last columns using forward/backward differences. 180 | for (uint32_t ii = 0; ii < height; ++ii) { 181 | // First column (forward difference). 182 | float left = img_ptr[ii * width]; 183 | float right = img_ptr[ii * width + 1]; 184 | gradx_ptr[ii * width] = right - left; 185 | 186 | // Last column (backward difference). 187 | left = img_ptr[ii * width + width - 2]; 188 | right = img_ptr[ii * width + width - 1]; 189 | gradx_ptr[ii * width + width - 1] = right - left; 190 | } 191 | 192 | // Compute grady. 193 | for (uint32_t ii = 1; ii < height - 1; ++ii) { 194 | uint32_t jj = 0; 195 | for (jj = 0; jj < width - 4; jj+=4) { 196 | int idx = ii * width + jj; 197 | 198 | __m128 up = _mm_loadu_ps(img_ptr + idx - width); 199 | __m128 down = _mm_loadu_ps(img_ptr + idx + width); 200 | __m128 diffy = _mm_sub_ps(down, up); 201 | __m128 gy = _mm_mul_ps(half, diffy); 202 | _mm_storeu_ps(grady_ptr + idx, gy); 203 | } 204 | 205 | // Finish remainder of row without SSE if not divisble by 4. 206 | for (; jj < width; ++jj) { 207 | int idx = ii * width + jj; 208 | float up = img_ptr[idx - width]; 209 | float down = img_ptr[idx + width]; 210 | grady_ptr[idx] = 0.5 * (down - up); 211 | } 212 | } 213 | 214 | // Compute gradient for first/last rows using forward/backward differences. 215 | for (uint32_t ii = 0; ii < width; ++ii) { 216 | // First row (forward difference). 217 | float up = img_ptr[ii]; 218 | float down = img_ptr[ii + width]; 219 | grady_ptr[ii] = down - up; 220 | 221 | // Last row (backward difference). 222 | up = img_ptr[(height - 2) * width + ii]; 223 | down = img_ptr[(height - 1) * width + ii]; 224 | grady_ptr[(height - 1) * width + ii] = down - up; 225 | } 226 | 227 | return; 228 | } 229 | 230 | /** 231 | * @brief SSE optimized specialization for float images. 232 | */ 233 | template<> 234 | void getCentralGradient(const cv::Mat_& img, 235 | cv::Mat_* gradx, 236 | cv::Mat_* grady) { 237 | int width = img.cols; 238 | int height = img.rows; 239 | 240 | // Allocate outputs if needed. 241 | if (gradx->empty()) { 242 | gradx->create(height, width); 243 | } 244 | if (grady->empty()) { 245 | grady->create(height, width); 246 | } 247 | 248 | FLAME_ASSERT(img.isContinuous()); 249 | FLAME_ASSERT(gradx->isContinuous()); 250 | FLAME_ASSERT(grady->isContinuous()); 251 | 252 | // Grab raw pointers to data. 253 | const float* img_ptr = reinterpret_cast(img.data); 254 | float* gradx_ptr = reinterpret_cast(gradx->data); 255 | float* grady_ptr = reinterpret_cast(grady->data); 256 | 257 | getCentralGradient(width, height, img_ptr, gradx_ptr, grady_ptr); 258 | 259 | return; 260 | } 261 | 262 | #endif 263 | 264 | // Liang-Barsky function by Daniel White @ 265 | // http://www.skytopia.com/project/articles/compsci/clipping.html This function 266 | // inputs 8 numbers, and outputs 4 new numbers (plus a boolean value to say 267 | // whether the clipped line is drawn at all). 268 | // 269 | bool clipLineLiangBarsky(float xmin, float xmax, 270 | float ymin, float ymax, 271 | float x0, float y0, 272 | float x1, float y1, 273 | float* x0_clip, float* y0_clip, 274 | float* x1_clip, float* y1_clip) { 275 | FLAME_ASSERT(!std::isnan(x0)); 276 | FLAME_ASSERT(!std::isnan(y0)); 277 | FLAME_ASSERT(!std::isnan(x1)); 278 | FLAME_ASSERT(!std::isnan(y1)); 279 | 280 | FLAME_ASSERT(!std::isnan(xmin)); 281 | FLAME_ASSERT(!std::isnan(xmax)); 282 | FLAME_ASSERT(!std::isnan(ymin)); 283 | FLAME_ASSERT(!std::isnan(ymax)); 284 | 285 | float t0 = 0.0f; 286 | float t1 = 1.0f; 287 | float xdelta = x1 - x0; 288 | float ydelta = y1 - y0; 289 | 290 | for (int edge = 0; edge < 4; edge++) { 291 | float p = 1.0f; 292 | float q = 0.0f; 293 | float r = 0.0f; 294 | 295 | if (edge == 0) { 296 | p = -xdelta; 297 | q = -(xmin-x0); 298 | } else if (edge == 1) { 299 | p = xdelta; 300 | q = (xmax-x0); 301 | } else if (edge == 2) { 302 | p = -ydelta; 303 | q = -(ymin-y0); 304 | } else if (edge == 3) { 305 | p = ydelta; 306 | q = (ymax-y0); 307 | } 308 | 309 | r = q/p; 310 | if (p == 0 && q < 0) 311 | return false; // Don't draw line at all. (parallel line outside) 312 | 313 | if (p < 0) { 314 | if (r > t1) 315 | return false; // Don't draw line at all. 316 | else if (r > t0) 317 | t0 = r; // Line is _clipped! 318 | } else if (p > 0) { 319 | if (r < t0) 320 | return false; // Don't draw line at all. 321 | else if (r < t1) 322 | t1 = r; // Line is _clipped! 323 | } 324 | } 325 | 326 | float x0_clip_int = x0 + t0 * xdelta; 327 | float y0_clip_int = y0 + t0 * ydelta; 328 | float x1_clip_int = x0 + t1 * xdelta; 329 | float y1_clip_int = y0 + t1 * ydelta; 330 | 331 | /* Final check for numerical errors. */ 332 | if (x0_clip_int < xmin) 333 | x0_clip_int = xmin; 334 | if (x0_clip_int > xmax) 335 | x0_clip_int = xmax; 336 | if (y0_clip_int < ymin) 337 | y0_clip_int = ymin; 338 | if (y0_clip_int > ymax) 339 | y0_clip_int = ymax; 340 | 341 | if (x1_clip_int < xmin) 342 | x1_clip_int = xmin; 343 | if (x1_clip_int > xmax) 344 | x1_clip_int = xmax; 345 | if (y1_clip_int < ymin) 346 | y1_clip_int = ymin; 347 | if (y1_clip_int > ymax) 348 | y1_clip_int = ymax; 349 | 350 | if (!(x0_clip_int >= xmin)) { 351 | fprintf(stderr, "x0_clip_int(%f) !>= xmin(%f)\n", 352 | x0_clip_int, xmin); 353 | } 354 | 355 | FLAME_ASSERT(x0_clip_int >= xmin); 356 | FLAME_ASSERT(x0_clip_int <= xmax); 357 | FLAME_ASSERT(y0_clip_int >= ymin); 358 | FLAME_ASSERT(y0_clip_int <= ymax); 359 | FLAME_ASSERT(x1_clip_int >= xmin); 360 | FLAME_ASSERT(x1_clip_int <= xmax); 361 | FLAME_ASSERT(y1_clip_int >= ymin); 362 | FLAME_ASSERT(y1_clip_int <= ymax); 363 | 364 | /* Set output. */ 365 | *x0_clip = x0_clip_int; 366 | *y0_clip = y0_clip_int; 367 | *x1_clip = x1_clip_int; 368 | *y1_clip = y1_clip_int; 369 | 370 | return true; // (clipped) line is drawn 371 | } 372 | 373 | void interpolateMesh(const std::vector& triangles, 374 | const std::vector& vertices, 375 | const std::vector& values, 376 | const std::vector& vtx_validity, 377 | const std::vector& tri_validity, 378 | cv::Mat* img) { 379 | for (int ii = 0; ii < triangles.size(); ++ii) { 380 | if (tri_validity[ii] && 381 | vtx_validity[triangles[ii][0]] && vtx_validity[triangles[ii][1]] && 382 | vtx_validity[triangles[ii][2]]) { 383 | // Triangle spits out points in clockwise order, but drawing function 384 | // expects CCW. 385 | utils::DrawShadedTriangleBarycentric(vertices[triangles[ii][2]], 386 | vertices[triangles[ii][1]], 387 | vertices[triangles[ii][0]], 388 | values[triangles[ii][2]], 389 | values[triangles[ii][1]], 390 | values[triangles[ii][0]], 391 | img); 392 | } 393 | } 394 | 395 | return; 396 | } 397 | 398 | void interpolateInverseMesh(const std::vector& triangles, 399 | const std::vector& vertices, 400 | const std::vector& ivalues, 401 | const std::vector& validity, 402 | cv::Mat* img) { 403 | for (int ii = 0; ii < triangles.size(); ++ii) { 404 | if (validity[triangles[ii][0]] && validity[triangles[ii][1]] && 405 | validity[triangles[ii][2]]) { 406 | // Triangle spits out points in clockwise order, but drawing function 407 | // expects CCW. 408 | utils::DrawShadedTriangleBarycentric(vertices[triangles[ii][2]], 409 | vertices[triangles[ii][1]], 410 | vertices[triangles[ii][0]], 411 | 1.0f/ivalues[triangles[ii][2]], 412 | 1.0f/ivalues[triangles[ii][1]], 413 | 1.0f/ivalues[triangles[ii][0]], 414 | img); 415 | } 416 | } 417 | 418 | return; 419 | } 420 | 421 | } // namespace utils 422 | 423 | } // namespace flame 424 | -------------------------------------------------------------------------------- /src/flame/stereo/line_stereo.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nicholas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file line_stereo.h 19 | * @author W. Nicholas Greene 20 | * @date 2017-06-16 20:41:47 (Fri) 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | #include 28 | 29 | #include "flame/utils/assert.h" 30 | #include "flame/utils/image_utils.h" 31 | 32 | namespace flame { 33 | 34 | namespace stereo { 35 | 36 | namespace line_stereo { 37 | 38 | enum Status { 39 | SUCCESS = 0, 40 | FAIL_AMBIGUOUS_MATCH = 1, 41 | FAIL_MAX_COST = 2, 42 | }; 43 | 44 | /** 45 | * @brief Parameter struct for line_stereo::match(). 46 | */ 47 | struct Params { 48 | float max_cost = 1300.0f; // Value for 5-sample SSD. 49 | 50 | bool do_subpixel = true; 51 | 52 | float sample_dist = 1.0f; // Distance in pixels between samples. 53 | 54 | // For a valid match, the second best cost must be greater than this factor 55 | // times the best cost. 56 | float second_best_factor = 1.5f; 57 | 58 | bool verbose = false; 59 | }; 60 | 61 | /** 62 | * @brief Match a 1D linear patch along the epipolar line. 63 | * 64 | * The comparison patch will be updating using a sliding window. 65 | * 66 | * **Derived from LSD-SLAM.** 67 | * 68 | * @param[in] ref_patch 1D reference patch. 69 | * @param[in] rescale_factor Warp of reference patch. 70 | * @param[in] start Start of search region in second image. 71 | * @param[in] end End of search region in second image. 72 | * @param[out] u_cmp Matched pixel. 73 | * @param[in] params Parameter struct. 74 | */ 75 | inline Status match(float rescale_factor, 76 | const cv::Mat1f& ref_patch, 77 | const cv::Mat1b& img_cmp, 78 | const cv::Point2f& start, const cv::Point2f& end, 79 | cv::Point2f* u_cmp, float* residual, 80 | const Params& params = Params()) { 81 | FLAME_ASSERT(ref_patch.rows == 1); 82 | FLAME_ASSERT(ref_patch.cols == 5); 83 | 84 | // These should be warped based on rescale_factor. 85 | float realVal_m2 = ref_patch(0, 0); 86 | float realVal_m1 = ref_patch(0, 1); 87 | float realVal = ref_patch(0, 2); 88 | float realVal_p1 = ref_patch(0, 3); 89 | float realVal_p2 = ref_patch(0, 4); 90 | 91 | // calculate increments in which we will step through the epipolar line. 92 | // they are sampleDist (or half sample dist) long 93 | float incx = end.x - start.x; 94 | float incy = end.y - start.y; 95 | float eplLength = sqrt(incx*incx + incy*incy); 96 | 97 | incx *= params.sample_dist / eplLength; 98 | incy *= params.sample_dist / eplLength; 99 | 100 | // from here on: 101 | // - pInf: search start-point 102 | // - p0: search end-point 103 | // - incx, incy: search steps in pixel 104 | // - eplLength, min_idepth, max_idepth: determines search-resolution, i.e. the result's variance. 105 | float cpx = start.x; 106 | float cpy = start.y; 107 | 108 | float val_cp_m2 = utils::bilinearInterp(img_cmp, 109 | cpx - 2.0f * incx, 110 | cpy - 2.0f * incy); 111 | float val_cp_m1 = utils::bilinearInterp(img_cmp, 112 | cpx - incx, 113 | cpy - incy); 114 | float val_cp = utils::bilinearInterp(img_cmp, cpx, cpy); 115 | float val_cp_p1 = utils::bilinearInterp(img_cmp, 116 | cpx + incx, 117 | cpy + incy); 118 | float val_cp_p2; 119 | 120 | /* 121 | * Subsequent exact minimum is found the following way: 122 | * - assuming lin. interpolation, the gradient of Error at p1 (towards p2) is given by 123 | * dE1 = -2sum(e1*e1 - e1*e2) 124 | * where e1 and e2 are summed over, and are the residuals (not squared). 125 | * 126 | * - the gradient at p2 (coming from p1) is given by 127 | * dE2 = +2sum(e2*e2 - e1*e2) 128 | * 129 | * - linear interpolation => gradient changes linearely; zero-crossing is hence given by 130 | * p1 + d*(p2-p1) with d = -dE1 / (-dE1 + dE2). 131 | * 132 | * => I for later exact min calculation, I need sum(e_i*e_i),sum(e_{i-1}*e_{i-1}),sum(e_{i+1}*e_{i+1}) 133 | * and sum(e_i * e_{i-1}) and sum(e_i * e_{i+1}), 134 | * where i is the respective winning index. 135 | */ 136 | 137 | // walk in equally sized steps, starting at depth=infinity. 138 | int loopCounter = 0; 139 | float best_match_x = -1; 140 | float best_match_y = -1; 141 | float best_match_err = std::numeric_limits::max(); 142 | float best_patch[5]; 143 | float second_best_match_err = std::numeric_limits::max(); 144 | 145 | // best pre and post errors. 146 | float best_match_errPre = std::numeric_limits::quiet_NaN(); 147 | float best_match_errPost = std::numeric_limits::quiet_NaN(); 148 | float best_match_DiffErrPre = std::numeric_limits::quiet_NaN(); 149 | float best_match_DiffErrPost = std::numeric_limits::quiet_NaN(); 150 | bool bestWasLastLoop = false; 151 | 152 | float eeLast = -1; // final error of last comp. 153 | 154 | // alternating intermediate vars 155 | float e1A = std::numeric_limits::quiet_NaN(); 156 | float e1B = std::numeric_limits::quiet_NaN(); 157 | float e2A = std::numeric_limits::quiet_NaN(); 158 | float e2B = std::numeric_limits::quiet_NaN(); 159 | float e3A = std::numeric_limits::quiet_NaN(); 160 | float e3B = std::numeric_limits::quiet_NaN(); 161 | float e4A = std::numeric_limits::quiet_NaN(); 162 | float e4B = std::numeric_limits::quiet_NaN(); 163 | float e5A = std::numeric_limits::quiet_NaN(); 164 | float e5B = std::numeric_limits::quiet_NaN(); 165 | 166 | int loopCBest = -1; 167 | int loopCSecond = -1; 168 | while (((incx < 0) == (cpx > end.x) && (incy < 0) == (cpy > end.y)) 169 | || loopCounter == 0) { 170 | // interpolate one new point 171 | val_cp_p2 = utils::bilinearInterp(img_cmp, 172 | cpx + 2 * incx, 173 | cpy + 2 * incy); 174 | 175 | // hacky but fast way to get error and differential error: switch buffer variables for last loop. 176 | float ee = 0.0f; 177 | if (loopCounter % 2 == 0) { 178 | // calc error and accumulate sums. 179 | e1A = val_cp_p2 - realVal_p2; 180 | ee += e1A*e1A; 181 | e2A = val_cp_p1 - realVal_p1; 182 | ee += e2A*e2A; 183 | e3A = val_cp - realVal; 184 | ee += e3A*e3A; 185 | e4A = val_cp_m1 - realVal_m1; 186 | ee += e4A*e4A; 187 | e5A = val_cp_m2 - realVal_m2; 188 | ee += e5A*e5A; 189 | } else { 190 | // calc error and accumulate sums. 191 | e1B = val_cp_p2 - realVal_p2; 192 | ee += e1B*e1B; 193 | e2B = val_cp_p1 - realVal_p1; 194 | ee += e2B*e2B; 195 | e3B = val_cp - realVal; 196 | ee += e3B*e3B; 197 | e4B = val_cp_m1 - realVal_m1; 198 | ee += e4B*e4B; 199 | e5B = val_cp_m2 - realVal_m2; 200 | ee += e5B*e5B; 201 | } 202 | 203 | // do I have a new winner?? 204 | // if so: set. 205 | if (ee < best_match_err) { 206 | // put to second-best 207 | second_best_match_err = best_match_err; 208 | loopCSecond = loopCBest; 209 | 210 | // set best. 211 | best_match_err = ee; 212 | loopCBest = loopCounter; 213 | 214 | best_match_errPre = eeLast; 215 | best_match_DiffErrPre = e1A*e1B + e2A*e2B + e3A*e3B + e4A*e4B + e5A*e5B; 216 | best_match_errPost = -1; 217 | best_match_DiffErrPost = -1; 218 | 219 | best_match_x = cpx; 220 | best_match_y = cpy; 221 | bestWasLastLoop = true; 222 | 223 | best_patch[0] = val_cp_m2; 224 | best_patch[1] = val_cp_m1; 225 | best_patch[2] = val_cp; 226 | best_patch[3] = val_cp_p1; 227 | best_patch[4] = val_cp_p2; 228 | } else { 229 | // otherwise: the last might be the current winner, in which case i have to save these values. 230 | if (bestWasLastLoop) { 231 | best_match_errPost = ee; 232 | best_match_DiffErrPost = e1A*e1B + e2A*e2B + e3A*e3B + 233 | e4A*e4B + e5A*e5B; 234 | bestWasLastLoop = false; 235 | } 236 | 237 | // collect second-best: 238 | // just take the best of all that are NOT equal to current best. 239 | if (ee < second_best_match_err) { 240 | second_best_match_err = ee; 241 | loopCSecond = loopCounter; 242 | } 243 | } 244 | 245 | // shift everything one further. 246 | eeLast = ee; 247 | val_cp_m2 = val_cp_m1; 248 | val_cp_m1 = val_cp; 249 | val_cp = val_cp_p1; 250 | val_cp_p1 = val_cp_p2; 251 | 252 | cpx += incx; 253 | cpy += incy; 254 | 255 | loopCounter++; 256 | } 257 | 258 | *residual = best_match_err; 259 | 260 | if (best_match_err > 4.0f * params.max_cost) { 261 | // Best error exceeded threshold. 262 | if (params.verbose) { 263 | fprintf(stderr, "line_stereo::match[FAIL]: best_cost (%f) is greater than threshold (%f)\n", 264 | best_match_err, 4.0f * params.max_cost); 265 | fprintf(stderr, "ref = [%.2f, %.2f, %.2f, %.2f, %.2f], cmp = [%.2f, %.2f, %.2f, %.2f, %.2f]\n", 266 | ref_patch(0, 0), ref_patch(0, 1), ref_patch(0, 2), ref_patch(0, 3), ref_patch(0, 4), 267 | best_patch[0], best_patch[1], best_patch[2], best_patch[3], best_patch[4]); 268 | } 269 | return Status::FAIL_MAX_COST; 270 | } 271 | 272 | // Check if best match is significantly better than second best. 273 | if ((utils::fast_abs(loopCBest - loopCSecond) > 1.0f) && 274 | (params.second_best_factor * best_match_err > second_best_match_err)) { 275 | if (params.verbose) { 276 | fprintf(stderr, "line_stereo::match[FAIL]: Ambiguous match (best_cost %f, second_best_cost = %f, ratio = %f)\n", 277 | best_match_err, second_best_match_err, second_best_match_err / best_match_err); 278 | fprintf(stderr, "ref = [%.2f, %.2f, %.2f, %.2f, %.2f], cmp = [%.2f, %.2f, %.2f, %.2f, %.2f]\n", 279 | ref_patch(0, 0), ref_patch(0, 1), ref_patch(0, 2), ref_patch(0, 3), ref_patch(0, 4), 280 | best_patch[0], best_patch[1], best_patch[2], best_patch[3], best_patch[4]); 281 | } 282 | return Status::FAIL_AMBIGUOUS_MATCH; 283 | } 284 | 285 | bool didSubpixel = false; 286 | if (params.do_subpixel) { 287 | // ================== compute exact match ========================= 288 | // compute gradients (they are actually only half the real gradient) 289 | float gradPre_pre = -(best_match_errPre - best_match_DiffErrPre); 290 | float gradPre_this = +(best_match_err - best_match_DiffErrPre); 291 | float gradPost_this = -(best_match_err - best_match_DiffErrPost); 292 | float gradPost_post = +(best_match_errPost - best_match_DiffErrPost); 293 | 294 | // final decisions here. 295 | bool interpPost = false; 296 | bool interpPre = false; 297 | 298 | // if one is oob: return false. 299 | if (best_match_errPre < 0 || best_match_errPost < 0) { 300 | // stats->num_stereo_invalid_atEnd++; 301 | } else if ((gradPost_this < 0) ^ (gradPre_this < 0)) { 302 | // - if zero-crossing occurs exactly in between (gradient Inconsistent), 303 | // return exact pos, if both central gradients are small compared to their counterpart. 304 | if (gradPost_this * gradPost_this > 0.1f * 0.1f * gradPost_post * gradPost_post || 305 | gradPre_this*gradPre_this > 0.1f*0.1f*gradPre_pre*gradPre_pre) { 306 | // stats->num_stereo_invalid_inexistantCrossing++; 307 | } 308 | } else if ((gradPre_pre < 0) ^ (gradPre_this < 0)) { 309 | // if pre has zero-crossing 310 | // if post has zero-crossing 311 | if ((gradPost_post < 0) ^ (gradPost_this < 0)) { 312 | // stats->num_stereo_invalid_twoCrossing++; 313 | } else { 314 | interpPre = true; 315 | } 316 | } else if ((gradPost_post < 0) ^ (gradPost_this < 0)) { 317 | // if post has zero-crossing 318 | interpPost = true; 319 | } else { 320 | // if none has zero-crossing 321 | // stats->num_stereo_invalid_noCrossing++; 322 | } 323 | 324 | // DO interpolation! 325 | // minimum occurs at zero-crossing of gradient, which is a straight line => easy to compute. 326 | // the error at that point is also computed by just integrating. 327 | if (interpPre) { 328 | float d = gradPre_this / (gradPre_this - gradPre_pre); 329 | best_match_x -= d*incx; 330 | best_match_y -= d*incy; 331 | best_match_err = best_match_err - 2*d*gradPre_this - 332 | (gradPre_pre - gradPre_this)*d*d; 333 | didSubpixel = true; 334 | } else if (interpPost) { 335 | float d = gradPost_this / (gradPost_this - gradPost_post); 336 | best_match_x += d*incx; 337 | best_match_y += d*incy; 338 | best_match_err = best_match_err + 2*d*gradPost_this + 339 | (gradPost_post - gradPost_this)*d*d; 340 | didSubpixel = true; 341 | } else { 342 | } 343 | } 344 | 345 | *residual = best_match_err; 346 | 347 | // sample_dist is the distance in pixel at which the realVal's were sampled 348 | float sampleDist = params.sample_dist * rescale_factor; 349 | 350 | float gradAlongLine = 0; 351 | float tmp = realVal_p2 - realVal_p1; 352 | gradAlongLine += tmp * tmp; 353 | tmp = realVal_p1 - realVal; 354 | gradAlongLine += tmp * tmp; 355 | tmp = realVal - realVal_m1; 356 | gradAlongLine += tmp * tmp; 357 | tmp = realVal_m1 - realVal_m2; 358 | gradAlongLine += tmp * tmp; 359 | 360 | gradAlongLine /= sampleDist * sampleDist; 361 | 362 | // check if interpolated error is OK. use evil hack to allow more error if there is a lot of gradient. 363 | if (best_match_err > params.max_cost + sqrtf(gradAlongLine) * 20) { 364 | if (params.verbose) { 365 | fprintf(stderr, "line_stereo::match[FAIL]: best_cost (%f) is greater than threshold (%f)\n", 366 | best_match_err, params.max_cost + sqrtf(gradAlongLine) * 20); 367 | fprintf(stderr, "ref = [%.2f, %.2f, %.2f, %.2f, %.2f], cmp = [%.2f, %.2f, %.2f, %.2f, %.2f]\n", 368 | ref_patch(0, 0), ref_patch(0, 1), ref_patch(0, 2), ref_patch(0, 3), ref_patch(0, 4), 369 | best_patch[0], best_patch[1], best_patch[2], best_patch[3], best_patch[4]); 370 | } 371 | return Status::FAIL_MAX_COST; 372 | } 373 | 374 | // if (params.verbose) { 375 | // fprintf(stderr, "line_stereo::match[SUCCESS]\n"); 376 | // fprintf(stderr, "ref = [%.2f, %.2f, %.2f, %.2f, %.2f], cmp = [%.2f, %.2f, %.2f, %.2f, %.2f]\n", 377 | // ref_patch(0, 0), ref_patch(0, 1), ref_patch(0, 2), ref_patch(0, 3), ref_patch(0, 4), 378 | // best_patch[0], best_patch[1], best_patch[2], best_patch[3], best_patch[4]); 379 | // } 380 | 381 | u_cmp->x = best_match_x; 382 | u_cmp->y = best_match_y; 383 | 384 | return Status::SUCCESS; 385 | } 386 | 387 | } // namespace line_stereo 388 | 389 | } // namespace stereo 390 | 391 | } // namespace flame 392 | -------------------------------------------------------------------------------- /src/flame/stereo/epipolar_geometry.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of FLaME. 3 | * Copyright (C) 2017 W. Nichoilas Greene (wng@csail.mit.edu) 4 | * 5 | * This program is free software; you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, see . 17 | * 18 | * @file epipolar_geometry.h 19 | * @author W. Nicholas Greene 20 | * @date 2017-08-18 19:17:13 (Fri) 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | #include 28 | 29 | #include "flame/types.h" 30 | #include "flame/utils/assert.h" 31 | #include "flame/utils/image_utils.h" 32 | 33 | namespace flame { 34 | 35 | namespace stereo { 36 | 37 | /** 38 | * \brief Class that represents an epipolar geometry setup. 39 | * 40 | * Useful for epipolar geometry queries (epipolar lines, projections, depth, 41 | * etc.) 42 | */ 43 | template 44 | class EpipolarGeometry final { 45 | // Convenience aliases. 46 | using Point2s = cv::Point_; // TODO(wng): Use core types. 47 | using Vector3s = Vector3; 48 | using Matrix3s = Matrix3; 49 | using Quaternions = Quaternion; 50 | 51 | public: 52 | /** 53 | * \brief Constructor. 54 | * 55 | * The comparison camera is the camera that pixels are projected onto to form 56 | * epipolar lines and compute disparity. The reference camera is the camera 57 | * that depths are define relative to. 58 | * 59 | * @param[in] K Camera intrinsic matrix. 60 | * @param[in] Kinv Inverse camera intrinsic matrix. 61 | */ 62 | EpipolarGeometry(const Matrix3s& K, const Matrix3s& Kinv) : 63 | K_(K), 64 | Kinv_(Kinv), 65 | q_ref_to_cmp_(), 66 | t_ref_to_cmp_(), 67 | t_cmp_to_ref_(), 68 | KRKinv3_(), 69 | Kt_(), 70 | epipole_() {} 71 | EpipolarGeometry() = default; 72 | ~EpipolarGeometry() = default; 73 | 74 | EpipolarGeometry(const EpipolarGeometry& rhs) = default; 75 | EpipolarGeometry& operator=(const EpipolarGeometry& rhs) = default; 76 | 77 | EpipolarGeometry(EpipolarGeometry&& rhs) = default; 78 | EpipolarGeometry& operator=(EpipolarGeometry&& rhs) = default; 79 | 80 | /** 81 | * \brief Load an epipolar geometry setup. 82 | * 83 | * @param[in] q_ref_to_cmp Rotation from reference to comparison frame. 84 | * @param[in] t_ref_to_cmp Translation from reference to compariosn frame. 85 | */ 86 | void loadGeometry(const Quaternions& q_ref_to_cmp, 87 | const Vector3s& t_ref_to_cmp) { 88 | q_ref_to_cmp_ = q_ref_to_cmp; 89 | t_ref_to_cmp_ = t_ref_to_cmp; 90 | t_cmp_to_ref_ = -(q_ref_to_cmp_.inverse() * t_ref_to_cmp_); 91 | KRKinv_ = K_ * q_ref_to_cmp_.toRotationMatrix() * Kinv_; 92 | KRKinv3_ = KRKinv_.row(2); 93 | Kt_ = K_ * t_ref_to_cmp_; 94 | 95 | if (t_ref_to_cmp_(2) > 0) { 96 | // Precompute epipole. 97 | epipole_.x = (K_(0, 0) * t_ref_to_cmp_(0) + K_(0, 2) * t_ref_to_cmp_(2)) / 98 | t_ref_to_cmp_(2); 99 | epipole_.y = (K_(1, 1) * t_ref_to_cmp_(1) + K_(1, 2) * t_ref_to_cmp_(2)) / 100 | t_ref_to_cmp_(2); 101 | } 102 | return; 103 | } 104 | 105 | /** 106 | * \brief Helper function for perspective projection. 107 | * 108 | * @param[in] K Camera intrinsic matrix. 109 | * @param[in] q Orientation of camera in world. 110 | * @param[in] t Translation of camera in world. 111 | * @param[in] p Point to project in world. 112 | * @return Projected pixel. 113 | */ 114 | static Point2s project(const Matrix3s& K, const Quaternions& q, 115 | const Vector3s& t, const Vector3s& p) { 116 | Vector3s p_cam(q.inverse() * (p - t)); 117 | return Point2s((K(0, 0) * p_cam(0) + K(0, 2) * p_cam(2)) / p_cam(2), 118 | (K(1, 1) * p_cam(1) + K(1, 2) * p_cam(2)) / p_cam(2)); 119 | } 120 | 121 | /** 122 | * \brief Project pixel u_ref into comparison frame assuming inverse depth. 123 | * 124 | * @param[in] u_ref Reference pixel. 125 | * @param[in] idepth Inverse depth. 126 | */ 127 | Point2s project(const Point2s& u_ref, Scalar idepth) const { 128 | FLAME_ASSERT(idepth >= 0.0f); 129 | if (idepth == 0.0f) { 130 | Point2s u_max; 131 | maxDepthProjection(u_ref, &u_max); 132 | return u_max; 133 | } 134 | 135 | Scalar depth = 1.0f / idepth; 136 | Vector3s u_ref_hom(u_ref.x * depth, u_ref.y * depth, depth); 137 | Vector3s u_cmp_hom(KRKinv_ * u_ref_hom + Kt_); 138 | 139 | FLAME_ASSERT(utils::fast_abs(u_cmp_hom(2)) > 0.0f); 140 | 141 | Scalar inv_u_cmp_hom2 = 1.0f / u_cmp_hom(2); 142 | return Point2s(u_cmp_hom(0) * inv_u_cmp_hom2, u_cmp_hom(1) * inv_u_cmp_hom2); 143 | } 144 | 145 | /** 146 | * \brief Project pixel u_ref into comparison frame assuming inverse depth. 147 | * 148 | * @param[in] u_ref Reference pixel. 149 | * @param[in] idepth Inverse depth. 150 | * @param[out] u_cmp Projected pixel. 151 | * @param[out] new_idepth New inverse depth. 152 | */ 153 | void project(const Point2s& u_ref, Scalar idepth, 154 | Point2s* u_cmp, Scalar* new_idepth) const { 155 | FLAME_ASSERT(idepth >= 0.0f); 156 | if (idepth == 0.0f) { 157 | Point2s u_max; 158 | maxDepthProjection(u_ref, &u_max); 159 | *u_cmp = u_max; 160 | *new_idepth = 0.0f; 161 | return; 162 | } 163 | 164 | Scalar depth = 1.0f / idepth; 165 | Vector3s p_ref(Kinv_(0, 0) * u_ref.x + Kinv_(0, 2), 166 | Kinv_(1, 1) * u_ref.y + Kinv_(1, 2), 167 | 1.0f); 168 | p_ref *= depth; 169 | 170 | Vector3s p_cmp(q_ref_to_cmp_ * p_ref + t_ref_to_cmp_); 171 | Vector3s u_cmp3(K_(0, 0) * p_cmp(0) + K_(0, 2) * p_cmp(2), 172 | K_(1, 1) * p_cmp(1) + K_(1, 2) * p_cmp(2), 173 | p_cmp(2)); 174 | FLAME_ASSERT(fabs(u_cmp3(2)) > 0.0f); 175 | 176 | *new_idepth = 1.0f / p_cmp(2); 177 | u_cmp->x = u_cmp3(0) * (*new_idepth); 178 | u_cmp->y = u_cmp3(1) * (*new_idepth); 179 | return; 180 | } 181 | 182 | /** 183 | * \brief Compute projection of pixel with infinite depth. 184 | * 185 | * Compute the projection of pixel u_ref into the comparison image assuming 186 | * infinite depth. 187 | * 188 | * @param u_ref[in] Pixel in the reference image to project. 189 | * @param u_inf[out] The projection corresponding to infinite depth. 190 | */ 191 | void maxDepthProjection(const Point2s& u_ref, Point2s* u_inf) const { 192 | Vector3s u_ref_hom(u_ref.x, u_ref.y, 1.0f); 193 | Vector3s u_cmp_hom(KRKinv_ * u_ref_hom); 194 | 195 | FLAME_ASSERT(fabs(u_cmp_hom(2)) > 0.0f); 196 | 197 | Scalar inv_u_cmp_hom2 = 1.0f / u_cmp_hom(2); 198 | u_inf->x = u_cmp_hom(0) * inv_u_cmp_hom2; 199 | u_inf->y = u_cmp_hom(1) * inv_u_cmp_hom2; 200 | return; 201 | } 202 | 203 | /** 204 | * \brief Compute the epiline endpoint. 205 | * 206 | * There are several ways to compute the epiline endpoint. The most natural 207 | * way is if the reference camera is in front of the comparison camera in the 208 | * comparison camera frame (that is t_ref_to_cmp_z > 0). Then the epiline 209 | * endpoint is just the epipole (i.e. the projection of the reference camera 210 | * into the comparison camera). This corresponds to the point in the world 211 | * having 0 depth. 212 | * 213 | * Things get more complicated however if t_ref_to_cmp_z <= 0 (i.e. the 214 | * reference camera lies at the same z or behind the comparison camera). 215 | 216 | * If t_ref_to_cmp_z = 0, then the epipole lies at infinity and all epilines 217 | * are parallel (typically the case for a traditional stereo setup). In this 218 | * case, the vector (fx * t_ref_to_cmp_x, fy * t_ref_to_cmp_y) is parallel to 219 | * the epiline. Given the infinity point, the minimum depth point is simply 220 | * the infinite point plus a large value times this vector. 221 | * 222 | * If t_ref_to_cmp_z < 0, then the minimum possible depth of the point must be 223 | * 0 in order to be projected into both camera. In this case, we simply 224 | * compute the depth in the reference frame such * that the point has depth 1 225 | * in the comparison frame and then project this * point into the comparison 226 | * camera 227 | * 228 | * It is also possible to compute the epiline using the fundamental matrix 229 | * F. If epiline is parameterized by the implicit equation l^T u_cmp = 0, 230 | * where u_cmp are homogenous pixels in the comparison image, then l = F 231 | * u_ref. This formulation, however, does not give the *direction* of the 232 | * epiline from far depth to near depth, which is what we would like. 233 | * 234 | * @param u_ref[in] Pixel in the reference image to project. 235 | * @param u_min[out] The projection corresponding to minimum depth. 236 | */ 237 | void minDepthProjection(const Point2s& u_ref, Point2s* u_min) const { 238 | if (t_ref_to_cmp_(2) > 0) { 239 | *u_min = epipole_; 240 | } else if (t_ref_to_cmp_(2) == 0) { 241 | // Compute epiline direction. 242 | Point2s epi(K_(0, 0) * t_ref_to_cmp_(0), K_(1, 1) * t_ref_to_cmp_(1)); 243 | Point2s u_inf; 244 | maxDepthProjection(u_ref, &u_inf); 245 | *u_min = u_inf + 1e6 * epi; 246 | } else { 247 | // Compute depth in the ref frame such that point has depth 1 in comparison 248 | // frame. 249 | Vector3s qp_ref(Kinv_(0, 0) * u_ref.x + Kinv_(0, 2), 250 | Kinv_(1, 1) * u_ref.y + Kinv_(1, 2), 251 | 1.0f); 252 | qp_ref = q_ref_to_cmp_ * qp_ref; 253 | Scalar min_depth = (1.0f - t_ref_to_cmp_(2)) / qp_ref(2); 254 | 255 | Vector3s p_cmp(min_depth * qp_ref + t_ref_to_cmp_); 256 | FLAME_ASSERT(p_cmp(2) > 0.0f); 257 | 258 | u_min->x = (K_(0, 0) * p_cmp(0) + K_(0, 2) * p_cmp(2)) / p_cmp(2); 259 | u_min->y = (K_(1, 1) * p_cmp(1) + K_(1, 2) * p_cmp(2)) / p_cmp(2); 260 | } 261 | 262 | return; 263 | } 264 | 265 | /** 266 | * \brief Compute epipolar line corresponding to pixel. 267 | * 268 | * Computes the epipolar line of pixel u_ref in the cmp image. The line points 269 | * from infinite depth to minimum depth and is computed by finding the pixels 270 | * corresponding to infinite depth and minimum depth in the comparison image. 271 | * 272 | * It is also possible to compute the epiline using the fundamental matrix 273 | * F. If epiline is parameterized by the implicit equation l^T u_cmp = 0, 274 | * where u_cmp are homogenous pixels in the comparison image, then l = F 275 | * u_ref. This formulation, however, does not give the *direction* of the 276 | * epiline from far depth to near depth, which is what we would like. 277 | * 278 | * @param u_ref[in] Reference pixel. 279 | * @param u_inf[out] Start of epipolar line (point of infinite depth). 280 | & @param epi[out] Epipolar unit vector. 281 | */ 282 | void epiline(const Point2s& u_ref, Point2s* u_inf, Point2s* epi) const { 283 | Point2s u_zero; 284 | minDepthProjection(u_ref, &u_zero); 285 | maxDepthProjection(u_ref, u_inf); 286 | *epi = u_zero - *u_inf; 287 | Scalar norm2 = epi->x*epi->x + epi->y*epi->y; 288 | 289 | if (norm2 > 1e-10) { 290 | Scalar inv_norm = 1.0f / sqrt(norm2); 291 | epi->x *= inv_norm; 292 | epi->y *= inv_norm; 293 | } else { 294 | // If u_zero == u_inf, then epi mag is 0. 295 | epi->x = 0.0f; 296 | epi->y = 0.0f; 297 | } 298 | 299 | return; 300 | } 301 | 302 | /** 303 | * @brief Return the epiline that corresponds to u_ref in the reference 304 | * image. This is the projection of the epipolar plane onto the reference 305 | * image at u_ref. It points from near depth to far depth (opposite of what's 306 | * returned from epiline). 307 | * 308 | * @param[in] u_ref Reference pixel. 309 | * @param[out] epi Epipolar line. 310 | */ 311 | void referenceEpiline(const Point2s& u_ref, Point2s* epi) const { 312 | // Get epiline in reference image for the template. 313 | // calculate the plane spanned by the two camera centers and the point (x,y,1) 314 | // intersect it with the keyframe's image plane (at depth = 1) 315 | // This is the epipolar line in the keyframe. 316 | Point2s epi_ref; 317 | epi_ref.x = -K_(0, 0) * t_cmp_to_ref_(0) + 318 | t_cmp_to_ref_(2)*(u_ref.x - K_(0, 2)); 319 | epi_ref.y = -K_(1, 1) * t_cmp_to_ref_(1) + 320 | t_cmp_to_ref_(2)*(u_ref.y - K_(1, 2)); 321 | 322 | Scalar epi_ref_norm2 = epi_ref.x * epi_ref.x + epi_ref.y * epi_ref.y; 323 | FLAME_ASSERT(epi_ref_norm2 > 0); 324 | Scalar inv_epi_ref_norm = 1.0f / sqrt(epi_ref_norm2); 325 | epi_ref.x *= inv_epi_ref_norm; 326 | epi_ref.y *= inv_epi_ref_norm; 327 | 328 | *epi = epi_ref; 329 | 330 | return; 331 | } 332 | 333 | /** 334 | * \brief Compute disparity from pixel correspondence. 335 | * 336 | * @param u_ref[in] Reference pixel. 337 | * @param u_cmp[in] Comparison pixel. 338 | * @param u_inf[out] Endpoint of epipolar line (point of infinite depth). 339 | * @param epi[out] Epipolar line direction. 340 | * @param disparity[out] Disparity. 341 | */ 342 | Scalar disparity(const Point2s& u_ref, const Point2s& u_cmp, 343 | Point2s* u_inf, Point2s* epi) const { 344 | epiline(u_ref, u_inf, epi); 345 | return epi->x*(u_cmp.x - u_inf->x) + epi->y*(u_cmp.y - u_inf->y); 346 | } 347 | Scalar disparity(const Point2s& u_ref, const Point2s& u_cmp) const { 348 | Point2s u_inf, epi; 349 | return disparity(u_ref, u_cmp, &u_inf, &epi); 350 | } 351 | Scalar disparity(const Point2s& u_ref, const Point2s& u_cmp, 352 | const Point2s& u_inf, const Point2s& epi) const { 353 | return epi.x*(u_cmp.x - u_inf.x) + epi.y*(u_cmp.y - u_inf.y); 354 | } 355 | 356 | /** 357 | * \brief Compute depth from disparity. 358 | * 359 | * @param u_ref[in] Reference pixel 360 | * @param u_inf[in] Epipolar line start point (infinite depth). 361 | * @param epi[in] Epipolar line direction. 362 | * @param disparity[in] Disparity. 363 | * @return depth 364 | */ 365 | Scalar disparityToDepth(const Point2s& u_ref, const Point2s& u_inf, 366 | const Point2s& epi, const Scalar disparity) const { 367 | FLAME_ASSERT(disparity >= 0.0f); 368 | Scalar w = KRKinv3_(0) * u_ref.x + KRKinv3_(1) * u_ref.y + KRKinv3_(2); 369 | Point2s A(w * disparity * epi); 370 | Point2s b(Kt_(0) - Kt_(2)*(u_inf.x + disparity * epi.x), 371 | Kt_(1) - Kt_(2)*(u_inf.y + disparity * epi.y)); 372 | 373 | Scalar ATA = A.x*A.x + A.y*A.y; 374 | Scalar ATb = A.x*b.x + A.y*b.y; 375 | 376 | FLAME_ASSERT(ATA > 0.0f); 377 | 378 | return ATb/ATA; 379 | } 380 | 381 | /** 382 | * \brief Compute inverse depth from disparity. 383 | * 384 | * @param KR_ref_to_cmpKinv3 Third row of K*R_ref_to_cmp*Kinv. 385 | * @param Kt_ref_to_cmp[in] K * translation from ref to cmp. 386 | * @param u_ref[in] Reference pixel 387 | * @param u_inf[in] Epipolar line start point (infinite depth). 388 | * @param epi[in] Epipolar line direction. 389 | * @param disparity[in] Disparity. 390 | * @return inverse depth 391 | */ 392 | Scalar disparityToInverseDepth(const Point2s& u_ref, const Point2s& u_inf, 393 | const Point2s& epi, 394 | const Scalar disparity) const { 395 | FLAME_ASSERT(disparity >= 0.0f); 396 | Scalar w = KRKinv3_(0) * u_ref.x + KRKinv3_(1) * u_ref.y + KRKinv3_(2); 397 | Point2s A(Kt_(0) - Kt_(2)*(u_inf.x + disparity * epi.x), 398 | Kt_(1) - Kt_(2)*(u_inf.y + disparity * epi.y)); 399 | Point2s b(w * disparity * epi); 400 | 401 | Scalar ATA = A.x*A.x + A.y*A.y; 402 | Scalar ATb = A.x*b.x + A.y*b.y; 403 | 404 | FLAME_ASSERT(ATA > 0.0f); 405 | 406 | return ATb/ATA; 407 | } 408 | 409 | // Accessors. 410 | const Matrix3s& K() const { return K_; } 411 | const Matrix3s& Kinv() const { return Kinv_; } 412 | const Quaternions& getRefToCmpQuat() const { return q_ref_to_cmp_; } 413 | const Vector3s& getRefToCmpTrans() const { return t_ref_to_cmp_; } 414 | 415 | private: 416 | // Camera parameters. 417 | Matrix3s K_; 418 | Matrix3s Kinv_; 419 | 420 | // Geometry. 421 | Quaternions q_ref_to_cmp_; 422 | Vector3s t_ref_to_cmp_; 423 | Vector3s t_cmp_to_ref_; 424 | Matrix3s KRKinv_; 425 | Vector3s KRKinv3_; 426 | Vector3s Kt_; 427 | Point2s epipole_; // Projection of cmp camera in ref camera. 428 | }; 429 | 430 | } // namespace stereo 431 | 432 | } // namespace flame 433 | --------------------------------------------------------------------------------