├── ptam ├── src │ ├── ptam │ │ └── cfg │ │ │ └── __init__.py │ ├── Map.cc │ ├── ShiTomasi.cc │ ├── main.cc │ ├── MapPoint.cc │ ├── Relocaliser.cc │ ├── Params.cpp │ ├── MiniPatch.cc │ ├── OctomapInterface.cc │ ├── RemotePtam.cpp │ ├── MapViewer.cc │ ├── CalibCornerPatch.cc │ └── SmallBlurryImage.cc ├── thirdparty │ ├── TooN │ │ ├── version.txt │ │ ├── Authors │ │ ├── Makefile │ │ └── COPYING │ ├── agast │ │ ├── Makefile │ │ ├── getIncludePaths.cmake │ │ ├── CMakeLists.txt │ │ ├── installfiles │ │ │ ├── Makefile_orig │ │ │ ├── Makefile │ │ │ └── README │ │ ├── include │ │ │ └── agast │ │ │ │ ├── cvWrapper.h │ │ │ │ ├── agast5_8.h │ │ │ │ ├── AstDetector.h │ │ │ │ ├── agast7_12d.h │ │ │ │ ├── agast7_12s.h │ │ │ │ ├── oast9_16.h │ │ │ │ └── agast_corner_detect.h │ │ └── src │ │ │ ├── AstDetector.cc │ │ │ └── nonMaximumSuppression.cc │ ├── libcvd │ │ ├── .gitignore │ │ └── Makefile │ ├── gvars3 │ │ ├── .gitignore │ │ └── Makefile │ ├── rosdep.yaml │ └── Makefile ├── Makefile_ptam ├── calib_pattern.pdf ├── .gitignore ├── Makefile ├── launch │ ├── ptam.launch │ └── cameracalibrator.launch ├── include │ └── ptam │ │ ├── ShiTomasi.h │ │ ├── OpenGL.h │ │ ├── OctomapInterface.h │ │ ├── Relocaliser.h │ │ ├── SmallMatrixOpts.h │ │ ├── MapViewer.h │ │ ├── SmallBlurryImage.h │ │ ├── Map.h │ │ ├── CalibCornerPatch.h │ │ ├── MiniPatch.h │ │ ├── CameraCalibrator.h │ │ ├── CalibImage.h │ │ ├── LevelHelpers.h │ │ ├── HomographyInit.h │ │ ├── GLWindowMenu.h │ │ ├── GLWindow2.h │ │ ├── Params.h │ │ ├── MapPoint.h │ │ ├── System.h │ │ ├── TrackerData.h │ │ ├── AxesArray.h │ │ ├── KeyFrame.h │ │ ├── Bundle.h │ │ ├── MapMaker.h │ │ ├── MEstimator.h │ │ ├── Tracker.h │ │ ├── PatchFinder.h │ │ └── ATANCamera.h ├── mainpage.dox ├── PtamFixParams.yaml ├── package.xml ├── cfg │ ├── PTAMVisualizerParams.cfg │ └── PtamParams.cfg ├── rvizconf.vcg ├── license.txt └── CMakeLists.txt ├── ptam_com ├── Makefile ├── msg │ ├── OctoMapPointArray.msg │ ├── Vector3Array.msg │ ├── KeyFrame_msg.msg │ ├── OctoMapScan.msg │ ├── OctoMapPointStamped.msg │ └── ptam_info.msg ├── srv │ ├── PointCloud.srv │ └── KeyFrame_srv.srv ├── mainpage.dox ├── CMakeLists.txt └── package.xml ├── rqt_ptam ├── Makefile ├── mainpage.dox ├── plugin.xml ├── CMakeLists.txt ├── package.xml ├── include │ └── rqt_ptam │ │ ├── ratio_layouted_frame.h │ │ └── remote_ptam.h └── src │ └── rqt_ptam │ ├── ratio_layouted_frame.cpp │ └── remote_ptam.ui ├── ethzasl_ptam ├── CMakeLists.txt └── package.xml ├── README.md ├── Makefile └── .gitignore /ptam/src/ptam/cfg/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /ptam/thirdparty/TooN/version.txt: -------------------------------------------------------------------------------- 1 | 2.0.0-beta7 2 | -------------------------------------------------------------------------------- /ptam_com/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /rqt_ptam/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /ptam/Makefile_ptam: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk 2 | -------------------------------------------------------------------------------- /ptam/thirdparty/agast/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /ptam/thirdparty/libcvd/.gitignore: -------------------------------------------------------------------------------- 1 | /include 2 | /lib 3 | /libcvd_built 4 | -------------------------------------------------------------------------------- /ptam_com/msg/OctoMapPointArray.msg: -------------------------------------------------------------------------------- 1 | OctoMapPointStamped[] mapPoints 2 | -------------------------------------------------------------------------------- /ptam_com/msg/Vector3Array.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Vector3[] vector3 -------------------------------------------------------------------------------- /ptam/thirdparty/gvars3/.gitignore: -------------------------------------------------------------------------------- 1 | /include 2 | /installfiles 3 | /lib 4 | /gvars_built 5 | -------------------------------------------------------------------------------- /ptam_com/srv/PointCloud.srv: -------------------------------------------------------------------------------- 1 | uint32 flags 2 | --- 3 | sensor_msgs/PointCloud2 pointcloud 4 | -------------------------------------------------------------------------------- /ptam/calib_pattern.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/ethzasl_ptam/HEAD/ptam/calib_pattern.pdf -------------------------------------------------------------------------------- /ptam_com/msg/KeyFrame_msg.msg: -------------------------------------------------------------------------------- 1 | uint32[] KFids 2 | geometry_msgs/PoseWithCovarianceStamped[] KFs 3 | -------------------------------------------------------------------------------- /ptam_com/msg/OctoMapScan.msg: -------------------------------------------------------------------------------- 1 | OctoMapPointArray mapPoints 2 | geometry_msgs/PoseWithCovarianceStamped keyFramePose 3 | 4 | -------------------------------------------------------------------------------- /ptam_com/srv/KeyFrame_srv.srv: -------------------------------------------------------------------------------- 1 | int32 flags 2 | --- 3 | uint32[] KFids 4 | geometry_msgs/PoseWithCovarianceStamped[] KFs 5 | -------------------------------------------------------------------------------- /ethzasl_ptam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ethzasl_ptam) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() -------------------------------------------------------------------------------- /ptam/thirdparty/TooN/Authors: -------------------------------------------------------------------------------- 1 | Created by Tom Drummond 2 | 3 | Contributions from Ethan Eade, Edward Rosten, Chris Kemp, Georg Klein, 4 | Timothy Gann, Paul Smith 5 | 6 | -------------------------------------------------------------------------------- /ptam_com/msg/OctoMapPointStamped.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8 INSERT = 0 3 | uint8 UPDATE = 1 4 | uint8 DELETE = 2 5 | 6 | uint8 action 7 | geometry_msgs/Vector3 position 8 | -------------------------------------------------------------------------------- /rqt_ptam/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b rqt_image_view provides a GUI plugin for displaying images using image_transport. 6 | */ 7 | -------------------------------------------------------------------------------- /ptam/.gitignore: -------------------------------------------------------------------------------- 1 | /cmake 2 | /lib 3 | *~ 4 | build 5 | bin 6 | thirdparty/TooN/toon_built 7 | thirdparty/TooN/include 8 | src/ptam/__init__.py 9 | .project 10 | .cproject 11 | -------------------------------------------------------------------------------- /ptam/Makefile: -------------------------------------------------------------------------------- 1 | all: 2 | cd thirdparty && make 3 | make -j8 -f Makefile_ptam 4 | 5 | .DEFAULT: 6 | cd thirdparty && make $@ 7 | make -j8 -f Makefile_ptam $@ 8 | 9 | 10 | -------------------------------------------------------------------------------- /ptam_com/msg/ptam_info.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 fps 3 | int32 keyframes 4 | int32 trackingQuality 5 | bool mapQuality 6 | string mapViewerMessage 7 | string trackerMessage -------------------------------------------------------------------------------- /ptam/thirdparty/rosdep.yaml: -------------------------------------------------------------------------------- 1 | atlas-sse2: 2 | ubuntu: 3 | 10.04: libatlas-sse2-dev 4 | 10.10: libatlas-sse2-dev 5 | 11.04: libatlas-base-dev 6 | 11.10: libatlas-base-dev 7 | 8 | lapack: 9 | ubuntu: liblapack-dev 10 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ethzasl_ptam 2 | ============ 3 | 4 | Modified version of Parallel Tracking and Mapping (PTAM) by George Klein. See http://wiki.ros.org/ethzasl_ptam for a detailed overview. 5 | 6 | [![Build Status](http://129.132.38.183:8080/job/ethzasl_ptam/badge/icon)](http://129.132.38.183:8080/job/ethzasl_ptam/) 7 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | all: 2 | cd ptam_com && make -j8 3 | cd .. 4 | cd ptam && make -j8 5 | cd .. 6 | 7 | clean: 8 | cd ptam && make clean 9 | cd .. 10 | cd ptam_com && make clean 11 | cd .. 12 | 13 | 14 | distclean: 15 | cd ptam && make distclean 16 | cd .. 17 | cd ptam_com && make distclean 18 | cd .. 19 | -------------------------------------------------------------------------------- /ptam/launch/ptam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /ptam/thirdparty/TooN/Makefile: -------------------------------------------------------------------------------- 1 | all: toon_built 2 | 3 | toon_built: 4 | mkdir -p include/ 5 | git clone https://github.com/edrosten/TooN.git include/TooN 6 | cd include/TooN && git checkout 35bb77741e7e2b7de5d75d0605f104f2ad728708 7 | touch toon_built 8 | 9 | clean: 10 | rm -rf include 11 | rm -f toon_built 12 | 13 | -------------------------------------------------------------------------------- /ptam/launch/cameracalibrator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /ptam/thirdparty/agast/getIncludePaths.cmake: -------------------------------------------------------------------------------- 1 | get_directory_property(inc_dirs INCLUDE_DIRECTORIES) 2 | 3 | set(filename includepaths.txt) 4 | file(WRITE ${filename} "copy this to .cproject\n\n") 5 | 6 | foreach(includeLine ${inc_dirs}) 7 | file(APPEND ${filename} "\n") 8 | endforeach(includeLine ${inc_dirs}) -------------------------------------------------------------------------------- /ptam/include/ptam/ShiTomasi.h: -------------------------------------------------------------------------------- 1 | // Copyright 2008 Isis Innovation Limited 2 | #ifndef __SHI_TOMASI__H 3 | #define __SHI_TOMASI__H 4 | 5 | #include 6 | #include 7 | 8 | 9 | double FindShiTomasiScoreAtPoint(CVD::BasicImage &image, 10 | int nHalfBoxSize, 11 | CVD::ImageRef irCenter); 12 | 13 | 14 | #endif 15 | -------------------------------------------------------------------------------- /ptam/include/ptam/OpenGL.h: -------------------------------------------------------------------------------- 1 | // Copyright 2008 Isis Innovation Limited 2 | #ifndef __OPENGL_INCLUDES_H 3 | #define __OPENGL_INCLUDES_H 4 | 5 | #ifdef _LINUX 6 | #include 7 | #include 8 | #endif 9 | 10 | #ifdef _OSX 11 | #include 12 | #include 13 | #endif 14 | 15 | #ifdef WIN32 16 | #define WIN32_LEAN_AND_MEAN 17 | #include 18 | #endif 19 | 20 | #include 21 | #endif 22 | -------------------------------------------------------------------------------- /rqt_ptam/plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A GUI plugin to remote control PTAM. 5 | 6 | 7 | 8 | image-x-generic 9 | A GUI plugin to remote control ptam. 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /ptam/thirdparty/Makefile: -------------------------------------------------------------------------------- 1 | all: 2 | @echo "making third party libs" 3 | cd TooN && make 4 | cd libcvd && make 5 | cd gvars3 && make 6 | cd agast && make 7 | 8 | 9 | distclean: 10 | @echo "cleaning up third party libs and doing a fresh checkout" 11 | cd TooN && make clean 12 | cd libcvd && make distclean 13 | cd gvars3 && make distclean 14 | cd agast && make distclean 15 | 16 | clean: 17 | @echo "cleaning up third party libs without doing a fresh checkout" 18 | #cd TooN && make clean #TooN is header only 19 | cd libcvd && make clean 20 | cd gvars3 && make clean 21 | cd agast && make clean 22 | 23 | 24 | -------------------------------------------------------------------------------- /ptam/src/Map.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2008 Isis Innovation Limited 2 | #include "ptam/Map.h" 3 | #include "ptam/MapPoint.h" 4 | 5 | Map::Map() 6 | { 7 | Reset(); 8 | } 9 | 10 | void Map::Reset() 11 | { 12 | vpPoints.clear(); 13 | bGood = false; 14 | EmptyTrash(); 15 | } 16 | 17 | void Map::MoveBadPointsToTrash() 18 | { 19 | int nBad = 0; 20 | for(int i = vpPoints.size()-1; i>=0; i--) 21 | { 22 | if(vpPoints[i]->bBad) 23 | { 24 | vpPointsTrash.push_back(vpPoints[i]); 25 | vpPoints.erase(vpPoints.begin() + i); 26 | nBad++; 27 | } 28 | }; 29 | }; 30 | 31 | void Map::EmptyTrash() 32 | { 33 | vpPointsTrash.clear(); 34 | }; 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /ptam/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b ptam is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /ptam_com/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b sfly_srvs is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /ptam/thirdparty/agast/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | 3 | add_definitions(-D_REENTRANT -Wall -O3 -Wextra -Winit-self -Woverloaded-virtual -Wnon-virtual-dtor -Wsign-promo -Wno-long-long ) 4 | 5 | #where the demo will be placed 6 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/installfiles) 7 | #where the libs will be placed 8 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 9 | 10 | file(GLOB AGAST_SOURCE_FILES "${CMAKE_CURRENT_SOURCE_DIR}/src/*.cc") 11 | 12 | file(GLOB AGAST_HEADER_FILES "${CMAKE_CURRENT_SOURCE_DIR}/include/agast/*.h") 13 | 14 | INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}/include/agast) 15 | 16 | #build the library dynamic and static versions 17 | add_library(agast ${AGAST_SOURCE_FILES} ${AGAST_HEADER_FILES}) 18 | 19 | 20 | find_package(OpenCV REQUIRED) 21 | target_link_libraries(agast ${OpenCV_LIBRARIES}) 22 | 23 | 24 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | dependencies/TooN/build/ 2 | dependencies/TooN/html/ 3 | dependencies/gvars3/build/ 4 | dependencies/gvars3/include/ 5 | dependencies/gvars3/installfiles/config.status 6 | dependencies/gvars3/lib/ 7 | dependencies/libcvd/bin/ 8 | dependencies/libcvd/build/ 9 | dependencies/libcvd/include/ 10 | dependencies/libcvd/lib/ 11 | ptam/.cproject 12 | ptam/.project 13 | ptam/bin/ 14 | ptam/build/ 15 | ptam/cfg/PTAMVisualizerParams.cfgc 16 | ptam/cfg/PtamParams.cfgc 17 | ptam/cfg/cpp/ 18 | ptam/docs/ 19 | ptam/src/ptam/cfg/PTAMVisualizerParamsConfig.py 20 | ptam/src/ptam/cfg/PtamParamsConfig.py 21 | ptam_com/build/ 22 | ptam_com/msg_gen/ 23 | ptam_com/src/ 24 | ptam_com/srv_gen/ 25 | ptam/thirdparty/agast/lib/ 26 | ptam/thirdparty/gvars3/git_checkedout 27 | ptam/thirdparty/gvars3/sources/ 28 | ptam/thirdparty/libcvd/git_checkedout 29 | ptam/thirdparty/libcvd/sources/ 30 | 31 | /build 32 | /Debug 33 | *~ 34 | -------------------------------------------------------------------------------- /ptam_com/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ptam_com) 3 | 4 | #List to make rest of code more readable 5 | set( MESSAGE_DEPENDENCIES 6 | std_msgs 7 | geometry_msgs 8 | sensor_msgs 9 | ) 10 | 11 | #Declare build dependencies 12 | find_package(catkin REQUIRED 13 | COMPONENTS 14 | message_generation 15 | ${MESSAGE_DEPENDENCIES} ) 16 | 17 | add_message_files(DIRECTORY msg 18 | FILES 19 | KeyFrame_msg.msg 20 | OctoMapPointArray.msg 21 | OctoMapPointStamped.msg 22 | OctoMapScan.msg 23 | Vector3Array.msg 24 | ptam_info.msg 25 | ) 26 | 27 | add_service_files(DIRECTORY srv 28 | FILES 29 | KeyFrame_srv.srv 30 | PointCloud.srv 31 | ) 32 | 33 | #And now generate the messages 34 | generate_messages(DEPENDENCIES ${MESSAGE_DEPENDENCIES}) 35 | 36 | #Declare package run-time dependencies 37 | catkin_package( CATKIN_DEPENDS message_runtime ${MESSAGE_DEPENDENCIES}) 38 | -------------------------------------------------------------------------------- /ptam/thirdparty/gvars3/Makefile: -------------------------------------------------------------------------------- 1 | all: git_checkedout gvars_built 2 | 3 | git_checkedout: 4 | mkdir -p sources 5 | git clone https://github.com/edrosten/gvars.git sources 6 | cd sources && git checkout 1f325cead6117b4fbd8016e46dc2f654f0ab7f25 7 | touch git_checkedout 8 | 9 | gvars_built: 10 | cp -r sources build 11 | cd build && if [ "`arch`" = "armv7l" ]; then sed -i 's/CXX=@CXX@/CXX=@CXX@ -fPIC/g' Makefile.in; fi 12 | cd build && export CPATH=../../TooN/include && ./configure --without-ffmpeg --without-dc1394v1 --without-dc1394v2 --with-lapack --with-pic=no && make -j4 -l4 13 | mkdir -p include 14 | mkdir -p lib 15 | cp build/*.so* lib 16 | cp -r build/gvars3 include 17 | touch gvars_built 18 | 19 | distclean: 20 | rm -rf sources 21 | rm git_checkedout 22 | rm -rf include lib 23 | rm -rf build 24 | rm -f gvars_built 25 | 26 | clean: 27 | rm -rf include lib 28 | rm -rf build 29 | rm -f gvars_built 30 | -------------------------------------------------------------------------------- /ptam/include/ptam/OctomapInterface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * octomapinterface.h 3 | * 4 | * Created on: Dec 26, 2012 5 | * Author: slynen 6 | */ 7 | 8 | #ifndef PCLINTERFACE_H_ 9 | #define PCLINTERFACE_H_ 10 | #include 11 | #include 12 | #include 13 | 14 | class OctoMapInterface 15 | { 16 | private: 17 | ros::NodeHandle& nh_; 18 | ros::Publisher pub_scan_; 19 | ros::Publisher pub_points_; 20 | 21 | std::set localUpdateQueue_; //to collect updates before publishing them all at once 22 | 23 | unsigned int kfseq_; 24 | unsigned int pointseq_; 25 | public: 26 | OctoMapInterface( ros::NodeHandle& nh); 27 | virtual ~OctoMapInterface(); 28 | 29 | void addKeyFrame(KeyFrame::Ptr k); 30 | void updatePoint(MapPoint::Ptr p); 31 | void updatePoints(std::set& p); 32 | void deletePoint(MapPoint::Ptr p); 33 | 34 | protected: 35 | void publishPointUpdateFromQueue(); 36 | 37 | }; 38 | 39 | #endif /* PCLINTERFACE_H_ */ 40 | -------------------------------------------------------------------------------- /ptam/include/ptam/Relocaliser.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | // 4 | // SmallBlurryImage-based relocaliser 5 | // 6 | // Each KF stores a small, blurred version of itself; 7 | // Just compare a small, blurred version of the input frame to all the KFs, 8 | // choose the closest match, and then estimate a camera rotation by direct image 9 | // minimisation. 10 | 11 | #ifndef __RELOCALISER_H 12 | #define __RELOCALISER_H 13 | #include 14 | #include "ATANCamera.h" 15 | #include "SmallBlurryImage.h" 16 | 17 | #include "Map.h" 18 | 19 | 20 | class Relocaliser 21 | { 22 | public: 23 | Relocaliser(Map &map, ATANCamera &camera); 24 | bool AttemptRecovery(KeyFrame& k); 25 | SE3<> BestPose(); 26 | 27 | protected: 28 | void ScoreKFs(KeyFrame& kCurrentF); 29 | Map &mMap; 30 | ATANCamera mCamera; 31 | int mnBest; 32 | double mdBestScore; 33 | SE2<> mse2; 34 | SE3<> mse3Best; 35 | 36 | }; 37 | #endif 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /ptam/include/ptam/SmallMatrixOpts.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | // 4 | // Inverse of 2-matrix 5 | // Must be invertible! 6 | #ifndef __SMALL_MATRIX_OPTS 7 | #define __SMALL_MATRIX_OPTS 8 | #include 9 | 10 | inline Matrix<2> M2Inverse(const Matrix<2> &m) 11 | { 12 | Matrix<2> m2Res; 13 | double dDet = m[0][0] * m[1][1] - m[1][0] * m[0][1]; 14 | assert(dDet!=0.0); 15 | double dInverseDet = 1.0 / dDet; 16 | m2Res[0][0] = m[1][1] * dInverseDet; 17 | m2Res[1][1] = m[0][0] * dInverseDet; 18 | m2Res[1][0] = -m[1][0] * dInverseDet; 19 | m2Res[0][1] = -m[0][1] * dInverseDet; 20 | return m2Res; 21 | }; 22 | 23 | // Determinant of 2x2 24 | inline double M2Det(Matrix<2> m) 25 | { 26 | return m[0][0] * m[1][1] - m[0][1] * m[1][0]; 27 | } 28 | 29 | // Determinant of 3x3 30 | inline double M3Det(Matrix<3> m ) 31 | { 32 | return 33 | m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) - 34 | m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) + 35 | m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); 36 | } 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /ptam/thirdparty/libcvd/Makefile: -------------------------------------------------------------------------------- 1 | ifeq ("$(shell arch)", "armv7l") 2 | CXXFLAGS+= -mfpu=neon 3 | endif 4 | CXXFLAGS+= -fsee -funsafe-loop-optimizations -ffinite-math-only -fno-signed-zeros -fno-math-errno -funroll-loops 5 | 6 | all: git_checkedout libcvd_built 7 | 8 | git_checkedout: 9 | mkdir -p sources 10 | git clone https://github.com/edrosten/libcvd.git sources 11 | cd sources && git checkout 26bb46b9845383a4724a5e24db42dc27d41477d4 12 | touch git_checkedout 13 | 14 | libcvd_built: 15 | cp -r sources build 16 | cd build && export CPATH=../../TooN/include && export CXXFLAGS="$(CXXFLAGS)" && ./configure --without-ffmpeg --without-dc1394v1 --without-dc1394v2 --with-toon --with-lapack 17 | cd build && export CPATH=../../TooN/include && make -j4 -l4 18 | mkdir -p include 19 | mkdir -p lib 20 | cp build/*.so* lib 21 | cp -r build/cvd include 22 | touch libcvd_built 23 | 24 | distclean: 25 | rm -rf sources 26 | rm git_checkedout 27 | rm -rf include lib 28 | rm -rf build 29 | rm -f libcvd_built 30 | 31 | clean: 32 | rm -rf include lib 33 | rm -rf build 34 | rm -f libcvd_built 35 | 36 | -------------------------------------------------------------------------------- /rqt_ptam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rqt_ptam) 3 | 4 | find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED) 5 | find_package(catkin REQUIRED) 6 | 7 | 8 | catkin_package() 9 | 10 | include(${QT_USE_FILE}) 11 | 12 | set(rqt_ptam_SRCS 13 | src/rqt_ptam/remote_ptam.cpp 14 | src/rqt_ptam/ratio_layouted_frame.cpp 15 | ) 16 | 17 | set(rqt_ptam_HDRS 18 | include/rqt_ptam/remote_ptam.h 19 | include/rqt_ptam/ratio_layouted_frame.h 20 | ) 21 | 22 | set(rqt_ptam_UIS 23 | src/rqt_ptam/remote_ptam.ui 24 | ) 25 | 26 | set(rqt_ptam_INCLUDE_DIRECTORIES 27 | # include 28 | ${CMAKE_CURRENT_BINARY_DIR} 29 | ) 30 | 31 | qt4_wrap_cpp(rqt_ptam_MOCS ${rqt_ptam_HDRS} OPTIONS -DBOOST_TT_HAS_OPERATOR_HPP_INCLUDED -DBOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) 32 | qt4_wrap_ui(rqt_ptam_UIS_H ${rqt_ptam_UIS}) 33 | 34 | include_directories(include ${rqt_ptam_INCLUDE_DIRECTORIES} ${catkin_INCLUDE_DIRS}) 35 | add_library(rqt_ptam ${rqt_ptam_SRCS} ${rqt_ptam_MOCS} ${rqt_ptam_UIS_H}) 36 | target_link_libraries(rqt_ptam ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY} ${catkin_LIBRARIES}) 37 | 38 | 39 | -------------------------------------------------------------------------------- /ptam/PtamFixParams.yaml: -------------------------------------------------------------------------------- 1 | gui: True 2 | ImageSizeX: 752 3 | ImageSizeY: 480 4 | ARBuffer_width: 1200 5 | ARBuffer_height: 900 6 | WiggleScale: 0.1 7 | BundleMEstimator: Tukey 8 | TrackerMEstimator: Tukey 9 | MinTukeySigma: 0.4 10 | CandidateMinSTScore: 70 11 | Calibrator_BlurSigma: 1.0 12 | Calibrator_MeanGate: 10 13 | Calibrator_MinCornersForGrabbedImage: 20 14 | Calibrator_Optimize: 0 15 | Calibrator_Show: 0 16 | Calibrator_NoDistortion: 0 17 | CameraCalibrator_MaxStepDistFraction: 0.3 18 | CameraCalibrator_CornerPatchSize: 20 19 | GLWindowMenu_Enable: True 20 | GLWindowMenu_mgvnMenuItemWidth: 90 21 | GLWindowMenu_mgvnMenuTextOffset: 20 22 | InitLevel: 1 23 | MaxKFDistWiggleMult: 1 24 | MaxPatchesPerFrame: 300 25 | MaxKF: 15 26 | parent_frame: world 27 | TrackingQualityFoundPixels: 50 28 | UseKFPixelDist: True 29 | NoLevelZeroMapPoints: True 30 | FASTMethod: OAST16 31 | MaxStereoInitLoops: 4 32 | AutoInitPixel: 20 33 | 34 | # --- bluefox - low distort lens --- 35 | Cam_fx: 0.795574 36 | Cam_fy: 1.25149 37 | Cam_cx: 0.50417 38 | Cam_cy: 0.51687 39 | Cam_s: 0.482014 40 | # ----------------------------------- 41 | 42 | -------------------------------------------------------------------------------- /ptam/include/ptam/MapViewer.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | // 4 | // MapViewer.h 5 | // 6 | // Defines the MapViewer class 7 | // 8 | // This defines a simple map viewer widget, which can draw the 9 | // current map and the camera/keyframe poses within it. 10 | // 11 | #ifndef __MAP_VIEWER_H 12 | #define __MAP_VIEWER_H 13 | 14 | #include "Map.h" 15 | #include 16 | using namespace TooN; 17 | #include 18 | #include 19 | #include "GLWindow2.h" 20 | 21 | struct Map; 22 | 23 | class MapViewer 24 | { 25 | public: 26 | MapViewer(Map &map, GLWindow2 &glw); 27 | void DrawMap(SE3<> se3CamFromWorld); 28 | std::string GetMessageForUser(); 29 | 30 | protected: 31 | Map &mMap; 32 | GLWindow2 &mGLWindow; 33 | 34 | void DrawGrid(); 35 | void DrawMapDots(); 36 | void DrawCamera(SE3<> se3, bool bSmall=false); 37 | void SetupFrustum(); 38 | void SetupModelView(SE3<> se3WorldFromCurrent = SE3<>()); 39 | 40 | Vector<3> mv3MassCenter; 41 | SE3<> mse3ViewerFromWorld; 42 | 43 | std::ostringstream mMessageForUser; 44 | }; 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /ethzasl_ptam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ethzasl_ptam 4 | 0.0.1 5 | 6 | 7 | Stack for the modified version of the monocular SLAM framework PTAM 8 | 9 | 10 | 11 | Markus Achtelik 12 | Stephan Weiss 13 | Simon Lynen 14 | 15 | 16 | see http://www.robots.ox.ac.uk/~gk/PTAM/download.html 17 | 18 | http://ros.org/wiki/ethzasl_ptam 19 | https://github.com/ethz-asl/ethzasl_ptam 20 | https://github.com/ethz-asl/ethzasl_ptam/issues 21 | 22 | Markus Achtelik 23 | Stephan Weiss 24 | Simon Lynen 25 | 26 | catkin 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /ptam/include/ptam/SmallBlurryImage.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | // 4 | // SmallBlurryImage - A small and blurry representation of an image. 5 | // used by the relocaliser. 6 | 7 | #ifndef __SMALLBLURRYIMAGE_H 8 | #define __SMALLBLURRYIMAGE_H 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include "KeyFrame.h" 14 | #include "ATANCamera.h" 15 | 16 | class SmallBlurryImage 17 | { 18 | public: 19 | SmallBlurryImage(); 20 | SmallBlurryImage(KeyFrame& kf, double dBlur = 2.5); 21 | void MakeFromKF(KeyFrame& kf, double dBlur = 2.5); 22 | void MakeJacs(); 23 | double ZMSSD(SmallBlurryImage &other); 24 | std::pair,double> IteratePosRelToTarget(SmallBlurryImage &other, int nIterations = 10); 25 | static SE3<> SE3fromSE2(SE2<> se2, ATANCamera camera); 26 | 27 | protected: 28 | CVD::Image mimSmall; 29 | CVD::Image mimTemplate; 30 | CVD::Image > mimImageJacs; 31 | bool mbMadeJacs; 32 | static CVD::ImageRef mirSize; 33 | }; 34 | 35 | 36 | 37 | #endif 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /ptam/thirdparty/agast/installfiles/Makefile_orig: -------------------------------------------------------------------------------- 1 | # Minimalistic Makefile for the AGAST demo package 2 | # author: Elmar Mair 3 | # date: 06/2010 4 | # license: GPL v3 5 | 6 | #################################################### 7 | # please adapt following lines to your environment # 8 | #################################################### 9 | OPENCV_INCLUDE_PATH = /usr/local/include/opencv 10 | OPENCV_LIBRARY_PATH = /usr/lib 11 | 12 | ######################################################## 13 | # the following lines should not be edited by the user # 14 | ######################################################## 15 | CXX = g++ 16 | PROG = demo 17 | OBJ = agast5_8.o agast5_8_nms.o agast7_12s.o agast7_12s_nms.o agast7_12d.o agast7_12d_nms.o oast9_16.o oast9_16_nms.o nonMaximumSuppression.o 18 | 19 | CXXFLAGS += -DHAVE_OPENCV -I$(OPENCV_INCLUDE_PATH) -O3 -Wall -Wextra 20 | LDFLAGS += -L$(OPENCV_LIBRARY_PATH) -lcxcore -lcv -lhighgui 21 | 22 | all: $(PROG) 23 | 24 | $(PROG) : $(PROG).cc $(OBJ) 25 | $(CXX) $(CXXFLAGS) $(PROG).cc $(LDFLAGS) $(OBJ) -o $@ 26 | 27 | %.o : %.c 28 | $(CXX) $(CXXFLAGS) -c $< 29 | 30 | clean: 31 | -@rm *.o $(PROG) 32 | 33 | -------------------------------------------------------------------------------- /ptam/include/ptam/Map.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | // 4 | // This header declares the Map class. 5 | // This is pretty light-weight: All it contains is 6 | // a vector of MapPoints and a vector of KeyFrames. 7 | // 8 | // N.b. since I don't do proper thread safety, 9 | // everything is stored as lists of pointers, 10 | // and map points are not erased if they are bad: 11 | // they are moved to the trash list. That way 12 | // old pointers which other threads are using are not 13 | // invalidated! 14 | 15 | #ifndef __MAP_H 16 | #define __MAP_H 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | struct MapPoint; 23 | struct KeyFrame; 24 | 25 | struct Map 26 | { 27 | Map(); 28 | inline bool IsGood() {return bGood;} 29 | void Reset(); 30 | 31 | void MoveBadPointsToTrash(); 32 | void EmptyTrash(); 33 | 34 | std::vector > vpPoints; 35 | std::vector > vpPointsTrash; 36 | std::vector > vpKeyFrames; 37 | 38 | bool bGood; 39 | }; 40 | 41 | 42 | 43 | 44 | #endif 45 | 46 | -------------------------------------------------------------------------------- /ptam/include/ptam/CalibCornerPatch.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | 4 | #ifndef __CALIB_CORNER_PATCH_H 5 | #define __CALIB_CORNER_PATCH_H 6 | #include 7 | using namespace TooN; 8 | #include 9 | #include 10 | 11 | class CalibCornerPatch 12 | { 13 | public: 14 | struct Params 15 | { 16 | Params(); 17 | Matrix<2> m2Warp(); 18 | Vector<2> v2Pos; 19 | Vector<2> v2Angles; 20 | double dMean; 21 | double dGain; 22 | }; 23 | 24 | CalibCornerPatch(int nSideSize = 8); 25 | bool IterateOnImage(Params ¶ms, CVD::Image &im); 26 | bool IterateOnImageWithDrawing(Params ¶ms, CVD::Image &im); 27 | 28 | protected: 29 | void MakeTemplateWithCurrentParams(); 30 | void FillTemplate(CVD::Image &im, Params params); 31 | double Iterate(CVD::Image &im); 32 | Params mParams; 33 | CVD::Image mimTemplate; 34 | CVD::Image > mimGradients; 35 | CVD::Image > mimAngleJacs; 36 | 37 | void MakeSharedTemplate(); 38 | static CVD::Image mimSharedSourceTemplate; 39 | 40 | double mdLastError; 41 | }; 42 | 43 | 44 | 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /ptam/thirdparty/agast/include/agast/cvWrapper.h: -------------------------------------------------------------------------------- 1 | // 2 | // cvWrapper - header file to define CvPoint in case openCV is not installed 3 | // 4 | // Copyright (C) 2010 Elmar Mair 5 | // 6 | // This program is free software: you can redistribute it and/or modify 7 | // it under the terms of the GNU General Public License as published by 8 | // the Free Software Foundation, either version 3 of the License, or 9 | // (at your option) any later version. 10 | // 11 | // This program is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | // GNU General Public License for more details. 15 | // 16 | // You should have received a copy of the GNU General Public License 17 | // along with this program. If not, see . 18 | 19 | #ifndef CVWRAPPER_H_ 20 | #define CVWRAPPER_H_ 21 | 22 | #define HAVE_OPENCV //we normally do have opencv 23 | 24 | #ifdef HAVE_OPENCV 25 | #include 26 | #else 27 | typedef struct CvPoint 28 | { 29 | int x; 30 | int y; 31 | } 32 | CvPoint; 33 | #endif 34 | 35 | #endif /* CVWRAPPER_H_ */ 36 | -------------------------------------------------------------------------------- /ptam/src/ShiTomasi.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2008 Isis Innovation Limited 2 | #include "ptam/ShiTomasi.h" 3 | #include 4 | 5 | using namespace CVD; 6 | 7 | double FindShiTomasiScoreAtPoint(BasicImage &image, 8 | int nHalfBoxSize, 9 | ImageRef irCenter) 10 | { 11 | double dXX = 0; 12 | double dYY = 0; 13 | double dXY = 0; 14 | 15 | ImageRef irStart = irCenter - ImageRef(nHalfBoxSize, nHalfBoxSize); 16 | ImageRef irEnd = irCenter + ImageRef(nHalfBoxSize, nHalfBoxSize); 17 | 18 | ImageRef ir; 19 | for(ir.y = irStart.y; ir.y<=irEnd.y; ir.y++) 20 | for(ir.x = irStart.x; ir.x<=irEnd.x; ir.x++) 21 | { 22 | double dx = image[ir + ImageRef(1,0)] - image[ir - ImageRef(1,0)]; 23 | double dy = image[ir + ImageRef(0,1)] - image[ir - ImageRef(0,1)]; 24 | dXX += dx*dx; 25 | dYY += dy*dy; 26 | dXY += dx*dy; 27 | } 28 | 29 | int nPixels = (irEnd - irStart + ImageRef(1,1)).area(); 30 | dXX = dXX / (2.0 * nPixels); 31 | dYY = dYY / (2.0 * nPixels); 32 | dXY = dXY / (2.0 * nPixels); 33 | 34 | // Find and return smaller eigenvalue: 35 | return 0.5 * (dXX + dYY - sqrt( (dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY) )); 36 | }; 37 | 38 | -------------------------------------------------------------------------------- /ptam/thirdparty/agast/installfiles/Makefile: -------------------------------------------------------------------------------- 1 | # Minimalistic Makefile for the AGAST demo package 2 | # author: Elmar Mair 3 | # date: 06/2010 4 | # license: GPL v3 5 | 6 | #################################################### 7 | # please adapt following lines to your environment # 8 | #################################################### 9 | #OPENCV_INCLUDE_PATH = /home/slynen/ros_cturtle/stacks/vision_opencv/opencv2/opencv/include 10 | #OPENCV_LIBRARY_PATH = /usr/lib 11 | 12 | ######################################################## 13 | # the following lines should not be edited by the user # 14 | ######################################################## 15 | CXX = g++ 16 | PROG = demo 17 | OBJ = agast5_8.o agast5_8_nms.o agast7_12s.o agast7_12s_nms.o agast7_12d.o agast7_12d_nms.o oast9_16.o oast9_16_nms.o nonMaximumSuppression.o 18 | #-I$(OPENCV_INCLUDE_PATH) 19 | CXXFLAGS += -DHAVE_OPENCV -O3 -Wall -Wextra $(AGAST_INCLUDE) $(OPENCV_INCLUDE) 20 | #-L$(OPENCV_LIBRARY_PATH) 21 | LDFLAGS += -lcv -lhighgui -lcxcore 22 | 23 | all: $(PROG) 24 | 25 | $(PROG) : ../src/$(PROG).cc $(OBJ) 26 | $(CXX) $(CXXFLAGS) ../src/$(PROG).cc $(LDFLAGS) $(OBJ) -o $@ 27 | 28 | %.o : ../src/%.c 29 | $(CXX) $(CXXFLAGS) -c $< 30 | 31 | clean: 32 | -@rm *.o $(PROG) 33 | 34 | -------------------------------------------------------------------------------- /ptam/thirdparty/TooN/COPYING: -------------------------------------------------------------------------------- 1 | This library is free software; you can redistribute it and/or modify it under 2 | the terms of the GNU General Public License as published by the Free Software 3 | Foundation; either version 2, or (at your option) any later version. 4 | 5 | As a special exception, you may use these files as part of a free software 6 | library without restriction. Specifically, if other files instantiate 7 | templates or use macros or inline functions from this library, or you compile 8 | this library and link it with other files to produce an executable, this 9 | library does not by itself cause the resulting executable to be covered by the 10 | GNU General Public License. This exception does not however invalidate any 11 | other reasons why the executable file might be covered by the GNU General 12 | Public License. 13 | 14 | This library is distributed in the hope that it will be useful, but WITHOUT ANY 15 | WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 16 | PARTICULAR PURPOSE. See the GNU General Public License for more details. 17 | 18 | You should have received a copy of the GNU General Public License along with 19 | this library; see the file GPL.txt. If not, write to the Free Software 20 | Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. 21 | -------------------------------------------------------------------------------- /ptam/include/ptam/MiniPatch.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | // 4 | // MiniPatch.h 5 | // 6 | // Declares MiniPatch class 7 | // 8 | // This is a simple pixel-patch class, used for tracking small patches 9 | // it's used by the tracker for building the initial map 10 | 11 | #ifndef __MINI_PATCH_H 12 | #define __MINI_PATCH_H 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | using namespace TooN; 19 | #include 20 | 21 | struct MiniPatch 22 | { 23 | void SampleFromImage(CVD::ImageRef irPos, CVD::BasicImage &im); // Copy pixels out of source image 24 | bool FindPatch(CVD::ImageRef &irPos, // Find patch in a new image 25 | CVD::BasicImage &im, 26 | int nRange, 27 | std::vector &vCorners, 28 | std::vector *pvRowLUT = NULL); 29 | 30 | inline int SSDAtPoint(CVD::BasicImage &im, const CVD::ImageRef &ir); // Score function 31 | static int mnHalfPatchSize; // How big is the patch? 32 | static int mnRange; // How far to search? 33 | static int mnMaxSSD; // Max SSD for matches? 34 | CVD::Image mimOrigPatch; // Original pixels 35 | }; 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /ptam/include/ptam/CameraCalibrator.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | 4 | #ifndef __CAMERACALIBRATOR_H 5 | #define __CAMERACALIBRATOR_H 6 | #include "CalibImage.h" 7 | #include 8 | #include 9 | #include "GLWindow2.h" 10 | #include 11 | #include 12 | #include 13 | 14 | class CameraCalibrator 15 | { 16 | public: 17 | CameraCalibrator(); 18 | ~CameraCalibrator(); 19 | void Run(); 20 | 21 | protected: 22 | void Reset(); 23 | void init(); 24 | GLWindow2 *mGLWindow; 25 | ATANCamera mCamera; 26 | bool mbDone; 27 | 28 | std::vector mvCalibImgs; 29 | void OptimizeOneStep(); 30 | 31 | bool mbGrabNextFrame; 32 | 33 | GVars3::gvar3 mgvnOptimizing; 34 | GVars3::gvar3 mgvnShowImage; 35 | GVars3::gvar3 mgvnDisableDistortion; 36 | 37 | double mdMeanPixelError; 38 | 39 | CVD::Image mCurrentImage; 40 | image_transport::Subscriber mImageSub; 41 | bool mDoOptimize; 42 | bool mNewImage; 43 | void imageCallback(const sensor_msgs::ImageConstPtr & img); 44 | 45 | void GUICommandHandler(std::string sCommand, std::string sParams); 46 | static void GUICommandCallBack(void* ptr, std::string sCommand, std::string sParams); 47 | }; 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /ptam/src/main.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2008 Isis Innovation Limited 2 | // This is the main extry point for PTAM 3 | #include 4 | #include 5 | #include 6 | #include "ptam/System.h" 7 | #include 8 | 9 | #include "ros/ros.h" 10 | 11 | using namespace std; 12 | using namespace GVars3; 13 | 14 | int main(int argc, char** argv) 15 | { 16 | ros::init(argc, argv, "ptam"); 17 | ROS_INFO("starting ptam with node name %s", ros::this_node::getName().c_str()); 18 | 19 | cout << " Welcome to PTAM " << endl; 20 | cout << " --------------- " << endl; 21 | cout << " Parallel tracking and mapping for Small AR workspaces" << endl; 22 | cout << " Copyright (C) Isis Innovation Limited 2008 " << endl; 23 | cout << endl; 24 | 25 | GUI.StartParserThread(); // Start parsing of the console input 26 | atexit(GUI.StopParserThread); 27 | 28 | try 29 | { 30 | std::cout<<"Gui is "<<(PtamParameters::fixparams().gui ? "on" : "off")< v3PlanePoint_C = k.se3CfromW * v3WorldPos; 12 | 13 | // Find the height of this above the plane. 14 | // Assumes the normal is pointing toward the camera. 15 | double dCamHeight = fabs(v3PlanePoint_C * v3Normal_NC); 16 | 17 | double dPixelRate = fabs(v3Center_NC * v3Normal_NC); 18 | double dOneRightRate = fabs(v3OneRightFromCenter_NC * v3Normal_NC); 19 | double dOneDownRate = fabs(v3OneDownFromCenter_NC * v3Normal_NC); 20 | 21 | // Find projections onto plane 22 | Vector<3> v3CenterOnPlane_C = v3Center_NC * dCamHeight / dPixelRate; 23 | Vector<3> v3OneRightOnPlane_C = v3OneRightFromCenter_NC * dCamHeight / dOneRightRate; 24 | Vector<3> v3OneDownOnPlane_C = v3OneDownFromCenter_NC * dCamHeight / dOneDownRate; 25 | 26 | // Find differences of these projections in the world frame 27 | v3PixelRight_W = k.se3CfromW.get_rotation().inverse() * (v3OneRightOnPlane_C - v3CenterOnPlane_C); 28 | v3PixelDown_W = k.se3CfromW.get_rotation().inverse() * (v3OneDownOnPlane_C - v3CenterOnPlane_C); 29 | } 30 | -------------------------------------------------------------------------------- /rqt_ptam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rqt_ptam 4 | 0.0.1 5 | 6 | 7 | Remote PTAM plugin for rqt, based on rqt_image_view from Dirk Thomas, TU Darmstadt 8 | 9 | 10 | 11 | Markus Achtelik 12 | Stephan Weiss 13 | Simon Lynen 14 | 15 | see http://www.robots.ox.ac.uk/~gk/PTAM/download.html 16 | 17 | http://ros.org/wiki/ethzasl_ptam/rqt_ptam 18 | https://github.com/ethz-asl/ethzasl_ptam 19 | https://github.com/ethz-asl/ethzasl_ptam/issues 20 | 21 | Markus Achtelik 22 | Stephan Weiss 23 | Simon Lynen 24 | 25 | catkin 26 | roscpp 27 | std_msgs 28 | ptam_com 29 | sensor_msgs 30 | cv_bridge 31 | 32 | roscpp 33 | rqt_gui 34 | rqt_gui_cpp 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /ptam_com/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ptam_com 4 | 0.0.1 5 | 6 | 7 | header files for the modified version of the monocular SLAM framework PTAM 8 | 9 | 10 | 11 | Markus Achtelik 12 | Stephan Weiss 13 | Simon Lynen 14 | 15 | see http://www.robots.ox.ac.uk/~gk/PTAM/download.html 16 | 17 | http://ros.org/wiki/ethzasl_ptam/ptam 18 | https://github.com/ethz-asl/ethzasl_ptam 19 | https://github.com/ethz-asl/ethzasl_ptam/issues 20 | 21 | Markus Achtelik 22 | Stephan Weiss 23 | Simon Lynen 24 | 25 | catkin 26 | 27 | message_generation 28 | std_msgs 29 | roscpp 30 | sensor_msgs 31 | geometry_msgs 32 | 33 | message_runtime 34 | std_msgs 35 | roscpp 36 | sensor_msgs 37 | geometry_msgs 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /ptam/thirdparty/agast/installfiles/README: -------------------------------------------------------------------------------- 1 | 2 | *** AGAST corner detector *** Version: 1.0.4 3 | ------------------------------- 4 | 5 | -- How to compile and use the demo program -- 6 | 7 | Adapt the OPENCV paths in the 'Makefile' and compile the sources by typing 'make' 8 | in the program folder. Run the program by typing 9 | demo 10 | where specifies your input image. 11 | 12 | As result you get four images with the names oast9_16.ppm, agast7_12d.ppm, 13 | agast7_12s.ppm and agast5_8.ppm. These images represent the result of 14 | - the optimal accelerated segment test on a pixel mask of 16 pixels (with a 9 pixel arc length), 15 | - the adaptive and generic AST with a 12 pixel diamond and square shaped mask and 16 | - the adaptive and generic AST with a 8 pixel mask. 17 | 18 | With the parameters AST_THR_16, AST_THR_12 and AST_THR_8 in the 'demo.cc' file you 19 | can define the thresholds to adjust the corner response of the AST. 20 | 21 | 22 | -- Referencing this work -- 23 | 24 | If you are publishing a project using this software, please refrence following 25 | publication: 26 | 27 | @inproceedings{mair2010_agast, 28 | title = "Adaptive and Generic Corner Detection Based on the Accelerated Segment Test", 29 | author = "Elmar Mair and Gregory D. Hager and Darius Burschka and Michael Suppa and Gerhard Hirzinger", 30 | year = "2010", 31 | month = "September", 32 | booktitle = "European Conference on Computer Vision (ECCV'10)", 33 | notes = "Poster presentation", 34 | url = "http://www6.in.tum.de/Main/ResearchAgast" 35 | } 36 | 37 | -------------------------------------------------------------------------------- /ptam/include/ptam/CalibImage.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | 4 | #ifndef __CALIB_IMAGE_H 5 | #define __CALIB_IMAGE_H 6 | #include "ATANCamera.h" 7 | #include "CalibCornerPatch.h" 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | const int N_NOT_TRIED=-1; 14 | const int N_FAILED=-2; 15 | 16 | struct CalibGridCorner 17 | { 18 | struct NeighborState 19 | { 20 | NeighborState() {val = N_NOT_TRIED;} 21 | int val; 22 | }; 23 | 24 | CalibCornerPatch::Params Params; 25 | CVD::ImageRef irGridPos; 26 | NeighborState aNeighborStates[4]; 27 | 28 | Matrix<2> GetSteps(std::vector &vgc); 29 | Matrix<2> mInheritedSteps; 30 | 31 | void Draw(); 32 | 33 | double ExpansionPotential(); 34 | }; 35 | 36 | class CalibImage 37 | { 38 | public: 39 | 40 | bool MakeFromImage(CVD::Image &im); 41 | SE3<> mse3CamFromWorld; 42 | void DrawImageGrid(); 43 | void Draw3DGrid(ATANCamera &Camera, bool bDrawErrors); 44 | void GuessInitialPose(ATANCamera &Camera); 45 | 46 | struct ErrorAndJacobians 47 | { 48 | Vector<2> v2Error; 49 | Matrix<2,6> m26PoseJac; 50 | Matrix<2,NUMTRACKERCAMPARAMETERS> m2NCameraJac; 51 | }; 52 | 53 | std::vector Project(ATANCamera &Camera); 54 | 55 | CVD::Image mim; 56 | 57 | protected: 58 | std::vector mvCorners; 59 | std::vector mvGridCorners; 60 | 61 | 62 | bool ExpandByAngle(int nSrc, int nDirn); 63 | int NextToExpand(); 64 | void ExpandByStep(int n); 65 | CVD::ImageRef IR_from_dirn(int nDirn); 66 | 67 | }; 68 | 69 | 70 | 71 | 72 | #endif 73 | 74 | -------------------------------------------------------------------------------- /ptam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ptam 4 | 0.0.1 5 | 6 | 7 | Modified version of the monocular SLAM framework PTAM 8 | 9 | 10 | 11 | Markus Achtelik 12 | Stephan Weiss 13 | Simon Lynen 14 | 15 | see http://www.robots.ox.ac.uk/~gk/PTAM/download.html 16 | 17 | http://ros.org/wiki/ethzasl_ptam/ptam 18 | https://github.com/ethz-asl/ethzasl_ptam 19 | https://github.com/ethz-asl/ethzasl_ptam/issues 20 | 21 | Markus Achtelik 22 | Stephan Weiss 23 | Simon Lynen 24 | 25 | catkin 26 | 27 | roscpp 28 | std_msgs 29 | ptam_com 30 | sensor_msgs 31 | image_transport 32 | tf 33 | visualization_msgs 34 | dynamic_reconfigure 35 | cv_bridge 36 | 37 | roscpp 38 | std_msgs 39 | ptam_com 40 | sensor_msgs 41 | image_transport 42 | tf 43 | visualization_msgs 44 | dynamic_reconfigure 45 | cv_bridge 46 | 47 | -------------------------------------------------------------------------------- /ptam/thirdparty/agast/src/AstDetector.cc: -------------------------------------------------------------------------------- 1 | // 2 | // AstDetector - the interface class for the AGAST corner detector 3 | // 4 | // Copyright (C) 2010 Elmar Mair 5 | // 6 | // This program is free software: you can redistribute it and/or modify 7 | // it under the terms of the GNU General Public License as published by 8 | // the Free Software Foundation, either version 3 of the License, or 9 | // (at your option) any later version. 10 | // 11 | // This program is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | // GNU General Public License for more details. 15 | // 16 | // You should have received a copy of the GNU General Public License 17 | // along with this program. If not, see . 18 | 19 | #include "AstDetector.h" 20 | #include "cvWrapper.h" 21 | 22 | using namespace std; 23 | using namespace agast; 24 | 25 | void AstDetector::score(const unsigned char* i, const std::vector& corners_all) 26 | { 27 | unsigned int n=0; 28 | unsigned int num_corners=corners_all.size(); 29 | 30 | if(num_corners > scores.capacity()) 31 | { 32 | if(scores.capacity()==0) 33 | { 34 | scores.reserve(512 > num_corners ? 512 : num_corners); 35 | } 36 | else 37 | { 38 | unsigned int nScores = scores.capacity()*2; 39 | if(num_corners > nScores) 40 | nScores = num_corners; 41 | scores.reserve(nScores); 42 | } 43 | } 44 | 45 | scores.resize(num_corners); 46 | 47 | for(; n < num_corners; n++) 48 | scores[n] = cornerScore(i + corners_all[n].y*xsize + corners_all[n].x); 49 | } 50 | 51 | void AstDetector::nms(const unsigned char* im, const std::vector& corners_all, 52 | std::vector& corners_nms) 53 | { 54 | score(im,corners_all); 55 | nonMaximumSuppression(corners_all, corners_nms); 56 | } 57 | -------------------------------------------------------------------------------- /ptam/include/ptam/LevelHelpers.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | 4 | // LevelHelpers.h - a few handy tools to ease using levels. 5 | // The important thing is the XXXPos functions, which convert 6 | // image positions from one level to another. Use these whenever 7 | // transforming positions to ensure consistent operation!! 8 | 9 | #ifndef __LEVEL_HELPERS_H 10 | #define __LEVEL_HELPERS_H 11 | #include 12 | using namespace TooN; 13 | #include 14 | 15 | // Set of global colours useful for drawing stuff: 16 | extern Vector<3> gavLevelColors[]; 17 | // (These are filled in in KeyFrame.cc) 18 | 19 | // What is the scale of a level? 20 | inline int LevelScale(int nLevel) 21 | { 22 | return 1 << nLevel; 23 | } 24 | 25 | // 1-D transform to level zero: 26 | inline double LevelZeroPos(double dLevelPos, int nLevel) 27 | { 28 | return (dLevelPos + 0.5) * LevelScale(nLevel) - 0.5; 29 | } 30 | 31 | // 2-D transforms to level zero: 32 | inline Vector<2> LevelZeroPos(Vector<2> v2LevelPos, int nLevel) 33 | { 34 | Vector<2> v2Ans; 35 | v2Ans[0] = LevelZeroPos(v2LevelPos[0], nLevel); 36 | v2Ans[1] = LevelZeroPos(v2LevelPos[1], nLevel); 37 | return v2Ans; 38 | } 39 | inline Vector<2> LevelZeroPos(CVD::ImageRef irLevelPos, int nLevel) 40 | { 41 | Vector<2> v2Ans; 42 | v2Ans[0] = LevelZeroPos(irLevelPos.x, nLevel); 43 | v2Ans[1] = LevelZeroPos(irLevelPos.y, nLevel); 44 | return v2Ans; 45 | } 46 | 47 | // 1-D transform from level zero to level N: 48 | inline double LevelNPos(double dRootPos, int nLevel) 49 | { 50 | return (dRootPos + 0.5) / LevelScale(nLevel) - 0.5; 51 | } 52 | 53 | // 2-D transform from level zero to level N: 54 | inline Vector<2> LevelNPos(Vector<2> v2RootPos, int nLevel) 55 | { 56 | Vector<2> v2Ans; 57 | v2Ans[0] = LevelNPos(v2RootPos[0], nLevel); 58 | v2Ans[1] = LevelNPos(v2RootPos[1], nLevel); 59 | return v2Ans; 60 | } 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /ptam/src/Relocaliser.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2008 Isis Innovation Limited 2 | #include "ptam/Relocaliser.h" 3 | #include "ptam/SmallBlurryImage.h" 4 | #include 5 | //#include 6 | 7 | using namespace CVD; 8 | using namespace std; 9 | //using namespace GVars3; 10 | 11 | Relocaliser::Relocaliser(Map &map, ATANCamera &camera) 12 | : mMap(map), 13 | mCamera(camera) 14 | { 15 | }; 16 | 17 | SE3<> Relocaliser::BestPose() 18 | { 19 | return mse3Best; 20 | } 21 | 22 | bool Relocaliser::AttemptRecovery(KeyFrame& kCurrent) 23 | { 24 | // Ensure the incoming frame has a SmallBlurryImage attached 25 | if(!kCurrent.pSBI) 26 | kCurrent.pSBI = new SmallBlurryImage(kCurrent); 27 | else 28 | kCurrent.pSBI->MakeFromKF(kCurrent); 29 | 30 | // Find the best ZMSSD match from all keyframes in map 31 | ScoreKFs(kCurrent); 32 | 33 | // And estimate a camera rotation from a 3DOF image alignment 34 | pair, double> result_pair = kCurrent.pSBI->IteratePosRelToTarget(*mMap.vpKeyFrames[mnBest]->pSBI, 6); 35 | mse2 = result_pair.first; 36 | double dScore =result_pair.second; 37 | 38 | SE3<> se3KeyFramePos = mMap.vpKeyFrames[mnBest]->se3CfromW; 39 | mse3Best = SmallBlurryImage::SE3fromSE2(mse2, mCamera) * se3KeyFramePos; 40 | 41 | //Weiss{ 42 | 43 | const ptam::PtamParamsConfig& pPars = PtamParameters::varparams(); 44 | //if(dScore < GV2.GetDouble("Reloc2.MaxScore", 9e6, SILENT)) 45 | if(dScore < pPars.RelocMaxScore) 46 | return true; 47 | else 48 | return false; 49 | //} 50 | }; 51 | 52 | // Compare current KF to all KFs stored in map by 53 | // Zero-mean SSD 54 | void Relocaliser::ScoreKFs(KeyFrame& kCurrent) 55 | { 56 | mdBestScore = 99999999999999.9; 57 | mnBest = -1; 58 | 59 | for(unsigned int i=0; iZMSSD(*mMap.vpKeyFrames[i]->pSBI); 62 | if(dSSD < mdBestScore) 63 | { 64 | mdBestScore = dSSD; 65 | mnBest = i; 66 | } 67 | } 68 | } 69 | 70 | -------------------------------------------------------------------------------- /ptam/include/ptam/HomographyInit.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | 4 | // HomographyInit.h 5 | // Declares the HomographyInit class and a few helper functions. 6 | // 7 | // This class is used by MapMaker to bootstrap the map, and implements 8 | // the homography decomposition of Faugeras and Lustman's 1988 tech 9 | // report. 10 | // 11 | // Implementation according to Faugeras and Lustman 12 | 13 | #ifndef __HOMOGRAPHY_INIT_H 14 | #define __HOMOGRAPHY_INIT_H 15 | #include 16 | using namespace TooN; 17 | #include 18 | #include 19 | 20 | // Homography matches are 2D-2D matches in a stereo pair, unprojected 21 | // to the Z=1 plane. 22 | struct HomographyMatch 23 | { 24 | // To be filled in by MapMaker: 25 | Vector<2> v2CamPlaneFirst; 26 | Vector<2> v2CamPlaneSecond; 27 | Matrix<2> m2PixelProjectionJac; 28 | }; 29 | 30 | // Storage for each homography decomposition 31 | struct HomographyDecomposition 32 | { 33 | Vector<3> v3Tp; 34 | Matrix<3> m3Rp; 35 | double d; 36 | Vector<3> v3n; 37 | 38 | // The resolved composition.. 39 | SE3<> se3SecondFromFirst; 40 | int nScore; 41 | }; 42 | 43 | class HomographyInit 44 | { 45 | public: 46 | bool Compute(std::vector vMatches, double dMaxPixelError, SE3<> &se3SecondCameraPose); 47 | protected: 48 | Matrix<3> HomographyFromMatches(std::vector vMatches); 49 | void BestHomographyFromMatches_MLESAC(); 50 | void DecomposeHomography(); 51 | void ChooseBestDecomposition(); 52 | void RefineHomographyWithInliers(); 53 | 54 | bool IsHomographyInlier(Matrix<3> m3Homography, HomographyMatch match); 55 | double MLESACScore(Matrix<3> m3Homography, HomographyMatch match); 56 | 57 | double mdMaxPixelErrorSquared; 58 | Matrix<3> mm3BestHomography; 59 | std::vector mvMatches; 60 | std::vector mvHomographyInliers; 61 | std::vector mvDecompositions; 62 | }; 63 | 64 | 65 | 66 | #endif 67 | -------------------------------------------------------------------------------- /ptam/include/ptam/GLWindowMenu.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | 4 | #ifndef __GL_WINDOW_MENU_H 5 | #define __GL_WINDOW_MENU_H 6 | 7 | // A simple gvars-driven menu system for GLWindow2 8 | // N.b. each GLWindowMenu class internally contains sub-menus 9 | 10 | #include 11 | #include 12 | #include 13 | #include "GLWindow2.h" 14 | #include 15 | #include 16 | 17 | class GLWindowMenu 18 | { 19 | public: 20 | 21 | GLWindowMenu(std::string sName, std::string sTitle); 22 | ~GLWindowMenu(); 23 | void Render(int nTop, int nHeight, int nWidth, GLWindow2 &glw); 24 | void FillBox(int l, int r, int t, int b); 25 | void LineBox(int l, int r, int t, int b); 26 | 27 | void GUICommandHandler(std::string sCommand, std::string sParams); 28 | static void GUICommandCallBack(void* ptr, std::string sCommand, std::string sParams); 29 | 30 | bool HandleClick(int button, int state, int x, int y); 31 | 32 | 33 | 34 | private: 35 | enum MenuItemType { Button, Toggle, Monitor, Slider }; 36 | 37 | struct MenuItem 38 | { 39 | MenuItemType type; 40 | std::string sName; 41 | std::string sParam; 42 | std::string sNextMenu; 43 | GVars3::gvar2_int gvnIntValue; // Not used by all, but used by some 44 | int min; 45 | int max; 46 | }; 47 | 48 | struct SubMenu 49 | { 50 | std::vector mvItems; 51 | }; 52 | 53 | std::map mmSubMenus; 54 | std::string msCurrentSubMenu; 55 | std::string msName; 56 | std::string msTitle; 57 | 58 | 59 | int mnWidth; 60 | int mnMenuTop; 61 | int mnMenuHeight; 62 | int mnTextOffset; 63 | 64 | //Weiss{ 65 | int mgvnEnabled; 66 | int mgvnMenuItemWidth; 67 | int mgvnMenuTextOffset; 68 | //GVars3::gvar2_int mgvnEnabled; 69 | //GVars3::gvar2_int mgvnMenuItemWidth; 70 | //GVars3::gvar2_int mgvnMenuTextOffset; 71 | //} 72 | int mnLeftMostCoord; 73 | 74 | }; 75 | 76 | #endif 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /ptam/cfg/PTAMVisualizerParams.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE='ptam' 4 | import roslib; roslib.load_manifest(PACKAGE) 5 | 6 | #from driver_base.msg import SensorLevels 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator() 10 | 11 | # @todo Double check these ranges, defaults 12 | 13 | RESTART_AR = gen.const("SEND_PARAMETERS", int_t, 0x00000001, "send_parameters") 14 | 15 | 16 | # Name Type Reconfiguration lvl Description Default Min Max 17 | gen.add("ShowPC", bool_t, 0, "Show point cloud", False) 18 | gen.add("ShowAllKFs", bool_t, 0, "Show all keyframes ever made", False) 19 | gen.add("ShowKFs", bool_t, 0, "Show keyframes", False) 20 | gen.add("KFLifetime", int_t, 0, "KF lifetime in sec", 1, 1, 60000) 21 | gen.add("ShowPath", bool_t, 0, "Show path", False) 22 | gen.add("PathLength", int_t, 0, "Number of path anchor points", 1000, 1, 60000) 23 | gen.add("ExportPC", bool_t, 0, "export point cloud to file", False) 24 | gen.add("ExportPrefix", str_t, 0, "prefix for export point cloud to file", "") 25 | gen.add("SaveMap", bool_t, 0, "save full map to file", False) 26 | gen.add("KFFlags", int_t, 0, "0:get all KFs, <0: get N newest KFs, 1: get new KFs only", 0, -100, 1) 27 | 28 | 29 | exit(gen.generate(PACKAGE, "Config", "PTAMVisualizerParams")) 30 | -------------------------------------------------------------------------------- /ptam/include/ptam/GLWindow2.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | #ifndef __GL_WINDOW_2_H 4 | #define __GL_WINDOW_2_H 5 | // 6 | // A class which wraps a CVD::GLWindow and provides some basic 7 | // user-interface funtionality: A gvars-driven clickable menu, and a 8 | // caption line for text display. Also provides some handy GL helpers 9 | // and a wrapper for CVD's text display routines. 10 | 11 | #include 12 | #include 13 | 14 | class GLWindowMenu; 15 | 16 | 17 | class GLWindow2 : public CVD::GLWindow, public CVD::GLWindow::EventHandler 18 | { 19 | public: 20 | GLWindow2(CVD::ImageRef irSize, std::string sTitle); 21 | 22 | // The preferred event handler.. 23 | void HandlePendingEvents(); 24 | 25 | // Menu interface: 26 | void AddMenu(std::string sName, std::string sTitle); 27 | void DrawMenus(); 28 | 29 | // Some OpenGL helpers: 30 | void SetupViewport(); 31 | void SetupVideoOrtho(); 32 | void SetupUnitOrtho(); 33 | void SetupWindowOrtho(); 34 | void SetupVideoRasterPosAndZoom(); 35 | 36 | // Text display functions: 37 | void PrintString(CVD::ImageRef irPos, std::string s); 38 | void DrawCaption(std::string s); 39 | 40 | // Map viewer mouse interface: 41 | std::pair, TooN::Vector<6> > GetMousePoseUpdate(); 42 | 43 | 44 | protected: 45 | void GUICommandHandler(std::string sCommand, std::string sParams); 46 | static void GUICommandCallBack(void* ptr, std::string sCommand, std::string sParams); 47 | 48 | // User interface menus: 49 | std::vector mvpGLWindowMenus; 50 | 51 | CVD::ImageRef mirVideoSize; // The size of the source video material. 52 | 53 | 54 | // Event handling routines: 55 | virtual void on_key_down(GLWindow&, int key); 56 | virtual void on_mouse_move(GLWindow& win, CVD::ImageRef where, int state); 57 | virtual void on_mouse_down(GLWindow& win, CVD::ImageRef where, int state, int button); 58 | virtual void on_event(GLWindow& win, int event); 59 | CVD::ImageRef mirLastMousePos; 60 | 61 | // Storage for map viewer updates: 62 | TooN::Vector<6> mvMCPoseUpdate; 63 | TooN::Vector<6> mvLeftPoseUpdate; 64 | 65 | 66 | }; 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /ptam/thirdparty/agast/include/agast/agast5_8.h: -------------------------------------------------------------------------------- 1 | // 2 | // agast5 - AGAST, an adaptive and generic corner detector based on the 3 | // accelerated segment test for a 8 pixel mask 4 | // 5 | // Copyright (C) 2010 Elmar Mair 6 | // 7 | // This program is free software: you can redistribute it and/or modify 8 | // it under the terms of the GNU General Public License as published by 9 | // the Free Software Foundation, either version 3 of the License, or 10 | // (at your option) any later version. 11 | // 12 | // This program is distributed in the hope that it will be useful, 13 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | // GNU General Public License for more details. 16 | // 17 | // You should have received a copy of the GNU General Public License 18 | // along with this program. If not, see . 19 | 20 | #ifndef AGAST5_8_H 21 | #define AGAST5_8_H 22 | 23 | #include 24 | #include "AstDetector.h" 25 | 26 | struct CvPoint; 27 | 28 | namespace agast{ 29 | 30 | class AgastDetector5_8 : public AstDetector 31 | { 32 | public: 33 | AgastDetector5_8():AstDetector(){;} 34 | AgastDetector5_8(int width, int height, int thr):AstDetector(width, height, thr){init_pattern();}; 35 | ~AgastDetector5_8(){} 36 | void detect(const unsigned char* im, 37 | std::vector& keypoints); 38 | void nms(const unsigned char* im, 39 | const std::vector& keypoints, std::vector& keypoints_nms); 40 | int get_borderWidth(){return borderWidth;} 41 | int cornerScore(const unsigned char* p, bool ignorePattern=false); 42 | 43 | private: 44 | static const int borderWidth=1; 45 | int_fast16_t s_offset0; 46 | int_fast16_t s_offset1; 47 | int_fast16_t s_offset2; 48 | int_fast16_t s_offset3; 49 | int_fast16_t s_offset4; 50 | int_fast16_t s_offset5; 51 | int_fast16_t s_offset6; 52 | int_fast16_t s_offset7; 53 | 54 | void init_pattern() 55 | { 56 | s_offset0=(-1)+(0)*xsize; 57 | s_offset1=(-1)+(-1)*xsize; 58 | s_offset2=(0)+(-1)*xsize; 59 | s_offset3=(1)+(-1)*xsize; 60 | s_offset4=(1)+(0)*xsize; 61 | s_offset5=(1)+(1)*xsize; 62 | s_offset6=(0)+(1)*xsize; 63 | s_offset7=(-1)+(1)*xsize; 64 | } 65 | }; 66 | 67 | } 68 | 69 | #endif /* AGAST5_8_H */ 70 | -------------------------------------------------------------------------------- /ptam/thirdparty/agast/include/agast/AstDetector.h: -------------------------------------------------------------------------------- 1 | // 2 | // AstDetector - the interface class for the AGAST corner detector 3 | // 4 | // Copyright (C) 2010 Elmar Mair 5 | // 6 | // This program is free software: you can redistribute it and/or modify 7 | // it under the terms of the GNU General Public License as published by 8 | // the Free Software Foundation, either version 3 of the License, or 9 | // (at your option) any later version. 10 | // 11 | // This program is distributed in the hope that it will be useful, 12 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | // GNU General Public License for more details. 15 | // 16 | // You should have received a copy of the GNU General Public License 17 | // along with this program. If not, see . 18 | 19 | #ifndef ASTDETECTOR_H 20 | #define ASTDETECTOR_H 21 | 22 | #include 23 | #include 24 | 25 | struct CvPoint; 26 | 27 | namespace agast{ 28 | 29 | class AstDetector 30 | { 31 | public: 32 | AstDetector():xsize(0),ysize(0),b(-1) {} 33 | AstDetector(int width, int height, int thr):xsize(width),ysize(height),b(thr) {} 34 | virtual ~AstDetector(){;} 35 | virtual void detect(const unsigned char* im, std::vector& corners_all)=0; 36 | virtual int get_borderWidth()=0; 37 | void nms(const unsigned char* im, 38 | const std::vector& corners_all, std::vector& corners_nms); 39 | void processImage(const unsigned char* im, 40 | std::vector& keypoints_nms) { 41 | std::vector keypoints; 42 | detect(im,keypoints); 43 | nms(im,keypoints,keypoints_nms);} 44 | void set_threshold(int b_){b=b_;} 45 | void set_imageSize(int xsize_, int ysize_){xsize=xsize_; ysize=ysize_; init_pattern();} 46 | virtual int cornerScore(const unsigned char* p, bool ignorePattern=false)=0; 47 | 48 | protected: 49 | virtual void init_pattern()=0; 50 | void score(const unsigned char* i, const std::vector& corners_all); 51 | void nonMaximumSuppression(const std::vector& corners_all, 52 | std::vector& corners_nms); 53 | std::vector scores; 54 | std::vector nmsFlags; 55 | int xsize, ysize; 56 | int b; 57 | }; 58 | 59 | } 60 | 61 | #endif /* AGASTDETECTOR_H */ 62 | -------------------------------------------------------------------------------- /ptam/include/ptam/Params.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Params.h 3 | * 4 | * Created on: Jul 21, 2010 5 | * Author: sweiss 6 | */ 7 | 8 | #ifndef PARAMS_H_ 9 | #define PARAMS_H_ 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | typedef dynamic_reconfigure::Server PtamParamsReconfigureServer; 20 | typedef ptam::PtamParamsConfig VarParams; 21 | 22 | class FixParams 23 | { 24 | public: 25 | int ImageSizeX; 26 | int ImageSizeY; 27 | int ARBuffer_width; 28 | int ARBuffer_height; 29 | double WiggleScale; 30 | std::string BundleMEstimator; 31 | std::string TrackerMEstimator; 32 | double MinTukeySigma; 33 | int CandidateMinSTScore; 34 | double Cam_fx; 35 | double Cam_fy; 36 | double Cam_cx; 37 | double Cam_cy; 38 | double Cam_s; 39 | double MaxFoV; 40 | double Calibrator_BlurSigma; 41 | double Calibrator_MeanGate; 42 | int Calibrator_MinCornersForGrabbedImage; 43 | bool Calibrator_Optimize; 44 | bool Calibrator_Show; 45 | bool Calibrator_NoDistortion; 46 | double CameraCalibrator_MaxStepDistFraction; 47 | int CameraCalibrator_CornerPatchSize; 48 | bool GLWindowMenu_Enable; 49 | int GLWindowMenu_mgvnMenuItemWidth; 50 | int GLWindowMenu_mgvnMenuTextOffset; 51 | int InitLevel; 52 | std::string parent_frame; 53 | bool gui; 54 | void readFixParams(); 55 | }; 56 | 57 | class PtamParameters{ 58 | private: 59 | ptam::PtamParamsConfig mVarParams; 60 | FixParams mFixParams; 61 | 62 | PtamParamsReconfigureServer *mpPtamParamsReconfigureServer; 63 | 64 | void ptamParamsConfig(ptam::PtamParamsConfig & config, uint32_t level){ 65 | mVarParams = config; 66 | }; 67 | static PtamParameters* inst_; 68 | PtamParameters() 69 | { 70 | mpPtamParamsReconfigureServer = new PtamParamsReconfigureServer(ros::NodeHandle("~")); 71 | PtamParamsReconfigureServer::CallbackType PtamParamCall = boost::bind(&PtamParameters::ptamParamsConfig, this, _1, _2); 72 | mpPtamParamsReconfigureServer->setCallback(PtamParamCall); 73 | 74 | mFixParams.readFixParams(); 75 | } 76 | ~PtamParameters(){ 77 | delete inst_; 78 | inst_ = NULL; 79 | } 80 | public: 81 | static const ptam::PtamParamsConfig& varparams(){ 82 | if(!inst_){ 83 | inst_ = new PtamParameters; 84 | } 85 | return inst_->mVarParams; 86 | } 87 | static const FixParams& fixparams(){ 88 | if(!inst_){ 89 | inst_ = new PtamParameters; 90 | } 91 | return inst_->mFixParams; 92 | } 93 | }; 94 | 95 | #endif /* PARAMS_H_ */ 96 | -------------------------------------------------------------------------------- /ptam/src/Params.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Params.cpp 3 | * 4 | * Created on: Sep 17, 2010 5 | * Author: asl 6 | */ 7 | 8 | #include 9 | 10 | 11 | void FixParams::readFixParams() 12 | { 13 | ros::NodeHandle nh("~"); 14 | 15 | // kill program, if we don't have a camera calibration 16 | bool have_calibration = true; 17 | have_calibration = have_calibration && nh.getParam("Cam_fx", Cam_fx); 18 | have_calibration = have_calibration && nh.getParam("Cam_fy", Cam_fy); 19 | have_calibration = have_calibration && nh.getParam("Cam_cx", Cam_cx); 20 | have_calibration = have_calibration && nh.getParam("Cam_cy", Cam_cy); 21 | have_calibration = have_calibration && nh.getParam("Cam_s", Cam_s); 22 | 23 | if (!have_calibration) 24 | { 25 | ROS_ERROR("Camera calibration is missing!"); 26 | // ROS_BREAK(); 27 | } 28 | 29 | nh.param("ImageSizeX", ImageSizeX, 640); 30 | nh.param("ImageSizeY", ImageSizeY, 480); 31 | nh.param("ARBuffer_width", ARBuffer_width, 1200); 32 | nh.param("ARBuffer_height", ARBuffer_height, 900); 33 | nh.param("WiggleScale", WiggleScale, 0.1); 34 | nh.param("BundleMEstimator", BundleMEstimator, std::string("Tukey")); 35 | nh.param("TrackerMEstimator", TrackerMEstimator, std::string("Tukey")); 36 | nh.param("MinTukeySigma", MinTukeySigma, 0.4); 37 | nh.param("CandidateMinSTScore", CandidateMinSTScore, 70); 38 | // nh.param("Cam_fx", Cam_fx,0.392); 39 | // nh.param("Cam_fy", Cam_fy,0.613); 40 | // nh.param("Cam_cx", Cam_cx,0.484); 41 | // nh.param("Cam_cy", Cam_cy,0.476); 42 | // nh.param("Cam_s", Cam_s,0.967); 43 | nh.param("Calibrator_BlurSigma", Calibrator_BlurSigma, 1.0); 44 | nh.param("Calibrator_MeanGate", Calibrator_MeanGate, 10.0); 45 | nh.param("Calibrator_MinCornersForGrabbedImage", Calibrator_MinCornersForGrabbedImage, 20); 46 | nh.param("Calibrator_Optimize", Calibrator_Optimize, false); 47 | nh.param("Calibrator_Show", Calibrator_Show, false); 48 | nh.param("Calibrator_NoDistortion", Calibrator_NoDistortion, false); 49 | nh.param("CameraCalibrator_MaxStepDistFraction", CameraCalibrator_MaxStepDistFraction, 0.3); 50 | nh.param("CameraCalibrator_CornerPatchSize", CameraCalibrator_CornerPatchSize, 20); 51 | nh.param("GLWindowMenu_Enable", GLWindowMenu_Enable, true); 52 | nh.param("GLWindowMenu_mgvnMenuItemWidth", GLWindowMenu_mgvnMenuItemWidth, 90); 53 | nh.param("GLWindowMenu_mgvnMenuTextOffset", GLWindowMenu_mgvnMenuTextOffset, 20); 54 | nh.param("InitLevel", InitLevel, 1); 55 | nh.param("parent_frame", parent_frame, std::string("world")); 56 | nh.param("gui", gui, false); 57 | nh.param("MaxFoV", MaxFoV, 130.0); 58 | } 59 | ; 60 | PtamParameters* PtamParameters::inst_ = NULL; 61 | -------------------------------------------------------------------------------- /ptam/thirdparty/agast/include/agast/agast7_12d.h: -------------------------------------------------------------------------------- 1 | // 2 | // agast7d - AGAST, an adaptive and generic corner detector based on the 3 | // accelerated segment test for a 12 pixel mask in diamond format 4 | // 5 | // Copyright (C) 2010 Elmar Mair 6 | // 7 | // This program is free software: you can redistribute it and/or modify 8 | // it under the terms of the GNU General Public License as published by 9 | // the Free Software Foundation, either version 3 of the License, or 10 | // (at your option) any later version. 11 | // 12 | // This program is distributed in the hope that it will be useful, 13 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | // GNU General Public License for more details. 16 | // 17 | // You should have received a copy of the GNU General Public License 18 | // along with this program. If not, see . 19 | 20 | #ifndef AGAST7_12D_H 21 | #define AGAST7_12D_H 22 | 23 | #include 24 | #include "AstDetector.h" 25 | 26 | struct CvPoint; 27 | 28 | namespace agast{ 29 | 30 | class AgastDetector7_12d: public AstDetector 31 | { 32 | public: 33 | AgastDetector7_12d():AstDetector(){;} 34 | AgastDetector7_12d(int width, int height, int thr):AstDetector(width, height, thr){init_pattern();}; 35 | ~AgastDetector7_12d(){} 36 | void detect(const unsigned char* im, 37 | std::vector& keypoints); 38 | void nms(const unsigned char* im, 39 | const std::vector& keypoints, std::vector& keypoints_nms); 40 | int get_borderWidth(){return borderWidth;} 41 | int cornerScore(const unsigned char* p, bool ignorePattern=false); 42 | 43 | private: 44 | static const int borderWidth=3; 45 | int_fast16_t s_offset0; 46 | int_fast16_t s_offset1; 47 | int_fast16_t s_offset2; 48 | int_fast16_t s_offset3; 49 | int_fast16_t s_offset4; 50 | int_fast16_t s_offset5; 51 | int_fast16_t s_offset6; 52 | int_fast16_t s_offset7; 53 | int_fast16_t s_offset8; 54 | int_fast16_t s_offset9; 55 | int_fast16_t s_offset10; 56 | int_fast16_t s_offset11; 57 | 58 | void init_pattern() 59 | { 60 | s_offset0=(-3)+(0)*xsize; 61 | s_offset1=(-2)+(-1)*xsize; 62 | s_offset2=(-1)+(-2)*xsize; 63 | s_offset3=(0)+(-3)*xsize; 64 | s_offset4=(1)+(-2)*xsize; 65 | s_offset5=(2)+(-1)*xsize; 66 | s_offset6=(3)+(0)*xsize; 67 | s_offset7=(2)+(1)*xsize; 68 | s_offset8=(1)+(2)*xsize; 69 | s_offset9=(0)+(3)*xsize; 70 | s_offset10=(-1)+(2)*xsize; 71 | s_offset11=(-2)+(1)*xsize; 72 | } 73 | }; 74 | 75 | } 76 | 77 | #endif /* AGAST7_12D_H */ 78 | -------------------------------------------------------------------------------- /ptam/thirdparty/agast/include/agast/agast7_12s.h: -------------------------------------------------------------------------------- 1 | // 2 | // agast7s - AGAST, an adaptive and generic corner detector based on the 3 | // accelerated segment test for a 12 pixel mask in square format 4 | // 5 | // Copyright (C) 2010 Elmar Mair 6 | // 7 | // This program is free software: you can redistribute it and/or modify 8 | // it under the terms of the GNU General Public License as published by 9 | // the Free Software Foundation, either version 3 of the License, or 10 | // (at your option) any later version. 11 | // 12 | // This program is distributed in the hope that it will be useful, 13 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | // GNU General Public License for more details. 16 | // 17 | // You should have received a copy of the GNU General Public License 18 | // along with this program. If not, see . 19 | 20 | #ifndef AGAST7_12S_H 21 | #define AGAST7_12S_H 22 | 23 | #include 24 | #include "AstDetector.h" 25 | 26 | struct CvPoint; 27 | 28 | namespace agast{ 29 | 30 | class AgastDetector7_12s : public AstDetector 31 | { 32 | public: 33 | AgastDetector7_12s():AstDetector(){;} 34 | AgastDetector7_12s(int width, int height, int thr):AstDetector(width, height, thr){init_pattern();}; 35 | ~AgastDetector7_12s(){} 36 | void detect(const unsigned char* im, 37 | std::vector& keypoints); 38 | void nms(const unsigned char* im, 39 | const std::vector& keypoints, std::vector& keypoints_nms); 40 | int get_borderWidth(){return borderWidth;} 41 | int cornerScore(const unsigned char* p, bool ignorePattern=false); 42 | 43 | private: 44 | static const int borderWidth=2; 45 | int_fast16_t s_offset0; 46 | int_fast16_t s_offset1; 47 | int_fast16_t s_offset2; 48 | int_fast16_t s_offset3; 49 | int_fast16_t s_offset4; 50 | int_fast16_t s_offset5; 51 | int_fast16_t s_offset6; 52 | int_fast16_t s_offset7; 53 | int_fast16_t s_offset8; 54 | int_fast16_t s_offset9; 55 | int_fast16_t s_offset10; 56 | int_fast16_t s_offset11; 57 | 58 | void init_pattern() 59 | { 60 | s_offset0=(-2)+(0)*xsize; 61 | s_offset1=(-2)+(-1)*xsize; 62 | s_offset2=(-1)+(-2)*xsize; 63 | s_offset3=(0)+(-2)*xsize; 64 | s_offset4=(1)+(-2)*xsize; 65 | s_offset5=(2)+(-1)*xsize; 66 | s_offset6=(2)+(0)*xsize; 67 | s_offset7=(2)+(1)*xsize; 68 | s_offset8=(1)+(2)*xsize; 69 | s_offset9=(0)+(2)*xsize; 70 | s_offset10=(-1)+(2)*xsize; 71 | s_offset11=(-2)+(1)*xsize; 72 | } 73 | }; 74 | 75 | } 76 | 77 | #endif /* AGAST7_12S_H */ 78 | -------------------------------------------------------------------------------- /ptam/thirdparty/agast/include/agast/oast9_16.h: -------------------------------------------------------------------------------- 1 | // 2 | // agast9 - OAST, an optimal corner detector based on the 3 | // accelerated segment test for a 16 pixel mask 4 | // 5 | // Copyright (C) 2010 Elmar Mair 6 | // 7 | // This program is free software: you can redistribute it and/or modify 8 | // it under the terms of the GNU General Public License as published by 9 | // the Free Software Foundation, either version 3 of the License, or 10 | // (at your option) any later version. 11 | // 12 | // This program is distributed in the hope that it will be useful, 13 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | // GNU General Public License for more details. 16 | // 17 | // You should have received a copy of the GNU General Public License 18 | // along with this program. If not, see . 19 | 20 | #ifndef OAST9_16_H 21 | #define OAST9_16_H 22 | 23 | #include 24 | #include "AstDetector.h" 25 | 26 | struct CvPoint; 27 | 28 | namespace agast{ 29 | 30 | class OastDetector9_16 : public AstDetector 31 | { 32 | public: 33 | OastDetector9_16():AstDetector(){;} 34 | OastDetector9_16(int width, int height, int thr):AstDetector(width, height, thr){init_pattern();}; 35 | ~OastDetector9_16(){} 36 | void detect(const unsigned char* im, 37 | std::vector& keypoints); 38 | void nms(const unsigned char* im, 39 | const std::vector& keypoints, std::vector& keypoints_nms); 40 | int get_borderWidth(){return borderWidth;} 41 | int cornerScore(const unsigned char* p, bool ignorePattern=false); 42 | 43 | private: 44 | static const int borderWidth=3; 45 | int_fast16_t s_offset0; 46 | int_fast16_t s_offset1; 47 | int_fast16_t s_offset2; 48 | int_fast16_t s_offset3; 49 | int_fast16_t s_offset4; 50 | int_fast16_t s_offset5; 51 | int_fast16_t s_offset6; 52 | int_fast16_t s_offset7; 53 | int_fast16_t s_offset8; 54 | int_fast16_t s_offset9; 55 | int_fast16_t s_offset10; 56 | int_fast16_t s_offset11; 57 | int_fast16_t s_offset12; 58 | int_fast16_t s_offset13; 59 | int_fast16_t s_offset14; 60 | int_fast16_t s_offset15; 61 | 62 | void init_pattern() 63 | { 64 | s_offset0=(-3)+(0)*xsize; 65 | s_offset1=(-3)+(-1)*xsize; 66 | s_offset2=(-2)+(-2)*xsize; 67 | s_offset3=(-1)+(-3)*xsize; 68 | s_offset4=(0)+(-3)*xsize; 69 | s_offset5=(1)+(-3)*xsize; 70 | s_offset6=(2)+(-2)*xsize; 71 | s_offset7=(3)+(-1)*xsize; 72 | s_offset8=(3)+(0)*xsize; 73 | s_offset9=(3)+(1)*xsize; 74 | s_offset10=(2)+(2)*xsize; 75 | s_offset11=(1)+(3)*xsize; 76 | s_offset12=(0)+(3)*xsize; 77 | s_offset13=(-1)+(3)*xsize; 78 | s_offset14=(-2)+(2)*xsize; 79 | s_offset15=(-3)+(1)*xsize; 80 | } 81 | }; 82 | 83 | } 84 | 85 | #endif /* OAST9_16_H */ 86 | -------------------------------------------------------------------------------- /rqt_ptam/include/rqt_ptam/ratio_layouted_frame.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011, Dirk Thomas, TU Darmstadt 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * * Neither the name of the TU Darmstadt nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #ifndef rqt_image_view__RatioLayoutedFrame_H 34 | #define rqt_image_view__RatioLayoutedFrame_H 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace rqt_ptam { 44 | 45 | /** 46 | * RatioLayoutedFrame is a layout containing a single frame with a fixed aspect ratio. 47 | * The default aspect ratio is 4:3. 48 | */ 49 | class RatioLayoutedFrame 50 | : public QFrame 51 | { 52 | 53 | Q_OBJECT 54 | 55 | public: 56 | 57 | RatioLayoutedFrame(QWidget* parent, Qt::WFlags flags = 0); 58 | 59 | virtual ~RatioLayoutedFrame(); 60 | 61 | QRect getAspectRatioCorrectPaintArea(); 62 | 63 | void resizeToFitAspectRatio(); 64 | 65 | void setInnerFrameMinimumSize(const QSize& size); 66 | 67 | void setInnerFrameMaximumSize(const QSize& size); 68 | 69 | void setInnerFrameFixedSize(const QSize& size); 70 | 71 | void setAspectRatio(unsigned short width, unsigned short height); 72 | 73 | private: 74 | 75 | static int greatestCommonDivisor(int a, int b); 76 | 77 | QSize aspect_ratio_; 78 | 79 | }; 80 | 81 | } 82 | 83 | #endif // rqt_image_view__RatioLayoutedFrame_H 84 | -------------------------------------------------------------------------------- /ptam/thirdparty/agast/include/agast/agast_corner_detect.h: -------------------------------------------------------------------------------- 1 | /* 2 | * agast_corner_detect.h 3 | * 4 | * Created on: 01.12.2010 5 | * Author: slynen 6 | * wrapper for the agast detector 7 | * to be used as PTAM corner detector functor 8 | */ 9 | 10 | #ifndef AGAST_CORNER_DETECT_H_ 11 | #define AGAST_CORNER_DETECT_H_ 12 | 13 | #include "agast7_12d.h" 14 | #include "oast9_16.h" 15 | #include 16 | #include 17 | #include 18 | 19 | namespace agast{ 20 | 21 | namespace agast7_12d{ 22 | 23 | static AgastDetector7_12d detector; 24 | 25 | //libcvd 26 | __inline__ void agast_corner_detect12d(const CVD::BasicImage& im, std::vector& corners, int barrier) 27 | { 28 | detector.set_imageSize(im.size().x, im.size().y); 29 | detector.set_threshold(barrier); 30 | std::vector keypoints; 31 | detector.detect((unsigned char*)im.data(),keypoints); 32 | corners.reserve(keypoints.size()); 33 | for(size_t i=0;i& corners, int barrier) 42 | { 43 | detector.set_imageSize(im.size().width, im.size().height); 44 | detector.set_threshold(barrier); 45 | std::vector keypoints; 46 | detector.detect((unsigned char*)im.data,keypoints); 47 | corners.reserve(keypoints.size()); 48 | for(size_t i=0;i& im, std::vector& corners, int barrier) 61 | { 62 | detector.set_imageSize(im.size().x, im.size().y); 63 | detector.set_threshold(barrier); 64 | std::vector keypoints; 65 | detector.detect((unsigned char*)im.data(),keypoints); 66 | corners.reserve(keypoints.size()); 67 | for(size_t i=0;i& corners, int barrier) 76 | { 77 | detector.set_imageSize(im.size().width, im.size().height); 78 | detector.set_threshold(barrier); 79 | std::vector keypoints; 80 | detector.detect((unsigned char*)im.data,keypoints); 81 | corners.reserve(keypoints.size()); 82 | for(size_t i=0;i &im, const CVD::ImageRef &ir) 8 | { 9 | if(!im.in_image_with_border(ir, mnHalfPatchSize)) 10 | return mnMaxSSD + 1; 11 | ImageRef irImgBase = ir - ImageRef(mnHalfPatchSize, mnHalfPatchSize); 12 | int nRows = mimOrigPatch.size().y; 13 | int nCols = mimOrigPatch.size().x; 14 | byte *imagepointer; 15 | byte *templatepointer; 16 | int nDiff; 17 | int nSumSqDiff = 0; 18 | for(int nRow = 0; nRow < nRows; nRow++) 19 | { 20 | imagepointer = &im[irImgBase + ImageRef(0,nRow)]; 21 | templatepointer = &mimOrigPatch[ImageRef(0,nRow)]; 22 | for(int nCol = 0; nCol < nCols; nCol++) 23 | { 24 | nDiff = imagepointer[nCol] - templatepointer[nCol]; 25 | nSumSqDiff += nDiff * nDiff; 26 | }; 27 | }; 28 | return nSumSqDiff; 29 | } 30 | 31 | // Find a patch by searching at FAST corners in an input image 32 | // If available, a row-corner LUT is used to speed up search through the 33 | // FAST corners 34 | bool MiniPatch::FindPatch(CVD::ImageRef &irPos, 35 | CVD::BasicImage &im, 36 | int nRange, 37 | vector &vCorners, 38 | std::vector *pvRowLUT) 39 | { 40 | ImageRef irCenter = irPos; 41 | ImageRef irBest; 42 | int nBestSSD = mnMaxSSD + 1; 43 | ImageRef irBBoxTL = irPos - ImageRef(nRange, nRange); 44 | ImageRef irBBoxBR = irPos + ImageRef(nRange, nRange); 45 | vector::iterator i; 46 | if(!pvRowLUT) 47 | { 48 | for(i = vCorners.begin(); i!=vCorners.end(); i++) 49 | if(i->y >= irBBoxTL.y) break; 50 | } 51 | else 52 | { 53 | int nTopRow = irBBoxTL.y; 54 | if(nTopRow < 0) 55 | nTopRow = 0; 56 | if(nTopRow >= (int) pvRowLUT->size()) 57 | nTopRow = (int) pvRowLUT->size() - 1; 58 | i = vCorners.begin() + (*pvRowLUT)[nTopRow]; 59 | } 60 | 61 | for(; i!=vCorners.end(); i++) 62 | { 63 | if(i->x < irBBoxTL.x || i->x > irBBoxBR.x) 64 | continue; 65 | if(i->y > irBBoxBR.y) 66 | break; 67 | int nSSD = SSDAtPoint(im, *i); 68 | 69 | if(nSSD < nBestSSD) 70 | { 71 | irBest = *i; 72 | nBestSSD = nSSD; 73 | } 74 | } 75 | if(nBestSSD < mnMaxSSD) 76 | { 77 | irPos = irBest; 78 | return true; 79 | } 80 | else 81 | return false; 82 | } 83 | 84 | // Define the patch from an input image 85 | void MiniPatch::SampleFromImage(ImageRef irPos, BasicImage &im) 86 | { 87 | assert(im.in_image_with_border(irPos, mnHalfPatchSize)); 88 | CVD::ImageRef irPatchSize( 2 * mnHalfPatchSize + 1 , 2 * mnHalfPatchSize + 1); 89 | mimOrigPatch.resize(irPatchSize); 90 | copy(im, mimOrigPatch, mimOrigPatch.size(), irPos - mimOrigPatch.size() / 2); 91 | } 92 | 93 | // Static members 94 | int MiniPatch::mnHalfPatchSize = 4; 95 | int MiniPatch::mnRange = 10; 96 | int MiniPatch::mnMaxSSD = 9999; 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | -------------------------------------------------------------------------------- /ptam/rvizconf.vcg: -------------------------------------------------------------------------------- 1 | Background\ ColorR=0 2 | Background\ ColorG=0 3 | Background\ ColorB=0 4 | Fixed\ Frame=/world 5 | Target\ Frame=/world 6 | Axes.Enabled=1 7 | Axes.Length=0.1 8 | Axes.Radius=0.01 9 | Axes.Reference\ Frame=/world 10 | Axes2.Enabled=1 11 | Axes2.Length=0.2 12 | Axes2.Radius=0.02 13 | Axes2.Reference\ Frame=/hex/cam_bottom 14 | Grid.Alpha=0.5 15 | Grid.Cell\ Size=0.1 16 | Grid.ColorR=0.5 17 | Grid.ColorG=0.5 18 | Grid.ColorB=0.5 19 | Grid.Enabled=1 20 | Grid.Line\ Style=0 21 | Grid.Line\ Width=0.03 22 | Grid.Normal\ Cell\ Count=0 23 | Grid.OffsetX=0 24 | Grid.OffsetY=0 25 | Grid.OffsetZ=0 26 | Grid.Plane=0 27 | Grid.Plane\ Cell\ Count=10 28 | Grid.Reference\ Frame=/world 29 | Markers.Enabled=1 30 | Markers.Marker\ Topic=/vslam/kf_visualization 31 | Markers.pointcloud_publisher=1 32 | Markers2.Enabled=1 33 | Markers2.Marker\ Topic=/vslam/path_visualization 34 | Markers2.pointcloud_publisher=1 35 | Point\ Cloud2.Alpha=1 36 | Point\ Cloud2.Billboard\ Size=0.005 37 | Point\ Cloud2.Color\ Transformer=RGB8 38 | Point\ Cloud2.Decay\ Time=10 39 | Point\ Cloud2.Enabled=1 40 | Point\ Cloud2.Position\ Transformer=XYZ 41 | Point\ Cloud2.Selectable=1 42 | Point\ Cloud2.Style=2 43 | Point\ Cloud2.Topic=/vslam/pc2 44 | Point\ Cloud2..AxisAutocompute\ Value\ Bounds=1 45 | Point\ Cloud2..AxisAxis=2 46 | Point\ Cloud2..AxisMax\ Value=0.080364 47 | Point\ Cloud2..AxisMin\ Value=-0.179101 48 | Point\ Cloud2..AxisUse\ Fixed\ Frame=1 49 | Point\ Cloud2..Flat\ ColorColorR=0.380392 50 | Point\ Cloud2..Flat\ ColorColorG=0.968627 51 | Point\ Cloud2..Flat\ ColorColorB=0.141176 52 | Point\ Cloud2..IntensityAutocompute\ Intensity\ Bounds=1 53 | Point\ Cloud2..IntensityMax\ ColorR=1 54 | Point\ Cloud2..IntensityMax\ ColorG=1 55 | Point\ Cloud2..IntensityMax\ ColorB=1 56 | Point\ Cloud2..IntensityMax\ Intensity=4096 57 | Point\ Cloud2..IntensityMin\ ColorR=0 58 | Point\ Cloud2..IntensityMin\ ColorG=0 59 | Point\ Cloud2..IntensityMin\ ColorB=0 60 | Point\ Cloud2..IntensityMin\ Intensity=0 61 | TF.All\ Enabled=1 62 | TF.Enabled=1 63 | TF.Frame\ Timeout=15 64 | TF.Show\ Arrows=1 65 | TF.Show\ Axes=1 66 | TF.Show\ Names=1 67 | TF.Update\ Interval=0 68 | Tool\ 2D\ Nav\ GoalTopic=goal 69 | Tool\ 2D\ Pose\ EstimateTopic=initialpose 70 | Camera\ Type=rviz::OrbitViewController 71 | Camera\ Config=1.621 0.0633592 1.73171 -0.246909 0.213668 -0.149132 72 | Property\ Grid\ State=selection=Point Cloud2.Enabled.Point Cloud2.Alpha;expanded=.Global Options,Grid.Enabled,Point Cloud2.Enabled,Axes.Enabled,TF.Enabled,TF./hex/cam_bottom/hex/cam_bottom,TF./world/world,TF.Enabled.TF.Tree,TF./hex/cam_bottomTree/hex/cam_bottom,TF./worldTree/world,Axes2.Enabled,Markers.Enabled,Markers.Enabled.Markers.Namespaces,Markers2.Enabled,Markers2.Enabled.Markers2.Namespaces;scrollpos=0,15;splitterpos=235,470;ispageselected=1 73 | [Display0] 74 | Name=Grid 75 | Package=rviz 76 | ClassName=rviz::GridDisplay 77 | [Display1] 78 | Name=Point Cloud2 79 | Package=rviz 80 | ClassName=rviz::PointCloud2Display 81 | [Display2] 82 | Name=Axes 83 | Package=rviz 84 | ClassName=rviz::AxesDisplay 85 | [Display3] 86 | Name=TF 87 | Package=rviz 88 | ClassName=rviz::TFDisplay 89 | [Display4] 90 | Name=Axes2 91 | Package=rviz 92 | ClassName=rviz::AxesDisplay 93 | [Display5] 94 | Name=Markers 95 | Package=rviz 96 | ClassName=rviz::MarkerDisplay 97 | [Display6] 98 | Name=Markers2 99 | Package=rviz 100 | ClassName=rviz::MarkerDisplay 101 | [TF.] 102 | worldEnabled=1 103 | [TF./hex] 104 | cam_bottomEnabled=1 105 | -------------------------------------------------------------------------------- /ptam/include/ptam/MapPoint.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | // 4 | // This file declares the MapPoint class 5 | // 6 | // The map is made up of a bunch of mappoints. 7 | // Each one is just a 3D point in the world; 8 | // it also includes information on where and in which key-frame the point was 9 | // originally made from, so that pixels from that keyframe can be used 10 | // to search for that point. 11 | // Also stores stuff like inlier/outlier counts, and privat information for 12 | // both Tracker and MapMaker. 13 | 14 | #ifndef __MAP_POINT_H 15 | #define __MAP_POINT_H 16 | #include 17 | using namespace TooN; 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | struct KeyFrame; 24 | struct TrackerData; 25 | struct MapMakerData; 26 | 27 | struct MapPoint 28 | { 29 | typedef boost::shared_ptr Ptr; 30 | 31 | // Constructor inserts sensible defaults and zeros pointers. 32 | inline MapPoint() 33 | { 34 | bBad = false; 35 | pTData = NULL; 36 | pMMData = NULL; 37 | nMEstimatorOutlierCount = 0; 38 | nMEstimatorInlierCount = 0; 39 | dCreationTime = CVD::timer.get_time(); 40 | //{slynen reprojection 41 | bAlreadyProjected=false; 42 | //} 43 | nSourceLevel = 0; 44 | //slynen pcl interface{ 45 | static size_t ID = 0; 46 | bID = ID++; 47 | //} 48 | }; 49 | 50 | 51 | // Where in the world is this point? The main bit of information, really. 52 | Vector<3> v3WorldPos; 53 | // Is it a dud? In that case it'll be moved to the trash soon. 54 | bool bBad; 55 | //{slynen reprojection 56 | bool bAlreadyProjected; 57 | //} 58 | //slynen pcl interface{ 59 | int bID; 60 | //} 61 | // What pixels should be used to search for this point? 62 | boost::shared_ptr pPatchSourceKF; // The KeyFrame the point was originally made in 63 | int nSourceLevel; // Pyramid level in source KeyFrame 64 | CVD::ImageRef irCenter; // This is in level-coords in the source pyramid level 65 | 66 | // What follows next is a bunch of intermediate vectors - they all lead up 67 | // to being able to calculate v3Pixel{Down,Right}_W, which the PatchFinder 68 | // needs for patch warping! 69 | 70 | Vector<3> v3Center_NC; // Unit vector in Source-KF coords pointing at the patch center 71 | Vector<3> v3OneDownFromCenter_NC; // Unit vector in Source-KF coords pointing towards one pixel down of the patch center 72 | Vector<3> v3OneRightFromCenter_NC; // Unit vector in Source-KF coords pointing towards one pixel right of the patch center 73 | Vector<3> v3Normal_NC; // Unit vector in Source-KF coords indicating patch normal 74 | 75 | Vector<3> v3PixelDown_W; // 3-Vector in World coords corresponding to a one-pixel move down the source image 76 | Vector<3> v3PixelRight_W; // 3-Vector in World coords corresponding to a one-pixel move right the source image 77 | void RefreshPixelVectors(); // Calculates above two vectors 78 | 79 | // Info for the Mapmaker (not to be trashed by the tracker:) 80 | MapMakerData *pMMData; 81 | 82 | // Info for the Tracker (not to be trashed by the MapMaker:) 83 | TrackerData *pTData; 84 | 85 | // Info provided by the tracker for the mapmaker: 86 | int nMEstimatorOutlierCount; 87 | int nMEstimatorInlierCount; 88 | 89 | // Random junk (e.g. for visualisation) 90 | double dCreationTime; //timer.get_time() time of creation 91 | }; 92 | 93 | #endif 94 | -------------------------------------------------------------------------------- /ptam/license.txt: -------------------------------------------------------------------------------- 1 | LICENCE 2 | 3 | Parallel tracking and mapping for small augmented reality workspaces – PTAM.1 4 | PTAM.1 © Isis Innovation Limited 2008 (the "Software") 5 | 6 | The Software is and remains the property of Isis Innovation Limited ("Isis") a wholly-owned subsidiary of the University of Oxford (the “University”). The Licensee will ensure that the Copyright Notice set out above appears prominently wherever the Software is used. 7 | 8 | The Software is distributed under this Licence: 9 | 10 | on a non-exclusive basis, 11 | solely for non-commercial use in the hope that it will be useful, 12 | “AS-IS” and in order that the University as a charitable foundation protects its assets for the benefit of its educational and research purposes, Isis makes clear that no condition is made or to be implied, nor is any representation or warranty given or to be implied, as to (i) the quality, accuracy or reliability of the Software; (ii) the suitability of the Software for any particular use or for use under any specific conditions; and (iii) whether use of the Software will infringe third-party rights. 13 | 14 | Isis disclaims: 15 | 16 | all responsibility for the use which is made of the Software; and 17 | any liability for the outcomes arising from using the Software. 18 | 19 | The Licensee may make public, results or data obtained from, dependent on or arising out of the use of the Software provided that any such publication includes a prominent statement identifying the Software as the source of the results or the data, including the Copyright Notice and stating that the Software has been made available for use by the Licensee under licence from Isis and the Licensee provides a copy of any such publication to Isis. 20 | 21 | The Licensee agrees to indemnify Isis and the University and hold them harmless from and against any and all claims, damages and liabilities asserted by third parties (including claims for negligence) which arise directly or indirectly from the use of the Software or any derivative of it or the sale of any products based on the Software. The Licensee undertakes to make no liability claim against any employee, student, agent or appointee of Isis or of the University, in connection with this Licence or the Software. 22 | 23 | No part of the Software may be reproduced, modified, transmitted or transferred in any form or by any means, electronic or mechanical, without the express permission of Isis. Isis’s permission is not required if the said reproduction, modification, transmission or transference is done without financial return, the conditions of this Licence are imposed upon the receiver of the product, and all original and amended source code is included in any transmitted product. You may be held legally responsible for any copyright infringement that is caused or encouraged by your failure to abide by these terms and conditions. 24 | 25 | You are not permitted under this Licence to use this Software commercially. Use for which any financial return is received shall be defined as commercial use, and includes (1) integration of all or part of the source code or the Software into a product for sale or license by or on behalf of Licensee to third parties or (2) use of the Software or any derivative of it for research with the final aim of developing software products for sale or license to a third party or (3) use of the Software or any derivative of it for research with the final aim of developing non-software products for sale or license to a third party, or (4) use of the Software to provide any service to an external organisation for which payment is received. If you are interested in using the Software commercially, please contact Isis to negotiate a licence. Contact details are: innovation@isis.ox.ac.uk quoting reference 3588. 26 | -------------------------------------------------------------------------------- /rqt_ptam/include/rqt_ptam/remote_ptam.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2013, Markus Achtelik, ASL, ETH Zurich, Switzerland 4 | You can contact the author at 5 | 6 | * Copyright (c) 2011, Dirk Thomas, TU Darmstadt 7 | 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | * Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | * Redistributions in binary form must reproduce the above copyright 15 | notice, this list of conditions and the following disclaimer in the 16 | documentation and/or other materials provided with the distribution. 17 | * Neither the name of ETHZ-ASL nor the 18 | names of its contributors may be used to endorse or promote products 19 | derived from this software without specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 22 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 23 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 25 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 26 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 28 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 29 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 30 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | 32 | */ 33 | 34 | 35 | #ifndef rqt_image_view__ImageView_H 36 | #define rqt_image_view__ImageView_H 37 | 38 | #include 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #include 55 | 56 | namespace rqt_ptam { 57 | 58 | class RemotePTAM 59 | : public rqt_gui_cpp::Plugin 60 | { 61 | 62 | Q_OBJECT 63 | 64 | public: 65 | 66 | RemotePTAM(); 67 | 68 | virtual void initPlugin(qt_gui_cpp::PluginContext& context); 69 | 70 | virtual bool eventFilter(QObject* watched, QEvent* event); 71 | 72 | virtual void shutdownPlugin(); 73 | 74 | virtual void saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const; 75 | 76 | virtual void restoreSettings(const qt_gui_cpp::Settings& plugin_settings, const qt_gui_cpp::Settings& instance_settings); 77 | 78 | protected slots: 79 | 80 | virtual void updateNamespaceList(); 81 | 82 | protected: 83 | 84 | virtual QList getTopicList(const QSet& message_types, const QList& transports); 85 | 86 | virtual void selectTopic(const QString& topic); 87 | 88 | protected slots: 89 | 90 | virtual void onNamespaceChanged(int index); 91 | 92 | virtual void onZoom1(bool checked); 93 | 94 | virtual void onSubscribe(bool checked); 95 | 96 | virtual void onSpace(); 97 | 98 | virtual void onReset(); 99 | 100 | protected: 101 | 102 | virtual void callbackImage(const sensor_msgs::Image::ConstPtr& msg); 103 | 104 | Ui::RemotePTAMWidget ui_; 105 | 106 | QWidget* widget_; 107 | 108 | image_transport::Subscriber subscriber_; 109 | 110 | ros::Publisher cmd_pub_; 111 | 112 | QImage qimage_; 113 | QMutex qimage_mutex_; 114 | 115 | cv::Mat conversion_mat_; 116 | 117 | }; 118 | 119 | } 120 | 121 | #endif // rqt_image_view__ImageView_H 122 | -------------------------------------------------------------------------------- /ptam/include/ptam/System.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | // 4 | // System.h 5 | // 6 | // Defines the System class 7 | // 8 | // This stores the main functional classes of the system, like the 9 | // mapmaker, map, tracker etc, and spawns the working threads. 10 | // 11 | #ifndef __SYSTEM_H 12 | #define __SYSTEM_H 13 | //#include "VideoSource.h" 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include "GLWindow2.h" 29 | //#include "ptam/RosNode.h" 30 | #include "ptam/Params.h" 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | class ATANCamera; 41 | struct Map; 42 | class MapMaker; 43 | class Tracker; 44 | class MapViewer; 45 | 46 | class System 47 | { 48 | typedef std::queue ImuQueue; 49 | typedef std::queue PoseQueue; 50 | 51 | public: 52 | System(); 53 | void Run(); 54 | 55 | private: 56 | ros::NodeHandle nh_, image_nh_; 57 | ros::Subscriber sub_imu_; 58 | ros::Subscriber sub_calibration_; 59 | ros::Subscriber sub_kb_input_; 60 | tf::TransformBroadcaster tf_pub_; 61 | tf::TransformListener tf_sub_; 62 | image_transport::Subscriber sub_image_; 63 | image_transport::Publisher pub_preview_image_; 64 | ros::Publisher pub_pose_; // world in the camera frame 65 | ros::Publisher pub_pose_world_; // camera in the world frame 66 | ros::Publisher pub_info_; 67 | ros::ServiceServer srvPC_; 68 | ros::ServiceServer srvKF_; 69 | 70 | ros::CallbackQueue image_queue_; 71 | 72 | ImuQueue imu_msgs_; 73 | 74 | bool first_frame_; 75 | 76 | GLWindow2 *mGLWindow; 77 | MapViewer *mpMapViewer; 78 | 79 | CVD::Image img_bw_; 80 | CVD::Image > img_rgb_; 81 | 82 | Map *mpMap; 83 | MapMaker *mpMapMaker; 84 | Tracker *mpTracker; 85 | ATANCamera *mpCamera; 86 | 87 | // bool mbDone; 88 | 89 | void init(const CVD::ImageRef & size); 90 | 91 | void publishPoseAndInfo(const std_msgs::Header & header); 92 | void publishPreviewImage(CVD::Image & img, const std_msgs::Header & header); 93 | bool pointcloudservice(ptam_com::PointCloudRequest & req, ptam_com::PointCloudResponse & resp); 94 | bool keyframesservice(ptam_com::KeyFrame_srvRequest & req, ptam_com::KeyFrame_srvResponse & resp); 95 | 96 | void imageCallback(const sensor_msgs::ImageConstPtr & img); 97 | void imuCallback(const sensor_msgs::ImuConstPtr & msg); 98 | void keyboardCallback(const std_msgs::StringConstPtr & kb_input); 99 | 100 | bool transformQuaternion(const std::string & target_frame, const std_msgs::Header & header, const geometry_msgs::Quaternion & q_in, TooN::SO3 & r_out); 101 | bool transformPoint(const std::string & target_frame, const std_msgs::Header & header, const geometry_msgs::Point & t_in, TooN::Vector<3> & t_out); 102 | void quaternionToRotationMatrix(const geometry_msgs::Quaternion & q, TooN::SO3 & R); 103 | 104 | /// finds object in queue with timestamp closest to timestamp. Requires that T has a std_msgs::header field named "header" 105 | template bool findClosest(const ros::Time & timestamp, std::queue & queue, T * obj, const double & max_delay = 0.01); 106 | 107 | static void GUICommandCallBack(void* ptr, std::string sCommand, std::string sParams); 108 | 109 | }; 110 | 111 | 112 | 113 | #endif 114 | -------------------------------------------------------------------------------- /ptam/include/ptam/TrackerData.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | #ifndef __TRACKERDATA_H 4 | #define __TRACKERDATA_H 5 | 6 | #include "PatchFinder.h" 7 | #include "ATANCamera.h" 8 | 9 | // This class contains all the intermediate results associated with 10 | // a map-point that the tracker keeps up-to-date. TrackerData 11 | // basically handles all the tracker's point-projection jobs, 12 | // and also contains the PatchFinder which does the image search. 13 | // It's very code-heavy for an h-file (it's a bunch of methods really) 14 | // but it's only included from Tracker.cc! 15 | 16 | struct TrackerData 17 | { 18 | TrackerData(MapPoint::Ptr pMapPoint) 19 | : Point(pMapPoint) 20 | {}; 21 | 22 | MapPoint::Ptr Point; 23 | PatchFinder Finder; 24 | 25 | // Projection itermediates: 26 | Vector<3> v3Cam; // Coords in current cam frame 27 | Vector<2> v2ImPlane; // Coords in current cam z=1 plane 28 | Vector<2> v2Image; // Pixel coords in LEVEL0 29 | Matrix<2> m2CamDerivs; // Camera projection derivs 30 | bool bInImage; 31 | bool bPotentiallyVisible; 32 | 33 | int nSearchLevel; 34 | bool bSearched; 35 | bool bFound; 36 | bool bDidSubPix; 37 | Vector<2> v2Found; // Pixel coords of found patch (L0) 38 | double dSqrtInvNoise; // Only depends on search level.. 39 | 40 | 41 | // Stuff for pose update: 42 | Vector<2> v2Error_CovScaled; 43 | Matrix<2,6> m26Jacobian; // Jacobian wrt camera position 44 | 45 | // Project point into image given certain pose and camera. 46 | // This can bail out at several stages if the point 47 | // will not be properly in the image. 48 | inline void Project(const SE3<> &se3CFromW, ATANCamera &Cam) 49 | { 50 | bInImage = bPotentiallyVisible = false; 51 | v3Cam = se3CFromW * Point->v3WorldPos; 52 | if(v3Cam[2] < 0.001) 53 | return; 54 | v2ImPlane = project(v3Cam); 55 | if(v2ImPlane*v2ImPlane > Cam.LargestRadiusInImage() * Cam.LargestRadiusInImage()) 56 | return; 57 | v2Image = Cam.Project(v2ImPlane); 58 | if(Cam.Invalid()) 59 | return; 60 | 61 | if(v2Image[0] < 0 || v2Image[1] < 0 || v2Image[0] > irImageSize[0] || v2Image[1] > irImageSize[1]) 62 | return; 63 | bInImage = true; 64 | } 65 | 66 | // Get the projection derivatives (depend only on the camera.) 67 | // This is called Unsafe because it depends on the camera caching 68 | // results from the previous projection: 69 | // Only do this right after the same point has been projected! 70 | inline void GetDerivsUnsafe(ATANCamera &Cam) 71 | { 72 | m2CamDerivs = Cam.GetProjectionDerivs(); 73 | } 74 | 75 | // Does projection and gets camera derivs all in one. 76 | inline void ProjectAndDerivs(SE3<> &se3, ATANCamera &Cam) 77 | { 78 | Project(se3, Cam); 79 | if(bFound) 80 | GetDerivsUnsafe(Cam); 81 | } 82 | 83 | // Jacobian of projection W.R.T. the camera position 84 | // I.e. if p_cam = SE3Old * p_world, 85 | // SE3New = SE3Motion * SE3Old 86 | inline void CalcJacobian() 87 | { 88 | double dOneOverCameraZ = 1.0 / v3Cam[2]; 89 | for(int m=0; m<6; m++) 90 | { 91 | const Vector<4> v4Motion = SE3<>::generator_field(m, unproject(v3Cam)); 92 | Vector<2> v2CamFrameMotion; 93 | v2CamFrameMotion[0] = (v4Motion[0] - v3Cam[0] * v4Motion[2] * dOneOverCameraZ) * dOneOverCameraZ; 94 | v2CamFrameMotion[1] = (v4Motion[1] - v3Cam[1] * v4Motion[2] * dOneOverCameraZ) * dOneOverCameraZ; 95 | m26Jacobian.T()[m] = m2CamDerivs * v2CamFrameMotion; 96 | }; 97 | } 98 | 99 | // Sometimes in tracker instead of reprojecting, just update the error linearly! 100 | inline void LinearUpdate(const Vector<6> &v6) 101 | { 102 | v2Image += m26Jacobian * v6; 103 | } 104 | 105 | // This static member is filled in by the tracker and allows in-image checks in this class above. 106 | static CVD::ImageRef irImageSize; 107 | }; 108 | 109 | 110 | 111 | 112 | 113 | 114 | #endif 115 | 116 | 117 | 118 | 119 | -------------------------------------------------------------------------------- /rqt_ptam/src/rqt_ptam/ratio_layouted_frame.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011, Dirk Thomas, TU Darmstadt 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * * Redistributions in binary form must reproduce the above 12 | * copyright notice, this list of conditions and the following 13 | * disclaimer in the documentation and/or other materials provided 14 | * with the distribution. 15 | * * Neither the name of the TU Darmstadt nor the names of its 16 | * contributors may be used to endorse or promote products derived 17 | * from this software without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | */ 32 | 33 | #include 34 | 35 | #include 36 | 37 | namespace rqt_ptam { 38 | 39 | RatioLayoutedFrame::RatioLayoutedFrame(QWidget* parent, Qt::WFlags flags) 40 | : QFrame() 41 | , aspect_ratio_(4, 3) 42 | { 43 | } 44 | 45 | RatioLayoutedFrame::~RatioLayoutedFrame() 46 | { 47 | } 48 | 49 | void RatioLayoutedFrame::resizeToFitAspectRatio() 50 | { 51 | QRect rect = contentsRect(); 52 | 53 | // reduce longer edge to aspect ration 54 | double width = double(rect.width()); 55 | double height = double(rect.height()); 56 | if (width * aspect_ratio_.height() / height > aspect_ratio_.width()) 57 | { 58 | // too large width 59 | width = height * aspect_ratio_.width() / aspect_ratio_.height(); 60 | rect.setWidth(int(width)); 61 | } 62 | else 63 | { 64 | // too large height 65 | height = width * aspect_ratio_.height() / aspect_ratio_.width(); 66 | rect.setHeight(int(height)); 67 | } 68 | 69 | // resize taking the border line into account 70 | int border = lineWidth(); 71 | resize(rect.width() + 2 * border, rect.height() + 2 * border); 72 | } 73 | 74 | void RatioLayoutedFrame::setInnerFrameMinimumSize(const QSize& size) 75 | { 76 | int border = lineWidth(); 77 | QSize new_size = size; 78 | new_size += QSize(2 * border, 2 * border); 79 | setMinimumSize(new_size); 80 | update(); 81 | } 82 | 83 | void RatioLayoutedFrame::setInnerFrameMaximumSize(const QSize& size) 84 | { 85 | int border = lineWidth(); 86 | QSize new_size = size; 87 | new_size += QSize(2 * border, 2 * border); 88 | setMaximumSize(new_size); 89 | update(); 90 | } 91 | 92 | void RatioLayoutedFrame::setInnerFrameFixedSize(const QSize& size) 93 | { 94 | setInnerFrameMinimumSize(size); 95 | setInnerFrameMaximumSize(size); 96 | } 97 | 98 | void RatioLayoutedFrame::setAspectRatio(unsigned short width, unsigned short height) 99 | { 100 | int divisor = greatestCommonDivisor(width, height); 101 | if (divisor != 0) { 102 | aspect_ratio_.setWidth(width / divisor); 103 | aspect_ratio_.setHeight(height / divisor); 104 | } 105 | } 106 | 107 | int RatioLayoutedFrame::greatestCommonDivisor(int a, int b) 108 | { 109 | if (b==0) 110 | { 111 | return a; 112 | } 113 | return greatestCommonDivisor(b, a % b); 114 | } 115 | 116 | } 117 | -------------------------------------------------------------------------------- /ptam/src/OctomapInterface.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Octomapinterface.cc 3 | * 4 | * Created on: Dec 26, 2012 5 | * Author: slynen 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | OctoMapInterface::OctoMapInterface( ros::NodeHandle& nh):nh_(nh) 15 | { 16 | pub_scan_= nh_.advertise ("vslam/octomapscan", 10); 17 | pub_points_= nh_.advertise ("vslam/octomappoints", 10); 18 | 19 | kfseq_ = 0; 20 | pointseq_ = 0; 21 | } 22 | 23 | OctoMapInterface::~OctoMapInterface() 24 | { 25 | 26 | } 27 | 28 | void OctoMapInterface::addKeyFrame(KeyFrame::Ptr k) 29 | { 30 | 31 | ptam_com::OctoMapScanPtr msg(new ptam_com::OctoMapScan); 32 | 33 | //assemble the keyframe pose from the SE3 stored with the PTAM KeyFrame strcut 34 | TooN::SE3 pose; 35 | TooN::Matrix<3, 3, double> rot; 36 | TooN::Vector<3, double> trans; 37 | tf::Quaternion q; 38 | tf::Vector3 t; 39 | geometry_msgs::PoseWithCovarianceStamped& buffpose = msg->keyFramePose; 40 | 41 | double scale = 1.0; 42 | 43 | pose = k->se3CfromW; 44 | rot = pose.get_rotation().get_matrix(); 45 | trans = pose.get_translation(); 46 | tf::Transform transform(tf::Matrix3x3(rot(0, 0), rot(0, 1), rot(0, 2), 47 | rot(1, 0), rot(1, 1), rot(1, 2), 48 | rot(2, 0), rot(2, 1), rot(2, 2)), 49 | tf::Vector3(trans[0] / scale, trans[1]/ scale, trans[2] / scale)); 50 | q = transform.getRotation(); 51 | t = transform.getOrigin(); 52 | buffpose.header.seq=kfseq_++; 53 | buffpose.header.stamp=ros::Time::now(); 54 | buffpose.pose.pose.position.x=t[0]; 55 | buffpose.pose.pose.position.y=t[1]; 56 | buffpose.pose.pose.position.z=t[2]; 57 | buffpose.pose.pose.orientation.w=q.w(); 58 | buffpose.pose.pose.orientation.x=q.x(); 59 | buffpose.pose.pose.orientation.y=q.y(); 60 | buffpose.pose.pose.orientation.z=q.z(); 61 | memset(&(buffpose.pose.covariance[0]),0,sizeof(double)*6*6); 62 | 63 | //add all points that this KF measures 64 | msg->mapPoints.mapPoints.reserve(k->mMeasurements.size()); 65 | for(std::map, Measurement>::const_iterator it = k->mMeasurements.begin() ; it != k->mMeasurements.end() ; ++it){ 66 | ptam_com::OctoMapPointStamped pt; 67 | pt.header.seq = pointseq_++; 68 | pt.header.stamp = ros::Time::now(); 69 | pt.action = ptam_com::OctoMapPointStamped::INSERT; 70 | pt.position.x = it->first->v3WorldPos[0]; 71 | pt.position.y = it->first->v3WorldPos[1]; 72 | pt.position.z = it->first->v3WorldPos[2]; 73 | msg->mapPoints.mapPoints.push_back(pt); 74 | } 75 | 76 | pub_scan_.publish(msg); 77 | } 78 | 79 | void OctoMapInterface::updatePoint(MapPoint::Ptr p){ 80 | 81 | localUpdateQueue_.insert(p); //slow down the update rate a little, this function is called at approx 1KHz 82 | 83 | publishPointUpdateFromQueue(); //request publishing of local queue 84 | } 85 | 86 | void OctoMapInterface::updatePoints(std::set& updateSet){ 87 | 88 | localUpdateQueue_.insert(updateSet.begin(), updateSet.end()); 89 | 90 | publishPointUpdateFromQueue(); //request publishing of local queue 91 | 92 | } 93 | 94 | void OctoMapInterface::publishPointUpdateFromQueue(){ 95 | 96 | static double lastTime = 0; 97 | if(pub_points_.getNumSubscribers()){ 98 | if(ros::Time::now().toSec() - lastTime > 1.0){ //only perform this once a second 99 | ptam_com::OctoMapPointArrayPtr msg(new ptam_com::OctoMapPointArray); 100 | for(std::set::const_iterator it = localUpdateQueue_.begin(); it != localUpdateQueue_.end() ; ++it){ 101 | ptam_com::OctoMapPointStamped pt; 102 | pt.header.seq = pointseq_++; 103 | pt.header.stamp = ros::Time::now(); 104 | pt.action = ptam_com::OctoMapPointStamped::UPDATE; 105 | pt.position.x = (*it)->v3WorldPos[0]; 106 | pt.position.y = (*it)->v3WorldPos[1]; 107 | pt.position.z = (*it)->v3WorldPos[2]; 108 | msg->mapPoints.push_back(pt); 109 | } 110 | pub_points_.publish(msg); 111 | 112 | lastTime = ros::Time::now().toSec(); 113 | localUpdateQueue_.clear(); 114 | 115 | // std::cout<<"publishing now "<v3WorldPos[0]; 128 | pt.position.y = p->v3WorldPos[1]; 129 | pt.position.z = p->v3WorldPos[2]; 130 | msg->mapPoints.push_back(pt); 131 | pub_points_.publish(msg); 132 | } 133 | -------------------------------------------------------------------------------- /ptam/thirdparty/agast/src/nonMaximumSuppression.cc: -------------------------------------------------------------------------------- 1 | // 2 | // nonMaximumSuppression - this nonMaximumSuppression is a re-implementation 3 | // of the NMS as proposed by Rosten. However, this 4 | // implementation is complete and about 40% faster 5 | // than the OpenCV-version 6 | // 7 | // Copyright (C) 2010 Elmar Mair 8 | // 9 | // This program is free software: you can redistribute it and/or modify 10 | // it under the terms of the GNU General Public License as published by 11 | // the Free Software Foundation, either version 3 of the License, or 12 | // (at your option) any later version. 13 | // 14 | // This program is distributed in the hope that it will be useful, 15 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | // GNU General Public License for more details. 18 | // 19 | // You should have received a copy of the GNU General Public License 20 | // along with this program. If not, see . 21 | 22 | #include 23 | #include "cvWrapper.h" 24 | #include "AstDetector.h" 25 | 26 | using namespace std; 27 | using namespace agast; 28 | 29 | void AstDetector::nonMaximumSuppression(const std::vector& corners_all, 30 | std::vector& corners_nms) 31 | { 32 | int currCorner_ind; 33 | int lastRow=0, next_lastRow=0; 34 | vector::const_iterator currCorner; 35 | int lastRowCorner_ind=0, next_lastRowCorner_ind=0; 36 | vector::iterator nmsFlags_p; 37 | vector::iterator currCorner_nms; 38 | int j; 39 | int numCorners_all=corners_all.size(); 40 | int nMaxCorners=corners_nms.capacity(); 41 | 42 | currCorner=corners_all.begin(); 43 | 44 | if(numCorners_all > nMaxCorners) 45 | { 46 | if(nMaxCorners==0) 47 | { 48 | nMaxCorners=512 > numCorners_all ? 512 : numCorners_all; 49 | corners_nms.reserve(nMaxCorners); 50 | nmsFlags.reserve(nMaxCorners); 51 | } 52 | else 53 | { 54 | nMaxCorners *=2; 55 | if(numCorners_all > nMaxCorners) 56 | nMaxCorners = numCorners_all; 57 | corners_nms.reserve(nMaxCorners); 58 | nmsFlags.reserve(nMaxCorners); 59 | } 60 | } 61 | corners_nms.resize(numCorners_all); 62 | nmsFlags.resize(numCorners_all); 63 | 64 | nmsFlags_p=nmsFlags.begin(); 65 | currCorner_nms=corners_nms.begin(); 66 | 67 | //set all flags to MAXIMUM 68 | for(j=numCorners_all; j>0; j--) 69 | *nmsFlags_p++=-1; 70 | nmsFlags_p=nmsFlags.begin(); 71 | 72 | for(currCorner_ind=0; currCorner_indy) 78 | { 79 | lastRow=next_lastRow; 80 | lastRowCorner_ind=next_lastRowCorner_ind; 81 | } 82 | if(next_lastRow!=currCorner->y) 83 | { 84 | next_lastRow=currCorner->y; 85 | next_lastRowCorner_ind=currCorner_ind; 86 | } 87 | if(lastRow+1==currCorner->y) 88 | { 89 | //find the corner above the current one 90 | while((corners_all[lastRowCorner_ind].x < currCorner->x) && (corners_all[lastRowCorner_ind].y == lastRow)) 91 | lastRowCorner_ind++; 92 | 93 | if( (corners_all[lastRowCorner_ind].x == currCorner->x) && (lastRowCorner_ind!=currCorner_ind) ) 94 | { 95 | int t=lastRowCorner_ind; 96 | while(nmsFlags[t]!=-1) //find the maximum in this block 97 | t=nmsFlags[t]; 98 | 99 | if( scores[currCorner_ind] < scores[t] ) 100 | { 101 | nmsFlags[currCorner_ind]=t; 102 | } 103 | else 104 | nmsFlags[t]=currCorner_ind; 105 | } 106 | } 107 | 108 | //check left 109 | t=currCorner_ind-1; 110 | if( (currCorner_ind!=0) && (corners_all[t].y == currCorner->y) && (corners_all[t].x+1 == currCorner->x) ) 111 | { 112 | int currCornerMaxAbove_ind=nmsFlags[currCorner_ind]; 113 | 114 | while(nmsFlags[t]!=-1) //find the maximum in that area 115 | t=nmsFlags[t]; 116 | 117 | if(currCornerMaxAbove_ind==-1) //no maximum above 118 | { 119 | if(t!=currCorner_ind) 120 | { 121 | if( scores[currCorner_ind] < scores[t] ) 122 | nmsFlags[currCorner_ind]=t; 123 | else 124 | nmsFlags[t]=currCorner_ind; 125 | } 126 | } 127 | else //maximum above 128 | { 129 | if(t!=currCornerMaxAbove_ind) 130 | { 131 | if(scores[currCornerMaxAbove_ind] < scores[t]) 132 | { 133 | nmsFlags[currCornerMaxAbove_ind]=t; 134 | nmsFlags[currCorner_ind]=t; 135 | } 136 | else 137 | { 138 | nmsFlags[t]=currCornerMaxAbove_ind; 139 | nmsFlags[currCorner_ind]=currCornerMaxAbove_ind; 140 | } 141 | } 142 | } 143 | } 144 | 145 | currCorner++; 146 | } 147 | 148 | //collecting maximum corners 149 | corners_nms.resize(0); 150 | for(currCorner_ind=0; currCorner_ind 2 | 3 | RemotePTAMWidget 4 | 5 | 6 | true 7 | 8 | 9 | 10 | 0 11 | 0 12 | 502 13 | 300 14 | 15 | 16 | 17 | Remote PTAM 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | QComboBox::AdjustToContents 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | false 36 | 37 | 38 | true 39 | 40 | 41 | 42 | 43 | 44 | 45 | subscribe to ptam image 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | send "space" to ptam 56 | 57 | 58 | space 59 | 60 | 61 | 62 | 63 | 64 | 65 | resets the map 66 | 67 | 68 | reset 69 | 70 | 71 | 72 | 73 | 74 | 75 | Qt::Horizontal 76 | 77 | 78 | 79 | 40 80 | 20 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 0 91 | 92 | 93 | 94 | 95 | 96 | 0 97 | 0 98 | 99 | 100 | 101 | 102 | 80 103 | 60 104 | 105 | 106 | 107 | Qt::ClickFocus 108 | 109 | 110 | QFrame::Box 111 | 112 | 113 | 1 114 | 115 | 116 | 117 | 118 | 119 | 120 | Qt::Horizontal 121 | 122 | 123 | QSizePolicy::Expanding 124 | 125 | 126 | 127 | 0 128 | 20 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | Qt::Vertical 139 | 140 | 141 | 142 | 20 143 | 0 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | rqt_ptam::RatioLayoutedFrame 153 | QFrame 154 |
rqt_ptam/ratio_layouted_frame.h
155 | 1 156 | 157 | 158 | 159 | 160 | 161 | -------------------------------------------------------------------------------- /ptam/include/ptam/AxesArray.h: -------------------------------------------------------------------------------- 1 | /* 2 | * AxesArray.h 3 | * 4 | * Created on: Jul 21, 2011 5 | * Author: sweiss 6 | */ 7 | 8 | #ifndef AXESARRAY_H_ 9 | #define AXESARRAY_H_ 10 | 11 | #define AX_DIST 0.1 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | class AxesArray 22 | { 23 | private: 24 | visualization_msgs::MarkerArray cubes; 25 | unsigned int ID; 26 | TooN::Matrix<3,3> quaternion2matrix(const double q[4]); 27 | 28 | // temporary variables 29 | geometry_msgs::Point p; 30 | geometry_msgs::Point buffp; 31 | TooN::SO3 rot; 32 | TooN::Vector<3,double> center; 33 | TooN::Vector<3,double> buffvec; 34 | visualization_msgs::Marker buffcube; 35 | static const float red[4]; 36 | static const float green[4]; 37 | static const float blue[4]; 38 | static const double dirx[3]; 39 | static const double diry[3]; 40 | static const double dirz[3]; 41 | 42 | 43 | public: 44 | AxesArray(){ID=0;}; 45 | void init(double lifetime); // shall we add here some init arguments? 46 | bool addAxes(const double pos[3], const double att[4],unsigned int id); 47 | void clearAxes() {cubes.markers.clear();}; 48 | visualization_msgs::MarkerArray getAxes(){return cubes;}; 49 | TooN::Vector<3> getCenter(const double pos[3], const double att[4]); 50 | }; 51 | 52 | const float AxesArray::red[4]={1,0,0,1}; 53 | const float AxesArray::green[4]={0,1,0,1}; 54 | const float AxesArray::blue[4]={0,0,1,1}; 55 | const double AxesArray::dirx[3]={AX_DIST,0.01,0.01}; 56 | const double AxesArray::diry[3]={0.01,AX_DIST,0.01}; 57 | const double AxesArray::dirz[3]={0.01,0.01,AX_DIST}; 58 | 59 | void AxesArray::init(double lifetime) 60 | { 61 | clearAxes(); 62 | buffcube.lifetime=ros::Duration(lifetime); 63 | buffcube.header.frame_id = "/world"; 64 | buffcube.header.stamp = ros::Time::now(); 65 | buffcube.ns = "pointcloud_publisher"; 66 | buffcube.action = visualization_msgs::Marker::ADD; 67 | buffcube.type = visualization_msgs::Marker::CUBE; 68 | memcpy(&(buffcube.scale.x),dirx,sizeof(double)*3); 69 | memcpy(&(buffcube.color.r),red,sizeof(float)*4); 70 | } 71 | 72 | TooN::Vector<3> AxesArray::getCenter(const double pos[3], const double att[4]) 73 | { 74 | rot = quaternion2matrix(att); 75 | // return -(rot*TooN::makeVector(pos[0],pos[1],pos[2])); 76 | return -(rot.inverse()*TooN::makeVector(pos[0],pos[1],pos[2])); 77 | } 78 | 79 | bool AxesArray::addAxes(const double pos[3], const double att[4], unsigned int id) 80 | { 81 | center = getCenter(pos, att); 82 | 83 | // set cube attitude 84 | buffcube.pose.orientation.w=att[0]; 85 | buffcube.pose.orientation.x=-att[1]; 86 | buffcube.pose.orientation.y=-att[2]; 87 | buffcube.pose.orientation.z=-att[3]; 88 | // memcpy(&(buffcube.pose.orientation.x),&(att[1]),sizeof(double)*3); 89 | 90 | // add x-axis 91 | buffcube.id = 10*id; 92 | buffvec=center+rot.inverse()*TooN::makeVector(AX_DIST/2,0.0,0.0); 93 | memcpy(&(buffcube.pose.position.x),&(buffvec[0]),sizeof(double)*3); 94 | memcpy(&(buffcube.scale.x),dirx,sizeof(double)*3); 95 | memcpy(&(buffcube.color.r),red,sizeof(float)*4); 96 | cubes.markers.push_back(buffcube); 97 | 98 | // add y-axis, keep orientation 99 | buffcube.id = 10*id+1; 100 | buffvec=center+rot.inverse()*TooN::makeVector(0.0, AX_DIST/2,0.0); 101 | memcpy(&(buffcube.pose.position.x),&(buffvec[0]),sizeof(double)*3); 102 | memcpy(&(buffcube.scale.x),diry,sizeof(double)*3); 103 | memcpy(&(buffcube.color.r),green,sizeof(float)*4); 104 | cubes.markers.push_back(buffcube); 105 | 106 | // add z-axis, keep orientation 107 | buffcube.id = 10*id+2; 108 | buffvec=center+rot.inverse()*TooN::makeVector(0.0,0.0,AX_DIST/2); 109 | memcpy(&(buffcube.pose.position.x),&(buffvec[0]),sizeof(double)*3); 110 | memcpy(&(buffcube.scale.x),dirz,sizeof(double)*3); 111 | memcpy(&(buffcube.color.r),blue,sizeof(float)*4); 112 | cubes.markers.push_back(buffcube); 113 | 114 | return true; 115 | } 116 | 117 | TooN::Matrix<3,3> AxesArray::quaternion2matrix(const double q[4]) 118 | { 119 | // stolen from Eigen3 and adapted to TooN 120 | 121 | TooN::Matrix<3, 3, double> res; 122 | 123 | const double tx = 2 * q[1]; 124 | const double ty = 2 * q[2]; 125 | const double tz = 2 * q[3]; 126 | const double twx = tx * q[0]; 127 | const double twy = ty * q[0]; 128 | const double twz = tz * q[0]; 129 | const double txx = tx * q[1]; 130 | const double txy = ty * q[1]; 131 | const double txz = tz * q[1]; 132 | const double tyy = ty * q[2]; 133 | const double tyz = tz * q[2]; 134 | const double tzz = tz * q[3]; 135 | 136 | res(0, 0) = 1 - (tyy + tzz); 137 | res(0, 1) = txy - twz; 138 | res(0, 2) = txz + twy; 139 | res(1, 0) = txy + twz; 140 | res(1, 1) = 1 - (txx + tzz); 141 | res(1, 2) = tyz - twx; 142 | res(2, 0) = txz - twy; 143 | res(2, 1) = tyz + twx; 144 | res(2, 2) = 1 - (txx + tyy); 145 | 146 | return res; 147 | } 148 | 149 | 150 | #endif /// AXESARRAY_H_ 151 | -------------------------------------------------------------------------------- /ptam/include/ptam/KeyFrame.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | 4 | // 5 | // This header declares the data structures to do with keyframes: 6 | // structs KeyFrame, Level, Measurement, Candidate. 7 | // 8 | // A KeyFrame contains an image pyramid stored as array of Level; 9 | // A KeyFrame also has associated map-point mesurements stored as a vector of Measurment; 10 | // Each individual Level contains an image, corner points, and special corner points 11 | // which are promoted to Candidate status (the mapmaker tries to make new map points from those.) 12 | // 13 | // KeyFrames are stored in the Map class and manipulated by the MapMaker. 14 | // However, the tracker also stores its current frame as a half-populated 15 | // KeyFrame struct. 16 | 17 | 18 | #ifndef __KEYFRAME_H 19 | #define __KEYFRAME_H 20 | #include 21 | using namespace TooN; 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "ptam/Params.h" 31 | 32 | struct MapPoint; 33 | class SmallBlurryImage; 34 | //slynen{ reprojection 35 | struct TrackerData; 36 | //} 37 | #define LEVELS 4 38 | 39 | // Candidate: a feature in an image which could be made into a map point 40 | struct Candidate 41 | { 42 | CVD::ImageRef irLevelPos; 43 | Vector<2> v2RootPos; 44 | double dSTScore; 45 | }; 46 | 47 | // Measurement: A 2D image measurement of a map point. Each keyframe stores a bunch of these. 48 | struct Measurement 49 | { 50 | int nLevel; // Which image level? 51 | bool bSubPix; // Has this measurement been refined to sub-pixel level? 52 | Vector<2> v2RootPos; // Position of the measurement, REFERED TO PYRAMID LEVEL ZERO 53 | enum {SRC_TRACKER, SRC_REFIND, SRC_ROOT, SRC_TRAIL, SRC_EPIPOLAR} Source; // Where has this measurement come frome? 54 | }; 55 | 56 | // Each keyframe is made of LEVELS pyramid levels, stored in struct Level. 57 | // This contains image data and corner points. 58 | struct Level 59 | { 60 | inline Level() 61 | { 62 | bImplaneCornersCached = false; 63 | }; 64 | 65 | CVD::Image im; // The pyramid level pixels 66 | std::vector vCorners; // All FAST corners on this level 67 | std::vector vCornerRowLUT; // Row-index into the FAST corners, speeds up access 68 | std::vector vMaxCorners; // The maximal FAST corners 69 | Level& operator=(const Level &rhs); 70 | 71 | std::vector vCandidates; // Potential locations of new map points 72 | 73 | bool bImplaneCornersCached; // Also keep image-plane (z=1) positions of FAST corners to speed up epipolar search 74 | std::vector > vImplaneCorners; // Corner points un-projected into z=1-plane coordinates 75 | }; 76 | 77 | // The actual KeyFrame struct. The map contains of a bunch of these. However, the tracker uses this 78 | // struct as well: every incoming frame is turned into a keyframe before tracking; most of these 79 | // are then simply discarded, but sometimes they're then just added to the map. 80 | struct KeyFrame 81 | { 82 | typedef boost::shared_ptr Ptr; 83 | 84 | inline KeyFrame() 85 | { 86 | pSBI = NULL; 87 | } 88 | SE3<> se3CfromW; // The coordinate frame of this key-frame as a Camera-From-World transformation 89 | bool bFixed; // Is the coordinate frame of this keyframe fixed? (only true for first KF!) 90 | Level aLevels[LEVELS]; // Images, corners, etc lives in this array of pyramid levels 91 | std::map, Measurement> mMeasurements; // All the measurements associated with the keyframe 92 | 93 | //slynen pcl interface{ 94 | int bID; 95 | //} 96 | void MakeKeyFrame_Lite(CVD::BasicImage &im); // This takes an image and calculates pyramid levels etc to fill the 97 | // keyframe data structures with everything that's needed by the tracker.. 98 | void MakeKeyFrame_Rest(); // ... while this calculates the rest of the data which the mapmaker needs. 99 | 100 | //slynen{ reprojection 101 | std::vector > vpPoints; // stores the map points found in this keyframe 102 | enum { iBestPointsCount = 5 }; // how many points to use for keyframe evaluation 103 | boost::shared_ptr apCurrentBestPoints[iBestPointsCount]; // stores the currently best Points for Keyframe identification 104 | void AddKeyMapPoint(boost::shared_ptr mp); // checks whether to add a MapPoint to the KeyMapPoint of the KeyFrame 105 | // those points help selecting the Keyframes which are visible for reprojection 106 | //} 107 | 108 | double dSceneDepthMean; // Hacky hueristics to improve epipolar search. 109 | double dSceneDepthSigma; 110 | 111 | //Weiss{ 112 | double dSceneDepthMedian; // used to keep same scale after auto-re-init 113 | int ID; // KF id used to identify KFs when exporting them... 114 | //} 115 | 116 | SmallBlurryImage *pSBI; // The relocaliser uses this 117 | }; 118 | 119 | typedef std::map, Measurement>::iterator meas_it; // For convenience, and to work around an emacs paren-matching bug 120 | 121 | 122 | #endif 123 | 124 | -------------------------------------------------------------------------------- /ptam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ptam) 3 | 4 | add_custom_target( 5 | DEPS ALL 6 | COMMAND echo Building all dependencies 7 | COMMAND cd ${PROJECT_SOURCE_DIR}/thirdparty && make 8 | ) 9 | 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | dynamic_reconfigure 13 | std_msgs 14 | image_transport 15 | dynamic_reconfigure 16 | sensor_msgs 17 | ptam_com 18 | tf 19 | visualization_msgs 20 | cv_bridge 21 | ) 22 | 23 | find_package(OpenCV REQUIRED) 24 | 25 | generate_dynamic_reconfigure_options(cfg/PTAMVisualizerParams.cfg cfg/PtamParams.cfg) 26 | 27 | ##Needed for ros packages 28 | catkin_package(CATKIN_DEPENDS roscpp dynamic_reconfigure std_msgs image_transport dynamic_reconfigure sensor_msgs ptam_com tf visualization_msgs cv_bridge) 29 | 30 | set(PTAM_SOURCE_FILES 31 | src/GLWindow2.cc 32 | src/main.cc 33 | src/MapViewer.cc 34 | src/ATANCamera.cc 35 | src/GLWindowMenu.cc 36 | src/Map.cc 37 | src/MiniPatch.cc 38 | src/ShiTomasi.cc 39 | src/Tracker.cc 40 | src/Bundle.cc 41 | src/HomographyInit.cc 42 | src/MapMaker.cc 43 | src/PatchFinder.cc 44 | src/SmallBlurryImage.cc 45 | src/KeyFrame.cc 46 | src/MapPoint.cc 47 | src/Relocaliser.cc 48 | src/System.cc 49 | src/Params.cpp 50 | src/OctomapInterface.cc 51 | ) 52 | 53 | set(PTAM_HEADER_FILES 54 | include/ptam/CameraCalibrator.h 55 | include/ptam/KeyFrame.h 56 | include/ptam/MapViewer.h 57 | include/ptam/Relocaliser.h 58 | include/ptam/TrackerData.h 59 | include/ptam/ATANCamera.h 60 | include/ptam/LevelHelpers.h 61 | include/ptam/MEstimator.h 62 | include/ptam/ShiTomasi.h 63 | include/ptam/Tracker.h 64 | include/ptam/Bundle.h 65 | include/ptam/GLWindow2.h 66 | include/ptam/Map.h 67 | include/ptam/MiniPatch.h 68 | include/ptam/SmallBlurryImage.h 69 | include/ptam/CalibCornerPatch.h 70 | include/ptam/GLWindowMenu.h 71 | include/ptam/MapMaker.h 72 | include/ptam/OpenGL.h 73 | include/ptam/SmallMatrixOpts.h 74 | include/ptam/CalibImage.h 75 | include/ptam/HomographyInit.h 76 | include/ptam/MapPoint.h 77 | include/ptam/PatchFinder.h 78 | include/ptam/System.h 79 | include/ptam/Params.h 80 | ) 81 | 82 | SET(CALIBRATOR_FILES 83 | src/CameraCalibrator.cc 84 | src/GLWindowMenu.cc 85 | src/CalibCornerPatch.cc 86 | src/CalibImage.cc 87 | src/GLWindow2.cc 88 | src/ATANCamera.cc 89 | src/Params.cpp 90 | ) 91 | 92 | include_directories( 93 | include 94 | ${PROJECT_SOURCE_DIR}/thirdparty/TooN/include 95 | ${PROJECT_SOURCE_DIR}/thirdparty/libcvd/include 96 | ${PROJECT_SOURCE_DIR}/thirdparty/gvars3/include 97 | ${PROJECT_SOURCE_DIR}/thirdparty/agast/include 98 | ${catkin_INCLUDE_DIRS} 99 | ) 100 | 101 | link_directories( 102 | ${PROJECT_SOURCE_DIR}/thirdparty/libcvd/lib 103 | ${PROJECT_SOURCE_DIR}/thirdparty/gvars3/lib 104 | ${PROJECT_SOURCE_DIR}/thirdparty/agast/lib 105 | ) 106 | 107 | set(PTAM_LIBRARIES GL glut cvd GVars3_headless agast blas lapack) 108 | 109 | add_definitions(-DCVD_HAVE_TOON) 110 | add_definitions(-DKF_REPROJ) 111 | set(CMAKE_BUILD_TYPE Release) 112 | 113 | # PTAM executable 114 | add_executable(ptam ${PTAM_SOURCE_FILES} ${PTAM_HEADER_FILES}) 115 | set_property(TARGET ptam APPEND_STRING PROPERTY COMPILE_FLAGS "-D_LINUX -D_REENTRANT -Wall -march=native ") 116 | IF(CMAKE_BUILD_TYPE MATCHES Release) 117 | set_property(TARGET ptam APPEND_STRING PROPERTY COMPILE_FLAGS "-O3 ") 118 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fsee -funsafe-loop-optimizations -fno-signed-zeros -fno-math-errno -funroll-loops") 119 | ENDIF(CMAKE_BUILD_TYPE MATCHES Release) 120 | add_dependencies(ptam DEPS ${${PROJECT_NAME}_EXPORTED_TARGETS}}) 121 | 122 | target_link_libraries(ptam ${PTAM_LIBRARIES} ${OpenCV_LIBS} ${catkin_LIBRARIES}) 123 | 124 | 125 | # Camera calibrator 126 | add_executable(cameracalibrator ${CALIBRATOR_FILES}) 127 | add_dependencies(cameracalibrator DEPS ${${PROJECT_NAME}_EXPORTED_TARGETS}}) 128 | target_link_libraries(cameracalibrator ${PTAM_LIBRARIES} ${OpenCV_LIBS} ${catkin_LIBRARIES}) 129 | 130 | # PTAM remote control 131 | # this is unfortunately neccessary to close app, when opencvwindow gets closed 132 | include(FindPkgConfig) 133 | if (PKG_CONFIG_FOUND AND UNIX AND NOT APPLE) 134 | pkg_check_modules(GTK gtk+-2.0) 135 | if (GTK_FOUND) 136 | include_directories(${GTK_INCLUDE_DIRS}) 137 | link_directories(${GTK_LIBRARY_DIRS}) 138 | add_definitions(-DHAVE_GTK) 139 | # Hack around gdk-pixbuf move issues for now 140 | # http://ubuntuforums.org/showthread.php?t=1596930 141 | include_directories(/usr/include/gdk-pixbuf-2.0) 142 | endif(GTK_FOUND) 143 | endif(PKG_CONFIG_FOUND AND UNIX AND NOT APPLE) 144 | 145 | add_executable(remote_ptam src/RemotePtam.cpp) 146 | add_dependencies(remote_ptam DEPS ${${PROJECT_NAME}_EXPORTED_TARGETS}}) 147 | target_link_libraries(remote_ptam ${GTK_LIBRARIES} ${OpenCV_LIBS} ${catkin_LIBRARIES}) 148 | 149 | add_executable(ptam_visualizer src/PTAMVisualizer.cpp) 150 | add_dependencies(ptam_visualizer DEPS ${${PROJECT_NAME}_EXPORTED_TARGETS}}) 151 | target_link_libraries(ptam_visualizer ${catkin_LIBRARIES}) 152 | -------------------------------------------------------------------------------- /ptam/include/ptam/Bundle.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | #ifndef __BUNDLE_H 4 | #define __BUNDLE_H 5 | // Bundle.h 6 | // 7 | // This file declares the Bundle class along with a few helper classes. 8 | // Bundle is the bundle adjustment core of the mapping system; instances 9 | // of Bundle are generated by MapMaker to adjust the positions of 10 | // keyframes (called Cameras in this file) and map points. 11 | // 12 | // It's a pretty straight-forward Levenberg-Marquardt bundle adjustment 13 | // implementation closely following Hartley and Zisserman's MVG book, with 14 | // the addition of a robust M-Estimator. 15 | // 16 | // Unfortunately, having undergone a few tweaks, the code is now 17 | // not the easiest to read! 18 | // 19 | // Basic operation: MapMaker creates a new Bundle object; 20 | // then adds map points and keyframes to adjust; 21 | // then adds measurements of map points in keyframes; 22 | // then calls Compute() to do bundle adjustment; 23 | // then reads results back to update the map. 24 | 25 | #include "ATANCamera.h" 26 | #include 27 | using namespace TooN; 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include "ptam/Params.h" 34 | 35 | // An index into the big measurement map which stores all the measurements. 36 | 37 | // Camera struct holds the pose of a keyframe 38 | // and some computation intermediates 39 | struct Camera 40 | { 41 | bool bFixed; 42 | SE3<> se3CfW; 43 | SE3<> se3CfWNew; 44 | Matrix<6> m6U; // Accumulator 45 | Vector<6> v6EpsilonA; // Accumulator 46 | int nStartRow; 47 | }; 48 | 49 | // Camera-camera pair index 50 | struct OffDiagScriptEntry 51 | { 52 | int j; 53 | int k; 54 | }; 55 | 56 | // A map point, plus computation intermediates. 57 | struct Point 58 | { 59 | inline Point() 60 | { nMeasurements = 0; nOutliers = 0;} 61 | Vector<3> v3Pos; 62 | Vector<3> v3PosNew; 63 | Matrix<3> m3V; // Accumulator 64 | Vector<3> v3EpsilonB; // Accumulator 65 | Matrix<3> m3VStarInv; 66 | 67 | int nMeasurements; 68 | int nOutliers; 69 | std::set sCameras; // Which cameras observe this point? 70 | std::vector vOffDiagonalScript; // A record of all camera-camera pairs observing this point 71 | }; 72 | 73 | // A measurement of a point by a camera, plus 74 | // computation intermediates. 75 | struct Meas 76 | { 77 | inline Meas() 78 | {bBad = false;} 79 | 80 | // Which camera/point did this measurement come from? 81 | int p; // The point - called i in MVG 82 | int c; // The camera - called j in MVG 83 | 84 | inline bool operator<(const Meas &rhs) const 85 | { return(c v2Found; 90 | Vector<2> v2Epsilon; 91 | Matrix<2,6> m26A; 92 | Matrix<2,3> m23B; 93 | Matrix<6,3> m63W; 94 | Matrix<6,3> m63Y; 95 | double dSqrtInvNoise; 96 | 97 | // Temporary projection quantities 98 | Vector<3> v3Cam; 99 | double dErrorSquared; 100 | Matrix<2> m2CamDerivs; 101 | }; 102 | 103 | // Core bundle adjustment class 104 | class Bundle 105 | { 106 | public: 107 | 108 | Bundle(const ATANCamera &TCam); // We need the camera model because we do full distorting projection in the bundle adjuster. Could probably get away with a linear approximation. 109 | int AddCamera(SE3<> se3CamFromWorld, bool bFixed); // Add a viewpoint. bFixed signifies that this one is not to be adjusted. 110 | int AddPoint(Vector<3> v3Pos); // Add a map point. 111 | void AddMeas(int nCam, int nPoint, Vector<2> v2Pos, double dSigmaSquared); // Add a measurement 112 | int Compute(bool *pbAbortSignal); // Perform bundle adjustment. Aborts if *pbAbortSignal gets set to true. Returns number of accepted update iterations, or negative on error. 113 | inline bool Converged() { return mbConverged;} // Has bundle adjustment converged? 114 | Vector<3> GetPoint(int n); // Point coords after adjustment 115 | SE3<> GetCamera(int n); // Camera pose after adjustment 116 | std::vector > GetOutlierMeasurements(); // Measurements flagged as outliers 117 | std::set GetOutliers(); // Points flagged as outliers 118 | 119 | protected: 120 | 121 | inline void ProjectAndFindSquaredError(Meas &meas); // Project a single point in a single view, compare to measurement 122 | template bool Do_LM_Step(bool *pbAbortSignal); 123 | template double FindNewError(); 124 | void GenerateMeasLUTs(); 125 | void GenerateOffDiagScripts(); 126 | void ClearAccumulators(); // Zero temporary quantities stored in cameras and points 127 | void ModifyLambda_GoodStep(); 128 | void ModifyLambda_BadStep(); 129 | 130 | std::vector mvPoints; 131 | std::vector mvCameras; 132 | std::list mMeasList; 133 | std::vector > mvOutlierMeasurementIdx; // p-c pair 134 | std::vector > mvMeasLUTs; //Each camera gets a per-point table of pointers to valid measurements 135 | 136 | ATANCamera mCamera; 137 | int mnCamsToUpdate; 138 | int mnStartRow; 139 | double mdSigmaSquared; 140 | double mdLambda; 141 | double mdLambdaFactor; 142 | bool mbConverged; 143 | bool mbHitMaxIterations; 144 | int mnCounter; 145 | int mnAccepted; 146 | 147 | //Weiss{ 148 | //GVars3::gvar3 mgvnMaxIterations; 149 | //GVars3::gvar3 mgvdUpdateConvergenceLimit; 150 | //GVars3::gvar3 mgvnBundleCout; 151 | //} 152 | bool *mpbAbortSignal; 153 | }; 154 | 155 | 156 | 157 | 158 | 159 | #endif 160 | -------------------------------------------------------------------------------- /ptam/src/RemotePtam.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | Copyright (c) 2008, Willow Garage, Inc. 4 | 5 | Copyright (c) 2011, Markus Achtelik, ASL, ETH Zurich, Switzerland 6 | You can contact the author at 7 | 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | * Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | * Redistributions in binary form must reproduce the above copyright 15 | notice, this list of conditions and the following disclaimer in the 16 | documentation and/or other materials provided with the distribution. 17 | * Neither the name of ETHZ-ASL nor the 18 | names of its contributors may be used to endorse or promote products 19 | derived from this software without specific prior written permission. 20 | 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 22 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 23 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 24 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 25 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 26 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 28 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 29 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 30 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 | 32 | */ 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | #ifdef HAVE_GTK 46 | #include 47 | 48 | // Platform-specific workaround for #3026: image_view doesn't close when 49 | // closing image window. On platforms using GTK+ we connect this to the 50 | // window's "destroy" event so that image_view exits. 51 | static void destroyNode(GtkWidget *widget, gpointer data) 52 | { 53 | ros::shutdown(); 54 | } 55 | 56 | #endif 57 | 58 | class RemotePtam 59 | { 60 | private: 61 | image_transport::Subscriber *sub_; 62 | std::string window_name_; 63 | std::string transport_; 64 | std::string topic_; 65 | 66 | public: 67 | RemotePtam(const ros::NodeHandle& nh, const std::string& _transport) 68 | { 69 | topic_ = "vslam/preview"; 70 | ros::NodeHandle local_nh("~"); 71 | local_nh.param("window_name", window_name_, topic_); 72 | 73 | transport_ = _transport; 74 | 75 | bool autosize; 76 | local_nh.param("autosize", autosize, false); 77 | cv::namedWindow(window_name_, autosize ? 1 : 0); 78 | 79 | #ifdef HAVE_GTK 80 | // Register appropriate handler for when user closes the display window 81 | GtkWidget *widget = GTK_WIDGET( cvGetWindowHandle(window_name_.c_str()) ); 82 | g_signal_connect(widget, "destroy", G_CALLBACK(destroyNode), NULL); 83 | #endif 84 | 85 | sub_ = NULL; 86 | subscribe(nh); 87 | } 88 | 89 | ~RemotePtam() 90 | { 91 | unsubscribe(); 92 | cv::destroyWindow(window_name_); 93 | } 94 | 95 | void image_cb(const sensor_msgs::ImageConstPtr& msg) 96 | { 97 | cv_bridge::CvImageConstPtr cv_ptr; 98 | try 99 | { 100 | cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8); 101 | } 102 | catch (cv_bridge::Exception& e) 103 | { 104 | ROS_ERROR("cv_bridge exception: %s", e.what()); 105 | return; 106 | } 107 | 108 | cv::imshow(window_name_, cv_ptr->image); 109 | } 110 | void subscribe(const ros::NodeHandle& nh) 111 | { 112 | if (sub_ == NULL) 113 | { 114 | image_transport::ImageTransport it(nh); 115 | sub_ = new image_transport::Subscriber(it.subscribe(topic_, 1, &RemotePtam::image_cb, this, transport_)); 116 | } 117 | } 118 | 119 | void unsubscribe() 120 | { 121 | if (sub_ != NULL) 122 | { 123 | delete sub_; 124 | sub_ = NULL; 125 | } 126 | } 127 | }; 128 | 129 | int main(int argc, char **argv) 130 | { 131 | ros::init(argc, argv, "vslam_remote", ros::init_options::AnonymousName); 132 | ros::NodeHandle n; 133 | 134 | RemotePtam remote(n, (argc > 1) ? argv[1] : "raw"); 135 | 136 | char key = 0; 137 | 138 | remote.subscribe(n); 139 | bool subscribed = true; 140 | 141 | ros::Publisher key_pub = n.advertise ("vslam/key_pressed", 10); 142 | std_msgs::StringPtr msg(new std_msgs::String); 143 | 144 | while (ros::ok()) 145 | { 146 | key = cvWaitKey(10); 147 | 148 | if (key == ' ') 149 | { 150 | std::cout << "Sending \"Space\" to ptam" << std::endl; 151 | msg->data = "Space"; 152 | key_pub.publish(msg); 153 | } 154 | else if (key == 'r') 155 | { 156 | std::cout << "Sending \"r\" to ptam" << std::endl; 157 | msg->data = "r"; 158 | key_pub.publish(msg); 159 | } 160 | else if (key == 'a') 161 | { 162 | std::cout << "Sending \"a\" to ptam" << std::endl; 163 | msg->data = "a"; 164 | key_pub.publish(msg); 165 | } 166 | else if (key == 'q') 167 | { 168 | std::cout << "Sending \"q\" to ptam" << std::endl; 169 | msg->data = "q"; 170 | key_pub.publish(msg); 171 | } 172 | else if (key == 's') 173 | { 174 | if (subscribed) 175 | { 176 | remote.unsubscribe(); 177 | subscribed = false; 178 | std::cout << "unsubscribed" << std::endl; 179 | } 180 | else 181 | { 182 | remote.subscribe(n); 183 | subscribed = true; 184 | std::cout << "subscribed" << std::endl; 185 | } 186 | } 187 | 188 | ros::spinOnce(); 189 | } 190 | return 0; 191 | } 192 | -------------------------------------------------------------------------------- /ptam/src/MapViewer.cc: -------------------------------------------------------------------------------- 1 | #include "ptam/MapViewer.h" 2 | #include "ptam/MapPoint.h" 3 | #include "ptam/KeyFrame.h" 4 | #include "ptam/LevelHelpers.h" 5 | #include 6 | 7 | #include 8 | 9 | using namespace CVD; 10 | using namespace std; 11 | 12 | 13 | MapViewer::MapViewer(Map &map, GLWindow2 &glw): 14 | mMap(map), mGLWindow(glw) 15 | { 16 | mse3ViewerFromWorld = 17 | SE3<>::exp(makeVector(0,0,2,0,0,0)) * SE3<>::exp(makeVector(0,0,0,0.8 * M_PI,0,0)); 18 | } 19 | 20 | void MapViewer::DrawMapDots() 21 | { 22 | SetupFrustum(); 23 | SetupModelView(); 24 | 25 | int nForMass = 0; 26 | glColor3f(0,1,1); 27 | glPointSize(3); 28 | glBegin(GL_POINTS); 29 | mv3MassCenter = Zeros; 30 | for(size_t i=0; i v3Pos = mMap.vpPoints[i]->v3WorldPos; 33 | glColor(gavLevelColors[mMap.vpPoints[i]->nSourceLevel]); 34 | if(v3Pos * v3Pos < 10000) 35 | { 36 | nForMass++; 37 | mv3MassCenter += v3Pos; 38 | } 39 | glVertex(v3Pos); 40 | } 41 | glEnd(); 42 | mv3MassCenter = mv3MassCenter / (0.1 + nForMass); 43 | } 44 | 45 | 46 | void MapViewer::DrawGrid() 47 | { 48 | SetupFrustum(); 49 | SetupModelView(); 50 | glLineWidth(1); 51 | 52 | glBegin(GL_LINES); 53 | 54 | // Draw a larger grid around the outside.. 55 | double dGridInterval = 0.1; 56 | 57 | double dMin = -100.0 * dGridInterval; 58 | double dMax = 100.0 * dGridInterval; 59 | 60 | for(int x=-10;x<=10;x+=1) 61 | { 62 | if(x==0) 63 | glColor3f(1,1,1); 64 | else 65 | glColor3f(0.3,0.3,0.3); 66 | glVertex3d((double)x * 10 * dGridInterval, dMin, 0.0); 67 | glVertex3d((double)x * 10 * dGridInterval, dMax, 0.0); 68 | } 69 | for(int y=-10;y<=10;y+=1) 70 | { 71 | if(y==0) 72 | glColor3f(1,1,1); 73 | else 74 | glColor3f(0.3,0.3,0.3); 75 | glVertex3d(dMin, (double)y * 10 * dGridInterval, 0.0); 76 | glVertex3d(dMax, (double)y * 10 * dGridInterval, 0.0); 77 | } 78 | 79 | glEnd(); 80 | 81 | glBegin(GL_LINES); 82 | dMin = -10.0 * dGridInterval; 83 | dMax = 10.0 * dGridInterval; 84 | 85 | for(int x=-10;x<=10;x++) 86 | { 87 | if(x==0) 88 | glColor3f(1,1,1); 89 | else 90 | glColor3f(0.5,0.5,0.5); 91 | 92 | glVertex3d((double)x * dGridInterval, dMin, 0.0); 93 | glVertex3d((double)x * dGridInterval, dMax, 0.0); 94 | } 95 | for(int y=-10;y<=10;y++) 96 | { 97 | if(y==0) 98 | glColor3f(1,1,1); 99 | else 100 | glColor3f(0.5,0.5,0.5); 101 | glVertex3d(dMin, (double)y * dGridInterval, 0.0); 102 | glVertex3d(dMax, (double)y * dGridInterval, 0.0); 103 | } 104 | 105 | glColor3f(1,0,0); 106 | glVertex3d(0,0,0); 107 | glVertex3d(1,0,0); 108 | glColor3f(0,1,0); 109 | glVertex3d(0,0,0); 110 | glVertex3d(0,1,0); 111 | glColor3f(1,1,1); 112 | glVertex3d(0,0,0); 113 | glVertex3d(0,0,1); 114 | glEnd(); 115 | 116 | // glColor3f(0.8,0.8,0.8); 117 | // glRasterPos3f(1.1,0,0); 118 | // mGLWindow.PrintString("x"); 119 | // glRasterPos3f(0,1.1,0); 120 | // mGLWindow.PrintString("y"); 121 | // glRasterPos3f(0,0,1.1); 122 | // mGLWindow.PrintString("z"); 123 | } 124 | 125 | void MapViewer::DrawMap(SE3<> se3CamFromWorld) 126 | { 127 | mMessageForUser.str(""); // Wipe the user message clean 128 | 129 | // Update viewer position according to mouse input: 130 | { 131 | pair, Vector<6> > pv6 = mGLWindow.GetMousePoseUpdate(); 132 | SE3<> se3CamFromMC; 133 | se3CamFromMC.get_translation() = mse3ViewerFromWorld * mv3MassCenter; 134 | mse3ViewerFromWorld = SE3<>::exp(pv6.first) * 135 | se3CamFromMC * SE3<>::exp(pv6.second) * se3CamFromMC.inverse() * mse3ViewerFromWorld; 136 | } 137 | 138 | mGLWindow.SetupViewport(); 139 | glClearColor(0,0,0,0); 140 | glClearDepth(1); 141 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 142 | glMatrixMode(GL_PROJECTION); 143 | glLoadIdentity(); 144 | glEnable(GL_POINT_SMOOTH); 145 | glEnable(GL_LINE_SMOOTH); 146 | glEnable(GL_BLEND); 147 | glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); 148 | glColorMask(1,1,1,1); 149 | 150 | glEnable(GL_DEPTH_TEST); 151 | DrawGrid(); 152 | DrawMapDots(); 153 | DrawCamera(se3CamFromWorld); 154 | for(size_t i=0; ise3CfromW, true); 156 | glDisable(GL_DEPTH_TEST); 157 | glMatrixMode(GL_MODELVIEW); 158 | glLoadIdentity(); 159 | 160 | mMessageForUser << " Map: " << mMap.vpPoints.size() << "P, " << mMap.vpKeyFrames.size() << "KF"; 161 | mMessageForUser << setprecision(4); 162 | mMessageForUser << " Camera Pos: " << se3CamFromWorld.inverse().get_translation(); 163 | } 164 | 165 | string MapViewer::GetMessageForUser() 166 | { 167 | return mMessageForUser.str(); 168 | } 169 | 170 | void MapViewer::SetupFrustum() 171 | { 172 | glMatrixMode(GL_PROJECTION); 173 | glLoadIdentity(); 174 | double zNear = 0.03; 175 | glFrustum(-zNear, zNear, 0.75*zNear,-0.75*zNear,zNear,50); 176 | glScalef(1,1,-1); 177 | return; 178 | }; 179 | 180 | void MapViewer::SetupModelView(SE3<> se3WorldFromCurrent) 181 | { 182 | glMatrixMode(GL_MODELVIEW); 183 | glLoadIdentity(); 184 | glMultMatrix(mse3ViewerFromWorld * se3WorldFromCurrent); 185 | return; 186 | }; 187 | 188 | void MapViewer::DrawCamera(SE3<> se3CfromW, bool bSmall) 189 | { 190 | 191 | SetupModelView(se3CfromW.inverse()); 192 | SetupFrustum(); 193 | 194 | if(bSmall) 195 | glLineWidth(1); 196 | else 197 | glLineWidth(3); 198 | 199 | glBegin(GL_LINES); 200 | glColor3f(1,0,0); 201 | glVertex3f(0.0f, 0.0f, 0.0f); 202 | glVertex3f(0.1f, 0.0f, 0.0f); 203 | glColor3f(0,1,0); 204 | glVertex3f(0.0f, 0.0f, 0.0f); 205 | glVertex3f(0.0f, 0.1f, 0.0f); 206 | glColor3f(1,1,1); 207 | glVertex3f(0.0f, 0.0f, 0.0f); 208 | glVertex3f(0.0f, 0.0f, 0.1f); 209 | glEnd(); 210 | 211 | 212 | if(!bSmall) 213 | { 214 | glLineWidth(1); 215 | glColor3f(0.5,0.5,0.5); 216 | SetupModelView(); 217 | Vector<2> v2CamPosXY = se3CfromW.inverse().get_translation().slice<0,2>(); 218 | glBegin(GL_LINES); 219 | glColor3f(1,1,1); 220 | glVertex2d(v2CamPosXY[0] - 0.04, v2CamPosXY[1] + 0.04); 221 | glVertex2d(v2CamPosXY[0] + 0.04, v2CamPosXY[1] - 0.04); 222 | glVertex2d(v2CamPosXY[0] - 0.04, v2CamPosXY[1] - 0.04); 223 | glVertex2d(v2CamPosXY[0] + 0.04, v2CamPosXY[1] + 0.04); 224 | glEnd(); 225 | } 226 | 227 | } 228 | 229 | 230 | -------------------------------------------------------------------------------- /ptam/include/ptam/MapMaker.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | 4 | // 5 | // This header declares the MapMaker class 6 | // MapMaker makes and maintains the Map struct 7 | // Starting with stereo initialisation from a bunch of matches 8 | // over keyframe insertion, continual bundle adjustment and 9 | // data-association refinement. 10 | // MapMaker runs in its own thread, although some functions 11 | // (notably stereo init) are called by the tracker and run in the 12 | // tracker's thread. 13 | 14 | #ifndef __MAPMAKER_H 15 | #define __MAPMAKER_H 16 | #include 17 | #include 18 | #include 19 | 20 | #include "Map.h" 21 | #include "KeyFrame.h" 22 | #include "ATANCamera.h" 23 | #include "SmallBlurryImage.h" 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | 30 | // Each MapPoint has an associated MapMakerData class 31 | // Where the mapmaker can store extra information 32 | 33 | struct MapMakerData 34 | { 35 | std::set sMeasurementKFs; // Which keyframes has this map point got measurements in? 36 | std::set sNeverRetryKFs; // Which keyframes have measurements failed enough so I should never retry? 37 | inline int GoodMeasCount() 38 | { return sMeasurementKFs.size(); } 39 | }; 40 | 41 | // MapMaker dervives from CVD::Thread, so everything in void run() is its own thread. 42 | class MapMaker : protected CVD::Thread 43 | { 44 | public: 45 | MapMaker(Map &m, const ATANCamera &cam, ros::NodeHandle& nh); 46 | ~MapMaker(); 47 | 48 | // Make a map from scratch. Called by the tracker. 49 | bool InitFromStereo(KeyFrame::Ptr kFirst, KeyFrame::Ptr kSecond, 50 | std::vector > &vMatches, 51 | SE3<> &se3CameraPos); 52 | 53 | bool InitFromStereo_OLD(KeyFrame::Ptr kFirst, KeyFrame::Ptr kSecond, // EXPERIMENTAL HACK 54 | std::vector > &vMatches, 55 | SE3<> &se3CameraPos); 56 | 57 | 58 | void AddKeyFrame(KeyFrame::Ptr k); // Add a key-frame to the map. Called by the tracker. 59 | void RequestReset(); // Request that the we reset. Called by the tracker. 60 | bool ResetDone(); // Returns true if the has been done. 61 | int QueueSize() { return mvpKeyFrameQueue.size() ;} // How many KFs in the queue waiting to be added? 62 | bool NeedNewKeyFrame(KeyFrame::Ptr kCurrent); // Is it a good camera pose to add another KeyFrame? 63 | bool IsDistanceToNearestKeyFrameExcessive(KeyFrame::Ptr kCurrent); // Is the camera far away from the nearest KeyFrame (i.e. maybe lost?) 64 | 65 | std::string getMessageForUser(){return mMessageForUser.str();}; 66 | void resetMessageForUser(){mMessageForUser.str(""); }; 67 | 68 | //Weiss{ moved from protected 69 | void ApplyGlobalScaleToMap(double dScale); 70 | void ApplyGlobalTransformationToMap(SE3<> se3NewFromOld); 71 | KeyFrame::Ptr ClosestKeyFrame(KeyFrame::Ptr k); 72 | std::vector NClosestKeyFrames(KeyFrame::Ptr k, unsigned int N); 73 | //} 74 | 75 | protected: 76 | 77 | Map &mMap; // The map 78 | ATANCamera mCamera; // Same as the tracker's camera: N.B. not a reference variable! 79 | virtual void run(); // The MapMaker thread code lives here 80 | 81 | //slynen octomap_interface{ 82 | OctoMapInterface octomap_interface; 83 | //} 84 | 85 | // Functions for starting the map from scratch: 86 | SE3<> CalcPlaneAligner(); 87 | 88 | // Map expansion functions: 89 | void AddKeyFrameFromTopOfQueue(); 90 | void ThinCandidates(KeyFrame::Ptr k, int nLevel); 91 | bool AddSomeMapPoints(int nLevel); 92 | bool AddPointEpipolar(KeyFrame::Ptr kSrc, KeyFrame::Ptr kTarget, int nLevel, int nCandidate); 93 | // Returns point in ref frame B 94 | Vector<3> ReprojectPoint(SE3<> se3AfromB, const Vector<2> &v2A, const Vector<2> &v2B); 95 | 96 | // Bundle adjustment functions: 97 | void BundleAdjust(std::set, std::set, std::set >, bool); 98 | void BundleAdjustAll(); 99 | void BundleAdjustRecent(); 100 | 101 | // Data association functions: 102 | int ReFindInSingleKeyFrame(KeyFrame::Ptr k); 103 | void ReFindFromFailureQueue(); 104 | void ReFindNewlyMade(); 105 | void ReFindAll(); 106 | bool ReFind_Common(KeyFrame::Ptr k, boost::shared_ptr p); 107 | void SubPixelRefineMatches(KeyFrame::Ptr k, int nLevel); 108 | 109 | // General Maintenance/Utility: 110 | void Reset(); 111 | void HandleBadPoints(); 112 | double DistToNearestKeyFrame(KeyFrame::Ptr kCurrent); 113 | double KeyFrameLinearDist(KeyFrame::Ptr k1, KeyFrame::Ptr k2); 114 | bool RefreshSceneDepth(KeyFrame::Ptr pKF); 115 | 116 | 117 | // GUI Interface: 118 | void GUICommandHandler(std::string sCommand, std::string sParams); 119 | static void GUICommandCallBack(void* ptr, std::string sCommand, std::string sParams); 120 | struct Command {std::string sCommand; std::string sParams; }; 121 | std::vector mvQueuedCommands; 122 | 123 | 124 | // Member variables: 125 | std::vector mvpKeyFrameQueue; // Queue of keyframes from the tracker waiting to be processed 126 | std::vector > > mvFailureQueue; // Queue of failed observations to re-find 127 | std::queue > mqNewQueue; // Queue of newly-made map points to re-find in other KeyFrames 128 | 129 | double mdWiggleScale; // Metric distance between the first two KeyFrames (copied from GVar) 130 | // This sets the scale of the map 131 | //GVars3::gvar3 mgvdWiggleScale; // GVar for above 132 | double mdWiggleScaleDepthNormalized; // The above normalized against scene depth, 133 | // this controls keyframe separation 134 | 135 | bool mbBundleConverged_Full; // Has global bundle adjustment converged? 136 | bool mbBundleConverged_Recent; // Has local bundle adjustment converged? 137 | 138 | // Thread interaction signalling stuff 139 | bool mbResetRequested; // A reset has been requested 140 | bool mbResetDone; // The reset was done. 141 | bool mbBundleAbortRequested; // We should stop bundle adjustment 142 | bool mbBundleRunning; // Bundle adjustment is running 143 | bool mbBundleRunningIsRecent; // ... and it's a local bundle adjustment. 144 | 145 | 146 | std::ostringstream mMessageForUser; 147 | }; 148 | 149 | #endif 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | -------------------------------------------------------------------------------- /ptam/include/ptam/MEstimator.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | 4 | // MEstimator.h 5 | // 6 | // Defines various MEstimators which can be used by the Tracker and 7 | // the Bundle adjuster. Not that some of the inputs are square 8 | // quantities! 9 | 10 | #ifndef __MESTIMATOR_H 11 | #define __MESTIMATOR_H 12 | #include 13 | using namespace TooN; 14 | #include 15 | #include 16 | #include 17 | 18 | struct Tukey 19 | { 20 | inline static double FindSigmaSquared(std::vector &vdErrorSquared); 21 | inline static double SquareRootWeight(double dErrorSquared, double dSigmaSquared); 22 | inline static double Weight(double dErrorSquared, double dSigmaSquared); 23 | inline static double ObjectiveScore(double dErrorSquared, double dSigmaSquared); 24 | }; 25 | 26 | struct Cauchy 27 | { 28 | inline static double FindSigmaSquared(std::vector &vdErrorSquared); 29 | inline static double SquareRootWeight(double dErrorSquared, double dSigmaSquared); 30 | inline static double Weight(double dErrorSquared, double dSigmaSquared); 31 | inline static double ObjectiveScore(double dErrorSquared, double dSigmaSquared); 32 | }; 33 | 34 | struct Huber 35 | { 36 | inline static double FindSigmaSquared(std::vector &vdErrorSquared); 37 | inline static double SquareRootWeight(double dErrorSquared, double dSigmaSquared); 38 | inline static double Weight(double dErrorSquared, double dSigmaSquared); 39 | inline static double ObjectiveScore(double dErrorSquared, double dSigmaSquared); 40 | }; 41 | 42 | struct LeastSquares 43 | { 44 | inline static double FindSigmaSquared(std::vector &vdErrorSquared); 45 | inline static double SquareRootWeight(double dErrorSquared, double dSigmaSquared); 46 | inline static double Weight(double dErrorSquared, double dSigmaSquared); 47 | inline static double ObjectiveScore(double dErrorSquared, double dSigmaSquared); 48 | }; 49 | 50 | 51 | inline double Tukey::Weight(double dErrorSquared, double dSigmaSquared) 52 | { 53 | double dSqrt = SquareRootWeight(dErrorSquared, dSigmaSquared); 54 | return dSqrt * dSqrt; 55 | } 56 | 57 | inline double Tukey::SquareRootWeight(double dErrorSquared, double dSigmaSquared) 58 | { 59 | if(dErrorSquared > dSigmaSquared) 60 | return 0.0; 61 | else 62 | return 1.0 - (dErrorSquared / dSigmaSquared); 63 | } 64 | 65 | inline double Tukey::ObjectiveScore(double dErrorSquared, const double dSigmaSquared) 66 | { 67 | // NB All returned are scaled because 68 | // I'm not multiplying by sigmasquared/6.0 69 | if(dErrorSquared > dSigmaSquared) 70 | return 1.0; 71 | double d = 1.0 - dErrorSquared / dSigmaSquared; 72 | return (1.0 - d*d*d); 73 | } 74 | 75 | 76 | inline double Tukey::FindSigmaSquared(std::vector &vdErrorSquared) 77 | { 78 | double dSigmaSquared; 79 | assert(vdErrorSquared.size() > 0); 80 | std::sort(vdErrorSquared.begin(), vdErrorSquared.end()); 81 | double dMedianSquared = vdErrorSquared[vdErrorSquared.size() / 2]; 82 | double dSigma = 1.4826 * (1 + 5.0 / (vdErrorSquared.size() * 2 - 6)) * sqrt(dMedianSquared); 83 | dSigma = 4.6851 * dSigma; 84 | dSigmaSquared = dSigma * dSigma; 85 | return dSigmaSquared; 86 | } 87 | 88 | 89 | /////////////////////////////////////// 90 | /////////////////////////////////////// 91 | /////////////////////////////////////// 92 | /////////////////////////////////////// 93 | 94 | inline double Cauchy::Weight(double dErrorSquared, double dSigmaSquared) 95 | { 96 | return 1.0 / (1.0 + dErrorSquared / dSigmaSquared); 97 | } 98 | 99 | inline double Cauchy::SquareRootWeight(double dErrorSquared, double dSigmaSquared) 100 | { 101 | return sqrt(Weight(dErrorSquared, dSigmaSquared)); 102 | } 103 | 104 | inline double Cauchy::ObjectiveScore(double dErrorSquared, const double dSigmaSquared) 105 | { 106 | return log(1.0 + dErrorSquared / dSigmaSquared); 107 | } 108 | 109 | 110 | inline double Cauchy::FindSigmaSquared(std::vector &vdErrorSquared) 111 | { 112 | double dSigmaSquared; 113 | assert(vdErrorSquared.size() > 0); 114 | std::sort(vdErrorSquared.begin(), vdErrorSquared.end()); 115 | double dMedianSquared = vdErrorSquared[vdErrorSquared.size() / 2]; 116 | double dSigma = 1.4826 * (1 + 5.0 / (vdErrorSquared.size() * 2 - 6)) * sqrt(dMedianSquared); 117 | dSigma = 4.6851 * dSigma; 118 | dSigmaSquared = dSigma * dSigma; 119 | return dSigmaSquared; 120 | } 121 | 122 | 123 | /////////////////////////////////////// 124 | /////////////////////////////////////// 125 | /////////////////////////////////////// 126 | /////////////////////////////////////// 127 | 128 | inline double Huber::Weight(double dErrorSquared, double dSigmaSquared) 129 | { 130 | if(dErrorSquared < dSigmaSquared) 131 | return 1; 132 | else 133 | return sqrt(dSigmaSquared / dErrorSquared); 134 | } 135 | 136 | inline double Huber::SquareRootWeight(double dErrorSquared, double dSigmaSquared) 137 | { 138 | return sqrt(Weight(dErrorSquared, dSigmaSquared)); 139 | } 140 | 141 | inline double Huber::ObjectiveScore(double dErrorSquared, const double dSigmaSquared) 142 | { 143 | if(dErrorSquared< dSigmaSquared) 144 | return 0.5 * dErrorSquared; 145 | else 146 | { 147 | double dSigma = sqrt(dSigmaSquared); 148 | double dError = sqrt(dErrorSquared); 149 | return dSigma * ( dError - 0.5 * dSigma); 150 | } 151 | } 152 | 153 | 154 | inline double Huber::FindSigmaSquared(std::vector &vdErrorSquared) 155 | { 156 | double dSigmaSquared; 157 | assert(vdErrorSquared.size() > 0); 158 | std::sort(vdErrorSquared.begin(), vdErrorSquared.end()); 159 | double dMedianSquared = vdErrorSquared[vdErrorSquared.size() / 2]; 160 | double dSigma = 1.4826 * (1 + 5.0 / (vdErrorSquared.size() * 2 - 6)) * sqrt(dMedianSquared); 161 | dSigma = 1.345 * dSigma; 162 | dSigmaSquared = dSigma * dSigma; 163 | return dSigmaSquared; 164 | } 165 | 166 | /////////////////////////////////////// 167 | /////////////////////////////////////// 168 | /////////////////////////////////////// 169 | /////////////////////////////////////// 170 | 171 | inline double LeastSquares::Weight(double dErrorSquared, double dSigmaSquared) 172 | { 173 | return 1.0; 174 | } 175 | 176 | inline double LeastSquares::SquareRootWeight(double dErrorSquared, double dSigmaSquared) 177 | { 178 | return 1.0; 179 | } 180 | 181 | inline double LeastSquares::ObjectiveScore(double dErrorSquared, const double dSigmaSquared) 182 | { 183 | return dErrorSquared; 184 | } 185 | 186 | 187 | inline double LeastSquares::FindSigmaSquared(std::vector &vdErrorSquared) 188 | { 189 | if(vdErrorSquared.size() == 0) 190 | return 0.0; 191 | double dSum = 0.0; 192 | for(unsigned int i=0; i 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "ptam/SmallMatrixOpts.h" 12 | 13 | using namespace std; 14 | using namespace CVD; 15 | 16 | Image CalibCornerPatch::mimSharedSourceTemplate; 17 | 18 | CalibCornerPatch::CalibCornerPatch(int nSideSize) 19 | { 20 | mimTemplate.resize(ImageRef(nSideSize, nSideSize)); 21 | mimGradients.resize(ImageRef(nSideSize, nSideSize)); 22 | mimAngleJacs.resize(ImageRef(nSideSize, nSideSize)); 23 | if(mimSharedSourceTemplate.size().x == 0) 24 | { 25 | MakeSharedTemplate(); 26 | } 27 | } 28 | 29 | void CalibCornerPatch::MakeTemplateWithCurrentParams() 30 | { 31 | double dBlurSigma = 2.0; 32 | int nExtraPixels = (int) (dBlurSigma * 6.0) + 2; 33 | 34 | Image imToBlur(mimTemplate.size() + ImageRef(nExtraPixels, nExtraPixels)); 35 | Image imTwiceToBlur(imToBlur.size() * 2); 36 | 37 | // Make actual template: 38 | int nOffset; 39 | { 40 | Matrix<2> m2Warp = mParams.m2Warp(); 41 | CVD::transform(mimSharedSourceTemplate, imTwiceToBlur, 42 | M2Inverse(m2Warp), 43 | vec(mimSharedSourceTemplate.size() - ImageRef(1,1)) * 0.5, 44 | vec(imTwiceToBlur.size() - ImageRef(1,1)) * 0.5); 45 | halfSample(imTwiceToBlur, imToBlur); 46 | convolveGaussian(imToBlur, dBlurSigma); 47 | 48 | nOffset = (imToBlur.size().x - mimTemplate.size().x) / 2; 49 | copy(imToBlur, mimTemplate, mimTemplate.size(), ImageRef(nOffset, nOffset)); 50 | }; 51 | 52 | // Make numerical angle jac images: 53 | for(int dof=0; dof<2; dof++) 54 | { 55 | Matrix<2> m2Warp; 56 | for(int i=0; i<2; i++) 57 | { 58 | double dAngle = mParams.v2Angles[i]; 59 | if(dof == i) 60 | dAngle += 0.01; 61 | m2Warp[0][i] = cos(dAngle); 62 | m2Warp[1][i] = sin(dAngle); 63 | }; 64 | 65 | CVD::transform(mimSharedSourceTemplate, imTwiceToBlur, 66 | M2Inverse(m2Warp), 67 | vec(mimSharedSourceTemplate.size() - ImageRef(1,1)) * 0.5, 68 | vec(imTwiceToBlur.size() - ImageRef(1,1)) * 0.5); 69 | halfSample(imTwiceToBlur, imToBlur); 70 | convolveGaussian(imToBlur, dBlurSigma); 71 | ImageRef ir; 72 | do 73 | mimAngleJacs[ir][dof] = (imToBlur[ir + ImageRef(nOffset, nOffset)] - mimTemplate[ir]) / 0.01; 74 | while(ir.next(mimTemplate.size())); 75 | }; 76 | 77 | // Make the image of image gradients here too (while we have the bigger template to work from) 78 | ImageRef ir; 79 | do 80 | { 81 | mimGradients[ir][0] = 0.5 * 82 | (imToBlur[ir + ImageRef(nOffset + 1, nOffset)] - 83 | imToBlur[ir + ImageRef(nOffset - 1, nOffset)]); 84 | mimGradients[ir][1] = 0.5 * 85 | (imToBlur[ir + ImageRef(nOffset, nOffset + 1 )] - 86 | imToBlur[ir + ImageRef(nOffset, nOffset - 1 )]); 87 | } 88 | while(ir.next(mimGradients.size())); 89 | } 90 | 91 | bool CalibCornerPatch::IterateOnImageWithDrawing(CalibCornerPatch::Params ¶ms, Image &im) 92 | { 93 | bool bReturn = IterateOnImage(params, im); 94 | if(!bReturn) 95 | { 96 | glPointSize(3); 97 | glColor3f(1,0,0); 98 | glBegin(GL_POINTS); 99 | glVertex(params.v2Pos); 100 | glEnd(); 101 | } 102 | return bReturn; 103 | } 104 | 105 | bool CalibCornerPatch::IterateOnImage(CalibCornerPatch::Params ¶ms, Image &im) 106 | { 107 | mParams = params; 108 | double dLastUpdate = 0.0; 109 | for(int i=0; i<20; i++) 110 | { 111 | MakeTemplateWithCurrentParams(); 112 | dLastUpdate = Iterate(im); 113 | 114 | if(dLastUpdate < 0) 115 | return false; 116 | if(dLastUpdate < 0.00001) 117 | break; 118 | } 119 | if(dLastUpdate > 0.001) 120 | return false; 121 | if(fabs(sin(mParams.v2Angles[0] - mParams.v2Angles[1])) < sin(M_PI / 6.0)) 122 | return false; 123 | if(fabs(mParams.dGain) < 20.0) 124 | return false; 125 | if(mdLastError > 25.0) 126 | return false; 127 | 128 | params = mParams; 129 | return true; 130 | } 131 | 132 | double CalibCornerPatch::Iterate(Image &im) 133 | { 134 | Vector<2> v2TL = mParams.v2Pos - vec(mimTemplate.size() - ImageRef(1,1)) / 2.0; 135 | if(v2TL[0] < 0.0 || v2TL[1] < 0.0) 136 | return -1.0; 137 | Vector<2> v2BR = mParams.v2Pos + vec(mimTemplate.size()) + vec(mimTemplate.size() - ImageRef(1,1)) / 2.0; 138 | if(v2BR[0] > (im.size().x - 1.0) || v2BR[1] > (im.size().y - 1.0)) 139 | return -1.0; 140 | 141 | image_interpolate imInterp(im); 142 | Matrix<6> m6JTJ = Zeros; 143 | Vector<6> v6JTD = Zeros; 144 | 145 | 146 | 147 | ImageRef ir; 148 | double dSum = 0.0; 149 | do 150 | { 151 | Vector<2> v2Pos_Template = vec(ir) - vec(mimTemplate.size() - ImageRef(1,1)) / 2.0; 152 | Vector<2> v2Pos_Image = mParams.v2Pos + v2Pos_Template; 153 | double dDiff = imInterp[v2Pos_Image] - (mParams.dGain * mimTemplate[ir] + mParams.dMean); 154 | dSum += fabs(dDiff); 155 | Vector<6> v6Jac; 156 | // Jac for center pos: Minus sign because +pos equates to sliding template - 157 | v6Jac.slice<0,2>() = -1.0 * mParams.dGain * mimGradients[ir]; 158 | // Jac for angles: dPos/dAngle needs finishing by multiplying by pos.. 159 | v6Jac[2] = mimAngleJacs[ir][0] * mParams.dGain; 160 | v6Jac[3] = mimAngleJacs[ir][1] * mParams.dGain; 161 | // Jac for mean: 162 | v6Jac[4] = 1.0; 163 | // Jac for gain: 164 | v6Jac[5] = mimTemplate[ir]; 165 | 166 | m6JTJ += v6Jac.as_col() * v6Jac.as_row(); 167 | v6JTD += dDiff * v6Jac; 168 | } 169 | while(ir.next(mimTemplate.size())); 170 | 171 | Cholesky<6> chol(m6JTJ); 172 | Vector<6> v6Update = 0.7 * chol.backsub(v6JTD); 173 | 174 | //reject bogus updates 175 | if(std::isnan(v6Update[4]) || std::isnan(v6Update[5])){ 176 | return -1.0; 177 | } 178 | 179 | mParams.v2Pos += v6Update.slice<0,2>(); 180 | mParams.v2Angles += v6Update.slice<2,2>(); 181 | mParams.dMean += v6Update[4]; 182 | mParams.dGain += v6Update[5]; 183 | mdLastError = dSum / mimTemplate.size().area(); 184 | return sqrt(v6Update.slice<0,2>() * v6Update.slice<0,2>()); 185 | } 186 | 187 | void CalibCornerPatch::MakeSharedTemplate() 188 | { 189 | const int nSideSize = 100; 190 | const int nHalf = nSideSize / 2; 191 | 192 | mimSharedSourceTemplate.resize(ImageRef(nSideSize, nSideSize)); 193 | 194 | ImageRef ir; 195 | 196 | do 197 | { 198 | float fX = (ir.x < nHalf) ? 1.0 : -1.0; 199 | float fY = (ir.y < nHalf) ? 1.0 : -1.0; 200 | mimSharedSourceTemplate[ir] = fX * fY; 201 | } 202 | while(ir.next(mimSharedSourceTemplate.size())); 203 | } 204 | 205 | CalibCornerPatch::Params::Params() 206 | { 207 | v2Angles[0] = 0.0; 208 | v2Angles[1] = M_PI / 2.0; 209 | dMean = 0.0; 210 | dGain = 1.0; 211 | } 212 | 213 | Matrix<2> CalibCornerPatch::Params::m2Warp() 214 | { 215 | Matrix<2> m2Warp; 216 | for(int i=0; i<2; i++) 217 | { 218 | m2Warp[0][i] = cos(v2Angles[i]); 219 | m2Warp[1][i] = sin(v2Angles[i]); 220 | }; 221 | return m2Warp; 222 | } 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | -------------------------------------------------------------------------------- /ptam/include/ptam/Tracker.h: -------------------------------------------------------------------------------- 1 | //-*- C++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | // 4 | // This header declares the Tracker class. 5 | // The Tracker is one of main components of the system, 6 | // and is responsible for determining the pose of a camera 7 | // from a video feed. It uses the Map to track, and communicates 8 | // with the MapMaker (which runs in a different thread) 9 | // to help construct this map. 10 | // 11 | // Initially there is no map, so the Tracker also has a mode to 12 | // do simple patch tracking across a stereo pair. This is handled 13 | // by the TrackForInitialMap() method and associated sub-methods. 14 | // Once there is a map, TrackMap() is used. 15 | // 16 | // Externally, the tracker should be used by calling TrackFrame() 17 | // with every new input video frame. This then calls either 18 | // TrackForInitialMap() or TrackMap() as appropriate. 19 | // 20 | 21 | #ifndef __TRACKER_H 22 | #define __TRACKER_H 23 | 24 | #include "MapMaker.h" 25 | #include "ATANCamera.h" 26 | #include "MiniPatch.h" 27 | #include "Relocaliser.h" 28 | #include "ptam/Params.h" 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | struct TrackerData; 35 | struct Trail // This struct is used for initial correspondences of the first stereo pair. 36 | { 37 | MiniPatch mPatch; 38 | CVD::ImageRef irCurrentPos; 39 | CVD::ImageRef irInitialPos; 40 | }; 41 | 42 | class Tracker 43 | { 44 | public: 45 | Tracker(CVD::ImageRef irVideoSize, const ATANCamera &c, Map &m, MapMaker &mm); 46 | 47 | // TrackFrame is the main working part of the tracker: call this every frame. 48 | void TrackFrame(CVD::Image &imFrame, bool bDraw); 49 | void TrackFrame(CVD::Image &imFrame, bool bDraw, const TooN::SO3 & imuOrientation); 50 | 51 | inline SE3<> GetCurrentPose() { return mse3CamFromWorld;} 52 | //Weiss{ 53 | inline Matrix<6> GetCurrentCov() { return mmCovariances;} 54 | inline KeyFrame::Ptr GetCurrentKF() { return mCurrentKF;} 55 | Vector<3> CalcSBIRotation(SmallBlurryImage *SBI1, SmallBlurryImage *SBI2); 56 | //} 57 | // Gets messages to be printed on-screen for the user. 58 | std::string GetMessageForUser(); 59 | int getTrackingQuality(){return mTrackingQuality;} 60 | 61 | void command(const std::string & params); 62 | std::list & getTrails(){return mlTrails;}; 63 | bool getTrailTrackingStarted(){return mnInitialStage == TRAIL_TRACKING_STARTED;}; 64 | bool getTrailTrackingComplete(){return mnInitialStage == TRAIL_TRACKING_COMPLETE;}; 65 | CVD::Image > & ComputeGrid(); // Computes the reference grid; 66 | 67 | protected: 68 | CVD::Image > mProjVertices; 69 | KeyFrame::Ptr mCurrentKF; // The current working frame as a keyframe struct 70 | 71 | // The major components to which the tracker needs access: 72 | Map &mMap; // The map, consisting of points and keyframes 73 | MapMaker &mMapMaker; // The class which maintains the map 74 | ATANCamera mCamera; // Projection model 75 | Relocaliser mRelocaliser; // Relocalisation module 76 | 77 | CVD::ImageRef mirSize; // Image size of whole image 78 | 79 | void Reset(); // Restart from scratch. Also tells the mapmaker to reset itself. 80 | void RenderGrid(); // Draws the reference grid 81 | 82 | // The following members are used for initial map tracking (to get the first stereo pair and correspondences): 83 | void TrackForInitialMap(); // This is called by TrackFrame if there is not a map yet. 84 | enum {TRAIL_TRACKING_NOT_STARTED, 85 | TRAIL_TRACKING_STARTED, 86 | TRAIL_TRACKING_COMPLETE} mnInitialStage; // How far are we towards making the initial map? 87 | void TrailTracking_Start(); // First frame of initial trail tracking. Called by TrackForInitialMap. 88 | int TrailTracking_Advance(); // Steady-state of initial trail tracking. Called by TrackForInitialMap. 89 | std::list mlTrails; // Used by trail tracking 90 | KeyFrame::Ptr mFirstKF; // First of the stereo pair 91 | KeyFrame::Ptr mPreviousFrameKF; // Used by trail tracking to check married matches 92 | 93 | // Methods for tracking the map once it has been made: 94 | void TrackMap(); // Called by TrackFrame if there is a map. 95 | void AssessTrackingQuality(); // Heuristics to choose between good, poor, bad. 96 | void ApplyMotionModel(); // Decaying velocity motion model applied prior to TrackMap 97 | void UpdateMotionModel(); // Motion model is updated after TrackMap 98 | int SearchForPoints(std::vector &vTD, 99 | int nRange, 100 | int nFineIts); // Finds points in the image 101 | Vector<6> CalcPoseUpdate(std::vector vTD, 102 | double dOverrideSigma = 0.0, 103 | bool bMarkOutliers = false); // Updates pose from found points. 104 | SE3<> mse3CamFromWorld; // Camera pose: this is what the tracker updates every frame. 105 | SE3<> mse3StartPos; // What the camera pose was at the start of the frame. 106 | Vector<6> mv6CameraVelocity; // Motion model 107 | double mdVelocityMagnitude; // Used to decide on coarse tracking 108 | double mdMSDScaledVelocityMagnitude; // Velocity magnitude scaled by relative scene depth. 109 | bool mbDidCoarse; // Did tracking use the coarse tracking stage? 110 | 111 | bool mbDraw; // Should the tracker draw anything to OpenGL? 112 | 113 | // achtelik{ 114 | SO3<> mso3CurrentImu; 115 | SO3<> mso3LastImu; 116 | 117 | //} 118 | 119 | 120 | // Weiss{ 121 | Matrix<6> mmCovariances; // covariance of current converged pose estimate 122 | KeyFrame::Ptr mOldKF; 123 | bool mAutoreset; // indicates if user pressed reset or not 124 | //} 125 | 126 | // Interface with map maker: 127 | int mnFrame; // Frames processed since last reset 128 | int mnLastKeyFrameDropped; // Counter of last keyframe inserted. 129 | void AddNewKeyFrame(); // Gives the current frame to the mapmaker to use as a keyframe 130 | 131 | // Tracking quality control: 132 | int manMeasAttempted[LEVELS]; 133 | int manMeasFound[LEVELS]; 134 | enum {BAD, DODGY, GOOD} mTrackingQuality; 135 | int mnLostFrames; 136 | 137 | // Relocalisation functions: 138 | bool AttemptRecovery(); // Called by TrackFrame if tracking is lost. 139 | bool mbJustRecoveredSoUseCoarse;// Always use coarse tracking after recovery! 140 | 141 | // Frame-to-frame motion init: 142 | SmallBlurryImage *mpSBILastFrame; 143 | SmallBlurryImage *mpSBIThisFrame; 144 | void CalcSBIRotation(); 145 | Vector<6> mv6SBIRot; 146 | bool mbUseSBIInit; 147 | 148 | // User interaction for initial tracking: 149 | bool mbUserPressedSpacebar; 150 | std::ostringstream mMessageForUser; 151 | 152 | // GUI interface: 153 | void GUICommandHandler(std::string sCommand, std::string sParams); 154 | static void GUICommandCallBack(void* ptr, std::string sCommand, std::string sParams); 155 | struct Command {std::string sCommand; std::string sParams; }; 156 | std::vector mvQueuedCommands; 157 | 158 | 159 | }; 160 | 161 | #endif 162 | 163 | 164 | 165 | 166 | 167 | 168 | -------------------------------------------------------------------------------- /ptam/include/ptam/PatchFinder.h: -------------------------------------------------------------------------------- 1 | // -*- c++ -*- 2 | // Copyright 2008 Isis Innovation Limited 3 | // 4 | // This header declares the PatchFinder class. 5 | // This is quite a low-level class. 6 | // 7 | // The purpose of the PatchFinder is to find a map point in a new view. 8 | // This proceeds in multiple stages: 9 | // 1. A warping matrix appropriate for the current view is calculated, 10 | // 2. A `coarse' matching template of the map point is generated by warping; 11 | // 3. The new view is searched for this template at corner locations; 12 | // 4. An inverse-composition (`fine') matching template is generated; 13 | // 5. A sub-pixel accurate position is calculated using inverse composition. 14 | // 15 | // To clarify points 1 and 2 above: Each map point does _not_ store a `patch' 16 | // in the style of many other SLAM implementations! Each map point came from 17 | // a key-frame, and that key-frame is stored in the map. Each map point only 18 | // stores the information on where it came from in what key-frame. Patches 19 | // are then generated from the pixels of the source key-frame. According to the 20 | // relative camera poses now and of the source key-frame, a warp has to be 21 | // applied to generate an NxN pixel square template appropriate for the current 22 | // view. Thus, the matching template is always NxN, but the region of source 23 | // pixels used can change size and shape! This is all described in the 24 | // Klein & Murray ISMAR 2007. 25 | // 26 | // Most of the above stages are optional, and there are different versions 27 | // of stages 1/2, including some for operating when there is not yet a 3D map 28 | // point. The class provides no safety checks to ensure that the operations 29 | // are carried out in the right order - it's the caller's responsibility. 30 | // 31 | // The patch finder uses zero-mean SSD as its difference metric. 32 | // 33 | // Although PatchFinder can use arbitrary-sized search templates (it's determined 34 | // at construction), the use of 8x8 pixel templates (the default) is highly 35 | // recommended, as the coarse search for this size is SSE-optimised. 36 | 37 | #ifndef __PATCHFINDER_H 38 | #define __PATCHFINDER_H 39 | 40 | #include 41 | using namespace TooN; 42 | #include 43 | #include 44 | #include 45 | #include "MapPoint.h" 46 | #include "LevelHelpers.h" 47 | #include 48 | 49 | class PatchFinder 50 | { 51 | public: 52 | // Constructor defines size of search patch. 53 | PatchFinder(int nPatchSize = 8); 54 | 55 | // Step 1 Function. 56 | // This calculates the warping matrix appropriate for observing point p 57 | // from the current view (described as an SE3.) This also needs the camera 58 | // projection derivates at level zero for the point's projection in the new view. 59 | // It also calculates which pyramid level we should search in, and this is 60 | // returned as an int. Negative level returned denotes an inappropriate 61 | // transformation. 62 | int CalcSearchLevelAndWarpMatrix(boost::shared_ptr p, SE3<> se3CFromW, Matrix<2> &m2CamDerivs); 63 | inline int GetLevel() { return mnSearchLevel; } 64 | inline int GetLevelScale() { return LevelScale(mnSearchLevel); } 65 | 66 | // Step 2 Functions 67 | // Generates the NxN search template either from the pre-calculated warping matrix, 68 | // or an identity transformation. 69 | void MakeTemplateCoarseCont(boost::shared_ptr p); // If the warping matrix has already been pre-calced, use this. 70 | void MakeTemplateCoarse(boost::shared_ptr, SE3<> se3CFromW, Matrix<2> &m2CamDerivs); // This also calculates the warp. 71 | void MakeTemplateCoarseNoWarp(boost::shared_ptr p); // Identity warp: just copies pixels from the source KF. 72 | void MakeTemplateCoarseNoWarp(boost::shared_ptr k, int nLevel, CVD::ImageRef irLevelPos); // Identity warp if no MapPoint struct exists yet. 73 | 74 | // If the template making failed (i.e. it needed pixels outside the source image), 75 | // this bool will return false. 76 | inline bool TemplateBad() { return mbTemplateBad;} 77 | 78 | // Step 3 Functions 79 | // This is the raison d'etre of the class: finds the patch in the current input view, 80 | // centered around ir, Searching around FAST corner locations only within a radius nRange only. 81 | // Inputs are given in level-zero coordinates! Returns true if the patch was found. 82 | bool FindPatchCoarse(CVD::ImageRef ir, boost::shared_ptr kf, unsigned int nRange); 83 | int ZMSSDAtPoint(CVD::BasicImage &im, const CVD::ImageRef &ir); // This evaluates the score at one location 84 | // Results from step 3: 85 | // All positions are in the scale of level 0. 86 | inline CVD::ImageRef GetCoarsePos() { return CVD::ImageRef((int) mv2CoarsePos[0], (int) mv2CoarsePos[1]);} 87 | inline Vector<2> GetCoarsePosAsVector() { return mv2CoarsePos; } 88 | 89 | // Step 4 90 | void MakeSubPixTemplate(); // Generate the inverse composition template and jacobians 91 | 92 | // Step 5 Functions 93 | bool IterateSubPixToConvergence(KeyFrame& kf, int nMaxIts); // Run inverse composition till convergence 94 | double IterateSubPix(KeyFrame& kf); // Single iteration of IC. Returns sum-squared pixel update dist, or negative if out of imag 95 | inline Vector<2> GetSubPixPos() { return mv2SubPixPos; } // Get result 96 | void SetSubPixPos(Vector<2> v2) { mv2SubPixPos = v2; } // Set starting point 97 | 98 | // Get the uncertainty estimate of a found patch; 99 | // This for just returns an appropriately-scaled identity! 100 | inline Matrix<2> GetCov() 101 | { 102 | return LevelScale(mnSearchLevel) * Identity; 103 | }; 104 | 105 | int mnMaxSSD; // This is the max ZMSSD for a valid match. It's set in the constructor. 106 | 107 | protected: 108 | 109 | int mnPatchSize; // Size of one side of the matching template. 110 | 111 | // Some values stored for the coarse template: 112 | int mnTemplateSum; // Cached pixel-sum of the coarse template 113 | int mnTemplateSumSq; // Cached pixel-squared sum of the coarse template 114 | inline void MakeTemplateSums(); // Calculate above values 115 | 116 | CVD::Image mimTemplate; // The matching template 117 | CVD::Image > mimJacs; // Inverse composition jacobians; stored as floats to save a bit of space. 118 | 119 | Matrix<2> mm2WarpInverse; // Warping matrix 120 | int mnSearchLevel; // Search level in input pyramid 121 | Matrix<3> mm3HInv; // Inverse composition JtJ^-1 122 | Vector<2> mv2SubPixPos; // In the scale of level 0 123 | double mdMeanDiff; // Updated during inverse composition 124 | 125 | CVD::ImageRef mirPredictedPos; // Search center location of FindPatchCoarse in L0 126 | Vector<2> mv2CoarsePos; // In the scale of level 0; hence the use of vector rather than ImageRef 127 | CVD::ImageRef mirCenter; // Quantized location of the center pixel of the NxN pixel template 128 | bool mbFound; // Was the patch found? 129 | bool mbTemplateBad; // Error during template generation? 130 | 131 | // Some cached values to avoid duplicating work if the camera is stopped: 132 | MapPoint::Ptr mpLastTemplateMapPoint; // Which was the last map point this PatchFinder used? 133 | Matrix<2> mm2LastWarpMatrix; // What was the last warp matrix this PatchFinder used? 134 | }; 135 | 136 | #endif 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | -------------------------------------------------------------------------------- /ptam/cfg/PtamParams.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE='ptam' 4 | import roslib; roslib.load_manifest(PACKAGE) 5 | 6 | #from driver_base.msg import SensorLevels 7 | from dynamic_reconfigure.parameter_generator_catkin import * 8 | 9 | gen = ParameterGenerator() 10 | 11 | # @todo Double check these ranges, defaults 12 | 13 | enum_bundle = gen.enum([ 14 | gen.const("LOCAL_GLOBAL", str_t, "LOCAL_GLOBAL", "local and global bundle adjustment"), 15 | gen.const("LOCAL", str_t, "LOCAL", "local bundle adjustment only"), 16 | gen.const("GLOBAL", str_t, "GLOBAL", "global bundle adjustment only") 17 | ], "An enum to set the bundle adjustment mode.") 18 | 19 | enum_FAST = gen.enum([ 20 | gen.const("FAST9", str_t, "FAST9", "FAST 9"), 21 | gen.const("FAST10", str_t, "FAST10", "FAST 10"), 22 | gen.const("FAST9_nonmax", str_t, "FAST9_nonmax", "FAST 9 with nonmax suppression"), 23 | gen.const("AGAST12d", str_t, "AGAST12d", "AGAST 12 pixel diamond"), 24 | gen.const("OAST16", str_t, "OAST16", "AGAST 16 pixel circular") 25 | ], "An enum to set the FAST corner extraction method.") 26 | 27 | enum_MOTIONMODEL = gen.enum([ 28 | gen.const("MM_CONSTANT", str_t, "CONSTANT", "use constant motion model."), 29 | gen.const("MM_IMU", str_t, "IMU", "use imu orientation for the motion model."), 30 | gen.const("MM_FULL_POSE", str_t, "FULL_POSE", "use full pose estimated externally for motion model.") 31 | ], "An enum to set wich input to use for the motion model.") 32 | 33 | RESTART_AR = gen.const("SEND_PARAMETERS", int_t, 0x00000001, "send_parameters") 34 | 35 | 36 | # Name Type Reconfiguration lvl Description Default Min Max 37 | gen.add("Scale", double_t, 0, "Scale", 1, 0.01, 30) 38 | gen.add("MotionModelSource", str_t, 0, "selects the source for the motion model.", "CONSTANT", edit_method=enum_MOTIONMODEL) 39 | gen.add("MaxPatchesPerFrame", double_t, 0, "max features per frame", 500, 10, 1000) 40 | gen.add("MaxKFDistWiggleMult", double_t, 0, "'distance' after which a new kf is requested", 3, 0.1, 10) 41 | gen.add("UseKFPixelDist", bool_t, 0, "use AutoInitPixel as new KF request criteria", False) 42 | gen.add("NoLevelZeroMapPoints", bool_t, 0, "do not add map points at level zero", False) 43 | gen.add("EpiDepthSearchMaxRange", double_t, 0, "depth variance to search for features", 100, 1, 100) 44 | gen.add("CoarseMin", double_t, 0, "min number of features for coarse tracking", 20, 1, 100) 45 | gen.add("CoarseMax", double_t, 0, "max number of features for coarse tracking", 60, 1, 100) 46 | gen.add("CoarseRange", double_t, 0, "Pixel search radius for coarse features", 30, 1, 100) 47 | gen.add("CoarseSubPixIts", double_t, 0, "coarse tracking sub-pixel iterations", 8, 1, 100) 48 | gen.add("DisableCoarse", bool_t, 0, "enable/disable coarse tracking", False) 49 | gen.add("CoarseMinVelocity", double_t, 0, "speed above which coarse stage is used", 0.006, 0, 1) 50 | gen.add("TrackingQualityGood", double_t, 0, "min ratio features visible/features found for good tracking", 0.3, 0, 1) 51 | gen.add("TrackingQualityLost", double_t, 0, "max ratio features visible/features found before lost", 0.13, 0, 1) 52 | gen.add("TrackingQualityFoundPixels", int_t, 0, "min pixels to be found for good tracking", 100, 1, 1000) 53 | gen.add("MaxIterations", double_t, 0, "max iterations for bundle adjustment", 20, 1, 100) 54 | gen.add("MaxKF", int_t, 0, "number of keyframes kept in the map (0 or 1 = inf)", 0, 0, 1000) 55 | gen.add("BundleMethod", str_t, 0, "bundleadjustment method", "LOCAL_GLOBAL", edit_method=enum_bundle) 56 | 57 | gen.add("UpdateSquaredConvergenceLimit", double_t, 0, "limit for convergence in bundle adjustment", 0.000001, 0, 1) 58 | gen.add("BundleDebugMessages", bool_t, 0, "print bundle debug messages", False) 59 | gen.add("FASTMethod", str_t, 0, "FAST corner method", "FAST9_nonmax", edit_method=enum_FAST) 60 | gen.add("Thres_lvl0", int_t, 0, "threshold for FAST features on level 0", 10, 0, 255) 61 | gen.add("Thres_lvl1", int_t, 0, "threshold for FAST features on level 1", 15, 0, 255) 62 | gen.add("Thres_lvl2", int_t, 0, "threshold for FAST features on level 2", 15, 0, 255) 63 | gen.add("Thres_lvl3", int_t, 0, "threshold for FAST features on level 3", 10, 0, 255) 64 | gen.add("AdaptiveThrs", bool_t, 0, "adaptive threshold for corner extraction", False) 65 | gen.add("AdaptiveThrsMult", double_t, 0, "controls adaptive threshold to MaxPatches*N corners", 5, 0.5, 20) 66 | 67 | gen.add("RotationEstimatorBlur", double_t, 0, "small images for the rotation estimator blur", 0.75, 0, 10) 68 | gen.add("UseRotationEstimator", bool_t, 0, "small images for the rotation estimator enable/disable", True) 69 | gen.add("MiniPatchMaxSSD", int_t, 0, "MiniPatch tracking threshhold", 100000, 0, 10000000) 70 | 71 | gen.add("PlaneAlignerRansacs", int_t, 0, "number of dominant plane RANSACs", 100, 0, 1000) 72 | gen.add("RelocMaxScore", int_t, 0, "score for relocalization", 9000000, 0, 90000000) 73 | 74 | gen.add("AutoInit", bool_t, 0, "enable auto initialization", False) 75 | gen.add("CheckInitMapVar", bool_t, 0, "check map point variance for initial map", False) 76 | gen.add("AutoInitPixel", int_t, 0, "min pixel distance for auto initialization", 20, 1, 100) 77 | gen.add("MaxStereoInitLoops", int_t, 0, "max # of loops for stereo initialization", 10, 1, 100) 78 | 79 | exit(gen.generate(PACKAGE, "Config", "PtamParams")) 80 | -------------------------------------------------------------------------------- /ptam/include/ptam/ATANCamera.h: -------------------------------------------------------------------------------- 1 | // *-* c++ *-* 2 | // Copyright 2008 Isis Innovation Limited 3 | 4 | // N-th implementation of a camera model 5 | // GK 2007 6 | // Evolved a half dozen times from the CVD-like model I was given by 7 | // TWD in 2000 8 | // 9 | // This one uses the ``FOV'' distortion model of 10 | // Deverneay and Faugeras, Straight lines have to be straight, 2001 11 | // 12 | // BEWARE: This camera model caches intermediate results in member variables 13 | // Some functions therefore depend on being called in order: i.e. 14 | // GetProjectionDerivs() uses data stored from the last Project() or UnProject() 15 | // THIS MEANS YOU MUST BE CAREFUL WITH MULTIPLE THREADS 16 | // Best bet is to give each thread its own version of the camera! 17 | // 18 | // Camera parameters are stored in a GVar, but changing the gvar has no effect 19 | // until the next call to RefreshParams() or SetImageSize(). 20 | // 21 | // Pixel conventions are as follows: 22 | // For Project() and Unproject(), 23 | // round pixel values - i.e. (0.0, 0.0) - refer to pixel centers 24 | // I.e. the top left pixel in the image covers is centered on (0,0) 25 | // and covers the area (-.5, -.5) to (.5, .5) 26 | // 27 | // Be aware that this is not the same as what opengl uses but makes sense 28 | // for acessing pixels using ImageRef, especially ir_rounded. 29 | // 30 | // What is the UFB? 31 | // This is for projecting the visible image area 32 | // to a unit square coordinate system, with the top-left at 0,0, 33 | // and the bottom-right at 1,1 34 | // This is useful for rendering into textures! The top-left pixel is NOT 35 | // centered at 0,0, rather the top-left corner of the top-left pixel is at 36 | // 0,0!!! This is the way OpenGL thinks of pixel coords. 37 | // There's the Linear and the Distorting version - 38 | // For the linear version, can use 39 | // glMatrixMode(GL_PROJECTION); glLoadIdentity(); 40 | // glMultMatrix(Camera.MakeUFBLinearFrustumMatrix(near,far)); 41 | // To render un-distorted geometry with full frame coverage. 42 | // 43 | 44 | #ifndef __ATAN_CAMERA_H 45 | #define __ATAN_CAMERA_H 46 | 47 | #include 48 | #include 49 | using namespace TooN; 50 | #include 51 | //#include 52 | 53 | #include 54 | #include 55 | 56 | #define NUMTRACKERCAMPARAMETERS 5 57 | 58 | class CameraCalibrator; 59 | class CalibImage; 60 | 61 | // The parameters are: 62 | // 0 - normalized x focal length 63 | // 1 - normalized y focal length 64 | // 2 - normalized x offset 65 | // 3 - normalized y offset 66 | // 4 - w (distortion parameter) 67 | 68 | class ATANCamera { 69 | public: 70 | ATANCamera(std::string sName); 71 | 72 | // Image size get/set: updates the internal projection params to that target image size. 73 | void SetImageSize(Vector<2> v2ImageSize); 74 | inline void SetImageSize(CVD::ImageRef irImageSize) {SetImageSize(vec(irImageSize));}; 75 | inline Vector<2> GetImageSize() {return mvImageSize;}; 76 | void RefreshParams(); 77 | 78 | // Various projection functions 79 | Vector<2> Project(const Vector<2>& camframe); // Projects from camera z=1 plane to pixel coordinates, with radial distortion 80 | inline Vector<2> Project(CVD::ImageRef ir) { return Project(vec(ir)); } 81 | Vector<2> UnProject(const Vector<2>& imframe); // Inverse operation 82 | inline Vector<2> UnProject(CVD::ImageRef ir) { return UnProject(vec(ir)); } 83 | 84 | Vector<2> UFBProject(const Vector<2>& camframe); 85 | Vector<2> UFBUnProject(const Vector<2>& camframe); 86 | inline Vector<2> UFBLinearProject(const Vector<2>& camframe); 87 | inline Vector<2> UFBLinearUnProject(const Vector<2>& fbframe); 88 | 89 | Matrix<2,2> GetProjectionDerivs(); // Projection jacobian 90 | 91 | inline bool Invalid() { return mbInvalid;} 92 | inline double LargestRadiusInImage() { return mdLargestRadius; } 93 | inline double OnePixelDist() { return mdOnePixelDist; } 94 | 95 | // The z=1 plane bounding box of what the camera can see 96 | inline Vector<2> ImplaneTL(); 97 | inline Vector<2> ImplaneBR(); 98 | 99 | // OpenGL helper function 100 | Matrix<4> MakeUFBLinearFrustumMatrix(double near, double far); 101 | 102 | // Feedback for Camera Calibrator 103 | double PixelAspectRatio() { return mvFocal[1] / mvFocal[0];} 104 | 105 | 106 | // Useful for gvar-related reasons (in case some external func tries to read the camera params gvar, and needs some defaults.) 107 | static const Vector mvDefaultParams; 108 | 109 | //Weiss{ 110 | // GVars3::gvar3 > mgvvCameraParams; // The actual camera parameters 111 | Vector mgvvCameraParams; // The actual camera parameters 112 | //} 113 | 114 | protected: 115 | 116 | 117 | Matrix<2, NUMTRACKERCAMPARAMETERS> GetCameraParameterDerivs(); 118 | void UpdateParams(Vector vUpdate); 119 | void DisableRadialDistortion(); 120 | 121 | // maximal allowable field of view (for fisheye cams) 122 | double MaxFOV_; // maximal field of view: in radians and half angle 123 | 124 | // Cached from the last project/unproject: 125 | Vector<2> mvLastCam; // Last z=1 coord 126 | Vector<2> mvLastIm; // Last image/UFB coord 127 | Vector<2> mvLastDistCam; // Last distorted z=1 coord 128 | double mdLastR; // Last z=1 radius 129 | double mdLastDistR; // Last z=1 distorted radius 130 | double mdLastFactor; // Last ratio of z=1 radii 131 | bool mbInvalid; // Was the last projection invalid? 132 | 133 | // Cached from last RefreshParams: 134 | double mdLargestRadius; // Largest R in the image 135 | double mdMaxR; // Largest R for which we consider projection valid 136 | double mdOnePixelDist; // z=1 distance covered by a single pixel offset (a rough estimate!) 137 | double md2Tan; // distortion model coeff 138 | double mdOneOver2Tan; // distortion model coeff 139 | double mdW; // distortion model coeff 140 | double mdWinv; // distortion model coeff 141 | double mdDistortionEnabled; // One or zero depending on if distortion is on or off. 142 | Vector<2> mvCenter; // Pixel projection center 143 | Vector<2> mvFocal; // Pixel focal length 144 | Vector<2> mvInvFocal; // Inverse pixel focal length 145 | Vector<2> mvImageSize; 146 | Vector<2> mvUFBLinearFocal; 147 | Vector<2> mvUFBLinearInvFocal; 148 | Vector<2> mvUFBLinearCenter; 149 | Vector<2> mvImplaneTL; 150 | Vector<2> mvImplaneBR; 151 | 152 | // Radial distortion transformation factor: returns ration of distorted / undistorted radius. 153 | inline double rtrans_factor(double r) 154 | { 155 | if(r < 0.001 || mdW == 0.0) 156 | return 1.0; 157 | else 158 | return (mdWinv* atan(r * md2Tan) / r); 159 | }; 160 | 161 | // Inverse radial distortion: returns un-distorted radius from distorted. 162 | inline double invrtrans(double r) 163 | { 164 | if(mdW == 0.0) 165 | return r; 166 | return tan(r * mdW) * mdOneOver2Tan; 167 | }; 168 | 169 | std::string msName; 170 | 171 | friend class CameraCalibrator; // friend declarations allow access to calibration jacobian and camera update function. 172 | friend class CalibImage; 173 | }; 174 | 175 | // Some inline projection functions: 176 | inline Vector<2> ATANCamera::UFBLinearProject(const Vector<2>& camframe) 177 | { 178 | Vector<2> v2Res; 179 | v2Res[0] = camframe[0] * mvUFBLinearFocal[0] + mvUFBLinearCenter[0]; 180 | v2Res[1] = camframe[1] * mvUFBLinearFocal[1] + mvUFBLinearCenter[1]; 181 | return v2Res; 182 | } 183 | 184 | inline Vector<2> ATANCamera::UFBLinearUnProject(const Vector<2>& fbframe) 185 | { 186 | Vector<2> v2Res; 187 | v2Res[0] = (fbframe[0] - mvUFBLinearCenter[0]) * mvUFBLinearInvFocal[0]; 188 | v2Res[1] = (fbframe[1] - mvUFBLinearCenter[1]) * mvUFBLinearInvFocal[1]; 189 | return v2Res; 190 | } 191 | 192 | 193 | #endif 194 | 195 | -------------------------------------------------------------------------------- /ptam/src/SmallBlurryImage.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2008 Isis Innovation Limited 2 | #include "ptam/SmallBlurryImage.h" 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace CVD; 11 | using namespace std; 12 | 13 | ImageRef SmallBlurryImage::mirSize(-1,-1); 14 | 15 | SmallBlurryImage::SmallBlurryImage(KeyFrame& kf, double dBlur) 16 | { 17 | mbMadeJacs = false; 18 | MakeFromKF(kf, dBlur); 19 | } 20 | 21 | SmallBlurryImage::SmallBlurryImage() 22 | { 23 | mbMadeJacs = false; 24 | } 25 | 26 | // Make a SmallBlurryImage from a KeyFrame This fills in the mimSmall 27 | // image (Which is just a small un-blurred version of the KF) and 28 | // mimTemplate (which is a floating-point, zero-mean blurred version 29 | // of the above) 30 | void SmallBlurryImage::MakeFromKF(KeyFrame& kf, double dBlur) 31 | { 32 | if(mirSize[0] == -1) 33 | mirSize = kf.aLevels[3].im.size() / 2; 34 | mbMadeJacs = false; 35 | 36 | mimSmall.resize(mirSize); 37 | mimTemplate.resize(mirSize); 38 | 39 | mbMadeJacs = false; 40 | halfSample(kf.aLevels[3].im, mimSmall); 41 | ImageRef ir; 42 | unsigned int nSum = 0; 43 | do 44 | nSum += mimSmall[ir]; 45 | while(ir.next(mirSize)); 46 | 47 | float fMean = ((float) nSum) / mirSize.area(); 48 | 49 | ir.home(); 50 | do 51 | mimTemplate[ir] = mimSmall[ir] - fMean; 52 | while(ir.next(mirSize)); 53 | 54 | convolveGaussian(mimTemplate, dBlur); 55 | } 56 | 57 | // Make the jacobians (actually, no more than a gradient image) 58 | // of the blurred template 59 | void SmallBlurryImage::MakeJacs() 60 | { 61 | mimImageJacs.resize(mirSize); 62 | // Fill in the gradient image 63 | ImageRef ir; 64 | do 65 | { 66 | Vector<2> &v2Grad = mimImageJacs[ir]; 67 | if(mimTemplate.in_image_with_border(ir,1)) 68 | { 69 | v2Grad[0] = mimTemplate[ir + ImageRef(1,0)] - mimTemplate[ir - ImageRef(1,0)]; 70 | v2Grad[1] = mimTemplate[ir + ImageRef(0,1)] - mimTemplate[ir - ImageRef(0,1)]; 71 | // N.b. missing 0.5 factor in above, this will be added later. 72 | } 73 | else 74 | v2Grad = Zeros; 75 | } 76 | while(ir.next(mirSize)); 77 | mbMadeJacs = true; 78 | }; 79 | 80 | // Calculate the zero-mean SSD between one image and the next. 81 | // Since both are zero mean already, just calculate the SSD... 82 | double SmallBlurryImage::ZMSSD(SmallBlurryImage &other) 83 | { 84 | double dSSD = 0.0; 85 | ImageRef ir; 86 | do 87 | { 88 | double dDiff = mimTemplate[ir] - other.mimTemplate[ir]; 89 | dSSD += dDiff * dDiff; 90 | } 91 | while(ir.next(mirSize)); 92 | return dSSD; 93 | } 94 | 95 | 96 | // Find an SE2 which best aligns an SBI to a target 97 | // Do this by ESM-tracking a la Benhimane & Malis 98 | pair,double> SmallBlurryImage::IteratePosRelToTarget(SmallBlurryImage &other, int nIterations) 99 | { 100 | SE2<> se2CtoC; 101 | SE2<> se2WfromC; 102 | ImageRef irCenter = mirSize / 2; 103 | se2WfromC.get_translation() = vec(irCenter); 104 | 105 | pair, double> result_pair; 106 | if(!other.mbMadeJacs) 107 | { 108 | cerr << "You spanner, you didn't make the jacs for the target." << endl; 109 | assert(other.mbMadeJacs); 110 | }; 111 | 112 | double dMeanOffset = 0.0; 113 | Vector<4> v4Accum; 114 | 115 | Vector<10> v10Triangle; 116 | Image imWarped(mirSize); 117 | 118 | double dFinalScore = 0.0; 119 | for(int it = 0; it v4Jac; 125 | v4Jac[3] = 1.0; 126 | 127 | SE2<> se2XForm = se2WfromC * se2CtoC * se2WfromC.inverse(); 128 | 129 | // Make the warped current image template: 130 | Vector<2> v2Zero = Zeros; 131 | CVD::transform(mimTemplate, imWarped, se2XForm.get_rotation().get_matrix(), se2XForm.get_translation(), v2Zero, -9e20f); 132 | 133 | // Now compare images, calc differences, and current image jacobian: 134 | ImageRef ir; 135 | do 136 | { 137 | if(!imWarped.in_image_with_border(ir,1)) 138 | continue; 139 | float l,r,u,d,here; 140 | l = imWarped[ir - ImageRef(1,0)]; 141 | r = imWarped[ir + ImageRef(1,0)]; 142 | u = imWarped[ir - ImageRef(0,1)]; 143 | d = imWarped[ir + ImageRef(0,1)]; 144 | here = imWarped[ir]; 145 | if(l + r + u + d + here < -9999.9) // This means it's out of the image; c.f. the -9e20f param to transform. 146 | continue; 147 | 148 | Vector<2> v2CurrentGrad; 149 | v2CurrentGrad[0] = r - l; // Missing 0.5 factor 150 | v2CurrentGrad[1] = d - u; 151 | 152 | Vector<2> v2SumGrad = 0.25 * (v2CurrentGrad + other.mimImageJacs[ir]); 153 | // Why 0.25? This is from missing 0.5 factors: One for 154 | // the fact we average two gradients, the other from 155 | // each gradient missing a 0.5 factor. 156 | 157 | v4Jac[0] = v2SumGrad[0]; 158 | v4Jac[1] = v2SumGrad[1]; 159 | v4Jac[2] = -(ir.y - irCenter.y) * v2SumGrad[0] + (ir.x - irCenter.x) * v2SumGrad[1]; 160 | // v4Jac[3] = 1.0; 161 | 162 | double dDiff = imWarped[ir] - other.mimTemplate[ir] + dMeanOffset; 163 | dFinalScore += dDiff * dDiff; 164 | 165 | v4Accum += dDiff * v4Jac; 166 | 167 | // Speedy fill of the LL triangle of JTJ: 168 | double *p = &v10Triangle[0]; 169 | *p++ += v4Jac[0] * v4Jac[0]; 170 | *p++ += v4Jac[1] * v4Jac[0]; 171 | *p++ += v4Jac[1] * v4Jac[1]; 172 | *p++ += v4Jac[2] * v4Jac[0]; 173 | *p++ += v4Jac[2] * v4Jac[1]; 174 | *p++ += v4Jac[2] * v4Jac[2]; 175 | *p++ += v4Jac[0]; 176 | *p++ += v4Jac[1]; 177 | *p++ += v4Jac[2]; 178 | *p++ += 1.0; 179 | } 180 | while(ir.next(mirSize)); 181 | 182 | Vector<4> v4Update; 183 | 184 | // Solve for JTJ-1JTv; 185 | { 186 | Matrix<4> m4; 187 | int v=0; 188 | for(int j=0; j<4; j++) 189 | for(int i=0; i<=j; i++) 190 | m4[j][i] = m4[i][j] = v10Triangle[v++]; 191 | Cholesky<4> chol(m4); 192 | v4Update = chol.backsub(v4Accum); 193 | } 194 | 195 | SE2<> se2Update; 196 | se2Update.get_translation() = -v4Update.slice<0,2>(); 197 | se2Update.get_rotation() = SO2<>::exp(-v4Update[2]); 198 | se2CtoC = se2CtoC * se2Update; 199 | dMeanOffset -= v4Update[3]; 200 | } 201 | 202 | result_pair.first = se2CtoC; 203 | result_pair.second = dFinalScore; 204 | return result_pair; 205 | } 206 | 207 | 208 | // What is the 3D camera rotation (zero trans) SE3<> which causes an 209 | // input image SO2 rotation? 210 | SE3<> SmallBlurryImage::SE3fromSE2(SE2<> se2, ATANCamera camera) 211 | { 212 | // Do this by projecting two points, and then iterating the SE3<> (SO3 213 | // actually) until convergence. It might seem stupid doing this so 214 | // precisely when the whole SE2-finding is one big hack, but hey. 215 | 216 | camera.SetImageSize(mirSize); 217 | 218 | Vector<2> av2Turned[2]; // Our two warped points in pixels 219 | av2Turned[0] = vec(mirSize / 2) + se2 * vec(ImageRef(5,0)); 220 | av2Turned[1] = vec(mirSize / 2) + se2 * vec(ImageRef(-5,0)); 221 | 222 | Vector<3> av3OrigPoints[2]; // 3D versions of these points. 223 | av3OrigPoints[0] = unproject(camera.UnProject(vec(mirSize / 2) + vec(ImageRef(5,0)))); 224 | av3OrigPoints[1] = unproject(camera.UnProject(vec(mirSize / 2) + vec(ImageRef(-5,0)))); 225 | 226 | SO3<> so3; 227 | for(int it = 0; it<3; it++) 228 | { 229 | WLS<3> wls; // lazy; no need for the 'W' 230 | wls.add_prior(10.0); 231 | for(int i=0; i<2; i++) 232 | { 233 | // Project into the image to find error 234 | Vector<3> v3Cam = so3 * av3OrigPoints[i]; 235 | Vector<2> v2Implane = project(v3Cam); 236 | Vector<2> v2Pixels = camera.Project(v2Implane); 237 | Vector<2> v2Error = av2Turned[i] - v2Pixels; 238 | 239 | Matrix<2> m2CamDerivs = camera.GetProjectionDerivs(); 240 | Matrix<2,3> m23Jacobian; 241 | double dOneOverCameraZ = 1.0 / v3Cam[2]; 242 | for(int m=0; m<3; m++) 243 | { 244 | const Vector<3> v3Motion = SO3<>::generator_field(m, v3Cam); 245 | Vector<2> v2CamFrameMotion; 246 | v2CamFrameMotion[0] = (v3Motion[0] - v3Cam[0] * v3Motion[2] * dOneOverCameraZ) * dOneOverCameraZ; 247 | v2CamFrameMotion[1] = (v3Motion[1] - v3Cam[1] * v3Motion[2] * dOneOverCameraZ) * dOneOverCameraZ; 248 | m23Jacobian.T()[m] = m2CamDerivs * v2CamFrameMotion; 249 | }; 250 | wls.add_mJ(v2Error[0], m23Jacobian[0], 1.0); 251 | wls.add_mJ(v2Error[1], m23Jacobian[1], 1.0); 252 | }; 253 | 254 | wls.compute(); 255 | Vector<3> v3Res = wls.get_mu(); 256 | so3 = SO3<>::exp(v3Res) * so3; 257 | }; 258 | 259 | SE3<> se3Result; 260 | se3Result.get_rotation() = so3; 261 | return se3Result; 262 | } 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | --------------------------------------------------------------------------------